ALMATH: optimization and new function position6DFromPosition3D

Change-Id: I23468e85781ed220db7469aac0107d56a8cac1af