/// <summary> /// Computes a rotation from the given heading, pitch and roll angles. /// Heading is the rotation about the negative z axis. /// Pitch is the rotation about the negative y axis. /// Roll is the rotation about the positive x axis. /// </summary> /// <param name="hpr"></param> /// <returns></returns> public static Quaternion FromHeadingPitchRoll(HeadingPitchRoll hpr) { var scratchRollQuaternion = FromAxisAngle(new Vec3(1.0, 0, 0), hpr.Roll); var scratchPitchQuaternion = FromAxisAngle(new Vec3(0, 1.0, 0), -hpr.Pitch); scratchPitchQuaternion = Multiply(scratchPitchQuaternion, scratchRollQuaternion); var scratchHeadingQuaternion = FromAxisAngle(new Vec3(0, 0, 1.0), -hpr.Heading); return(Multiply(scratchHeadingQuaternion, scratchPitchQuaternion)); }
internal static Matrix4 HeadingPitchRollToFixedFrame(Vec3 origin, HeadingPitchRoll hpr) { var hprQuaternion = Quaternion.FromHeadingPitchRoll(hpr); var scratchScale = new Vec3(1.0); var hprMatrix = Matrix4.FromTranslationQuaternionRotationScale( new Vec3(), hprQuaternion, scratchScale); var fixedFrameTransform = EastNorthUpToFixedFrame(origin, Ellipsoid.Wgs84); return(Matrix4.Multiply(fixedFrameTransform, hprMatrix)); }