/// <summary>
/// See http://www.ee.ucr.edu/~farell/AidedNavigation/D_App_Quaternions/Rot2Quat.pdf
/// </summary>
/// <param name="rm">The rotation matrix</param>
private void Diagonal1Solution(RotationMatrix rm)
{
this.W = (float)(0.5 * Math.Sqrt(1 + rm[0, 0] + rm[1, 1] + rm[2, 2]));
this.X = (rm[2, 1] - rm[1, 2]) / (4 * this.W);
this.Y = (rm[0, 2] - rm[2, 0]) / (4 * this.W);
this.Z = (rm[1, 0] - rm[0, 1]) / (4 * this.W);
}
/// <summary>
/// Returns the rotation matrix that is in the transformation matrix.
/// </summary>
/// <returns>The 3x3 rotation matrix which is in the transformation matrix.</returns>
public RotationMatrix GetRotation()
{
RotationMatrix res = new RotationMatrix();
this.SubMatrix(0, 3, 0, 3).CopyTo(res);
return(res);
}
/// <summary>
/// Initializes a new instance of the <see cref="Quaternion"/> class.
/// Creates quaternion based equivalent to a rotation matrix.
/// </summary>
/// <param name="rm">The rotation matrix.</param>
public Quaternion(RotationMatrix rm)
{
float[] methods =
{
rm[0, 0] + rm[1, 1] + rm[2, 2],
rm[0, 0] - rm[1, 1] - rm[2, 2],
((-1 * rm[0, 0]) + rm[1, 1]) - rm[2, 2],
((-1 * rm[0, 0]) - rm[1, 1]) + rm[2, 2]
};
switch (methods.ToList().IndexOf(methods.Max()))
{
case 0:
this.Diagonal1Solution(rm);
break;
case 1:
this.Diagonal2Solution(rm);
break;
case 2:
this.Diagonal3Solution(rm);
break;
case 3:
this.Diagonal4Solution(rm);
break;
}
this.EulerAnglesDegree = this.CalcEulerAngles();
}
/// <summary>
/// Initializes a new instance of the <see cref="TransformationMatrix"/> class.
/// Creates a 4x4 transformation matrix which corresponds to a rotation followed by a translation.
/// Sets w to default value 1.
/// </summary>
/// <param name="xt">X axis translation.</param>
/// <param name="yt">Y axis translation.</param>
/// <param name="zt">Z axis translation.</param>
/// <param name="rm">The rotationmatrix defining the rotation to use.</param>
public TransformationMatrix(float xt, float yt, float zt, RotationMatrix rm)
: base(4, 4, CreateMatrixArray(xt, yt, zt, 1))
{
this.SetSubMatrix(0, 0, rm);
}
