/// <summary>
/// Multiply two matrices. Notice: matrix multiplication is not commutative.
/// </summary>
/// <param name="matrix1">The first matrix.</param>
/// <param name="matrix2">The second matrix.</param>
/// <param name="result">The product of both matrices.</param>
public static void Multiply(ref VMatrix4x4 matrix1, ref VMatrix4x4 matrix2, out VMatrix4x4 result)
{
// First row
result.M11 = matrix1.M11 * matrix2.M11 + matrix1.M12 * matrix2.M21 + matrix1.M13 * matrix2.M31 + matrix1.M14 * matrix2.M41;
result.M12 = matrix1.M11 * matrix2.M12 + matrix1.M12 * matrix2.M22 + matrix1.M13 * matrix2.M32 + matrix1.M14 * matrix2.M42;
result.M13 = matrix1.M11 * matrix2.M13 + matrix1.M12 * matrix2.M23 + matrix1.M13 * matrix2.M33 + matrix1.M14 * matrix2.M43;
result.M14 = matrix1.M11 * matrix2.M14 + matrix1.M12 * matrix2.M24 + matrix1.M13 * matrix2.M34 + matrix1.M14 * matrix2.M44;
// Second row
result.M21 = matrix1.M21 * matrix2.M11 + matrix1.M22 * matrix2.M21 + matrix1.M23 * matrix2.M31 + matrix1.M24 * matrix2.M41;
result.M22 = matrix1.M21 * matrix2.M12 + matrix1.M22 * matrix2.M22 + matrix1.M23 * matrix2.M32 + matrix1.M24 * matrix2.M42;
result.M23 = matrix1.M21 * matrix2.M13 + matrix1.M22 * matrix2.M23 + matrix1.M23 * matrix2.M33 + matrix1.M24 * matrix2.M43;
result.M24 = matrix1.M21 * matrix2.M14 + matrix1.M22 * matrix2.M24 + matrix1.M23 * matrix2.M34 + matrix1.M24 * matrix2.M44;
// Third row
result.M31 = matrix1.M31 * matrix2.M11 + matrix1.M32 * matrix2.M21 + matrix1.M33 * matrix2.M31 + matrix1.M34 * matrix2.M41;
result.M32 = matrix1.M31 * matrix2.M12 + matrix1.M32 * matrix2.M22 + matrix1.M33 * matrix2.M32 + matrix1.M34 * matrix2.M42;
result.M33 = matrix1.M31 * matrix2.M13 + matrix1.M32 * matrix2.M23 + matrix1.M33 * matrix2.M33 + matrix1.M34 * matrix2.M43;
result.M34 = matrix1.M31 * matrix2.M14 + matrix1.M32 * matrix2.M24 + matrix1.M33 * matrix2.M34 + matrix1.M34 * matrix2.M44;
// Fourth row
result.M41 = matrix1.M41 * matrix2.M11 + matrix1.M42 * matrix2.M21 + matrix1.M43 * matrix2.M31 + matrix1.M44 * matrix2.M41;
result.M42 = matrix1.M41 * matrix2.M12 + matrix1.M42 * matrix2.M22 + matrix1.M43 * matrix2.M32 + matrix1.M44 * matrix2.M42;
result.M43 = matrix1.M41 * matrix2.M13 + matrix1.M42 * matrix2.M23 + matrix1.M43 * matrix2.M33 + matrix1.M44 * matrix2.M43;
result.M44 = matrix1.M41 * matrix2.M14 + matrix1.M42 * matrix2.M24 + matrix1.M43 * matrix2.M34 + matrix1.M44 * matrix2.M44;
}
/// <summary>
/// Matrices are added.
/// </summary>
/// <param name="matrix1">The first matrix.</param>
/// <param name="matrix2">The second matrix.</param>
/// <returns>The sum of both matrices.</returns>
public static VMatrix4x4 Add(VMatrix4x4 matrix1, VMatrix4x4 matrix2)
{
VMatrix4x4 result;
VMatrix4x4.Add(ref matrix1, ref matrix2, out result);
return(result);
}
/// <summary>
/// Multiply two matrices. Notice: matrix multiplication is not commutative.
/// </summary>
/// <param name="matrix1">The first matrix.</param>
/// <param name="matrix2">The second matrix.</param>
/// <returns>The product of both matrices.</returns>
public static VMatrix4x4 Multiply(VMatrix4x4 matrix1, VMatrix4x4 matrix2)
{
VMatrix4x4 result;
VMatrix4x4.Multiply(ref matrix1, ref matrix2, out result);
return(result);
}
/// <summary>
/// Multiply a matrix by a scalefactor.
/// </summary>
/// <param name="matrix1">The matrix.</param>
/// <param name="scaleFactor">The scale factor.</param>
/// <returns>A JMatrix multiplied by the scale factor.</returns>
public static VMatrix4x4 Multiply(VMatrix4x4 matrix1, long scaleFactor)
{
VMatrix4x4 result;
VMatrix4x4.Multiply(ref matrix1, scaleFactor, out result);
return(result);
}
public override bool Equals(object obj)
{
if (!(obj is VMatrix4x4))
{
return(false);
}
VMatrix4x4 other = (VMatrix4x4)obj;
return(this.M11 == other.M11 &&
this.M12 == other.M12 &&
this.M13 == other.M13 &&
this.M14 == other.M14 &&
this.M21 == other.M21 &&
this.M22 == other.M22 &&
this.M23 == other.M23 &&
this.M24 == other.M24 &&
this.M31 == other.M31 &&
this.M32 == other.M32 &&
this.M33 == other.M33 &&
this.M34 == other.M44 &&
this.M41 == other.M41 &&
this.M42 == other.M42 &&
this.M43 == other.M43 &&
this.M44 == other.M44);
}
/// <summary>
/// Multiplies two matrices.
/// </summary>
/// <param name="value1">The first matrix.</param>
/// <param name="value2">The second matrix.</param>
/// <returns>The product of both values.</returns>
public static VMatrix4x4 operator *(VMatrix4x4 value1, VMatrix4x4 value2)
{
VMatrix4x4 result;
VMatrix4x4.Multiply(ref value1, ref value2, out result);
return(result);
}
/// <summary>
/// Adds two matrices.
/// </summary>
/// <param name="value1">The first matrix.</param>
/// <param name="value2">The second matrix.</param>
/// <returns>The sum of both values.</returns>
public static VMatrix4x4 operator +(VMatrix4x4 value1, VMatrix4x4 value2)
{
VMatrix4x4 result;
VMatrix4x4.Add(ref value1, ref value2, out result);
return(result);
}
/// <summary>
/// Creates the transposed matrix.
/// </summary>
/// <param name="matrix">The matrix which should be transposed.</param>
/// <returns>The transposed JMatrix.</returns>
public static VMatrix4x4 Transpose(VMatrix4x4 matrix)
{
VMatrix4x4 result;
VMatrix4x4.Transpose(ref matrix, out result);
return(result);
}
public static VMatrix4x4 Rotate(VQuaternion quaternion)
{
VMatrix4x4 result;
VMatrix4x4.Rotate(ref quaternion, out result);
return(result);
}
/// <summary>
/// Creates a JMatrix representing an orientation from a quaternion.
/// </summary>
/// <param name="quaternion">The quaternion the matrix should be created from.</param>
/// <param name="result">JMatrix representing an orientation.</param>
public static void Rotate(ref VQuaternion quaternion, out VMatrix4x4 result)
{
// Precalculate coordinate products
long x = quaternion.x * 2;
long y = quaternion.y * 2;
long z = quaternion.z * 2;
long xx = quaternion.x * x;
long yy = quaternion.y * y;
long zz = quaternion.z * z;
long xy = quaternion.x * y;
long xz = quaternion.x * z;
long yz = quaternion.y * z;
long wx = quaternion.w * x;
long wy = quaternion.w * y;
long wz = quaternion.w * z;
// Calculate 3x3 matrix from orthonormal basis
result.M11 = 1 - (yy + zz);
result.M21 = xy + wz;
result.M31 = xz - wy;
result.M41 = 0;
result.M12 = xy - wz;
result.M22 = 1 - (xx + zz);
result.M32 = yz + wx;
result.M42 = 0;
result.M13 = xz + wy;
result.M23 = yz - wx;
result.M33 = 1 - (xx + yy);
result.M43 = 0;
result.M14 = 0;
result.M24 = 0;
result.M34 = 0;
result.M44 = 1;
}
/// <summary>
/// Calculates the inverse of a give matrix.
/// </summary>
/// <param name="matrix">The matrix to invert.</param>
/// <returns>The inverted JMatrix.</returns>
public static VMatrix4x4 Inverse(VMatrix4x4 matrix)
{
VMatrix4x4 result;
VMatrix4x4.Inverse(ref matrix, out result);
return(result);
}
static VMatrix4x4()
{
Zero = new VMatrix4x4();
Identity = new VMatrix4x4();
Identity.M11 = 1;
Identity.M22 = 1;
Identity.M33 = 1;
Identity.M44 = 1;
InternalIdentity = Identity;
}
/// <summary>
/// Creates the transposed matrix.
/// </summary>
/// <param name="matrix">The matrix which should be transposed.</param>
/// <param name="result">The transposed JMatrix.</param>
public static void Transpose(ref VMatrix4x4 matrix, out VMatrix4x4 result)
{
result.M11 = matrix.M11;
result.M12 = matrix.M21;
result.M13 = matrix.M31;
result.M14 = matrix.M41;
result.M21 = matrix.M12;
result.M22 = matrix.M22;
result.M23 = matrix.M32;
result.M24 = matrix.M42;
result.M31 = matrix.M13;
result.M32 = matrix.M23;
result.M33 = matrix.M33;
result.M34 = matrix.M43;
result.M41 = matrix.M14;
result.M42 = matrix.M24;
result.M43 = matrix.M34;
result.M44 = matrix.M44;
}
/// <summary>
/// Matrices are added.
/// </summary>
/// <param name="matrix1">The first matrix.</param>
/// <param name="matrix2">The second matrix.</param>
/// <param name="result">The sum of both matrices.</param>
public static void Add(ref VMatrix4x4 matrix1, ref VMatrix4x4 matrix2, out VMatrix4x4 result)
{
result.M11 = matrix1.M11 + matrix2.M11;
result.M12 = matrix1.M12 + matrix2.M12;
result.M13 = matrix1.M13 + matrix2.M13;
result.M14 = matrix1.M14 + matrix2.M14;
result.M21 = matrix1.M21 + matrix2.M21;
result.M22 = matrix1.M22 + matrix2.M22;
result.M23 = matrix1.M23 + matrix2.M23;
result.M24 = matrix1.M24 + matrix2.M24;
result.M31 = matrix1.M31 + matrix2.M31;
result.M32 = matrix1.M32 + matrix2.M32;
result.M33 = matrix1.M33 + matrix2.M33;
result.M34 = matrix1.M34 + matrix2.M34;
result.M41 = matrix1.M41 + matrix2.M41;
result.M42 = matrix1.M42 + matrix2.M42;
result.M43 = matrix1.M43 + matrix2.M43;
result.M44 = matrix1.M44 + matrix2.M44;
}
/// <summary>
/// Multiply a matrix by a scalefactor.
/// </summary>
/// <param name="matrix1">The matrix.</param>
/// <param name="scaleFactor">The scale factor.</param>
/// <param name="result">A JMatrix multiplied by the scale factor.</param>
public static void Multiply(ref VMatrix4x4 matrix1, long scaleFactor, out VMatrix4x4 result)
{
long num = scaleFactor;
result.M11 = matrix1.M11 * num;
result.M12 = matrix1.M12 * num;
result.M13 = matrix1.M13 * num;
result.M14 = matrix1.M14 * num;
result.M21 = matrix1.M21 * num;
result.M22 = matrix1.M22 * num;
result.M23 = matrix1.M23 * num;
result.M24 = matrix1.M24 * num;
result.M31 = matrix1.M31 * num;
result.M32 = matrix1.M32 * num;
result.M33 = matrix1.M33 * num;
result.M34 = matrix1.M34 * num;
result.M41 = matrix1.M41 * num;
result.M42 = matrix1.M42 * num;
result.M43 = matrix1.M43 * num;
result.M44 = matrix1.M44 * num;
}
public static void TRS(VInt3 translation, VQuaternion rotation, VInt3 scale, out VMatrix4x4 matrix)
{
matrix = VMatrix4x4.Translate(translation) * VMatrix4x4.Rotate(rotation) * VMatrix4x4.Scale(scale);
}
/// <summary>
/// Calculates the inverse of a give matrix.
/// </summary>
/// <param name="matrix">The matrix to invert.</param>
/// <param name="result">The inverted JMatrix.</param>
public static void Inverse(ref VMatrix4x4 matrix, out VMatrix4x4 result)
{
// -1
// If you have matrix M, inverse Matrix M can compute
//
// -1 1
// M = --------- A
// det(M)
//
// A is adjugate (adjoint) of M, where,
//
// T
// A = C
//
// C is Cofactor matrix of M, where,
// i + j
// C = (-1) * det(M )
// ij ij
//
// [ a b c d ]
// M = [ e f g h ]
// [ i j k l ]
// [ m n o p ]
//
// First Row
// 2 | f g h |
// C = (-1) | j k l | = + ( f ( kp - lo ) - g ( jp - ln ) + h ( jo - kn ) )
// 11 | n o p |
//
// 3 | e g h |
// C = (-1) | i k l | = - ( e ( kp - lo ) - g ( ip - lm ) + h ( io - km ) )
// 12 | m o p |
//
// 4 | e f h |
// C = (-1) | i j l | = + ( e ( jp - ln ) - f ( ip - lm ) + h ( in - jm ) )
// 13 | m n p |
//
// 5 | e f g |
// C = (-1) | i j k | = - ( e ( jo - kn ) - f ( io - km ) + g ( in - jm ) )
// 14 | m n o |
//
// Second Row
// 3 | b c d |
// C = (-1) | j k l | = - ( b ( kp - lo ) - c ( jp - ln ) + d ( jo - kn ) )
// 21 | n o p |
//
// 4 | a c d |
// C = (-1) | i k l | = + ( a ( kp - lo ) - c ( ip - lm ) + d ( io - km ) )
// 22 | m o p |
//
// 5 | a b d |
// C = (-1) | i j l | = - ( a ( jp - ln ) - b ( ip - lm ) + d ( in - jm ) )
// 23 | m n p |
//
// 6 | a b c |
// C = (-1) | i j k | = + ( a ( jo - kn ) - b ( io - km ) + c ( in - jm ) )
// 24 | m n o |
//
// Third Row
// 4 | b c d |
// C = (-1) | f g h | = + ( b ( gp - ho ) - c ( long - hn ) + d ( fo - gn ) )
// 31 | n o p |
//
// 5 | a c d |
// C = (-1) | e g h | = - ( a ( gp - ho ) - c ( ep - hm ) + d ( eo - gm ) )
// 32 | m o p |
//
// 6 | a b d |
// C = (-1) | e f h | = + ( a ( long - hn ) - b ( ep - hm ) + d ( en - fm ) )
// 33 | m n p |
//
// 7 | a b c |
// C = (-1) | e f g | = - ( a ( fo - gn ) - b ( eo - gm ) + c ( en - fm ) )
// 34 | m n o |
//
// Fourth Row
// 5 | b c d |
// C = (-1) | f g h | = - ( b ( gl - hk ) - c ( fl - hj ) + d ( fk - gj ) )
// 41 | j k l |
//
// 6 | a c d |
// C = (-1) | e g h | = + ( a ( gl - hk ) - c ( el - hi ) + d ( ek - gi ) )
// 42 | i k l |
//
// 7 | a b d |
// C = (-1) | e f h | = - ( a ( fl - hj ) - b ( el - hi ) + d ( ej - fi ) )
// 43 | i j l |
//
// 8 | a b c |
// C = (-1) | e f g | = + ( a ( fk - gj ) - b ( ek - gi ) + c ( ej - fi ) )
// 44 | i j k |
//
// Cost of operation
// 53 adds, 104 muls, and 1 div.
long a = matrix.M11, b = matrix.M12, c = matrix.M13, d = matrix.M14;
long e = matrix.M21, f = matrix.M22, g = matrix.M23, h = matrix.M24;
long i = matrix.M31, j = matrix.M32, k = matrix.M33, l = matrix.M34;
long m = matrix.M41, n = matrix.M42, o = matrix.M43, p = matrix.M44;
long kp_lo = k * p - l * o;
long jp_ln = j * p - l * n;
long jo_kn = j * o - k * n;
long ip_lm = i * p - l * m;
long io_km = i * o - k * m;
long in_jm = i * n - j * m;
long a11 = (f * kp_lo - g * jp_ln + h * jo_kn);
long a12 = -(e * kp_lo - g * ip_lm + h * io_km);
long a13 = (e * jp_ln - f * ip_lm + h * in_jm);
long a14 = -(e * jo_kn - f * io_km + g * in_jm);
long det = a * a11 + b * a12 + c * a13 + d * a14;
if (det == 0)
{
result.M11 = long.MaxValue;
result.M12 = long.MaxValue;
result.M13 = long.MaxValue;
result.M14 = long.MaxValue;
result.M21 = long.MaxValue;
result.M22 = long.MaxValue;
result.M23 = long.MaxValue;
result.M24 = long.MaxValue;
result.M31 = long.MaxValue;
result.M32 = long.MaxValue;
result.M33 = long.MaxValue;
result.M34 = long.MaxValue;
result.M41 = long.MaxValue;
result.M42 = long.MaxValue;
result.M43 = long.MaxValue;
result.M44 = long.MaxValue;
}
else
{
long invDet = 1 / det;
result.M11 = a11 * invDet;
result.M21 = a12 * invDet;
result.M31 = a13 * invDet;
result.M41 = a14 * invDet;
result.M12 = -(b * kp_lo - c * jp_ln + d * jo_kn) * invDet;
result.M22 = (a * kp_lo - c * ip_lm + d * io_km) * invDet;
result.M32 = -(a * jp_ln - b * ip_lm + d * in_jm) * invDet;
result.M42 = (a * jo_kn - b * io_km + c * in_jm) * invDet;
long gp_ho = g * p - h * o;
long long_hn = f * p - h * n;
long fo_gn = f * o - g * n;
long ep_hm = e * p - h * m;
long eo_gm = e * o - g * m;
long en_fm = e * n - f * m;
result.M13 = (b * gp_ho - c * long_hn + d * fo_gn) * invDet;
result.M23 = -(a * gp_ho - c * ep_hm + d * eo_gm) * invDet;
result.M33 = (a * long_hn - b * ep_hm + d * en_fm) * invDet;
result.M43 = -(a * fo_gn - b * eo_gm + c * en_fm) * invDet;
long gl_hk = g * l - h * k;
long fl_hj = f * l - h * j;
long fk_gj = f * k - g * j;
long el_hi = e * l - h * i;
long ek_gi = e * k - g * i;
long ej_fi = e * j - f * i;
result.M14 = -(b * gl_hk - c * fl_hj + d * fk_gj) * invDet;
result.M24 = (a * gl_hk - c * el_hi + d * ek_gi) * invDet;
result.M34 = -(a * fl_hj - b * el_hi + d * ej_fi) * invDet;
result.M44 = (a * fk_gj - b * ek_gi + c * ej_fi) * invDet;
}
}
