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