/// <summary>
/// Transforms the current vector by a matrix.
/// </summary>
/// <param name="m">Matrix to transform current vector with.</param>
public void Mul(ref Matrix4 m)
{
float ox = x;
float oy = y;
float oz = z;
x = ox * m.A1 + oy * m.B1 + oz * m.C1 + m.D1;
y = ox * m.A2 + oy * m.B2 + oz * m.C2 + m.D2;
z = ox * m.A3 + oy * m.B3 + oz * m.C3 + m.D3;
}
/// <summary>
/// Spherical quadratic interpolation between matrices.
/// </summary>
/// <remarks>The matrices are converted to quaternion for every invocation => it is recommended to use the Quaternion.Squad
/// method instead.</remarks>
/// <param name="pre">One matrix before the actual interpolation matrices.</param>
/// <param name="a">First matrix.</param>
/// <param name="b">Second matrix.</param>
/// <param name="post">One matrix after the actual interpolation matrices.</param>
/// <param name="time">The interpolation time.</param>
/// <returns>The interpolated matrix.</returns>
public static Matrix4 Squad(Matrix4 pre, Matrix4 a, Matrix4 b, Matrix4 post, float time)
{
Quaternion qPre = pre.Quaternion;
Quaternion qa = a.Quaternion;
Quaternion qb = b.Quaternion;
Quaternion qPost = post.Quaternion;
Quaternion interpolated = //Quaternion.Slerp(qa, qb, time);
Quaternion.SimpleSquad(qPre, qa, qb, qPost, time);
Matrix4 result = interpolated.Matrix4;
//result.TranslationVector = CRSpline.Interpolate(time, pre.TranslationVector,
// a.TranslationVector, b.TranslationVector, post.TranslationVector);
return result;
}
//---------------------------------------------------------------
/// <summary>
/// Adds the weighted skinned source vector.
/// </summary>
public void AddSkinned(Vector3 sourceVec, ref Matrix4 joint, float weight)
{
x += (sourceVec.x * joint.A1 + sourceVec.y * joint.B1 + sourceVec.z * joint.C1 + joint.D1) * weight;
y += (sourceVec.x * joint.A2 + sourceVec.y * joint.B2 + sourceVec.z * joint.C2 + joint.D2) * weight;
z += (sourceVec.x * joint.A3 + sourceVec.y * joint.B3 + sourceVec.z * joint.C3 + joint.D3) * weight;
}
/// <summary>
/// builds a rotation matrix for a given vector and an angle
/// </summary>
/// <param name="vec">to rotate around</param>
/// <param name="angle">rotation angle</param>
/// <returns>rotation matrix</returns>
public static Matrix4 Rotation(Vector3 vec, float angle)
{
float c = Math.Trigonometry.Cos(angle);
float s = Math.Trigonometry.Sin(angle);
float t = 1 - c;
Matrix4 m =
new Matrix4(t * vec.X * vec.X + c, t * vec.X * vec.Y + s * vec.Z, t * vec.X * vec.Z + s * vec.Y, 0,
t * vec.X * vec.Y - s * vec.Z, t * vec.Y * vec.Y + c, t * vec.Y * vec.Z + s * vec.X, 0,
t * vec.X * vec.Z + s * vec.Y, t * vec.Y * vec.Z - s * vec.X, t * vec.Z * vec.Z + c, 0,
0, 0, 0, 1);
return Matrix4.Transpose(m);
}
/// <summary>
/// Spherical linear interpolation between to matrices.
/// </summary>
/// <param name="a">First matrix.</param>
/// <param name="b">Second matrix.</param>
/// <param name="time">Interpolation time [0..1].</param>
/// <returns>Interpolated matrix.</returns>
public static Matrix4 Slerp(Matrix4 a, Matrix4 b, float time)
{
Quaternion qa = a.Quaternion;
Quaternion qb = b.Quaternion;
Quaternion interpolated = Quaternion.Slerp(qa, qb, time);
Matrix4 result = interpolated.Matrix4;
result.TranslationVector = Vector3.Lerp(a.TranslationVector, b.TranslationVector, time);
return result;
}
/// <summary>
/// get 3x3 matrix from 4x4 matrix without specified column and row
/// </summary>
/// <param name="source">source matrix (4x4)</param>
/// <param name="column">column to remove</param>
/// <param name="row">row to remove</param>
/// <returns>3x3 matrix from 4x4 matrix without specified column and row</returns>
public static Matrix3 Minor(Matrix4 source, int column, int row)
{
int r = 0;
Matrix3 result = new Matrix3();
for (int iRow = 0; iRow < 4; iRow++)
{
int c = 0;
if (iRow != row)
{
for (int iColumn = 0; iColumn < 4; iColumn++)
{
if (iColumn != column)
{
result[c, r] = source[iColumn, iRow];
c++;
}
}
r++;
}
}
return result;
}
/// <summary>
/// calculates the inverse matrix of a source matrix
/// </summary>
/// <param name="m">to calculate inverse matrix from</param>
/// <returns>inverse matrix</returns>
public static Matrix4 Invert(Matrix4 m)
{
// not that efficient at the moment - however, who cares?
//Matrix4 inverse = Matrix4.Adjoint(m) / m.Det();
// Hmm perhaps I do care ;-) This is about 50% faster
Matrix4 inverse = Matrix4.Zero;
inverse.A1 = m.B3 * m.C4 * m.D2 - m.B4 * m.C3 * m.D2 + m.B4 * m.C2 * m.D3 - m.B2 * m.C4 * m.D3 - m.B3 * m.C2 * m.D4 + m.B2 * m.C3 * m.D4;
inverse.A2 = m.A4 * m.C3 * m.D2 - m.A3 * m.C4 * m.D2 - m.A4 * m.C2 * m.D3 + m.A2 * m.C4 * m.D3 + m.A3 * m.C2 * m.D4 - m.A2 * m.C3 * m.D4;
inverse.A3 = m.A3 * m.B4 * m.D2 - m.A4 * m.B3 * m.D2 + m.A4 * m.B2 * m.D3 - m.A2 * m.B4 * m.D3 - m.A3 * m.B2 * m.D4 + m.A2 * m.B3 * m.D4;
inverse.A4 = m.A4 * m.B3 * m.C2 - m.A3 * m.B4 * m.C2 - m.A4 * m.B2 * m.C3 + m.A2 * m.B4 * m.C3 + m.A3 * m.B2 * m.C4 - m.A2 * m.B3 * m.C4;
inverse.B1 = m.B4 * m.C3 * m.D1 - m.B3 * m.C4 * m.D1 - m.B4 * m.C1 * m.D3 + m.B1 * m.C4 * m.D3 + m.B3 * m.C1 * m.D4 - m.B1 * m.C3 * m.D4;
inverse.B2 = m.A3 * m.C4 * m.D1 - m.A4 * m.C3 * m.D1 + m.A4 * m.C1 * m.D3 - m.A1 * m.C4 * m.D3 - m.A3 * m.C1 * m.D4 + m.A1 * m.C3 * m.D4;
inverse.B3 = m.A4 * m.B3 * m.D1 - m.A3 * m.B4 * m.D1 - m.A4 * m.B1 * m.D3 + m.A1 * m.B4 * m.D3 + m.A3 * m.B1 * m.D4 - m.A1 * m.B3 * m.D4;
inverse.B4 = m.A3 * m.B4 * m.C1 - m.A4 * m.B3 * m.C1 + m.A4 * m.B1 * m.C3 - m.A1 * m.B4 * m.C3 - m.A3 * m.B1 * m.C4 + m.A1 * m.B3 * m.C4;
inverse.C1 = m.B2 * m.C4 * m.D1 - m.B4 * m.C2 * m.D1 + m.B4 * m.C1 * m.D2 - m.B1 * m.C4 * m.D2 - m.B2 * m.C1 * m.D4 + m.B1 * m.C2 * m.D4;
inverse.C2 = m.A4 * m.C2 * m.D1 - m.A2 * m.C4 * m.D1 - m.A4 * m.C1 * m.D2 + m.A1 * m.C4 * m.D2 + m.A2 * m.C1 * m.D4 - m.A1 * m.C2 * m.D4;
inverse.C3 = m.A2 * m.B4 * m.D1 - m.A4 * m.B2 * m.D1 + m.A4 * m.B1 * m.D2 - m.A1 * m.B4 * m.D2 - m.A2 * m.B1 * m.D4 + m.A1 * m.B2 * m.D4;
inverse.C4 = m.A4 * m.B2 * m.C1 - m.A2 * m.B4 * m.C1 - m.A4 * m.B1 * m.C2 + m.A1 * m.B4 * m.C2 + m.A2 * m.B1 * m.C4 - m.A1 * m.B2 * m.C4;
inverse.D1 = m.B3 * m.C2 * m.D1 - m.B2 * m.C3 * m.D1 - m.B3 * m.C1 * m.D2 + m.B1 * m.C3 * m.D2 + m.B2 * m.C1 * m.D3 - m.B1 * m.C2 * m.D3;
inverse.D2 = m.A2 * m.C3 * m.D1 - m.A3 * m.C2 * m.D1 + m.A3 * m.C1 * m.D2 - m.A1 * m.C3 * m.D2 - m.A2 * m.C1 * m.D3 + m.A1 * m.C2 * m.D3;
inverse.D3 = m.A3 * m.B2 * m.D1 - m.A2 * m.B3 * m.D1 - m.A3 * m.B1 * m.D2 + m.A1 * m.B3 * m.D2 + m.A2 * m.B1 * m.D3 - m.A1 * m.B2 * m.D3;
inverse.D4 = m.A2 * m.B3 * m.C1 - m.A3 * m.B2 * m.C1 + m.A3 * m.B1 * m.C2 - m.A1 * m.B3 * m.C2 - m.A2 * m.B1 * m.C3 + m.A1 * m.B2 * m.C3;
return inverse * (1 / m.Det());
}
/// <summary>
/// Returns an array of matrices.
/// </summary>
/// <param name="data">The data stream containing the matrices.</param>
/// <param name="offset">The offset in the stream to start with.</param>
/// <param name="count">The number of matrices to load.</param>
/// <returns>The array of matrices.</returns>
public static unsafe Matrix4[] From(byte[] data, int offset, int count)
{
Matrix4[] ret = new Matrix4[count];
fixed (byte* p = &data[offset])
{
float* floatPtr = (float*)(p);
for (int i = 0; i < count; i++)
{
ret[i] = new Matrix4(
floatPtr[0], floatPtr[1], floatPtr[2], floatPtr[3],
floatPtr[4], floatPtr[5], floatPtr[6], floatPtr[7],
floatPtr[8], floatPtr[9], floatPtr[10], floatPtr[11],
floatPtr[12], floatPtr[13], floatPtr[14], floatPtr[15]);
floatPtr += 16;
}
}
return ret;
}
/// <summary>
/// returns the adjoint matrix
/// </summary>
/// <param name="source">matrix to calculate adjoint matrix from</param>
/// <returns></returns>
public static Matrix4 Adjoint(Matrix4 source)
{
Matrix4 result = Matrix4.Zero;
for (int iRow = 0; iRow < 4; iRow++)
for (int iCol = 0; iCol < 4; iCol++)
{
if (((iCol + iRow) % 2) == 0)
result[iCol, iRow] = Matrix4.Minor(source, iRow, iCol).Det();
else
result[iCol, iRow] = -Matrix4.Minor(source, iRow, iCol).Det();
}
return result;
}
/// <summary>
/// returns the transposed matrix
/// </summary>
/// <param name="matrix">matrix to transpose</param>
/// <returns>the transposed matrix</returns>
public static Matrix4 Transpose(Matrix4 matrix)
{
Matrix4 m = Matrix4.Zero;
m.A1 = matrix.A1; m.A2 = matrix.B1; m.A3 = matrix.C1; m.A4 = matrix.D1;
m.B1 = matrix.A2; m.B2 = matrix.B2; m.B3 = matrix.C2; m.B4 = matrix.D2;
m.C1 = matrix.A3; m.C2 = matrix.B3; m.C3 = matrix.C3; m.C4 = matrix.D3;
m.D1 = matrix.A4; m.D2 = matrix.B4; m.D3 = matrix.C4; m.D4 = matrix.D4;
return m;
}
