/// <summary>
/// Convert the rotation matrix to the euler angle
/// </summary>
/// <param name="rmat">The roatation matrix</param>
/// <returns>The Euler Angle vector</returns>
public static Vector3F RMatrix2Euler(Matrix3F rmat)
{
return(new Vector3F(
Math.Atan2(rmat[2, 1], rmat[2, 2]),
Math.Atan2(-rmat[2, 0], Math.Sqrt(rmat[2, 1] * rmat[2, 1] + rmat[2, 2] * rmat[2, 2])),
Math.Atan2(rmat[1, 0], rmat[0, 0])));
}
/// <summary>
/// Initialize with a rotation matrix
/// </summary>
/// <param name="pos">The point position</param>
/// <param name="rotate">The point rotation</param>
public Transform3D(Vector3F pos, Matrix3F rotate)
{
Position = pos;
RotationMat = rotate;
}
/// <summary>
/// Initial empty state
/// </summary>
public Transform3D()
{
Position = new Vector3F();
RotationMat = Matrix3F.GetUnitMatrix();
}