/// <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 FPQuaternion quaternion, out FPMatrix4x4 result)
{
// Precalculate coordinate products
FP x = quaternion.x * 2;
FP y = quaternion.y * 2;
FP z = quaternion.z * 2;
FP xx = quaternion.x * x;
FP yy = quaternion.y * y;
FP zz = quaternion.z * z;
FP xy = quaternion.x * y;
FP xz = quaternion.x * z;
FP yz = quaternion.y * z;
FP wx = quaternion.w * x;
FP wy = quaternion.w * y;
FP wz = quaternion.w * z;
// Calculate 3x3 matrix from orthonormal basis
result.M11 = FP.One - (yy + zz);
result.M21 = xy + wz;
result.M31 = xz - wy;
result.M41 = FP.Zero;
result.M12 = xy - wz;
result.M22 = FP.One - (xx + zz);
result.M32 = yz + wx;
result.M42 = FP.Zero;
result.M13 = xz + wy;
result.M23 = yz - wx;
result.M33 = FP.One - (xx + yy);
result.M43 = FP.Zero;
result.M14 = FP.Zero;
result.M24 = FP.Zero;
result.M34 = FP.Zero;
result.M44 = FP.One;
}
/// <summary>
/// Creates the transposed matrix.
/// </summary>
/// <param name="matrix">The matrix which should be transposed.</param>
/// <returns>The transposed JMatrix.</returns>
public static FPMatrix4x4 Transpose(FPMatrix4x4 matrix)
{
FPMatrix4x4 result;
FPMatrix4x4.Transpose(ref matrix, 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 FPMatrix4x4 operator +(FPMatrix4x4 value1, FPMatrix4x4 value2)
{
FPMatrix4x4 result;
FPMatrix4x4.Add(ref value1, ref value2, out result);
return(result);
}
public static FPMatrix4x4 Rotate(FPQuaternion quaternion)
{
FPMatrix4x4 result;
FPMatrix4x4.Rotate(ref quaternion, out result);
return(result);
}
public override bool Equals(object obj)
{
if (!(obj is FPMatrix4x4))
{
return(false);
}
FPMatrix4x4 other = (FPMatrix4x4)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>
/// 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 FPMatrix4x4 Multiply(FPMatrix4x4 matrix1, FP scaleFactor)
{
FPMatrix4x4 result;
FPMatrix4x4.Multiply(ref matrix1, scaleFactor, out result);
return(result);
}
/// <summary>
/// Calculates the inverse of a give matrix.
/// </summary>
/// <param name="matrix">The matrix to invert.</param>
/// <returns>The inverted JMatrix.</returns>
public static FPMatrix4x4 Inverse(FPMatrix4x4 matrix)
{
FPMatrix4x4 result;
FPMatrix4x4.Inverse(ref matrix, out result);
return(result);
}
/// <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 FPMatrix4x4 Add(FPMatrix4x4 matrix1, FPMatrix4x4 matrix2)
{
FPMatrix4x4 result;
FPMatrix4x4.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>
/// <param name="result">The product of both matrices.</param>
public static void Multiply(ref FPMatrix4x4 matrix1, ref FPMatrix4x4 matrix2, out FPMatrix4x4 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>
/// 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 FPMatrix4x4 Multiply(FPMatrix4x4 matrix1, FPMatrix4x4 matrix2)
{
FPMatrix4x4 result;
FPMatrix4x4.Multiply(ref matrix1, ref matrix2, out result);
return(result);
}
/// <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 FPMatrix4x4 operator *(FPMatrix4x4 value1, FPMatrix4x4 value2)
{
FPMatrix4x4 result;
FPMatrix4x4.Multiply(ref value1, ref value2, out result);
return(result);
}
/// <summary>
/// Subtracts two matrices.
/// </summary>
/// <param name="value1">The first matrix.</param>
/// <param name="value2">The second matrix.</param>
/// <returns>The difference of both values.</returns>
public static FPMatrix4x4 operator -(FPMatrix4x4 value1, FPMatrix4x4 value2)
{
FPMatrix4x4 result;
FPMatrix4x4.Multiply(ref value2, -FP.One, out value2);
FPMatrix4x4.Add(ref value1, ref value2, out result);
return(result);
}
static FPMatrix4x4()
{
Zero = new FPMatrix4x4();
Identity = new FPMatrix4x4();
Identity.M11 = FP.One;
Identity.M22 = FP.One;
Identity.M33 = FP.One;
Identity.M44 = FP.One;
InternalIdentity = Identity;
}
/// <summary>
/// Creates a matrix which rotates around the given axis by the given angle.
/// </summary>
/// <param name="axis">The axis.</param>
/// <param name="angle">The angle.</param>
/// <param name="result">The resulting rotation matrix</param>
public static void AxisAngle(ref FPVector axis, FP angle, out FPMatrix4x4 result)
{
// a: angle
// x, y, z: unit vector for axis.
//
// Rotation matrix M can compute by using below equation.
//
// T T
// M = uu + (cos a)( I-uu ) + (sin a)S
//
// Where:
//
// u = ( x, y, z )
//
// [ 0 -z y ]
// S = [ z 0 -x ]
// [ -y x 0 ]
//
// [ 1 0 0 ]
// I = [ 0 1 0 ]
// [ 0 0 1 ]
//
//
// [ xx+cosa*(1-xx) yx-cosa*yx-sina*z zx-cosa*xz+sina*y ]
// M = [ xy-cosa*yx+sina*z yy+cosa(1-yy) yz-cosa*yz-sina*x ]
// [ zx-cosa*zx-sina*y zy-cosa*zy+sina*x zz+cosa*(1-zz) ]
//
FP x = axis.x, y = axis.y, z = axis.z;
FP sa = FPMath.Sin(angle), ca = FPMath.Cos(angle);
FP xx = x * x, yy = y * y, zz = z * z;
FP xy = x * y, xz = x * z, yz = y * z;
result.M11 = xx + ca * (FP.One - xx);
result.M12 = xy - ca * xy + sa * z;
result.M13 = xz - ca * xz - sa * y;
result.M14 = FP.Zero;
result.M21 = xy - ca * xy - sa * z;
result.M22 = yy + ca * (FP.One - yy);
result.M23 = yz - ca * yz + sa * x;
result.M24 = FP.Zero;
result.M31 = xz - ca * xz + sa * y;
result.M32 = yz - ca * yz - sa * x;
result.M33 = zz + ca * (FP.One - zz);
result.M34 = FP.Zero;
result.M41 = FP.Zero;
result.M42 = FP.Zero;
result.M43 = FP.Zero;
result.M44 = FP.One;
}
/// <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 FPMatrix4x4 matrix, out FPMatrix4x4 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 FPMatrix4x4 matrix1, ref FPMatrix4x4 matrix2, out FPMatrix4x4 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 FPMatrix4x4 matrix1, FP scaleFactor, out FPMatrix4x4 result)
{
FP 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;
}
/// <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 FPMatrix4x4 matrix, out FPMatrix4x4 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 ( fp - 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 ( fp - 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.
FP a = matrix.M11, b = matrix.M12, c = matrix.M13, d = matrix.M14;
FP e = matrix.M21, f = matrix.M22, g = matrix.M23, h = matrix.M24;
FP i = matrix.M31, j = matrix.M32, k = matrix.M33, l = matrix.M34;
FP m = matrix.M41, n = matrix.M42, o = matrix.M43, p = matrix.M44;
FP kp_lo = k * p - l * o;
FP jp_ln = j * p - l * n;
FP jo_kn = j * o - k * n;
FP ip_lm = i * p - l * m;
FP io_km = i * o - k * m;
FP in_jm = i * n - j * m;
FP a11 = (f * kp_lo - g * jp_ln + h * jo_kn);
FP a12 = -(e * kp_lo - g * ip_lm + h * io_km);
FP a13 = (e * jp_ln - f * ip_lm + h * in_jm);
FP a14 = -(e * jo_kn - f * io_km + g * in_jm);
FP det = a * a11 + b * a12 + c * a13 + d * a14;
if (det == FP.Zero)
{
result.M11 = FP.PositiveInfinity;
result.M12 = FP.PositiveInfinity;
result.M13 = FP.PositiveInfinity;
result.M14 = FP.PositiveInfinity;
result.M21 = FP.PositiveInfinity;
result.M22 = FP.PositiveInfinity;
result.M23 = FP.PositiveInfinity;
result.M24 = FP.PositiveInfinity;
result.M31 = FP.PositiveInfinity;
result.M32 = FP.PositiveInfinity;
result.M33 = FP.PositiveInfinity;
result.M34 = FP.PositiveInfinity;
result.M41 = FP.PositiveInfinity;
result.M42 = FP.PositiveInfinity;
result.M43 = FP.PositiveInfinity;
result.M44 = FP.PositiveInfinity;
}
else
{
FP invDet = FP.One / 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;
FP gp_ho = g * p - h * o;
FP fp_hn = f * p - h * n;
FP fo_gn = f * o - g * n;
FP ep_hm = e * p - h * m;
FP eo_gm = e * o - g * m;
FP en_fm = e * n - f * m;
result.M13 = (b * gp_ho - c * fp_hn + d * fo_gn) * invDet;
result.M23 = -(a * gp_ho - c * ep_hm + d * eo_gm) * invDet;
result.M33 = (a * fp_hn - b * ep_hm + d * en_fm) * invDet;
result.M43 = -(a * fo_gn - b * eo_gm + c * en_fm) * invDet;
FP gl_hk = g * l - h * k;
FP fl_hj = f * l - h * j;
FP fk_gj = f * k - g * j;
FP el_hi = e * l - h * i;
FP ek_gi = e * k - g * i;
FP 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;
}
}
public static void TRS(FPVector translation, FPQuaternion rotation, FPVector scale, out FPMatrix4x4 matrix)
{
matrix = FPMatrix4x4.Translate(translation) * FPMatrix4x4.Rotate(rotation) * FPMatrix4x4.Scale(scale);
}
