- sgdMat4 hlTrans;
- sgdMakeRotMat4(hlTrans, hdg, pitch, roll);
- sgdXformPnt3(sgdOff, hlTrans);
-
- // Now transform to the wgs84 earth centeres system.
- Point3D pos2(pos.lon()* SGD_DEGREES_TO_RADIANS,
- pos.lat() * SGD_DEGREES_TO_RADIANS,
- pos.elev());
- Point3D cartPos3D = sgGeodToCart(pos2);
- sgdMat4 ecTrans;
- sgdMakeCoordMat4(ecTrans, cartPos3D.x(), cartPos3D.y(), cartPos3D.z(),
- pos.lon(), 0, - 90 - pos.lat());
- sgdXformPnt3(sgdOff, ecTrans);
-
- return Point3D(sgdOff[0], sgdOff[1], sgdOff[2]);
-}
+ SGQuatd hlTrans = SGQuatd::fromLonLatDeg(pos.lon(), pos.lat());
+ // and postrotate the orientation of the AIModel wrt the horizontal
+ // local frame
+ hlTrans *= SGQuatd::fromYawPitchRollDeg(hdg, pitch, roll);