/// <summary> /// Converts a direction cosine matrix to yaw, pitch, roll in radians. /// </summary> /// <param name="dcm"> /// The direction cosine matrix. /// </param> /// <returns> /// The corresponding yaw, pitch, roll in radians. /// </returns> public static vec3f Dcm2YprInRads(mat3f dcm) { return(new vec3f( (float)SMath.Atan2(dcm.E01, dcm.E00), (float)SMath.Asin(-dcm.E02), (float)SMath.Atan2(dcm.E12, dcm.E22))); }
/// <summary> /// Subtracts the provided <c>mat3f</c> from <c>this mat3f</c>. /// </summary> /// <param name="rhs"> /// The <c>mat3f</c> to subtract from <c>this mat3f</c>. /// </param> /// <returns> /// The resultant <c>mat3f</c> from the subtraction. /// </returns> public mat3f Subtract(mat3f rhs) { return(new mat3f( E00 - rhs.E00, E01 - rhs.E01, E02 - rhs.E02, E10 - rhs.E10, E11 - rhs.E11, E12 - rhs.E12, E20 - rhs.E20, E21 - rhs.E21, E22 - rhs.E22)); }
/// <summary> /// Adds <c>this Matrix3F</c> with the provided <c>mat3f</c> together. /// </summary> /// <param name="matrix"> /// The <c>mat3f</c> to add to <c>this mat3f</c>. /// </param> /// <returns> /// The resultant <c>mat3f</c> from the addition. /// </returns> public mat3f Add(mat3f matrix) { return(new mat3f( E00 + matrix.E00, E01 + matrix.E01, E02 + matrix.E02, E10 + matrix.E10, E11 + matrix.E11, E12 + matrix.E12, E20 + matrix.E20, E21 + matrix.E21, E22 + matrix.E22)); }
/// <summary> /// Multiplies <c>this mat3f</c> by the provided right-side <c>mat3f</c>. /// </summary> /// <param name="rhs"> /// The right-side <c>mat3f</c>. /// </param> /// <returns> /// The resulting matrix. /// </returns> public mat3f Multiply(mat3f rhs) { var result = new mat3f(); for (var i = 0; i < 3; i++) { for (var j = 0; j < 3; j++) { for (var k = 0; k < 3; k++) { result[i, j] += this[i, k] * rhs[k, j]; } } } return(result); }
/// <summary> /// Creates a new <c>AttitudeF</c> from a direction cosine matrix. /// </summary> /// <param name="dcm"> /// The direction cosine matrix. /// </param> /// <returns> /// The new <c>AttitudeF</c>. /// </returns> public static AttitudeF FromDcm(mat3f dcm) { return(new AttitudeF(AttitudeType.Dcm, dcm)); }
/// <summary> /// Converts an orientation represented as a direction cosine matrix to a /// quaternion. /// </summary> /// <param name="dcm"> /// The direction cosine matrix to convert. /// </param> /// <returns> /// The corresponding quaternion. /// </returns> public static vec4f Dcm2Quat(mat3f dcm) { var tr = dcm.E00 + dcm.E11 + dcm.E22; var b2 = new[] { (1f + tr) / 4f, (1f + 2f * dcm.E00 - tr) / 4f, (1f + 2f * dcm.E11 - tr) / 4f, (1f + 2f * dcm.E22 - tr) / 4f }; var maxNum = b2[0]; var maxIndex = 0; for (var i = 1; i < b2.Length; i++) { if (b2[i] > maxNum) { maxNum = b2[i]; maxIndex = i; } } var q = new vec4f(); switch (maxIndex) { case 0: q.W = (float)System.Math.Sqrt(b2[0]); q.X = (dcm.E12 - dcm.E21) / 4f / q.W; q.Y = (dcm.E20 - dcm.E02) / 4f / q.W; q.Z = (dcm.E01 - dcm.E10) / 4f / q.W; break; case 1: q.X = (float)System.Math.Sqrt(b2[1]); q.W = (dcm.E12 - dcm.E21) / 4f / q.X; if (q.W < 0) { q.X = -q.X; q.W = -q.W; } q.Y = (dcm.E01 + dcm.E10) / 4f / q.X; q.Z = (dcm.E20 + dcm.E02) / 4f / q.X; break; case 2: q.Y = (float)System.Math.Sqrt(b2[2]); q.W = (dcm.E20 - dcm.E02) / 4f / q.Y; if (q.W < 0) { q.Y = -q.Y; q.W = -q.W; } q.X = (dcm.E01 + dcm.E10) / 4f / q.Y; q.Z = (dcm.E12 + dcm.E21) / 4f / q.Y; break; case 3: q.Z = (float)System.Math.Sqrt(b2[3]); q.W = (dcm.E01 - dcm.E10) / 4f / q.Z; if (q.W < 0) { q.Z = -q.Z; q.W = -q.W; } q.X = (dcm.E20 + dcm.E02) / 4f / q.Z; q.Y = (dcm.E12 + dcm.E21) / 4f / q.Z; break; } return(q); }
/// <summary> /// Converts a direction cosine matrix to yaw, pitch, roll in degrees. /// </summary> /// <param name="dcm"> /// The direction cosine matrix. /// </param> /// <returns> /// The corresponding yaw, pitch, roll in degrees. /// </returns> public static vec3f Dcm2YprInDegs(mat3f dcm) { return(Rad2Deg(Dcm2YprInRads(dcm))); }