public bool InverseSelf() { // 18+3+9 = 30 multiplications // 1 division idMat3 inverse = new idMat3(); idVec3[] inverse_mat = inverse.mat; inverse_mat[0].x = mat[1].y * mat[2].z - mat[1].z * mat[2].y; inverse_mat[1].x = mat[1].z * mat[2].x - mat[1].x * mat[2].z; inverse_mat[2].x = mat[1].x * mat[2].y - mat[1].y * mat[2].x; double det = mat[0].x * inverse_mat[0].x + mat[0].y * inverse_mat[1].x + mat[0].z * inverse_mat[2].x; if (idMath.Fabs(det) < MATRIX_INVERSE_EPSILON) { return(false); } double invDet = 1.0f / det; inverse_mat[0].y = mat[0].z * mat[2].y - mat[0].y * mat[2].z; inverse_mat[0].z = mat[0].y * mat[1].z - mat[0].z * mat[1].y; inverse_mat[1].y = mat[0].x * mat[2].z - mat[0].z * mat[2].x; inverse_mat[1].z = mat[0].z * mat[1].x - mat[0].x * mat[1].z; inverse_mat[2].y = mat[0].y * mat[2].x - mat[0].x * mat[2].y; inverse_mat[2].z = mat[0].x * mat[1].y - mat[0].y * mat[1].x; mat[0].x = (float)(inverse_mat[0].x * invDet); mat[0].y = (float)(inverse_mat[0].y * invDet); mat[0].z = (float)(inverse_mat[0].z * invDet); mat[1].x = (float)(inverse_mat[1].x * invDet); mat[1].y = (float)(inverse_mat[1].y * invDet); mat[1].z = (float)(inverse_mat[1].z * invDet); mat[2].x = (float)(inverse_mat[2].x * invDet); mat[2].y = (float)(inverse_mat[2].y * invDet); mat[2].z = (float)(inverse_mat[2].z * invDet); return(true); }
public bool Compare(ref idMat3 a, float epsilon) { return( mat[0].Compare(ref a.mat[0], epsilon) && mat[1].Compare(ref a.mat[1], epsilon) && mat[2].Compare(ref a.mat[2], epsilon)); }
public bool Compare(ref idMat3 a) { return( mat[0].Compare(ref a.mat[0]) && mat[1].Compare(ref a.mat[1]) && mat[2].Compare(ref a.mat[2])); }
public idMat3 ToMat3() { float x2 = x + x; float y2 = y + y; float z2 = z + z; float xx = x * x2; float xy = x * y2; float xz = x * z2; float yy = y * y2; float yz = y * z2; float zz = z * z2; float wx = w * x2; float wy = w * y2; float wz = w * z2; idMat3 mat = new idMat3(); idVec3[] mat_mat = mat.mat; mat_mat[0].x = 1.0f - (yy + zz); mat_mat[0].y = xy - wz; mat_mat[0].z = xz + wy; mat_mat[1].x = xy + wz; mat_mat[1].y = 1.0f - (xx + zz); mat_mat[1].z = yz - wx; mat_mat[2].x = xz - wy; mat_mat[2].y = yz + wx; mat_mat[2].z = 1.0f - (xx + yy); return(mat); }
public idMat3 opAdd(idMat3 a) { idVec3[] a_mat = a.mat; mat[0].x += a_mat[0].x; mat[0].y += a_mat[0].y; mat[0].z += a_mat[0].z; mat[1].x += a_mat[1].x; mat[1].y += a_mat[1].y; mat[1].z += a_mat[1].z; mat[2].x += a_mat[2].x; mat[2].y += a_mat[2].y; mat[2].z += a_mat[2].z; return(this); }
public idMat3 InverseFast() { idMat3 invMat = this; bool r = invMat.InverseFastSelf(); Debug.Assert(r); return(invMat); }
public idMat3 opSub(idMat3 a) { idVec3[] a_mat = a.mat; mat[0].x -= a_mat[0].x; mat[0].y -= a_mat[0].y; mat[0].z -= a_mat[0].z; mat[1].x -= a_mat[1].x; mat[1].y -= a_mat[1].y; mat[1].z -= a_mat[1].z; mat[2].x -= a_mat[2].x; mat[2].y -= a_mat[2].y; mat[2].z -= a_mat[2].z; return(this); }
void Set( ref idMat3 m1, ref idMat3 m2 ) { SetSize( 3, 6 ); for (int i = 0; i < 3; i++ ) { for (int j = 0; j < 3; j++ ) { mat[(i+0) * numColumns + (j+0)] = m1[i][j]; mat[(i+0) * numColumns + (j+3)] = m2[i][j]; } } }
public idMat6(ref idMat3 m0, ref idMat3 m1, ref idMat3 m2, ref idMat3 m3) { idVec3[] m0_mat = m0.mat; idVec3[] m1_mat = m1.mat; idVec3[] m2_mat = m2.mat; idVec3[] m3_mat = m3.mat; mat[0] = new idVec6(m0_mat[0].x, m0_mat[0].y, m0_mat[0].z, m1_mat[0].x, m1_mat[0].y, m1_mat[0].z); mat[1] = new idVec6(m0_mat[1].x, m0_mat[1].y, m0_mat[1].z, m1_mat[1].x, m1_mat[1].y, m1_mat[1].z); mat[2] = new idVec6(m0_mat[2].x, m0_mat[2].y, m0_mat[2].z, m1_mat[2].x, m1_mat[2].y, m1_mat[2].z); mat[3] = new idVec6(m2_mat[0].x, m2_mat[0].y, m2_mat[0].z, m3_mat[0].x, m3_mat[0].y, m3_mat[0].z); mat[4] = new idVec6(m2_mat[1].x, m2_mat[1].y, m2_mat[1].z, m3_mat[1].x, m3_mat[1].y, m3_mat[1].z); mat[5] = new idVec6(m2_mat[2].x, m2_mat[2].y, m2_mat[2].z, m3_mat[2].x, m3_mat[2].y, m3_mat[2].z); }
public idMat3 OrthoNormalize() { idMat3 ortho = this; ortho.mat[0].Normalize(); ortho.mat[2].Cross(mat[0], mat[1]); ortho.mat[2].Normalize(); ortho.mat[1].Cross(mat[2], mat[0]); ortho.mat[1].Normalize(); return(ortho); }
// extra public idVec3 opMul(ref idVec3 vec, ref idMat3 mat) { idVec3[] mat_mat = mat.mat; float x = mat_mat[0].x * vec.x + mat_mat[1].x * vec.y + mat_mat[2].x * vec.z; float y = mat_mat[0].y * vec.x + mat_mat[1].y * vec.y + mat_mat[2].y * vec.z; vec.z = mat_mat[0].z * vec.x + mat_mat[1].z * vec.y + mat_mat[2].z * vec.z; vec.x = x; vec.y = y; return(vec); }
void Set(ref idMat3 m1, ref idMat3 m2) { SetSize(3, 6); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { mat[(i + 0) * numColumns + (j + 0)] = m1[i][j]; mat[(i + 0) * numColumns + (j + 3)] = m2[i][j]; } } }
public idMat3 ToMat3() { float sr, sp, sy, cr, cp, cy; idMath.SinCos(DEG2RAD(yaw), out sy, out cy); idMath.SinCos(DEG2RAD(pitch), out sp, out cp); idMath.SinCos(DEG2RAD(roll), out sr, out cr); idMat3 mat = new idMat3(); idVec3[] mat_mat = mat.mat; mat_mat[0].Set(cp * cy, cp * sy, -sp); mat_mat[1].Set(sr * sp * cy + cr * -sy, sr * sp * sy + cr * cy, sr * cp); mat_mat[2].Set(cr * sp * cy + -sr * -sy, cr * sp * sy + -sr * cy, cr * cp); return(mat); }
public idMat3 opMul(idMat3 a) { //m1Ptr = reinterpret_cast<float *>(this); //m2Ptr = reinterpret_cast<const float *>(&a); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { // dst[j] = m1Ptr[0] * m2Ptr[ 0 * 3 + j ] // + m1Ptr[1] * m2Ptr[ 1 * 3 + j ] // + m1Ptr[2] * m2Ptr[ 2 * 3 + j ]; } // m1Ptr[0] = dst[0]; m1Ptr[1] = dst[1]; m1Ptr[2] = dst[2]; // m1Ptr += 3; } return(this); }
public static idMat3 operator *(idMat3 a, idMat3 b) { //m1Ptr = reinterpret_cast<const float *>(this); //m2Ptr = reinterpret_cast<const float *>(&a); //dstPtr = reinterpret_cast<float *>(&dst); idMat3 dst = new idMat3(); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { // *dstPtr = m1Ptr[0] * m2Ptr[ 0 * 3 + j ] // + m1Ptr[1] * m2Ptr[ 1 * 3 + j ] // + m1Ptr[2] * m2Ptr[ 2 * 3 + j ]; // dstPtr++; } //m1Ptr += 3; } return dst; }
public static idMat3 operator *(idMat3 a, idMat3 b) { //m1Ptr = reinterpret_cast<const float *>(this); //m2Ptr = reinterpret_cast<const float *>(&a); //dstPtr = reinterpret_cast<float *>(&dst); idMat3 dst = new idMat3(); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { // *dstPtr = m1Ptr[0] * m2Ptr[ 0 * 3 + j ] // + m1Ptr[1] * m2Ptr[ 1 * 3 + j ] // + m1Ptr[2] * m2Ptr[ 2 * 3 + j ]; // dstPtr++; } //m1Ptr += 3; } return(dst); }
public idMat4(ref idMat3 rotation, ref idVec3 translation) { idVec3[] rotation_mat = rotation.mat; // NOTE: idMat3 is transposed because it is column-major mat[0].x = rotation_mat[0].x; mat[0].y = rotation_mat[1].x; mat[0].z = rotation_mat[2].x; mat[0].w = translation.x; mat[1].x = rotation_mat[0].y; mat[1].y = rotation_mat[1].y; mat[1].z = rotation_mat[2].y; mat[1].w = translation.y; mat[2].x = rotation_mat[0].z; mat[2].y = rotation_mat[1].z; mat[2].z = rotation_mat[2].z; mat[2].w = translation.z; mat[3].x = 0.0f; mat[3].y = 0.0f; mat[3].z = 0.0f; mat[3].w = 1.0f; }
public idMat4(ref idMat3 rotation, ref idVec3 translation) { idVec3[] rotation_mat = rotation.mat; // NOTE: idMat3 is transposed because it is column-major mat[0].x = rotation_mat[0].x; mat[0].y = rotation_mat[1].x; mat[0].z = rotation_mat[2].x; mat[0].w = translation.x; mat[1].x = rotation_mat[0].y; mat[1].y = rotation_mat[1].y; mat[1].z = rotation_mat[2].y; mat[1].w = translation.y; mat[2].x = rotation_mat[0].z; mat[2].y = rotation_mat[1].z; mat[2].z = rotation_mat[2].z; mat[2].w = translation.z; mat[3].x = 0.0f; mat[3].y = 0.0f; mat[3].z = 0.0f; mat[3].w = 1.0f; }
public bool InverseFastSelf() { #if true // 18+3+9 = 30 multiplications // 1 division idMat3 inverse = new idMat3(); idVec3[] inverse_mat = inverse.mat; inverse_mat[0].x = mat[1].y * mat[2].z - mat[1].z * mat[2].y; inverse_mat[1].x = mat[1].z * mat[2].x - mat[1].x * mat[2].z; inverse_mat[2].x = mat[1].x * mat[2].y - mat[1].y * mat[2].x; double det = mat[0].x * inverse_mat[0].x + mat[0].y * inverse_mat[1].x + mat[0].z * inverse_mat[2].x; if (idMath.Fabs(det) < MATRIX_INVERSE_EPSILON) return false; double invDet = 1.0f / det; inverse_mat[0].y = mat[0].z * mat[2].y - mat[0].y * mat[2].z; inverse_mat[0].z = mat[0].y * mat[1].z - mat[0].z * mat[1].y; inverse_mat[1].y = mat[0].x * mat[2].z - mat[0].z * mat[2].x; inverse_mat[1].z = mat[0].z * mat[1].x - mat[0].x * mat[1].z; inverse_mat[2].y = mat[0].y * mat[2].x - mat[0].x * mat[2].y; inverse_mat[2].z = mat[0].x * mat[1].y - mat[0].y * mat[1].x; mat[0].x = (float)(inverse_mat[0].x * invDet); mat[0].y = (float)(inverse_mat[0].y * invDet); mat[0].z = (float)(inverse_mat[0].z * invDet); mat[1].x = (float)(inverse_mat[1].x * invDet); mat[1].y = (float)(inverse_mat[1].y * invDet); mat[1].z = (float)(inverse_mat[1].z * invDet); mat[2].x = (float)(inverse_mat[2].x * invDet); mat[2].y = (float)(inverse_mat[2].y * invDet); mat[2].z = (float)(inverse_mat[2].z * invDet); return true; #elif false // 3*10 = 30 multiplications // 3 divisions float *mat = reinterpret_cast<float *>(this); float s; double d, di; di = mat[0]; s = di; mat[0] = d = 1.0f / di; mat[1] *= d; mat[2] *= d; d = -d; mat[3] *= d; mat[6] *= d; d = mat[3] * di; mat[4] += mat[1] * d; mat[5] += mat[2] * d; d = mat[6] * di; mat[7] += mat[1] * d; mat[8] += mat[2] * d; di = mat[4]; s *= di; mat[4] = d = 1.0f / di; mat[3] *= d; mat[5] *= d; d = -d; mat[1] *= d; mat[7] *= d; d = mat[1] * di; mat[0] += mat[3] * d; mat[2] += mat[5] * d; d = mat[7] * di; mat[6] += mat[3] * d; mat[8] += mat[5] * d; di = mat[8]; s *= di; mat[8] = d = 1.0f / di; mat[6] *= d; mat[7] *= d; d = -d; mat[2] *= d; mat[5] *= d; d = mat[2] * di; mat[0] += mat[6] * d; mat[1] += mat[7] * d; d = mat[5] * di; mat[3] += mat[6] * d; mat[4] += mat[7] * d; return ( s != 0.0f && !FLOAT_IS_NAN( s ) ); #else // 4*2+4*4 = 24 multiplications // 2*1 = 2 divisions idMat2 r0; float r1[2], r2[2], r3;
public bool InverseSelf() { // 18+3+9 = 30 multiplications // 1 division idMat3 inverse = new idMat3(); idVec3[] inverse_mat = inverse.mat; inverse_mat[0].x = mat[1].y * mat[2].z - mat[1].z * mat[2].y; inverse_mat[1].x = mat[1].z * mat[2].x - mat[1].x * mat[2].z; inverse_mat[2].x = mat[1].x * mat[2].y - mat[1].y * mat[2].x; double det = mat[0].x * inverse_mat[0].x + mat[0].y * inverse_mat[1].x + mat[0].z * inverse_mat[2].x; if (idMath.Fabs(det) < MATRIX_INVERSE_EPSILON) return false; double invDet = 1.0f / det; inverse_mat[0].y = mat[0].z * mat[2].y - mat[0].y * mat[2].z; inverse_mat[0].z = mat[0].y * mat[1].z - mat[0].z * mat[1].y; inverse_mat[1].y = mat[0].x * mat[2].z - mat[0].z * mat[2].x; inverse_mat[1].z = mat[0].z * mat[1].x - mat[0].x * mat[1].z; inverse_mat[2].y = mat[0].y * mat[2].x - mat[0].x * mat[2].y; inverse_mat[2].z = mat[0].x * mat[1].y - mat[0].y * mat[1].x; mat[0].x = (float)(inverse_mat[0].x * invDet); mat[0].y = (float)(inverse_mat[0].y * invDet); mat[0].z = (float)(inverse_mat[0].z * invDet); mat[1].x = (float)(inverse_mat[1].x * invDet); mat[1].y = (float)(inverse_mat[1].y * invDet); mat[1].z = (float)(inverse_mat[1].z * invDet); mat[2].x = (float)(inverse_mat[2].x * invDet); mat[2].y = (float)(inverse_mat[2].y * invDet); mat[2].z = (float)(inverse_mat[2].z * invDet); return true; }
public bool Compare(ref idMat3 a, float epsilon) { return ( mat[0].Compare(ref a.mat[0], epsilon) && mat[1].Compare(ref a.mat[1], epsilon) && mat[2].Compare(ref a.mat[2], epsilon)); }
public bool Compare(ref idMat3 a) { return ( mat[0].Compare(ref a.mat[0]) && mat[1].Compare(ref a.mat[1]) && mat[2].Compare(ref a.mat[2])); }
// extra public idVec3 opMul(ref idVec3 vec, ref idMat3 mat) { idVec3[] mat_mat = mat.mat; float x = mat_mat[0].x * vec.x + mat_mat[1].x * vec.y + mat_mat[2].x * vec.z; float y = mat_mat[0].y * vec.x + mat_mat[1].y * vec.y + mat_mat[2].y * vec.z; vec.z = mat_mat[0].z * vec.x + mat_mat[1].z * vec.y + mat_mat[2].z * vec.z; vec.x = x; vec.y = y; return vec; }
public idMat3 opSub(idMat3 a) { idVec3[] a_mat = a.mat; mat[0].x -= a_mat[0].x; mat[0].y -= a_mat[0].y; mat[0].z -= a_mat[0].z; mat[1].x -= a_mat[1].x; mat[1].y -= a_mat[1].y; mat[1].z -= a_mat[1].z; mat[2].x -= a_mat[2].x; mat[2].y -= a_mat[2].y; mat[2].z -= a_mat[2].z; return this; }
public idMat3 opAdd(idMat3 a) { idVec3[] a_mat = a.mat; mat[0].x += a_mat[0].x; mat[0].y += a_mat[0].y; mat[0].z += a_mat[0].z; mat[1].x += a_mat[1].x; mat[1].y += a_mat[1].y; mat[1].z += a_mat[1].z; mat[2].x += a_mat[2].x; mat[2].y += a_mat[2].y; mat[2].z += a_mat[2].z; return this; }
public idMat3 opMul(idMat3 a) { //m1Ptr = reinterpret_cast<float *>(this); //m2Ptr = reinterpret_cast<const float *>(&a); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { // dst[j] = m1Ptr[0] * m2Ptr[ 0 * 3 + j ] // + m1Ptr[1] * m2Ptr[ 1 * 3 + j ] // + m1Ptr[2] * m2Ptr[ 2 * 3 + j ]; } // m1Ptr[0] = dst[0]; m1Ptr[1] = dst[1]; m1Ptr[2] = dst[2]; // m1Ptr += 3; } return this; }
public idMat3 ToMat3() { float x2 = x + x; float y2 = y + y; float z2 = z + z; float xx = x * x2; float xy = x * y2; float xz = x * z2; float yy = y * y2; float yz = y * z2; float zz = z * z2; float wx = w * x2; float wy = w * y2; float wz = w * z2; idMat3 mat = new idMat3(); idVec3[] mat_mat = mat.mat; mat_mat[0].x = 1.0f - (yy + zz); mat_mat[0].y = xy - wz; mat_mat[0].z = xz + wy; mat_mat[1].x = xy + wz; mat_mat[1].y = 1.0f - (xx + zz); mat_mat[1].z = yz - wx; mat_mat[2].x = xz - wy; mat_mat[2].y = yz + wx; mat_mat[2].z = 1.0f - (xx + yy); return mat; }
public idMat3 ToMat3() { float sr, sp, sy, cr, cp, cy; idMath.SinCos(DEG2RAD(yaw), out sy, out cy); idMath.SinCos(DEG2RAD(pitch), out sp, out cp); idMath.SinCos(DEG2RAD(roll), out sr, out cr); idMat3 mat = new idMat3(); idVec3[] mat_mat = mat.mat; mat_mat[0].Set(cp * cy, cp * sy, -sp); mat_mat[1].Set(sr * sp * cy + cr * -sy, sr * sp * sy + cr * cy, sr * cp); mat_mat[2].Set(cr * sp * cy + -sr * -sy, cr * sp * sy + -sr * cy, cr * cp); return mat; }
public bool InverseFastSelf() { #if false // 810+6+36 = 852 multiplications // 1 division // 2x2 sub-determinants required to calculate 6x6 determinant float det2_45_01 = mat[4].p[0] * mat[5].p[1] - mat[4].p[1] * mat[5].p[0]; float det2_45_02 = mat[4].p[0] * mat[5].p[2] - mat[4].p[2] * mat[5].p[0]; float det2_45_03 = mat[4].p[0] * mat[5].p[3] - mat[4].p[3] * mat[5].p[0]; float det2_45_04 = mat[4].p[0] * mat[5].p[4] - mat[4].p[4] * mat[5].p[0]; float det2_45_05 = mat[4].p[0] * mat[5].p[5] - mat[4].p[5] * mat[5].p[0]; float det2_45_12 = mat[4].p[1] * mat[5].p[2] - mat[4].p[2] * mat[5].p[1]; float det2_45_13 = mat[4].p[1] * mat[5].p[3] - mat[4].p[3] * mat[5].p[1]; float det2_45_14 = mat[4].p[1] * mat[5].p[4] - mat[4].p[4] * mat[5].p[1]; float det2_45_15 = mat[4].p[1] * mat[5].p[5] - mat[4].p[5] * mat[5].p[1]; float det2_45_23 = mat[4].p[2] * mat[5].p[3] - mat[4].p[3] * mat[5].p[2]; float det2_45_24 = mat[4].p[2] * mat[5].p[4] - mat[4].p[4] * mat[5].p[2]; float det2_45_25 = mat[4].p[2] * mat[5].p[5] - mat[4].p[5] * mat[5].p[2]; float det2_45_34 = mat[4].p[3] * mat[5].p[4] - mat[4].p[4] * mat[5].p[3]; float det2_45_35 = mat[4].p[3] * mat[5].p[5] - mat[4].p[5] * mat[5].p[3]; float det2_45_45 = mat[4].p[4] * mat[5].p[5] - mat[4].p[5] * mat[5].p[4]; // 3x3 sub-determinants required to calculate 6x6 determinant float det3_345_012 = mat[3].p[0] * det2_45_12 - mat[3].p[1] * det2_45_02 + mat[3].p[2] * det2_45_01; float det3_345_013 = mat[3].p[0] * det2_45_13 - mat[3].p[1] * det2_45_03 + mat[3].p[3] * det2_45_01; float det3_345_014 = mat[3].p[0] * det2_45_14 - mat[3].p[1] * det2_45_04 + mat[3].p[4] * det2_45_01; float det3_345_015 = mat[3].p[0] * det2_45_15 - mat[3].p[1] * det2_45_05 + mat[3].p[5] * det2_45_01; float det3_345_023 = mat[3].p[0] * det2_45_23 - mat[3].p[2] * det2_45_03 + mat[3].p[3] * det2_45_02; float det3_345_024 = mat[3].p[0] * det2_45_24 - mat[3].p[2] * det2_45_04 + mat[3].p[4] * det2_45_02; float det3_345_025 = mat[3].p[0] * det2_45_25 - mat[3].p[2] * det2_45_05 + mat[3].p[5] * det2_45_02; float det3_345_034 = mat[3].p[0] * det2_45_34 - mat[3].p[3] * det2_45_04 + mat[3].p[4] * det2_45_03; float det3_345_035 = mat[3].p[0] * det2_45_35 - mat[3].p[3] * det2_45_05 + mat[3].p[5] * det2_45_03; float det3_345_045 = mat[3].p[0] * det2_45_45 - mat[3].p[4] * det2_45_05 + mat[3].p[5] * det2_45_04; float det3_345_123 = mat[3].p[1] * det2_45_23 - mat[3].p[2] * det2_45_13 + mat[3].p[3] * det2_45_12; float det3_345_124 = mat[3].p[1] * det2_45_24 - mat[3].p[2] * det2_45_14 + mat[3].p[4] * det2_45_12; float det3_345_125 = mat[3].p[1] * det2_45_25 - mat[3].p[2] * det2_45_15 + mat[3].p[5] * det2_45_12; float det3_345_134 = mat[3].p[1] * det2_45_34 - mat[3].p[3] * det2_45_14 + mat[3].p[4] * det2_45_13; float det3_345_135 = mat[3].p[1] * det2_45_35 - mat[3].p[3] * det2_45_15 + mat[3].p[5] * det2_45_13; float det3_345_145 = mat[3].p[1] * det2_45_45 - mat[3].p[4] * det2_45_15 + mat[3].p[5] * det2_45_14; float det3_345_234 = mat[3].p[2] * det2_45_34 - mat[3].p[3] * det2_45_24 + mat[3].p[4] * det2_45_23; float det3_345_235 = mat[3].p[2] * det2_45_35 - mat[3].p[3] * det2_45_25 + mat[3].p[5] * det2_45_23; float det3_345_245 = mat[3].p[2] * det2_45_45 - mat[3].p[4] * det2_45_25 + mat[3].p[5] * det2_45_24; float det3_345_345 = mat[3].p[3] * det2_45_45 - mat[3].p[4] * det2_45_35 + mat[3].p[5] * det2_45_34; // 4x4 sub-determinants required to calculate 6x6 determinant float det4_2345_0123 = mat[2].p[0] * det3_345_123 - mat[2].p[1] * det3_345_023 + mat[2].p[2] * det3_345_013 - mat[2].p[3] * det3_345_012; float det4_2345_0124 = mat[2].p[0] * det3_345_124 - mat[2].p[1] * det3_345_024 + mat[2].p[2] * det3_345_014 - mat[2].p[4] * det3_345_012; float det4_2345_0125 = mat[2].p[0] * det3_345_125 - mat[2].p[1] * det3_345_025 + mat[2].p[2] * det3_345_015 - mat[2].p[5] * det3_345_012; float det4_2345_0134 = mat[2].p[0] * det3_345_134 - mat[2].p[1] * det3_345_034 + mat[2].p[3] * det3_345_014 - mat[2].p[4] * det3_345_013; float det4_2345_0135 = mat[2].p[0] * det3_345_135 - mat[2].p[1] * det3_345_035 + mat[2].p[3] * det3_345_015 - mat[2].p[5] * det3_345_013; float det4_2345_0145 = mat[2].p[0] * det3_345_145 - mat[2].p[1] * det3_345_045 + mat[2].p[4] * det3_345_015 - mat[2].p[5] * det3_345_014; float det4_2345_0234 = mat[2].p[0] * det3_345_234 - mat[2].p[2] * det3_345_034 + mat[2].p[3] * det3_345_024 - mat[2].p[4] * det3_345_023; float det4_2345_0235 = mat[2].p[0] * det3_345_235 - mat[2].p[2] * det3_345_035 + mat[2].p[3] * det3_345_025 - mat[2].p[5] * det3_345_023; float det4_2345_0245 = mat[2].p[0] * det3_345_245 - mat[2].p[2] * det3_345_045 + mat[2].p[4] * det3_345_025 - mat[2].p[5] * det3_345_024; float det4_2345_0345 = mat[2].p[0] * det3_345_345 - mat[2].p[3] * det3_345_045 + mat[2].p[4] * det3_345_035 - mat[2].p[5] * det3_345_034; float det4_2345_1234 = mat[2].p[1] * det3_345_234 - mat[2].p[2] * det3_345_134 + mat[2].p[3] * det3_345_124 - mat[2].p[4] * det3_345_123; float det4_2345_1235 = mat[2].p[1] * det3_345_235 - mat[2].p[2] * det3_345_135 + mat[2].p[3] * det3_345_125 - mat[2].p[5] * det3_345_123; float det4_2345_1245 = mat[2].p[1] * det3_345_245 - mat[2].p[2] * det3_345_145 + mat[2].p[4] * det3_345_125 - mat[2].p[5] * det3_345_124; float det4_2345_1345 = mat[2].p[1] * det3_345_345 - mat[2].p[3] * det3_345_145 + mat[2].p[4] * det3_345_135 - mat[2].p[5] * det3_345_134; float det4_2345_2345 = mat[2].p[2] * det3_345_345 - mat[2].p[3] * det3_345_245 + mat[2].p[4] * det3_345_235 - mat[2].p[5] * det3_345_234; // 5x5 sub-determinants required to calculate 6x6 determinant float det5_12345_01234 = mat[1].p[0] * det4_2345_1234 - mat[1].p[1] * det4_2345_0234 + mat[1].p[2] * det4_2345_0134 - mat[1].p[3] * det4_2345_0124 + mat[1].p[4] * det4_2345_0123; float det5_12345_01235 = mat[1].p[0] * det4_2345_1235 - mat[1].p[1] * det4_2345_0235 + mat[1].p[2] * det4_2345_0135 - mat[1].p[3] * det4_2345_0125 + mat[1].p[5] * det4_2345_0123; float det5_12345_01245 = mat[1].p[0] * det4_2345_1245 - mat[1].p[1] * det4_2345_0245 + mat[1].p[2] * det4_2345_0145 - mat[1].p[4] * det4_2345_0125 + mat[1].p[5] * det4_2345_0124; float det5_12345_01345 = mat[1].p[0] * det4_2345_1345 - mat[1].p[1] * det4_2345_0345 + mat[1].p[3] * det4_2345_0145 - mat[1].p[4] * det4_2345_0135 + mat[1].p[5] * det4_2345_0134; float det5_12345_02345 = mat[1].p[0] * det4_2345_2345 - mat[1].p[2] * det4_2345_0345 + mat[1].p[3] * det4_2345_0245 - mat[1].p[4] * det4_2345_0235 + mat[1].p[5] * det4_2345_0234; float det5_12345_12345 = mat[1].p[1] * det4_2345_2345 - mat[1].p[2] * det4_2345_1345 + mat[1].p[3] * det4_2345_1245 - mat[1].p[4] * det4_2345_1235 + mat[1].p[5] * det4_2345_1234; // determinant of 6x6 matrix double det = mat[0].p[0] * det5_12345_12345 - mat[0].p[1] * det5_12345_02345 + mat[0].p[2] * det5_12345_01345 - mat[0].p[3] * det5_12345_01245 + mat[0].p[4] * det5_12345_01235 - mat[0].p[5] * det5_12345_01234; if (idMath.Fabs(det) < MATRIX_INVERSE_EPSILON) return false; double invDet = 1.0f / det; // remaining 2x2 sub-determinants float det2_34_01 = mat[3].p[0] * mat[4].p[1] - mat[3].p[1] * mat[4].p[0]; float det2_34_02 = mat[3].p[0] * mat[4].p[2] - mat[3].p[2] * mat[4].p[0]; float det2_34_03 = mat[3].p[0] * mat[4].p[3] - mat[3].p[3] * mat[4].p[0]; float det2_34_04 = mat[3].p[0] * mat[4].p[4] - mat[3].p[4] * mat[4].p[0]; float det2_34_05 = mat[3].p[0] * mat[4].p[5] - mat[3].p[5] * mat[4].p[0]; float det2_34_12 = mat[3].p[1] * mat[4].p[2] - mat[3].p[2] * mat[4].p[1]; float det2_34_13 = mat[3].p[1] * mat[4].p[3] - mat[3].p[3] * mat[4].p[1]; float det2_34_14 = mat[3].p[1] * mat[4].p[4] - mat[3].p[4] * mat[4].p[1]; float det2_34_15 = mat[3].p[1] * mat[4].p[5] - mat[3].p[5] * mat[4].p[1]; float det2_34_23 = mat[3].p[2] * mat[4].p[3] - mat[3].p[3] * mat[4].p[2]; float det2_34_24 = mat[3].p[2] * mat[4].p[4] - mat[3].p[4] * mat[4].p[2]; float det2_34_25 = mat[3].p[2] * mat[4].p[5] - mat[3].p[5] * mat[4].p[2]; float det2_34_34 = mat[3].p[3] * mat[4].p[4] - mat[3].p[4] * mat[4].p[3]; float det2_34_35 = mat[3].p[3] * mat[4].p[5] - mat[3].p[5] * mat[4].p[3]; float det2_34_45 = mat[3].p[4] * mat[4].p[5] - mat[3].p[5] * mat[4].p[4]; float det2_35_01 = mat[3].p[0] * mat[5].p[1] - mat[3].p[1] * mat[5].p[0]; float det2_35_02 = mat[3].p[0] * mat[5].p[2] - mat[3].p[2] * mat[5].p[0]; float det2_35_03 = mat[3].p[0] * mat[5].p[3] - mat[3].p[3] * mat[5].p[0]; float det2_35_04 = mat[3].p[0] * mat[5].p[4] - mat[3].p[4] * mat[5].p[0]; float det2_35_05 = mat[3].p[0] * mat[5].p[5] - mat[3].p[5] * mat[5].p[0]; float det2_35_12 = mat[3].p[1] * mat[5].p[2] - mat[3].p[2] * mat[5].p[1]; float det2_35_13 = mat[3].p[1] * mat[5].p[3] - mat[3].p[3] * mat[5].p[1]; float det2_35_14 = mat[3].p[1] * mat[5].p[4] - mat[3].p[4] * mat[5].p[1]; float det2_35_15 = mat[3].p[1] * mat[5].p[5] - mat[3].p[5] * mat[5].p[1]; float det2_35_23 = mat[3].p[2] * mat[5].p[3] - mat[3].p[3] * mat[5].p[2]; float det2_35_24 = mat[3].p[2] * mat[5].p[4] - mat[3].p[4] * mat[5].p[2]; float det2_35_25 = mat[3].p[2] * mat[5].p[5] - mat[3].p[5] * mat[5].p[2]; float det2_35_34 = mat[3].p[3] * mat[5].p[4] - mat[3].p[4] * mat[5].p[3]; float det2_35_35 = mat[3].p[3] * mat[5].p[5] - mat[3].p[5] * mat[5].p[3]; float det2_35_45 = mat[3].p[4] * mat[5].p[5] - mat[3].p[5] * mat[5].p[4]; // remaining 3x3 sub-determinants float det3_234_012 = mat[2].p[0] * det2_34_12 - mat[2].p[1] * det2_34_02 + mat[2].p[2] * det2_34_01; float det3_234_013 = mat[2].p[0] * det2_34_13 - mat[2].p[1] * det2_34_03 + mat[2].p[3] * det2_34_01; float det3_234_014 = mat[2].p[0] * det2_34_14 - mat[2].p[1] * det2_34_04 + mat[2].p[4] * det2_34_01; float det3_234_015 = mat[2].p[0] * det2_34_15 - mat[2].p[1] * det2_34_05 + mat[2].p[5] * det2_34_01; float det3_234_023 = mat[2].p[0] * det2_34_23 - mat[2].p[2] * det2_34_03 + mat[2].p[3] * det2_34_02; float det3_234_024 = mat[2].p[0] * det2_34_24 - mat[2].p[2] * det2_34_04 + mat[2].p[4] * det2_34_02; float det3_234_025 = mat[2].p[0] * det2_34_25 - mat[2].p[2] * det2_34_05 + mat[2].p[5] * det2_34_02; float det3_234_034 = mat[2].p[0] * det2_34_34 - mat[2].p[3] * det2_34_04 + mat[2].p[4] * det2_34_03; float det3_234_035 = mat[2].p[0] * det2_34_35 - mat[2].p[3] * det2_34_05 + mat[2].p[5] * det2_34_03; float det3_234_045 = mat[2].p[0] * det2_34_45 - mat[2].p[4] * det2_34_05 + mat[2].p[5] * det2_34_04; float det3_234_123 = mat[2].p[1] * det2_34_23 - mat[2].p[2] * det2_34_13 + mat[2].p[3] * det2_34_12; float det3_234_124 = mat[2].p[1] * det2_34_24 - mat[2].p[2] * det2_34_14 + mat[2].p[4] * det2_34_12; float det3_234_125 = mat[2].p[1] * det2_34_25 - mat[2].p[2] * det2_34_15 + mat[2].p[5] * det2_34_12; float det3_234_134 = mat[2].p[1] * det2_34_34 - mat[2].p[3] * det2_34_14 + mat[2].p[4] * det2_34_13; float det3_234_135 = mat[2].p[1] * det2_34_35 - mat[2].p[3] * det2_34_15 + mat[2].p[5] * det2_34_13; float det3_234_145 = mat[2].p[1] * det2_34_45 - mat[2].p[4] * det2_34_15 + mat[2].p[5] * det2_34_14; float det3_234_234 = mat[2].p[2] * det2_34_34 - mat[2].p[3] * det2_34_24 + mat[2].p[4] * det2_34_23; float det3_234_235 = mat[2].p[2] * det2_34_35 - mat[2].p[3] * det2_34_25 + mat[2].p[5] * det2_34_23; float det3_234_245 = mat[2].p[2] * det2_34_45 - mat[2].p[4] * det2_34_25 + mat[2].p[5] * det2_34_24; float det3_234_345 = mat[2].p[3] * det2_34_45 - mat[2].p[4] * det2_34_35 + mat[2].p[5] * det2_34_34; float det3_235_012 = mat[2].p[0] * det2_35_12 - mat[2].p[1] * det2_35_02 + mat[2].p[2] * det2_35_01; float det3_235_013 = mat[2].p[0] * det2_35_13 - mat[2].p[1] * det2_35_03 + mat[2].p[3] * det2_35_01; float det3_235_014 = mat[2].p[0] * det2_35_14 - mat[2].p[1] * det2_35_04 + mat[2].p[4] * det2_35_01; float det3_235_015 = mat[2].p[0] * det2_35_15 - mat[2].p[1] * det2_35_05 + mat[2].p[5] * det2_35_01; float det3_235_023 = mat[2].p[0] * det2_35_23 - mat[2].p[2] * det2_35_03 + mat[2].p[3] * det2_35_02; float det3_235_024 = mat[2].p[0] * det2_35_24 - mat[2].p[2] * det2_35_04 + mat[2].p[4] * det2_35_02; float det3_235_025 = mat[2].p[0] * det2_35_25 - mat[2].p[2] * det2_35_05 + mat[2].p[5] * det2_35_02; float det3_235_034 = mat[2].p[0] * det2_35_34 - mat[2].p[3] * det2_35_04 + mat[2].p[4] * det2_35_03; float det3_235_035 = mat[2].p[0] * det2_35_35 - mat[2].p[3] * det2_35_05 + mat[2].p[5] * det2_35_03; float det3_235_045 = mat[2].p[0] * det2_35_45 - mat[2].p[4] * det2_35_05 + mat[2].p[5] * det2_35_04; float det3_235_123 = mat[2].p[1] * det2_35_23 - mat[2].p[2] * det2_35_13 + mat[2].p[3] * det2_35_12; float det3_235_124 = mat[2].p[1] * det2_35_24 - mat[2].p[2] * det2_35_14 + mat[2].p[4] * det2_35_12; float det3_235_125 = mat[2].p[1] * det2_35_25 - mat[2].p[2] * det2_35_15 + mat[2].p[5] * det2_35_12; float det3_235_134 = mat[2].p[1] * det2_35_34 - mat[2].p[3] * det2_35_14 + mat[2].p[4] * det2_35_13; float det3_235_135 = mat[2].p[1] * det2_35_35 - mat[2].p[3] * det2_35_15 + mat[2].p[5] * det2_35_13; float det3_235_145 = mat[2].p[1] * det2_35_45 - mat[2].p[4] * det2_35_15 + mat[2].p[5] * det2_35_14; float det3_235_234 = mat[2].p[2] * det2_35_34 - mat[2].p[3] * det2_35_24 + mat[2].p[4] * det2_35_23; float det3_235_235 = mat[2].p[2] * det2_35_35 - mat[2].p[3] * det2_35_25 + mat[2].p[5] * det2_35_23; float det3_235_245 = mat[2].p[2] * det2_35_45 - mat[2].p[4] * det2_35_25 + mat[2].p[5] * det2_35_24; float det3_235_345 = mat[2].p[3] * det2_35_45 - mat[2].p[4] * det2_35_35 + mat[2].p[5] * det2_35_34; float det3_245_012 = mat[2].p[0] * det2_45_12 - mat[2].p[1] * det2_45_02 + mat[2].p[2] * det2_45_01; float det3_245_013 = mat[2].p[0] * det2_45_13 - mat[2].p[1] * det2_45_03 + mat[2].p[3] * det2_45_01; float det3_245_014 = mat[2].p[0] * det2_45_14 - mat[2].p[1] * det2_45_04 + mat[2].p[4] * det2_45_01; float det3_245_015 = mat[2].p[0] * det2_45_15 - mat[2].p[1] * det2_45_05 + mat[2].p[5] * det2_45_01; float det3_245_023 = mat[2].p[0] * det2_45_23 - mat[2].p[2] * det2_45_03 + mat[2].p[3] * det2_45_02; float det3_245_024 = mat[2].p[0] * det2_45_24 - mat[2].p[2] * det2_45_04 + mat[2].p[4] * det2_45_02; float det3_245_025 = mat[2].p[0] * det2_45_25 - mat[2].p[2] * det2_45_05 + mat[2].p[5] * det2_45_02; float det3_245_034 = mat[2].p[0] * det2_45_34 - mat[2].p[3] * det2_45_04 + mat[2].p[4] * det2_45_03; float det3_245_035 = mat[2].p[0] * det2_45_35 - mat[2].p[3] * det2_45_05 + mat[2].p[5] * det2_45_03; float det3_245_045 = mat[2].p[0] * det2_45_45 - mat[2].p[4] * det2_45_05 + mat[2].p[5] * det2_45_04; float det3_245_123 = mat[2].p[1] * det2_45_23 - mat[2].p[2] * det2_45_13 + mat[2].p[3] * det2_45_12; float det3_245_124 = mat[2].p[1] * det2_45_24 - mat[2].p[2] * det2_45_14 + mat[2].p[4] * det2_45_12; float det3_245_125 = mat[2].p[1] * det2_45_25 - mat[2].p[2] * det2_45_15 + mat[2].p[5] * det2_45_12; float det3_245_134 = mat[2].p[1] * det2_45_34 - mat[2].p[3] * det2_45_14 + mat[2].p[4] * det2_45_13; float det3_245_135 = mat[2].p[1] * det2_45_35 - mat[2].p[3] * det2_45_15 + mat[2].p[5] * det2_45_13; float det3_245_145 = mat[2].p[1] * det2_45_45 - mat[2].p[4] * det2_45_15 + mat[2].p[5] * det2_45_14; float det3_245_234 = mat[2].p[2] * det2_45_34 - mat[2].p[3] * det2_45_24 + mat[2].p[4] * det2_45_23; float det3_245_235 = mat[2].p[2] * det2_45_35 - mat[2].p[3] * det2_45_25 + mat[2].p[5] * det2_45_23; float det3_245_245 = mat[2].p[2] * det2_45_45 - mat[2].p[4] * det2_45_25 + mat[2].p[5] * det2_45_24; float det3_245_345 = mat[2].p[3] * det2_45_45 - mat[2].p[4] * det2_45_35 + mat[2].p[5] * det2_45_34; // remaining 4x4 sub-determinants float det4_1234_0123 = mat[1].p[0] * det3_234_123 - mat[1].p[1] * det3_234_023 + mat[1].p[2] * det3_234_013 - mat[1].p[3] * det3_234_012; float det4_1234_0124 = mat[1].p[0] * det3_234_124 - mat[1].p[1] * det3_234_024 + mat[1].p[2] * det3_234_014 - mat[1].p[4] * det3_234_012; float det4_1234_0125 = mat[1].p[0] * det3_234_125 - mat[1].p[1] * det3_234_025 + mat[1].p[2] * det3_234_015 - mat[1].p[5] * det3_234_012; float det4_1234_0134 = mat[1].p[0] * det3_234_134 - mat[1].p[1] * det3_234_034 + mat[1].p[3] * det3_234_014 - mat[1].p[4] * det3_234_013; float det4_1234_0135 = mat[1].p[0] * det3_234_135 - mat[1].p[1] * det3_234_035 + mat[1].p[3] * det3_234_015 - mat[1].p[5] * det3_234_013; float det4_1234_0145 = mat[1].p[0] * det3_234_145 - mat[1].p[1] * det3_234_045 + mat[1].p[4] * det3_234_015 - mat[1].p[5] * det3_234_014; float det4_1234_0234 = mat[1].p[0] * det3_234_234 - mat[1].p[2] * det3_234_034 + mat[1].p[3] * det3_234_024 - mat[1].p[4] * det3_234_023; float det4_1234_0235 = mat[1].p[0] * det3_234_235 - mat[1].p[2] * det3_234_035 + mat[1].p[3] * det3_234_025 - mat[1].p[5] * det3_234_023; float det4_1234_0245 = mat[1].p[0] * det3_234_245 - mat[1].p[2] * det3_234_045 + mat[1].p[4] * det3_234_025 - mat[1].p[5] * det3_234_024; float det4_1234_0345 = mat[1].p[0] * det3_234_345 - mat[1].p[3] * det3_234_045 + mat[1].p[4] * det3_234_035 - mat[1].p[5] * det3_234_034; float det4_1234_1234 = mat[1].p[1] * det3_234_234 - mat[1].p[2] * det3_234_134 + mat[1].p[3] * det3_234_124 - mat[1].p[4] * det3_234_123; float det4_1234_1235 = mat[1].p[1] * det3_234_235 - mat[1].p[2] * det3_234_135 + mat[1].p[3] * det3_234_125 - mat[1].p[5] * det3_234_123; float det4_1234_1245 = mat[1].p[1] * det3_234_245 - mat[1].p[2] * det3_234_145 + mat[1].p[4] * det3_234_125 - mat[1].p[5] * det3_234_124; float det4_1234_1345 = mat[1].p[1] * det3_234_345 - mat[1].p[3] * det3_234_145 + mat[1].p[4] * det3_234_135 - mat[1].p[5] * det3_234_134; float det4_1234_2345 = mat[1].p[2] * det3_234_345 - mat[1].p[3] * det3_234_245 + mat[1].p[4] * det3_234_235 - mat[1].p[5] * det3_234_234; float det4_1235_0123 = mat[1].p[0] * det3_235_123 - mat[1].p[1] * det3_235_023 + mat[1].p[2] * det3_235_013 - mat[1].p[3] * det3_235_012; float det4_1235_0124 = mat[1].p[0] * det3_235_124 - mat[1].p[1] * det3_235_024 + mat[1].p[2] * det3_235_014 - mat[1].p[4] * det3_235_012; float det4_1235_0125 = mat[1].p[0] * det3_235_125 - mat[1].p[1] * det3_235_025 + mat[1].p[2] * det3_235_015 - mat[1].p[5] * det3_235_012; float det4_1235_0134 = mat[1].p[0] * det3_235_134 - mat[1].p[1] * det3_235_034 + mat[1].p[3] * det3_235_014 - mat[1].p[4] * det3_235_013; float det4_1235_0135 = mat[1].p[0] * det3_235_135 - mat[1].p[1] * det3_235_035 + mat[1].p[3] * det3_235_015 - mat[1].p[5] * det3_235_013; float det4_1235_0145 = mat[1].p[0] * det3_235_145 - mat[1].p[1] * det3_235_045 + mat[1].p[4] * det3_235_015 - mat[1].p[5] * det3_235_014; float det4_1235_0234 = mat[1].p[0] * det3_235_234 - mat[1].p[2] * det3_235_034 + mat[1].p[3] * det3_235_024 - mat[1].p[4] * det3_235_023; float det4_1235_0235 = mat[1].p[0] * det3_235_235 - mat[1].p[2] * det3_235_035 + mat[1].p[3] * det3_235_025 - mat[1].p[5] * det3_235_023; float det4_1235_0245 = mat[1].p[0] * det3_235_245 - mat[1].p[2] * det3_235_045 + mat[1].p[4] * det3_235_025 - mat[1].p[5] * det3_235_024; float det4_1235_0345 = mat[1].p[0] * det3_235_345 - mat[1].p[3] * det3_235_045 + mat[1].p[4] * det3_235_035 - mat[1].p[5] * det3_235_034; float det4_1235_1234 = mat[1].p[1] * det3_235_234 - mat[1].p[2] * det3_235_134 + mat[1].p[3] * det3_235_124 - mat[1].p[4] * det3_235_123; float det4_1235_1235 = mat[1].p[1] * det3_235_235 - mat[1].p[2] * det3_235_135 + mat[1].p[3] * det3_235_125 - mat[1].p[5] * det3_235_123; float det4_1235_1245 = mat[1].p[1] * det3_235_245 - mat[1].p[2] * det3_235_145 + mat[1].p[4] * det3_235_125 - mat[1].p[5] * det3_235_124; float det4_1235_1345 = mat[1].p[1] * det3_235_345 - mat[1].p[3] * det3_235_145 + mat[1].p[4] * det3_235_135 - mat[1].p[5] * det3_235_134; float det4_1235_2345 = mat[1].p[2] * det3_235_345 - mat[1].p[3] * det3_235_245 + mat[1].p[4] * det3_235_235 - mat[1].p[5] * det3_235_234; float det4_1245_0123 = mat[1].p[0] * det3_245_123 - mat[1].p[1] * det3_245_023 + mat[1].p[2] * det3_245_013 - mat[1].p[3] * det3_245_012; float det4_1245_0124 = mat[1].p[0] * det3_245_124 - mat[1].p[1] * det3_245_024 + mat[1].p[2] * det3_245_014 - mat[1].p[4] * det3_245_012; float det4_1245_0125 = mat[1].p[0] * det3_245_125 - mat[1].p[1] * det3_245_025 + mat[1].p[2] * det3_245_015 - mat[1].p[5] * det3_245_012; float det4_1245_0134 = mat[1].p[0] * det3_245_134 - mat[1].p[1] * det3_245_034 + mat[1].p[3] * det3_245_014 - mat[1].p[4] * det3_245_013; float det4_1245_0135 = mat[1].p[0] * det3_245_135 - mat[1].p[1] * det3_245_035 + mat[1].p[3] * det3_245_015 - mat[1].p[5] * det3_245_013; float det4_1245_0145 = mat[1].p[0] * det3_245_145 - mat[1].p[1] * det3_245_045 + mat[1].p[4] * det3_245_015 - mat[1].p[5] * det3_245_014; float det4_1245_0234 = mat[1].p[0] * det3_245_234 - mat[1].p[2] * det3_245_034 + mat[1].p[3] * det3_245_024 - mat[1].p[4] * det3_245_023; float det4_1245_0235 = mat[1].p[0] * det3_245_235 - mat[1].p[2] * det3_245_035 + mat[1].p[3] * det3_245_025 - mat[1].p[5] * det3_245_023; float det4_1245_0245 = mat[1].p[0] * det3_245_245 - mat[1].p[2] * det3_245_045 + mat[1].p[4] * det3_245_025 - mat[1].p[5] * det3_245_024; float det4_1245_0345 = mat[1].p[0] * det3_245_345 - mat[1].p[3] * det3_245_045 + mat[1].p[4] * det3_245_035 - mat[1].p[5] * det3_245_034; float det4_1245_1234 = mat[1].p[1] * det3_245_234 - mat[1].p[2] * det3_245_134 + mat[1].p[3] * det3_245_124 - mat[1].p[4] * det3_245_123; float det4_1245_1235 = mat[1].p[1] * det3_245_235 - mat[1].p[2] * det3_245_135 + mat[1].p[3] * det3_245_125 - mat[1].p[5] * det3_245_123; float det4_1245_1245 = mat[1].p[1] * det3_245_245 - mat[1].p[2] * det3_245_145 + mat[1].p[4] * det3_245_125 - mat[1].p[5] * det3_245_124; float det4_1245_1345 = mat[1].p[1] * det3_245_345 - mat[1].p[3] * det3_245_145 + mat[1].p[4] * det3_245_135 - mat[1].p[5] * det3_245_134; float det4_1245_2345 = mat[1].p[2] * det3_245_345 - mat[1].p[3] * det3_245_245 + mat[1].p[4] * det3_245_235 - mat[1].p[5] * det3_245_234; float det4_1345_0123 = mat[1].p[0] * det3_345_123 - mat[1].p[1] * det3_345_023 + mat[1].p[2] * det3_345_013 - mat[1].p[3] * det3_345_012; float det4_1345_0124 = mat[1].p[0] * det3_345_124 - mat[1].p[1] * det3_345_024 + mat[1].p[2] * det3_345_014 - mat[1].p[4] * det3_345_012; float det4_1345_0125 = mat[1].p[0] * det3_345_125 - mat[1].p[1] * det3_345_025 + mat[1].p[2] * det3_345_015 - mat[1].p[5] * det3_345_012; float det4_1345_0134 = mat[1].p[0] * det3_345_134 - mat[1].p[1] * det3_345_034 + mat[1].p[3] * det3_345_014 - mat[1].p[4] * det3_345_013; float det4_1345_0135 = mat[1].p[0] * det3_345_135 - mat[1].p[1] * det3_345_035 + mat[1].p[3] * det3_345_015 - mat[1].p[5] * det3_345_013; float det4_1345_0145 = mat[1].p[0] * det3_345_145 - mat[1].p[1] * det3_345_045 + mat[1].p[4] * det3_345_015 - mat[1].p[5] * det3_345_014; float det4_1345_0234 = mat[1].p[0] * det3_345_234 - mat[1].p[2] * det3_345_034 + mat[1].p[3] * det3_345_024 - mat[1].p[4] * det3_345_023; float det4_1345_0235 = mat[1].p[0] * det3_345_235 - mat[1].p[2] * det3_345_035 + mat[1].p[3] * det3_345_025 - mat[1].p[5] * det3_345_023; float det4_1345_0245 = mat[1].p[0] * det3_345_245 - mat[1].p[2] * det3_345_045 + mat[1].p[4] * det3_345_025 - mat[1].p[5] * det3_345_024; float det4_1345_0345 = mat[1].p[0] * det3_345_345 - mat[1].p[3] * det3_345_045 + mat[1].p[4] * det3_345_035 - mat[1].p[5] * det3_345_034; float det4_1345_1234 = mat[1].p[1] * det3_345_234 - mat[1].p[2] * det3_345_134 + mat[1].p[3] * det3_345_124 - mat[1].p[4] * det3_345_123; float det4_1345_1235 = mat[1].p[1] * det3_345_235 - mat[1].p[2] * det3_345_135 + mat[1].p[3] * det3_345_125 - mat[1].p[5] * det3_345_123; float det4_1345_1245 = mat[1].p[1] * det3_345_245 - mat[1].p[2] * det3_345_145 + mat[1].p[4] * det3_345_125 - mat[1].p[5] * det3_345_124; float det4_1345_1345 = mat[1].p[1] * det3_345_345 - mat[1].p[3] * det3_345_145 + mat[1].p[4] * det3_345_135 - mat[1].p[5] * det3_345_134; float det4_1345_2345 = mat[1].p[2] * det3_345_345 - mat[1].p[3] * det3_345_245 + mat[1].p[4] * det3_345_235 - mat[1].p[5] * det3_345_234; // remaining 5x5 sub-determinants float det5_01234_01234 = mat[0].p[0] * det4_1234_1234 - mat[0].p[1] * det4_1234_0234 + mat[0].p[2] * det4_1234_0134 - mat[0].p[3] * det4_1234_0124 + mat[0].p[4] * det4_1234_0123; float det5_01234_01235 = mat[0].p[0] * det4_1234_1235 - mat[0].p[1] * det4_1234_0235 + mat[0].p[2] * det4_1234_0135 - mat[0].p[3] * det4_1234_0125 + mat[0].p[5] * det4_1234_0123; float det5_01234_01245 = mat[0].p[0] * det4_1234_1245 - mat[0].p[1] * det4_1234_0245 + mat[0].p[2] * det4_1234_0145 - mat[0].p[4] * det4_1234_0125 + mat[0].p[5] * det4_1234_0124; float det5_01234_01345 = mat[0].p[0] * det4_1234_1345 - mat[0].p[1] * det4_1234_0345 + mat[0].p[3] * det4_1234_0145 - mat[0].p[4] * det4_1234_0135 + mat[0].p[5] * det4_1234_0134; float det5_01234_02345 = mat[0].p[0] * det4_1234_2345 - mat[0].p[2] * det4_1234_0345 + mat[0].p[3] * det4_1234_0245 - mat[0].p[4] * det4_1234_0235 + mat[0].p[5] * det4_1234_0234; float det5_01234_12345 = mat[0].p[1] * det4_1234_2345 - mat[0].p[2] * det4_1234_1345 + mat[0].p[3] * det4_1234_1245 - mat[0].p[4] * det4_1234_1235 + mat[0].p[5] * det4_1234_1234; float det5_01235_01234 = mat[0].p[0] * det4_1235_1234 - mat[0].p[1] * det4_1235_0234 + mat[0].p[2] * det4_1235_0134 - mat[0].p[3] * det4_1235_0124 + mat[0].p[4] * det4_1235_0123; float det5_01235_01235 = mat[0].p[0] * det4_1235_1235 - mat[0].p[1] * det4_1235_0235 + mat[0].p[2] * det4_1235_0135 - mat[0].p[3] * det4_1235_0125 + mat[0].p[5] * det4_1235_0123; float det5_01235_01245 = mat[0].p[0] * det4_1235_1245 - mat[0].p[1] * det4_1235_0245 + mat[0].p[2] * det4_1235_0145 - mat[0].p[4] * det4_1235_0125 + mat[0].p[5] * det4_1235_0124; float det5_01235_01345 = mat[0].p[0] * det4_1235_1345 - mat[0].p[1] * det4_1235_0345 + mat[0].p[3] * det4_1235_0145 - mat[0].p[4] * det4_1235_0135 + mat[0].p[5] * det4_1235_0134; float det5_01235_02345 = mat[0].p[0] * det4_1235_2345 - mat[0].p[2] * det4_1235_0345 + mat[0].p[3] * det4_1235_0245 - mat[0].p[4] * det4_1235_0235 + mat[0].p[5] * det4_1235_0234; float det5_01235_12345 = mat[0].p[1] * det4_1235_2345 - mat[0].p[2] * det4_1235_1345 + mat[0].p[3] * det4_1235_1245 - mat[0].p[4] * det4_1235_1235 + mat[0].p[5] * det4_1235_1234; float det5_01245_01234 = mat[0].p[0] * det4_1245_1234 - mat[0].p[1] * det4_1245_0234 + mat[0].p[2] * det4_1245_0134 - mat[0].p[3] * det4_1245_0124 + mat[0].p[4] * det4_1245_0123; float det5_01245_01235 = mat[0].p[0] * det4_1245_1235 - mat[0].p[1] * det4_1245_0235 + mat[0].p[2] * det4_1245_0135 - mat[0].p[3] * det4_1245_0125 + mat[0].p[5] * det4_1245_0123; float det5_01245_01245 = mat[0].p[0] * det4_1245_1245 - mat[0].p[1] * det4_1245_0245 + mat[0].p[2] * det4_1245_0145 - mat[0].p[4] * det4_1245_0125 + mat[0].p[5] * det4_1245_0124; float det5_01245_01345 = mat[0].p[0] * det4_1245_1345 - mat[0].p[1] * det4_1245_0345 + mat[0].p[3] * det4_1245_0145 - mat[0].p[4] * det4_1245_0135 + mat[0].p[5] * det4_1245_0134; float det5_01245_02345 = mat[0].p[0] * det4_1245_2345 - mat[0].p[2] * det4_1245_0345 + mat[0].p[3] * det4_1245_0245 - mat[0].p[4] * det4_1245_0235 + mat[0].p[5] * det4_1245_0234; float det5_01245_12345 = mat[0].p[1] * det4_1245_2345 - mat[0].p[2] * det4_1245_1345 + mat[0].p[3] * det4_1245_1245 - mat[0].p[4] * det4_1245_1235 + mat[0].p[5] * det4_1245_1234; float det5_01345_01234 = mat[0].p[0] * det4_1345_1234 - mat[0].p[1] * det4_1345_0234 + mat[0].p[2] * det4_1345_0134 - mat[0].p[3] * det4_1345_0124 + mat[0].p[4] * det4_1345_0123; float det5_01345_01235 = mat[0].p[0] * det4_1345_1235 - mat[0].p[1] * det4_1345_0235 + mat[0].p[2] * det4_1345_0135 - mat[0].p[3] * det4_1345_0125 + mat[0].p[5] * det4_1345_0123; float det5_01345_01245 = mat[0].p[0] * det4_1345_1245 - mat[0].p[1] * det4_1345_0245 + mat[0].p[2] * det4_1345_0145 - mat[0].p[4] * det4_1345_0125 + mat[0].p[5] * det4_1345_0124; float det5_01345_01345 = mat[0].p[0] * det4_1345_1345 - mat[0].p[1] * det4_1345_0345 + mat[0].p[3] * det4_1345_0145 - mat[0].p[4] * det4_1345_0135 + mat[0].p[5] * det4_1345_0134; float det5_01345_02345 = mat[0].p[0] * det4_1345_2345 - mat[0].p[2] * det4_1345_0345 + mat[0].p[3] * det4_1345_0245 - mat[0].p[4] * det4_1345_0235 + mat[0].p[5] * det4_1345_0234; float det5_01345_12345 = mat[0].p[1] * det4_1345_2345 - mat[0].p[2] * det4_1345_1345 + mat[0].p[3] * det4_1345_1245 - mat[0].p[4] * det4_1345_1235 + mat[0].p[5] * det4_1345_1234; float det5_02345_01234 = mat[0].p[0] * det4_2345_1234 - mat[0].p[1] * det4_2345_0234 + mat[0].p[2] * det4_2345_0134 - mat[0].p[3] * det4_2345_0124 + mat[0].p[4] * det4_2345_0123; float det5_02345_01235 = mat[0].p[0] * det4_2345_1235 - mat[0].p[1] * det4_2345_0235 + mat[0].p[2] * det4_2345_0135 - mat[0].p[3] * det4_2345_0125 + mat[0].p[5] * det4_2345_0123; float det5_02345_01245 = mat[0].p[0] * det4_2345_1245 - mat[0].p[1] * det4_2345_0245 + mat[0].p[2] * det4_2345_0145 - mat[0].p[4] * det4_2345_0125 + mat[0].p[5] * det4_2345_0124; float det5_02345_01345 = mat[0].p[0] * det4_2345_1345 - mat[0].p[1] * det4_2345_0345 + mat[0].p[3] * det4_2345_0145 - mat[0].p[4] * det4_2345_0135 + mat[0].p[5] * det4_2345_0134; float det5_02345_02345 = mat[0].p[0] * det4_2345_2345 - mat[0].p[2] * det4_2345_0345 + mat[0].p[3] * det4_2345_0245 - mat[0].p[4] * det4_2345_0235 + mat[0].p[5] * det4_2345_0234; float det5_02345_12345 = mat[0].p[1] * det4_2345_2345 - mat[0].p[2] * det4_2345_1345 + mat[0].p[3] * det4_2345_1245 - mat[0].p[4] * det4_2345_1235 + mat[0].p[5] * det4_2345_1234; // mat[0].p[0] = (float)(det5_12345_12345 * invDet); mat[0].p[1] = (float)(-det5_02345_12345 * invDet); mat[0].p[2] = (float)(det5_01345_12345 * invDet); mat[0].p[3] = (float)(-det5_01245_12345 * invDet); mat[0].p[4] = (float)(det5_01235_12345 * invDet); mat[0].p[5] = (float)(-det5_01234_12345 * invDet); // mat[1].p[0] = (float)(-det5_12345_02345 * invDet); mat[1].p[1] = (float)(det5_02345_02345 * invDet); mat[1].p[2] = (float)(-det5_01345_02345 * invDet); mat[1].p[3] = (float)(det5_01245_02345 * invDet); mat[1].p[4] = (float)(-det5_01235_02345 * invDet); mat[1].p[5] = (float)(det5_01234_02345 * invDet); // mat[2].p[0] = (float)(det5_12345_01345 * invDet); mat[2].p[1] = (float)(-det5_02345_01345 * invDet); mat[2].p[2] = (float)(det5_01345_01345 * invDet); mat[2].p[3] = (float)(-det5_01245_01345 * invDet); mat[2].p[4] = (float)(det5_01235_01345 * invDet); mat[2].p[5] = (float)(-det5_01234_01345 * invDet); // mat[3].p[0] = (float)(-det5_12345_01245 * invDet); mat[3].p[1] = (float)(det5_02345_01245 * invDet); mat[3].p[2] = (float)(-det5_01345_01245 * invDet); mat[3].p[3] = (float)(det5_01245_01245 * invDet); mat[3].p[4] = (float)(-det5_01235_01245 * invDet); mat[3].p[5] = (float)(det5_01234_01245 * invDet); // mat[4].p[0] = (float)(det5_12345_01235 * invDet); mat[4].p[1] = (float)(-det5_02345_01235 * invDet); mat[4].p[2] = (float)(det5_01345_01235 * invDet); mat[4].p[3] = (float)(-det5_01245_01235 * invDet); mat[4].p[4] = (float)(det5_01235_01235 * invDet); mat[4].p[5] = (float)(-det5_01234_01235 * invDet); // mat[5].p[0] = (float)(-det5_12345_01234 * invDet); mat[5].p[1] = (float)(det5_02345_01234 * invDet); mat[5].p[2] = (float)(-det5_01345_01234 * invDet); mat[5].p[3] = (float)(det5_01245_01234 * invDet); mat[5].p[4] = (float)(-det5_01235_01234 * invDet); mat[5].p[5] = (float)(det5_01234_01234 * invDet); return true; #elif false // 6*40 = 240 multiplications // 6 divisions float *mat = reinterpret_cast<float *>(this); float s; double d, di; di = mat[0]; s = di; mat[0] = d = 1.0f / di; mat[1] *= d; mat[2] *= d; mat[3] *= d; mat[4] *= d; mat[5] *= d; d = -d; mat[6] *= d; mat[12] *= d; mat[18] *= d; mat[24] *= d; mat[30] *= d; d = mat[6] * di; mat[7] += mat[1] * d; mat[8] += mat[2] * d; mat[9] += mat[3] * d; mat[10] += mat[4] * d; mat[11] += mat[5] * d; d = mat[12] * di; mat[13] += mat[1] * d; mat[14] += mat[2] * d; mat[15] += mat[3] * d; mat[16] += mat[4] * d; mat[17] += mat[5] * d; d = mat[18] * di; mat[19] += mat[1] * d; mat[20] += mat[2] * d; mat[21] += mat[3] * d; mat[22] += mat[4] * d; mat[23] += mat[5] * d; d = mat[24] * di; mat[25] += mat[1] * d; mat[26] += mat[2] * d; mat[27] += mat[3] * d; mat[28] += mat[4] * d; mat[29] += mat[5] * d; d = mat[30] * di; mat[31] += mat[1] * d; mat[32] += mat[2] * d; mat[33] += mat[3] * d; mat[34] += mat[4] * d; mat[35] += mat[5] * d; di = mat[7]; s *= di; mat[7] = d = 1.0f / di; mat[6] *= d; mat[8] *= d; mat[9] *= d; mat[10] *= d; mat[11] *= d; d = -d; mat[1] *= d; mat[13] *= d; mat[19] *= d; mat[25] *= d; mat[31] *= d; d = mat[1] * di; mat[0] += mat[6] * d; mat[2] += mat[8] * d; mat[3] += mat[9] * d; mat[4] += mat[10] * d; mat[5] += mat[11] * d; d = mat[13] * di; mat[12] += mat[6] * d; mat[14] += mat[8] * d; mat[15] += mat[9] * d; mat[16] += mat[10] * d; mat[17] += mat[11] * d; d = mat[19] * di; mat[18] += mat[6] * d; mat[20] += mat[8] * d; mat[21] += mat[9] * d; mat[22] += mat[10] * d; mat[23] += mat[11] * d; d = mat[25] * di; mat[24] += mat[6] * d; mat[26] += mat[8] * d; mat[27] += mat[9] * d; mat[28] += mat[10] * d; mat[29] += mat[11] * d; d = mat[31] * di; mat[30] += mat[6] * d; mat[32] += mat[8] * d; mat[33] += mat[9] * d; mat[34] += mat[10] * d; mat[35] += mat[11] * d; di = mat[14]; s *= di; mat[14] = d = 1.0f / di; mat[12] *= d; mat[13] *= d; mat[15] *= d; mat[16] *= d; mat[17] *= d; d = -d; mat[2] *= d; mat[8] *= d; mat[20] *= d; mat[26] *= d; mat[32] *= d; d = mat[2] * di; mat[0] += mat[12] * d; mat[1] += mat[13] * d; mat[3] += mat[15] * d; mat[4] += mat[16] * d; mat[5] += mat[17] * d; d = mat[8] * di; mat[6] += mat[12] * d; mat[7] += mat[13] * d; mat[9] += mat[15] * d; mat[10] += mat[16] * d; mat[11] += mat[17] * d; d = mat[20] * di; mat[18] += mat[12] * d; mat[19] += mat[13] * d; mat[21] += mat[15] * d; mat[22] += mat[16] * d; mat[23] += mat[17] * d; d = mat[26] * di; mat[24] += mat[12] * d; mat[25] += mat[13] * d; mat[27] += mat[15] * d; mat[28] += mat[16] * d; mat[29] += mat[17] * d; d = mat[32] * di; mat[30] += mat[12] * d; mat[31] += mat[13] * d; mat[33] += mat[15] * d; mat[34] += mat[16] * d; mat[35] += mat[17] * d; di = mat[21]; s *= di; mat[21] = d = 1.0f / di; mat[18] *= d; mat[19] *= d; mat[20] *= d; mat[22] *= d; mat[23] *= d; d = -d; mat[3] *= d; mat[9] *= d; mat[15] *= d; mat[27] *= d; mat[33] *= d; d = mat[3] * di; mat[0] += mat[18] * d; mat[1] += mat[19] * d; mat[2] += mat[20] * d; mat[4] += mat[22] * d; mat[5] += mat[23] * d; d = mat[9] * di; mat[6] += mat[18] * d; mat[7] += mat[19] * d; mat[8] += mat[20] * d; mat[10] += mat[22] * d; mat[11] += mat[23] * d; d = mat[15] * di; mat[12] += mat[18] * d; mat[13] += mat[19] * d; mat[14] += mat[20] * d; mat[16] += mat[22] * d; mat[17] += mat[23] * d; d = mat[27] * di; mat[24] += mat[18] * d; mat[25] += mat[19] * d; mat[26] += mat[20] * d; mat[28] += mat[22] * d; mat[29] += mat[23] * d; d = mat[33] * di; mat[30] += mat[18] * d; mat[31] += mat[19] * d; mat[32] += mat[20] * d; mat[34] += mat[22] * d; mat[35] += mat[23] * d; di = mat[28]; s *= di; mat[28] = d = 1.0f / di; mat[24] *= d; mat[25] *= d; mat[26] *= d; mat[27] *= d; mat[29] *= d; d = -d; mat[4] *= d; mat[10] *= d; mat[16] *= d; mat[22] *= d; mat[34] *= d; d = mat[4] * di; mat[0] += mat[24] * d; mat[1] += mat[25] * d; mat[2] += mat[26] * d; mat[3] += mat[27] * d; mat[5] += mat[29] * d; d = mat[10] * di; mat[6] += mat[24] * d; mat[7] += mat[25] * d; mat[8] += mat[26] * d; mat[9] += mat[27] * d; mat[11] += mat[29] * d; d = mat[16] * di; mat[12] += mat[24] * d; mat[13] += mat[25] * d; mat[14] += mat[26] * d; mat[15] += mat[27] * d; mat[17] += mat[29] * d; d = mat[22] * di; mat[18] += mat[24] * d; mat[19] += mat[25] * d; mat[20] += mat[26] * d; mat[21] += mat[27] * d; mat[23] += mat[29] * d; d = mat[34] * di; mat[30] += mat[24] * d; mat[31] += mat[25] * d; mat[32] += mat[26] * d; mat[33] += mat[27] * d; mat[35] += mat[29] * d; di = mat[35]; s *= di; mat[35] = d = 1.0f / di; mat[30] *= d; mat[31] *= d; mat[32] *= d; mat[33] *= d; mat[34] *= d; d = -d; mat[5] *= d; mat[11] *= d; mat[17] *= d; mat[23] *= d; mat[29] *= d; d = mat[5] * di; mat[0] += mat[30] * d; mat[1] += mat[31] * d; mat[2] += mat[32] * d; mat[3] += mat[33] * d; mat[4] += mat[34] * d; d = mat[11] * di; mat[6] += mat[30] * d; mat[7] += mat[31] * d; mat[8] += mat[32] * d; mat[9] += mat[33] * d; mat[10] += mat[34] * d; d = mat[17] * di; mat[12] += mat[30] * d; mat[13] += mat[31] * d; mat[14] += mat[32] * d; mat[15] += mat[33] * d; mat[16] += mat[34] * d; d = mat[23] * di; mat[18] += mat[30] * d; mat[19] += mat[31] * d; mat[20] += mat[32] * d; mat[21] += mat[33] * d; mat[22] += mat[34] * d; d = mat[29] * di; mat[24] += mat[30] * d; mat[25] += mat[31] * d; mat[26] += mat[32] * d; mat[27] += mat[33] * d; mat[28] += mat[34] * d; return ( s != 0.0f && !FLOAT_IS_NAN( s ) ); #else // 6*27+2*30 = 222 multiplications // 2*1 = 2 divisions float[] mat = this; // r0 = m0.Inverse(); float c0 = mat[1 * 6 + 1] * mat[2 * 6 + 2] - mat[1 * 6 + 2] * mat[2 * 6 + 1]; float c1 = mat[1 * 6 + 2] * mat[2 * 6 + 0] - mat[1 * 6 + 0] * mat[2 * 6 + 2]; float c2 = mat[1 * 6 + 0] * mat[2 * 6 + 1] - mat[1 * 6 + 1] * mat[2 * 6 + 0]; float det = mat[0 * 6 + 0] * c0 + mat[0 * 6 + 1] * c1 + mat[0 * 6 + 2] * c2; if (idMath.Fabs(det) < MATRIX_INVERSE_EPSILON) return false; float invDet = 1.0f / det; idMat3 r0 = new idMat3(); idVec3[] r0_mat = r0.mat; r0_mat[0].x = c0 * invDet; r0_mat[0].y = (mat[0 * 6 + 2] * mat[2 * 6 + 1] - mat[0 * 6 + 1] * mat[2 * 6 + 2]) * invDet; r0_mat[0].z = (mat[0 * 6 + 1] * mat[1 * 6 + 2] - mat[0 * 6 + 2] * mat[1 * 6 + 1]) * invDet; r0_mat[1].x = c1 * invDet; r0_mat[1].y = (mat[0 * 6 + 0] * mat[2 * 6 + 2] - mat[0 * 6 + 2] * mat[2 * 6 + 0]) * invDet; r0_mat[1].z = (mat[0 * 6 + 2] * mat[1 * 6 + 0] - mat[0 * 6 + 0] * mat[1 * 6 + 2]) * invDet; r0_mat[2].x = c2 * invDet; r0_mat[2].y = (mat[0 * 6 + 1] * mat[2 * 6 + 0] - mat[0 * 6 + 0] * mat[2 * 6 + 1]) * invDet; r0_mat[2].z = (mat[0 * 6 + 0] * mat[1 * 6 + 1] - mat[0 * 6 + 1] * mat[1 * 6 + 0]) * invDet; // r1 = r0 * m1; idMat3 r1 = new idMat3(); idVec3[] r1_mat = r1.mat; r1_mat[0].x = r0_mat[0].x * mat[0 * 6 + 3] + r0_mat[0].y * mat[1 * 6 + 3] + r0_mat[0].z * mat[2 * 6 + 3]; r1_mat[0].y = r0_mat[0].x * mat[0 * 6 + 4] + r0_mat[0].y * mat[1 * 6 + 4] + r0_mat[0].z * mat[2 * 6 + 4]; r1_mat[0].z = r0_mat[0].x * mat[0 * 6 + 5] + r0_mat[0].y * mat[1 * 6 + 5] + r0_mat[0].z * mat[2 * 6 + 5]; r1_mat[1].x = r0_mat[1].x * mat[0 * 6 + 3] + r0_mat[1].y * mat[1 * 6 + 3] + r0_mat[1].z * mat[2 * 6 + 3]; r1_mat[1].y = r0_mat[1].x * mat[0 * 6 + 4] + r0_mat[1].y * mat[1 * 6 + 4] + r0_mat[1].z * mat[2 * 6 + 4]; r1_mat[1].z = r0_mat[1].x * mat[0 * 6 + 5] + r0_mat[1].y * mat[1 * 6 + 5] + r0_mat[1].z * mat[2 * 6 + 5]; r1_mat[2].x = r0_mat[2].x * mat[0 * 6 + 3] + r0_mat[2].y * mat[1 * 6 + 3] + r0_mat[2].z * mat[2 * 6 + 3]; r1_mat[2].y = r0_mat[2].x * mat[0 * 6 + 4] + r0_mat[2].y * mat[1 * 6 + 4] + r0_mat[2].z * mat[2 * 6 + 4]; r1_mat[2].z = r0_mat[2].x * mat[0 * 6 + 5] + r0_mat[2].y * mat[1 * 6 + 5] + r0_mat[2].z * mat[2 * 6 + 5]; // r2 = m2 * r1; idMat3 r2 = new idMat3(); idVec3[] r2_mat = r2.mat; r2_mat[0].x = mat[3 * 6 + 0] * r1_mat[0].x + mat[3 * 6 + 1] * r1_mat[1].x + mat[3 * 6 + 2] * r1_mat[2].x; r2_mat[0].y = mat[3 * 6 + 0] * r1_mat[0].y + mat[3 * 6 + 1] * r1_mat[1].y + mat[3 * 6 + 2] * r1_mat[2].y; r2_mat[0].z = mat[3 * 6 + 0] * r1_mat[0].z + mat[3 * 6 + 1] * r1_mat[1].z + mat[3 * 6 + 2] * r1_mat[2].z; r2_mat[1].x = mat[4 * 6 + 0] * r1_mat[0].x + mat[4 * 6 + 1] * r1_mat[1].x + mat[4 * 6 + 2] * r1_mat[2].x; r2_mat[1].y = mat[4 * 6 + 0] * r1_mat[0].y + mat[4 * 6 + 1] * r1_mat[1].y + mat[4 * 6 + 2] * r1_mat[2].y; r2_mat[1].z = mat[4 * 6 + 0] * r1_mat[0].z + mat[4 * 6 + 1] * r1_mat[1].z + mat[4 * 6 + 2] * r1_mat[2].z; r2_mat[2].x = mat[5 * 6 + 0] * r1_mat[0].x + mat[5 * 6 + 1] * r1_mat[1].x + mat[5 * 6 + 2] * r1_mat[2].x; r2_mat[2].y = mat[5 * 6 + 0] * r1_mat[0].y + mat[5 * 6 + 1] * r1_mat[1].y + mat[5 * 6 + 2] * r1_mat[2].y; r2_mat[2].z = mat[5 * 6 + 0] * r1_mat[0].z + mat[5 * 6 + 1] * r1_mat[1].z + mat[5 * 6 + 2] * r1_mat[2].z; // r3 = r2 - m3; idMat3 r3 = new idMat3(); idVec3[] r3_mat = r3.mat; r3_mat[0].x = r2_mat[0].x - mat[3 * 6 + 3]; r3_mat[0].y = r2_mat[0].y - mat[3 * 6 + 4]; r3_mat[0].z = r2_mat[0].z - mat[3 * 6 + 5]; r3_mat[1].x = r2_mat[1].x - mat[4 * 6 + 3]; r3_mat[1].y = r2_mat[1].y - mat[4 * 6 + 4]; r3_mat[1].z = r2_mat[1].z - mat[4 * 6 + 5]; r3_mat[2].x = r2_mat[2].x - mat[5 * 6 + 3]; r3_mat[2].y = r2_mat[2].y - mat[5 * 6 + 4]; r3_mat[2].z = r2_mat[2].z - mat[5 * 6 + 5]; // r3.InverseSelf(); r2_mat[0].x = r3_mat[1].y * r3_mat[2].z - r3_mat[1].z * r3_mat[2].y; r2_mat[1].x = r3_mat[1].z * r3_mat[2].x - r3_mat[1].x * r3_mat[2].z; r2_mat[2].x = r3_mat[1].x * r3_mat[2].y - r3_mat[1].y * r3_mat[2].x; // det = r3_mat[0].x * r2_mat[0].x + r3_mat[0].y * r2_mat[1].x + r3_mat[0].z * r2_mat[2].x; if (idMath.Fabs(det) < MATRIX_INVERSE_EPSILON) return false; invDet = 1.0f / det; // r2_mat[0].y = r3_mat[0].z * r3_mat[2].y - r3_mat[0].y * r3_mat[2].z; r2_mat[0].z = r3_mat[0].y * r3_mat[1].z - r3_mat[0].z * r3_mat[1].y; r2_mat[1].y = r3_mat[0].x * r3_mat[2].z - r3_mat[0].z * r3_mat[2].x; r2_mat[1].z = r3_mat[0].z * r3_mat[1].x - r3_mat[0].x * r3_mat[1].z; r2_mat[2].y = r3_mat[0].y * r3_mat[2].x - r3_mat[0].x * r3_mat[2].y; r2_mat[2].z = r3_mat[0].x * r3_mat[1].y - r3_mat[0].y * r3_mat[1].x; // r3_mat[0].x = r2_mat[0].x * invDet; r3_mat[0].y = r2_mat[0].y * invDet; r3_mat[0].z = r2_mat[0].z * invDet; r3_mat[1].x = r2_mat[1].x * invDet; r3_mat[1].y = r2_mat[1].y * invDet; r3_mat[1].z = r2_mat[1].z * invDet; r3_mat[2].x = r2_mat[2].x * invDet; r3_mat[2].y = r2_mat[2].y * invDet; r3_mat[2].z = r2_mat[2].z * invDet; // r2 = m2 * r0; r2_mat[0].x = mat[3 * 6 + 0] * r0_mat[0].x + mat[3 * 6 + 1] * r0_mat[1].x + mat[3 * 6 + 2] * r0_mat[2].x; r2_mat[0].y = mat[3 * 6 + 0] * r0_mat[0].y + mat[3 * 6 + 1] * r0_mat[1].y + mat[3 * 6 + 2] * r0_mat[2].y; r2_mat[0].z = mat[3 * 6 + 0] * r0_mat[0].z + mat[3 * 6 + 1] * r0_mat[1].z + mat[3 * 6 + 2] * r0_mat[2].z; r2_mat[1].x = mat[4 * 6 + 0] * r0_mat[0].x + mat[4 * 6 + 1] * r0_mat[1].x + mat[4 * 6 + 2] * r0_mat[2].x; r2_mat[1].y = mat[4 * 6 + 0] * r0_mat[0].y + mat[4 * 6 + 1] * r0_mat[1].y + mat[4 * 6 + 2] * r0_mat[2].y; r2_mat[1].z = mat[4 * 6 + 0] * r0_mat[0].z + mat[4 * 6 + 1] * r0_mat[1].z + mat[4 * 6 + 2] * r0_mat[2].z; r2_mat[2].x = mat[5 * 6 + 0] * r0_mat[0].x + mat[5 * 6 + 1] * r0_mat[1].x + mat[5 * 6 + 2] * r0_mat[2].x; r2_mat[2].y = mat[5 * 6 + 0] * r0_mat[0].y + mat[5 * 6 + 1] * r0_mat[1].y + mat[5 * 6 + 2] * r0_mat[2].y; r2_mat[2].z = mat[5 * 6 + 0] * r0_mat[0].z + mat[5 * 6 + 1] * r0_mat[1].z + mat[5 * 6 + 2] * r0_mat[2].z; // m2 = r3 * r2; mat[3 * 6 + 0] = r3_mat[0].x * r2_mat[0].x + r3_mat[0].y * r2_mat[1].x + r3_mat[0].z * r2_mat[2].x; mat[3 * 6 + 1] = r3_mat[0].x * r2_mat[0].y + r3_mat[0].y * r2_mat[1].y + r3_mat[0].z * r2_mat[2].y; mat[3 * 6 + 2] = r3_mat[0].x * r2_mat[0].z + r3_mat[0].y * r2_mat[1].z + r3_mat[0].z * r2_mat[2].z; mat[4 * 6 + 0] = r3_mat[1].x * r2_mat[0].x + r3_mat[1].y * r2_mat[1].x + r3_mat[1].z * r2_mat[2].x; mat[4 * 6 + 1] = r3_mat[1].x * r2_mat[0].y + r3_mat[1].y * r2_mat[1].y + r3_mat[1].z * r2_mat[2].y; mat[4 * 6 + 2] = r3_mat[1].x * r2_mat[0].z + r3_mat[1].y * r2_mat[1].z + r3_mat[1].z * r2_mat[2].z; mat[5 * 6 + 0] = r3_mat[2].x * r2_mat[0].x + r3_mat[2].y * r2_mat[1].x + r3_mat[2].z * r2_mat[2].x; mat[5 * 6 + 1] = r3_mat[2].x * r2_mat[0].y + r3_mat[2].y * r2_mat[1].y + r3_mat[2].z * r2_mat[2].y; mat[5 * 6 + 2] = r3_mat[2].x * r2_mat[0].z + r3_mat[2].y * r2_mat[1].z + r3_mat[2].z * r2_mat[2].z; // m0 = r0 - r1 * m2; mat[0 * 6 + 0] = r0_mat[0].x - r1_mat[0].x * mat[3 * 6 + 0] - r1_mat[0].y * mat[4 * 6 + 0] - r1_mat[0].z * mat[5 * 6 + 0]; mat[0 * 6 + 1] = r0_mat[0].y - r1_mat[0].x * mat[3 * 6 + 1] - r1_mat[0].y * mat[4 * 6 + 1] - r1_mat[0].z * mat[5 * 6 + 1]; mat[0 * 6 + 2] = r0_mat[0].z - r1_mat[0].x * mat[3 * 6 + 2] - r1_mat[0].y * mat[4 * 6 + 2] - r1_mat[0].z * mat[5 * 6 + 2]; mat[1 * 6 + 0] = r0_mat[1].x - r1_mat[1].x * mat[3 * 6 + 0] - r1_mat[1].y * mat[4 * 6 + 0] - r1_mat[1].z * mat[5 * 6 + 0]; mat[1 * 6 + 1] = r0_mat[1].y - r1_mat[1].x * mat[3 * 6 + 1] - r1_mat[1].y * mat[4 * 6 + 1] - r1_mat[1].z * mat[5 * 6 + 1]; mat[1 * 6 + 2] = r0_mat[1].z - r1_mat[1].x * mat[3 * 6 + 2] - r1_mat[1].y * mat[4 * 6 + 2] - r1_mat[1].z * mat[5 * 6 + 2]; mat[2 * 6 + 0] = r0_mat[2].x - r1_mat[2].x * mat[3 * 6 + 0] - r1_mat[2].y * mat[4 * 6 + 0] - r1_mat[2].z * mat[5 * 6 + 0]; mat[2 * 6 + 1] = r0_mat[2].y - r1_mat[2].x * mat[3 * 6 + 1] - r1_mat[2].y * mat[4 * 6 + 1] - r1_mat[2].z * mat[5 * 6 + 1]; mat[2 * 6 + 2] = r0_mat[2].z - r1_mat[2].x * mat[3 * 6 + 2] - r1_mat[2].y * mat[4 * 6 + 2] - r1_mat[2].z * mat[5 * 6 + 2]; // m1 = r1 * r3; mat[0 * 6 + 3] = r1_mat[0].x * r3_mat[0].x + r1_mat[0].y * r3_mat[1].x + r1_mat[0].z * r3_mat[2].x; mat[0 * 6 + 4] = r1_mat[0].x * r3_mat[0].y + r1_mat[0].y * r3_mat[1].y + r1_mat[0].z * r3_mat[2].y; mat[0 * 6 + 5] = r1_mat[0].x * r3_mat[0].z + r1_mat[0].y * r3_mat[1].z + r1_mat[0].z * r3_mat[2].z; mat[1 * 6 + 3] = r1_mat[1].x * r3_mat[0].x + r1_mat[1].y * r3_mat[1].x + r1_mat[1].z * r3_mat[2].x; mat[1 * 6 + 4] = r1_mat[1].x * r3_mat[0].y + r1_mat[1].y * r3_mat[1].y + r1_mat[1].z * r3_mat[2].y; mat[1 * 6 + 5] = r1_mat[1].x * r3_mat[0].z + r1_mat[1].y * r3_mat[1].z + r1_mat[1].z * r3_mat[2].z; mat[2 * 6 + 3] = r1_mat[2].x * r3_mat[0].x + r1_mat[2].y * r3_mat[1].x + r1_mat[2].z * r3_mat[2].x; mat[2 * 6 + 4] = r1_mat[2].x * r3_mat[0].y + r1_mat[2].y * r3_mat[1].y + r1_mat[2].z * r3_mat[2].y; mat[2 * 6 + 5] = r1_mat[2].x * r3_mat[0].z + r1_mat[2].y * r3_mat[1].z + r1_mat[2].z * r3_mat[2].z; // m3 = -r3; mat[3 * 6 + 3] = -r3_mat[0].x; mat[3 * 6 + 4] = -r3_mat[0].y; mat[3 * 6 + 5] = -r3_mat[0].z; mat[4 * 6 + 3] = -r3_mat[1].x; mat[4 * 6 + 4] = -r3_mat[1].y; mat[4 * 6 + 5] = -r3_mat[1].z; mat[5 * 6 + 3] = -r3_mat[2].x; mat[5 * 6 + 4] = -r3_mat[2].y; mat[5 * 6 + 5] = -r3_mat[2].z; return true; #endif }
public bool InverseFastSelf() { #if true // 18+3+9 = 30 multiplications // 1 division idMat3 inverse = new idMat3(); idVec3[] inverse_mat = inverse.mat; inverse_mat[0].x = mat[1].y * mat[2].z - mat[1].z * mat[2].y; inverse_mat[1].x = mat[1].z * mat[2].x - mat[1].x * mat[2].z; inverse_mat[2].x = mat[1].x * mat[2].y - mat[1].y * mat[2].x; double det = mat[0].x * inverse_mat[0].x + mat[0].y * inverse_mat[1].x + mat[0].z * inverse_mat[2].x; if (idMath.Fabs(det) < MATRIX_INVERSE_EPSILON) { return(false); } double invDet = 1.0f / det; inverse_mat[0].y = mat[0].z * mat[2].y - mat[0].y * mat[2].z; inverse_mat[0].z = mat[0].y * mat[1].z - mat[0].z * mat[1].y; inverse_mat[1].y = mat[0].x * mat[2].z - mat[0].z * mat[2].x; inverse_mat[1].z = mat[0].z * mat[1].x - mat[0].x * mat[1].z; inverse_mat[2].y = mat[0].y * mat[2].x - mat[0].x * mat[2].y; inverse_mat[2].z = mat[0].x * mat[1].y - mat[0].y * mat[1].x; mat[0].x = (float)(inverse_mat[0].x * invDet); mat[0].y = (float)(inverse_mat[0].y * invDet); mat[0].z = (float)(inverse_mat[0].z * invDet); mat[1].x = (float)(inverse_mat[1].x * invDet); mat[1].y = (float)(inverse_mat[1].y * invDet); mat[1].z = (float)(inverse_mat[1].z * invDet); mat[2].x = (float)(inverse_mat[2].x * invDet); mat[2].y = (float)(inverse_mat[2].y * invDet); mat[2].z = (float)(inverse_mat[2].z * invDet); return(true); #elif false // 3*10 = 30 multiplications // 3 divisions float *mat = reinterpret_cast <float *>(this); float s; double d, di; di = mat[0]; s = di; mat[0] = d = 1.0f / di; mat[1] *= d; mat[2] *= d; d = -d; mat[3] *= d; mat[6] *= d; d = mat[3] * di; mat[4] += mat[1] * d; mat[5] += mat[2] * d; d = mat[6] * di; mat[7] += mat[1] * d; mat[8] += mat[2] * d; di = mat[4]; s *= di; mat[4] = d = 1.0f / di; mat[3] *= d; mat[5] *= d; d = -d; mat[1] *= d; mat[7] *= d; d = mat[1] * di; mat[0] += mat[3] * d; mat[2] += mat[5] * d; d = mat[7] * di; mat[6] += mat[3] * d; mat[8] += mat[5] * d; di = mat[8]; s *= di; mat[8] = d = 1.0f / di; mat[6] *= d; mat[7] *= d; d = -d; mat[2] *= d; mat[5] *= d; d = mat[2] * di; mat[0] += mat[6] * d; mat[1] += mat[7] * d; d = mat[5] * di; mat[3] += mat[6] * d; mat[4] += mat[7] * d; return(s != 0.0f && !FLOAT_IS_NAN(s)); #else // 4*2+4*4 = 24 multiplications // 2*1 = 2 divisions idMat2 r0; float r1[2], r2[2], r3;