/// <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));
}