Ejemplo n.º 1
0
 /// <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)));
 }
Ejemplo n.º 2
0
 /// <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));
 }
Ejemplo n.º 3
0
 /// <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));
 }
Ejemplo n.º 4
0
        /// <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);
        }
Ejemplo n.º 5
0
 /// <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));
 }
Ejemplo n.º 6
0
        /// <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);
        }
Ejemplo n.º 7
0
 /// <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)));
 }