/// <summary>Transform a Position by the given Matrix</summary>
/// <param name="pos">The position to transform</param>
/// <param name="mat">The desired transformation</param>
/// <param name="result">The transformed position</param>
public static void TransformPosition(ref Vector3d pos, ref Matrix4d mat, out Vector3d result)
{
result.X = pos.X * mat.Row0.X +
pos.Y * mat.Row1.X +
pos.Z * mat.Row2.X +
mat.Row3.X;
result.Y = pos.X * mat.Row0.Y +
pos.Y * mat.Row1.Y +
pos.Z * mat.Row2.Y +
mat.Row3.Y;
result.Z = pos.X * mat.Row0.Z +
pos.Y * mat.Row1.Z +
pos.Z * mat.Row2.Z +
mat.Row3.Z;
}
/// <summary>Constructs left Quaterniond from the given matrix. Only contains rotation information.</summary>
/// <param name="matrix">The matrix for the components of the Quaterniond.</param>
public Quaterniond(ref Matrix4d matrix)
{
double scale = System.Math.Pow(matrix.Determinant, 1.0d / 3.0d);
W = System.Math.Sqrt(System.Math.Max(0, scale + matrix[0, 0] + matrix[1, 1] + matrix[2, 2])) / 2;
X = System.Math.Sqrt(System.Math.Max(0, scale + matrix[0, 0] - matrix[1, 1] - matrix[2, 2])) / 2;
Y = System.Math.Sqrt(System.Math.Max(0, scale - matrix[0, 0] + matrix[1, 1] - matrix[2, 2])) / 2;
Z = System.Math.Sqrt(System.Math.Max(0, scale - matrix[0, 0] - matrix[1, 1] + matrix[2, 2])) / 2;
if (matrix[2, 1] - matrix[1, 2] < 0)
{
X = -X;
}
if (matrix[0, 2] - matrix[2, 0] < 0)
{
Y = -Y;
}
if (matrix[1, 0] - matrix[0, 1] < 0)
{
Z = -Z;
}
}
/// <summary>Transform a Normal by the given Matrix</summary>
/// <remarks>
/// This calculates the inverse of the given matrix, use TransformNormalInverse if you
/// already have the inverse to avoid this extra calculation
/// </remarks>
/// <param name="norm">The normal to transform</param>
/// <param name="mat">The desired transformation</param>
/// <returns>The transformed normal</returns>
public static Vector3d TransformNormal(Vector3d norm, Matrix4d mat)
{
mat.Invert();
return(TransformNormalInverse(norm, mat));
}
/// <summary>Transform a Normal by the given Matrix</summary>
/// <remarks>
/// This calculates the inverse of the given matrix, use TransformNormalInverse if you
/// already have the inverse to avoid this extra calculation
/// </remarks>
/// <param name="norm">The normal to transform</param>
/// <param name="mat">The desired transformation</param>
/// <param name="result">The transformed normal</param>
public static void TransformNormal(ref Vector3d norm, ref Matrix4d mat, out Vector3d result)
{
Matrix4d Inverse = Matrix4d.Invert(mat);
Vector3d.TransformNormalInverse(ref norm, ref Inverse, out result);
}
/// <summary>Returns left matrix for this Quaterniond.</summary>
public void Matrix4d(out Matrix4d result)
{
// TODO Expand
result = new Matrix4d(ref this);
}