THEN {
length: REAL ← G3dVector.Length[shape.orientation];
cosA, sinA, cosB, sinB: REAL;
cosA ← shape.orientation.x / hypotenuse;
sinA ← shape.orientation.y / hypotenuse;
shape.position ← G3dMatrix.Mul[
shape.position, -- longitudinal rotation into x-z plane, left handed about z-up
NEW[ Xfm3DRep ← [ [cosA,-sinA,0.,0.], [sinA,cosA,0.,0.], [0.,0.,1.,0.], [0.,0.,0.,1.] ] ]
];
cosB ← shape.orientation.z / length;
sinB ← hypotenuse / length;
shape.position ← G3dMatrix.Mul[
shape.position, -- latitudinal rotation, right-handed about y-north
NEW[ Xfm3DRep ← [ [cosB,0.,-sinB,0.], [0.,1.,0.,0.], [sinB,0.,cosB,0.], [0.,0.,0.,1.] ] ]
];
shape.position ← G3dMatrix.Mul[
shape.position, -- longitudinal rotation from x-z plane, right handed about z-up
NEW[ Xfm3DRep ← [ [cosA,sinA,0.,0.], [-sinA,cosA,0.,0.], [0.,0.,1.,0.], [0.,0.,0.,1.] ] ]
];
}
ELSE
IF shape.orientation.z < 0.0
THEN shape.position ← G3dMatrix.Mul[
shape.position, -- turn upside down
NEW[ Xfm3DRep ← [ [-1.,0.,0.,0.], [0.,1.,0.,0.], [0.,0.,-1.,0.], [0.,0.,0.,1.] ] ]
];