/// <summary>
/// Calculate the transpose of the given matrix
/// </summary>
/// <param name="mat">The matrix to transpose</param>
public static void Transpose(ref Matrix4 mat, ref Matrix4 result)
{
result.Row0 = mat.Column0;
result.Row1 = mat.Column1;
result.Row2 = mat.Column2;
result.Row3 = 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 float4(inverse[0, 0], inverse[0, 1], inverse[0, 2], inverse[0, 3]);
mat.Row1 = new float4(inverse[1, 0], inverse[1, 1], inverse[1, 2], inverse[1, 3]);
mat.Row2 = new float4(inverse[2, 0], inverse[2, 1], inverse[2, 2], inverse[2, 3]);
mat.Row3 = new float4(inverse[3, 0], inverse[3, 1], inverse[3, 2], inverse[3, 3]);
return mat;
}
/// <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);
}
public static void Mult(ref Matrix4 left, ref Matrix4 right, ref Matrix4 result)
{
float4 col0 = right.Column0;
float4 col1 = right.Column1;
float4 col2 = right.Column2;
float4 col3 = right.Column3;
result.Row0 = new float4(left.Row0 * col0, left.Row0 * col1, left.Row0 * col2, left.Row0 * col3);
result.Row1 = new float4(left.Row1 * col0, left.Row1 * col1, left.Row1 * col2, left.Row1 * col3);
result.Row2 = new float4(left.Row2 * col0, left.Row2 * col1, left.Row2 * col2, left.Row2 * col3);
result.Row3 = new float4(left.Row3 * col0, left.Row3 * col1, left.Row3 * col2, 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)
{
float4 col0 = right.Column0;
float4 col1 = right.Column1;
float4 col2 = right.Column2;
float4 col3 = right.Column3;
left.Row0 = new float4(left.Row0*col0, left.Row0*col1, left.Row0*col2, left.Row0*col3);
left.Row1 = new float4(left.Row1*col0, left.Row1*col1, left.Row1*col2, left.Row1*col3);
left.Row2 = new float4(left.Row2*col0, left.Row2*col1, left.Row2*col2, left.Row2*col3);
left.Row3 = new float4(left.Row3*col0, left.Row3*col1, left.Row3*col2, 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(Vector3f eye, Vector3f target, Vector3f up)
{
Vector3f z = Vector3f.Normalize(eye - target);
Vector3f x = Vector3f.Normalize(Vector3f.Cross(up, z));
Vector3f y = Vector3f.Normalize(Vector3f.Cross(z, x));
Matrix4 rot = new Matrix4(new float4(x.x, y.x, z.x, 0.0f),
new float4(x.y, y.y, z.y, 0.0f),
new float4(x.z, y.z, z.z, 0.0f),
float4.UnitW);
Matrix4 trans = Matrix4.Translation(-eye);
return trans * rot;
}
/// <summary>
/// Build a rotation matrix to rotate about the given axis
/// </summary>
/// <param name="axis">the axis to rotate about</param>
/// <param name="angle">angle in radians to rotate counter-clockwise (looking in the direction of the given axis)</param>
/// <returns>A rotation matrix</returns>
public static Matrix4 Rotate(float3 axis, float angle)
{
float cos = (float)System.Math.Cos(-angle);
float sin = (float)System.Math.Sin(-angle);
float t = 1.0f - cos;
axis = axis.Normalize;
Matrix4 result = new Matrix4();
result.Row0 = new float4(t * axis.x * axis.x + cos, t * axis.x * axis.y - sin * axis.z, t * axis.x * axis.z + sin * axis.y, 0.0f);
result.Row1 = new float4(t * axis.x * axis.y + sin * axis.z, t * axis.y * axis.y + cos, t * axis.y * axis.z - sin * axis.x, 0.0f);
result.Row2 = new float4(t * axis.x * axis.z - sin * axis.y, t * axis.y * axis.z + sin * axis.x, t * axis.z * axis.z + cos, 0.0f);
result.Row3 = float4.UnitW;
return result;
}
/// <summary>
/// Build a rotation matrix that rotates about the z-axis
/// </summary>
/// <param name="angle">angle in radians to rotate counter-clockwise around the z-axis</param>
/// <returns>A rotation matrix</returns>
public static Matrix4 RotateZ(float angle)
{
float cos = (float)System.Math.Cos(angle);
float sin = (float)System.Math.Sin(angle);
Matrix4 result = new Matrix4() ;
result.Row0 = new float4(cos, sin, 0.0f, 0.0f);
result.Row1 = new float4(-sin, cos, 0.0f, 0.0f);
result.Row2 = float4.UnitZ;
result.Row3 = float4.UnitW;
return result;
}
