public void MeshAt(Mesh mesh, Matrix4 transform)
{
if (meshAt != null)
{
meshAt(meshAtCalls, mesh, transform);
meshAtCalls++;
}
}
public Matrix4 ToRotation()
{
Vector3 side = this.Side;
Vector3 up = Vector3.Cross(side, Direction);
Matrix4 lookMatrix = new Matrix4(new Vector4(side, 0),
new Vector4(up, 0),
new Vector4(-Direction, 0),
new Vector4(0, 0, 0, 1));
return lookMatrix;
}
/// <summary>
/// Transform a Vector3 by the given Matrix, and project the resulting Vector4 back to a Vector3
/// </summary>
/// <param name="pos">The vector to transform</param>
/// <param name="mat">The desired transformation</param>
/// <returns>The transformed vector</returns>
public static Vector3 TransformPerspective(Vector3 vec, Matrix4 mat)
{
Vector4 h = Transform(vec, mat);
return new Vector3(h.X / h.W, h.Y / h.W, h.Z / h.W);
}
/// <summary>
/// Transform a Vector by the given Matrix
/// </summary>
/// <param name="pos">The vector to transform</param>
/// <param name="mat">The desired transformation</param>
/// <returns>The transformed vector</returns>
public static Vector4 Transform(Vector3 vec, Matrix4 mat)
{
Vector4 v4 = new Vector4(vec.X, vec.Y, vec.Z, 1.0f);
Vector4 result;
result.X = Vector4.Dot(v4, mat.Column0);
result.Y = Vector4.Dot(v4, mat.Column1);
result.Z = Vector4.Dot(v4, mat.Column2);
result.W = Vector4.Dot(v4, mat.Column3);
return result;
}
/// <summary>
/// Transform a Position by the given Matrix
/// </summary>
/// <param name="pos">The position to transform</param>
/// <param name="mat">The desired transformation</param>
/// <returns>The transformed position</returns>
public static Vector3 TransformPosition(Vector3 pos, Matrix4 mat)
{
Vector3 p;
p.X = Vector3.Dot(pos, new Vector3(mat.Column0)) + mat.Row3.X;
p.Y = Vector3.Dot(pos, new Vector3(mat.Column1)) + mat.Row3.Y;
p.Z = Vector3.Dot(pos, new Vector3(mat.Column2)) + mat.Row3.Z;
return p;
}
/// <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="mat">The inverse of the desired transformation</param>
/// <returns>The transformed normal</returns>
public static Vector3 TransformNormalInverse(Vector3 norm, Matrix4 invMat)
{
Vector3 n;
n.X = Vector3.Dot(norm, new Vector3(invMat.Row0));
n.Y = Vector3.Dot(norm, new Vector3(invMat.Row1));
n.Z = Vector3.Dot(norm, new Vector3(invMat.Row2));
return n;
}
/// <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 Vector3 TransformNormal(Vector3 norm, Matrix4 mat)
{
mat.Invert();
return TransformNormalInverse(norm, mat);
}
/// <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>
/// <returns>The transformed vector</returns>
public static Vector3 TransformVector(Vector3 vec, Matrix4 mat)
{
Vector3 v;
v.X = Vector3.Dot(vec, new Vector3(mat.Column0));
v.Y = Vector3.Dot(vec, new Vector3(mat.Column1));
v.Z = Vector3.Dot(vec, new Vector3(mat.Column2));
return v;
}
/// <summary>
/// Transform a Vector by the given Matrix
/// </summary>
/// <param name="pos">The vector to transform</param>
/// <param name="mat">The desired transformation</param>
/// <returns>The transformed vector</returns>
public static Vector4 Transform(Vector4 vec, Matrix4 mat)
{
Vector4 result;
result.X = Vector4.Dot(vec, mat.Column0);
result.Y = Vector4.Dot(vec, mat.Column1);
result.Z = Vector4.Dot(vec, mat.Column2);
result.W = Vector4.Dot(vec, mat.Column3);
return result;
}
/// <summary>
/// Calculate the transpose of the given matrix
/// </summary>
/// <param name="mat">The matrix to transpose</param>
public static void Transpose(ref Matrix4 mat, out Matrix4 result)
{
result.Row0 = mat.Column0;
result.Row1 = mat.Column1;
result.Row2 = mat.Column2;
result.Row3 = mat.Column3;
}
/// <summary>
/// Calculate the transpose of the given matrix
/// </summary>
/// <param name="mat">The matrix to transpose</param>
/// <returns>The transpose of the given matrix</returns>
public static Matrix4 Transpose(Matrix4 mat)
{
return new Matrix4(mat.Column0, mat.Column1, mat.Column2, mat.Column3);
}
/// <summary>
/// Calculate the inverse of the given matrix
/// </summary>
/// <param name="mat">The matrix to invert</param>
/// <returns>The inverse of the given matrix if it has one, or the input if it is singular</returns>
/// <exception cref="InvalidOperationException">Thrown if the Matrix4 is singular.</exception>
public static Matrix4 Invert(Matrix4 mat)
{
int[] colIdx = { 0, 0, 0, 0 };
int[] rowIdx = { 0, 0, 0, 0 };
int[] pivotIdx = { -1, -1, -1, -1 };
// convert the matrix to an array for easy looping
float[,] inverse = {{mat.Row0.X, mat.Row0.Y, mat.Row0.Z, mat.Row0.W},
{mat.Row1.X, mat.Row1.Y, mat.Row1.Z, mat.Row1.W},
{mat.Row2.X, mat.Row2.Y, mat.Row2.Z, mat.Row2.W},
{mat.Row3.X, mat.Row3.Y, mat.Row3.Z, mat.Row3.W} };
int icol = 0;
int irow = 0;
for (int i = 0; i < 4; i++)
{
// Find the largest pivot value
float maxPivot = 0.0f;
for (int j = 0; j < 4; j++)
{
if (pivotIdx[j] != 0)
{
for (int k = 0; k < 4; ++k)
{
if (pivotIdx[k] == -1)
{
float absVal = System.Math.Abs(inverse[j, k]);
if (absVal > maxPivot)
{
maxPivot = absVal;
irow = j;
icol = k;
}
}
else if (pivotIdx[k] > 0)
{
return mat;
}
}
}
}
++(pivotIdx[icol]);
// Swap rows over so pivot is on diagonal
if (irow != icol)
{
for (int k = 0; k < 4; ++k)
{
float f = inverse[irow, k];
inverse[irow, k] = inverse[icol, k];
inverse[icol, k] = f;
}
}
rowIdx[i] = irow;
colIdx[i] = icol;
float pivot = inverse[icol, icol];
// check for singular matrix
if (pivot == 0.0f)
{
throw new InvalidOperationException("Matrix is singular and cannot be inverted.");
//return mat;
}
// Scale row so it has a unit diagonal
float oneOverPivot = 1.0f / pivot;
inverse[icol, icol] = 1.0f;
for (int k = 0; k < 4; ++k)
inverse[icol, k] *= oneOverPivot;
// Do elimination of non-diagonal elements
for (int j = 0; j < 4; ++j)
{
// check this isn't on the diagonal
if (icol != j)
{
float f = inverse[j, icol];
inverse[j, icol] = 0.0f;
for (int k = 0; k < 4; ++k)
inverse[j, k] -= inverse[icol, k] * f;
}
}
}
for (int j = 3; j >= 0; --j)
{
int ir = rowIdx[j];
int ic = colIdx[j];
for (int k = 0; k < 4; ++k)
{
float f = inverse[k, ir];
inverse[k, ir] = inverse[k, ic];
inverse[k, ic] = f;
}
}
mat.Row0 = new Vector4(inverse[0, 0], inverse[0, 1], inverse[0, 2], inverse[0, 3]);
mat.Row1 = new Vector4(inverse[1, 0], inverse[1, 1], inverse[1, 2], inverse[1, 3]);
mat.Row2 = new Vector4(inverse[2, 0], inverse[2, 1], inverse[2, 2], inverse[2, 3]);
mat.Row3 = new Vector4(inverse[3, 0], inverse[3, 1], inverse[3, 2], inverse[3, 3]);
return mat;
}
public static void Mult(ref Matrix4 left, ref Matrix4 right, out Matrix4 result)
{
Vector4 col0 = right.Column0;
Vector4 col1 = right.Column1;
Vector4 col2 = right.Column2;
Vector4 col3 = right.Column3;
result.Row0 = new Vector4(Vector4.Dot(left.Row0, col0), Vector4.Dot(left.Row0, col1), Vector4.Dot(left.Row0, col2), Vector4.Dot(left.Row0, col3));
result.Row1 = new Vector4(Vector4.Dot(left.Row1, col0), Vector4.Dot(left.Row1, col1), Vector4.Dot(left.Row1, col2), Vector4.Dot(left.Row1, col3));
result.Row2 = new Vector4(Vector4.Dot(left.Row2, col0), Vector4.Dot(left.Row2, col1), Vector4.Dot(left.Row2, col2), Vector4.Dot(left.Row2, col3));
result.Row3 = new Vector4(Vector4.Dot(left.Row3, col0), Vector4.Dot(left.Row3, col1), Vector4.Dot(left.Row3, col2), Vector4.Dot(left.Row3, col3));
}
/// <summary>
/// Post multiply this matrix by another matrix
/// </summary>
/// <param name="right">The matrix to multiply</param>
/// <returns>A new Matrix44 that is the result of the multiplication</returns>
public static Matrix4 Mult(Matrix4 left, Matrix4 right)
{
Vector4 col0 = right.Column0;
Vector4 col1 = right.Column1;
Vector4 col2 = right.Column2;
Vector4 col3 = right.Column3;
left.Row0 = new Vector4(Vector4.Dot(left.Row0, col0), Vector4.Dot(left.Row0, col1), Vector4.Dot(left.Row0, col2), Vector4.Dot(left.Row0, col3));
left.Row1 = new Vector4(Vector4.Dot(left.Row1, col0), Vector4.Dot(left.Row1, col1), Vector4.Dot(left.Row1, col2), Vector4.Dot(left.Row1, col3));
left.Row2 = new Vector4(Vector4.Dot(left.Row2, col0), Vector4.Dot(left.Row2, col1), Vector4.Dot(left.Row2, col2), Vector4.Dot(left.Row2, col3));
left.Row3 = new Vector4(Vector4.Dot(left.Row3, col0), Vector4.Dot(left.Row3, col1), Vector4.Dot(left.Row3, col2), Vector4.Dot(left.Row3, col3));
return left;
}
/// <summary>
/// Build a world space to camera space matrix
/// </summary>
/// <param name="eye">Eye (camera) position in world space</param>
/// <param name="target">Target position in world space</param>
/// <param name="up">Up vector in world space (should not be parallel to the camera direction, that is target - eye)</param>
/// <returns>A Matrix that transforms world space to camera space</returns>
public static Matrix4 LookAt(Vector3 eye, Vector3 target, Vector3 up)
{
Vector3 z = Vector3.Normalize(eye - target);
Vector3 x = Vector3.Normalize(Vector3.Cross(up, z));
Vector3 y = Vector3.Normalize(Vector3.Cross(z, x));
Matrix4 rot = new Matrix4(new Vector4(x.X, y.X, z.X, 0.0f),
new Vector4(x.Y, y.Y, z.Y, 0.0f),
new Vector4(x.Z, y.Z, z.Z, 0.0f),
Vector4.UnitW);
Matrix4 trans = Matrix4.Translation(-eye);
return trans * rot;
}
public static void MultMatrix(Matrix4 mat)
{
MultMatrix(ref mat.Row0.X);
}