/// <summary>Transform a Normal by the (transpose of the) given Matrix</summary>
/// <remarks>
/// This version doesn't calculate the inverse matrix.
/// Use this version if you already have the inverse of the desired transform to hand
/// </remarks>
/// <param name="norm">The normal to transform</param>
/// <param name="invMat">The inverse of the desired transformation</param>
/// <param name="result">The transformed normal</param>
public static void TransformNormalInverse(ref Vector3d norm, ref Matrix4d invMat, out Vector3d result)
{
result.X = norm.X * invMat.Row0.X +
norm.Y * invMat.Row0.Y +
norm.Z * invMat.Row0.Z;
result.Y = norm.X * invMat.Row1.X +
norm.Y * invMat.Row1.Y +
norm.Z * invMat.Row1.Z;
result.Z = norm.X * invMat.Row2.X +
norm.Y * invMat.Row2.Y +
norm.Z * invMat.Row2.Z;
}
/// <summary>Transform a direction vector by the given Matrix
/// Assumes the matrix has a bottom row of (0,0,0,1), that is the translation part is ignored.
/// </summary>
/// <param name="vec">The vector to transform</param>
/// <param name="mat">The desired transformation</param>
/// <param name="result">The transformed vector</param>
public static void TransformVector(ref Vector3d vec, ref Matrix4d mat, out Vector3d result)
{
result.X = vec.X * mat.Row0.X +
vec.Y * mat.Row1.X +
vec.Z * mat.Row2.X;
result.Y = vec.X * mat.Row0.Y +
vec.Y * mat.Row1.Y +
vec.Z * mat.Row2.Y;
result.Z = vec.X * mat.Row0.Z +
vec.Y * mat.Row1.Z +
vec.Z * mat.Row2.Z;
}
/// <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 Vector by the given Matrix</summary>
/// <param name="vec">The vector to transform</param>
/// <param name="mat">The desired transformation</param>
/// <param name="result">The transformed vector</param>
public static void Transform(ref Vector4d vec, ref Matrix4d mat, out Vector4d result)
{
result.X = vec.X * mat.Row0.X +
vec.Y * mat.Row1.X +
vec.Z * mat.Row2.X +
vec.W * mat.Row3.X;
result.Y = vec.X * mat.Row0.Y +
vec.Y * mat.Row1.Y +
vec.Z * mat.Row2.Y +
vec.W * mat.Row3.Y;
result.Z = vec.X * mat.Row0.Z +
vec.Y * mat.Row1.Z +
vec.Z * mat.Row2.Z +
vec.W * mat.Row3.Z;
result.W = vec.X * mat.Row0.W +
vec.Y * mat.Row1.W +
vec.Z * mat.Row2.W +
vec.W * mat.Row3.W;
}
/// <summary>Returns left matrix for this Quaterniond.</summary>
public void Matrix4d(out Matrix4d result)
{
// TODO Expand
result = new Matrix4d(ref this);
}
/// <summary>
/// Transform a Vector3d by the given Matrix, and project the resulting Vector4 back to a Vector3
/// </summary>
/// <param name="vec">The vector to transform</param>
/// <param name="mat">The desired transformation</param>
/// <returns>The transformed vector</returns>
public static Vector3d TransformPerspective(Vector3d vec, Matrix4d mat)
{
Vector4d h = Transform(vec, mat);
return(new Vector3d(h.X / h.W, h.Y / h.W, h.Z / h.W));
}
/// <summary>Transform a Vector by the given Matrix</summary>
/// <param name="vec">The vector to transform</param>
/// <param name="mat">The desired transformation</param>
/// <param name="result">The transformed vector</param>
public static void Transform(ref Vector3d vec, ref Matrix4d mat, out Vector4d result)
{
Vector4d v4 = new Vector4d(vec.X, vec.Y, vec.Z, 1.0f);
Vector4d.Transform(ref v4, ref mat, out result);
}
/// <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>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));
}