mirror of
https://github.com/jmarshall23/DoomRTX.git
synced 2026-08-12 08:11:10 +02:00
Justin Marshall
8331 lines
246 KiB
C++
8331 lines
246 KiB
C++
/*
|
|
===========================================================================
|
|
|
|
IceTech GPL Source Code
|
|
Copyright (C) 2026 Justin Marshall
|
|
|
|
This file is part of the IceTech GPL Source Code (?IceTech Source Code?).
|
|
|
|
IceTech Source Code is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
IceTech Source Code is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with IceTech Source Code. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
In addition, the IceTech Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the IceTech Source Code. If not, please request a copy in writing from id Software at the address below.
|
|
|
|
If you have questions concerning this license or the applicable additional terms, you may contact in writing Justin Marshall, justinmarshall20@gmail.com
|
|
|
|
===========================================================================
|
|
*/
|
|
|
|
#include "precompiled.h"
|
|
#pragma hdrstop
|
|
|
|
|
|
//===============================================================
|
|
//
|
|
// idMat2
|
|
//
|
|
//===============================================================
|
|
|
|
idMat2 mat2_zero(idVec2(0, 0), idVec2(0, 0));
|
|
idMat2 mat2_identity(idVec2(1, 0), idVec2(0, 1));
|
|
|
|
/*
|
|
============
|
|
idMat2::InverseSelf
|
|
============
|
|
*/
|
|
bool idMat2::InverseSelf(void) {
|
|
// 2+4 = 6 multiplications
|
|
// 1 division
|
|
double det, invDet, a;
|
|
|
|
det = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
|
|
|
|
if (idMath::Fabs(det) < MATRIX_INVERSE_EPSILON) {
|
|
return false;
|
|
}
|
|
|
|
invDet = 1.0f / det;
|
|
|
|
a = mat[0][0];
|
|
mat[0][0] = mat[1][1] * invDet;
|
|
mat[0][1] = -mat[0][1] * invDet;
|
|
mat[1][0] = -mat[1][0] * invDet;
|
|
mat[1][1] = a * invDet;
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMat2::InverseFastSelf
|
|
============
|
|
*/
|
|
bool idMat2::InverseFastSelf(void) {
|
|
#if 1
|
|
// 2+4 = 6 multiplications
|
|
// 1 division
|
|
double det, invDet, a;
|
|
|
|
det = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
|
|
|
|
if (idMath::Fabs(det) < MATRIX_INVERSE_EPSILON) {
|
|
return false;
|
|
}
|
|
|
|
invDet = 1.0f / det;
|
|
|
|
a = mat[0][0];
|
|
mat[0][0] = mat[1][1] * invDet;
|
|
mat[0][1] = -mat[0][1] * invDet;
|
|
mat[1][0] = -mat[1][0] * invDet;
|
|
mat[1][1] = a * invDet;
|
|
|
|
return true;
|
|
#else
|
|
// 2*4 = 8 multiplications
|
|
// 2 division
|
|
float* mat = reinterpret_cast<float*>(this);
|
|
double d, di;
|
|
float s;
|
|
|
|
di = mat[0];
|
|
s = di;
|
|
mat[0 * 2 + 0] = d = 1.0f / di;
|
|
mat[0 * 2 + 1] *= d;
|
|
d = -d;
|
|
mat[1 * 2 + 0] *= d;
|
|
d = mat[1 * 2 + 0] * di;
|
|
mat[1 * 2 + 1] += mat[0 * 2 + 1] * d;
|
|
di = mat[1 * 2 + 1];
|
|
s *= di;
|
|
mat[1 * 2 + 1] = d = 1.0f / di;
|
|
mat[1 * 2 + 0] *= d;
|
|
d = -d;
|
|
mat[0 * 2 + 1] *= d;
|
|
d = mat[0 * 2 + 1] * di;
|
|
mat[0 * 2 + 0] += mat[1 * 2 + 0] * d;
|
|
|
|
return (s != 0.0f && !FLOAT_IS_NAN(s));
|
|
#endif
|
|
}
|
|
|
|
/*
|
|
=============
|
|
idMat2::ToString
|
|
=============
|
|
*/
|
|
const char* idMat2::ToString(int precision) const {
|
|
return idStr::FloatArrayToString(ToFloatPtr(), GetDimension(), precision);
|
|
}
|
|
|
|
|
|
//===============================================================
|
|
//
|
|
// idMat3
|
|
//
|
|
//===============================================================
|
|
|
|
idMat3 mat3_zero(idVec3(0, 0, 0), idVec3(0, 0, 0), idVec3(0, 0, 0));
|
|
idMat3 mat3_identity(idVec3(1, 0, 0), idVec3(0, 1, 0), idVec3(0, 0, 1));
|
|
|
|
/*
|
|
============
|
|
idMat3::ToAngles
|
|
============
|
|
*/
|
|
idAngles idMat3::ToAngles(void) const {
|
|
idAngles angles;
|
|
double theta;
|
|
double cp;
|
|
float sp;
|
|
|
|
sp = mat[0][2];
|
|
|
|
// cap off our sin value so that we don't get any NANs
|
|
if (sp > 1.0f) {
|
|
sp = 1.0f;
|
|
}
|
|
else if (sp < -1.0f) {
|
|
sp = -1.0f;
|
|
}
|
|
|
|
theta = -asin(sp);
|
|
#ifdef QUAKE4
|
|
cp = idMath::Cos(theta);
|
|
|
|
if (cp > 8192.0f * idMath::FLOAT_EPSILON) {
|
|
angles.pitch = RAD2DEG(theta);
|
|
angles.yaw = RAD2DEG(idMath::ATan(mat[0][1], mat[0][0]));
|
|
angles.roll = RAD2DEG(idMath::ATan(mat[1][2], mat[2][2]));
|
|
}
|
|
else {
|
|
angles.pitch = RAD2DEG(theta);
|
|
angles.yaw = RAD2DEG(-idMath::ATan(mat[1][0], mat[1][1]));
|
|
angles.roll = 0;
|
|
}
|
|
#else
|
|
cp = cos(theta);
|
|
|
|
if (cp > 8192.0f * idMath::FLT_EPSILON) {
|
|
angles.pitch = RAD2DEG(theta);
|
|
angles.yaw = RAD2DEG(atan2(mat[0][1], mat[0][0]));
|
|
angles.roll = RAD2DEG(atan2(mat[1][2], mat[2][2]));
|
|
}
|
|
else {
|
|
angles.pitch = RAD2DEG(theta);
|
|
angles.yaw = RAD2DEG(-atan2(mat[1][0], mat[1][1]));
|
|
angles.roll = 0;
|
|
}
|
|
#endif
|
|
return angles;
|
|
}
|
|
|
|
#ifdef QUAKE4
|
|
// idMat3::RotateAbsolute -- rotates around world x, y, or z
|
|
void idMat3::RotateAbsolute(int whichAxis, float howManyDegrees)
|
|
{
|
|
idMat3 rotation;
|
|
|
|
float sinVal, cosVal;
|
|
|
|
idMath::SinCos(DEG2RAD(howManyDegrees), sinVal, cosVal);
|
|
|
|
rotation.Identity();
|
|
switch (whichAxis)
|
|
{
|
|
case 0: // x-axis
|
|
rotation[1][1] = cosVal;
|
|
rotation[1][2] = sinVal;
|
|
rotation[2][1] = -sinVal;
|
|
rotation[2][2] = cosVal;
|
|
break;
|
|
case 1: // y-axis
|
|
rotation[0][0] = cosVal;
|
|
rotation[0][2] = -sinVal;
|
|
rotation[2][0] = sinVal;
|
|
rotation[2][2] = cosVal;
|
|
break;
|
|
case 2: // z-axis
|
|
rotation[0][0] = cosVal;
|
|
rotation[0][1] = sinVal;
|
|
rotation[1][0] = -sinVal;
|
|
rotation[1][1] = cosVal;
|
|
break;
|
|
default:
|
|
assert(!"idMat3::RotateAbsolute -- valid axis numbers are 0, 1, and 2");
|
|
return;
|
|
}
|
|
*this *= rotation;
|
|
}
|
|
|
|
// idMat3::RotateRelative -- rotates around the fwd, left, or up vector of the matrix
|
|
void idMat3::RotateRelative(int whichAxis, float howManyDegrees)
|
|
{
|
|
idMat3 rotation;
|
|
|
|
float sinVal, cosVal;
|
|
|
|
idMath::SinCos(DEG2RAD(howManyDegrees), sinVal, cosVal);
|
|
|
|
rotation.Identity();
|
|
switch (whichAxis)
|
|
{
|
|
case 0: // x-axis
|
|
rotation[1][1] = cosVal;
|
|
rotation[1][2] = sinVal;
|
|
rotation[2][1] = -sinVal;
|
|
rotation[2][2] = cosVal;
|
|
break;
|
|
case 1: // y-axis
|
|
rotation[0][0] = cosVal;
|
|
rotation[0][2] = -sinVal;
|
|
rotation[2][0] = sinVal;
|
|
rotation[2][2] = cosVal;
|
|
break;
|
|
case 2: // z-axis
|
|
rotation[0][0] = cosVal;
|
|
rotation[0][1] = sinVal;
|
|
rotation[1][0] = -sinVal;
|
|
rotation[1][1] = cosVal;
|
|
break;
|
|
default:
|
|
assert(!"idMat3::RotateRelative -- valid axis numbers are 0, 1, and 2");
|
|
return;
|
|
}
|
|
*this = rotation * (*this);
|
|
}
|
|
|
|
// idMat3::RotateArbitrary -- rotates around the given unit vector
|
|
void idMat3::RotateArbitrary(const idVec3& rotAxis, float howManyDegrees)
|
|
{
|
|
float sinVal, cosVal;
|
|
|
|
idMath::SinCos(DEG2RAD(howManyDegrees), sinVal, cosVal);
|
|
|
|
float d = idMath::Sqrt(rotAxis[1] * rotAxis[1] + rotAxis[2] * rotAxis[2]);
|
|
|
|
// if the rotation axis turns out to be one of the world axes, just do a RotateAbsolute on it
|
|
if (rotAxis.Compare(idVec3(1, 0, 0)))
|
|
{
|
|
RotateAbsolute(0, howManyDegrees);
|
|
return;
|
|
}
|
|
if (rotAxis.Compare(idVec3(-1, 0, 0)))
|
|
{
|
|
RotateAbsolute(0, -howManyDegrees);
|
|
return;
|
|
}
|
|
if (rotAxis.Compare(idVec3(0, 1, 0)))
|
|
{
|
|
RotateAbsolute(1, howManyDegrees);
|
|
return;
|
|
}
|
|
if (rotAxis.Compare(idVec3(0, -1, 0)))
|
|
{
|
|
RotateAbsolute(1, -howManyDegrees);
|
|
return;
|
|
}
|
|
if (rotAxis.Compare(idVec3(0, 0, 1)))
|
|
{
|
|
RotateAbsolute(2, howManyDegrees);
|
|
return;
|
|
}
|
|
if (rotAxis.Compare(idVec3(0, 0, -1)))
|
|
{
|
|
RotateAbsolute(2, -howManyDegrees);
|
|
return;
|
|
}
|
|
|
|
|
|
idMat3 rotationX(1, 0, 0, 0, rotAxis[2] / d, rotAxis[1] / d, 0, -rotAxis[1] / d, rotAxis[2] / d);
|
|
idMat3 rotationY(d, 0, rotAxis[0], 0, 1, 0, -rotAxis[0], 0, d);
|
|
idMat3 rotationZ(cosVal, sinVal, 0, -sinVal, cosVal, 0, 0, 0, 1);
|
|
idMat3 rotationXinv = rotationX.Inverse();
|
|
idMat3 rotationYinv = rotationY.Inverse();
|
|
idMat3 tempMat;
|
|
|
|
tempMat = rotationYinv * rotationXinv;
|
|
tempMat = rotationZ * tempMat;
|
|
tempMat = rotationY * tempMat;
|
|
tempMat = rotationX * tempMat;
|
|
*this = *this * tempMat;
|
|
}
|
|
|
|
#endif
|
|
|
|
/*
|
|
============
|
|
idMat3::ToQuat
|
|
============
|
|
*/
|
|
idQuat idMat3::ToQuat(void) const {
|
|
idQuat q;
|
|
float trace;
|
|
float s;
|
|
float t;
|
|
int i;
|
|
int j;
|
|
int k;
|
|
|
|
static int next[3] = { 1, 2, 0 };
|
|
|
|
trace = mat[0][0] + mat[1][1] + mat[2][2];
|
|
|
|
if (trace > 0.0f) {
|
|
|
|
t = trace + 1.0f;
|
|
s = idMath::InvSqrt(t) * 0.5f;
|
|
|
|
q[3] = s * t;
|
|
q[0] = (mat[2][1] - mat[1][2]) * s;
|
|
q[1] = (mat[0][2] - mat[2][0]) * s;
|
|
q[2] = (mat[1][0] - mat[0][1]) * s;
|
|
|
|
}
|
|
else {
|
|
|
|
i = 0;
|
|
if (mat[1][1] > mat[0][0]) {
|
|
i = 1;
|
|
}
|
|
if (mat[2][2] > mat[i][i]) {
|
|
i = 2;
|
|
}
|
|
j = next[i];
|
|
k = next[j];
|
|
|
|
t = (mat[i][i] - (mat[j][j] + mat[k][k])) + 1.0f;
|
|
s = idMath::InvSqrt(t) * 0.5f;
|
|
|
|
q[i] = s * t;
|
|
q[3] = (mat[k][j] - mat[j][k]) * s;
|
|
q[j] = (mat[j][i] + mat[i][j]) * s;
|
|
q[k] = (mat[k][i] + mat[i][k]) * s;
|
|
}
|
|
return q;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMat3::ToCQuat
|
|
============
|
|
*/
|
|
idCQuat idMat3::ToCQuat(void) const {
|
|
idQuat q = ToQuat();
|
|
if (q.w < 0.0f) {
|
|
return idCQuat(-q.x, -q.y, -q.z);
|
|
}
|
|
return idCQuat(q.x, q.y, q.z);
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMat3::ToRotation
|
|
============
|
|
*/
|
|
idRotation idMat3::ToRotation(void) const {
|
|
idRotation r;
|
|
float trace;
|
|
float s;
|
|
float t;
|
|
int i;
|
|
int j;
|
|
int k;
|
|
static int next[3] = { 1, 2, 0 };
|
|
|
|
trace = mat[0][0] + mat[1][1] + mat[2][2];
|
|
if (trace > 0.0f) {
|
|
|
|
t = trace + 1.0f;
|
|
s = idMath::InvSqrt(t) * 0.5f;
|
|
|
|
r.angle = s * t;
|
|
r.vec[0] = (mat[2][1] - mat[1][2]) * s;
|
|
r.vec[1] = (mat[0][2] - mat[2][0]) * s;
|
|
r.vec[2] = (mat[1][0] - mat[0][1]) * s;
|
|
|
|
}
|
|
else {
|
|
|
|
i = 0;
|
|
if (mat[1][1] > mat[0][0]) {
|
|
i = 1;
|
|
}
|
|
if (mat[2][2] > mat[i][i]) {
|
|
i = 2;
|
|
}
|
|
j = next[i];
|
|
k = next[j];
|
|
|
|
t = (mat[i][i] - (mat[j][j] + mat[k][k])) + 1.0f;
|
|
s = idMath::InvSqrt(t) * 0.5f;
|
|
|
|
r.vec[i] = s * t;
|
|
r.angle = (mat[k][j] - mat[j][k]) * s;
|
|
r.vec[j] = (mat[j][i] + mat[i][j]) * s;
|
|
r.vec[k] = (mat[k][i] + mat[i][k]) * s;
|
|
}
|
|
r.angle = idMath::ACos(r.angle);
|
|
if (idMath::Fabs(r.angle) < 1e-10f) {
|
|
r.vec.Set(0.0f, 0.0f, 1.0f);
|
|
r.angle = 0.0f;
|
|
}
|
|
else {
|
|
//vec *= (1.0f / sin( angle ));
|
|
r.vec.Normalize();
|
|
r.vec.FixDegenerateNormal();
|
|
r.angle *= 2.0f * idMath::M_RAD2DEG;
|
|
}
|
|
|
|
r.origin.Zero();
|
|
r.axis = *this;
|
|
r.axisValid = true;
|
|
return r;
|
|
}
|
|
|
|
/*
|
|
=================
|
|
idMat3::ToAngularVelocity
|
|
=================
|
|
*/
|
|
idVec3 idMat3::ToAngularVelocity(void) const {
|
|
idRotation rotation = ToRotation();
|
|
return rotation.GetVec() * DEG2RAD(rotation.GetAngle());
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMat3::Determinant
|
|
============
|
|
*/
|
|
float idMat3::Determinant(void) const {
|
|
|
|
float det2_12_01 = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];
|
|
float det2_12_02 = mat[1][0] * mat[2][2] - mat[1][2] * mat[2][0];
|
|
float det2_12_12 = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1];
|
|
|
|
return mat[0][0] * det2_12_12 - mat[0][1] * det2_12_02 + mat[0][2] * det2_12_01;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMat3::InverseSelf
|
|
============
|
|
*/
|
|
bool idMat3::InverseSelf(void) {
|
|
// 18+3+9 = 30 multiplications
|
|
// 1 division
|
|
idMat3 inverse;
|
|
double det, invDet;
|
|
|
|
inverse[0][0] = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1];
|
|
inverse[1][0] = mat[1][2] * mat[2][0] - mat[1][0] * mat[2][2];
|
|
inverse[2][0] = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];
|
|
|
|
det = mat[0][0] * inverse[0][0] + mat[0][1] * inverse[1][0] + mat[0][2] * inverse[2][0];
|
|
|
|
if (idMath::Fabs(det) < MATRIX_INVERSE_EPSILON) {
|
|
return false;
|
|
}
|
|
|
|
invDet = 1.0f / det;
|
|
|
|
inverse[0][1] = mat[0][2] * mat[2][1] - mat[0][1] * mat[2][2];
|
|
inverse[0][2] = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
|
|
inverse[1][1] = mat[0][0] * mat[2][2] - mat[0][2] * mat[2][0];
|
|
inverse[1][2] = mat[0][2] * mat[1][0] - mat[0][0] * mat[1][2];
|
|
inverse[2][1] = mat[0][1] * mat[2][0] - mat[0][0] * mat[2][1];
|
|
inverse[2][2] = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
|
|
|
|
mat[0][0] = inverse[0][0] * invDet;
|
|
mat[0][1] = inverse[0][1] * invDet;
|
|
mat[0][2] = inverse[0][2] * invDet;
|
|
|
|
mat[1][0] = inverse[1][0] * invDet;
|
|
mat[1][1] = inverse[1][1] * invDet;
|
|
mat[1][2] = inverse[1][2] * invDet;
|
|
|
|
mat[2][0] = inverse[2][0] * invDet;
|
|
mat[2][1] = inverse[2][1] * invDet;
|
|
mat[2][2] = inverse[2][2] * invDet;
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMat3::InverseFastSelf
|
|
============
|
|
*/
|
|
bool idMat3::InverseFastSelf(void) {
|
|
#if 1
|
|
// 18+3+9 = 30 multiplications
|
|
// 1 division
|
|
idMat3 inverse;
|
|
double det, invDet;
|
|
|
|
inverse[0][0] = mat[1][1] * mat[2][2] - mat[1][2] * mat[2][1];
|
|
inverse[1][0] = mat[1][2] * mat[2][0] - mat[1][0] * mat[2][2];
|
|
inverse[2][0] = mat[1][0] * mat[2][1] - mat[1][1] * mat[2][0];
|
|
|
|
det = mat[0][0] * inverse[0][0] + mat[0][1] * inverse[1][0] + mat[0][2] * inverse[2][0];
|
|
|
|
if (idMath::Fabs(det) < MATRIX_INVERSE_EPSILON) {
|
|
return false;
|
|
}
|
|
|
|
invDet = 1.0f / det;
|
|
|
|
inverse[0][1] = mat[0][2] * mat[2][1] - mat[0][1] * mat[2][2];
|
|
inverse[0][2] = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
|
|
inverse[1][1] = mat[0][0] * mat[2][2] - mat[0][2] * mat[2][0];
|
|
inverse[1][2] = mat[0][2] * mat[1][0] - mat[0][0] * mat[1][2];
|
|
inverse[2][1] = mat[0][1] * mat[2][0] - mat[0][0] * mat[2][1];
|
|
inverse[2][2] = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
|
|
|
|
mat[0][0] = inverse[0][0] * invDet;
|
|
mat[0][1] = inverse[0][1] * invDet;
|
|
mat[0][2] = inverse[0][2] * invDet;
|
|
|
|
mat[1][0] = inverse[1][0] * invDet;
|
|
mat[1][1] = inverse[1][1] * invDet;
|
|
mat[1][2] = inverse[1][2] * invDet;
|
|
|
|
mat[2][0] = inverse[2][0] * invDet;
|
|
mat[2][1] = inverse[2][1] * invDet;
|
|
mat[2][2] = inverse[2][2] * invDet;
|
|
|
|
return true;
|
|
#elif 0
|
|
// 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;
|
|
float det, invDet;
|
|
float* mat = reinterpret_cast<float*>(this);
|
|
|
|
// r0 = m0.Inverse(); // 2x2
|
|
det = mat[0 * 3 + 0] * mat[1 * 3 + 1] - mat[0 * 3 + 1] * mat[1 * 3 + 0];
|
|
|
|
if (idMath::Fabs(det) < MATRIX_INVERSE_EPSILON) {
|
|
return false;
|
|
}
|
|
|
|
invDet = 1.0f / det;
|
|
|
|
r0[0][0] = mat[1 * 3 + 1] * invDet;
|
|
r0[0][1] = -mat[0 * 3 + 1] * invDet;
|
|
r0[1][0] = -mat[1 * 3 + 0] * invDet;
|
|
r0[1][1] = mat[0 * 3 + 0] * invDet;
|
|
|
|
// r1 = r0 * m1; // 2x1 = 2x2 * 2x1
|
|
r1[0] = r0[0][0] * mat[0 * 3 + 2] + r0[0][1] * mat[1 * 3 + 2];
|
|
r1[1] = r0[1][0] * mat[0 * 3 + 2] + r0[1][1] * mat[1 * 3 + 2];
|
|
|
|
// r2 = m2 * r1; // 1x1 = 1x2 * 2x1
|
|
r2[0] = mat[2 * 3 + 0] * r1[0] + mat[2 * 3 + 1] * r1[1];
|
|
|
|
// r3 = r2 - m3; // 1x1 = 1x1 - 1x1
|
|
r3 = r2[0] - mat[2 * 3 + 2];
|
|
|
|
// r3.InverseSelf();
|
|
if (idMath::Fabs(r3) < MATRIX_INVERSE_EPSILON) {
|
|
return false;
|
|
}
|
|
|
|
r3 = 1.0f / r3;
|
|
|
|
// r2 = m2 * r0; // 1x2 = 1x2 * 2x2
|
|
r2[0] = mat[2 * 3 + 0] * r0[0][0] + mat[2 * 3 + 1] * r0[1][0];
|
|
r2[1] = mat[2 * 3 + 0] * r0[0][1] + mat[2 * 3 + 1] * r0[1][1];
|
|
|
|
// m2 = r3 * r2; // 1x2 = 1x1 * 1x2
|
|
mat[2 * 3 + 0] = r3 * r2[0];
|
|
mat[2 * 3 + 1] = r3 * r2[1];
|
|
|
|
// m0 = r0 - r1 * m2; // 2x2 - 2x1 * 1x2
|
|
mat[0 * 3 + 0] = r0[0][0] - r1[0] * mat[2 * 3 + 0];
|
|
mat[0 * 3 + 1] = r0[0][1] - r1[0] * mat[2 * 3 + 1];
|
|
mat[1 * 3 + 0] = r0[1][0] - r1[1] * mat[2 * 3 + 0];
|
|
mat[1 * 3 + 1] = r0[1][1] - r1[1] * mat[2 * 3 + 1];
|
|
|
|
// m1 = r1 * r3; // 2x1 = 2x1 * 1x1
|
|
mat[0 * 3 + 2] = r1[0] * r3;
|
|
mat[1 * 3 + 2] = r1[1] * r3;
|
|
|
|
// m3 = -r3;
|
|
mat[2 * 3 + 2] = -r3;
|
|
|
|
return true;
|
|
#endif
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMat3::InertiaTranslate
|
|
============
|
|
*/
|
|
idMat3 idMat3::InertiaTranslate(const float mass, const idVec3& centerOfMass, const idVec3& translation) const {
|
|
idMat3 m;
|
|
idVec3 newCenter;
|
|
|
|
newCenter = centerOfMass + translation;
|
|
|
|
m[0][0] = mass * ((centerOfMass[1] * centerOfMass[1] + centerOfMass[2] * centerOfMass[2])
|
|
- (newCenter[1] * newCenter[1] + newCenter[2] * newCenter[2]));
|
|
m[1][1] = mass * ((centerOfMass[0] * centerOfMass[0] + centerOfMass[2] * centerOfMass[2])
|
|
- (newCenter[0] * newCenter[0] + newCenter[2] * newCenter[2]));
|
|
m[2][2] = mass * ((centerOfMass[0] * centerOfMass[0] + centerOfMass[1] * centerOfMass[1])
|
|
- (newCenter[0] * newCenter[0] + newCenter[1] * newCenter[1]));
|
|
|
|
m[0][1] = m[1][0] = mass * (newCenter[0] * newCenter[1] - centerOfMass[0] * centerOfMass[1]);
|
|
m[1][2] = m[2][1] = mass * (newCenter[1] * newCenter[2] - centerOfMass[1] * centerOfMass[2]);
|
|
m[0][2] = m[2][0] = mass * (newCenter[0] * newCenter[2] - centerOfMass[0] * centerOfMass[2]);
|
|
|
|
return (*this) + m;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMat3::InertiaTranslateSelf
|
|
============
|
|
*/
|
|
idMat3& idMat3::InertiaTranslateSelf(const float mass, const idVec3& centerOfMass, const idVec3& translation) {
|
|
idMat3 m;
|
|
idVec3 newCenter;
|
|
|
|
newCenter = centerOfMass + translation;
|
|
|
|
m[0][0] = mass * ((centerOfMass[1] * centerOfMass[1] + centerOfMass[2] * centerOfMass[2])
|
|
- (newCenter[1] * newCenter[1] + newCenter[2] * newCenter[2]));
|
|
m[1][1] = mass * ((centerOfMass[0] * centerOfMass[0] + centerOfMass[2] * centerOfMass[2])
|
|
- (newCenter[0] * newCenter[0] + newCenter[2] * newCenter[2]));
|
|
m[2][2] = mass * ((centerOfMass[0] * centerOfMass[0] + centerOfMass[1] * centerOfMass[1])
|
|
- (newCenter[0] * newCenter[0] + newCenter[1] * newCenter[1]));
|
|
|
|
m[0][1] = m[1][0] = mass * (newCenter[0] * newCenter[1] - centerOfMass[0] * centerOfMass[1]);
|
|
m[1][2] = m[2][1] = mass * (newCenter[1] * newCenter[2] - centerOfMass[1] * centerOfMass[2]);
|
|
m[0][2] = m[2][0] = mass * (newCenter[0] * newCenter[2] - centerOfMass[0] * centerOfMass[2]);
|
|
|
|
(*this) += m;
|
|
|
|
return (*this);
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMat3::InertiaRotate
|
|
============
|
|
*/
|
|
idMat3 idMat3::InertiaRotate(const idMat3& rotation) const {
|
|
// NOTE: the rotation matrix is stored column-major
|
|
return rotation.Transpose() * (*this) * rotation;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMat3::InertiaRotateSelf
|
|
============
|
|
*/
|
|
idMat3& idMat3::InertiaRotateSelf(const idMat3& rotation) {
|
|
// NOTE: the rotation matrix is stored column-major
|
|
*this = rotation.Transpose() * (*this) * rotation;
|
|
return *this;
|
|
}
|
|
|
|
/*
|
|
=============
|
|
idMat3::ToString
|
|
=============
|
|
*/
|
|
const char* idMat3::ToString(int precision) const {
|
|
return idStr::FloatArrayToString(ToFloatPtr(), GetDimension(), precision);
|
|
}
|
|
|
|
|
|
//===============================================================
|
|
//
|
|
// idMat4
|
|
//
|
|
//===============================================================
|
|
|
|
idMat4 mat4_zero(idVec4(0, 0, 0, 0), idVec4(0, 0, 0, 0), idVec4(0, 0, 0, 0), idVec4(0, 0, 0, 0));
|
|
idMat4 mat4_identity(idVec4(1, 0, 0, 0), idVec4(0, 1, 0, 0), idVec4(0, 0, 1, 0), idVec4(0, 0, 0, 1));
|
|
|
|
/*
|
|
============
|
|
idMat4::Transpose
|
|
============
|
|
*/
|
|
idMat4 idMat4::Transpose(void) const {
|
|
idMat4 transpose;
|
|
int i, j;
|
|
|
|
for (i = 0; i < 4; i++) {
|
|
for (j = 0; j < 4; j++) {
|
|
transpose[i][j] = mat[j][i];
|
|
}
|
|
}
|
|
return transpose;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMat4::TransposeSelf
|
|
============
|
|
*/
|
|
idMat4& idMat4::TransposeSelf(void) {
|
|
float temp;
|
|
int i, j;
|
|
|
|
for (i = 0; i < 4; i++) {
|
|
for (j = i + 1; j < 4; j++) {
|
|
temp = mat[i][j];
|
|
mat[i][j] = mat[j][i];
|
|
mat[j][i] = temp;
|
|
}
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMat4::Determinant
|
|
============
|
|
*/
|
|
float idMat4::Determinant(void) const {
|
|
|
|
// 2x2 sub-determinants
|
|
float det2_01_01 = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
|
|
float det2_01_02 = mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0];
|
|
float det2_01_03 = mat[0][0] * mat[1][3] - mat[0][3] * mat[1][0];
|
|
float det2_01_12 = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
|
|
float det2_01_13 = mat[0][1] * mat[1][3] - mat[0][3] * mat[1][1];
|
|
float det2_01_23 = mat[0][2] * mat[1][3] - mat[0][3] * mat[1][2];
|
|
|
|
// 3x3 sub-determinants
|
|
float det3_201_012 = mat[2][0] * det2_01_12 - mat[2][1] * det2_01_02 + mat[2][2] * det2_01_01;
|
|
float det3_201_013 = mat[2][0] * det2_01_13 - mat[2][1] * det2_01_03 + mat[2][3] * det2_01_01;
|
|
float det3_201_023 = mat[2][0] * det2_01_23 - mat[2][2] * det2_01_03 + mat[2][3] * det2_01_02;
|
|
float det3_201_123 = mat[2][1] * det2_01_23 - mat[2][2] * det2_01_13 + mat[2][3] * det2_01_12;
|
|
|
|
return (-det3_201_123 * mat[3][0] + det3_201_023 * mat[3][1] - det3_201_013 * mat[3][2] + det3_201_012 * mat[3][3]);
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMat4::InverseSelf
|
|
============
|
|
*/
|
|
bool idMat4::InverseSelf(void) {
|
|
// 84+4+16 = 104 multiplications
|
|
// 1 division
|
|
double det, invDet;
|
|
|
|
// 2x2 sub-determinants required to calculate 4x4 determinant
|
|
float det2_01_01 = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
|
|
float det2_01_02 = mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0];
|
|
float det2_01_03 = mat[0][0] * mat[1][3] - mat[0][3] * mat[1][0];
|
|
float det2_01_12 = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
|
|
float det2_01_13 = mat[0][1] * mat[1][3] - mat[0][3] * mat[1][1];
|
|
float det2_01_23 = mat[0][2] * mat[1][3] - mat[0][3] * mat[1][2];
|
|
|
|
// 3x3 sub-determinants required to calculate 4x4 determinant
|
|
float det3_201_012 = mat[2][0] * det2_01_12 - mat[2][1] * det2_01_02 + mat[2][2] * det2_01_01;
|
|
float det3_201_013 = mat[2][0] * det2_01_13 - mat[2][1] * det2_01_03 + mat[2][3] * det2_01_01;
|
|
float det3_201_023 = mat[2][0] * det2_01_23 - mat[2][2] * det2_01_03 + mat[2][3] * det2_01_02;
|
|
float det3_201_123 = mat[2][1] * det2_01_23 - mat[2][2] * det2_01_13 + mat[2][3] * det2_01_12;
|
|
|
|
det = (-det3_201_123 * mat[3][0] + det3_201_023 * mat[3][1] - det3_201_013 * mat[3][2] + det3_201_012 * mat[3][3]);
|
|
|
|
if (idMath::Fabs(det) < MATRIX_INVERSE_EPSILON) {
|
|
return false;
|
|
}
|
|
|
|
invDet = 1.0f / det;
|
|
|
|
// remaining 2x2 sub-determinants
|
|
float det2_03_01 = mat[0][0] * mat[3][1] - mat[0][1] * mat[3][0];
|
|
float det2_03_02 = mat[0][0] * mat[3][2] - mat[0][2] * mat[3][0];
|
|
float det2_03_03 = mat[0][0] * mat[3][3] - mat[0][3] * mat[3][0];
|
|
float det2_03_12 = mat[0][1] * mat[3][2] - mat[0][2] * mat[3][1];
|
|
float det2_03_13 = mat[0][1] * mat[3][3] - mat[0][3] * mat[3][1];
|
|
float det2_03_23 = mat[0][2] * mat[3][3] - mat[0][3] * mat[3][2];
|
|
|
|
float det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0];
|
|
float det2_13_02 = mat[1][0] * mat[3][2] - mat[1][2] * mat[3][0];
|
|
float det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0];
|
|
float det2_13_12 = mat[1][1] * mat[3][2] - mat[1][2] * mat[3][1];
|
|
float det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1];
|
|
float det2_13_23 = mat[1][2] * mat[3][3] - mat[1][3] * mat[3][2];
|
|
|
|
// remaining 3x3 sub-determinants
|
|
float det3_203_012 = mat[2][0] * det2_03_12 - mat[2][1] * det2_03_02 + mat[2][2] * det2_03_01;
|
|
float det3_203_013 = mat[2][0] * det2_03_13 - mat[2][1] * det2_03_03 + mat[2][3] * det2_03_01;
|
|
float det3_203_023 = mat[2][0] * det2_03_23 - mat[2][2] * det2_03_03 + mat[2][3] * det2_03_02;
|
|
float det3_203_123 = mat[2][1] * det2_03_23 - mat[2][2] * det2_03_13 + mat[2][3] * det2_03_12;
|
|
|
|
float det3_213_012 = mat[2][0] * det2_13_12 - mat[2][1] * det2_13_02 + mat[2][2] * det2_13_01;
|
|
float det3_213_013 = mat[2][0] * det2_13_13 - mat[2][1] * det2_13_03 + mat[2][3] * det2_13_01;
|
|
float det3_213_023 = mat[2][0] * det2_13_23 - mat[2][2] * det2_13_03 + mat[2][3] * det2_13_02;
|
|
float det3_213_123 = mat[2][1] * det2_13_23 - mat[2][2] * det2_13_13 + mat[2][3] * det2_13_12;
|
|
|
|
float det3_301_012 = mat[3][0] * det2_01_12 - mat[3][1] * det2_01_02 + mat[3][2] * det2_01_01;
|
|
float det3_301_013 = mat[3][0] * det2_01_13 - mat[3][1] * det2_01_03 + mat[3][3] * det2_01_01;
|
|
float det3_301_023 = mat[3][0] * det2_01_23 - mat[3][2] * det2_01_03 + mat[3][3] * det2_01_02;
|
|
float det3_301_123 = mat[3][1] * det2_01_23 - mat[3][2] * det2_01_13 + mat[3][3] * det2_01_12;
|
|
|
|
mat[0][0] = -det3_213_123 * invDet;
|
|
mat[1][0] = +det3_213_023 * invDet;
|
|
mat[2][0] = -det3_213_013 * invDet;
|
|
mat[3][0] = +det3_213_012 * invDet;
|
|
|
|
mat[0][1] = +det3_203_123 * invDet;
|
|
mat[1][1] = -det3_203_023 * invDet;
|
|
mat[2][1] = +det3_203_013 * invDet;
|
|
mat[3][1] = -det3_203_012 * invDet;
|
|
|
|
mat[0][2] = +det3_301_123 * invDet;
|
|
mat[1][2] = -det3_301_023 * invDet;
|
|
mat[2][2] = +det3_301_013 * invDet;
|
|
mat[3][2] = -det3_301_012 * invDet;
|
|
|
|
mat[0][3] = -det3_201_123 * invDet;
|
|
mat[1][3] = +det3_201_023 * invDet;
|
|
mat[2][3] = -det3_201_013 * invDet;
|
|
mat[3][3] = +det3_201_012 * invDet;
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMat4::InverseFastSelf
|
|
============
|
|
*/
|
|
bool idMat4::InverseFastSelf(void) {
|
|
#if 0
|
|
// 84+4+16 = 104 multiplications
|
|
// 1 division
|
|
double det, invDet;
|
|
|
|
// 2x2 sub-determinants required to calculate 4x4 determinant
|
|
float det2_01_01 = mat[0][0] * mat[1][1] - mat[0][1] * mat[1][0];
|
|
float det2_01_02 = mat[0][0] * mat[1][2] - mat[0][2] * mat[1][0];
|
|
float det2_01_03 = mat[0][0] * mat[1][3] - mat[0][3] * mat[1][0];
|
|
float det2_01_12 = mat[0][1] * mat[1][2] - mat[0][2] * mat[1][1];
|
|
float det2_01_13 = mat[0][1] * mat[1][3] - mat[0][3] * mat[1][1];
|
|
float det2_01_23 = mat[0][2] * mat[1][3] - mat[0][3] * mat[1][2];
|
|
|
|
// 3x3 sub-determinants required to calculate 4x4 determinant
|
|
float det3_201_012 = mat[2][0] * det2_01_12 - mat[2][1] * det2_01_02 + mat[2][2] * det2_01_01;
|
|
float det3_201_013 = mat[2][0] * det2_01_13 - mat[2][1] * det2_01_03 + mat[2][3] * det2_01_01;
|
|
float det3_201_023 = mat[2][0] * det2_01_23 - mat[2][2] * det2_01_03 + mat[2][3] * det2_01_02;
|
|
float det3_201_123 = mat[2][1] * det2_01_23 - mat[2][2] * det2_01_13 + mat[2][3] * det2_01_12;
|
|
|
|
det = (-det3_201_123 * mat[3][0] + det3_201_023 * mat[3][1] - det3_201_013 * mat[3][2] + det3_201_012 * mat[3][3]);
|
|
|
|
if (idMath::Fabs(det) < MATRIX_INVERSE_EPSILON) {
|
|
return false;
|
|
}
|
|
|
|
invDet = 1.0f / det;
|
|
|
|
// remaining 2x2 sub-determinants
|
|
float det2_03_01 = mat[0][0] * mat[3][1] - mat[0][1] * mat[3][0];
|
|
float det2_03_02 = mat[0][0] * mat[3][2] - mat[0][2] * mat[3][0];
|
|
float det2_03_03 = mat[0][0] * mat[3][3] - mat[0][3] * mat[3][0];
|
|
float det2_03_12 = mat[0][1] * mat[3][2] - mat[0][2] * mat[3][1];
|
|
float det2_03_13 = mat[0][1] * mat[3][3] - mat[0][3] * mat[3][1];
|
|
float det2_03_23 = mat[0][2] * mat[3][3] - mat[0][3] * mat[3][2];
|
|
|
|
float det2_13_01 = mat[1][0] * mat[3][1] - mat[1][1] * mat[3][0];
|
|
float det2_13_02 = mat[1][0] * mat[3][2] - mat[1][2] * mat[3][0];
|
|
float det2_13_03 = mat[1][0] * mat[3][3] - mat[1][3] * mat[3][0];
|
|
float det2_13_12 = mat[1][1] * mat[3][2] - mat[1][2] * mat[3][1];
|
|
float det2_13_13 = mat[1][1] * mat[3][3] - mat[1][3] * mat[3][1];
|
|
float det2_13_23 = mat[1][2] * mat[3][3] - mat[1][3] * mat[3][2];
|
|
|
|
// remaining 3x3 sub-determinants
|
|
float det3_203_012 = mat[2][0] * det2_03_12 - mat[2][1] * det2_03_02 + mat[2][2] * det2_03_01;
|
|
float det3_203_013 = mat[2][0] * det2_03_13 - mat[2][1] * det2_03_03 + mat[2][3] * det2_03_01;
|
|
float det3_203_023 = mat[2][0] * det2_03_23 - mat[2][2] * det2_03_03 + mat[2][3] * det2_03_02;
|
|
float det3_203_123 = mat[2][1] * det2_03_23 - mat[2][2] * det2_03_13 + mat[2][3] * det2_03_12;
|
|
|
|
float det3_213_012 = mat[2][0] * det2_13_12 - mat[2][1] * det2_13_02 + mat[2][2] * det2_13_01;
|
|
float det3_213_013 = mat[2][0] * det2_13_13 - mat[2][1] * det2_13_03 + mat[2][3] * det2_13_01;
|
|
float det3_213_023 = mat[2][0] * det2_13_23 - mat[2][2] * det2_13_03 + mat[2][3] * det2_13_02;
|
|
float det3_213_123 = mat[2][1] * det2_13_23 - mat[2][2] * det2_13_13 + mat[2][3] * det2_13_12;
|
|
|
|
float det3_301_012 = mat[3][0] * det2_01_12 - mat[3][1] * det2_01_02 + mat[3][2] * det2_01_01;
|
|
float det3_301_013 = mat[3][0] * det2_01_13 - mat[3][1] * det2_01_03 + mat[3][3] * det2_01_01;
|
|
float det3_301_023 = mat[3][0] * det2_01_23 - mat[3][2] * det2_01_03 + mat[3][3] * det2_01_02;
|
|
float det3_301_123 = mat[3][1] * det2_01_23 - mat[3][2] * det2_01_13 + mat[3][3] * det2_01_12;
|
|
|
|
mat[0][0] = -det3_213_123 * invDet;
|
|
mat[1][0] = +det3_213_023 * invDet;
|
|
mat[2][0] = -det3_213_013 * invDet;
|
|
mat[3][0] = +det3_213_012 * invDet;
|
|
|
|
mat[0][1] = +det3_203_123 * invDet;
|
|
mat[1][1] = -det3_203_023 * invDet;
|
|
mat[2][1] = +det3_203_013 * invDet;
|
|
mat[3][1] = -det3_203_012 * invDet;
|
|
|
|
mat[0][2] = +det3_301_123 * invDet;
|
|
mat[1][2] = -det3_301_023 * invDet;
|
|
mat[2][2] = +det3_301_013 * invDet;
|
|
mat[3][2] = -det3_301_012 * invDet;
|
|
|
|
mat[0][3] = -det3_201_123 * invDet;
|
|
mat[1][3] = +det3_201_023 * invDet;
|
|
mat[2][3] = -det3_201_013 * invDet;
|
|
mat[3][3] = +det3_201_012 * invDet;
|
|
|
|
return true;
|
|
#elif 0
|
|
// 4*18 = 72 multiplications
|
|
// 4 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;
|
|
d = -d;
|
|
mat[4] *= d;
|
|
mat[8] *= d;
|
|
mat[12] *= d;
|
|
d = mat[4] * di;
|
|
mat[5] += mat[1] * d;
|
|
mat[6] += mat[2] * d;
|
|
mat[7] += mat[3] * d;
|
|
d = mat[8] * di;
|
|
mat[9] += mat[1] * d;
|
|
mat[10] += mat[2] * d;
|
|
mat[11] += mat[3] * d;
|
|
d = mat[12] * di;
|
|
mat[13] += mat[1] * d;
|
|
mat[14] += mat[2] * d;
|
|
mat[15] += mat[3] * d;
|
|
di = mat[5];
|
|
s *= di;
|
|
mat[5] = d = 1.0f / di;
|
|
mat[4] *= d;
|
|
mat[6] *= d;
|
|
mat[7] *= d;
|
|
d = -d;
|
|
mat[1] *= d;
|
|
mat[9] *= d;
|
|
mat[13] *= d;
|
|
d = mat[1] * di;
|
|
mat[0] += mat[4] * d;
|
|
mat[2] += mat[6] * d;
|
|
mat[3] += mat[7] * d;
|
|
d = mat[9] * di;
|
|
mat[8] += mat[4] * d;
|
|
mat[10] += mat[6] * d;
|
|
mat[11] += mat[7] * d;
|
|
d = mat[13] * di;
|
|
mat[12] += mat[4] * d;
|
|
mat[14] += mat[6] * d;
|
|
mat[15] += mat[7] * d;
|
|
di = mat[10];
|
|
s *= di;
|
|
mat[10] = d = 1.0f / di;
|
|
mat[8] *= d;
|
|
mat[9] *= d;
|
|
mat[11] *= d;
|
|
d = -d;
|
|
mat[2] *= d;
|
|
mat[6] *= d;
|
|
mat[14] *= d;
|
|
d = mat[2] * di;
|
|
mat[0] += mat[8] * d;
|
|
mat[1] += mat[9] * d;
|
|
mat[3] += mat[11] * d;
|
|
d = mat[6] * di;
|
|
mat[4] += mat[8] * d;
|
|
mat[5] += mat[9] * d;
|
|
mat[7] += mat[11] * d;
|
|
d = mat[14] * di;
|
|
mat[12] += mat[8] * d;
|
|
mat[13] += mat[9] * d;
|
|
mat[15] += mat[11] * d;
|
|
di = mat[15];
|
|
s *= di;
|
|
mat[15] = d = 1.0f / di;
|
|
mat[12] *= d;
|
|
mat[13] *= d;
|
|
mat[14] *= d;
|
|
d = -d;
|
|
mat[3] *= d;
|
|
mat[7] *= d;
|
|
mat[11] *= d;
|
|
d = mat[3] * di;
|
|
mat[0] += mat[12] * d;
|
|
mat[1] += mat[13] * d;
|
|
mat[2] += mat[14] * d;
|
|
d = mat[7] * di;
|
|
mat[4] += mat[12] * d;
|
|
mat[5] += mat[13] * d;
|
|
mat[6] += mat[14] * d;
|
|
d = mat[11] * di;
|
|
mat[8] += mat[12] * d;
|
|
mat[9] += mat[13] * d;
|
|
mat[10] += mat[14] * d;
|
|
|
|
return (s != 0.0f && !FLOAT_IS_NAN(s));
|
|
#else
|
|
// 6*8+2*6 = 60 multiplications
|
|
// 2*1 = 2 divisions
|
|
idMat2 r0, r1, r2, r3;
|
|
float a, det, invDet;
|
|
float* mat = reinterpret_cast<float*>(this);
|
|
|
|
// r0 = m0.Inverse();
|
|
det = mat[0 * 4 + 0] * mat[1 * 4 + 1] - mat[0 * 4 + 1] * mat[1 * 4 + 0];
|
|
|
|
if (idMath::Fabs(det) < MATRIX_INVERSE_EPSILON) {
|
|
return false;
|
|
}
|
|
|
|
invDet = 1.0f / det;
|
|
|
|
r0[0][0] = mat[1 * 4 + 1] * invDet;
|
|
r0[0][1] = -mat[0 * 4 + 1] * invDet;
|
|
r0[1][0] = -mat[1 * 4 + 0] * invDet;
|
|
r0[1][1] = mat[0 * 4 + 0] * invDet;
|
|
|
|
// r1 = r0 * m1;
|
|
r1[0][0] = r0[0][0] * mat[0 * 4 + 2] + r0[0][1] * mat[1 * 4 + 2];
|
|
r1[0][1] = r0[0][0] * mat[0 * 4 + 3] + r0[0][1] * mat[1 * 4 + 3];
|
|
r1[1][0] = r0[1][0] * mat[0 * 4 + 2] + r0[1][1] * mat[1 * 4 + 2];
|
|
r1[1][1] = r0[1][0] * mat[0 * 4 + 3] + r0[1][1] * mat[1 * 4 + 3];
|
|
|
|
// r2 = m2 * r1;
|
|
r2[0][0] = mat[2 * 4 + 0] * r1[0][0] + mat[2 * 4 + 1] * r1[1][0];
|
|
r2[0][1] = mat[2 * 4 + 0] * r1[0][1] + mat[2 * 4 + 1] * r1[1][1];
|
|
r2[1][0] = mat[3 * 4 + 0] * r1[0][0] + mat[3 * 4 + 1] * r1[1][0];
|
|
r2[1][1] = mat[3 * 4 + 0] * r1[0][1] + mat[3 * 4 + 1] * r1[1][1];
|
|
|
|
// r3 = r2 - m3;
|
|
r3[0][0] = r2[0][0] - mat[2 * 4 + 2];
|
|
r3[0][1] = r2[0][1] - mat[2 * 4 + 3];
|
|
r3[1][0] = r2[1][0] - mat[3 * 4 + 2];
|
|
r3[1][1] = r2[1][1] - mat[3 * 4 + 3];
|
|
|
|
// r3.InverseSelf();
|
|
det = r3[0][0] * r3[1][1] - r3[0][1] * r3[1][0];
|
|
|
|
if (idMath::Fabs(det) < MATRIX_INVERSE_EPSILON) {
|
|
return false;
|
|
}
|
|
|
|
invDet = 1.0f / det;
|
|
|
|
a = r3[0][0];
|
|
r3[0][0] = r3[1][1] * invDet;
|
|
r3[0][1] = -r3[0][1] * invDet;
|
|
r3[1][0] = -r3[1][0] * invDet;
|
|
r3[1][1] = a * invDet;
|
|
|
|
// r2 = m2 * r0;
|
|
r2[0][0] = mat[2 * 4 + 0] * r0[0][0] + mat[2 * 4 + 1] * r0[1][0];
|
|
r2[0][1] = mat[2 * 4 + 0] * r0[0][1] + mat[2 * 4 + 1] * r0[1][1];
|
|
r2[1][0] = mat[3 * 4 + 0] * r0[0][0] + mat[3 * 4 + 1] * r0[1][0];
|
|
r2[1][1] = mat[3 * 4 + 0] * r0[0][1] + mat[3 * 4 + 1] * r0[1][1];
|
|
|
|
// m2 = r3 * r2;
|
|
mat[2 * 4 + 0] = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0];
|
|
mat[2 * 4 + 1] = r3[0][0] * r2[0][1] + r3[0][1] * r2[1][1];
|
|
mat[3 * 4 + 0] = r3[1][0] * r2[0][0] + r3[1][1] * r2[1][0];
|
|
mat[3 * 4 + 1] = r3[1][0] * r2[0][1] + r3[1][1] * r2[1][1];
|
|
|
|
// m0 = r0 - r1 * m2;
|
|
mat[0 * 4 + 0] = r0[0][0] - r1[0][0] * mat[2 * 4 + 0] - r1[0][1] * mat[3 * 4 + 0];
|
|
mat[0 * 4 + 1] = r0[0][1] - r1[0][0] * mat[2 * 4 + 1] - r1[0][1] * mat[3 * 4 + 1];
|
|
mat[1 * 4 + 0] = r0[1][0] - r1[1][0] * mat[2 * 4 + 0] - r1[1][1] * mat[3 * 4 + 0];
|
|
mat[1 * 4 + 1] = r0[1][1] - r1[1][0] * mat[2 * 4 + 1] - r1[1][1] * mat[3 * 4 + 1];
|
|
|
|
// m1 = r1 * r3;
|
|
mat[0 * 4 + 2] = r1[0][0] * r3[0][0] + r1[0][1] * r3[1][0];
|
|
mat[0 * 4 + 3] = r1[0][0] * r3[0][1] + r1[0][1] * r3[1][1];
|
|
mat[1 * 4 + 2] = r1[1][0] * r3[0][0] + r1[1][1] * r3[1][0];
|
|
mat[1 * 4 + 3] = r1[1][0] * r3[0][1] + r1[1][1] * r3[1][1];
|
|
|
|
// m3 = -r3;
|
|
mat[2 * 4 + 2] = -r3[0][0];
|
|
mat[2 * 4 + 3] = -r3[0][1];
|
|
mat[3 * 4 + 2] = -r3[1][0];
|
|
mat[3 * 4 + 3] = -r3[1][1];
|
|
|
|
return true;
|
|
#endif
|
|
}
|
|
|
|
/*
|
|
=============
|
|
idMat4::ToString
|
|
=============
|
|
*/
|
|
const char* idMat4::ToString(int precision) const {
|
|
return idStr::FloatArrayToString(ToFloatPtr(), GetDimension(), precision);
|
|
}
|
|
|
|
|
|
//===============================================================
|
|
//
|
|
// idMat5
|
|
//
|
|
//===============================================================
|
|
|
|
idMat5 mat5_zero(idVec5(0, 0, 0, 0, 0), idVec5(0, 0, 0, 0, 0), idVec5(0, 0, 0, 0, 0), idVec5(0, 0, 0, 0, 0), idVec5(0, 0, 0, 0, 0));
|
|
idMat5 mat5_identity(idVec5(1, 0, 0, 0, 0), idVec5(0, 1, 0, 0, 0), idVec5(0, 0, 1, 0, 0), idVec5(0, 0, 0, 1, 0), idVec5(0, 0, 0, 0, 1));
|
|
|
|
/*
|
|
============
|
|
idMat5::Transpose
|
|
============
|
|
*/
|
|
idMat5 idMat5::Transpose(void) const {
|
|
idMat5 transpose;
|
|
int i, j;
|
|
|
|
for (i = 0; i < 5; i++) {
|
|
for (j = 0; j < 5; j++) {
|
|
transpose[i][j] = mat[j][i];
|
|
}
|
|
}
|
|
return transpose;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMat5::TransposeSelf
|
|
============
|
|
*/
|
|
idMat5& idMat5::TransposeSelf(void) {
|
|
float temp;
|
|
int i, j;
|
|
|
|
for (i = 0; i < 5; i++) {
|
|
for (j = i + 1; j < 5; j++) {
|
|
temp = mat[i][j];
|
|
mat[i][j] = mat[j][i];
|
|
mat[j][i] = temp;
|
|
}
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMat5::Determinant
|
|
============
|
|
*/
|
|
float idMat5::Determinant(void) const {
|
|
|
|
// 2x2 sub-determinants required to calculate 5x5 determinant
|
|
float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0];
|
|
float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0];
|
|
float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0];
|
|
float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0];
|
|
float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1];
|
|
float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1];
|
|
float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1];
|
|
float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2];
|
|
float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2];
|
|
float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3];
|
|
|
|
// 3x3 sub-determinants required to calculate 5x5 determinant
|
|
float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01;
|
|
float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01;
|
|
float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01;
|
|
float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02;
|
|
float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02;
|
|
float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03;
|
|
float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12;
|
|
float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12;
|
|
float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13;
|
|
float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23;
|
|
|
|
// 4x4 sub-determinants required to calculate 5x5 determinant
|
|
float det4_1234_0123 = mat[1][0] * det3_234_123 - mat[1][1] * det3_234_023 + mat[1][2] * det3_234_013 - mat[1][3] * det3_234_012;
|
|
float det4_1234_0124 = mat[1][0] * det3_234_124 - mat[1][1] * det3_234_024 + mat[1][2] * det3_234_014 - mat[1][4] * det3_234_012;
|
|
float det4_1234_0134 = mat[1][0] * det3_234_134 - mat[1][1] * det3_234_034 + mat[1][3] * det3_234_014 - mat[1][4] * det3_234_013;
|
|
float det4_1234_0234 = mat[1][0] * det3_234_234 - mat[1][2] * det3_234_034 + mat[1][3] * det3_234_024 - mat[1][4] * det3_234_023;
|
|
float det4_1234_1234 = mat[1][1] * det3_234_234 - mat[1][2] * det3_234_134 + mat[1][3] * det3_234_124 - mat[1][4] * det3_234_123;
|
|
|
|
// determinant of 5x5 matrix
|
|
return mat[0][0] * det4_1234_1234 - mat[0][1] * det4_1234_0234 + mat[0][2] * det4_1234_0134 - mat[0][3] * det4_1234_0124 + mat[0][4] * det4_1234_0123;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMat5::InverseSelf
|
|
============
|
|
*/
|
|
bool idMat5::InverseSelf(void) {
|
|
// 280+5+25 = 310 multiplications
|
|
// 1 division
|
|
double det, invDet;
|
|
|
|
// 2x2 sub-determinants required to calculate 5x5 determinant
|
|
float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0];
|
|
float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0];
|
|
float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0];
|
|
float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0];
|
|
float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1];
|
|
float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1];
|
|
float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1];
|
|
float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2];
|
|
float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2];
|
|
float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3];
|
|
|
|
// 3x3 sub-determinants required to calculate 5x5 determinant
|
|
float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01;
|
|
float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01;
|
|
float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01;
|
|
float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02;
|
|
float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02;
|
|
float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03;
|
|
float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12;
|
|
float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12;
|
|
float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13;
|
|
float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23;
|
|
|
|
// 4x4 sub-determinants required to calculate 5x5 determinant
|
|
float det4_1234_0123 = mat[1][0] * det3_234_123 - mat[1][1] * det3_234_023 + mat[1][2] * det3_234_013 - mat[1][3] * det3_234_012;
|
|
float det4_1234_0124 = mat[1][0] * det3_234_124 - mat[1][1] * det3_234_024 + mat[1][2] * det3_234_014 - mat[1][4] * det3_234_012;
|
|
float det4_1234_0134 = mat[1][0] * det3_234_134 - mat[1][1] * det3_234_034 + mat[1][3] * det3_234_014 - mat[1][4] * det3_234_013;
|
|
float det4_1234_0234 = mat[1][0] * det3_234_234 - mat[1][2] * det3_234_034 + mat[1][3] * det3_234_024 - mat[1][4] * det3_234_023;
|
|
float det4_1234_1234 = mat[1][1] * det3_234_234 - mat[1][2] * det3_234_134 + mat[1][3] * det3_234_124 - mat[1][4] * det3_234_123;
|
|
|
|
// determinant of 5x5 matrix
|
|
det = mat[0][0] * det4_1234_1234 - mat[0][1] * det4_1234_0234 + mat[0][2] * det4_1234_0134 - mat[0][3] * det4_1234_0124 + mat[0][4] * det4_1234_0123;
|
|
|
|
if (idMath::Fabs(det) < MATRIX_INVERSE_EPSILON) {
|
|
return false;
|
|
}
|
|
|
|
invDet = 1.0f / det;
|
|
|
|
// remaining 2x2 sub-determinants
|
|
float det2_23_01 = mat[2][0] * mat[3][1] - mat[2][1] * mat[3][0];
|
|
float det2_23_02 = mat[2][0] * mat[3][2] - mat[2][2] * mat[3][0];
|
|
float det2_23_03 = mat[2][0] * mat[3][3] - mat[2][3] * mat[3][0];
|
|
float det2_23_04 = mat[2][0] * mat[3][4] - mat[2][4] * mat[3][0];
|
|
float det2_23_12 = mat[2][1] * mat[3][2] - mat[2][2] * mat[3][1];
|
|
float det2_23_13 = mat[2][1] * mat[3][3] - mat[2][3] * mat[3][1];
|
|
float det2_23_14 = mat[2][1] * mat[3][4] - mat[2][4] * mat[3][1];
|
|
float det2_23_23 = mat[2][2] * mat[3][3] - mat[2][3] * mat[3][2];
|
|
float det2_23_24 = mat[2][2] * mat[3][4] - mat[2][4] * mat[3][2];
|
|
float det2_23_34 = mat[2][3] * mat[3][4] - mat[2][4] * mat[3][3];
|
|
float det2_24_01 = mat[2][0] * mat[4][1] - mat[2][1] * mat[4][0];
|
|
float det2_24_02 = mat[2][0] * mat[4][2] - mat[2][2] * mat[4][0];
|
|
float det2_24_03 = mat[2][0] * mat[4][3] - mat[2][3] * mat[4][0];
|
|
float det2_24_04 = mat[2][0] * mat[4][4] - mat[2][4] * mat[4][0];
|
|
float det2_24_12 = mat[2][1] * mat[4][2] - mat[2][2] * mat[4][1];
|
|
float det2_24_13 = mat[2][1] * mat[4][3] - mat[2][3] * mat[4][1];
|
|
float det2_24_14 = mat[2][1] * mat[4][4] - mat[2][4] * mat[4][1];
|
|
float det2_24_23 = mat[2][2] * mat[4][3] - mat[2][3] * mat[4][2];
|
|
float det2_24_24 = mat[2][2] * mat[4][4] - mat[2][4] * mat[4][2];
|
|
float det2_24_34 = mat[2][3] * mat[4][4] - mat[2][4] * mat[4][3];
|
|
|
|
// remaining 3x3 sub-determinants
|
|
float det3_123_012 = mat[1][0] * det2_23_12 - mat[1][1] * det2_23_02 + mat[1][2] * det2_23_01;
|
|
float det3_123_013 = mat[1][0] * det2_23_13 - mat[1][1] * det2_23_03 + mat[1][3] * det2_23_01;
|
|
float det3_123_014 = mat[1][0] * det2_23_14 - mat[1][1] * det2_23_04 + mat[1][4] * det2_23_01;
|
|
float det3_123_023 = mat[1][0] * det2_23_23 - mat[1][2] * det2_23_03 + mat[1][3] * det2_23_02;
|
|
float det3_123_024 = mat[1][0] * det2_23_24 - mat[1][2] * det2_23_04 + mat[1][4] * det2_23_02;
|
|
float det3_123_034 = mat[1][0] * det2_23_34 - mat[1][3] * det2_23_04 + mat[1][4] * det2_23_03;
|
|
float det3_123_123 = mat[1][1] * det2_23_23 - mat[1][2] * det2_23_13 + mat[1][3] * det2_23_12;
|
|
float det3_123_124 = mat[1][1] * det2_23_24 - mat[1][2] * det2_23_14 + mat[1][4] * det2_23_12;
|
|
float det3_123_134 = mat[1][1] * det2_23_34 - mat[1][3] * det2_23_14 + mat[1][4] * det2_23_13;
|
|
float det3_123_234 = mat[1][2] * det2_23_34 - mat[1][3] * det2_23_24 + mat[1][4] * det2_23_23;
|
|
float det3_124_012 = mat[1][0] * det2_24_12 - mat[1][1] * det2_24_02 + mat[1][2] * det2_24_01;
|
|
float det3_124_013 = mat[1][0] * det2_24_13 - mat[1][1] * det2_24_03 + mat[1][3] * det2_24_01;
|
|
float det3_124_014 = mat[1][0] * det2_24_14 - mat[1][1] * det2_24_04 + mat[1][4] * det2_24_01;
|
|
float det3_124_023 = mat[1][0] * det2_24_23 - mat[1][2] * det2_24_03 + mat[1][3] * det2_24_02;
|
|
float det3_124_024 = mat[1][0] * det2_24_24 - mat[1][2] * det2_24_04 + mat[1][4] * det2_24_02;
|
|
float det3_124_034 = mat[1][0] * det2_24_34 - mat[1][3] * det2_24_04 + mat[1][4] * det2_24_03;
|
|
float det3_124_123 = mat[1][1] * det2_24_23 - mat[1][2] * det2_24_13 + mat[1][3] * det2_24_12;
|
|
float det3_124_124 = mat[1][1] * det2_24_24 - mat[1][2] * det2_24_14 + mat[1][4] * det2_24_12;
|
|
float det3_124_134 = mat[1][1] * det2_24_34 - mat[1][3] * det2_24_14 + mat[1][4] * det2_24_13;
|
|
float det3_124_234 = mat[1][2] * det2_24_34 - mat[1][3] * det2_24_24 + mat[1][4] * det2_24_23;
|
|
float det3_134_012 = mat[1][0] * det2_34_12 - mat[1][1] * det2_34_02 + mat[1][2] * det2_34_01;
|
|
float det3_134_013 = mat[1][0] * det2_34_13 - mat[1][1] * det2_34_03 + mat[1][3] * det2_34_01;
|
|
float det3_134_014 = mat[1][0] * det2_34_14 - mat[1][1] * det2_34_04 + mat[1][4] * det2_34_01;
|
|
float det3_134_023 = mat[1][0] * det2_34_23 - mat[1][2] * det2_34_03 + mat[1][3] * det2_34_02;
|
|
float det3_134_024 = mat[1][0] * det2_34_24 - mat[1][2] * det2_34_04 + mat[1][4] * det2_34_02;
|
|
float det3_134_034 = mat[1][0] * det2_34_34 - mat[1][3] * det2_34_04 + mat[1][4] * det2_34_03;
|
|
float det3_134_123 = mat[1][1] * det2_34_23 - mat[1][2] * det2_34_13 + mat[1][3] * det2_34_12;
|
|
float det3_134_124 = mat[1][1] * det2_34_24 - mat[1][2] * det2_34_14 + mat[1][4] * det2_34_12;
|
|
float det3_134_134 = mat[1][1] * det2_34_34 - mat[1][3] * det2_34_14 + mat[1][4] * det2_34_13;
|
|
float det3_134_234 = mat[1][2] * det2_34_34 - mat[1][3] * det2_34_24 + mat[1][4] * det2_34_23;
|
|
|
|
// remaining 4x4 sub-determinants
|
|
float det4_0123_0123 = mat[0][0] * det3_123_123 - mat[0][1] * det3_123_023 + mat[0][2] * det3_123_013 - mat[0][3] * det3_123_012;
|
|
float det4_0123_0124 = mat[0][0] * det3_123_124 - mat[0][1] * det3_123_024 + mat[0][2] * det3_123_014 - mat[0][4] * det3_123_012;
|
|
float det4_0123_0134 = mat[0][0] * det3_123_134 - mat[0][1] * det3_123_034 + mat[0][3] * det3_123_014 - mat[0][4] * det3_123_013;
|
|
float det4_0123_0234 = mat[0][0] * det3_123_234 - mat[0][2] * det3_123_034 + mat[0][3] * det3_123_024 - mat[0][4] * det3_123_023;
|
|
float det4_0123_1234 = mat[0][1] * det3_123_234 - mat[0][2] * det3_123_134 + mat[0][3] * det3_123_124 - mat[0][4] * det3_123_123;
|
|
float det4_0124_0123 = mat[0][0] * det3_124_123 - mat[0][1] * det3_124_023 + mat[0][2] * det3_124_013 - mat[0][3] * det3_124_012;
|
|
float det4_0124_0124 = mat[0][0] * det3_124_124 - mat[0][1] * det3_124_024 + mat[0][2] * det3_124_014 - mat[0][4] * det3_124_012;
|
|
float det4_0124_0134 = mat[0][0] * det3_124_134 - mat[0][1] * det3_124_034 + mat[0][3] * det3_124_014 - mat[0][4] * det3_124_013;
|
|
float det4_0124_0234 = mat[0][0] * det3_124_234 - mat[0][2] * det3_124_034 + mat[0][3] * det3_124_024 - mat[0][4] * det3_124_023;
|
|
float det4_0124_1234 = mat[0][1] * det3_124_234 - mat[0][2] * det3_124_134 + mat[0][3] * det3_124_124 - mat[0][4] * det3_124_123;
|
|
float det4_0134_0123 = mat[0][0] * det3_134_123 - mat[0][1] * det3_134_023 + mat[0][2] * det3_134_013 - mat[0][3] * det3_134_012;
|
|
float det4_0134_0124 = mat[0][0] * det3_134_124 - mat[0][1] * det3_134_024 + mat[0][2] * det3_134_014 - mat[0][4] * det3_134_012;
|
|
float det4_0134_0134 = mat[0][0] * det3_134_134 - mat[0][1] * det3_134_034 + mat[0][3] * det3_134_014 - mat[0][4] * det3_134_013;
|
|
float det4_0134_0234 = mat[0][0] * det3_134_234 - mat[0][2] * det3_134_034 + mat[0][3] * det3_134_024 - mat[0][4] * det3_134_023;
|
|
float det4_0134_1234 = mat[0][1] * det3_134_234 - mat[0][2] * det3_134_134 + mat[0][3] * det3_134_124 - mat[0][4] * det3_134_123;
|
|
float det4_0234_0123 = mat[0][0] * det3_234_123 - mat[0][1] * det3_234_023 + mat[0][2] * det3_234_013 - mat[0][3] * det3_234_012;
|
|
float det4_0234_0124 = mat[0][0] * det3_234_124 - mat[0][1] * det3_234_024 + mat[0][2] * det3_234_014 - mat[0][4] * det3_234_012;
|
|
float det4_0234_0134 = mat[0][0] * det3_234_134 - mat[0][1] * det3_234_034 + mat[0][3] * det3_234_014 - mat[0][4] * det3_234_013;
|
|
float det4_0234_0234 = mat[0][0] * det3_234_234 - mat[0][2] * det3_234_034 + mat[0][3] * det3_234_024 - mat[0][4] * det3_234_023;
|
|
float det4_0234_1234 = mat[0][1] * det3_234_234 - mat[0][2] * det3_234_134 + mat[0][3] * det3_234_124 - mat[0][4] * det3_234_123;
|
|
|
|
mat[0][0] = det4_1234_1234 * invDet;
|
|
mat[0][1] = -det4_0234_1234 * invDet;
|
|
mat[0][2] = det4_0134_1234 * invDet;
|
|
mat[0][3] = -det4_0124_1234 * invDet;
|
|
mat[0][4] = det4_0123_1234 * invDet;
|
|
|
|
mat[1][0] = -det4_1234_0234 * invDet;
|
|
mat[1][1] = det4_0234_0234 * invDet;
|
|
mat[1][2] = -det4_0134_0234 * invDet;
|
|
mat[1][3] = det4_0124_0234 * invDet;
|
|
mat[1][4] = -det4_0123_0234 * invDet;
|
|
|
|
mat[2][0] = det4_1234_0134 * invDet;
|
|
mat[2][1] = -det4_0234_0134 * invDet;
|
|
mat[2][2] = det4_0134_0134 * invDet;
|
|
mat[2][3] = -det4_0124_0134 * invDet;
|
|
mat[2][4] = det4_0123_0134 * invDet;
|
|
|
|
mat[3][0] = -det4_1234_0124 * invDet;
|
|
mat[3][1] = det4_0234_0124 * invDet;
|
|
mat[3][2] = -det4_0134_0124 * invDet;
|
|
mat[3][3] = det4_0124_0124 * invDet;
|
|
mat[3][4] = -det4_0123_0124 * invDet;
|
|
|
|
mat[4][0] = det4_1234_0123 * invDet;
|
|
mat[4][1] = -det4_0234_0123 * invDet;
|
|
mat[4][2] = det4_0134_0123 * invDet;
|
|
mat[4][3] = -det4_0124_0123 * invDet;
|
|
mat[4][4] = det4_0123_0123 * invDet;
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMat5::InverseFastSelf
|
|
============
|
|
*/
|
|
bool idMat5::InverseFastSelf(void) {
|
|
#if 0
|
|
// 280+5+25 = 310 multiplications
|
|
// 1 division
|
|
double det, invDet;
|
|
|
|
// 2x2 sub-determinants required to calculate 5x5 determinant
|
|
float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0];
|
|
float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0];
|
|
float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0];
|
|
float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0];
|
|
float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1];
|
|
float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1];
|
|
float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1];
|
|
float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2];
|
|
float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2];
|
|
float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3];
|
|
|
|
// 3x3 sub-determinants required to calculate 5x5 determinant
|
|
float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01;
|
|
float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01;
|
|
float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01;
|
|
float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02;
|
|
float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02;
|
|
float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03;
|
|
float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12;
|
|
float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12;
|
|
float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13;
|
|
float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23;
|
|
|
|
// 4x4 sub-determinants required to calculate 5x5 determinant
|
|
float det4_1234_0123 = mat[1][0] * det3_234_123 - mat[1][1] * det3_234_023 + mat[1][2] * det3_234_013 - mat[1][3] * det3_234_012;
|
|
float det4_1234_0124 = mat[1][0] * det3_234_124 - mat[1][1] * det3_234_024 + mat[1][2] * det3_234_014 - mat[1][4] * det3_234_012;
|
|
float det4_1234_0134 = mat[1][0] * det3_234_134 - mat[1][1] * det3_234_034 + mat[1][3] * det3_234_014 - mat[1][4] * det3_234_013;
|
|
float det4_1234_0234 = mat[1][0] * det3_234_234 - mat[1][2] * det3_234_034 + mat[1][3] * det3_234_024 - mat[1][4] * det3_234_023;
|
|
float det4_1234_1234 = mat[1][1] * det3_234_234 - mat[1][2] * det3_234_134 + mat[1][3] * det3_234_124 - mat[1][4] * det3_234_123;
|
|
|
|
// determinant of 5x5 matrix
|
|
det = mat[0][0] * det4_1234_1234 - mat[0][1] * det4_1234_0234 + mat[0][2] * det4_1234_0134 - mat[0][3] * det4_1234_0124 + mat[0][4] * det4_1234_0123;
|
|
|
|
if (idMath::Fabs(det) < MATRIX_INVERSE_EPSILON) {
|
|
return false;
|
|
}
|
|
|
|
invDet = 1.0f / det;
|
|
|
|
// remaining 2x2 sub-determinants
|
|
float det2_23_01 = mat[2][0] * mat[3][1] - mat[2][1] * mat[3][0];
|
|
float det2_23_02 = mat[2][0] * mat[3][2] - mat[2][2] * mat[3][0];
|
|
float det2_23_03 = mat[2][0] * mat[3][3] - mat[2][3] * mat[3][0];
|
|
float det2_23_04 = mat[2][0] * mat[3][4] - mat[2][4] * mat[3][0];
|
|
float det2_23_12 = mat[2][1] * mat[3][2] - mat[2][2] * mat[3][1];
|
|
float det2_23_13 = mat[2][1] * mat[3][3] - mat[2][3] * mat[3][1];
|
|
float det2_23_14 = mat[2][1] * mat[3][4] - mat[2][4] * mat[3][1];
|
|
float det2_23_23 = mat[2][2] * mat[3][3] - mat[2][3] * mat[3][2];
|
|
float det2_23_24 = mat[2][2] * mat[3][4] - mat[2][4] * mat[3][2];
|
|
float det2_23_34 = mat[2][3] * mat[3][4] - mat[2][4] * mat[3][3];
|
|
float det2_24_01 = mat[2][0] * mat[4][1] - mat[2][1] * mat[4][0];
|
|
float det2_24_02 = mat[2][0] * mat[4][2] - mat[2][2] * mat[4][0];
|
|
float det2_24_03 = mat[2][0] * mat[4][3] - mat[2][3] * mat[4][0];
|
|
float det2_24_04 = mat[2][0] * mat[4][4] - mat[2][4] * mat[4][0];
|
|
float det2_24_12 = mat[2][1] * mat[4][2] - mat[2][2] * mat[4][1];
|
|
float det2_24_13 = mat[2][1] * mat[4][3] - mat[2][3] * mat[4][1];
|
|
float det2_24_14 = mat[2][1] * mat[4][4] - mat[2][4] * mat[4][1];
|
|
float det2_24_23 = mat[2][2] * mat[4][3] - mat[2][3] * mat[4][2];
|
|
float det2_24_24 = mat[2][2] * mat[4][4] - mat[2][4] * mat[4][2];
|
|
float det2_24_34 = mat[2][3] * mat[4][4] - mat[2][4] * mat[4][3];
|
|
|
|
// remaining 3x3 sub-determinants
|
|
float det3_123_012 = mat[1][0] * det2_23_12 - mat[1][1] * det2_23_02 + mat[1][2] * det2_23_01;
|
|
float det3_123_013 = mat[1][0] * det2_23_13 - mat[1][1] * det2_23_03 + mat[1][3] * det2_23_01;
|
|
float det3_123_014 = mat[1][0] * det2_23_14 - mat[1][1] * det2_23_04 + mat[1][4] * det2_23_01;
|
|
float det3_123_023 = mat[1][0] * det2_23_23 - mat[1][2] * det2_23_03 + mat[1][3] * det2_23_02;
|
|
float det3_123_024 = mat[1][0] * det2_23_24 - mat[1][2] * det2_23_04 + mat[1][4] * det2_23_02;
|
|
float det3_123_034 = mat[1][0] * det2_23_34 - mat[1][3] * det2_23_04 + mat[1][4] * det2_23_03;
|
|
float det3_123_123 = mat[1][1] * det2_23_23 - mat[1][2] * det2_23_13 + mat[1][3] * det2_23_12;
|
|
float det3_123_124 = mat[1][1] * det2_23_24 - mat[1][2] * det2_23_14 + mat[1][4] * det2_23_12;
|
|
float det3_123_134 = mat[1][1] * det2_23_34 - mat[1][3] * det2_23_14 + mat[1][4] * det2_23_13;
|
|
float det3_123_234 = mat[1][2] * det2_23_34 - mat[1][3] * det2_23_24 + mat[1][4] * det2_23_23;
|
|
float det3_124_012 = mat[1][0] * det2_24_12 - mat[1][1] * det2_24_02 + mat[1][2] * det2_24_01;
|
|
float det3_124_013 = mat[1][0] * det2_24_13 - mat[1][1] * det2_24_03 + mat[1][3] * det2_24_01;
|
|
float det3_124_014 = mat[1][0] * det2_24_14 - mat[1][1] * det2_24_04 + mat[1][4] * det2_24_01;
|
|
float det3_124_023 = mat[1][0] * det2_24_23 - mat[1][2] * det2_24_03 + mat[1][3] * det2_24_02;
|
|
float det3_124_024 = mat[1][0] * det2_24_24 - mat[1][2] * det2_24_04 + mat[1][4] * det2_24_02;
|
|
float det3_124_034 = mat[1][0] * det2_24_34 - mat[1][3] * det2_24_04 + mat[1][4] * det2_24_03;
|
|
float det3_124_123 = mat[1][1] * det2_24_23 - mat[1][2] * det2_24_13 + mat[1][3] * det2_24_12;
|
|
float det3_124_124 = mat[1][1] * det2_24_24 - mat[1][2] * det2_24_14 + mat[1][4] * det2_24_12;
|
|
float det3_124_134 = mat[1][1] * det2_24_34 - mat[1][3] * det2_24_14 + mat[1][4] * det2_24_13;
|
|
float det3_124_234 = mat[1][2] * det2_24_34 - mat[1][3] * det2_24_24 + mat[1][4] * det2_24_23;
|
|
float det3_134_012 = mat[1][0] * det2_34_12 - mat[1][1] * det2_34_02 + mat[1][2] * det2_34_01;
|
|
float det3_134_013 = mat[1][0] * det2_34_13 - mat[1][1] * det2_34_03 + mat[1][3] * det2_34_01;
|
|
float det3_134_014 = mat[1][0] * det2_34_14 - mat[1][1] * det2_34_04 + mat[1][4] * det2_34_01;
|
|
float det3_134_023 = mat[1][0] * det2_34_23 - mat[1][2] * det2_34_03 + mat[1][3] * det2_34_02;
|
|
float det3_134_024 = mat[1][0] * det2_34_24 - mat[1][2] * det2_34_04 + mat[1][4] * det2_34_02;
|
|
float det3_134_034 = mat[1][0] * det2_34_34 - mat[1][3] * det2_34_04 + mat[1][4] * det2_34_03;
|
|
float det3_134_123 = mat[1][1] * det2_34_23 - mat[1][2] * det2_34_13 + mat[1][3] * det2_34_12;
|
|
float det3_134_124 = mat[1][1] * det2_34_24 - mat[1][2] * det2_34_14 + mat[1][4] * det2_34_12;
|
|
float det3_134_134 = mat[1][1] * det2_34_34 - mat[1][3] * det2_34_14 + mat[1][4] * det2_34_13;
|
|
float det3_134_234 = mat[1][2] * det2_34_34 - mat[1][3] * det2_34_24 + mat[1][4] * det2_34_23;
|
|
|
|
// remaining 4x4 sub-determinants
|
|
float det4_0123_0123 = mat[0][0] * det3_123_123 - mat[0][1] * det3_123_023 + mat[0][2] * det3_123_013 - mat[0][3] * det3_123_012;
|
|
float det4_0123_0124 = mat[0][0] * det3_123_124 - mat[0][1] * det3_123_024 + mat[0][2] * det3_123_014 - mat[0][4] * det3_123_012;
|
|
float det4_0123_0134 = mat[0][0] * det3_123_134 - mat[0][1] * det3_123_034 + mat[0][3] * det3_123_014 - mat[0][4] * det3_123_013;
|
|
float det4_0123_0234 = mat[0][0] * det3_123_234 - mat[0][2] * det3_123_034 + mat[0][3] * det3_123_024 - mat[0][4] * det3_123_023;
|
|
float det4_0123_1234 = mat[0][1] * det3_123_234 - mat[0][2] * det3_123_134 + mat[0][3] * det3_123_124 - mat[0][4] * det3_123_123;
|
|
float det4_0124_0123 = mat[0][0] * det3_124_123 - mat[0][1] * det3_124_023 + mat[0][2] * det3_124_013 - mat[0][3] * det3_124_012;
|
|
float det4_0124_0124 = mat[0][0] * det3_124_124 - mat[0][1] * det3_124_024 + mat[0][2] * det3_124_014 - mat[0][4] * det3_124_012;
|
|
float det4_0124_0134 = mat[0][0] * det3_124_134 - mat[0][1] * det3_124_034 + mat[0][3] * det3_124_014 - mat[0][4] * det3_124_013;
|
|
float det4_0124_0234 = mat[0][0] * det3_124_234 - mat[0][2] * det3_124_034 + mat[0][3] * det3_124_024 - mat[0][4] * det3_124_023;
|
|
float det4_0124_1234 = mat[0][1] * det3_124_234 - mat[0][2] * det3_124_134 + mat[0][3] * det3_124_124 - mat[0][4] * det3_124_123;
|
|
float det4_0134_0123 = mat[0][0] * det3_134_123 - mat[0][1] * det3_134_023 + mat[0][2] * det3_134_013 - mat[0][3] * det3_134_012;
|
|
float det4_0134_0124 = mat[0][0] * det3_134_124 - mat[0][1] * det3_134_024 + mat[0][2] * det3_134_014 - mat[0][4] * det3_134_012;
|
|
float det4_0134_0134 = mat[0][0] * det3_134_134 - mat[0][1] * det3_134_034 + mat[0][3] * det3_134_014 - mat[0][4] * det3_134_013;
|
|
float det4_0134_0234 = mat[0][0] * det3_134_234 - mat[0][2] * det3_134_034 + mat[0][3] * det3_134_024 - mat[0][4] * det3_134_023;
|
|
float det4_0134_1234 = mat[0][1] * det3_134_234 - mat[0][2] * det3_134_134 + mat[0][3] * det3_134_124 - mat[0][4] * det3_134_123;
|
|
float det4_0234_0123 = mat[0][0] * det3_234_123 - mat[0][1] * det3_234_023 + mat[0][2] * det3_234_013 - mat[0][3] * det3_234_012;
|
|
float det4_0234_0124 = mat[0][0] * det3_234_124 - mat[0][1] * det3_234_024 + mat[0][2] * det3_234_014 - mat[0][4] * det3_234_012;
|
|
float det4_0234_0134 = mat[0][0] * det3_234_134 - mat[0][1] * det3_234_034 + mat[0][3] * det3_234_014 - mat[0][4] * det3_234_013;
|
|
float det4_0234_0234 = mat[0][0] * det3_234_234 - mat[0][2] * det3_234_034 + mat[0][3] * det3_234_024 - mat[0][4] * det3_234_023;
|
|
float det4_0234_1234 = mat[0][1] * det3_234_234 - mat[0][2] * det3_234_134 + mat[0][3] * det3_234_124 - mat[0][4] * det3_234_123;
|
|
|
|
mat[0][0] = det4_1234_1234 * invDet;
|
|
mat[0][1] = -det4_0234_1234 * invDet;
|
|
mat[0][2] = det4_0134_1234 * invDet;
|
|
mat[0][3] = -det4_0124_1234 * invDet;
|
|
mat[0][4] = det4_0123_1234 * invDet;
|
|
|
|
mat[1][0] = -det4_1234_0234 * invDet;
|
|
mat[1][1] = det4_0234_0234 * invDet;
|
|
mat[1][2] = -det4_0134_0234 * invDet;
|
|
mat[1][3] = det4_0124_0234 * invDet;
|
|
mat[1][4] = -det4_0123_0234 * invDet;
|
|
|
|
mat[2][0] = det4_1234_0134 * invDet;
|
|
mat[2][1] = -det4_0234_0134 * invDet;
|
|
mat[2][2] = det4_0134_0134 * invDet;
|
|
mat[2][3] = -det4_0124_0134 * invDet;
|
|
mat[2][4] = det4_0123_0134 * invDet;
|
|
|
|
mat[3][0] = -det4_1234_0124 * invDet;
|
|
mat[3][1] = det4_0234_0124 * invDet;
|
|
mat[3][2] = -det4_0134_0124 * invDet;
|
|
mat[3][3] = det4_0124_0124 * invDet;
|
|
mat[3][4] = -det4_0123_0124 * invDet;
|
|
|
|
mat[4][0] = det4_1234_0123 * invDet;
|
|
mat[4][1] = -det4_0234_0123 * invDet;
|
|
mat[4][2] = det4_0134_0123 * invDet;
|
|
mat[4][3] = -det4_0124_0123 * invDet;
|
|
mat[4][4] = det4_0123_0123 * invDet;
|
|
|
|
return true;
|
|
#elif 0
|
|
// 5*28 = 140 multiplications
|
|
// 5 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;
|
|
d = -d;
|
|
mat[5] *= d;
|
|
mat[10] *= d;
|
|
mat[15] *= d;
|
|
mat[20] *= d;
|
|
d = mat[5] * di;
|
|
mat[6] += mat[1] * d;
|
|
mat[7] += mat[2] * d;
|
|
mat[8] += mat[3] * d;
|
|
mat[9] += mat[4] * d;
|
|
d = mat[10] * di;
|
|
mat[11] += mat[1] * d;
|
|
mat[12] += mat[2] * d;
|
|
mat[13] += mat[3] * d;
|
|
mat[14] += mat[4] * d;
|
|
d = mat[15] * di;
|
|
mat[16] += mat[1] * d;
|
|
mat[17] += mat[2] * d;
|
|
mat[18] += mat[3] * d;
|
|
mat[19] += mat[4] * d;
|
|
d = mat[20] * di;
|
|
mat[21] += mat[1] * d;
|
|
mat[22] += mat[2] * d;
|
|
mat[23] += mat[3] * d;
|
|
mat[24] += mat[4] * d;
|
|
di = mat[6];
|
|
s *= di;
|
|
mat[6] = d = 1.0f / di;
|
|
mat[5] *= d;
|
|
mat[7] *= d;
|
|
mat[8] *= d;
|
|
mat[9] *= d;
|
|
d = -d;
|
|
mat[1] *= d;
|
|
mat[11] *= d;
|
|
mat[16] *= d;
|
|
mat[21] *= d;
|
|
d = mat[1] * di;
|
|
mat[0] += mat[5] * d;
|
|
mat[2] += mat[7] * d;
|
|
mat[3] += mat[8] * d;
|
|
mat[4] += mat[9] * d;
|
|
d = mat[11] * di;
|
|
mat[10] += mat[5] * d;
|
|
mat[12] += mat[7] * d;
|
|
mat[13] += mat[8] * d;
|
|
mat[14] += mat[9] * d;
|
|
d = mat[16] * di;
|
|
mat[15] += mat[5] * d;
|
|
mat[17] += mat[7] * d;
|
|
mat[18] += mat[8] * d;
|
|
mat[19] += mat[9] * d;
|
|
d = mat[21] * di;
|
|
mat[20] += mat[5] * d;
|
|
mat[22] += mat[7] * d;
|
|
mat[23] += mat[8] * d;
|
|
mat[24] += mat[9] * d;
|
|
di = mat[12];
|
|
s *= di;
|
|
mat[12] = d = 1.0f / di;
|
|
mat[10] *= d;
|
|
mat[11] *= d;
|
|
mat[13] *= d;
|
|
mat[14] *= d;
|
|
d = -d;
|
|
mat[2] *= d;
|
|
mat[7] *= d;
|
|
mat[17] *= d;
|
|
mat[22] *= d;
|
|
d = mat[2] * di;
|
|
mat[0] += mat[10] * d;
|
|
mat[1] += mat[11] * d;
|
|
mat[3] += mat[13] * d;
|
|
mat[4] += mat[14] * d;
|
|
d = mat[7] * di;
|
|
mat[5] += mat[10] * d;
|
|
mat[6] += mat[11] * d;
|
|
mat[8] += mat[13] * d;
|
|
mat[9] += mat[14] * d;
|
|
d = mat[17] * di;
|
|
mat[15] += mat[10] * d;
|
|
mat[16] += mat[11] * d;
|
|
mat[18] += mat[13] * d;
|
|
mat[19] += mat[14] * d;
|
|
d = mat[22] * di;
|
|
mat[20] += mat[10] * d;
|
|
mat[21] += mat[11] * d;
|
|
mat[23] += mat[13] * d;
|
|
mat[24] += mat[14] * d;
|
|
di = mat[18];
|
|
s *= di;
|
|
mat[18] = d = 1.0f / di;
|
|
mat[15] *= d;
|
|
mat[16] *= d;
|
|
mat[17] *= d;
|
|
mat[19] *= d;
|
|
d = -d;
|
|
mat[3] *= d;
|
|
mat[8] *= d;
|
|
mat[13] *= d;
|
|
mat[23] *= d;
|
|
d = mat[3] * di;
|
|
mat[0] += mat[15] * d;
|
|
mat[1] += mat[16] * d;
|
|
mat[2] += mat[17] * d;
|
|
mat[4] += mat[19] * d;
|
|
d = mat[8] * di;
|
|
mat[5] += mat[15] * d;
|
|
mat[6] += mat[16] * d;
|
|
mat[7] += mat[17] * d;
|
|
mat[9] += mat[19] * d;
|
|
d = mat[13] * di;
|
|
mat[10] += mat[15] * d;
|
|
mat[11] += mat[16] * d;
|
|
mat[12] += mat[17] * d;
|
|
mat[14] += mat[19] * d;
|
|
d = mat[23] * di;
|
|
mat[20] += mat[15] * d;
|
|
mat[21] += mat[16] * d;
|
|
mat[22] += mat[17] * d;
|
|
mat[24] += mat[19] * d;
|
|
di = mat[24];
|
|
s *= di;
|
|
mat[24] = d = 1.0f / di;
|
|
mat[20] *= d;
|
|
mat[21] *= d;
|
|
mat[22] *= d;
|
|
mat[23] *= d;
|
|
d = -d;
|
|
mat[4] *= d;
|
|
mat[9] *= d;
|
|
mat[14] *= d;
|
|
mat[19] *= d;
|
|
d = mat[4] * di;
|
|
mat[0] += mat[20] * d;
|
|
mat[1] += mat[21] * d;
|
|
mat[2] += mat[22] * d;
|
|
mat[3] += mat[23] * d;
|
|
d = mat[9] * di;
|
|
mat[5] += mat[20] * d;
|
|
mat[6] += mat[21] * d;
|
|
mat[7] += mat[22] * d;
|
|
mat[8] += mat[23] * d;
|
|
d = mat[14] * di;
|
|
mat[10] += mat[20] * d;
|
|
mat[11] += mat[21] * d;
|
|
mat[12] += mat[22] * d;
|
|
mat[13] += mat[23] * d;
|
|
d = mat[19] * di;
|
|
mat[15] += mat[20] * d;
|
|
mat[16] += mat[21] * d;
|
|
mat[17] += mat[22] * d;
|
|
mat[18] += mat[23] * d;
|
|
|
|
return (s != 0.0f && !FLOAT_IS_NAN(s));
|
|
#else
|
|
// 86+30+6 = 122 multiplications
|
|
// 2*1 = 2 divisions
|
|
idMat3 r0, r1, r2, r3;
|
|
float c0, c1, c2, det, invDet;
|
|
float* mat = reinterpret_cast<float*>(this);
|
|
|
|
// r0 = m0.Inverse(); // 3x3
|
|
c0 = mat[1 * 5 + 1] * mat[2 * 5 + 2] - mat[1 * 5 + 2] * mat[2 * 5 + 1];
|
|
c1 = mat[1 * 5 + 2] * mat[2 * 5 + 0] - mat[1 * 5 + 0] * mat[2 * 5 + 2];
|
|
c2 = mat[1 * 5 + 0] * mat[2 * 5 + 1] - mat[1 * 5 + 1] * mat[2 * 5 + 0];
|
|
|
|
det = mat[0 * 5 + 0] * c0 + mat[0 * 5 + 1] * c1 + mat[0 * 5 + 2] * c2;
|
|
|
|
if (idMath::Fabs(det) < MATRIX_INVERSE_EPSILON) {
|
|
return false;
|
|
}
|
|
|
|
invDet = 1.0f / det;
|
|
|
|
r0[0][0] = c0 * invDet;
|
|
r0[0][1] = (mat[0 * 5 + 2] * mat[2 * 5 + 1] - mat[0 * 5 + 1] * mat[2 * 5 + 2]) * invDet;
|
|
r0[0][2] = (mat[0 * 5 + 1] * mat[1 * 5 + 2] - mat[0 * 5 + 2] * mat[1 * 5 + 1]) * invDet;
|
|
r0[1][0] = c1 * invDet;
|
|
r0[1][1] = (mat[0 * 5 + 0] * mat[2 * 5 + 2] - mat[0 * 5 + 2] * mat[2 * 5 + 0]) * invDet;
|
|
r0[1][2] = (mat[0 * 5 + 2] * mat[1 * 5 + 0] - mat[0 * 5 + 0] * mat[1 * 5 + 2]) * invDet;
|
|
r0[2][0] = c2 * invDet;
|
|
r0[2][1] = (mat[0 * 5 + 1] * mat[2 * 5 + 0] - mat[0 * 5 + 0] * mat[2 * 5 + 1]) * invDet;
|
|
r0[2][2] = (mat[0 * 5 + 0] * mat[1 * 5 + 1] - mat[0 * 5 + 1] * mat[1 * 5 + 0]) * invDet;
|
|
|
|
// r1 = r0 * m1; // 3x2 = 3x3 * 3x2
|
|
r1[0][0] = r0[0][0] * mat[0 * 5 + 3] + r0[0][1] * mat[1 * 5 + 3] + r0[0][2] * mat[2 * 5 + 3];
|
|
r1[0][1] = r0[0][0] * mat[0 * 5 + 4] + r0[0][1] * mat[1 * 5 + 4] + r0[0][2] * mat[2 * 5 + 4];
|
|
r1[1][0] = r0[1][0] * mat[0 * 5 + 3] + r0[1][1] * mat[1 * 5 + 3] + r0[1][2] * mat[2 * 5 + 3];
|
|
r1[1][1] = r0[1][0] * mat[0 * 5 + 4] + r0[1][1] * mat[1 * 5 + 4] + r0[1][2] * mat[2 * 5 + 4];
|
|
r1[2][0] = r0[2][0] * mat[0 * 5 + 3] + r0[2][1] * mat[1 * 5 + 3] + r0[2][2] * mat[2 * 5 + 3];
|
|
r1[2][1] = r0[2][0] * mat[0 * 5 + 4] + r0[2][1] * mat[1 * 5 + 4] + r0[2][2] * mat[2 * 5 + 4];
|
|
|
|
// r2 = m2 * r1; // 2x2 = 2x3 * 3x2
|
|
r2[0][0] = mat[3 * 5 + 0] * r1[0][0] + mat[3 * 5 + 1] * r1[1][0] + mat[3 * 5 + 2] * r1[2][0];
|
|
r2[0][1] = mat[3 * 5 + 0] * r1[0][1] + mat[3 * 5 + 1] * r1[1][1] + mat[3 * 5 + 2] * r1[2][1];
|
|
r2[1][0] = mat[4 * 5 + 0] * r1[0][0] + mat[4 * 5 + 1] * r1[1][0] + mat[4 * 5 + 2] * r1[2][0];
|
|
r2[1][1] = mat[4 * 5 + 0] * r1[0][1] + mat[4 * 5 + 1] * r1[1][1] + mat[4 * 5 + 2] * r1[2][1];
|
|
|
|
// r3 = r2 - m3; // 2x2 = 2x2 - 2x2
|
|
r3[0][0] = r2[0][0] - mat[3 * 5 + 3];
|
|
r3[0][1] = r2[0][1] - mat[3 * 5 + 4];
|
|
r3[1][0] = r2[1][0] - mat[4 * 5 + 3];
|
|
r3[1][1] = r2[1][1] - mat[4 * 5 + 4];
|
|
|
|
// r3.InverseSelf(); // 2x2
|
|
det = r3[0][0] * r3[1][1] - r3[0][1] * r3[1][0];
|
|
|
|
if (idMath::Fabs(det) < MATRIX_INVERSE_EPSILON) {
|
|
return false;
|
|
}
|
|
|
|
invDet = 1.0f / det;
|
|
|
|
c0 = r3[0][0];
|
|
r3[0][0] = r3[1][1] * invDet;
|
|
r3[0][1] = -r3[0][1] * invDet;
|
|
r3[1][0] = -r3[1][0] * invDet;
|
|
r3[1][1] = c0 * invDet;
|
|
|
|
// r2 = m2 * r0; // 2x3 = 2x3 * 3x3
|
|
r2[0][0] = mat[3 * 5 + 0] * r0[0][0] + mat[3 * 5 + 1] * r0[1][0] + mat[3 * 5 + 2] * r0[2][0];
|
|
r2[0][1] = mat[3 * 5 + 0] * r0[0][1] + mat[3 * 5 + 1] * r0[1][1] + mat[3 * 5 + 2] * r0[2][1];
|
|
r2[0][2] = mat[3 * 5 + 0] * r0[0][2] + mat[3 * 5 + 1] * r0[1][2] + mat[3 * 5 + 2] * r0[2][2];
|
|
r2[1][0] = mat[4 * 5 + 0] * r0[0][0] + mat[4 * 5 + 1] * r0[1][0] + mat[4 * 5 + 2] * r0[2][0];
|
|
r2[1][1] = mat[4 * 5 + 0] * r0[0][1] + mat[4 * 5 + 1] * r0[1][1] + mat[4 * 5 + 2] * r0[2][1];
|
|
r2[1][2] = mat[4 * 5 + 0] * r0[0][2] + mat[4 * 5 + 1] * r0[1][2] + mat[4 * 5 + 2] * r0[2][2];
|
|
|
|
// m2 = r3 * r2; // 2x3 = 2x2 * 2x3
|
|
mat[3 * 5 + 0] = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0];
|
|
mat[3 * 5 + 1] = r3[0][0] * r2[0][1] + r3[0][1] * r2[1][1];
|
|
mat[3 * 5 + 2] = r3[0][0] * r2[0][2] + r3[0][1] * r2[1][2];
|
|
mat[4 * 5 + 0] = r3[1][0] * r2[0][0] + r3[1][1] * r2[1][0];
|
|
mat[4 * 5 + 1] = r3[1][0] * r2[0][1] + r3[1][1] * r2[1][1];
|
|
mat[4 * 5 + 2] = r3[1][0] * r2[0][2] + r3[1][1] * r2[1][2];
|
|
|
|
// m0 = r0 - r1 * m2; // 3x3 = 3x3 - 3x2 * 2x3
|
|
mat[0 * 5 + 0] = r0[0][0] - r1[0][0] * mat[3 * 5 + 0] - r1[0][1] * mat[4 * 5 + 0];
|
|
mat[0 * 5 + 1] = r0[0][1] - r1[0][0] * mat[3 * 5 + 1] - r1[0][1] * mat[4 * 5 + 1];
|
|
mat[0 * 5 + 2] = r0[0][2] - r1[0][0] * mat[3 * 5 + 2] - r1[0][1] * mat[4 * 5 + 2];
|
|
mat[1 * 5 + 0] = r0[1][0] - r1[1][0] * mat[3 * 5 + 0] - r1[1][1] * mat[4 * 5 + 0];
|
|
mat[1 * 5 + 1] = r0[1][1] - r1[1][0] * mat[3 * 5 + 1] - r1[1][1] * mat[4 * 5 + 1];
|
|
mat[1 * 5 + 2] = r0[1][2] - r1[1][0] * mat[3 * 5 + 2] - r1[1][1] * mat[4 * 5 + 2];
|
|
mat[2 * 5 + 0] = r0[2][0] - r1[2][0] * mat[3 * 5 + 0] - r1[2][1] * mat[4 * 5 + 0];
|
|
mat[2 * 5 + 1] = r0[2][1] - r1[2][0] * mat[3 * 5 + 1] - r1[2][1] * mat[4 * 5 + 1];
|
|
mat[2 * 5 + 2] = r0[2][2] - r1[2][0] * mat[3 * 5 + 2] - r1[2][1] * mat[4 * 5 + 2];
|
|
|
|
// m1 = r1 * r3; // 3x2 = 3x2 * 2x2
|
|
mat[0 * 5 + 3] = r1[0][0] * r3[0][0] + r1[0][1] * r3[1][0];
|
|
mat[0 * 5 + 4] = r1[0][0] * r3[0][1] + r1[0][1] * r3[1][1];
|
|
mat[1 * 5 + 3] = r1[1][0] * r3[0][0] + r1[1][1] * r3[1][0];
|
|
mat[1 * 5 + 4] = r1[1][0] * r3[0][1] + r1[1][1] * r3[1][1];
|
|
mat[2 * 5 + 3] = r1[2][0] * r3[0][0] + r1[2][1] * r3[1][0];
|
|
mat[2 * 5 + 4] = r1[2][0] * r3[0][1] + r1[2][1] * r3[1][1];
|
|
|
|
// m3 = -r3; // 2x2 = - 2x2
|
|
mat[3 * 5 + 3] = -r3[0][0];
|
|
mat[3 * 5 + 4] = -r3[0][1];
|
|
mat[4 * 5 + 3] = -r3[1][0];
|
|
mat[4 * 5 + 4] = -r3[1][1];
|
|
|
|
return true;
|
|
#endif
|
|
}
|
|
|
|
/*
|
|
=============
|
|
idMat5::ToString
|
|
=============
|
|
*/
|
|
const char* idMat5::ToString(int precision) const {
|
|
return idStr::FloatArrayToString(ToFloatPtr(), GetDimension(), precision);
|
|
}
|
|
|
|
|
|
//===============================================================
|
|
//
|
|
// idMat6
|
|
//
|
|
//===============================================================
|
|
|
|
idMat6 mat6_zero(idVec6(0, 0, 0, 0, 0, 0), idVec6(0, 0, 0, 0, 0, 0), idVec6(0, 0, 0, 0, 0, 0), idVec6(0, 0, 0, 0, 0, 0), idVec6(0, 0, 0, 0, 0, 0), idVec6(0, 0, 0, 0, 0, 0));
|
|
idMat6 mat6_identity(idVec6(1, 0, 0, 0, 0, 0), idVec6(0, 1, 0, 0, 0, 0), idVec6(0, 0, 1, 0, 0, 0), idVec6(0, 0, 0, 1, 0, 0), idVec6(0, 0, 0, 0, 1, 0), idVec6(0, 0, 0, 0, 0, 1));
|
|
|
|
/*
|
|
============
|
|
idMat6::Transpose
|
|
============
|
|
*/
|
|
idMat6 idMat6::Transpose(void) const {
|
|
idMat6 transpose;
|
|
int i, j;
|
|
|
|
for (i = 0; i < 6; i++) {
|
|
for (j = 0; j < 6; j++) {
|
|
transpose[i][j] = mat[j][i];
|
|
}
|
|
}
|
|
return transpose;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMat6::TransposeSelf
|
|
============
|
|
*/
|
|
idMat6& idMat6::TransposeSelf(void) {
|
|
float temp;
|
|
int i, j;
|
|
|
|
for (i = 0; i < 6; i++) {
|
|
for (j = i + 1; j < 6; j++) {
|
|
temp = mat[i][j];
|
|
mat[i][j] = mat[j][i];
|
|
mat[j][i] = temp;
|
|
}
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMat6::Determinant
|
|
============
|
|
*/
|
|
float idMat6::Determinant(void) const {
|
|
|
|
// 2x2 sub-determinants required to calculate 6x6 determinant
|
|
float det2_45_01 = mat[4][0] * mat[5][1] - mat[4][1] * mat[5][0];
|
|
float det2_45_02 = mat[4][0] * mat[5][2] - mat[4][2] * mat[5][0];
|
|
float det2_45_03 = mat[4][0] * mat[5][3] - mat[4][3] * mat[5][0];
|
|
float det2_45_04 = mat[4][0] * mat[5][4] - mat[4][4] * mat[5][0];
|
|
float det2_45_05 = mat[4][0] * mat[5][5] - mat[4][5] * mat[5][0];
|
|
float det2_45_12 = mat[4][1] * mat[5][2] - mat[4][2] * mat[5][1];
|
|
float det2_45_13 = mat[4][1] * mat[5][3] - mat[4][3] * mat[5][1];
|
|
float det2_45_14 = mat[4][1] * mat[5][4] - mat[4][4] * mat[5][1];
|
|
float det2_45_15 = mat[4][1] * mat[5][5] - mat[4][5] * mat[5][1];
|
|
float det2_45_23 = mat[4][2] * mat[5][3] - mat[4][3] * mat[5][2];
|
|
float det2_45_24 = mat[4][2] * mat[5][4] - mat[4][4] * mat[5][2];
|
|
float det2_45_25 = mat[4][2] * mat[5][5] - mat[4][5] * mat[5][2];
|
|
float det2_45_34 = mat[4][3] * mat[5][4] - mat[4][4] * mat[5][3];
|
|
float det2_45_35 = mat[4][3] * mat[5][5] - mat[4][5] * mat[5][3];
|
|
float det2_45_45 = mat[4][4] * mat[5][5] - mat[4][5] * mat[5][4];
|
|
|
|
// 3x3 sub-determinants required to calculate 6x6 determinant
|
|
float det3_345_012 = mat[3][0] * det2_45_12 - mat[3][1] * det2_45_02 + mat[3][2] * det2_45_01;
|
|
float det3_345_013 = mat[3][0] * det2_45_13 - mat[3][1] * det2_45_03 + mat[3][3] * det2_45_01;
|
|
float det3_345_014 = mat[3][0] * det2_45_14 - mat[3][1] * det2_45_04 + mat[3][4] * det2_45_01;
|
|
float det3_345_015 = mat[3][0] * det2_45_15 - mat[3][1] * det2_45_05 + mat[3][5] * det2_45_01;
|
|
float det3_345_023 = mat[3][0] * det2_45_23 - mat[3][2] * det2_45_03 + mat[3][3] * det2_45_02;
|
|
float det3_345_024 = mat[3][0] * det2_45_24 - mat[3][2] * det2_45_04 + mat[3][4] * det2_45_02;
|
|
float det3_345_025 = mat[3][0] * det2_45_25 - mat[3][2] * det2_45_05 + mat[3][5] * det2_45_02;
|
|
float det3_345_034 = mat[3][0] * det2_45_34 - mat[3][3] * det2_45_04 + mat[3][4] * det2_45_03;
|
|
float det3_345_035 = mat[3][0] * det2_45_35 - mat[3][3] * det2_45_05 + mat[3][5] * det2_45_03;
|
|
float det3_345_045 = mat[3][0] * det2_45_45 - mat[3][4] * det2_45_05 + mat[3][5] * det2_45_04;
|
|
float det3_345_123 = mat[3][1] * det2_45_23 - mat[3][2] * det2_45_13 + mat[3][3] * det2_45_12;
|
|
float det3_345_124 = mat[3][1] * det2_45_24 - mat[3][2] * det2_45_14 + mat[3][4] * det2_45_12;
|
|
float det3_345_125 = mat[3][1] * det2_45_25 - mat[3][2] * det2_45_15 + mat[3][5] * det2_45_12;
|
|
float det3_345_134 = mat[3][1] * det2_45_34 - mat[3][3] * det2_45_14 + mat[3][4] * det2_45_13;
|
|
float det3_345_135 = mat[3][1] * det2_45_35 - mat[3][3] * det2_45_15 + mat[3][5] * det2_45_13;
|
|
float det3_345_145 = mat[3][1] * det2_45_45 - mat[3][4] * det2_45_15 + mat[3][5] * det2_45_14;
|
|
float det3_345_234 = mat[3][2] * det2_45_34 - mat[3][3] * det2_45_24 + mat[3][4] * det2_45_23;
|
|
float det3_345_235 = mat[3][2] * det2_45_35 - mat[3][3] * det2_45_25 + mat[3][5] * det2_45_23;
|
|
float det3_345_245 = mat[3][2] * det2_45_45 - mat[3][4] * det2_45_25 + mat[3][5] * det2_45_24;
|
|
float det3_345_345 = mat[3][3] * det2_45_45 - mat[3][4] * det2_45_35 + mat[3][5] * det2_45_34;
|
|
|
|
// 4x4 sub-determinants required to calculate 6x6 determinant
|
|
float det4_2345_0123 = mat[2][0] * det3_345_123 - mat[2][1] * det3_345_023 + mat[2][2] * det3_345_013 - mat[2][3] * det3_345_012;
|
|
float det4_2345_0124 = mat[2][0] * det3_345_124 - mat[2][1] * det3_345_024 + mat[2][2] * det3_345_014 - mat[2][4] * det3_345_012;
|
|
float det4_2345_0125 = mat[2][0] * det3_345_125 - mat[2][1] * det3_345_025 + mat[2][2] * det3_345_015 - mat[2][5] * det3_345_012;
|
|
float det4_2345_0134 = mat[2][0] * det3_345_134 - mat[2][1] * det3_345_034 + mat[2][3] * det3_345_014 - mat[2][4] * det3_345_013;
|
|
float det4_2345_0135 = mat[2][0] * det3_345_135 - mat[2][1] * det3_345_035 + mat[2][3] * det3_345_015 - mat[2][5] * det3_345_013;
|
|
float det4_2345_0145 = mat[2][0] * det3_345_145 - mat[2][1] * det3_345_045 + mat[2][4] * det3_345_015 - mat[2][5] * det3_345_014;
|
|
float det4_2345_0234 = mat[2][0] * det3_345_234 - mat[2][2] * det3_345_034 + mat[2][3] * det3_345_024 - mat[2][4] * det3_345_023;
|
|
float det4_2345_0235 = mat[2][0] * det3_345_235 - mat[2][2] * det3_345_035 + mat[2][3] * det3_345_025 - mat[2][5] * det3_345_023;
|
|
float det4_2345_0245 = mat[2][0] * det3_345_245 - mat[2][2] * det3_345_045 + mat[2][4] * det3_345_025 - mat[2][5] * det3_345_024;
|
|
float det4_2345_0345 = mat[2][0] * det3_345_345 - mat[2][3] * det3_345_045 + mat[2][4] * det3_345_035 - mat[2][5] * det3_345_034;
|
|
float det4_2345_1234 = mat[2][1] * det3_345_234 - mat[2][2] * det3_345_134 + mat[2][3] * det3_345_124 - mat[2][4] * det3_345_123;
|
|
float det4_2345_1235 = mat[2][1] * det3_345_235 - mat[2][2] * det3_345_135 + mat[2][3] * det3_345_125 - mat[2][5] * det3_345_123;
|
|
float det4_2345_1245 = mat[2][1] * det3_345_245 - mat[2][2] * det3_345_145 + mat[2][4] * det3_345_125 - mat[2][5] * det3_345_124;
|
|
float det4_2345_1345 = mat[2][1] * det3_345_345 - mat[2][3] * det3_345_145 + mat[2][4] * det3_345_135 - mat[2][5] * det3_345_134;
|
|
float det4_2345_2345 = mat[2][2] * det3_345_345 - mat[2][3] * det3_345_245 + mat[2][4] * det3_345_235 - mat[2][5] * det3_345_234;
|
|
|
|
// 5x5 sub-determinants required to calculate 6x6 determinant
|
|
float det5_12345_01234 = mat[1][0] * det4_2345_1234 - mat[1][1] * det4_2345_0234 + mat[1][2] * det4_2345_0134 - mat[1][3] * det4_2345_0124 + mat[1][4] * det4_2345_0123;
|
|
float det5_12345_01235 = mat[1][0] * det4_2345_1235 - mat[1][1] * det4_2345_0235 + mat[1][2] * det4_2345_0135 - mat[1][3] * det4_2345_0125 + mat[1][5] * det4_2345_0123;
|
|
float det5_12345_01245 = mat[1][0] * det4_2345_1245 - mat[1][1] * det4_2345_0245 + mat[1][2] * det4_2345_0145 - mat[1][4] * det4_2345_0125 + mat[1][5] * det4_2345_0124;
|
|
float det5_12345_01345 = mat[1][0] * det4_2345_1345 - mat[1][1] * det4_2345_0345 + mat[1][3] * det4_2345_0145 - mat[1][4] * det4_2345_0135 + mat[1][5] * det4_2345_0134;
|
|
float det5_12345_02345 = mat[1][0] * det4_2345_2345 - mat[1][2] * det4_2345_0345 + mat[1][3] * det4_2345_0245 - mat[1][4] * det4_2345_0235 + mat[1][5] * det4_2345_0234;
|
|
float det5_12345_12345 = mat[1][1] * det4_2345_2345 - mat[1][2] * det4_2345_1345 + mat[1][3] * det4_2345_1245 - mat[1][4] * det4_2345_1235 + mat[1][5] * det4_2345_1234;
|
|
|
|
// determinant of 6x6 matrix
|
|
return mat[0][0] * det5_12345_12345 - mat[0][1] * det5_12345_02345 + mat[0][2] * det5_12345_01345 -
|
|
mat[0][3] * det5_12345_01245 + mat[0][4] * det5_12345_01235 - mat[0][5] * det5_12345_01234;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMat6::InverseSelf
|
|
============
|
|
*/
|
|
bool idMat6::InverseSelf(void) {
|
|
// 810+6+36 = 852 multiplications
|
|
// 1 division
|
|
double det, invDet;
|
|
|
|
// 2x2 sub-determinants required to calculate 6x6 determinant
|
|
float det2_45_01 = mat[4][0] * mat[5][1] - mat[4][1] * mat[5][0];
|
|
float det2_45_02 = mat[4][0] * mat[5][2] - mat[4][2] * mat[5][0];
|
|
float det2_45_03 = mat[4][0] * mat[5][3] - mat[4][3] * mat[5][0];
|
|
float det2_45_04 = mat[4][0] * mat[5][4] - mat[4][4] * mat[5][0];
|
|
float det2_45_05 = mat[4][0] * mat[5][5] - mat[4][5] * mat[5][0];
|
|
float det2_45_12 = mat[4][1] * mat[5][2] - mat[4][2] * mat[5][1];
|
|
float det2_45_13 = mat[4][1] * mat[5][3] - mat[4][3] * mat[5][1];
|
|
float det2_45_14 = mat[4][1] * mat[5][4] - mat[4][4] * mat[5][1];
|
|
float det2_45_15 = mat[4][1] * mat[5][5] - mat[4][5] * mat[5][1];
|
|
float det2_45_23 = mat[4][2] * mat[5][3] - mat[4][3] * mat[5][2];
|
|
float det2_45_24 = mat[4][2] * mat[5][4] - mat[4][4] * mat[5][2];
|
|
float det2_45_25 = mat[4][2] * mat[5][5] - mat[4][5] * mat[5][2];
|
|
float det2_45_34 = mat[4][3] * mat[5][4] - mat[4][4] * mat[5][3];
|
|
float det2_45_35 = mat[4][3] * mat[5][5] - mat[4][5] * mat[5][3];
|
|
float det2_45_45 = mat[4][4] * mat[5][5] - mat[4][5] * mat[5][4];
|
|
|
|
// 3x3 sub-determinants required to calculate 6x6 determinant
|
|
float det3_345_012 = mat[3][0] * det2_45_12 - mat[3][1] * det2_45_02 + mat[3][2] * det2_45_01;
|
|
float det3_345_013 = mat[3][0] * det2_45_13 - mat[3][1] * det2_45_03 + mat[3][3] * det2_45_01;
|
|
float det3_345_014 = mat[3][0] * det2_45_14 - mat[3][1] * det2_45_04 + mat[3][4] * det2_45_01;
|
|
float det3_345_015 = mat[3][0] * det2_45_15 - mat[3][1] * det2_45_05 + mat[3][5] * det2_45_01;
|
|
float det3_345_023 = mat[3][0] * det2_45_23 - mat[3][2] * det2_45_03 + mat[3][3] * det2_45_02;
|
|
float det3_345_024 = mat[3][0] * det2_45_24 - mat[3][2] * det2_45_04 + mat[3][4] * det2_45_02;
|
|
float det3_345_025 = mat[3][0] * det2_45_25 - mat[3][2] * det2_45_05 + mat[3][5] * det2_45_02;
|
|
float det3_345_034 = mat[3][0] * det2_45_34 - mat[3][3] * det2_45_04 + mat[3][4] * det2_45_03;
|
|
float det3_345_035 = mat[3][0] * det2_45_35 - mat[3][3] * det2_45_05 + mat[3][5] * det2_45_03;
|
|
float det3_345_045 = mat[3][0] * det2_45_45 - mat[3][4] * det2_45_05 + mat[3][5] * det2_45_04;
|
|
float det3_345_123 = mat[3][1] * det2_45_23 - mat[3][2] * det2_45_13 + mat[3][3] * det2_45_12;
|
|
float det3_345_124 = mat[3][1] * det2_45_24 - mat[3][2] * det2_45_14 + mat[3][4] * det2_45_12;
|
|
float det3_345_125 = mat[3][1] * det2_45_25 - mat[3][2] * det2_45_15 + mat[3][5] * det2_45_12;
|
|
float det3_345_134 = mat[3][1] * det2_45_34 - mat[3][3] * det2_45_14 + mat[3][4] * det2_45_13;
|
|
float det3_345_135 = mat[3][1] * det2_45_35 - mat[3][3] * det2_45_15 + mat[3][5] * det2_45_13;
|
|
float det3_345_145 = mat[3][1] * det2_45_45 - mat[3][4] * det2_45_15 + mat[3][5] * det2_45_14;
|
|
float det3_345_234 = mat[3][2] * det2_45_34 - mat[3][3] * det2_45_24 + mat[3][4] * det2_45_23;
|
|
float det3_345_235 = mat[3][2] * det2_45_35 - mat[3][3] * det2_45_25 + mat[3][5] * det2_45_23;
|
|
float det3_345_245 = mat[3][2] * det2_45_45 - mat[3][4] * det2_45_25 + mat[3][5] * det2_45_24;
|
|
float det3_345_345 = mat[3][3] * det2_45_45 - mat[3][4] * det2_45_35 + mat[3][5] * det2_45_34;
|
|
|
|
// 4x4 sub-determinants required to calculate 6x6 determinant
|
|
float det4_2345_0123 = mat[2][0] * det3_345_123 - mat[2][1] * det3_345_023 + mat[2][2] * det3_345_013 - mat[2][3] * det3_345_012;
|
|
float det4_2345_0124 = mat[2][0] * det3_345_124 - mat[2][1] * det3_345_024 + mat[2][2] * det3_345_014 - mat[2][4] * det3_345_012;
|
|
float det4_2345_0125 = mat[2][0] * det3_345_125 - mat[2][1] * det3_345_025 + mat[2][2] * det3_345_015 - mat[2][5] * det3_345_012;
|
|
float det4_2345_0134 = mat[2][0] * det3_345_134 - mat[2][1] * det3_345_034 + mat[2][3] * det3_345_014 - mat[2][4] * det3_345_013;
|
|
float det4_2345_0135 = mat[2][0] * det3_345_135 - mat[2][1] * det3_345_035 + mat[2][3] * det3_345_015 - mat[2][5] * det3_345_013;
|
|
float det4_2345_0145 = mat[2][0] * det3_345_145 - mat[2][1] * det3_345_045 + mat[2][4] * det3_345_015 - mat[2][5] * det3_345_014;
|
|
float det4_2345_0234 = mat[2][0] * det3_345_234 - mat[2][2] * det3_345_034 + mat[2][3] * det3_345_024 - mat[2][4] * det3_345_023;
|
|
float det4_2345_0235 = mat[2][0] * det3_345_235 - mat[2][2] * det3_345_035 + mat[2][3] * det3_345_025 - mat[2][5] * det3_345_023;
|
|
float det4_2345_0245 = mat[2][0] * det3_345_245 - mat[2][2] * det3_345_045 + mat[2][4] * det3_345_025 - mat[2][5] * det3_345_024;
|
|
float det4_2345_0345 = mat[2][0] * det3_345_345 - mat[2][3] * det3_345_045 + mat[2][4] * det3_345_035 - mat[2][5] * det3_345_034;
|
|
float det4_2345_1234 = mat[2][1] * det3_345_234 - mat[2][2] * det3_345_134 + mat[2][3] * det3_345_124 - mat[2][4] * det3_345_123;
|
|
float det4_2345_1235 = mat[2][1] * det3_345_235 - mat[2][2] * det3_345_135 + mat[2][3] * det3_345_125 - mat[2][5] * det3_345_123;
|
|
float det4_2345_1245 = mat[2][1] * det3_345_245 - mat[2][2] * det3_345_145 + mat[2][4] * det3_345_125 - mat[2][5] * det3_345_124;
|
|
float det4_2345_1345 = mat[2][1] * det3_345_345 - mat[2][3] * det3_345_145 + mat[2][4] * det3_345_135 - mat[2][5] * det3_345_134;
|
|
float det4_2345_2345 = mat[2][2] * det3_345_345 - mat[2][3] * det3_345_245 + mat[2][4] * det3_345_235 - mat[2][5] * det3_345_234;
|
|
|
|
// 5x5 sub-determinants required to calculate 6x6 determinant
|
|
float det5_12345_01234 = mat[1][0] * det4_2345_1234 - mat[1][1] * det4_2345_0234 + mat[1][2] * det4_2345_0134 - mat[1][3] * det4_2345_0124 + mat[1][4] * det4_2345_0123;
|
|
float det5_12345_01235 = mat[1][0] * det4_2345_1235 - mat[1][1] * det4_2345_0235 + mat[1][2] * det4_2345_0135 - mat[1][3] * det4_2345_0125 + mat[1][5] * det4_2345_0123;
|
|
float det5_12345_01245 = mat[1][0] * det4_2345_1245 - mat[1][1] * det4_2345_0245 + mat[1][2] * det4_2345_0145 - mat[1][4] * det4_2345_0125 + mat[1][5] * det4_2345_0124;
|
|
float det5_12345_01345 = mat[1][0] * det4_2345_1345 - mat[1][1] * det4_2345_0345 + mat[1][3] * det4_2345_0145 - mat[1][4] * det4_2345_0135 + mat[1][5] * det4_2345_0134;
|
|
float det5_12345_02345 = mat[1][0] * det4_2345_2345 - mat[1][2] * det4_2345_0345 + mat[1][3] * det4_2345_0245 - mat[1][4] * det4_2345_0235 + mat[1][5] * det4_2345_0234;
|
|
float det5_12345_12345 = mat[1][1] * det4_2345_2345 - mat[1][2] * det4_2345_1345 + mat[1][3] * det4_2345_1245 - mat[1][4] * det4_2345_1235 + mat[1][5] * det4_2345_1234;
|
|
|
|
// determinant of 6x6 matrix
|
|
det = mat[0][0] * det5_12345_12345 - mat[0][1] * det5_12345_02345 + mat[0][2] * det5_12345_01345 -
|
|
mat[0][3] * det5_12345_01245 + mat[0][4] * det5_12345_01235 - mat[0][5] * det5_12345_01234;
|
|
|
|
if (idMath::Fabs(det) < MATRIX_INVERSE_EPSILON) {
|
|
return false;
|
|
}
|
|
|
|
invDet = 1.0f / det;
|
|
|
|
// remaining 2x2 sub-determinants
|
|
float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0];
|
|
float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0];
|
|
float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0];
|
|
float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0];
|
|
float det2_34_05 = mat[3][0] * mat[4][5] - mat[3][5] * mat[4][0];
|
|
float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1];
|
|
float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1];
|
|
float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1];
|
|
float det2_34_15 = mat[3][1] * mat[4][5] - mat[3][5] * mat[4][1];
|
|
float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2];
|
|
float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2];
|
|
float det2_34_25 = mat[3][2] * mat[4][5] - mat[3][5] * mat[4][2];
|
|
float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3];
|
|
float det2_34_35 = mat[3][3] * mat[4][5] - mat[3][5] * mat[4][3];
|
|
float det2_34_45 = mat[3][4] * mat[4][5] - mat[3][5] * mat[4][4];
|
|
float det2_35_01 = mat[3][0] * mat[5][1] - mat[3][1] * mat[5][0];
|
|
float det2_35_02 = mat[3][0] * mat[5][2] - mat[3][2] * mat[5][0];
|
|
float det2_35_03 = mat[3][0] * mat[5][3] - mat[3][3] * mat[5][0];
|
|
float det2_35_04 = mat[3][0] * mat[5][4] - mat[3][4] * mat[5][0];
|
|
float det2_35_05 = mat[3][0] * mat[5][5] - mat[3][5] * mat[5][0];
|
|
float det2_35_12 = mat[3][1] * mat[5][2] - mat[3][2] * mat[5][1];
|
|
float det2_35_13 = mat[3][1] * mat[5][3] - mat[3][3] * mat[5][1];
|
|
float det2_35_14 = mat[3][1] * mat[5][4] - mat[3][4] * mat[5][1];
|
|
float det2_35_15 = mat[3][1] * mat[5][5] - mat[3][5] * mat[5][1];
|
|
float det2_35_23 = mat[3][2] * mat[5][3] - mat[3][3] * mat[5][2];
|
|
float det2_35_24 = mat[3][2] * mat[5][4] - mat[3][4] * mat[5][2];
|
|
float det2_35_25 = mat[3][2] * mat[5][5] - mat[3][5] * mat[5][2];
|
|
float det2_35_34 = mat[3][3] * mat[5][4] - mat[3][4] * mat[5][3];
|
|
float det2_35_35 = mat[3][3] * mat[5][5] - mat[3][5] * mat[5][3];
|
|
float det2_35_45 = mat[3][4] * mat[5][5] - mat[3][5] * mat[5][4];
|
|
|
|
// remaining 3x3 sub-determinants
|
|
float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01;
|
|
float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01;
|
|
float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01;
|
|
float det3_234_015 = mat[2][0] * det2_34_15 - mat[2][1] * det2_34_05 + mat[2][5] * det2_34_01;
|
|
float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02;
|
|
float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02;
|
|
float det3_234_025 = mat[2][0] * det2_34_25 - mat[2][2] * det2_34_05 + mat[2][5] * det2_34_02;
|
|
float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03;
|
|
float det3_234_035 = mat[2][0] * det2_34_35 - mat[2][3] * det2_34_05 + mat[2][5] * det2_34_03;
|
|
float det3_234_045 = mat[2][0] * det2_34_45 - mat[2][4] * det2_34_05 + mat[2][5] * det2_34_04;
|
|
float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12;
|
|
float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12;
|
|
float det3_234_125 = mat[2][1] * det2_34_25 - mat[2][2] * det2_34_15 + mat[2][5] * det2_34_12;
|
|
float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13;
|
|
float det3_234_135 = mat[2][1] * det2_34_35 - mat[2][3] * det2_34_15 + mat[2][5] * det2_34_13;
|
|
float det3_234_145 = mat[2][1] * det2_34_45 - mat[2][4] * det2_34_15 + mat[2][5] * det2_34_14;
|
|
float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23;
|
|
float det3_234_235 = mat[2][2] * det2_34_35 - mat[2][3] * det2_34_25 + mat[2][5] * det2_34_23;
|
|
float det3_234_245 = mat[2][2] * det2_34_45 - mat[2][4] * det2_34_25 + mat[2][5] * det2_34_24;
|
|
float det3_234_345 = mat[2][3] * det2_34_45 - mat[2][4] * det2_34_35 + mat[2][5] * det2_34_34;
|
|
float det3_235_012 = mat[2][0] * det2_35_12 - mat[2][1] * det2_35_02 + mat[2][2] * det2_35_01;
|
|
float det3_235_013 = mat[2][0] * det2_35_13 - mat[2][1] * det2_35_03 + mat[2][3] * det2_35_01;
|
|
float det3_235_014 = mat[2][0] * det2_35_14 - mat[2][1] * det2_35_04 + mat[2][4] * det2_35_01;
|
|
float det3_235_015 = mat[2][0] * det2_35_15 - mat[2][1] * det2_35_05 + mat[2][5] * det2_35_01;
|
|
float det3_235_023 = mat[2][0] * det2_35_23 - mat[2][2] * det2_35_03 + mat[2][3] * det2_35_02;
|
|
float det3_235_024 = mat[2][0] * det2_35_24 - mat[2][2] * det2_35_04 + mat[2][4] * det2_35_02;
|
|
float det3_235_025 = mat[2][0] * det2_35_25 - mat[2][2] * det2_35_05 + mat[2][5] * det2_35_02;
|
|
float det3_235_034 = mat[2][0] * det2_35_34 - mat[2][3] * det2_35_04 + mat[2][4] * det2_35_03;
|
|
float det3_235_035 = mat[2][0] * det2_35_35 - mat[2][3] * det2_35_05 + mat[2][5] * det2_35_03;
|
|
float det3_235_045 = mat[2][0] * det2_35_45 - mat[2][4] * det2_35_05 + mat[2][5] * det2_35_04;
|
|
float det3_235_123 = mat[2][1] * det2_35_23 - mat[2][2] * det2_35_13 + mat[2][3] * det2_35_12;
|
|
float det3_235_124 = mat[2][1] * det2_35_24 - mat[2][2] * det2_35_14 + mat[2][4] * det2_35_12;
|
|
float det3_235_125 = mat[2][1] * det2_35_25 - mat[2][2] * det2_35_15 + mat[2][5] * det2_35_12;
|
|
float det3_235_134 = mat[2][1] * det2_35_34 - mat[2][3] * det2_35_14 + mat[2][4] * det2_35_13;
|
|
float det3_235_135 = mat[2][1] * det2_35_35 - mat[2][3] * det2_35_15 + mat[2][5] * det2_35_13;
|
|
float det3_235_145 = mat[2][1] * det2_35_45 - mat[2][4] * det2_35_15 + mat[2][5] * det2_35_14;
|
|
float det3_235_234 = mat[2][2] * det2_35_34 - mat[2][3] * det2_35_24 + mat[2][4] * det2_35_23;
|
|
float det3_235_235 = mat[2][2] * det2_35_35 - mat[2][3] * det2_35_25 + mat[2][5] * det2_35_23;
|
|
float det3_235_245 = mat[2][2] * det2_35_45 - mat[2][4] * det2_35_25 + mat[2][5] * det2_35_24;
|
|
float det3_235_345 = mat[2][3] * det2_35_45 - mat[2][4] * det2_35_35 + mat[2][5] * det2_35_34;
|
|
float det3_245_012 = mat[2][0] * det2_45_12 - mat[2][1] * det2_45_02 + mat[2][2] * det2_45_01;
|
|
float det3_245_013 = mat[2][0] * det2_45_13 - mat[2][1] * det2_45_03 + mat[2][3] * det2_45_01;
|
|
float det3_245_014 = mat[2][0] * det2_45_14 - mat[2][1] * det2_45_04 + mat[2][4] * det2_45_01;
|
|
float det3_245_015 = mat[2][0] * det2_45_15 - mat[2][1] * det2_45_05 + mat[2][5] * det2_45_01;
|
|
float det3_245_023 = mat[2][0] * det2_45_23 - mat[2][2] * det2_45_03 + mat[2][3] * det2_45_02;
|
|
float det3_245_024 = mat[2][0] * det2_45_24 - mat[2][2] * det2_45_04 + mat[2][4] * det2_45_02;
|
|
float det3_245_025 = mat[2][0] * det2_45_25 - mat[2][2] * det2_45_05 + mat[2][5] * det2_45_02;
|
|
float det3_245_034 = mat[2][0] * det2_45_34 - mat[2][3] * det2_45_04 + mat[2][4] * det2_45_03;
|
|
float det3_245_035 = mat[2][0] * det2_45_35 - mat[2][3] * det2_45_05 + mat[2][5] * det2_45_03;
|
|
float det3_245_045 = mat[2][0] * det2_45_45 - mat[2][4] * det2_45_05 + mat[2][5] * det2_45_04;
|
|
float det3_245_123 = mat[2][1] * det2_45_23 - mat[2][2] * det2_45_13 + mat[2][3] * det2_45_12;
|
|
float det3_245_124 = mat[2][1] * det2_45_24 - mat[2][2] * det2_45_14 + mat[2][4] * det2_45_12;
|
|
float det3_245_125 = mat[2][1] * det2_45_25 - mat[2][2] * det2_45_15 + mat[2][5] * det2_45_12;
|
|
float det3_245_134 = mat[2][1] * det2_45_34 - mat[2][3] * det2_45_14 + mat[2][4] * det2_45_13;
|
|
float det3_245_135 = mat[2][1] * det2_45_35 - mat[2][3] * det2_45_15 + mat[2][5] * det2_45_13;
|
|
float det3_245_145 = mat[2][1] * det2_45_45 - mat[2][4] * det2_45_15 + mat[2][5] * det2_45_14;
|
|
float det3_245_234 = mat[2][2] * det2_45_34 - mat[2][3] * det2_45_24 + mat[2][4] * det2_45_23;
|
|
float det3_245_235 = mat[2][2] * det2_45_35 - mat[2][3] * det2_45_25 + mat[2][5] * det2_45_23;
|
|
float det3_245_245 = mat[2][2] * det2_45_45 - mat[2][4] * det2_45_25 + mat[2][5] * det2_45_24;
|
|
float det3_245_345 = mat[2][3] * det2_45_45 - mat[2][4] * det2_45_35 + mat[2][5] * det2_45_34;
|
|
|
|
// remaining 4x4 sub-determinants
|
|
float det4_1234_0123 = mat[1][0] * det3_234_123 - mat[1][1] * det3_234_023 + mat[1][2] * det3_234_013 - mat[1][3] * det3_234_012;
|
|
float det4_1234_0124 = mat[1][0] * det3_234_124 - mat[1][1] * det3_234_024 + mat[1][2] * det3_234_014 - mat[1][4] * det3_234_012;
|
|
float det4_1234_0125 = mat[1][0] * det3_234_125 - mat[1][1] * det3_234_025 + mat[1][2] * det3_234_015 - mat[1][5] * det3_234_012;
|
|
float det4_1234_0134 = mat[1][0] * det3_234_134 - mat[1][1] * det3_234_034 + mat[1][3] * det3_234_014 - mat[1][4] * det3_234_013;
|
|
float det4_1234_0135 = mat[1][0] * det3_234_135 - mat[1][1] * det3_234_035 + mat[1][3] * det3_234_015 - mat[1][5] * det3_234_013;
|
|
float det4_1234_0145 = mat[1][0] * det3_234_145 - mat[1][1] * det3_234_045 + mat[1][4] * det3_234_015 - mat[1][5] * det3_234_014;
|
|
float det4_1234_0234 = mat[1][0] * det3_234_234 - mat[1][2] * det3_234_034 + mat[1][3] * det3_234_024 - mat[1][4] * det3_234_023;
|
|
float det4_1234_0235 = mat[1][0] * det3_234_235 - mat[1][2] * det3_234_035 + mat[1][3] * det3_234_025 - mat[1][5] * det3_234_023;
|
|
float det4_1234_0245 = mat[1][0] * det3_234_245 - mat[1][2] * det3_234_045 + mat[1][4] * det3_234_025 - mat[1][5] * det3_234_024;
|
|
float det4_1234_0345 = mat[1][0] * det3_234_345 - mat[1][3] * det3_234_045 + mat[1][4] * det3_234_035 - mat[1][5] * det3_234_034;
|
|
float det4_1234_1234 = mat[1][1] * det3_234_234 - mat[1][2] * det3_234_134 + mat[1][3] * det3_234_124 - mat[1][4] * det3_234_123;
|
|
float det4_1234_1235 = mat[1][1] * det3_234_235 - mat[1][2] * det3_234_135 + mat[1][3] * det3_234_125 - mat[1][5] * det3_234_123;
|
|
float det4_1234_1245 = mat[1][1] * det3_234_245 - mat[1][2] * det3_234_145 + mat[1][4] * det3_234_125 - mat[1][5] * det3_234_124;
|
|
float det4_1234_1345 = mat[1][1] * det3_234_345 - mat[1][3] * det3_234_145 + mat[1][4] * det3_234_135 - mat[1][5] * det3_234_134;
|
|
float det4_1234_2345 = mat[1][2] * det3_234_345 - mat[1][3] * det3_234_245 + mat[1][4] * det3_234_235 - mat[1][5] * det3_234_234;
|
|
float det4_1235_0123 = mat[1][0] * det3_235_123 - mat[1][1] * det3_235_023 + mat[1][2] * det3_235_013 - mat[1][3] * det3_235_012;
|
|
float det4_1235_0124 = mat[1][0] * det3_235_124 - mat[1][1] * det3_235_024 + mat[1][2] * det3_235_014 - mat[1][4] * det3_235_012;
|
|
float det4_1235_0125 = mat[1][0] * det3_235_125 - mat[1][1] * det3_235_025 + mat[1][2] * det3_235_015 - mat[1][5] * det3_235_012;
|
|
float det4_1235_0134 = mat[1][0] * det3_235_134 - mat[1][1] * det3_235_034 + mat[1][3] * det3_235_014 - mat[1][4] * det3_235_013;
|
|
float det4_1235_0135 = mat[1][0] * det3_235_135 - mat[1][1] * det3_235_035 + mat[1][3] * det3_235_015 - mat[1][5] * det3_235_013;
|
|
float det4_1235_0145 = mat[1][0] * det3_235_145 - mat[1][1] * det3_235_045 + mat[1][4] * det3_235_015 - mat[1][5] * det3_235_014;
|
|
float det4_1235_0234 = mat[1][0] * det3_235_234 - mat[1][2] * det3_235_034 + mat[1][3] * det3_235_024 - mat[1][4] * det3_235_023;
|
|
float det4_1235_0235 = mat[1][0] * det3_235_235 - mat[1][2] * det3_235_035 + mat[1][3] * det3_235_025 - mat[1][5] * det3_235_023;
|
|
float det4_1235_0245 = mat[1][0] * det3_235_245 - mat[1][2] * det3_235_045 + mat[1][4] * det3_235_025 - mat[1][5] * det3_235_024;
|
|
float det4_1235_0345 = mat[1][0] * det3_235_345 - mat[1][3] * det3_235_045 + mat[1][4] * det3_235_035 - mat[1][5] * det3_235_034;
|
|
float det4_1235_1234 = mat[1][1] * det3_235_234 - mat[1][2] * det3_235_134 + mat[1][3] * det3_235_124 - mat[1][4] * det3_235_123;
|
|
float det4_1235_1235 = mat[1][1] * det3_235_235 - mat[1][2] * det3_235_135 + mat[1][3] * det3_235_125 - mat[1][5] * det3_235_123;
|
|
float det4_1235_1245 = mat[1][1] * det3_235_245 - mat[1][2] * det3_235_145 + mat[1][4] * det3_235_125 - mat[1][5] * det3_235_124;
|
|
float det4_1235_1345 = mat[1][1] * det3_235_345 - mat[1][3] * det3_235_145 + mat[1][4] * det3_235_135 - mat[1][5] * det3_235_134;
|
|
float det4_1235_2345 = mat[1][2] * det3_235_345 - mat[1][3] * det3_235_245 + mat[1][4] * det3_235_235 - mat[1][5] * det3_235_234;
|
|
float det4_1245_0123 = mat[1][0] * det3_245_123 - mat[1][1] * det3_245_023 + mat[1][2] * det3_245_013 - mat[1][3] * det3_245_012;
|
|
float det4_1245_0124 = mat[1][0] * det3_245_124 - mat[1][1] * det3_245_024 + mat[1][2] * det3_245_014 - mat[1][4] * det3_245_012;
|
|
float det4_1245_0125 = mat[1][0] * det3_245_125 - mat[1][1] * det3_245_025 + mat[1][2] * det3_245_015 - mat[1][5] * det3_245_012;
|
|
float det4_1245_0134 = mat[1][0] * det3_245_134 - mat[1][1] * det3_245_034 + mat[1][3] * det3_245_014 - mat[1][4] * det3_245_013;
|
|
float det4_1245_0135 = mat[1][0] * det3_245_135 - mat[1][1] * det3_245_035 + mat[1][3] * det3_245_015 - mat[1][5] * det3_245_013;
|
|
float det4_1245_0145 = mat[1][0] * det3_245_145 - mat[1][1] * det3_245_045 + mat[1][4] * det3_245_015 - mat[1][5] * det3_245_014;
|
|
float det4_1245_0234 = mat[1][0] * det3_245_234 - mat[1][2] * det3_245_034 + mat[1][3] * det3_245_024 - mat[1][4] * det3_245_023;
|
|
float det4_1245_0235 = mat[1][0] * det3_245_235 - mat[1][2] * det3_245_035 + mat[1][3] * det3_245_025 - mat[1][5] * det3_245_023;
|
|
float det4_1245_0245 = mat[1][0] * det3_245_245 - mat[1][2] * det3_245_045 + mat[1][4] * det3_245_025 - mat[1][5] * det3_245_024;
|
|
float det4_1245_0345 = mat[1][0] * det3_245_345 - mat[1][3] * det3_245_045 + mat[1][4] * det3_245_035 - mat[1][5] * det3_245_034;
|
|
float det4_1245_1234 = mat[1][1] * det3_245_234 - mat[1][2] * det3_245_134 + mat[1][3] * det3_245_124 - mat[1][4] * det3_245_123;
|
|
float det4_1245_1235 = mat[1][1] * det3_245_235 - mat[1][2] * det3_245_135 + mat[1][3] * det3_245_125 - mat[1][5] * det3_245_123;
|
|
float det4_1245_1245 = mat[1][1] * det3_245_245 - mat[1][2] * det3_245_145 + mat[1][4] * det3_245_125 - mat[1][5] * det3_245_124;
|
|
float det4_1245_1345 = mat[1][1] * det3_245_345 - mat[1][3] * det3_245_145 + mat[1][4] * det3_245_135 - mat[1][5] * det3_245_134;
|
|
float det4_1245_2345 = mat[1][2] * det3_245_345 - mat[1][3] * det3_245_245 + mat[1][4] * det3_245_235 - mat[1][5] * det3_245_234;
|
|
float det4_1345_0123 = mat[1][0] * det3_345_123 - mat[1][1] * det3_345_023 + mat[1][2] * det3_345_013 - mat[1][3] * det3_345_012;
|
|
float det4_1345_0124 = mat[1][0] * det3_345_124 - mat[1][1] * det3_345_024 + mat[1][2] * det3_345_014 - mat[1][4] * det3_345_012;
|
|
float det4_1345_0125 = mat[1][0] * det3_345_125 - mat[1][1] * det3_345_025 + mat[1][2] * det3_345_015 - mat[1][5] * det3_345_012;
|
|
float det4_1345_0134 = mat[1][0] * det3_345_134 - mat[1][1] * det3_345_034 + mat[1][3] * det3_345_014 - mat[1][4] * det3_345_013;
|
|
float det4_1345_0135 = mat[1][0] * det3_345_135 - mat[1][1] * det3_345_035 + mat[1][3] * det3_345_015 - mat[1][5] * det3_345_013;
|
|
float det4_1345_0145 = mat[1][0] * det3_345_145 - mat[1][1] * det3_345_045 + mat[1][4] * det3_345_015 - mat[1][5] * det3_345_014;
|
|
float det4_1345_0234 = mat[1][0] * det3_345_234 - mat[1][2] * det3_345_034 + mat[1][3] * det3_345_024 - mat[1][4] * det3_345_023;
|
|
float det4_1345_0235 = mat[1][0] * det3_345_235 - mat[1][2] * det3_345_035 + mat[1][3] * det3_345_025 - mat[1][5] * det3_345_023;
|
|
float det4_1345_0245 = mat[1][0] * det3_345_245 - mat[1][2] * det3_345_045 + mat[1][4] * det3_345_025 - mat[1][5] * det3_345_024;
|
|
float det4_1345_0345 = mat[1][0] * det3_345_345 - mat[1][3] * det3_345_045 + mat[1][4] * det3_345_035 - mat[1][5] * det3_345_034;
|
|
float det4_1345_1234 = mat[1][1] * det3_345_234 - mat[1][2] * det3_345_134 + mat[1][3] * det3_345_124 - mat[1][4] * det3_345_123;
|
|
float det4_1345_1235 = mat[1][1] * det3_345_235 - mat[1][2] * det3_345_135 + mat[1][3] * det3_345_125 - mat[1][5] * det3_345_123;
|
|
float det4_1345_1245 = mat[1][1] * det3_345_245 - mat[1][2] * det3_345_145 + mat[1][4] * det3_345_125 - mat[1][5] * det3_345_124;
|
|
float det4_1345_1345 = mat[1][1] * det3_345_345 - mat[1][3] * det3_345_145 + mat[1][4] * det3_345_135 - mat[1][5] * det3_345_134;
|
|
float det4_1345_2345 = mat[1][2] * det3_345_345 - mat[1][3] * det3_345_245 + mat[1][4] * det3_345_235 - mat[1][5] * det3_345_234;
|
|
|
|
// remaining 5x5 sub-determinants
|
|
float det5_01234_01234 = mat[0][0] * det4_1234_1234 - mat[0][1] * det4_1234_0234 + mat[0][2] * det4_1234_0134 - mat[0][3] * det4_1234_0124 + mat[0][4] * det4_1234_0123;
|
|
float det5_01234_01235 = mat[0][0] * det4_1234_1235 - mat[0][1] * det4_1234_0235 + mat[0][2] * det4_1234_0135 - mat[0][3] * det4_1234_0125 + mat[0][5] * det4_1234_0123;
|
|
float det5_01234_01245 = mat[0][0] * det4_1234_1245 - mat[0][1] * det4_1234_0245 + mat[0][2] * det4_1234_0145 - mat[0][4] * det4_1234_0125 + mat[0][5] * det4_1234_0124;
|
|
float det5_01234_01345 = mat[0][0] * det4_1234_1345 - mat[0][1] * det4_1234_0345 + mat[0][3] * det4_1234_0145 - mat[0][4] * det4_1234_0135 + mat[0][5] * det4_1234_0134;
|
|
float det5_01234_02345 = mat[0][0] * det4_1234_2345 - mat[0][2] * det4_1234_0345 + mat[0][3] * det4_1234_0245 - mat[0][4] * det4_1234_0235 + mat[0][5] * det4_1234_0234;
|
|
float det5_01234_12345 = mat[0][1] * det4_1234_2345 - mat[0][2] * det4_1234_1345 + mat[0][3] * det4_1234_1245 - mat[0][4] * det4_1234_1235 + mat[0][5] * det4_1234_1234;
|
|
float det5_01235_01234 = mat[0][0] * det4_1235_1234 - mat[0][1] * det4_1235_0234 + mat[0][2] * det4_1235_0134 - mat[0][3] * det4_1235_0124 + mat[0][4] * det4_1235_0123;
|
|
float det5_01235_01235 = mat[0][0] * det4_1235_1235 - mat[0][1] * det4_1235_0235 + mat[0][2] * det4_1235_0135 - mat[0][3] * det4_1235_0125 + mat[0][5] * det4_1235_0123;
|
|
float det5_01235_01245 = mat[0][0] * det4_1235_1245 - mat[0][1] * det4_1235_0245 + mat[0][2] * det4_1235_0145 - mat[0][4] * det4_1235_0125 + mat[0][5] * det4_1235_0124;
|
|
float det5_01235_01345 = mat[0][0] * det4_1235_1345 - mat[0][1] * det4_1235_0345 + mat[0][3] * det4_1235_0145 - mat[0][4] * det4_1235_0135 + mat[0][5] * det4_1235_0134;
|
|
float det5_01235_02345 = mat[0][0] * det4_1235_2345 - mat[0][2] * det4_1235_0345 + mat[0][3] * det4_1235_0245 - mat[0][4] * det4_1235_0235 + mat[0][5] * det4_1235_0234;
|
|
float det5_01235_12345 = mat[0][1] * det4_1235_2345 - mat[0][2] * det4_1235_1345 + mat[0][3] * det4_1235_1245 - mat[0][4] * det4_1235_1235 + mat[0][5] * det4_1235_1234;
|
|
float det5_01245_01234 = mat[0][0] * det4_1245_1234 - mat[0][1] * det4_1245_0234 + mat[0][2] * det4_1245_0134 - mat[0][3] * det4_1245_0124 + mat[0][4] * det4_1245_0123;
|
|
float det5_01245_01235 = mat[0][0] * det4_1245_1235 - mat[0][1] * det4_1245_0235 + mat[0][2] * det4_1245_0135 - mat[0][3] * det4_1245_0125 + mat[0][5] * det4_1245_0123;
|
|
float det5_01245_01245 = mat[0][0] * det4_1245_1245 - mat[0][1] * det4_1245_0245 + mat[0][2] * det4_1245_0145 - mat[0][4] * det4_1245_0125 + mat[0][5] * det4_1245_0124;
|
|
float det5_01245_01345 = mat[0][0] * det4_1245_1345 - mat[0][1] * det4_1245_0345 + mat[0][3] * det4_1245_0145 - mat[0][4] * det4_1245_0135 + mat[0][5] * det4_1245_0134;
|
|
float det5_01245_02345 = mat[0][0] * det4_1245_2345 - mat[0][2] * det4_1245_0345 + mat[0][3] * det4_1245_0245 - mat[0][4] * det4_1245_0235 + mat[0][5] * det4_1245_0234;
|
|
float det5_01245_12345 = mat[0][1] * det4_1245_2345 - mat[0][2] * det4_1245_1345 + mat[0][3] * det4_1245_1245 - mat[0][4] * det4_1245_1235 + mat[0][5] * det4_1245_1234;
|
|
float det5_01345_01234 = mat[0][0] * det4_1345_1234 - mat[0][1] * det4_1345_0234 + mat[0][2] * det4_1345_0134 - mat[0][3] * det4_1345_0124 + mat[0][4] * det4_1345_0123;
|
|
float det5_01345_01235 = mat[0][0] * det4_1345_1235 - mat[0][1] * det4_1345_0235 + mat[0][2] * det4_1345_0135 - mat[0][3] * det4_1345_0125 + mat[0][5] * det4_1345_0123;
|
|
float det5_01345_01245 = mat[0][0] * det4_1345_1245 - mat[0][1] * det4_1345_0245 + mat[0][2] * det4_1345_0145 - mat[0][4] * det4_1345_0125 + mat[0][5] * det4_1345_0124;
|
|
float det5_01345_01345 = mat[0][0] * det4_1345_1345 - mat[0][1] * det4_1345_0345 + mat[0][3] * det4_1345_0145 - mat[0][4] * det4_1345_0135 + mat[0][5] * det4_1345_0134;
|
|
float det5_01345_02345 = mat[0][0] * det4_1345_2345 - mat[0][2] * det4_1345_0345 + mat[0][3] * det4_1345_0245 - mat[0][4] * det4_1345_0235 + mat[0][5] * det4_1345_0234;
|
|
float det5_01345_12345 = mat[0][1] * det4_1345_2345 - mat[0][2] * det4_1345_1345 + mat[0][3] * det4_1345_1245 - mat[0][4] * det4_1345_1235 + mat[0][5] * det4_1345_1234;
|
|
float det5_02345_01234 = mat[0][0] * det4_2345_1234 - mat[0][1] * det4_2345_0234 + mat[0][2] * det4_2345_0134 - mat[0][3] * det4_2345_0124 + mat[0][4] * det4_2345_0123;
|
|
float det5_02345_01235 = mat[0][0] * det4_2345_1235 - mat[0][1] * det4_2345_0235 + mat[0][2] * det4_2345_0135 - mat[0][3] * det4_2345_0125 + mat[0][5] * det4_2345_0123;
|
|
float det5_02345_01245 = mat[0][0] * det4_2345_1245 - mat[0][1] * det4_2345_0245 + mat[0][2] * det4_2345_0145 - mat[0][4] * det4_2345_0125 + mat[0][5] * det4_2345_0124;
|
|
float det5_02345_01345 = mat[0][0] * det4_2345_1345 - mat[0][1] * det4_2345_0345 + mat[0][3] * det4_2345_0145 - mat[0][4] * det4_2345_0135 + mat[0][5] * det4_2345_0134;
|
|
float det5_02345_02345 = mat[0][0] * det4_2345_2345 - mat[0][2] * det4_2345_0345 + mat[0][3] * det4_2345_0245 - mat[0][4] * det4_2345_0235 + mat[0][5] * det4_2345_0234;
|
|
float det5_02345_12345 = mat[0][1] * det4_2345_2345 - mat[0][2] * det4_2345_1345 + mat[0][3] * det4_2345_1245 - mat[0][4] * det4_2345_1235 + mat[0][5] * det4_2345_1234;
|
|
|
|
mat[0][0] = det5_12345_12345 * invDet;
|
|
mat[0][1] = -det5_02345_12345 * invDet;
|
|
mat[0][2] = det5_01345_12345 * invDet;
|
|
mat[0][3] = -det5_01245_12345 * invDet;
|
|
mat[0][4] = det5_01235_12345 * invDet;
|
|
mat[0][5] = -det5_01234_12345 * invDet;
|
|
|
|
mat[1][0] = -det5_12345_02345 * invDet;
|
|
mat[1][1] = det5_02345_02345 * invDet;
|
|
mat[1][2] = -det5_01345_02345 * invDet;
|
|
mat[1][3] = det5_01245_02345 * invDet;
|
|
mat[1][4] = -det5_01235_02345 * invDet;
|
|
mat[1][5] = det5_01234_02345 * invDet;
|
|
|
|
mat[2][0] = det5_12345_01345 * invDet;
|
|
mat[2][1] = -det5_02345_01345 * invDet;
|
|
mat[2][2] = det5_01345_01345 * invDet;
|
|
mat[2][3] = -det5_01245_01345 * invDet;
|
|
mat[2][4] = det5_01235_01345 * invDet;
|
|
mat[2][5] = -det5_01234_01345 * invDet;
|
|
|
|
mat[3][0] = -det5_12345_01245 * invDet;
|
|
mat[3][1] = det5_02345_01245 * invDet;
|
|
mat[3][2] = -det5_01345_01245 * invDet;
|
|
mat[3][3] = det5_01245_01245 * invDet;
|
|
mat[3][4] = -det5_01235_01245 * invDet;
|
|
mat[3][5] = det5_01234_01245 * invDet;
|
|
|
|
mat[4][0] = det5_12345_01235 * invDet;
|
|
mat[4][1] = -det5_02345_01235 * invDet;
|
|
mat[4][2] = det5_01345_01235 * invDet;
|
|
mat[4][3] = -det5_01245_01235 * invDet;
|
|
mat[4][4] = det5_01235_01235 * invDet;
|
|
mat[4][5] = -det5_01234_01235 * invDet;
|
|
|
|
mat[5][0] = -det5_12345_01234 * invDet;
|
|
mat[5][1] = det5_02345_01234 * invDet;
|
|
mat[5][2] = -det5_01345_01234 * invDet;
|
|
mat[5][3] = det5_01245_01234 * invDet;
|
|
mat[5][4] = -det5_01235_01234 * invDet;
|
|
mat[5][5] = det5_01234_01234 * invDet;
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMat6::InverseFastSelf
|
|
============
|
|
*/
|
|
bool idMat6::InverseFastSelf(void) {
|
|
#if 0
|
|
// 810+6+36 = 852 multiplications
|
|
// 1 division
|
|
double det, invDet;
|
|
|
|
// 2x2 sub-determinants required to calculate 6x6 determinant
|
|
float det2_45_01 = mat[4][0] * mat[5][1] - mat[4][1] * mat[5][0];
|
|
float det2_45_02 = mat[4][0] * mat[5][2] - mat[4][2] * mat[5][0];
|
|
float det2_45_03 = mat[4][0] * mat[5][3] - mat[4][3] * mat[5][0];
|
|
float det2_45_04 = mat[4][0] * mat[5][4] - mat[4][4] * mat[5][0];
|
|
float det2_45_05 = mat[4][0] * mat[5][5] - mat[4][5] * mat[5][0];
|
|
float det2_45_12 = mat[4][1] * mat[5][2] - mat[4][2] * mat[5][1];
|
|
float det2_45_13 = mat[4][1] * mat[5][3] - mat[4][3] * mat[5][1];
|
|
float det2_45_14 = mat[4][1] * mat[5][4] - mat[4][4] * mat[5][1];
|
|
float det2_45_15 = mat[4][1] * mat[5][5] - mat[4][5] * mat[5][1];
|
|
float det2_45_23 = mat[4][2] * mat[5][3] - mat[4][3] * mat[5][2];
|
|
float det2_45_24 = mat[4][2] * mat[5][4] - mat[4][4] * mat[5][2];
|
|
float det2_45_25 = mat[4][2] * mat[5][5] - mat[4][5] * mat[5][2];
|
|
float det2_45_34 = mat[4][3] * mat[5][4] - mat[4][4] * mat[5][3];
|
|
float det2_45_35 = mat[4][3] * mat[5][5] - mat[4][5] * mat[5][3];
|
|
float det2_45_45 = mat[4][4] * mat[5][5] - mat[4][5] * mat[5][4];
|
|
|
|
// 3x3 sub-determinants required to calculate 6x6 determinant
|
|
float det3_345_012 = mat[3][0] * det2_45_12 - mat[3][1] * det2_45_02 + mat[3][2] * det2_45_01;
|
|
float det3_345_013 = mat[3][0] * det2_45_13 - mat[3][1] * det2_45_03 + mat[3][3] * det2_45_01;
|
|
float det3_345_014 = mat[3][0] * det2_45_14 - mat[3][1] * det2_45_04 + mat[3][4] * det2_45_01;
|
|
float det3_345_015 = mat[3][0] * det2_45_15 - mat[3][1] * det2_45_05 + mat[3][5] * det2_45_01;
|
|
float det3_345_023 = mat[3][0] * det2_45_23 - mat[3][2] * det2_45_03 + mat[3][3] * det2_45_02;
|
|
float det3_345_024 = mat[3][0] * det2_45_24 - mat[3][2] * det2_45_04 + mat[3][4] * det2_45_02;
|
|
float det3_345_025 = mat[3][0] * det2_45_25 - mat[3][2] * det2_45_05 + mat[3][5] * det2_45_02;
|
|
float det3_345_034 = mat[3][0] * det2_45_34 - mat[3][3] * det2_45_04 + mat[3][4] * det2_45_03;
|
|
float det3_345_035 = mat[3][0] * det2_45_35 - mat[3][3] * det2_45_05 + mat[3][5] * det2_45_03;
|
|
float det3_345_045 = mat[3][0] * det2_45_45 - mat[3][4] * det2_45_05 + mat[3][5] * det2_45_04;
|
|
float det3_345_123 = mat[3][1] * det2_45_23 - mat[3][2] * det2_45_13 + mat[3][3] * det2_45_12;
|
|
float det3_345_124 = mat[3][1] * det2_45_24 - mat[3][2] * det2_45_14 + mat[3][4] * det2_45_12;
|
|
float det3_345_125 = mat[3][1] * det2_45_25 - mat[3][2] * det2_45_15 + mat[3][5] * det2_45_12;
|
|
float det3_345_134 = mat[3][1] * det2_45_34 - mat[3][3] * det2_45_14 + mat[3][4] * det2_45_13;
|
|
float det3_345_135 = mat[3][1] * det2_45_35 - mat[3][3] * det2_45_15 + mat[3][5] * det2_45_13;
|
|
float det3_345_145 = mat[3][1] * det2_45_45 - mat[3][4] * det2_45_15 + mat[3][5] * det2_45_14;
|
|
float det3_345_234 = mat[3][2] * det2_45_34 - mat[3][3] * det2_45_24 + mat[3][4] * det2_45_23;
|
|
float det3_345_235 = mat[3][2] * det2_45_35 - mat[3][3] * det2_45_25 + mat[3][5] * det2_45_23;
|
|
float det3_345_245 = mat[3][2] * det2_45_45 - mat[3][4] * det2_45_25 + mat[3][5] * det2_45_24;
|
|
float det3_345_345 = mat[3][3] * det2_45_45 - mat[3][4] * det2_45_35 + mat[3][5] * det2_45_34;
|
|
|
|
// 4x4 sub-determinants required to calculate 6x6 determinant
|
|
float det4_2345_0123 = mat[2][0] * det3_345_123 - mat[2][1] * det3_345_023 + mat[2][2] * det3_345_013 - mat[2][3] * det3_345_012;
|
|
float det4_2345_0124 = mat[2][0] * det3_345_124 - mat[2][1] * det3_345_024 + mat[2][2] * det3_345_014 - mat[2][4] * det3_345_012;
|
|
float det4_2345_0125 = mat[2][0] * det3_345_125 - mat[2][1] * det3_345_025 + mat[2][2] * det3_345_015 - mat[2][5] * det3_345_012;
|
|
float det4_2345_0134 = mat[2][0] * det3_345_134 - mat[2][1] * det3_345_034 + mat[2][3] * det3_345_014 - mat[2][4] * det3_345_013;
|
|
float det4_2345_0135 = mat[2][0] * det3_345_135 - mat[2][1] * det3_345_035 + mat[2][3] * det3_345_015 - mat[2][5] * det3_345_013;
|
|
float det4_2345_0145 = mat[2][0] * det3_345_145 - mat[2][1] * det3_345_045 + mat[2][4] * det3_345_015 - mat[2][5] * det3_345_014;
|
|
float det4_2345_0234 = mat[2][0] * det3_345_234 - mat[2][2] * det3_345_034 + mat[2][3] * det3_345_024 - mat[2][4] * det3_345_023;
|
|
float det4_2345_0235 = mat[2][0] * det3_345_235 - mat[2][2] * det3_345_035 + mat[2][3] * det3_345_025 - mat[2][5] * det3_345_023;
|
|
float det4_2345_0245 = mat[2][0] * det3_345_245 - mat[2][2] * det3_345_045 + mat[2][4] * det3_345_025 - mat[2][5] * det3_345_024;
|
|
float det4_2345_0345 = mat[2][0] * det3_345_345 - mat[2][3] * det3_345_045 + mat[2][4] * det3_345_035 - mat[2][5] * det3_345_034;
|
|
float det4_2345_1234 = mat[2][1] * det3_345_234 - mat[2][2] * det3_345_134 + mat[2][3] * det3_345_124 - mat[2][4] * det3_345_123;
|
|
float det4_2345_1235 = mat[2][1] * det3_345_235 - mat[2][2] * det3_345_135 + mat[2][3] * det3_345_125 - mat[2][5] * det3_345_123;
|
|
float det4_2345_1245 = mat[2][1] * det3_345_245 - mat[2][2] * det3_345_145 + mat[2][4] * det3_345_125 - mat[2][5] * det3_345_124;
|
|
float det4_2345_1345 = mat[2][1] * det3_345_345 - mat[2][3] * det3_345_145 + mat[2][4] * det3_345_135 - mat[2][5] * det3_345_134;
|
|
float det4_2345_2345 = mat[2][2] * det3_345_345 - mat[2][3] * det3_345_245 + mat[2][4] * det3_345_235 - mat[2][5] * det3_345_234;
|
|
|
|
// 5x5 sub-determinants required to calculate 6x6 determinant
|
|
float det5_12345_01234 = mat[1][0] * det4_2345_1234 - mat[1][1] * det4_2345_0234 + mat[1][2] * det4_2345_0134 - mat[1][3] * det4_2345_0124 + mat[1][4] * det4_2345_0123;
|
|
float det5_12345_01235 = mat[1][0] * det4_2345_1235 - mat[1][1] * det4_2345_0235 + mat[1][2] * det4_2345_0135 - mat[1][3] * det4_2345_0125 + mat[1][5] * det4_2345_0123;
|
|
float det5_12345_01245 = mat[1][0] * det4_2345_1245 - mat[1][1] * det4_2345_0245 + mat[1][2] * det4_2345_0145 - mat[1][4] * det4_2345_0125 + mat[1][5] * det4_2345_0124;
|
|
float det5_12345_01345 = mat[1][0] * det4_2345_1345 - mat[1][1] * det4_2345_0345 + mat[1][3] * det4_2345_0145 - mat[1][4] * det4_2345_0135 + mat[1][5] * det4_2345_0134;
|
|
float det5_12345_02345 = mat[1][0] * det4_2345_2345 - mat[1][2] * det4_2345_0345 + mat[1][3] * det4_2345_0245 - mat[1][4] * det4_2345_0235 + mat[1][5] * det4_2345_0234;
|
|
float det5_12345_12345 = mat[1][1] * det4_2345_2345 - mat[1][2] * det4_2345_1345 + mat[1][3] * det4_2345_1245 - mat[1][4] * det4_2345_1235 + mat[1][5] * det4_2345_1234;
|
|
|
|
// determinant of 6x6 matrix
|
|
det = mat[0][0] * det5_12345_12345 - mat[0][1] * det5_12345_02345 + mat[0][2] * det5_12345_01345 -
|
|
mat[0][3] * det5_12345_01245 + mat[0][4] * det5_12345_01235 - mat[0][5] * det5_12345_01234;
|
|
|
|
if (idMath::Fabs(det) < MATRIX_INVERSE_EPSILON) {
|
|
return false;
|
|
}
|
|
|
|
invDet = 1.0f / det;
|
|
|
|
// remaining 2x2 sub-determinants
|
|
float det2_34_01 = mat[3][0] * mat[4][1] - mat[3][1] * mat[4][0];
|
|
float det2_34_02 = mat[3][0] * mat[4][2] - mat[3][2] * mat[4][0];
|
|
float det2_34_03 = mat[3][0] * mat[4][3] - mat[3][3] * mat[4][0];
|
|
float det2_34_04 = mat[3][0] * mat[4][4] - mat[3][4] * mat[4][0];
|
|
float det2_34_05 = mat[3][0] * mat[4][5] - mat[3][5] * mat[4][0];
|
|
float det2_34_12 = mat[3][1] * mat[4][2] - mat[3][2] * mat[4][1];
|
|
float det2_34_13 = mat[3][1] * mat[4][3] - mat[3][3] * mat[4][1];
|
|
float det2_34_14 = mat[3][1] * mat[4][4] - mat[3][4] * mat[4][1];
|
|
float det2_34_15 = mat[3][1] * mat[4][5] - mat[3][5] * mat[4][1];
|
|
float det2_34_23 = mat[3][2] * mat[4][3] - mat[3][3] * mat[4][2];
|
|
float det2_34_24 = mat[3][2] * mat[4][4] - mat[3][4] * mat[4][2];
|
|
float det2_34_25 = mat[3][2] * mat[4][5] - mat[3][5] * mat[4][2];
|
|
float det2_34_34 = mat[3][3] * mat[4][4] - mat[3][4] * mat[4][3];
|
|
float det2_34_35 = mat[3][3] * mat[4][5] - mat[3][5] * mat[4][3];
|
|
float det2_34_45 = mat[3][4] * mat[4][5] - mat[3][5] * mat[4][4];
|
|
float det2_35_01 = mat[3][0] * mat[5][1] - mat[3][1] * mat[5][0];
|
|
float det2_35_02 = mat[3][0] * mat[5][2] - mat[3][2] * mat[5][0];
|
|
float det2_35_03 = mat[3][0] * mat[5][3] - mat[3][3] * mat[5][0];
|
|
float det2_35_04 = mat[3][0] * mat[5][4] - mat[3][4] * mat[5][0];
|
|
float det2_35_05 = mat[3][0] * mat[5][5] - mat[3][5] * mat[5][0];
|
|
float det2_35_12 = mat[3][1] * mat[5][2] - mat[3][2] * mat[5][1];
|
|
float det2_35_13 = mat[3][1] * mat[5][3] - mat[3][3] * mat[5][1];
|
|
float det2_35_14 = mat[3][1] * mat[5][4] - mat[3][4] * mat[5][1];
|
|
float det2_35_15 = mat[3][1] * mat[5][5] - mat[3][5] * mat[5][1];
|
|
float det2_35_23 = mat[3][2] * mat[5][3] - mat[3][3] * mat[5][2];
|
|
float det2_35_24 = mat[3][2] * mat[5][4] - mat[3][4] * mat[5][2];
|
|
float det2_35_25 = mat[3][2] * mat[5][5] - mat[3][5] * mat[5][2];
|
|
float det2_35_34 = mat[3][3] * mat[5][4] - mat[3][4] * mat[5][3];
|
|
float det2_35_35 = mat[3][3] * mat[5][5] - mat[3][5] * mat[5][3];
|
|
float det2_35_45 = mat[3][4] * mat[5][5] - mat[3][5] * mat[5][4];
|
|
|
|
// remaining 3x3 sub-determinants
|
|
float det3_234_012 = mat[2][0] * det2_34_12 - mat[2][1] * det2_34_02 + mat[2][2] * det2_34_01;
|
|
float det3_234_013 = mat[2][0] * det2_34_13 - mat[2][1] * det2_34_03 + mat[2][3] * det2_34_01;
|
|
float det3_234_014 = mat[2][0] * det2_34_14 - mat[2][1] * det2_34_04 + mat[2][4] * det2_34_01;
|
|
float det3_234_015 = mat[2][0] * det2_34_15 - mat[2][1] * det2_34_05 + mat[2][5] * det2_34_01;
|
|
float det3_234_023 = mat[2][0] * det2_34_23 - mat[2][2] * det2_34_03 + mat[2][3] * det2_34_02;
|
|
float det3_234_024 = mat[2][0] * det2_34_24 - mat[2][2] * det2_34_04 + mat[2][4] * det2_34_02;
|
|
float det3_234_025 = mat[2][0] * det2_34_25 - mat[2][2] * det2_34_05 + mat[2][5] * det2_34_02;
|
|
float det3_234_034 = mat[2][0] * det2_34_34 - mat[2][3] * det2_34_04 + mat[2][4] * det2_34_03;
|
|
float det3_234_035 = mat[2][0] * det2_34_35 - mat[2][3] * det2_34_05 + mat[2][5] * det2_34_03;
|
|
float det3_234_045 = mat[2][0] * det2_34_45 - mat[2][4] * det2_34_05 + mat[2][5] * det2_34_04;
|
|
float det3_234_123 = mat[2][1] * det2_34_23 - mat[2][2] * det2_34_13 + mat[2][3] * det2_34_12;
|
|
float det3_234_124 = mat[2][1] * det2_34_24 - mat[2][2] * det2_34_14 + mat[2][4] * det2_34_12;
|
|
float det3_234_125 = mat[2][1] * det2_34_25 - mat[2][2] * det2_34_15 + mat[2][5] * det2_34_12;
|
|
float det3_234_134 = mat[2][1] * det2_34_34 - mat[2][3] * det2_34_14 + mat[2][4] * det2_34_13;
|
|
float det3_234_135 = mat[2][1] * det2_34_35 - mat[2][3] * det2_34_15 + mat[2][5] * det2_34_13;
|
|
float det3_234_145 = mat[2][1] * det2_34_45 - mat[2][4] * det2_34_15 + mat[2][5] * det2_34_14;
|
|
float det3_234_234 = mat[2][2] * det2_34_34 - mat[2][3] * det2_34_24 + mat[2][4] * det2_34_23;
|
|
float det3_234_235 = mat[2][2] * det2_34_35 - mat[2][3] * det2_34_25 + mat[2][5] * det2_34_23;
|
|
float det3_234_245 = mat[2][2] * det2_34_45 - mat[2][4] * det2_34_25 + mat[2][5] * det2_34_24;
|
|
float det3_234_345 = mat[2][3] * det2_34_45 - mat[2][4] * det2_34_35 + mat[2][5] * det2_34_34;
|
|
float det3_235_012 = mat[2][0] * det2_35_12 - mat[2][1] * det2_35_02 + mat[2][2] * det2_35_01;
|
|
float det3_235_013 = mat[2][0] * det2_35_13 - mat[2][1] * det2_35_03 + mat[2][3] * det2_35_01;
|
|
float det3_235_014 = mat[2][0] * det2_35_14 - mat[2][1] * det2_35_04 + mat[2][4] * det2_35_01;
|
|
float det3_235_015 = mat[2][0] * det2_35_15 - mat[2][1] * det2_35_05 + mat[2][5] * det2_35_01;
|
|
float det3_235_023 = mat[2][0] * det2_35_23 - mat[2][2] * det2_35_03 + mat[2][3] * det2_35_02;
|
|
float det3_235_024 = mat[2][0] * det2_35_24 - mat[2][2] * det2_35_04 + mat[2][4] * det2_35_02;
|
|
float det3_235_025 = mat[2][0] * det2_35_25 - mat[2][2] * det2_35_05 + mat[2][5] * det2_35_02;
|
|
float det3_235_034 = mat[2][0] * det2_35_34 - mat[2][3] * det2_35_04 + mat[2][4] * det2_35_03;
|
|
float det3_235_035 = mat[2][0] * det2_35_35 - mat[2][3] * det2_35_05 + mat[2][5] * det2_35_03;
|
|
float det3_235_045 = mat[2][0] * det2_35_45 - mat[2][4] * det2_35_05 + mat[2][5] * det2_35_04;
|
|
float det3_235_123 = mat[2][1] * det2_35_23 - mat[2][2] * det2_35_13 + mat[2][3] * det2_35_12;
|
|
float det3_235_124 = mat[2][1] * det2_35_24 - mat[2][2] * det2_35_14 + mat[2][4] * det2_35_12;
|
|
float det3_235_125 = mat[2][1] * det2_35_25 - mat[2][2] * det2_35_15 + mat[2][5] * det2_35_12;
|
|
float det3_235_134 = mat[2][1] * det2_35_34 - mat[2][3] * det2_35_14 + mat[2][4] * det2_35_13;
|
|
float det3_235_135 = mat[2][1] * det2_35_35 - mat[2][3] * det2_35_15 + mat[2][5] * det2_35_13;
|
|
float det3_235_145 = mat[2][1] * det2_35_45 - mat[2][4] * det2_35_15 + mat[2][5] * det2_35_14;
|
|
float det3_235_234 = mat[2][2] * det2_35_34 - mat[2][3] * det2_35_24 + mat[2][4] * det2_35_23;
|
|
float det3_235_235 = mat[2][2] * det2_35_35 - mat[2][3] * det2_35_25 + mat[2][5] * det2_35_23;
|
|
float det3_235_245 = mat[2][2] * det2_35_45 - mat[2][4] * det2_35_25 + mat[2][5] * det2_35_24;
|
|
float det3_235_345 = mat[2][3] * det2_35_45 - mat[2][4] * det2_35_35 + mat[2][5] * det2_35_34;
|
|
float det3_245_012 = mat[2][0] * det2_45_12 - mat[2][1] * det2_45_02 + mat[2][2] * det2_45_01;
|
|
float det3_245_013 = mat[2][0] * det2_45_13 - mat[2][1] * det2_45_03 + mat[2][3] * det2_45_01;
|
|
float det3_245_014 = mat[2][0] * det2_45_14 - mat[2][1] * det2_45_04 + mat[2][4] * det2_45_01;
|
|
float det3_245_015 = mat[2][0] * det2_45_15 - mat[2][1] * det2_45_05 + mat[2][5] * det2_45_01;
|
|
float det3_245_023 = mat[2][0] * det2_45_23 - mat[2][2] * det2_45_03 + mat[2][3] * det2_45_02;
|
|
float det3_245_024 = mat[2][0] * det2_45_24 - mat[2][2] * det2_45_04 + mat[2][4] * det2_45_02;
|
|
float det3_245_025 = mat[2][0] * det2_45_25 - mat[2][2] * det2_45_05 + mat[2][5] * det2_45_02;
|
|
float det3_245_034 = mat[2][0] * det2_45_34 - mat[2][3] * det2_45_04 + mat[2][4] * det2_45_03;
|
|
float det3_245_035 = mat[2][0] * det2_45_35 - mat[2][3] * det2_45_05 + mat[2][5] * det2_45_03;
|
|
float det3_245_045 = mat[2][0] * det2_45_45 - mat[2][4] * det2_45_05 + mat[2][5] * det2_45_04;
|
|
float det3_245_123 = mat[2][1] * det2_45_23 - mat[2][2] * det2_45_13 + mat[2][3] * det2_45_12;
|
|
float det3_245_124 = mat[2][1] * det2_45_24 - mat[2][2] * det2_45_14 + mat[2][4] * det2_45_12;
|
|
float det3_245_125 = mat[2][1] * det2_45_25 - mat[2][2] * det2_45_15 + mat[2][5] * det2_45_12;
|
|
float det3_245_134 = mat[2][1] * det2_45_34 - mat[2][3] * det2_45_14 + mat[2][4] * det2_45_13;
|
|
float det3_245_135 = mat[2][1] * det2_45_35 - mat[2][3] * det2_45_15 + mat[2][5] * det2_45_13;
|
|
float det3_245_145 = mat[2][1] * det2_45_45 - mat[2][4] * det2_45_15 + mat[2][5] * det2_45_14;
|
|
float det3_245_234 = mat[2][2] * det2_45_34 - mat[2][3] * det2_45_24 + mat[2][4] * det2_45_23;
|
|
float det3_245_235 = mat[2][2] * det2_45_35 - mat[2][3] * det2_45_25 + mat[2][5] * det2_45_23;
|
|
float det3_245_245 = mat[2][2] * det2_45_45 - mat[2][4] * det2_45_25 + mat[2][5] * det2_45_24;
|
|
float det3_245_345 = mat[2][3] * det2_45_45 - mat[2][4] * det2_45_35 + mat[2][5] * det2_45_34;
|
|
|
|
// remaining 4x4 sub-determinants
|
|
float det4_1234_0123 = mat[1][0] * det3_234_123 - mat[1][1] * det3_234_023 + mat[1][2] * det3_234_013 - mat[1][3] * det3_234_012;
|
|
float det4_1234_0124 = mat[1][0] * det3_234_124 - mat[1][1] * det3_234_024 + mat[1][2] * det3_234_014 - mat[1][4] * det3_234_012;
|
|
float det4_1234_0125 = mat[1][0] * det3_234_125 - mat[1][1] * det3_234_025 + mat[1][2] * det3_234_015 - mat[1][5] * det3_234_012;
|
|
float det4_1234_0134 = mat[1][0] * det3_234_134 - mat[1][1] * det3_234_034 + mat[1][3] * det3_234_014 - mat[1][4] * det3_234_013;
|
|
float det4_1234_0135 = mat[1][0] * det3_234_135 - mat[1][1] * det3_234_035 + mat[1][3] * det3_234_015 - mat[1][5] * det3_234_013;
|
|
float det4_1234_0145 = mat[1][0] * det3_234_145 - mat[1][1] * det3_234_045 + mat[1][4] * det3_234_015 - mat[1][5] * det3_234_014;
|
|
float det4_1234_0234 = mat[1][0] * det3_234_234 - mat[1][2] * det3_234_034 + mat[1][3] * det3_234_024 - mat[1][4] * det3_234_023;
|
|
float det4_1234_0235 = mat[1][0] * det3_234_235 - mat[1][2] * det3_234_035 + mat[1][3] * det3_234_025 - mat[1][5] * det3_234_023;
|
|
float det4_1234_0245 = mat[1][0] * det3_234_245 - mat[1][2] * det3_234_045 + mat[1][4] * det3_234_025 - mat[1][5] * det3_234_024;
|
|
float det4_1234_0345 = mat[1][0] * det3_234_345 - mat[1][3] * det3_234_045 + mat[1][4] * det3_234_035 - mat[1][5] * det3_234_034;
|
|
float det4_1234_1234 = mat[1][1] * det3_234_234 - mat[1][2] * det3_234_134 + mat[1][3] * det3_234_124 - mat[1][4] * det3_234_123;
|
|
float det4_1234_1235 = mat[1][1] * det3_234_235 - mat[1][2] * det3_234_135 + mat[1][3] * det3_234_125 - mat[1][5] * det3_234_123;
|
|
float det4_1234_1245 = mat[1][1] * det3_234_245 - mat[1][2] * det3_234_145 + mat[1][4] * det3_234_125 - mat[1][5] * det3_234_124;
|
|
float det4_1234_1345 = mat[1][1] * det3_234_345 - mat[1][3] * det3_234_145 + mat[1][4] * det3_234_135 - mat[1][5] * det3_234_134;
|
|
float det4_1234_2345 = mat[1][2] * det3_234_345 - mat[1][3] * det3_234_245 + mat[1][4] * det3_234_235 - mat[1][5] * det3_234_234;
|
|
float det4_1235_0123 = mat[1][0] * det3_235_123 - mat[1][1] * det3_235_023 + mat[1][2] * det3_235_013 - mat[1][3] * det3_235_012;
|
|
float det4_1235_0124 = mat[1][0] * det3_235_124 - mat[1][1] * det3_235_024 + mat[1][2] * det3_235_014 - mat[1][4] * det3_235_012;
|
|
float det4_1235_0125 = mat[1][0] * det3_235_125 - mat[1][1] * det3_235_025 + mat[1][2] * det3_235_015 - mat[1][5] * det3_235_012;
|
|
float det4_1235_0134 = mat[1][0] * det3_235_134 - mat[1][1] * det3_235_034 + mat[1][3] * det3_235_014 - mat[1][4] * det3_235_013;
|
|
float det4_1235_0135 = mat[1][0] * det3_235_135 - mat[1][1] * det3_235_035 + mat[1][3] * det3_235_015 - mat[1][5] * det3_235_013;
|
|
float det4_1235_0145 = mat[1][0] * det3_235_145 - mat[1][1] * det3_235_045 + mat[1][4] * det3_235_015 - mat[1][5] * det3_235_014;
|
|
float det4_1235_0234 = mat[1][0] * det3_235_234 - mat[1][2] * det3_235_034 + mat[1][3] * det3_235_024 - mat[1][4] * det3_235_023;
|
|
float det4_1235_0235 = mat[1][0] * det3_235_235 - mat[1][2] * det3_235_035 + mat[1][3] * det3_235_025 - mat[1][5] * det3_235_023;
|
|
float det4_1235_0245 = mat[1][0] * det3_235_245 - mat[1][2] * det3_235_045 + mat[1][4] * det3_235_025 - mat[1][5] * det3_235_024;
|
|
float det4_1235_0345 = mat[1][0] * det3_235_345 - mat[1][3] * det3_235_045 + mat[1][4] * det3_235_035 - mat[1][5] * det3_235_034;
|
|
float det4_1235_1234 = mat[1][1] * det3_235_234 - mat[1][2] * det3_235_134 + mat[1][3] * det3_235_124 - mat[1][4] * det3_235_123;
|
|
float det4_1235_1235 = mat[1][1] * det3_235_235 - mat[1][2] * det3_235_135 + mat[1][3] * det3_235_125 - mat[1][5] * det3_235_123;
|
|
float det4_1235_1245 = mat[1][1] * det3_235_245 - mat[1][2] * det3_235_145 + mat[1][4] * det3_235_125 - mat[1][5] * det3_235_124;
|
|
float det4_1235_1345 = mat[1][1] * det3_235_345 - mat[1][3] * det3_235_145 + mat[1][4] * det3_235_135 - mat[1][5] * det3_235_134;
|
|
float det4_1235_2345 = mat[1][2] * det3_235_345 - mat[1][3] * det3_235_245 + mat[1][4] * det3_235_235 - mat[1][5] * det3_235_234;
|
|
float det4_1245_0123 = mat[1][0] * det3_245_123 - mat[1][1] * det3_245_023 + mat[1][2] * det3_245_013 - mat[1][3] * det3_245_012;
|
|
float det4_1245_0124 = mat[1][0] * det3_245_124 - mat[1][1] * det3_245_024 + mat[1][2] * det3_245_014 - mat[1][4] * det3_245_012;
|
|
float det4_1245_0125 = mat[1][0] * det3_245_125 - mat[1][1] * det3_245_025 + mat[1][2] * det3_245_015 - mat[1][5] * det3_245_012;
|
|
float det4_1245_0134 = mat[1][0] * det3_245_134 - mat[1][1] * det3_245_034 + mat[1][3] * det3_245_014 - mat[1][4] * det3_245_013;
|
|
float det4_1245_0135 = mat[1][0] * det3_245_135 - mat[1][1] * det3_245_035 + mat[1][3] * det3_245_015 - mat[1][5] * det3_245_013;
|
|
float det4_1245_0145 = mat[1][0] * det3_245_145 - mat[1][1] * det3_245_045 + mat[1][4] * det3_245_015 - mat[1][5] * det3_245_014;
|
|
float det4_1245_0234 = mat[1][0] * det3_245_234 - mat[1][2] * det3_245_034 + mat[1][3] * det3_245_024 - mat[1][4] * det3_245_023;
|
|
float det4_1245_0235 = mat[1][0] * det3_245_235 - mat[1][2] * det3_245_035 + mat[1][3] * det3_245_025 - mat[1][5] * det3_245_023;
|
|
float det4_1245_0245 = mat[1][0] * det3_245_245 - mat[1][2] * det3_245_045 + mat[1][4] * det3_245_025 - mat[1][5] * det3_245_024;
|
|
float det4_1245_0345 = mat[1][0] * det3_245_345 - mat[1][3] * det3_245_045 + mat[1][4] * det3_245_035 - mat[1][5] * det3_245_034;
|
|
float det4_1245_1234 = mat[1][1] * det3_245_234 - mat[1][2] * det3_245_134 + mat[1][3] * det3_245_124 - mat[1][4] * det3_245_123;
|
|
float det4_1245_1235 = mat[1][1] * det3_245_235 - mat[1][2] * det3_245_135 + mat[1][3] * det3_245_125 - mat[1][5] * det3_245_123;
|
|
float det4_1245_1245 = mat[1][1] * det3_245_245 - mat[1][2] * det3_245_145 + mat[1][4] * det3_245_125 - mat[1][5] * det3_245_124;
|
|
float det4_1245_1345 = mat[1][1] * det3_245_345 - mat[1][3] * det3_245_145 + mat[1][4] * det3_245_135 - mat[1][5] * det3_245_134;
|
|
float det4_1245_2345 = mat[1][2] * det3_245_345 - mat[1][3] * det3_245_245 + mat[1][4] * det3_245_235 - mat[1][5] * det3_245_234;
|
|
float det4_1345_0123 = mat[1][0] * det3_345_123 - mat[1][1] * det3_345_023 + mat[1][2] * det3_345_013 - mat[1][3] * det3_345_012;
|
|
float det4_1345_0124 = mat[1][0] * det3_345_124 - mat[1][1] * det3_345_024 + mat[1][2] * det3_345_014 - mat[1][4] * det3_345_012;
|
|
float det4_1345_0125 = mat[1][0] * det3_345_125 - mat[1][1] * det3_345_025 + mat[1][2] * det3_345_015 - mat[1][5] * det3_345_012;
|
|
float det4_1345_0134 = mat[1][0] * det3_345_134 - mat[1][1] * det3_345_034 + mat[1][3] * det3_345_014 - mat[1][4] * det3_345_013;
|
|
float det4_1345_0135 = mat[1][0] * det3_345_135 - mat[1][1] * det3_345_035 + mat[1][3] * det3_345_015 - mat[1][5] * det3_345_013;
|
|
float det4_1345_0145 = mat[1][0] * det3_345_145 - mat[1][1] * det3_345_045 + mat[1][4] * det3_345_015 - mat[1][5] * det3_345_014;
|
|
float det4_1345_0234 = mat[1][0] * det3_345_234 - mat[1][2] * det3_345_034 + mat[1][3] * det3_345_024 - mat[1][4] * det3_345_023;
|
|
float det4_1345_0235 = mat[1][0] * det3_345_235 - mat[1][2] * det3_345_035 + mat[1][3] * det3_345_025 - mat[1][5] * det3_345_023;
|
|
float det4_1345_0245 = mat[1][0] * det3_345_245 - mat[1][2] * det3_345_045 + mat[1][4] * det3_345_025 - mat[1][5] * det3_345_024;
|
|
float det4_1345_0345 = mat[1][0] * det3_345_345 - mat[1][3] * det3_345_045 + mat[1][4] * det3_345_035 - mat[1][5] * det3_345_034;
|
|
float det4_1345_1234 = mat[1][1] * det3_345_234 - mat[1][2] * det3_345_134 + mat[1][3] * det3_345_124 - mat[1][4] * det3_345_123;
|
|
float det4_1345_1235 = mat[1][1] * det3_345_235 - mat[1][2] * det3_345_135 + mat[1][3] * det3_345_125 - mat[1][5] * det3_345_123;
|
|
float det4_1345_1245 = mat[1][1] * det3_345_245 - mat[1][2] * det3_345_145 + mat[1][4] * det3_345_125 - mat[1][5] * det3_345_124;
|
|
float det4_1345_1345 = mat[1][1] * det3_345_345 - mat[1][3] * det3_345_145 + mat[1][4] * det3_345_135 - mat[1][5] * det3_345_134;
|
|
float det4_1345_2345 = mat[1][2] * det3_345_345 - mat[1][3] * det3_345_245 + mat[1][4] * det3_345_235 - mat[1][5] * det3_345_234;
|
|
|
|
// remaining 5x5 sub-determinants
|
|
float det5_01234_01234 = mat[0][0] * det4_1234_1234 - mat[0][1] * det4_1234_0234 + mat[0][2] * det4_1234_0134 - mat[0][3] * det4_1234_0124 + mat[0][4] * det4_1234_0123;
|
|
float det5_01234_01235 = mat[0][0] * det4_1234_1235 - mat[0][1] * det4_1234_0235 + mat[0][2] * det4_1234_0135 - mat[0][3] * det4_1234_0125 + mat[0][5] * det4_1234_0123;
|
|
float det5_01234_01245 = mat[0][0] * det4_1234_1245 - mat[0][1] * det4_1234_0245 + mat[0][2] * det4_1234_0145 - mat[0][4] * det4_1234_0125 + mat[0][5] * det4_1234_0124;
|
|
float det5_01234_01345 = mat[0][0] * det4_1234_1345 - mat[0][1] * det4_1234_0345 + mat[0][3] * det4_1234_0145 - mat[0][4] * det4_1234_0135 + mat[0][5] * det4_1234_0134;
|
|
float det5_01234_02345 = mat[0][0] * det4_1234_2345 - mat[0][2] * det4_1234_0345 + mat[0][3] * det4_1234_0245 - mat[0][4] * det4_1234_0235 + mat[0][5] * det4_1234_0234;
|
|
float det5_01234_12345 = mat[0][1] * det4_1234_2345 - mat[0][2] * det4_1234_1345 + mat[0][3] * det4_1234_1245 - mat[0][4] * det4_1234_1235 + mat[0][5] * det4_1234_1234;
|
|
float det5_01235_01234 = mat[0][0] * det4_1235_1234 - mat[0][1] * det4_1235_0234 + mat[0][2] * det4_1235_0134 - mat[0][3] * det4_1235_0124 + mat[0][4] * det4_1235_0123;
|
|
float det5_01235_01235 = mat[0][0] * det4_1235_1235 - mat[0][1] * det4_1235_0235 + mat[0][2] * det4_1235_0135 - mat[0][3] * det4_1235_0125 + mat[0][5] * det4_1235_0123;
|
|
float det5_01235_01245 = mat[0][0] * det4_1235_1245 - mat[0][1] * det4_1235_0245 + mat[0][2] * det4_1235_0145 - mat[0][4] * det4_1235_0125 + mat[0][5] * det4_1235_0124;
|
|
float det5_01235_01345 = mat[0][0] * det4_1235_1345 - mat[0][1] * det4_1235_0345 + mat[0][3] * det4_1235_0145 - mat[0][4] * det4_1235_0135 + mat[0][5] * det4_1235_0134;
|
|
float det5_01235_02345 = mat[0][0] * det4_1235_2345 - mat[0][2] * det4_1235_0345 + mat[0][3] * det4_1235_0245 - mat[0][4] * det4_1235_0235 + mat[0][5] * det4_1235_0234;
|
|
float det5_01235_12345 = mat[0][1] * det4_1235_2345 - mat[0][2] * det4_1235_1345 + mat[0][3] * det4_1235_1245 - mat[0][4] * det4_1235_1235 + mat[0][5] * det4_1235_1234;
|
|
float det5_01245_01234 = mat[0][0] * det4_1245_1234 - mat[0][1] * det4_1245_0234 + mat[0][2] * det4_1245_0134 - mat[0][3] * det4_1245_0124 + mat[0][4] * det4_1245_0123;
|
|
float det5_01245_01235 = mat[0][0] * det4_1245_1235 - mat[0][1] * det4_1245_0235 + mat[0][2] * det4_1245_0135 - mat[0][3] * det4_1245_0125 + mat[0][5] * det4_1245_0123;
|
|
float det5_01245_01245 = mat[0][0] * det4_1245_1245 - mat[0][1] * det4_1245_0245 + mat[0][2] * det4_1245_0145 - mat[0][4] * det4_1245_0125 + mat[0][5] * det4_1245_0124;
|
|
float det5_01245_01345 = mat[0][0] * det4_1245_1345 - mat[0][1] * det4_1245_0345 + mat[0][3] * det4_1245_0145 - mat[0][4] * det4_1245_0135 + mat[0][5] * det4_1245_0134;
|
|
float det5_01245_02345 = mat[0][0] * det4_1245_2345 - mat[0][2] * det4_1245_0345 + mat[0][3] * det4_1245_0245 - mat[0][4] * det4_1245_0235 + mat[0][5] * det4_1245_0234;
|
|
float det5_01245_12345 = mat[0][1] * det4_1245_2345 - mat[0][2] * det4_1245_1345 + mat[0][3] * det4_1245_1245 - mat[0][4] * det4_1245_1235 + mat[0][5] * det4_1245_1234;
|
|
float det5_01345_01234 = mat[0][0] * det4_1345_1234 - mat[0][1] * det4_1345_0234 + mat[0][2] * det4_1345_0134 - mat[0][3] * det4_1345_0124 + mat[0][4] * det4_1345_0123;
|
|
float det5_01345_01235 = mat[0][0] * det4_1345_1235 - mat[0][1] * det4_1345_0235 + mat[0][2] * det4_1345_0135 - mat[0][3] * det4_1345_0125 + mat[0][5] * det4_1345_0123;
|
|
float det5_01345_01245 = mat[0][0] * det4_1345_1245 - mat[0][1] * det4_1345_0245 + mat[0][2] * det4_1345_0145 - mat[0][4] * det4_1345_0125 + mat[0][5] * det4_1345_0124;
|
|
float det5_01345_01345 = mat[0][0] * det4_1345_1345 - mat[0][1] * det4_1345_0345 + mat[0][3] * det4_1345_0145 - mat[0][4] * det4_1345_0135 + mat[0][5] * det4_1345_0134;
|
|
float det5_01345_02345 = mat[0][0] * det4_1345_2345 - mat[0][2] * det4_1345_0345 + mat[0][3] * det4_1345_0245 - mat[0][4] * det4_1345_0235 + mat[0][5] * det4_1345_0234;
|
|
float det5_01345_12345 = mat[0][1] * det4_1345_2345 - mat[0][2] * det4_1345_1345 + mat[0][3] * det4_1345_1245 - mat[0][4] * det4_1345_1235 + mat[0][5] * det4_1345_1234;
|
|
float det5_02345_01234 = mat[0][0] * det4_2345_1234 - mat[0][1] * det4_2345_0234 + mat[0][2] * det4_2345_0134 - mat[0][3] * det4_2345_0124 + mat[0][4] * det4_2345_0123;
|
|
float det5_02345_01235 = mat[0][0] * det4_2345_1235 - mat[0][1] * det4_2345_0235 + mat[0][2] * det4_2345_0135 - mat[0][3] * det4_2345_0125 + mat[0][5] * det4_2345_0123;
|
|
float det5_02345_01245 = mat[0][0] * det4_2345_1245 - mat[0][1] * det4_2345_0245 + mat[0][2] * det4_2345_0145 - mat[0][4] * det4_2345_0125 + mat[0][5] * det4_2345_0124;
|
|
float det5_02345_01345 = mat[0][0] * det4_2345_1345 - mat[0][1] * det4_2345_0345 + mat[0][3] * det4_2345_0145 - mat[0][4] * det4_2345_0135 + mat[0][5] * det4_2345_0134;
|
|
float det5_02345_02345 = mat[0][0] * det4_2345_2345 - mat[0][2] * det4_2345_0345 + mat[0][3] * det4_2345_0245 - mat[0][4] * det4_2345_0235 + mat[0][5] * det4_2345_0234;
|
|
float det5_02345_12345 = mat[0][1] * det4_2345_2345 - mat[0][2] * det4_2345_1345 + mat[0][3] * det4_2345_1245 - mat[0][4] * det4_2345_1235 + mat[0][5] * det4_2345_1234;
|
|
|
|
mat[0][0] = det5_12345_12345 * invDet;
|
|
mat[0][1] = -det5_02345_12345 * invDet;
|
|
mat[0][2] = det5_01345_12345 * invDet;
|
|
mat[0][3] = -det5_01245_12345 * invDet;
|
|
mat[0][4] = det5_01235_12345 * invDet;
|
|
mat[0][5] = -det5_01234_12345 * invDet;
|
|
|
|
mat[1][0] = -det5_12345_02345 * invDet;
|
|
mat[1][1] = det5_02345_02345 * invDet;
|
|
mat[1][2] = -det5_01345_02345 * invDet;
|
|
mat[1][3] = det5_01245_02345 * invDet;
|
|
mat[1][4] = -det5_01235_02345 * invDet;
|
|
mat[1][5] = det5_01234_02345 * invDet;
|
|
|
|
mat[2][0] = det5_12345_01345 * invDet;
|
|
mat[2][1] = -det5_02345_01345 * invDet;
|
|
mat[2][2] = det5_01345_01345 * invDet;
|
|
mat[2][3] = -det5_01245_01345 * invDet;
|
|
mat[2][4] = det5_01235_01345 * invDet;
|
|
mat[2][5] = -det5_01234_01345 * invDet;
|
|
|
|
mat[3][0] = -det5_12345_01245 * invDet;
|
|
mat[3][1] = det5_02345_01245 * invDet;
|
|
mat[3][2] = -det5_01345_01245 * invDet;
|
|
mat[3][3] = det5_01245_01245 * invDet;
|
|
mat[3][4] = -det5_01235_01245 * invDet;
|
|
mat[3][5] = det5_01234_01245 * invDet;
|
|
|
|
mat[4][0] = det5_12345_01235 * invDet;
|
|
mat[4][1] = -det5_02345_01235 * invDet;
|
|
mat[4][2] = det5_01345_01235 * invDet;
|
|
mat[4][3] = -det5_01245_01235 * invDet;
|
|
mat[4][4] = det5_01235_01235 * invDet;
|
|
mat[4][5] = -det5_01234_01235 * invDet;
|
|
|
|
mat[5][0] = -det5_12345_01234 * invDet;
|
|
mat[5][1] = det5_02345_01234 * invDet;
|
|
mat[5][2] = -det5_01345_01234 * invDet;
|
|
mat[5][3] = det5_01245_01234 * invDet;
|
|
mat[5][4] = -det5_01235_01234 * invDet;
|
|
mat[5][5] = det5_01234_01234 * invDet;
|
|
|
|
return true;
|
|
#elif 0
|
|
// 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
|
|
idMat3 r0, r1, r2, r3;
|
|
float c0, c1, c2, det, invDet;
|
|
float* mat = reinterpret_cast<float*>(this);
|
|
|
|
// r0 = m0.Inverse();
|
|
c0 = mat[1 * 6 + 1] * mat[2 * 6 + 2] - mat[1 * 6 + 2] * mat[2 * 6 + 1];
|
|
c1 = mat[1 * 6 + 2] * mat[2 * 6 + 0] - mat[1 * 6 + 0] * mat[2 * 6 + 2];
|
|
c2 = mat[1 * 6 + 0] * mat[2 * 6 + 1] - mat[1 * 6 + 1] * mat[2 * 6 + 0];
|
|
|
|
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;
|
|
}
|
|
|
|
invDet = 1.0f / det;
|
|
|
|
r0[0][0] = c0 * invDet;
|
|
r0[0][1] = (mat[0 * 6 + 2] * mat[2 * 6 + 1] - mat[0 * 6 + 1] * mat[2 * 6 + 2]) * invDet;
|
|
r0[0][2] = (mat[0 * 6 + 1] * mat[1 * 6 + 2] - mat[0 * 6 + 2] * mat[1 * 6 + 1]) * invDet;
|
|
r0[1][0] = c1 * invDet;
|
|
r0[1][1] = (mat[0 * 6 + 0] * mat[2 * 6 + 2] - mat[0 * 6 + 2] * mat[2 * 6 + 0]) * invDet;
|
|
r0[1][2] = (mat[0 * 6 + 2] * mat[1 * 6 + 0] - mat[0 * 6 + 0] * mat[1 * 6 + 2]) * invDet;
|
|
r0[2][0] = c2 * invDet;
|
|
r0[2][1] = (mat[0 * 6 + 1] * mat[2 * 6 + 0] - mat[0 * 6 + 0] * mat[2 * 6 + 1]) * invDet;
|
|
r0[2][2] = (mat[0 * 6 + 0] * mat[1 * 6 + 1] - mat[0 * 6 + 1] * mat[1 * 6 + 0]) * invDet;
|
|
|
|
// r1 = r0 * m1;
|
|
r1[0][0] = r0[0][0] * mat[0 * 6 + 3] + r0[0][1] * mat[1 * 6 + 3] + r0[0][2] * mat[2 * 6 + 3];
|
|
r1[0][1] = r0[0][0] * mat[0 * 6 + 4] + r0[0][1] * mat[1 * 6 + 4] + r0[0][2] * mat[2 * 6 + 4];
|
|
r1[0][2] = r0[0][0] * mat[0 * 6 + 5] + r0[0][1] * mat[1 * 6 + 5] + r0[0][2] * mat[2 * 6 + 5];
|
|
r1[1][0] = r0[1][0] * mat[0 * 6 + 3] + r0[1][1] * mat[1 * 6 + 3] + r0[1][2] * mat[2 * 6 + 3];
|
|
r1[1][1] = r0[1][0] * mat[0 * 6 + 4] + r0[1][1] * mat[1 * 6 + 4] + r0[1][2] * mat[2 * 6 + 4];
|
|
r1[1][2] = r0[1][0] * mat[0 * 6 + 5] + r0[1][1] * mat[1 * 6 + 5] + r0[1][2] * mat[2 * 6 + 5];
|
|
r1[2][0] = r0[2][0] * mat[0 * 6 + 3] + r0[2][1] * mat[1 * 6 + 3] + r0[2][2] * mat[2 * 6 + 3];
|
|
r1[2][1] = r0[2][0] * mat[0 * 6 + 4] + r0[2][1] * mat[1 * 6 + 4] + r0[2][2] * mat[2 * 6 + 4];
|
|
r1[2][2] = r0[2][0] * mat[0 * 6 + 5] + r0[2][1] * mat[1 * 6 + 5] + r0[2][2] * mat[2 * 6 + 5];
|
|
|
|
// r2 = m2 * r1;
|
|
r2[0][0] = mat[3 * 6 + 0] * r1[0][0] + mat[3 * 6 + 1] * r1[1][0] + mat[3 * 6 + 2] * r1[2][0];
|
|
r2[0][1] = mat[3 * 6 + 0] * r1[0][1] + mat[3 * 6 + 1] * r1[1][1] + mat[3 * 6 + 2] * r1[2][1];
|
|
r2[0][2] = mat[3 * 6 + 0] * r1[0][2] + mat[3 * 6 + 1] * r1[1][2] + mat[3 * 6 + 2] * r1[2][2];
|
|
r2[1][0] = mat[4 * 6 + 0] * r1[0][0] + mat[4 * 6 + 1] * r1[1][0] + mat[4 * 6 + 2] * r1[2][0];
|
|
r2[1][1] = mat[4 * 6 + 0] * r1[0][1] + mat[4 * 6 + 1] * r1[1][1] + mat[4 * 6 + 2] * r1[2][1];
|
|
r2[1][2] = mat[4 * 6 + 0] * r1[0][2] + mat[4 * 6 + 1] * r1[1][2] + mat[4 * 6 + 2] * r1[2][2];
|
|
r2[2][0] = mat[5 * 6 + 0] * r1[0][0] + mat[5 * 6 + 1] * r1[1][0] + mat[5 * 6 + 2] * r1[2][0];
|
|
r2[2][1] = mat[5 * 6 + 0] * r1[0][1] + mat[5 * 6 + 1] * r1[1][1] + mat[5 * 6 + 2] * r1[2][1];
|
|
r2[2][2] = mat[5 * 6 + 0] * r1[0][2] + mat[5 * 6 + 1] * r1[1][2] + mat[5 * 6 + 2] * r1[2][2];
|
|
|
|
// r3 = r2 - m3;
|
|
r3[0][0] = r2[0][0] - mat[3 * 6 + 3];
|
|
r3[0][1] = r2[0][1] - mat[3 * 6 + 4];
|
|
r3[0][2] = r2[0][2] - mat[3 * 6 + 5];
|
|
r3[1][0] = r2[1][0] - mat[4 * 6 + 3];
|
|
r3[1][1] = r2[1][1] - mat[4 * 6 + 4];
|
|
r3[1][2] = r2[1][2] - mat[4 * 6 + 5];
|
|
r3[2][0] = r2[2][0] - mat[5 * 6 + 3];
|
|
r3[2][1] = r2[2][1] - mat[5 * 6 + 4];
|
|
r3[2][2] = r2[2][2] - mat[5 * 6 + 5];
|
|
|
|
// r3.InverseSelf();
|
|
r2[0][0] = r3[1][1] * r3[2][2] - r3[1][2] * r3[2][1];
|
|
r2[1][0] = r3[1][2] * r3[2][0] - r3[1][0] * r3[2][2];
|
|
r2[2][0] = r3[1][0] * r3[2][1] - r3[1][1] * r3[2][0];
|
|
|
|
det = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0] + r3[0][2] * r2[2][0];
|
|
|
|
if (idMath::Fabs(det) < MATRIX_INVERSE_EPSILON) {
|
|
return false;
|
|
}
|
|
|
|
invDet = 1.0f / det;
|
|
|
|
r2[0][1] = r3[0][2] * r3[2][1] - r3[0][1] * r3[2][2];
|
|
r2[0][2] = r3[0][1] * r3[1][2] - r3[0][2] * r3[1][1];
|
|
r2[1][1] = r3[0][0] * r3[2][2] - r3[0][2] * r3[2][0];
|
|
r2[1][2] = r3[0][2] * r3[1][0] - r3[0][0] * r3[1][2];
|
|
r2[2][1] = r3[0][1] * r3[2][0] - r3[0][0] * r3[2][1];
|
|
r2[2][2] = r3[0][0] * r3[1][1] - r3[0][1] * r3[1][0];
|
|
|
|
r3[0][0] = r2[0][0] * invDet;
|
|
r3[0][1] = r2[0][1] * invDet;
|
|
r3[0][2] = r2[0][2] * invDet;
|
|
r3[1][0] = r2[1][0] * invDet;
|
|
r3[1][1] = r2[1][1] * invDet;
|
|
r3[1][2] = r2[1][2] * invDet;
|
|
r3[2][0] = r2[2][0] * invDet;
|
|
r3[2][1] = r2[2][1] * invDet;
|
|
r3[2][2] = r2[2][2] * invDet;
|
|
|
|
// r2 = m2 * r0;
|
|
r2[0][0] = mat[3 * 6 + 0] * r0[0][0] + mat[3 * 6 + 1] * r0[1][0] + mat[3 * 6 + 2] * r0[2][0];
|
|
r2[0][1] = mat[3 * 6 + 0] * r0[0][1] + mat[3 * 6 + 1] * r0[1][1] + mat[3 * 6 + 2] * r0[2][1];
|
|
r2[0][2] = mat[3 * 6 + 0] * r0[0][2] + mat[3 * 6 + 1] * r0[1][2] + mat[3 * 6 + 2] * r0[2][2];
|
|
r2[1][0] = mat[4 * 6 + 0] * r0[0][0] + mat[4 * 6 + 1] * r0[1][0] + mat[4 * 6 + 2] * r0[2][0];
|
|
r2[1][1] = mat[4 * 6 + 0] * r0[0][1] + mat[4 * 6 + 1] * r0[1][1] + mat[4 * 6 + 2] * r0[2][1];
|
|
r2[1][2] = mat[4 * 6 + 0] * r0[0][2] + mat[4 * 6 + 1] * r0[1][2] + mat[4 * 6 + 2] * r0[2][2];
|
|
r2[2][0] = mat[5 * 6 + 0] * r0[0][0] + mat[5 * 6 + 1] * r0[1][0] + mat[5 * 6 + 2] * r0[2][0];
|
|
r2[2][1] = mat[5 * 6 + 0] * r0[0][1] + mat[5 * 6 + 1] * r0[1][1] + mat[5 * 6 + 2] * r0[2][1];
|
|
r2[2][2] = mat[5 * 6 + 0] * r0[0][2] + mat[5 * 6 + 1] * r0[1][2] + mat[5 * 6 + 2] * r0[2][2];
|
|
|
|
// m2 = r3 * r2;
|
|
mat[3 * 6 + 0] = r3[0][0] * r2[0][0] + r3[0][1] * r2[1][0] + r3[0][2] * r2[2][0];
|
|
mat[3 * 6 + 1] = r3[0][0] * r2[0][1] + r3[0][1] * r2[1][1] + r3[0][2] * r2[2][1];
|
|
mat[3 * 6 + 2] = r3[0][0] * r2[0][2] + r3[0][1] * r2[1][2] + r3[0][2] * r2[2][2];
|
|
mat[4 * 6 + 0] = r3[1][0] * r2[0][0] + r3[1][1] * r2[1][0] + r3[1][2] * r2[2][0];
|
|
mat[4 * 6 + 1] = r3[1][0] * r2[0][1] + r3[1][1] * r2[1][1] + r3[1][2] * r2[2][1];
|
|
mat[4 * 6 + 2] = r3[1][0] * r2[0][2] + r3[1][1] * r2[1][2] + r3[1][2] * r2[2][2];
|
|
mat[5 * 6 + 0] = r3[2][0] * r2[0][0] + r3[2][1] * r2[1][0] + r3[2][2] * r2[2][0];
|
|
mat[5 * 6 + 1] = r3[2][0] * r2[0][1] + r3[2][1] * r2[1][1] + r3[2][2] * r2[2][1];
|
|
mat[5 * 6 + 2] = r3[2][0] * r2[0][2] + r3[2][1] * r2[1][2] + r3[2][2] * r2[2][2];
|
|
|
|
// m0 = r0 - r1 * m2;
|
|
mat[0 * 6 + 0] = r0[0][0] - r1[0][0] * mat[3 * 6 + 0] - r1[0][1] * mat[4 * 6 + 0] - r1[0][2] * mat[5 * 6 + 0];
|
|
mat[0 * 6 + 1] = r0[0][1] - r1[0][0] * mat[3 * 6 + 1] - r1[0][1] * mat[4 * 6 + 1] - r1[0][2] * mat[5 * 6 + 1];
|
|
mat[0 * 6 + 2] = r0[0][2] - r1[0][0] * mat[3 * 6 + 2] - r1[0][1] * mat[4 * 6 + 2] - r1[0][2] * mat[5 * 6 + 2];
|
|
mat[1 * 6 + 0] = r0[1][0] - r1[1][0] * mat[3 * 6 + 0] - r1[1][1] * mat[4 * 6 + 0] - r1[1][2] * mat[5 * 6 + 0];
|
|
mat[1 * 6 + 1] = r0[1][1] - r1[1][0] * mat[3 * 6 + 1] - r1[1][1] * mat[4 * 6 + 1] - r1[1][2] * mat[5 * 6 + 1];
|
|
mat[1 * 6 + 2] = r0[1][2] - r1[1][0] * mat[3 * 6 + 2] - r1[1][1] * mat[4 * 6 + 2] - r1[1][2] * mat[5 * 6 + 2];
|
|
mat[2 * 6 + 0] = r0[2][0] - r1[2][0] * mat[3 * 6 + 0] - r1[2][1] * mat[4 * 6 + 0] - r1[2][2] * mat[5 * 6 + 0];
|
|
mat[2 * 6 + 1] = r0[2][1] - r1[2][0] * mat[3 * 6 + 1] - r1[2][1] * mat[4 * 6 + 1] - r1[2][2] * mat[5 * 6 + 1];
|
|
mat[2 * 6 + 2] = r0[2][2] - r1[2][0] * mat[3 * 6 + 2] - r1[2][1] * mat[4 * 6 + 2] - r1[2][2] * mat[5 * 6 + 2];
|
|
|
|
// m1 = r1 * r3;
|
|
mat[0 * 6 + 3] = r1[0][0] * r3[0][0] + r1[0][1] * r3[1][0] + r1[0][2] * r3[2][0];
|
|
mat[0 * 6 + 4] = r1[0][0] * r3[0][1] + r1[0][1] * r3[1][1] + r1[0][2] * r3[2][1];
|
|
mat[0 * 6 + 5] = r1[0][0] * r3[0][2] + r1[0][1] * r3[1][2] + r1[0][2] * r3[2][2];
|
|
mat[1 * 6 + 3] = r1[1][0] * r3[0][0] + r1[1][1] * r3[1][0] + r1[1][2] * r3[2][0];
|
|
mat[1 * 6 + 4] = r1[1][0] * r3[0][1] + r1[1][1] * r3[1][1] + r1[1][2] * r3[2][1];
|
|
mat[1 * 6 + 5] = r1[1][0] * r3[0][2] + r1[1][1] * r3[1][2] + r1[1][2] * r3[2][2];
|
|
mat[2 * 6 + 3] = r1[2][0] * r3[0][0] + r1[2][1] * r3[1][0] + r1[2][2] * r3[2][0];
|
|
mat[2 * 6 + 4] = r1[2][0] * r3[0][1] + r1[2][1] * r3[1][1] + r1[2][2] * r3[2][1];
|
|
mat[2 * 6 + 5] = r1[2][0] * r3[0][2] + r1[2][1] * r3[1][2] + r1[2][2] * r3[2][2];
|
|
|
|
// m3 = -r3;
|
|
mat[3 * 6 + 3] = -r3[0][0];
|
|
mat[3 * 6 + 4] = -r3[0][1];
|
|
mat[3 * 6 + 5] = -r3[0][2];
|
|
mat[4 * 6 + 3] = -r3[1][0];
|
|
mat[4 * 6 + 4] = -r3[1][1];
|
|
mat[4 * 6 + 5] = -r3[1][2];
|
|
mat[5 * 6 + 3] = -r3[2][0];
|
|
mat[5 * 6 + 4] = -r3[2][1];
|
|
mat[5 * 6 + 5] = -r3[2][2];
|
|
|
|
return true;
|
|
#endif
|
|
}
|
|
|
|
/*
|
|
=============
|
|
idMat6::ToString
|
|
=============
|
|
*/
|
|
const char* idMat6::ToString(int precision) const {
|
|
return idStr::FloatArrayToString(ToFloatPtr(), GetDimension(), precision);
|
|
}
|
|
|
|
|
|
//===============================================================
|
|
//
|
|
// idMatX
|
|
//
|
|
//===============================================================
|
|
|
|
#ifdef QUAKE4
|
|
float idMatX::tempPtr[MATX_MAX_TEMP];
|
|
#else
|
|
float idMatX::temp[MATX_MAX_TEMP + 4];
|
|
float* idMatX::tempPtr = (float*)(((INT_PTR)idMatX::temp + 15) & ~15);
|
|
#endif
|
|
int idMatX::tempIndex = 0;
|
|
|
|
|
|
/*
|
|
============
|
|
idMatX::ChangeSize
|
|
============
|
|
*/
|
|
void idMatX::ChangeSize(int rows, int columns, bool makeZero) {
|
|
int alloc = (rows * columns + 3) & ~3;
|
|
if (alloc > alloced && alloced != -1) {
|
|
float* oldMat = mat;
|
|
#ifdef QUAKE4
|
|
mat = (float*)Mem_Alloc16(alloc * sizeof(float));
|
|
#else
|
|
mat = (float*)Mem_Alloc16(alloc * sizeof(float));
|
|
#endif
|
|
if (makeZero) {
|
|
memset(mat, 0, alloc * sizeof(float));
|
|
}
|
|
alloced = alloc;
|
|
if (oldMat) {
|
|
int minRow = Min(numRows, rows);
|
|
int minColumn = Min(numColumns, columns);
|
|
for (int i = 0; i < minRow; i++) {
|
|
for (int j = 0; j < minColumn; j++) {
|
|
mat[i * columns + j] = oldMat[i * numColumns + j];
|
|
}
|
|
}
|
|
Mem_Free16(oldMat);
|
|
}
|
|
}
|
|
else {
|
|
if (columns < numColumns) {
|
|
int minRow = Min(numRows, rows);
|
|
for (int i = 0; i < minRow; i++) {
|
|
for (int j = 0; j < columns; j++) {
|
|
mat[i * columns + j] = mat[i * numColumns + j];
|
|
}
|
|
}
|
|
}
|
|
else if (columns > numColumns) {
|
|
for (int i = Min(numRows, rows) - 1; i >= 0; i--) {
|
|
if (makeZero) {
|
|
for (int j = columns - 1; j >= numColumns; j--) {
|
|
mat[i * columns + j] = 0.0f;
|
|
}
|
|
}
|
|
for (int j = numColumns - 1; j >= 0; j--) {
|
|
mat[i * columns + j] = mat[i * numColumns + j];
|
|
}
|
|
}
|
|
}
|
|
if (makeZero && rows > numRows) {
|
|
memset(mat + numRows * columns, 0, (rows - numRows) * columns * sizeof(float));
|
|
}
|
|
}
|
|
numRows = rows;
|
|
numColumns = columns;
|
|
MATX_CLEAREND();
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::RemoveRow
|
|
============
|
|
*/
|
|
idMatX& idMatX::RemoveRow(int r) {
|
|
int i;
|
|
|
|
assert(r < numRows);
|
|
|
|
numRows--;
|
|
|
|
for (i = r; i < numRows; i++) {
|
|
memcpy(&mat[i * numColumns], &mat[(i + 1) * numColumns], numColumns * sizeof(float));
|
|
}
|
|
|
|
return *this;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::RemoveColumn
|
|
============
|
|
*/
|
|
idMatX& idMatX::RemoveColumn(int r) {
|
|
int i;
|
|
|
|
assert(r < numColumns);
|
|
|
|
numColumns--;
|
|
|
|
for (i = 0; i < numRows - 1; i++) {
|
|
memmove(&mat[i * numColumns + r], &mat[i * (numColumns + 1) + r + 1], numColumns * sizeof(float));
|
|
}
|
|
memmove(&mat[i * numColumns + r], &mat[i * (numColumns + 1) + r + 1], (numColumns - r) * sizeof(float));
|
|
|
|
return *this;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::RemoveRowColumn
|
|
============
|
|
*/
|
|
idMatX& idMatX::RemoveRowColumn(int r) {
|
|
int i;
|
|
|
|
assert(r < numRows && r < numColumns);
|
|
|
|
numRows--;
|
|
numColumns--;
|
|
|
|
if (r > 0) {
|
|
for (i = 0; i < r - 1; i++) {
|
|
memmove(&mat[i * numColumns + r], &mat[i * (numColumns + 1) + r + 1], numColumns * sizeof(float));
|
|
}
|
|
memmove(&mat[i * numColumns + r], &mat[i * (numColumns + 1) + r + 1], (numColumns - r) * sizeof(float));
|
|
}
|
|
|
|
memcpy(&mat[r * numColumns], &mat[(r + 1) * (numColumns + 1)], r * sizeof(float));
|
|
|
|
for (i = r; i < numRows - 1; i++) {
|
|
memcpy(&mat[i * numColumns + r], &mat[(i + 1) * (numColumns + 1) + r + 1], numColumns * sizeof(float));
|
|
}
|
|
memcpy(&mat[i * numColumns + r], &mat[(i + 1) * (numColumns + 1) + r + 1], (numColumns - r) * sizeof(float));
|
|
|
|
return *this;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::IsOrthogonal
|
|
|
|
returns true if (*this) * this->Transpose() == Identity
|
|
============
|
|
*/
|
|
bool idMatX::IsOrthogonal(const float epsilon) const {
|
|
float* ptr1, * ptr2, sum;
|
|
|
|
if (!IsSquare()) {
|
|
return false;
|
|
}
|
|
|
|
ptr1 = mat;
|
|
for (int i = 0; i < numRows; i++) {
|
|
for (int j = 0; j < numColumns; j++) {
|
|
ptr2 = mat + j;
|
|
sum = ptr1[0] * ptr2[0] - (float)(i == j);
|
|
for (int n = 1; n < numColumns; n++) {
|
|
ptr2 += numColumns;
|
|
sum += ptr1[n] * ptr2[0];
|
|
}
|
|
if (idMath::Fabs(sum) > epsilon) {
|
|
return false;
|
|
}
|
|
}
|
|
ptr1 += numColumns;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::IsOrthonormal
|
|
|
|
returns true if (*this) * this->Transpose() == Identity and the length of each column vector is 1
|
|
============
|
|
*/
|
|
bool idMatX::IsOrthonormal(const float epsilon) const {
|
|
float* ptr1, * ptr2, sum;
|
|
|
|
if (!IsSquare()) {
|
|
return false;
|
|
}
|
|
|
|
ptr1 = mat;
|
|
for (int i = 0; i < numRows; i++) {
|
|
for (int j = 0; j < numColumns; j++) {
|
|
ptr2 = mat + j;
|
|
sum = ptr1[0] * ptr2[0] - (float)(i == j);
|
|
for (int n = 1; n < numColumns; n++) {
|
|
ptr2 += numColumns;
|
|
sum += ptr1[n] * ptr2[0];
|
|
}
|
|
if (idMath::Fabs(sum) > epsilon) {
|
|
return false;
|
|
}
|
|
}
|
|
ptr1 += numColumns;
|
|
|
|
ptr2 = mat + i;
|
|
sum = ptr2[0] * ptr2[0] - 1.0f;
|
|
for (i = 1; i < numRows; i++) {
|
|
ptr2 += numColumns;
|
|
sum += ptr2[i] * ptr2[i];
|
|
}
|
|
if (idMath::Fabs(sum) > epsilon) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::IsPMatrix
|
|
|
|
returns true if the matrix is a P-matrix
|
|
A square matrix is a P-matrix if all its principal minors are positive.
|
|
============
|
|
*/
|
|
bool idMatX::IsPMatrix(const float epsilon) const {
|
|
int i, j;
|
|
float d;
|
|
idMatX m;
|
|
|
|
if (!IsSquare()) {
|
|
return false;
|
|
}
|
|
|
|
if (numRows <= 0) {
|
|
return true;
|
|
}
|
|
|
|
if ((*this)[0][0] <= epsilon) {
|
|
return false;
|
|
}
|
|
|
|
if (numRows <= 1) {
|
|
return true;
|
|
}
|
|
|
|
m.SetData(numRows - 1, numColumns - 1, MATX_ALLOCA((numRows - 1) * (numColumns - 1)));
|
|
|
|
for (i = 1; i < numRows; i++) {
|
|
for (j = 1; j < numColumns; j++) {
|
|
m[i - 1][j - 1] = (*this)[i][j];
|
|
}
|
|
}
|
|
|
|
if (!m.IsPMatrix(epsilon)) {
|
|
return false;
|
|
}
|
|
|
|
for (i = 1; i < numRows; i++) {
|
|
d = (*this)[i][0] / (*this)[0][0];
|
|
for (j = 1; j < numColumns; j++) {
|
|
m[i - 1][j - 1] = (*this)[i][j] - d * (*this)[0][j];
|
|
}
|
|
}
|
|
|
|
if (!m.IsPMatrix(epsilon)) {
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::IsZMatrix
|
|
|
|
returns true if the matrix is a Z-matrix
|
|
A square matrix M is a Z-matrix if M[i][j] <= 0 for all i != j.
|
|
============
|
|
*/
|
|
bool idMatX::IsZMatrix(const float epsilon) const {
|
|
int i, j;
|
|
|
|
if (!IsSquare()) {
|
|
return false;
|
|
}
|
|
|
|
for (i = 0; i < numRows; i++) {
|
|
for (j = 0; j < numColumns; j++) {
|
|
if ((*this)[i][j] > epsilon && i != j) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::IsPositiveDefinite
|
|
|
|
returns true if the matrix is Positive Definite (PD)
|
|
A square matrix M of order n is said to be PD if y'My > 0 for all vectors y of dimension n, y != 0.
|
|
============
|
|
*/
|
|
bool idMatX::IsPositiveDefinite(const float epsilon) const {
|
|
int i, j, k;
|
|
float d, s;
|
|
idMatX m;
|
|
|
|
// the matrix must be square
|
|
if (!IsSquare()) {
|
|
return false;
|
|
}
|
|
|
|
// copy matrix
|
|
m.SetData(numRows, numColumns, MATX_ALLOCA(numRows * numColumns));
|
|
m = *this;
|
|
|
|
// add transpose
|
|
for (i = 0; i < numRows; i++) {
|
|
for (j = 0; j < numColumns; j++) {
|
|
m[i][j] += (*this)[j][i];
|
|
}
|
|
}
|
|
|
|
// test Positive Definiteness with Gaussian pivot steps
|
|
for (i = 0; i < numRows; i++) {
|
|
|
|
for (j = i; j < numColumns; j++) {
|
|
if (m[j][j] <= epsilon) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
d = 1.0f / m[i][i];
|
|
for (j = i + 1; j < numColumns; j++) {
|
|
s = d * m[j][i];
|
|
m[j][i] = 0.0f;
|
|
for (k = i + 1; k < numRows; k++) {
|
|
m[j][k] -= s * m[i][k];
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::IsSymmetricPositiveDefinite
|
|
|
|
returns true if the matrix is Symmetric Positive Definite (PD)
|
|
============
|
|
*/
|
|
bool idMatX::IsSymmetricPositiveDefinite(const float epsilon) const {
|
|
idMatX m;
|
|
|
|
// the matrix must be symmetric
|
|
if (!IsSymmetric(epsilon)) {
|
|
return false;
|
|
}
|
|
|
|
// copy matrix
|
|
m.SetData(numRows, numColumns, MATX_ALLOCA(numRows * numColumns));
|
|
m = *this;
|
|
|
|
// being able to obtain Cholesky factors is both a necessary and sufficient condition for positive definiteness
|
|
return m.Cholesky_Factor();
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::IsPositiveSemiDefinite
|
|
|
|
returns true if the matrix is Positive Semi Definite (PSD)
|
|
A square matrix M of order n is said to be PSD if y'My >= 0 for all vectors y of dimension n, y != 0.
|
|
============
|
|
*/
|
|
bool idMatX::IsPositiveSemiDefinite(const float epsilon) const {
|
|
int i, j, k;
|
|
float d, s;
|
|
idMatX m;
|
|
|
|
// the matrix must be square
|
|
if (!IsSquare()) {
|
|
return false;
|
|
}
|
|
|
|
// copy original matrix
|
|
m.SetData(numRows, numColumns, MATX_ALLOCA(numRows * numColumns));
|
|
m = *this;
|
|
|
|
// add transpose
|
|
for (i = 0; i < numRows; i++) {
|
|
for (j = 0; j < numColumns; j++) {
|
|
m[i][j] += (*this)[j][i];
|
|
}
|
|
}
|
|
|
|
// test Positive Semi Definiteness with Gaussian pivot steps
|
|
for (i = 0; i < numRows; i++) {
|
|
|
|
for (j = i; j < numColumns; j++) {
|
|
if (m[j][j] < -epsilon) {
|
|
return false;
|
|
}
|
|
if (m[j][j] > epsilon) {
|
|
continue;
|
|
}
|
|
for (k = 0; k < numRows; k++) {
|
|
if (idMath::Fabs(m[k][j]) > epsilon) {
|
|
return false;
|
|
}
|
|
if (idMath::Fabs(m[j][k]) > epsilon) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (m[i][i] <= epsilon) {
|
|
continue;
|
|
}
|
|
|
|
d = 1.0f / m[i][i];
|
|
for (j = i + 1; j < numColumns; j++) {
|
|
s = d * m[j][i];
|
|
m[j][i] = 0.0f;
|
|
for (k = i + 1; k < numRows; k++) {
|
|
m[j][k] -= s * m[i][k];
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::IsSymmetricPositiveSemiDefinite
|
|
|
|
returns true if the matrix is Symmetric Positive Semi Definite (PSD)
|
|
============
|
|
*/
|
|
bool idMatX::IsSymmetricPositiveSemiDefinite(const float epsilon) const {
|
|
|
|
// the matrix must be symmetric
|
|
if (!IsSymmetric(epsilon)) {
|
|
return false;
|
|
}
|
|
|
|
return IsPositiveSemiDefinite(epsilon);
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LowerTriangularInverse
|
|
|
|
in-place inversion of the lower triangular matrix
|
|
============
|
|
*/
|
|
bool idMatX::LowerTriangularInverse(void) {
|
|
int i, j, k;
|
|
double d, sum;
|
|
|
|
for (i = 0; i < numRows; i++) {
|
|
d = (*this)[i][i];
|
|
if (d == 0.0f) {
|
|
return false;
|
|
}
|
|
(*this)[i][i] = d = 1.0f / d;
|
|
|
|
for (j = 0; j < i; j++) {
|
|
sum = 0.0f;
|
|
for (k = j; k < i; k++) {
|
|
sum -= (*this)[i][k] * (*this)[k][j];
|
|
}
|
|
(*this)[i][j] = sum * d;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::UpperTriangularInverse
|
|
|
|
in-place inversion of the upper triangular matrix
|
|
============
|
|
*/
|
|
bool idMatX::UpperTriangularInverse(void) {
|
|
int i, j, k;
|
|
double d, sum;
|
|
|
|
for (i = numRows - 1; i >= 0; i--) {
|
|
d = (*this)[i][i];
|
|
if (d == 0.0f) {
|
|
return false;
|
|
}
|
|
(*this)[i][i] = d = 1.0f / d;
|
|
|
|
for (j = numRows - 1; j > i; j--) {
|
|
sum = 0.0f;
|
|
for (k = j; k > i; k--) {
|
|
sum -= (*this)[i][k] * (*this)[k][j];
|
|
}
|
|
(*this)[i][j] = sum * d;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
=============
|
|
idMatX::ToString
|
|
=============
|
|
*/
|
|
const char* idMatX::ToString(int precision) const {
|
|
return idStr::FloatArrayToString(ToFloatPtr(), GetDimension(), precision);
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Update_RankOne
|
|
|
|
Updates the matrix to obtain the matrix: A + alpha * v * w'
|
|
============
|
|
*/
|
|
void idMatX::Update_RankOne(const idVecX& v, const idVecX& w, float alpha) {
|
|
int i, j;
|
|
float s;
|
|
|
|
assert(v.GetSize() >= numRows);
|
|
assert(w.GetSize() >= numColumns);
|
|
|
|
for (i = 0; i < numRows; i++) {
|
|
s = alpha * v[i];
|
|
for (j = 0; j < numColumns; j++) {
|
|
(*this)[i][j] += s * w[j];
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Update_RankOneSymmetric
|
|
|
|
Updates the matrix to obtain the matrix: A + alpha * v * v'
|
|
============
|
|
*/
|
|
void idMatX::Update_RankOneSymmetric(const idVecX& v, float alpha) {
|
|
int i, j;
|
|
float s;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(v.GetSize() >= numRows);
|
|
|
|
for (i = 0; i < numRows; i++) {
|
|
s = alpha * v[i];
|
|
for (j = 0; j < numColumns; j++) {
|
|
(*this)[i][j] += s * v[j];
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Update_RowColumn
|
|
|
|
Updates the matrix to obtain the matrix:
|
|
|
|
[ 0 a 0 ]
|
|
A + [ d b e ]
|
|
[ 0 c 0 ]
|
|
|
|
where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1], d = w[0,r-1], w[r] = 0.0f, e = w[r+1,numColumns-1]
|
|
============
|
|
*/
|
|
void idMatX::Update_RowColumn(const idVecX& v, const idVecX& w, int r) {
|
|
int i;
|
|
|
|
assert(w[r] == 0.0f);
|
|
assert(v.GetSize() >= numColumns);
|
|
assert(w.GetSize() >= numRows);
|
|
|
|
for (i = 0; i < numRows; i++) {
|
|
(*this)[i][r] += v[i];
|
|
}
|
|
for (i = 0; i < numColumns; i++) {
|
|
(*this)[r][i] += w[i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Update_RowColumnSymmetric
|
|
|
|
Updates the matrix to obtain the matrix:
|
|
|
|
[ 0 a 0 ]
|
|
A + [ a b c ]
|
|
[ 0 c 0 ]
|
|
|
|
where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1]
|
|
============
|
|
*/
|
|
void idMatX::Update_RowColumnSymmetric(const idVecX& v, int r) {
|
|
int i;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(v.GetSize() >= numRows);
|
|
|
|
for (i = 0; i < r; i++) {
|
|
(*this)[i][r] += v[i];
|
|
(*this)[r][i] += v[i];
|
|
}
|
|
(*this)[r][r] += v[r];
|
|
for (i = r + 1; i < numRows; i++) {
|
|
(*this)[i][r] += v[i];
|
|
(*this)[r][i] += v[i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Update_Increment
|
|
|
|
Updates the matrix to obtain the matrix:
|
|
|
|
[ A a ]
|
|
[ c b ]
|
|
|
|
where: a = v[0,numRows-1], b = v[numRows], c = w[0,numColumns-1]], w[numColumns] = 0
|
|
============
|
|
*/
|
|
void idMatX::Update_Increment(const idVecX& v, const idVecX& w) {
|
|
int i;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(v.GetSize() >= numRows + 1);
|
|
assert(w.GetSize() >= numColumns + 1);
|
|
|
|
ChangeSize(numRows + 1, numColumns + 1, false);
|
|
|
|
for (i = 0; i < numRows; i++) {
|
|
(*this)[i][numColumns - 1] = v[i];
|
|
}
|
|
for (i = 0; i < numColumns - 1; i++) {
|
|
(*this)[numRows - 1][i] = w[i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Update_IncrementSymmetric
|
|
|
|
Updates the matrix to obtain the matrix:
|
|
|
|
[ A a ]
|
|
[ a b ]
|
|
|
|
where: a = v[0,numRows-1], b = v[numRows]
|
|
============
|
|
*/
|
|
void idMatX::Update_IncrementSymmetric(const idVecX& v) {
|
|
int i;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(v.GetSize() >= numRows + 1);
|
|
|
|
ChangeSize(numRows + 1, numColumns + 1, false);
|
|
|
|
for (i = 0; i < numRows - 1; i++) {
|
|
(*this)[i][numColumns - 1] = v[i];
|
|
}
|
|
for (i = 0; i < numColumns; i++) {
|
|
(*this)[numRows - 1][i] = v[i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Update_Decrement
|
|
|
|
Updates the matrix to obtain a matrix with row r and column r removed.
|
|
============
|
|
*/
|
|
void idMatX::Update_Decrement(int r) {
|
|
RemoveRowColumn(r);
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Inverse_GaussJordan
|
|
|
|
in-place inversion using Gauss-Jordan elimination
|
|
============
|
|
*/
|
|
bool idMatX::Inverse_GaussJordan(void) {
|
|
int i, j, k, r, c;
|
|
float d, max;
|
|
|
|
assert(numRows == numColumns);
|
|
|
|
int* columnIndex = (int*)_alloca16(numRows * sizeof(int));
|
|
int* rowIndex = (int*)_alloca16(numRows * sizeof(int));
|
|
bool* pivot = (bool*)_alloca16(numRows * sizeof(bool));
|
|
|
|
memset(pivot, 0, numRows * sizeof(bool));
|
|
|
|
// elimination with full pivoting
|
|
for (i = 0; i < numRows; i++) {
|
|
|
|
// search the whole matrix except for pivoted rows for the maximum absolute value
|
|
max = 0.0f;
|
|
r = c = 0;
|
|
for (j = 0; j < numRows; j++) {
|
|
if (!pivot[j]) {
|
|
for (k = 0; k < numRows; k++) {
|
|
if (!pivot[k]) {
|
|
d = idMath::Fabs((*this)[j][k]);
|
|
if (d > max) {
|
|
max = d;
|
|
r = j;
|
|
c = k;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (max == 0.0f) {
|
|
// matrix is not invertible
|
|
return false;
|
|
}
|
|
|
|
pivot[c] = true;
|
|
|
|
// swap rows such that entry (c,c) has the pivot entry
|
|
if (r != c) {
|
|
SwapRows(r, c);
|
|
}
|
|
|
|
// keep track of the row permutation
|
|
rowIndex[i] = r;
|
|
columnIndex[i] = c;
|
|
|
|
// scale the row to make the pivot entry equal to 1
|
|
d = 1.0f / (*this)[c][c];
|
|
(*this)[c][c] = 1.0f;
|
|
for (k = 0; k < numRows; k++) {
|
|
(*this)[c][k] *= d;
|
|
}
|
|
|
|
// zero out the pivot column entries in the other rows
|
|
for (j = 0; j < numRows; j++) {
|
|
if (j != c) {
|
|
d = (*this)[j][c];
|
|
(*this)[j][c] = 0.0f;
|
|
for (k = 0; k < numRows; k++) {
|
|
(*this)[j][k] -= (*this)[c][k] * d;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// reorder rows to store the inverse of the original matrix
|
|
for (j = numRows - 1; j >= 0; j--) {
|
|
if (rowIndex[j] != columnIndex[j]) {
|
|
for (k = 0; k < numRows; k++) {
|
|
d = (*this)[k][rowIndex[j]];
|
|
(*this)[k][rowIndex[j]] = (*this)[k][columnIndex[j]];
|
|
(*this)[k][columnIndex[j]] = d;
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Inverse_UpdateRankOne
|
|
|
|
Updates the in-place inverse using the Sherman-Morrison formula to obtain the inverse for the matrix: A + alpha * v * w'
|
|
============
|
|
*/
|
|
bool idMatX::Inverse_UpdateRankOne(const idVecX& v, const idVecX& w, float alpha) {
|
|
int i, j;
|
|
float beta, s;
|
|
idVecX y, z;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(v.GetSize() >= numColumns);
|
|
assert(w.GetSize() >= numRows);
|
|
|
|
y.SetData(numRows, VECX_ALLOCA(numRows));
|
|
z.SetData(numRows, VECX_ALLOCA(numRows));
|
|
|
|
Multiply(y, v);
|
|
TransposeMultiply(z, w);
|
|
beta = 1.0f + (w * y);
|
|
|
|
if (beta == 0.0f) {
|
|
return false;
|
|
}
|
|
|
|
alpha /= beta;
|
|
|
|
for (i = 0; i < numRows; i++) {
|
|
s = y[i] * alpha;
|
|
for (j = 0; j < numColumns; j++) {
|
|
(*this)[i][j] -= s * z[j];
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Inverse_UpdateRowColumn
|
|
|
|
Updates the in-place inverse to obtain the inverse for the matrix:
|
|
|
|
[ 0 a 0 ]
|
|
A + [ d b e ]
|
|
[ 0 c 0 ]
|
|
|
|
where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1], d = w[0,r-1], w[r] = 0.0f, e = w[r+1,numColumns-1]
|
|
============
|
|
*/
|
|
bool idMatX::Inverse_UpdateRowColumn(const idVecX& v, const idVecX& w, int r) {
|
|
idVecX s;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(v.GetSize() >= numColumns);
|
|
assert(w.GetSize() >= numRows);
|
|
assert(r >= 0 && r < numRows && r < numColumns);
|
|
assert(w[r] == 0.0f);
|
|
|
|
s.SetData(Max(numRows, numColumns), VECX_ALLOCA(Max(numRows, numColumns)));
|
|
s.Zero();
|
|
s[r] = 1.0f;
|
|
|
|
if (!Inverse_UpdateRankOne(v, s, 1.0f)) {
|
|
return false;
|
|
}
|
|
if (!Inverse_UpdateRankOne(s, w, 1.0f)) {
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Inverse_UpdateIncrement
|
|
|
|
Updates the in-place inverse to obtain the inverse for the matrix:
|
|
|
|
[ A a ]
|
|
[ c b ]
|
|
|
|
where: a = v[0,numRows-1], b = v[numRows], c = w[0,numColumns-1], w[numColumns] = 0
|
|
============
|
|
*/
|
|
bool idMatX::Inverse_UpdateIncrement(const idVecX& v, const idVecX& w) {
|
|
idVecX v2;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(v.GetSize() >= numRows + 1);
|
|
assert(w.GetSize() >= numColumns + 1);
|
|
|
|
ChangeSize(numRows + 1, numColumns + 1, true);
|
|
(*this)[numRows - 1][numRows - 1] = 1.0f;
|
|
|
|
v2.SetData(numRows, VECX_ALLOCA(numRows));
|
|
v2 = v;
|
|
v2[numRows - 1] -= 1.0f;
|
|
|
|
return Inverse_UpdateRowColumn(v2, w, numRows - 1);
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Inverse_UpdateDecrement
|
|
|
|
Updates the in-place inverse to obtain the inverse of the matrix with row r and column r removed.
|
|
v and w should store the column and row of the original matrix respectively.
|
|
============
|
|
*/
|
|
bool idMatX::Inverse_UpdateDecrement(const idVecX& v, const idVecX& w, int r) {
|
|
idVecX v1, w1;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(v.GetSize() >= numRows);
|
|
assert(w.GetSize() >= numColumns);
|
|
assert(r >= 0 && r < numRows && r < numColumns);
|
|
|
|
v1.SetData(numRows, VECX_ALLOCA(numRows));
|
|
w1.SetData(numRows, VECX_ALLOCA(numRows));
|
|
|
|
// update the row and column to identity
|
|
v1 = -v;
|
|
w1 = -w;
|
|
v1[r] += 1.0f;
|
|
w1[r] = 0.0f;
|
|
|
|
if (!Inverse_UpdateRowColumn(v1, w1, r)) {
|
|
return false;
|
|
}
|
|
|
|
// physically remove the row and column
|
|
Update_Decrement(r);
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Inverse_Solve
|
|
|
|
Solve Ax = b with A inverted
|
|
============
|
|
*/
|
|
void idMatX::Inverse_Solve(idVecX& x, const idVecX& b) const {
|
|
Multiply(x, b);
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LU_Factor
|
|
|
|
in-place factorization: LU
|
|
L is a triangular matrix stored in the lower triangle.
|
|
L has ones on the diagonal that are not stored.
|
|
U is a triangular matrix stored in the upper triangle.
|
|
If index != NULL partial pivoting is used for numerical stability.
|
|
If index != NULL it must point to an array of numRow integers and is used to keep track of the row permutation.
|
|
If det != NULL the determinant of the matrix is calculated and stored.
|
|
============
|
|
*/
|
|
bool idMatX::LU_Factor(int* index, float* det) {
|
|
int i, j, k, newi, min;
|
|
double s, t, d, w;
|
|
|
|
// if partial pivoting should be used
|
|
if (index) {
|
|
for (i = 0; i < numRows; i++) {
|
|
index[i] = i;
|
|
}
|
|
}
|
|
|
|
w = 1.0f;
|
|
min = Min(numRows, numColumns);
|
|
for (i = 0; i < min; i++) {
|
|
|
|
newi = i;
|
|
s = idMath::Fabs((*this)[i][i]);
|
|
|
|
if (index) {
|
|
// find the largest absolute pivot
|
|
for (j = i + 1; j < numRows; j++) {
|
|
t = idMath::Fabs((*this)[j][i]);
|
|
if (t > s) {
|
|
newi = j;
|
|
s = t;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (s == 0.0f) {
|
|
return false;
|
|
}
|
|
|
|
if (newi != i) {
|
|
|
|
w = -w;
|
|
|
|
// swap index elements
|
|
k = index[i];
|
|
index[i] = index[newi];
|
|
index[newi] = k;
|
|
|
|
// swap rows
|
|
for (j = 0; j < numColumns; j++) {
|
|
t = (*this)[newi][j];
|
|
(*this)[newi][j] = (*this)[i][j];
|
|
(*this)[i][j] = t;
|
|
}
|
|
}
|
|
|
|
if (i < numRows) {
|
|
d = 1.0f / (*this)[i][i];
|
|
for (j = i + 1; j < numRows; j++) {
|
|
(*this)[j][i] *= d;
|
|
}
|
|
}
|
|
|
|
if (i < min - 1) {
|
|
for (j = i + 1; j < numRows; j++) {
|
|
d = (*this)[j][i];
|
|
for (k = i + 1; k < numColumns; k++) {
|
|
(*this)[j][k] -= d * (*this)[i][k];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if (det) {
|
|
for (i = 0; i < numRows; i++) {
|
|
w *= (*this)[i][i];
|
|
}
|
|
*det = w;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LU_UpdateRankOne
|
|
|
|
Updates the in-place LU factorization to obtain the factors for the matrix: LU + alpha * v * w'
|
|
============
|
|
*/
|
|
bool idMatX::LU_UpdateRankOne(const idVecX& v, const idVecX& w, float alpha, int* index) {
|
|
int i, j, max;
|
|
float* y, * z;
|
|
double diag, beta, p0, p1, d;
|
|
|
|
assert(v.GetSize() >= numColumns);
|
|
assert(w.GetSize() >= numRows);
|
|
|
|
y = (float*)_alloca16(v.GetSize() * sizeof(float));
|
|
z = (float*)_alloca16(w.GetSize() * sizeof(float));
|
|
|
|
if (index != NULL) {
|
|
for (i = 0; i < numRows; i++) {
|
|
y[i] = alpha * v[index[i]];
|
|
}
|
|
}
|
|
else {
|
|
for (i = 0; i < numRows; i++) {
|
|
y[i] = alpha * v[i];
|
|
}
|
|
}
|
|
|
|
memcpy(z, w.ToFloatPtr(), w.GetSize() * sizeof(float));
|
|
|
|
max = Min(numRows, numColumns);
|
|
for (i = 0; i < max; i++) {
|
|
diag = (*this)[i][i];
|
|
|
|
p0 = y[i];
|
|
p1 = z[i];
|
|
diag += p0 * p1;
|
|
|
|
if (diag == 0.0f) {
|
|
return false;
|
|
}
|
|
|
|
beta = p1 / diag;
|
|
|
|
(*this)[i][i] = diag;
|
|
|
|
for (j = i + 1; j < numColumns; j++) {
|
|
|
|
d = (*this)[i][j];
|
|
|
|
d += p0 * z[j];
|
|
z[j] -= beta * d;
|
|
|
|
(*this)[i][j] = d;
|
|
}
|
|
|
|
for (j = i + 1; j < numRows; j++) {
|
|
|
|
d = (*this)[j][i];
|
|
|
|
y[j] -= p0 * d;
|
|
d += beta * y[j];
|
|
|
|
(*this)[j][i] = d;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LU_UpdateRowColumn
|
|
|
|
Updates the in-place LU factorization to obtain the factors for the matrix:
|
|
|
|
[ 0 a 0 ]
|
|
LU + [ d b e ]
|
|
[ 0 c 0 ]
|
|
|
|
where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1], d = w[0,r-1], w[r] = 0.0f, e = w[r+1,numColumns-1]
|
|
============
|
|
*/
|
|
bool idMatX::LU_UpdateRowColumn(const idVecX& v, const idVecX& w, int r, int* index) {
|
|
#if 0
|
|
|
|
idVecX s;
|
|
|
|
assert(v.GetSize() >= numColumns);
|
|
assert(w.GetSize() >= numRows);
|
|
assert(r >= 0 && r < numRows && r < numColumns);
|
|
assert(w[r] == 0.0f);
|
|
|
|
s.SetData(Max(numRows, numColumns), VECX_ALLOCA(Max(numRows, numColumns)));
|
|
s.Zero();
|
|
s[r] = 1.0f;
|
|
|
|
if (!LU_UpdateRankOne(v, s, 1.0f, index)) {
|
|
return false;
|
|
}
|
|
if (!LU_UpdateRankOne(s, w, 1.0f, index)) {
|
|
return false;
|
|
}
|
|
return true;
|
|
|
|
#else
|
|
|
|
int i, j, min, max, rp;
|
|
float* y0, * y1, * z0, * z1;
|
|
double diag, beta0, beta1, p0, p1, q0, q1, d;
|
|
|
|
assert(v.GetSize() >= numColumns);
|
|
assert(w.GetSize() >= numRows);
|
|
assert(r >= 0 && r < numColumns && r < numRows);
|
|
assert(w[r] == 0.0f);
|
|
|
|
y0 = (float*)_alloca16(v.GetSize() * sizeof(float));
|
|
z0 = (float*)_alloca16(w.GetSize() * sizeof(float));
|
|
y1 = (float*)_alloca16(v.GetSize() * sizeof(float));
|
|
z1 = (float*)_alloca16(w.GetSize() * sizeof(float));
|
|
|
|
if (index != NULL) {
|
|
for (i = 0; i < numRows; i++) {
|
|
y0[i] = v[index[i]];
|
|
}
|
|
rp = r;
|
|
for (i = 0; i < numRows; i++) {
|
|
if (index[i] == r) {
|
|
rp = i;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
else {
|
|
memcpy(y0, v.ToFloatPtr(), v.GetSize() * sizeof(float));
|
|
rp = r;
|
|
}
|
|
|
|
memset(y1, 0, v.GetSize() * sizeof(float));
|
|
y1[rp] = 1.0f;
|
|
|
|
memset(z0, 0, w.GetSize() * sizeof(float));
|
|
z0[r] = 1.0f;
|
|
|
|
memcpy(z1, w.ToFloatPtr(), w.GetSize() * sizeof(float));
|
|
|
|
// update the beginning of the to be updated row and column
|
|
min = Min(r, rp);
|
|
for (i = 0; i < min; i++) {
|
|
p0 = y0[i];
|
|
beta1 = z1[i] / (*this)[i][i];
|
|
|
|
(*this)[i][r] += p0;
|
|
for (j = i + 1; j < numColumns; j++) {
|
|
z1[j] -= beta1 * (*this)[i][j];
|
|
}
|
|
for (j = i + 1; j < numRows; j++) {
|
|
y0[j] -= p0 * (*this)[j][i];
|
|
}
|
|
(*this)[rp][i] += beta1;
|
|
}
|
|
|
|
// update the lower right corner starting at r,r
|
|
max = Min(numRows, numColumns);
|
|
for (i = min; i < max; i++) {
|
|
diag = (*this)[i][i];
|
|
|
|
p0 = y0[i];
|
|
p1 = z0[i];
|
|
diag += p0 * p1;
|
|
|
|
if (diag == 0.0f) {
|
|
return false;
|
|
}
|
|
|
|
beta0 = p1 / diag;
|
|
|
|
q0 = y1[i];
|
|
q1 = z1[i];
|
|
diag += q0 * q1;
|
|
|
|
if (diag == 0.0f) {
|
|
return false;
|
|
}
|
|
|
|
beta1 = q1 / diag;
|
|
|
|
(*this)[i][i] = diag;
|
|
|
|
for (j = i + 1; j < numColumns; j++) {
|
|
|
|
d = (*this)[i][j];
|
|
|
|
d += p0 * z0[j];
|
|
z0[j] -= beta0 * d;
|
|
|
|
d += q0 * z1[j];
|
|
z1[j] -= beta1 * d;
|
|
|
|
(*this)[i][j] = d;
|
|
}
|
|
|
|
for (j = i + 1; j < numRows; j++) {
|
|
|
|
d = (*this)[j][i];
|
|
|
|
y0[j] -= p0 * d;
|
|
d += beta0 * y0[j];
|
|
|
|
y1[j] -= q0 * d;
|
|
d += beta1 * y1[j];
|
|
|
|
(*this)[j][i] = d;
|
|
}
|
|
}
|
|
return true;
|
|
|
|
#endif
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LU_UpdateIncrement
|
|
|
|
Updates the in-place LU factorization to obtain the factors for the matrix:
|
|
|
|
[ A a ]
|
|
[ c b ]
|
|
|
|
where: a = v[0,numRows-1], b = v[numRows], c = w[0,numColumns-1], w[numColumns] = 0
|
|
============
|
|
*/
|
|
bool idMatX::LU_UpdateIncrement(const idVecX& v, const idVecX& w, int* index) {
|
|
int i, j;
|
|
float sum;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(v.GetSize() >= numRows + 1);
|
|
assert(w.GetSize() >= numColumns + 1);
|
|
|
|
ChangeSize(numRows + 1, numColumns + 1, true);
|
|
|
|
// add row to L
|
|
for (i = 0; i < numRows - 1; i++) {
|
|
sum = w[i];
|
|
for (j = 0; j < i; j++) {
|
|
sum -= (*this)[numRows - 1][j] * (*this)[j][i];
|
|
}
|
|
(*this)[numRows - 1][i] = sum / (*this)[i][i];
|
|
}
|
|
|
|
// add row to the permutation index
|
|
if (index != NULL) {
|
|
index[numRows - 1] = numRows - 1;
|
|
}
|
|
|
|
// add column to U
|
|
for (i = 0; i < numRows; i++) {
|
|
if (index != NULL) {
|
|
sum = v[index[i]];
|
|
}
|
|
else {
|
|
sum = v[i];
|
|
}
|
|
for (j = 0; j < i; j++) {
|
|
sum -= (*this)[i][j] * (*this)[j][numRows - 1];
|
|
}
|
|
(*this)[i][numRows - 1] = sum;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LU_UpdateDecrement
|
|
|
|
Updates the in-place LU factorization to obtain the factors for the matrix with row r and column r removed.
|
|
v and w should store the column and row of the original matrix respectively.
|
|
If index != NULL then u should store row index[r] of the original matrix. If index == NULL then u = w.
|
|
============
|
|
*/
|
|
bool idMatX::LU_UpdateDecrement(const idVecX& v, const idVecX& w, const idVecX& u, int r, int* index) {
|
|
int i, p;
|
|
idVecX v1, w1;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(v.GetSize() >= numColumns);
|
|
assert(w.GetSize() >= numRows);
|
|
assert(r >= 0 && r < numRows && r < numColumns);
|
|
|
|
v1.SetData(numRows, VECX_ALLOCA(numRows));
|
|
w1.SetData(numRows, VECX_ALLOCA(numRows));
|
|
|
|
if (index != NULL) {
|
|
|
|
// find the pivot row
|
|
for (p = i = 0; i < numRows; i++) {
|
|
if (index[i] == r) {
|
|
p = i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
// update the row and column to identity
|
|
v1 = -v;
|
|
w1 = -u;
|
|
|
|
if (p != r) {
|
|
idSwap(v1[index[r]], v1[index[p]]);
|
|
idSwap(index[r], index[p]);
|
|
}
|
|
|
|
v1[r] += 1.0f;
|
|
w1[r] = 0.0f;
|
|
|
|
if (!LU_UpdateRowColumn(v1, w1, r, index)) {
|
|
return false;
|
|
}
|
|
|
|
if (p != r) {
|
|
|
|
if (idMath::Fabs(u[p]) < 1e-4f) {
|
|
// NOTE: an additional row interchange is required for numerical stability
|
|
}
|
|
|
|
// move row index[r] of the original matrix to row index[p] of the original matrix
|
|
v1.Zero();
|
|
v1[index[p]] = 1.0f;
|
|
w1 = u - w;
|
|
|
|
if (!LU_UpdateRankOne(v1, w1, 1.0f, index)) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// remove the row from the permutation index
|
|
for (i = r; i < numRows - 1; i++) {
|
|
index[i] = index[i + 1];
|
|
}
|
|
for (i = 0; i < numRows - 1; i++) {
|
|
if (index[i] > r) {
|
|
index[i]--;
|
|
}
|
|
}
|
|
|
|
}
|
|
else {
|
|
|
|
v1 = -v;
|
|
w1 = -w;
|
|
v1[r] += 1.0f;
|
|
w1[r] = 0.0f;
|
|
|
|
if (!LU_UpdateRowColumn(v1, w1, r, index)) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// physically remove the row and column
|
|
Update_Decrement(r);
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LU_Solve
|
|
|
|
Solve Ax = b with A factored in-place as: LU
|
|
============
|
|
*/
|
|
void idMatX::LU_Solve(idVecX& x, const idVecX& b, const int* index) const {
|
|
int i, j;
|
|
#ifdef QUAKE4
|
|
float sum;
|
|
#else
|
|
double sum;
|
|
#endif
|
|
|
|
assert(x.GetSize() == numColumns && b.GetSize() == numRows);
|
|
|
|
// solve L
|
|
for (i = 0; i < numRows; i++) {
|
|
if (index != NULL) {
|
|
sum = b[index[i]];
|
|
}
|
|
else {
|
|
sum = b[i];
|
|
}
|
|
for (j = 0; j < i; j++) {
|
|
sum -= (*this)[i][j] * x[j];
|
|
}
|
|
x[i] = sum;
|
|
}
|
|
|
|
// solve U
|
|
for (i = numRows - 1; i >= 0; i--) {
|
|
sum = x[i];
|
|
for (j = i + 1; j < numRows; j++) {
|
|
sum -= (*this)[i][j] * x[j];
|
|
}
|
|
x[i] = sum / (*this)[i][i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LU_Inverse
|
|
|
|
Calculates the inverse of the matrix which is factored in-place as LU
|
|
============
|
|
*/
|
|
void idMatX::LU_Inverse(idMatX& inv, const int* index) const {
|
|
int i, j;
|
|
idVecX x, b;
|
|
|
|
assert(numRows == numColumns);
|
|
|
|
x.SetData(numRows, VECX_ALLOCA(numRows));
|
|
b.SetData(numRows, VECX_ALLOCA(numRows));
|
|
b.Zero();
|
|
inv.SetSize(numRows, numColumns);
|
|
|
|
for (i = 0; i < numRows; i++) {
|
|
|
|
b[i] = 1.0f;
|
|
LU_Solve(x, b, index);
|
|
for (j = 0; j < numRows; j++) {
|
|
inv[j][i] = x[j];
|
|
}
|
|
b[i] = 0.0f;
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LU_UnpackFactors
|
|
|
|
Unpacks the in-place LU factorization.
|
|
============
|
|
*/
|
|
void idMatX::LU_UnpackFactors(idMatX& L, idMatX& U) const {
|
|
int i, j;
|
|
|
|
L.Zero(numRows, numColumns);
|
|
U.Zero(numRows, numColumns);
|
|
for (i = 0; i < numRows; i++) {
|
|
for (j = 0; j < i; j++) {
|
|
L[i][j] = (*this)[i][j];
|
|
}
|
|
L[i][i] = 1.0f;
|
|
for (j = i; j < numColumns; j++) {
|
|
U[i][j] = (*this)[i][j];
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LU_MultiplyFactors
|
|
|
|
Multiplies the factors of the in-place LU factorization to form the original matrix.
|
|
============
|
|
*/
|
|
void idMatX::LU_MultiplyFactors(idMatX& m, const int* index) const {
|
|
int r, rp, i, j;
|
|
double sum;
|
|
|
|
m.SetSize(numRows, numColumns);
|
|
|
|
for (r = 0; r < numRows; r++) {
|
|
|
|
if (index != NULL) {
|
|
rp = index[r];
|
|
}
|
|
else {
|
|
rp = r;
|
|
}
|
|
|
|
// calculate row of matrix
|
|
for (i = 0; i < numColumns; i++) {
|
|
if (i >= r) {
|
|
sum = (*this)[r][i];
|
|
}
|
|
else {
|
|
sum = 0.0f;
|
|
}
|
|
for (j = 0; j <= i && j < r; j++) {
|
|
sum += (*this)[r][j] * (*this)[j][i];
|
|
}
|
|
m[rp][i] = sum;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QR_Factor
|
|
|
|
in-place factorization: QR
|
|
Q is an orthogonal matrix represented as a product of Householder matrices stored in the lower triangle and c.
|
|
R is a triangular matrix stored in the upper triangle except for the diagonal elements which are stored in d.
|
|
The initial matrix has to be square.
|
|
============
|
|
*/
|
|
bool idMatX::QR_Factor(idVecX& c, idVecX& d) {
|
|
int i, j, k;
|
|
double scale, s, t, sum;
|
|
bool singular = false;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(c.GetSize() >= numRows && d.GetSize() >= numRows);
|
|
|
|
for (k = 0; k < numRows - 1; k++) {
|
|
|
|
scale = 0.0f;
|
|
for (i = k; i < numRows; i++) {
|
|
s = idMath::Fabs((*this)[i][k]);
|
|
if (s > scale) {
|
|
scale = s;
|
|
}
|
|
}
|
|
if (scale == 0.0f) {
|
|
singular = true;
|
|
c[k] = d[k] = 0.0f;
|
|
}
|
|
else {
|
|
|
|
s = 1.0f / scale;
|
|
for (i = k; i < numRows; i++) {
|
|
(*this)[i][k] *= s;
|
|
}
|
|
|
|
sum = 0.0f;
|
|
for (i = k; i < numRows; i++) {
|
|
s = (*this)[i][k];
|
|
sum += s * s;
|
|
}
|
|
|
|
s = idMath::Sqrt(sum);
|
|
if ((*this)[k][k] < 0.0f) {
|
|
s = -s;
|
|
}
|
|
(*this)[k][k] += s;
|
|
c[k] = s * (*this)[k][k];
|
|
d[k] = -scale * s;
|
|
|
|
for (j = k + 1; j < numRows; j++) {
|
|
|
|
sum = 0.0f;
|
|
for (i = k; i < numRows; i++) {
|
|
sum += (*this)[i][k] * (*this)[i][j];
|
|
}
|
|
t = sum / c[k];
|
|
for (i = k; i < numRows; i++) {
|
|
(*this)[i][j] -= t * (*this)[i][k];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
d[numRows - 1] = (*this)[(numRows - 1)][(numRows - 1)];
|
|
if (d[numRows - 1] == 0.0f) {
|
|
singular = true;
|
|
}
|
|
|
|
return !singular;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QR_Rotate
|
|
|
|
Performs a Jacobi rotation on the rows i and i+1 of the unpacked QR factors.
|
|
============
|
|
*/
|
|
void idMatX::QR_Rotate(idMatX& R, int i, float a, float b) {
|
|
int j;
|
|
float f, c, s, w, y;
|
|
|
|
if (a == 0.0f) {
|
|
c = 0.0f;
|
|
s = (b >= 0.0f) ? 1.0f : -1.0f;
|
|
}
|
|
else if (idMath::Fabs(a) > idMath::Fabs(b)) {
|
|
f = b / a;
|
|
c = idMath::Fabs(1.0f / idMath::Sqrt(1.0f + f * f));
|
|
if (a < 0.0f) {
|
|
c = -c;
|
|
}
|
|
s = f * c;
|
|
}
|
|
else {
|
|
f = a / b;
|
|
s = idMath::Fabs(1.0f / idMath::Sqrt(1.0f + f * f));
|
|
if (b < 0.0f) {
|
|
s = -s;
|
|
}
|
|
c = f * s;
|
|
}
|
|
for (j = i; j < numRows; j++) {
|
|
y = R[i][j];
|
|
w = R[i + 1][j];
|
|
R[i][j] = c * y - s * w;
|
|
R[i + 1][j] = s * y + c * w;
|
|
}
|
|
for (j = 0; j < numRows; j++) {
|
|
y = (*this)[j][i];
|
|
w = (*this)[j][i + 1];
|
|
(*this)[j][i] = c * y - s * w;
|
|
(*this)[j][i + 1] = s * y + c * w;
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QR_UpdateRankOne
|
|
|
|
Updates the unpacked QR factorization to obtain the factors for the matrix: QR + alpha * v * w'
|
|
============
|
|
*/
|
|
bool idMatX::QR_UpdateRankOne(idMatX& R, const idVecX& v, const idVecX& w, float alpha) {
|
|
int i, k;
|
|
float f;
|
|
idVecX u;
|
|
|
|
assert(v.GetSize() >= numColumns);
|
|
assert(w.GetSize() >= numRows);
|
|
|
|
u.SetData(v.GetSize(), VECX_ALLOCA(v.GetSize()));
|
|
TransposeMultiply(u, v);
|
|
u *= alpha;
|
|
|
|
for (k = v.GetSize() - 1; k > 0; k--) {
|
|
if (u[k] != 0.0f) {
|
|
break;
|
|
}
|
|
}
|
|
for (i = k - 1; i >= 0; i--) {
|
|
QR_Rotate(R, i, u[i], -u[i + 1]);
|
|
if (u[i] == 0.0f) {
|
|
u[i] = idMath::Fabs(u[i + 1]);
|
|
}
|
|
else if (idMath::Fabs(u[i]) > idMath::Fabs(u[i + 1])) {
|
|
f = u[i + 1] / u[i];
|
|
u[i] = idMath::Fabs(u[i]) * idMath::Sqrt(1.0f + f * f);
|
|
}
|
|
else {
|
|
f = u[i] / u[i + 1];
|
|
u[i] = idMath::Fabs(u[i + 1]) * idMath::Sqrt(1.0f + f * f);
|
|
}
|
|
}
|
|
for (i = 0; i < v.GetSize(); i++) {
|
|
R[0][i] += u[0] * w[i];
|
|
}
|
|
for (i = 0; i < k; i++) {
|
|
QR_Rotate(R, i, -R[i][i], R[i + 1][i]);
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QR_UpdateRowColumn
|
|
|
|
Updates the unpacked QR factorization to obtain the factors for the matrix:
|
|
|
|
[ 0 a 0 ]
|
|
QR + [ d b e ]
|
|
[ 0 c 0 ]
|
|
|
|
where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1], d = w[0,r-1], w[r] = 0.0f, e = w[r+1,numColumns-1]
|
|
============
|
|
*/
|
|
bool idMatX::QR_UpdateRowColumn(idMatX& R, const idVecX& v, const idVecX& w, int r) {
|
|
idVecX s;
|
|
|
|
assert(v.GetSize() >= numColumns);
|
|
assert(w.GetSize() >= numRows);
|
|
assert(r >= 0 && r < numRows && r < numColumns);
|
|
assert(w[r] == 0.0f);
|
|
|
|
s.SetData(Max(numRows, numColumns), VECX_ALLOCA(Max(numRows, numColumns)));
|
|
s.Zero();
|
|
s[r] = 1.0f;
|
|
|
|
if (!QR_UpdateRankOne(R, v, s, 1.0f)) {
|
|
return false;
|
|
}
|
|
if (!QR_UpdateRankOne(R, s, w, 1.0f)) {
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QR_UpdateIncrement
|
|
|
|
Updates the unpacked QR factorization to obtain the factors for the matrix:
|
|
|
|
[ A a ]
|
|
[ c b ]
|
|
|
|
where: a = v[0,numRows-1], b = v[numRows], c = w[0,numColumns-1], w[numColumns] = 0
|
|
============
|
|
*/
|
|
bool idMatX::QR_UpdateIncrement(idMatX& R, const idVecX& v, const idVecX& w) {
|
|
idVecX v2;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(v.GetSize() >= numRows + 1);
|
|
assert(w.GetSize() >= numColumns + 1);
|
|
|
|
ChangeSize(numRows + 1, numColumns + 1, true);
|
|
(*this)[numRows - 1][numRows - 1] = 1.0f;
|
|
|
|
R.ChangeSize(R.numRows + 1, R.numColumns + 1, true);
|
|
R[R.numRows - 1][R.numRows - 1] = 1.0f;
|
|
|
|
v2.SetData(numRows, VECX_ALLOCA(numRows));
|
|
v2 = v;
|
|
v2[numRows - 1] -= 1.0f;
|
|
|
|
return QR_UpdateRowColumn(R, v2, w, numRows - 1);
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QR_UpdateDecrement
|
|
|
|
Updates the unpacked QR factorization to obtain the factors for the matrix with row r and column r removed.
|
|
v and w should store the column and row of the original matrix respectively.
|
|
============
|
|
*/
|
|
bool idMatX::QR_UpdateDecrement(idMatX& R, const idVecX& v, const idVecX& w, int r) {
|
|
idVecX v1, w1;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(v.GetSize() >= numRows);
|
|
assert(w.GetSize() >= numColumns);
|
|
assert(r >= 0 && r < numRows && r < numColumns);
|
|
|
|
v1.SetData(numRows, VECX_ALLOCA(numRows));
|
|
w1.SetData(numRows, VECX_ALLOCA(numRows));
|
|
|
|
// update the row and column to identity
|
|
v1 = -v;
|
|
w1 = -w;
|
|
v1[r] += 1.0f;
|
|
w1[r] = 0.0f;
|
|
|
|
if (!QR_UpdateRowColumn(R, v1, w1, r)) {
|
|
return false;
|
|
}
|
|
|
|
// physically remove the row and column
|
|
Update_Decrement(r);
|
|
R.Update_Decrement(r);
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QR_Solve
|
|
|
|
Solve Ax = b with A factored in-place as: QR
|
|
============
|
|
*/
|
|
void idMatX::QR_Solve(idVecX& x, const idVecX& b, const idVecX& c, const idVecX& d) const {
|
|
int i, j;
|
|
double sum, t;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(x.GetSize() >= numRows && b.GetSize() >= numRows);
|
|
assert(c.GetSize() >= numRows && d.GetSize() >= numRows);
|
|
|
|
for (i = 0; i < numRows; i++) {
|
|
x[i] = b[i];
|
|
}
|
|
|
|
// multiply b with transpose of Q
|
|
for (i = 0; i < numRows - 1; i++) {
|
|
|
|
sum = 0.0f;
|
|
for (j = i; j < numRows; j++) {
|
|
sum += (*this)[j][i] * x[j];
|
|
}
|
|
t = sum / c[i];
|
|
for (j = i; j < numRows; j++) {
|
|
x[j] -= t * (*this)[j][i];
|
|
}
|
|
}
|
|
|
|
// backsubstitution with R
|
|
for (i = numRows - 1; i >= 0; i--) {
|
|
|
|
sum = x[i];
|
|
for (j = i + 1; j < numRows; j++) {
|
|
sum -= (*this)[i][j] * x[j];
|
|
}
|
|
x[i] = sum / d[i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QR_Solve
|
|
|
|
Solve Ax = b with A factored as: QR
|
|
============
|
|
*/
|
|
void idMatX::QR_Solve(idVecX& x, const idVecX& b, const idMatX& R) const {
|
|
int i, j;
|
|
double sum;
|
|
|
|
assert(numRows == numColumns);
|
|
|
|
// multiply b with transpose of Q
|
|
TransposeMultiply(x, b);
|
|
|
|
// backsubstitution with R
|
|
for (i = numRows - 1; i >= 0; i--) {
|
|
|
|
sum = x[i];
|
|
for (j = i + 1; j < numRows; j++) {
|
|
sum -= R[i][j] * x[j];
|
|
}
|
|
x[i] = sum / R[i][i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QR_Inverse
|
|
|
|
Calculates the inverse of the matrix which is factored in-place as: QR
|
|
============
|
|
*/
|
|
void idMatX::QR_Inverse(idMatX& inv, const idVecX& c, const idVecX& d) const {
|
|
int i, j;
|
|
idVecX x, b;
|
|
|
|
assert(numRows == numColumns);
|
|
|
|
x.SetData(numRows, VECX_ALLOCA(numRows));
|
|
b.SetData(numRows, VECX_ALLOCA(numRows));
|
|
b.Zero();
|
|
inv.SetSize(numRows, numColumns);
|
|
|
|
for (i = 0; i < numRows; i++) {
|
|
|
|
b[i] = 1.0f;
|
|
QR_Solve(x, b, c, d);
|
|
for (j = 0; j < numRows; j++) {
|
|
inv[j][i] = x[j];
|
|
}
|
|
b[i] = 0.0f;
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QR_UnpackFactors
|
|
|
|
Unpacks the in-place QR factorization.
|
|
============
|
|
*/
|
|
void idMatX::QR_UnpackFactors(idMatX& Q, idMatX& R, const idVecX& c, const idVecX& d) const {
|
|
int i, j, k;
|
|
double sum;
|
|
|
|
Q.Identity(numRows, numColumns);
|
|
for (i = 0; i < numColumns - 1; i++) {
|
|
if (c[i] == 0.0f) {
|
|
continue;
|
|
}
|
|
for (j = 0; j < numRows; j++) {
|
|
sum = 0.0f;
|
|
for (k = i; k < numColumns; k++) {
|
|
sum += (*this)[k][i] * Q[j][k];
|
|
}
|
|
sum /= c[i];
|
|
for (k = i; k < numColumns; k++) {
|
|
Q[j][k] -= sum * (*this)[k][i];
|
|
}
|
|
}
|
|
}
|
|
|
|
R.Zero(numRows, numColumns);
|
|
for (i = 0; i < numRows; i++) {
|
|
R[i][i] = d[i];
|
|
for (j = i + 1; j < numColumns; j++) {
|
|
R[i][j] = (*this)[i][j];
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QR_MultiplyFactors
|
|
|
|
Multiplies the factors of the in-place QR factorization to form the original matrix.
|
|
============
|
|
*/
|
|
void idMatX::QR_MultiplyFactors(idMatX& m, const idVecX& c, const idVecX& d) const {
|
|
int i, j, k;
|
|
double sum;
|
|
idMatX Q;
|
|
|
|
Q.Identity(numRows, numColumns);
|
|
for (i = 0; i < numColumns - 1; i++) {
|
|
if (c[i] == 0.0f) {
|
|
continue;
|
|
}
|
|
for (j = 0; j < numRows; j++) {
|
|
sum = 0.0f;
|
|
for (k = i; k < numColumns; k++) {
|
|
sum += (*this)[k][i] * Q[j][k];
|
|
}
|
|
sum /= c[i];
|
|
for (k = i; k < numColumns; k++) {
|
|
Q[j][k] -= sum * (*this)[k][i];
|
|
}
|
|
}
|
|
}
|
|
|
|
for (i = 0; i < numRows; i++) {
|
|
for (j = 0; j < numColumns; j++) {
|
|
sum = Q[i][j] * d[i];
|
|
for (k = 0; k < i; k++) {
|
|
sum += Q[i][k] * (*this)[j][k];
|
|
}
|
|
m[i][j] = sum;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Pythag
|
|
|
|
Computes (a^2 + b^2)^1/2 without underflow or overflow.
|
|
============
|
|
*/
|
|
float idMatX::Pythag(float a, float b) const {
|
|
double at, bt, ct;
|
|
|
|
at = idMath::Fabs(a);
|
|
bt = idMath::Fabs(b);
|
|
if (at > bt) {
|
|
ct = bt / at;
|
|
return at * idMath::Sqrt(1.0f + ct * ct);
|
|
}
|
|
else {
|
|
if (bt) {
|
|
ct = at / bt;
|
|
return bt * idMath::Sqrt(1.0f + ct * ct);
|
|
}
|
|
else {
|
|
return 0.0f;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::SVD_BiDiag
|
|
============
|
|
*/
|
|
void idMatX::SVD_BiDiag(idVecX& w, idVecX& rv1, float& anorm) {
|
|
int i, j, k, l;
|
|
double f, h, r, g, s, scale;
|
|
|
|
anorm = 0.0f;
|
|
g = s = scale = 0.0f;
|
|
for (i = 0; i < numColumns; i++) {
|
|
l = i + 1;
|
|
rv1[i] = scale * g;
|
|
g = s = scale = 0.0f;
|
|
if (i < numRows) {
|
|
for (k = i; k < numRows; k++) {
|
|
scale += idMath::Fabs((*this)[k][i]);
|
|
}
|
|
if (scale) {
|
|
for (k = i; k < numRows; k++) {
|
|
(*this)[k][i] /= scale;
|
|
s += (*this)[k][i] * (*this)[k][i];
|
|
}
|
|
f = (*this)[i][i];
|
|
g = idMath::Sqrt(s);
|
|
if (f >= 0.0f) {
|
|
g = -g;
|
|
}
|
|
h = f * g - s;
|
|
(*this)[i][i] = f - g;
|
|
if (i != (numColumns - 1)) {
|
|
for (j = l; j < numColumns; j++) {
|
|
for (s = 0.0f, k = i; k < numRows; k++) {
|
|
s += (*this)[k][i] * (*this)[k][j];
|
|
}
|
|
f = s / h;
|
|
for (k = i; k < numRows; k++) {
|
|
(*this)[k][j] += f * (*this)[k][i];
|
|
}
|
|
}
|
|
}
|
|
for (k = i; k < numRows; k++) {
|
|
(*this)[k][i] *= scale;
|
|
}
|
|
}
|
|
}
|
|
w[i] = scale * g;
|
|
g = s = scale = 0.0f;
|
|
if (i < numRows && i != (numColumns - 1)) {
|
|
for (k = l; k < numColumns; k++) {
|
|
scale += idMath::Fabs((*this)[i][k]);
|
|
}
|
|
if (scale) {
|
|
for (k = l; k < numColumns; k++) {
|
|
(*this)[i][k] /= scale;
|
|
s += (*this)[i][k] * (*this)[i][k];
|
|
}
|
|
f = (*this)[i][l];
|
|
g = idMath::Sqrt(s);
|
|
if (f >= 0.0f) {
|
|
g = -g;
|
|
}
|
|
h = 1.0f / (f * g - s);
|
|
(*this)[i][l] = f - g;
|
|
for (k = l; k < numColumns; k++) {
|
|
rv1[k] = (*this)[i][k] * h;
|
|
}
|
|
if (i != (numRows - 1)) {
|
|
for (j = l; j < numRows; j++) {
|
|
for (s = 0.0f, k = l; k < numColumns; k++) {
|
|
s += (*this)[j][k] * (*this)[i][k];
|
|
}
|
|
for (k = l; k < numColumns; k++) {
|
|
(*this)[j][k] += s * rv1[k];
|
|
}
|
|
}
|
|
}
|
|
for (k = l; k < numColumns; k++) {
|
|
(*this)[i][k] *= scale;
|
|
}
|
|
}
|
|
}
|
|
r = idMath::Fabs(w[i]) + idMath::Fabs(rv1[i]);
|
|
if (r > anorm) {
|
|
anorm = r;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::SVD_InitialWV
|
|
============
|
|
*/
|
|
void idMatX::SVD_InitialWV(idVecX& w, idMatX& V, idVecX& rv1) {
|
|
int i, j, k, l;
|
|
double f, g, s;
|
|
|
|
g = 0.0f;
|
|
for (i = (numColumns - 1); i >= 0; i--) {
|
|
l = i + 1;
|
|
if (i < (numColumns - 1)) {
|
|
if (g) {
|
|
for (j = l; j < numColumns; j++) {
|
|
V[j][i] = ((*this)[i][j] / (*this)[i][l]) / g;
|
|
}
|
|
// double division to reduce underflow
|
|
for (j = l; j < numColumns; j++) {
|
|
for (s = 0.0f, k = l; k < numColumns; k++) {
|
|
s += (*this)[i][k] * V[k][j];
|
|
}
|
|
for (k = l; k < numColumns; k++) {
|
|
V[k][j] += s * V[k][i];
|
|
}
|
|
}
|
|
}
|
|
for (j = l; j < numColumns; j++) {
|
|
V[i][j] = V[j][i] = 0.0f;
|
|
}
|
|
}
|
|
V[i][i] = 1.0f;
|
|
g = rv1[i];
|
|
}
|
|
for (i = numColumns - 1; i >= 0; i--) {
|
|
l = i + 1;
|
|
g = w[i];
|
|
if (i < (numColumns - 1)) {
|
|
for (j = l; j < numColumns; j++) {
|
|
(*this)[i][j] = 0.0f;
|
|
}
|
|
}
|
|
if (g) {
|
|
g = 1.0f / g;
|
|
if (i != (numColumns - 1)) {
|
|
for (j = l; j < numColumns; j++) {
|
|
for (s = 0.0f, k = l; k < numRows; k++) {
|
|
s += (*this)[k][i] * (*this)[k][j];
|
|
}
|
|
f = (s / (*this)[i][i]) * g;
|
|
for (k = i; k < numRows; k++) {
|
|
(*this)[k][j] += f * (*this)[k][i];
|
|
}
|
|
}
|
|
}
|
|
for (j = i; j < numRows; j++) {
|
|
(*this)[j][i] *= g;
|
|
}
|
|
}
|
|
else {
|
|
for (j = i; j < numRows; j++) {
|
|
(*this)[j][i] = 0.0f;
|
|
}
|
|
}
|
|
(*this)[i][i] += 1.0f;
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::SVD_Factor
|
|
|
|
in-place factorization: U * Diag(w) * V.Transpose()
|
|
known as the Singular Value Decomposition.
|
|
U is a column-orthogonal matrix which overwrites the original matrix.
|
|
w is a diagonal matrix with all elements >= 0 which are the singular values.
|
|
V is the transpose of an orthogonal matrix.
|
|
============
|
|
*/
|
|
bool idMatX::SVD_Factor(idVecX& w, idMatX& V) {
|
|
int flag, i, its, j, jj, k, l, nm;
|
|
double c, f, h, s, x, y, z, r, g = 0.0f;
|
|
float anorm = 0.0f;
|
|
idVecX rv1;
|
|
|
|
if (numRows < numColumns) {
|
|
return false;
|
|
}
|
|
|
|
rv1.SetData(numColumns, VECX_ALLOCA(numColumns));
|
|
rv1.Zero();
|
|
w.Zero(numColumns);
|
|
V.Zero(numColumns, numColumns);
|
|
|
|
SVD_BiDiag(w, rv1, anorm);
|
|
SVD_InitialWV(w, V, rv1);
|
|
|
|
for (k = numColumns - 1; k >= 0; k--) {
|
|
for (its = 1; its <= 30; its++) {
|
|
flag = 1;
|
|
nm = 0;
|
|
for (l = k; l >= 0; l--) {
|
|
nm = l - 1;
|
|
if ((idMath::Fabs(rv1[l]) + anorm) == anorm /* idMath::Fabs( rv1[l] ) < idMath::FLT_EPSILON */) {
|
|
flag = 0;
|
|
break;
|
|
}
|
|
if ((idMath::Fabs(w[nm]) + anorm) == anorm /* idMath::Fabs( w[nm] ) < idMath::FLT_EPSILON */) {
|
|
break;
|
|
}
|
|
}
|
|
if (flag) {
|
|
c = 0.0f;
|
|
s = 1.0f;
|
|
for (i = l; i <= k; i++) {
|
|
f = s * rv1[i];
|
|
|
|
if ((idMath::Fabs(f) + anorm) != anorm /* idMath::Fabs( f ) > idMath::FLT_EPSILON */) {
|
|
g = w[i];
|
|
h = Pythag(f, g);
|
|
w[i] = h;
|
|
h = 1.0f / h;
|
|
c = g * h;
|
|
s = -f * h;
|
|
for (j = 0; j < numRows; j++) {
|
|
y = (*this)[j][nm];
|
|
z = (*this)[j][i];
|
|
(*this)[j][nm] = y * c + z * s;
|
|
(*this)[j][i] = z * c - y * s;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
z = w[k];
|
|
if (l == k) {
|
|
if (z < 0.0f) {
|
|
w[k] = -z;
|
|
for (j = 0; j < numColumns; j++) {
|
|
V[j][k] = -V[j][k];
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
if (its == 30) {
|
|
return false; // no convergence
|
|
}
|
|
x = w[l];
|
|
nm = k - 1;
|
|
y = w[nm];
|
|
g = rv1[nm];
|
|
h = rv1[k];
|
|
f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0f * h * y);
|
|
g = Pythag(f, 1.0f);
|
|
r = (f >= 0.0f ? g : -g);
|
|
f = ((x - z) * (x + z) + h * ((y / (f + r)) - h)) / x;
|
|
c = s = 1.0f;
|
|
for (j = l; j <= nm; j++) {
|
|
i = j + 1;
|
|
g = rv1[i];
|
|
y = w[i];
|
|
h = s * g;
|
|
g = c * g;
|
|
z = Pythag(f, h);
|
|
rv1[j] = z;
|
|
c = f / z;
|
|
s = h / z;
|
|
f = x * c + g * s;
|
|
g = g * c - x * s;
|
|
h = y * s;
|
|
y = y * c;
|
|
for (jj = 0; jj < numColumns; jj++) {
|
|
x = V[jj][j];
|
|
z = V[jj][i];
|
|
V[jj][j] = x * c + z * s;
|
|
V[jj][i] = z * c - x * s;
|
|
}
|
|
z = Pythag(f, h);
|
|
w[j] = z;
|
|
if (z) {
|
|
z = 1.0f / z;
|
|
c = f * z;
|
|
s = h * z;
|
|
}
|
|
f = (c * g) + (s * y);
|
|
x = (c * y) - (s * g);
|
|
for (jj = 0; jj < numRows; jj++) {
|
|
y = (*this)[jj][j];
|
|
z = (*this)[jj][i];
|
|
(*this)[jj][j] = y * c + z * s;
|
|
(*this)[jj][i] = z * c - y * s;
|
|
}
|
|
}
|
|
rv1[l] = 0.0f;
|
|
rv1[k] = f;
|
|
w[k] = x;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::SVD_Solve
|
|
|
|
Solve Ax = b with A factored as: U * Diag(w) * V.Transpose()
|
|
============
|
|
*/
|
|
void idMatX::SVD_Solve(idVecX& x, const idVecX& b, const idVecX& w, const idMatX& V) const {
|
|
int i, j;
|
|
double sum;
|
|
idVecX tmp;
|
|
|
|
assert(x.GetSize() >= numColumns);
|
|
assert(b.GetSize() >= numColumns);
|
|
assert(w.GetSize() == numColumns);
|
|
assert(V.GetNumRows() == numColumns && V.GetNumColumns() == numColumns);
|
|
|
|
tmp.SetData(numColumns, VECX_ALLOCA(numColumns));
|
|
|
|
for (i = 0; i < numColumns; i++) {
|
|
sum = 0.0f;
|
|
#ifdef QUAKE4
|
|
if (w[i] >= idMath::FLOAT_EPSILON) {
|
|
#else
|
|
if (w[i] >= idMath::FLT_EPSILON) {
|
|
#endif
|
|
for (j = 0; j < numRows; j++) {
|
|
sum += (*this)[j][i] * b[j];
|
|
}
|
|
sum /= w[i];
|
|
}
|
|
tmp[i] = sum;
|
|
}
|
|
for (i = 0; i < numColumns; i++) {
|
|
sum = 0.0f;
|
|
for (j = 0; j < numColumns; j++) {
|
|
sum += V[i][j] * tmp[j];
|
|
}
|
|
x[i] = sum;
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::SVD_Inverse
|
|
|
|
Calculates the inverse of the matrix which is factored in-place as: U * Diag(w) * V.Transpose()
|
|
============
|
|
*/
|
|
void idMatX::SVD_Inverse(idMatX & inv, const idVecX & w, const idMatX & V) const {
|
|
int i, j, k;
|
|
double wi, sum;
|
|
idMatX V2;
|
|
|
|
assert(numRows == numColumns);
|
|
|
|
V2 = V;
|
|
|
|
// V * [diag(1/w[i])]
|
|
for (i = 0; i < numRows; i++) {
|
|
wi = w[i];
|
|
#ifdef QUAKE4
|
|
wi = (wi < idMath::FLOAT_EPSILON) ? 0.0f : 1.0f / wi;
|
|
#else
|
|
wi = (wi < idMath::FLT_EPSILON) ? 0.0f : 1.0f / wi;
|
|
#endif
|
|
for (j = 0; j < numColumns; j++) {
|
|
V2[j][i] *= wi;
|
|
}
|
|
}
|
|
|
|
// V * [diag(1/w[i])] * Ut
|
|
for (i = 0; i < numRows; i++) {
|
|
for (j = 0; j < numColumns; j++) {
|
|
sum = V2[i][0] * (*this)[j][0];
|
|
for (k = 1; k < numColumns; k++) {
|
|
sum += V2[i][k] * (*this)[j][k];
|
|
}
|
|
inv[i][j] = sum;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::SVD_MultiplyFactors
|
|
|
|
Multiplies the factors of the in-place SVD factorization to form the original matrix.
|
|
============
|
|
*/
|
|
void idMatX::SVD_MultiplyFactors(idMatX & m, const idVecX & w, const idMatX & V) const {
|
|
int r, i, j;
|
|
double sum;
|
|
|
|
m.SetSize(numRows, V.GetNumRows());
|
|
|
|
for (r = 0; r < numRows; r++) {
|
|
// calculate row of matrix
|
|
#ifdef QUAKE4
|
|
if (w[r] >= idMath::FLOAT_EPSILON) {
|
|
#else
|
|
if (w[r] >= idMath::FLT_EPSILON) {
|
|
#endif
|
|
for (i = 0; i < V.GetNumRows(); i++) {
|
|
sum = 0.0f;
|
|
for (j = 0; j < numColumns; j++) {
|
|
sum += (*this)[r][j] * V[i][j];
|
|
}
|
|
m[r][i] = sum * w[r];
|
|
}
|
|
}
|
|
else {
|
|
for (i = 0; i < V.GetNumRows(); i++) {
|
|
m[r][i] = 0.0f;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Cholesky_Factor
|
|
|
|
in-place Cholesky factorization: LL'
|
|
L is a triangular matrix stored in the lower triangle.
|
|
The upper triangle is not cleared.
|
|
The initial matrix has to be symmetric positive definite.
|
|
============
|
|
*/
|
|
bool idMatX::Cholesky_Factor(void) {
|
|
int i, j, k;
|
|
float* invSqrt;
|
|
double sum;
|
|
|
|
assert(numRows == numColumns);
|
|
|
|
invSqrt = (float*)_alloca16(numRows * sizeof(float));
|
|
|
|
for (i = 0; i < numRows; i++) {
|
|
|
|
for (j = 0; j < i; j++) {
|
|
|
|
sum = (*this)[i][j];
|
|
for (k = 0; k < j; k++) {
|
|
sum -= (*this)[i][k] * (*this)[j][k];
|
|
}
|
|
(*this)[i][j] = sum * invSqrt[j];
|
|
}
|
|
|
|
sum = (*this)[i][i];
|
|
for (k = 0; k < i; k++) {
|
|
sum -= (*this)[i][k] * (*this)[i][k];
|
|
}
|
|
|
|
if (sum <= 0.0f) {
|
|
return false;
|
|
}
|
|
|
|
invSqrt[i] = idMath::InvSqrt(sum);
|
|
(*this)[i][i] = invSqrt[i] * sum;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Cholesky_UpdateRankOne
|
|
|
|
Updates the in-place Cholesky factorization to obtain the factors for the matrix: LL' + alpha * v * v'
|
|
If offset > 0 only the lower right corner starting at (offset, offset) is updated.
|
|
============
|
|
*/
|
|
bool idMatX::Cholesky_UpdateRankOne(const idVecX & v, float alpha, int offset) {
|
|
int i, j;
|
|
float* y;
|
|
double diag, invDiag, diagSqr, newDiag, newDiagSqr, beta, p, d;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(v.GetSize() >= numRows);
|
|
assert(offset >= 0 && offset < numRows);
|
|
|
|
y = (float*)_alloca16(v.GetSize() * sizeof(float));
|
|
memcpy(y, v.ToFloatPtr(), v.GetSize() * sizeof(float));
|
|
|
|
for (i = offset; i < numColumns; i++) {
|
|
p = y[i];
|
|
diag = (*this)[i][i];
|
|
invDiag = 1.0f / diag;
|
|
diagSqr = diag * diag;
|
|
newDiagSqr = diagSqr + alpha * p * p;
|
|
|
|
if (newDiagSqr <= 0.0f) {
|
|
return false;
|
|
}
|
|
|
|
(*this)[i][i] = newDiag = idMath::Sqrt(newDiagSqr);
|
|
|
|
alpha /= newDiagSqr;
|
|
beta = p * alpha;
|
|
alpha *= diagSqr;
|
|
|
|
for (j = i + 1; j < numRows; j++) {
|
|
|
|
d = (*this)[j][i] * invDiag;
|
|
|
|
y[j] -= p * d;
|
|
d += beta * y[j];
|
|
|
|
(*this)[j][i] = d * newDiag;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Cholesky_UpdateRowColumn
|
|
|
|
Updates the in-place Cholesky factorization to obtain the factors for the matrix:
|
|
|
|
[ 0 a 0 ]
|
|
LL' + [ a b c ]
|
|
[ 0 c 0 ]
|
|
|
|
where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1]
|
|
============
|
|
*/
|
|
bool idMatX::Cholesky_UpdateRowColumn(const idVecX & v, int r) {
|
|
int i, j;
|
|
double sum;
|
|
float* original, * y;
|
|
idVecX addSub;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(v.GetSize() >= numRows);
|
|
assert(r >= 0 && r < numRows);
|
|
|
|
addSub.SetData(numColumns, (float*)_alloca16(numColumns * sizeof(float)));
|
|
|
|
if (r == 0) {
|
|
|
|
if (numColumns == 1) {
|
|
sum = (*this)[0][0];
|
|
#ifdef QUAKE4
|
|
sum = (sum * sum) + v[0];
|
|
#else
|
|
double v0 = v[0];
|
|
sum = sum * sum;
|
|
sum = sum + v0;
|
|
#endif
|
|
if (sum <= 0.0f) {
|
|
return false;
|
|
}
|
|
(*this)[0][0] = idMath::Sqrt(sum);
|
|
return true;
|
|
}
|
|
for (i = 0; i < numColumns; i++) {
|
|
addSub[i] = v[i];
|
|
}
|
|
|
|
}
|
|
else {
|
|
|
|
original = (float*)_alloca16(numColumns * sizeof(float));
|
|
y = (float*)_alloca16(numColumns * sizeof(float));
|
|
|
|
// calculate original row/column of matrix
|
|
for (i = 0; i < numRows; i++) {
|
|
sum = 0.0f;
|
|
for (j = 0; j <= i; j++) {
|
|
sum += (*this)[r][j] * (*this)[i][j];
|
|
}
|
|
original[i] = sum;
|
|
}
|
|
|
|
// solve for y in L * y = original + v
|
|
for (i = 0; i < r; i++) {
|
|
sum = original[i] + v[i];
|
|
for (j = 0; j < i; j++) {
|
|
sum -= (*this)[r][j] * (*this)[i][j];
|
|
}
|
|
(*this)[r][i] = sum / (*this)[i][i];
|
|
}
|
|
|
|
// if the last row/column of the matrix is updated
|
|
if (r == numColumns - 1) {
|
|
// only calculate new diagonal
|
|
sum = original[r] + v[r];
|
|
for (j = 0; j < r; j++) {
|
|
sum -= (*this)[r][j] * (*this)[r][j];
|
|
}
|
|
if (sum <= 0.0f) {
|
|
return false;
|
|
}
|
|
(*this)[r][r] = idMath::Sqrt(sum);
|
|
return true;
|
|
}
|
|
|
|
// calculate the row/column to be added to the lower right sub matrix starting at (r, r)
|
|
for (i = r; i < numColumns; i++) {
|
|
sum = 0.0f;
|
|
for (j = 0; j <= r; j++) {
|
|
sum += (*this)[r][j] * (*this)[i][j];
|
|
}
|
|
addSub[i] = v[i] - (sum - original[i]);
|
|
}
|
|
}
|
|
|
|
// add row/column to the lower right sub matrix starting at (r, r)
|
|
|
|
#if 0
|
|
|
|
idVecX v1, v2;
|
|
double d;
|
|
|
|
v1.SetData(numColumns, (float*)_alloca16(numColumns * sizeof(float)));
|
|
v2.SetData(numColumns, (float*)_alloca16(numColumns * sizeof(float)));
|
|
|
|
d = idMath::SQRT_1OVER2;
|
|
v1[r] = (0.5f * addSub[r] + 1.0f) * d;
|
|
v2[r] = (0.5f * addSub[r] - 1.0f) * d;
|
|
for (i = r + 1; i < numColumns; i++) {
|
|
v1[i] = v2[i] = addSub[i] * d;
|
|
}
|
|
|
|
// update
|
|
if (!Cholesky_UpdateRankOne(v1, 1.0f, r)) {
|
|
return false;
|
|
}
|
|
// downdate
|
|
if (!Cholesky_UpdateRankOne(v2, -1.0f, r)) {
|
|
return false;
|
|
}
|
|
|
|
#else
|
|
|
|
float* v1, * v2;
|
|
double diag, invDiag, diagSqr, newDiag, newDiagSqr;
|
|
double alpha1, alpha2, beta1, beta2, p1, p2, d;
|
|
|
|
v1 = (float*)_alloca16(numColumns * sizeof(float));
|
|
v2 = (float*)_alloca16(numColumns * sizeof(float));
|
|
|
|
d = idMath::SQRT_1OVER2;
|
|
v1[r] = (0.5f * addSub[r] + 1.0f) * d;
|
|
v2[r] = (0.5f * addSub[r] - 1.0f) * d;
|
|
for (i = r + 1; i < numColumns; i++) {
|
|
v1[i] = v2[i] = addSub[i] * d;
|
|
}
|
|
|
|
alpha1 = 1.0f;
|
|
alpha2 = -1.0f;
|
|
|
|
// simultaneous update/downdate of the sub matrix starting at (r, r)
|
|
for (i = r; i < numColumns; i++) {
|
|
p1 = v1[i];
|
|
diag = (*this)[i][i];
|
|
invDiag = 1.0f / diag;
|
|
diagSqr = diag * diag;
|
|
newDiagSqr = diagSqr + alpha1 * p1 * p1;
|
|
|
|
if (newDiagSqr <= 0.0f) {
|
|
return false;
|
|
}
|
|
|
|
alpha1 /= newDiagSqr;
|
|
beta1 = p1 * alpha1;
|
|
alpha1 *= diagSqr;
|
|
|
|
p2 = v2[i];
|
|
diagSqr = newDiagSqr;
|
|
newDiagSqr = diagSqr + alpha2 * p2 * p2;
|
|
|
|
if (newDiagSqr <= 0.0f) {
|
|
return false;
|
|
}
|
|
|
|
(*this)[i][i] = newDiag = idMath::Sqrt(newDiagSqr);
|
|
|
|
alpha2 /= newDiagSqr;
|
|
beta2 = p2 * alpha2;
|
|
alpha2 *= diagSqr;
|
|
|
|
for (j = i + 1; j < numRows; j++) {
|
|
|
|
d = (*this)[j][i] * invDiag;
|
|
|
|
v1[j] -= p1 * d;
|
|
d += beta1 * v1[j];
|
|
|
|
v2[j] -= p2 * d;
|
|
d += beta2 * v2[j];
|
|
|
|
(*this)[j][i] = d * newDiag;
|
|
}
|
|
}
|
|
|
|
#endif
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Cholesky_UpdateIncrement
|
|
|
|
Updates the in-place Cholesky factorization to obtain the factors for the matrix:
|
|
|
|
[ A a ]
|
|
[ a b ]
|
|
|
|
where: a = v[0,numRows-1], b = v[numRows]
|
|
============
|
|
*/
|
|
bool idMatX::Cholesky_UpdateIncrement(const idVecX & v) {
|
|
int i, j;
|
|
float* x;
|
|
double sum;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(v.GetSize() >= numRows + 1);
|
|
|
|
ChangeSize(numRows + 1, numColumns + 1, false);
|
|
|
|
x = (float*)_alloca16(numRows * sizeof(float));
|
|
|
|
// solve for x in L * x = v
|
|
for (i = 0; i < numRows - 1; i++) {
|
|
sum = v[i];
|
|
for (j = 0; j < i; j++) {
|
|
sum -= (*this)[i][j] * x[j];
|
|
}
|
|
x[i] = sum / (*this)[i][i];
|
|
}
|
|
|
|
// calculate new row of L and calculate the square of the diagonal entry
|
|
sum = v[numRows - 1];
|
|
for (i = 0; i < numRows - 1; i++) {
|
|
(*this)[numRows - 1][i] = x[i];
|
|
sum -= x[i] * x[i];
|
|
}
|
|
|
|
if (sum <= 0.0f) {
|
|
return false;
|
|
}
|
|
|
|
// store the diagonal entry
|
|
(*this)[numRows - 1][numRows - 1] = idMath::Sqrt(sum);
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Cholesky_UpdateDecrement
|
|
|
|
Updates the in-place Cholesky factorization to obtain the factors for the matrix with row r and column r removed.
|
|
v should store the row of the original matrix.
|
|
============
|
|
*/
|
|
bool idMatX::Cholesky_UpdateDecrement(const idVecX & v, int r) {
|
|
idVecX v1;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(v.GetSize() >= numRows);
|
|
assert(r >= 0 && r < numRows);
|
|
|
|
v1.SetData(numRows, VECX_ALLOCA(numRows));
|
|
|
|
// update the row and column to identity
|
|
v1 = -v;
|
|
v1[r] += 1.0f;
|
|
|
|
#ifdef QUAKE4
|
|
if (!Cholesky_UpdateRowColumn(v1, r)) {
|
|
#else
|
|
// NOTE: msvc compiler bug: the this pointer stored in edi is expected to stay
|
|
// untouched when calling Cholesky_UpdateRowColumn in the if statement
|
|
#if 0
|
|
if (!Cholesky_UpdateRowColumn(v1, r)) {
|
|
#else
|
|
bool ret = Cholesky_UpdateRowColumn(v1, r);
|
|
if (!ret) {
|
|
#endif
|
|
#endif
|
|
return false;
|
|
}
|
|
|
|
// physically remove the row and column
|
|
Update_Decrement(r);
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Cholesky_Solve
|
|
|
|
Solve Ax = b with A factored in-place as: LL'
|
|
============
|
|
*/
|
|
void idMatX::Cholesky_Solve(idVecX & x, const idVecX & b) const {
|
|
int i, j;
|
|
double sum;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(x.GetSize() >= numRows && b.GetSize() >= numRows);
|
|
|
|
// solve L
|
|
for (i = 0; i < numRows; i++) {
|
|
sum = b[i];
|
|
for (j = 0; j < i; j++) {
|
|
sum -= (*this)[i][j] * x[j];
|
|
}
|
|
x[i] = sum / (*this)[i][i];
|
|
}
|
|
|
|
// solve Lt
|
|
for (i = numRows - 1; i >= 0; i--) {
|
|
sum = x[i];
|
|
for (j = i + 1; j < numRows; j++) {
|
|
sum -= (*this)[j][i] * x[j];
|
|
}
|
|
x[i] = sum / (*this)[i][i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Cholesky_Inverse
|
|
|
|
Calculates the inverse of the matrix which is factored in-place as: LL'
|
|
============
|
|
*/
|
|
void idMatX::Cholesky_Inverse(idMatX & inv) const {
|
|
int i, j;
|
|
idVecX x, b;
|
|
|
|
assert(numRows == numColumns);
|
|
|
|
x.SetData(numRows, VECX_ALLOCA(numRows));
|
|
b.SetData(numRows, VECX_ALLOCA(numRows));
|
|
b.Zero();
|
|
inv.SetSize(numRows, numColumns);
|
|
|
|
for (i = 0; i < numRows; i++) {
|
|
|
|
b[i] = 1.0f;
|
|
Cholesky_Solve(x, b);
|
|
for (j = 0; j < numRows; j++) {
|
|
inv[j][i] = x[j];
|
|
}
|
|
b[i] = 0.0f;
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Cholesky_MultiplyFactors
|
|
|
|
Multiplies the factors of the in-place Cholesky factorization to form the original matrix.
|
|
============
|
|
*/
|
|
void idMatX::Cholesky_MultiplyFactors(idMatX & m) const {
|
|
int r, i, j;
|
|
double sum;
|
|
|
|
m.SetSize(numRows, numColumns);
|
|
|
|
for (r = 0; r < numRows; r++) {
|
|
|
|
// calculate row of matrix
|
|
for (i = 0; i < numRows; i++) {
|
|
sum = 0.0f;
|
|
for (j = 0; j <= i && j <= r; j++) {
|
|
sum += (*this)[r][j] * (*this)[i][j];
|
|
}
|
|
m[r][i] = sum;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LDLT_Factor
|
|
|
|
in-place factorization: LDL'
|
|
L is a triangular matrix stored in the lower triangle.
|
|
L has ones on the diagonal that are not stored.
|
|
D is a diagonal matrix stored on the diagonal.
|
|
The upper triangle is not cleared.
|
|
The initial matrix has to be symmetric.
|
|
============
|
|
*/
|
|
bool idMatX::LDLT_Factor(void) {
|
|
int i, j, k;
|
|
float* v;
|
|
double d, sum;
|
|
|
|
assert(numRows == numColumns);
|
|
|
|
v = (float*)_alloca16(numRows * sizeof(float));
|
|
|
|
for (i = 0; i < numRows; i++) {
|
|
|
|
sum = (*this)[i][i];
|
|
for (j = 0; j < i; j++) {
|
|
d = (*this)[i][j];
|
|
v[j] = (*this)[j][j] * d;
|
|
sum -= v[j] * d;
|
|
}
|
|
|
|
if (sum == 0.0f) {
|
|
return false;
|
|
}
|
|
|
|
(*this)[i][i] = sum;
|
|
d = 1.0f / sum;
|
|
|
|
for (j = i + 1; j < numRows; j++) {
|
|
sum = (*this)[j][i];
|
|
for (k = 0; k < i; k++) {
|
|
sum -= (*this)[j][k] * v[k];
|
|
}
|
|
(*this)[j][i] = sum * d;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LDLT_UpdateRankOne
|
|
|
|
Updates the in-place LDL' factorization to obtain the factors for the matrix: LDL' + alpha * v * v'
|
|
If offset > 0 only the lower right corner starting at (offset, offset) is updated.
|
|
============
|
|
*/
|
|
bool idMatX::LDLT_UpdateRankOne(const idVecX & v, float alpha, int offset) {
|
|
int i, j;
|
|
float* y;
|
|
double diag, newDiag, beta, p, d;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(v.GetSize() >= numRows);
|
|
assert(offset >= 0 && offset < numRows);
|
|
|
|
y = (float*)_alloca16(v.GetSize() * sizeof(float));
|
|
memcpy(y, v.ToFloatPtr(), v.GetSize() * sizeof(float));
|
|
|
|
for (i = offset; i < numColumns; i++) {
|
|
p = y[i];
|
|
diag = (*this)[i][i];
|
|
(*this)[i][i] = newDiag = diag + alpha * p * p;
|
|
|
|
if (newDiag == 0.0f) {
|
|
return false;
|
|
}
|
|
|
|
alpha /= newDiag;
|
|
beta = p * alpha;
|
|
alpha *= diag;
|
|
|
|
for (j = i + 1; j < numRows; j++) {
|
|
|
|
d = (*this)[j][i];
|
|
|
|
y[j] -= p * d;
|
|
d += beta * y[j];
|
|
|
|
(*this)[j][i] = d;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LDLT_UpdateRowColumn
|
|
|
|
Updates the in-place LDL' factorization to obtain the factors for the matrix:
|
|
|
|
[ 0 a 0 ]
|
|
LDL' + [ a b c ]
|
|
[ 0 c 0 ]
|
|
|
|
where: a = v[0,r-1], b = v[r], c = v[r+1,numRows-1]
|
|
============
|
|
*/
|
|
bool idMatX::LDLT_UpdateRowColumn(const idVecX & v, int r) {
|
|
int i, j;
|
|
double sum;
|
|
float* original, * y;
|
|
idVecX addSub;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(v.GetSize() >= numRows);
|
|
assert(r >= 0 && r < numRows);
|
|
|
|
addSub.SetData(numColumns, (float*)_alloca16(numColumns * sizeof(float)));
|
|
|
|
if (r == 0) {
|
|
|
|
if (numColumns == 1) {
|
|
(*this)[0][0] += v[0];
|
|
return true;
|
|
}
|
|
for (i = 0; i < numColumns; i++) {
|
|
addSub[i] = v[i];
|
|
}
|
|
|
|
}
|
|
else {
|
|
|
|
original = (float*)_alloca16(numColumns * sizeof(float));
|
|
y = (float*)_alloca16(numColumns * sizeof(float));
|
|
|
|
// calculate original row/column of matrix
|
|
for (i = 0; i < r; i++) {
|
|
y[i] = (*this)[r][i] * (*this)[i][i];
|
|
}
|
|
for (i = 0; i < numColumns; i++) {
|
|
if (i < r) {
|
|
sum = (*this)[i][i] * (*this)[r][i];
|
|
}
|
|
else if (i == r) {
|
|
sum = (*this)[r][r];
|
|
}
|
|
else {
|
|
sum = (*this)[r][r] * (*this)[i][r];
|
|
}
|
|
for (j = 0; j < i && j < r; j++) {
|
|
sum += (*this)[i][j] * y[j];
|
|
}
|
|
original[i] = sum;
|
|
}
|
|
|
|
// solve for y in L * y = original + v
|
|
for (i = 0; i < r; i++) {
|
|
sum = original[i] + v[i];
|
|
for (j = 0; j < i; j++) {
|
|
sum -= (*this)[i][j] * y[j];
|
|
}
|
|
y[i] = sum;
|
|
}
|
|
|
|
// calculate new row of L
|
|
for (i = 0; i < r; i++) {
|
|
(*this)[r][i] = y[i] / (*this)[i][i];
|
|
}
|
|
|
|
// if the last row/column of the matrix is updated
|
|
if (r == numColumns - 1) {
|
|
// only calculate new diagonal
|
|
sum = original[r] + v[r];
|
|
for (j = 0; j < r; j++) {
|
|
sum -= (*this)[r][j] * y[j];
|
|
}
|
|
if (sum == 0.0f) {
|
|
return false;
|
|
}
|
|
(*this)[r][r] = sum;
|
|
return true;
|
|
}
|
|
|
|
// calculate the row/column to be added to the lower right sub matrix starting at (r, r)
|
|
for (i = 0; i < r; i++) {
|
|
y[i] = (*this)[r][i] * (*this)[i][i];
|
|
}
|
|
for (i = r; i < numColumns; i++) {
|
|
if (i == r) {
|
|
sum = (*this)[r][r];
|
|
}
|
|
else {
|
|
sum = (*this)[r][r] * (*this)[i][r];
|
|
}
|
|
for (j = 0; j < r; j++) {
|
|
sum += (*this)[i][j] * y[j];
|
|
}
|
|
addSub[i] = v[i] - (sum - original[i]);
|
|
}
|
|
}
|
|
|
|
// add row/column to the lower right sub matrix starting at (r, r)
|
|
|
|
#if 0
|
|
|
|
idVecX v1, v2;
|
|
double d;
|
|
|
|
v1.SetData(numColumns, (float*)_alloca16(numColumns * sizeof(float)));
|
|
v2.SetData(numColumns, (float*)_alloca16(numColumns * sizeof(float)));
|
|
|
|
d = idMath::SQRT_1OVER2;
|
|
v1[r] = (0.5f * addSub[r] + 1.0f) * d;
|
|
v2[r] = (0.5f * addSub[r] - 1.0f) * d;
|
|
for (i = r + 1; i < numColumns; i++) {
|
|
v1[i] = v2[i] = addSub[i] * d;
|
|
}
|
|
|
|
// update
|
|
if (!LDLT_UpdateRankOne(v1, 1.0f, r)) {
|
|
return false;
|
|
}
|
|
// downdate
|
|
if (!LDLT_UpdateRankOne(v2, -1.0f, r)) {
|
|
return false;
|
|
}
|
|
|
|
#else
|
|
|
|
float* v1, * v2;
|
|
double d, diag, newDiag, p1, p2, alpha1, alpha2, beta1, beta2;
|
|
|
|
v1 = (float*)_alloca16(numColumns * sizeof(float));
|
|
v2 = (float*)_alloca16(numColumns * sizeof(float));
|
|
|
|
d = idMath::SQRT_1OVER2;
|
|
v1[r] = (0.5f * addSub[r] + 1.0f) * d;
|
|
v2[r] = (0.5f * addSub[r] - 1.0f) * d;
|
|
for (i = r + 1; i < numColumns; i++) {
|
|
v1[i] = v2[i] = addSub[i] * d;
|
|
}
|
|
|
|
alpha1 = 1.0f;
|
|
alpha2 = -1.0f;
|
|
|
|
// simultaneous update/downdate of the sub matrix starting at (r, r)
|
|
for (i = r; i < numColumns; i++) {
|
|
|
|
diag = (*this)[i][i];
|
|
p1 = v1[i];
|
|
newDiag = diag + alpha1 * p1 * p1;
|
|
|
|
if (newDiag == 0.0f) {
|
|
return false;
|
|
}
|
|
|
|
alpha1 /= newDiag;
|
|
beta1 = p1 * alpha1;
|
|
alpha1 *= diag;
|
|
|
|
diag = newDiag;
|
|
p2 = v2[i];
|
|
newDiag = diag + alpha2 * p2 * p2;
|
|
|
|
if (newDiag == 0.0f) {
|
|
return false;
|
|
}
|
|
|
|
alpha2 /= newDiag;
|
|
beta2 = p2 * alpha2;
|
|
alpha2 *= diag;
|
|
|
|
(*this)[i][i] = newDiag;
|
|
|
|
for (j = i + 1; j < numRows; j++) {
|
|
|
|
d = (*this)[j][i];
|
|
|
|
v1[j] -= p1 * d;
|
|
d += beta1 * v1[j];
|
|
|
|
v2[j] -= p2 * d;
|
|
d += beta2 * v2[j];
|
|
|
|
(*this)[j][i] = d;
|
|
}
|
|
}
|
|
|
|
#endif
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LDLT_UpdateIncrement
|
|
|
|
Updates the in-place LDL' factorization to obtain the factors for the matrix:
|
|
|
|
[ A a ]
|
|
[ a b ]
|
|
|
|
where: a = v[0,numRows-1], b = v[numRows]
|
|
============
|
|
*/
|
|
bool idMatX::LDLT_UpdateIncrement(const idVecX & v) {
|
|
int i, j;
|
|
float* x;
|
|
double sum, d;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(v.GetSize() >= numRows + 1);
|
|
|
|
ChangeSize(numRows + 1, numColumns + 1, false);
|
|
|
|
x = (float*)_alloca16(numRows * sizeof(float));
|
|
|
|
// solve for x in L * x = v
|
|
for (i = 0; i < numRows - 1; i++) {
|
|
sum = v[i];
|
|
for (j = 0; j < i; j++) {
|
|
sum -= (*this)[i][j] * x[j];
|
|
}
|
|
x[i] = sum;
|
|
}
|
|
|
|
// calculate new row of L and calculate the diagonal entry
|
|
sum = v[numRows - 1];
|
|
for (i = 0; i < numRows - 1; i++) {
|
|
(*this)[numRows - 1][i] = d = x[i] / (*this)[i][i];
|
|
sum -= d * x[i];
|
|
}
|
|
|
|
if (sum == 0.0f) {
|
|
return false;
|
|
}
|
|
|
|
// store the diagonal entry
|
|
(*this)[numRows - 1][numRows - 1] = sum;
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LDLT_UpdateDecrement
|
|
|
|
Updates the in-place LDL' factorization to obtain the factors for the matrix with row r and column r removed.
|
|
v should store the row of the original matrix.
|
|
============
|
|
*/
|
|
bool idMatX::LDLT_UpdateDecrement(const idVecX & v, int r) {
|
|
idVecX v1;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(v.GetSize() >= numRows);
|
|
assert(r >= 0 && r < numRows);
|
|
|
|
v1.SetData(numRows, VECX_ALLOCA(numRows));
|
|
|
|
// update the row and column to identity
|
|
v1 = -v;
|
|
v1[r] += 1.0f;
|
|
|
|
// NOTE: msvc compiler bug: the this pointer stored in edi is expected to stay
|
|
// untouched when calling LDLT_UpdateRowColumn in the if statement
|
|
#if 0
|
|
if (!LDLT_UpdateRowColumn(v1, r)) {
|
|
#else
|
|
bool ret = LDLT_UpdateRowColumn(v1, r);
|
|
if (!ret) {
|
|
#endif
|
|
return false;
|
|
}
|
|
|
|
// physically remove the row and column
|
|
Update_Decrement(r);
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LDLT_Solve
|
|
|
|
Solve Ax = b with A factored in-place as: LDL'
|
|
============
|
|
*/
|
|
void idMatX::LDLT_Solve(idVecX & x, const idVecX & b) const {
|
|
int i, j;
|
|
double sum;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(x.GetSize() >= numRows && b.GetSize() >= numRows);
|
|
|
|
// solve L
|
|
for (i = 0; i < numRows; i++) {
|
|
sum = b[i];
|
|
for (j = 0; j < i; j++) {
|
|
sum -= (*this)[i][j] * x[j];
|
|
}
|
|
x[i] = sum;
|
|
}
|
|
|
|
// solve D
|
|
for (i = 0; i < numRows; i++) {
|
|
x[i] /= (*this)[i][i];
|
|
}
|
|
|
|
// solve Lt
|
|
for (i = numRows - 2; i >= 0; i--) {
|
|
sum = x[i];
|
|
for (j = i + 1; j < numRows; j++) {
|
|
sum -= (*this)[j][i] * x[j];
|
|
}
|
|
x[i] = sum;
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LDLT_Inverse
|
|
|
|
Calculates the inverse of the matrix which is factored in-place as: LDL'
|
|
============
|
|
*/
|
|
void idMatX::LDLT_Inverse(idMatX & inv) const {
|
|
int i, j;
|
|
idVecX x, b;
|
|
|
|
assert(numRows == numColumns);
|
|
|
|
x.SetData(numRows, VECX_ALLOCA(numRows));
|
|
b.SetData(numRows, VECX_ALLOCA(numRows));
|
|
b.Zero();
|
|
inv.SetSize(numRows, numColumns);
|
|
|
|
for (i = 0; i < numRows; i++) {
|
|
|
|
b[i] = 1.0f;
|
|
LDLT_Solve(x, b);
|
|
for (j = 0; j < numRows; j++) {
|
|
inv[j][i] = x[j];
|
|
}
|
|
b[i] = 0.0f;
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LDLT_UnpackFactors
|
|
|
|
Unpacks the in-place LDL' factorization.
|
|
============
|
|
*/
|
|
void idMatX::LDLT_UnpackFactors(idMatX & L, idMatX & D) const {
|
|
int i, j;
|
|
|
|
L.Zero(numRows, numColumns);
|
|
D.Zero(numRows, numColumns);
|
|
for (i = 0; i < numRows; i++) {
|
|
for (j = 0; j < i; j++) {
|
|
L[i][j] = (*this)[i][j];
|
|
}
|
|
L[i][i] = 1.0f;
|
|
D[i][i] = (*this)[i][i];
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::LDLT_MultiplyFactors
|
|
|
|
Multiplies the factors of the in-place LDL' factorization to form the original matrix.
|
|
============
|
|
*/
|
|
void idMatX::LDLT_MultiplyFactors(idMatX & m) const {
|
|
int r, i, j;
|
|
float* v;
|
|
double sum;
|
|
|
|
v = (float*)_alloca16(numRows * sizeof(float));
|
|
m.SetSize(numRows, numColumns);
|
|
|
|
for (r = 0; r < numRows; r++) {
|
|
|
|
// calculate row of matrix
|
|
for (i = 0; i < r; i++) {
|
|
v[i] = (*this)[r][i] * (*this)[i][i];
|
|
}
|
|
for (i = 0; i < numColumns; i++) {
|
|
if (i < r) {
|
|
sum = (*this)[i][i] * (*this)[r][i];
|
|
}
|
|
else if (i == r) {
|
|
sum = (*this)[r][r];
|
|
}
|
|
else {
|
|
sum = (*this)[r][r] * (*this)[i][r];
|
|
}
|
|
for (j = 0; j < i && j < r; j++) {
|
|
sum += (*this)[i][j] * v[j];
|
|
}
|
|
m[r][i] = sum;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::TriDiagonal_ClearTriangles
|
|
============
|
|
*/
|
|
void idMatX::TriDiagonal_ClearTriangles(void) {
|
|
int i, j;
|
|
|
|
assert(numRows == numColumns);
|
|
for (i = 0; i < numRows - 2; i++) {
|
|
for (j = i + 2; j < numColumns; j++) {
|
|
(*this)[i][j] = 0.0f;
|
|
(*this)[j][i] = 0.0f;
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::TriDiagonal_Solve
|
|
|
|
Solve Ax = b with A being tridiagonal.
|
|
============
|
|
*/
|
|
bool idMatX::TriDiagonal_Solve(idVecX & x, const idVecX & b) const {
|
|
int i;
|
|
float d;
|
|
idVecX tmp;
|
|
|
|
assert(numRows == numColumns);
|
|
assert(x.GetSize() >= numRows && b.GetSize() >= numRows);
|
|
|
|
tmp.SetData(numRows, VECX_ALLOCA(numRows));
|
|
|
|
d = (*this)[0][0];
|
|
if (d == 0.0f) {
|
|
return false;
|
|
}
|
|
d = 1.0f / d;
|
|
x[0] = b[0] * d;
|
|
for (i = 1; i < numRows; i++) {
|
|
tmp[i] = (*this)[i - 1][i] * d;
|
|
d = (*this)[i][i] - (*this)[i][i - 1] * tmp[i];
|
|
if (d == 0.0f) {
|
|
return false;
|
|
}
|
|
d = 1.0f / d;
|
|
x[i] = (b[i] - (*this)[i][i - 1] * x[i - 1]) * d;
|
|
}
|
|
for (i = numRows - 2; i >= 0; i--) {
|
|
x[i] -= tmp[i + 1] * x[i + 1];
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::TriDiagonal_Inverse
|
|
|
|
Calculates the inverse of a tri-diagonal matrix.
|
|
============
|
|
*/
|
|
void idMatX::TriDiagonal_Inverse(idMatX & inv) const {
|
|
int i, j;
|
|
idVecX x, b;
|
|
|
|
assert(numRows == numColumns);
|
|
|
|
x.SetData(numRows, VECX_ALLOCA(numRows));
|
|
b.SetData(numRows, VECX_ALLOCA(numRows));
|
|
b.Zero();
|
|
inv.SetSize(numRows, numColumns);
|
|
|
|
for (i = 0; i < numRows; i++) {
|
|
|
|
b[i] = 1.0f;
|
|
TriDiagonal_Solve(x, b);
|
|
for (j = 0; j < numRows; j++) {
|
|
inv[j][i] = x[j];
|
|
}
|
|
b[i] = 0.0f;
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::HouseholderReduction
|
|
|
|
Householder reduction to symmetric tri-diagonal form.
|
|
The original matrix is replaced by an orthogonal matrix effecting the accumulated householder transformations.
|
|
The diagonal elements of the diagonal matrix are stored in diag.
|
|
The off-diagonal elements of the diagonal matrix are stored in subd.
|
|
The initial matrix has to be symmetric.
|
|
============
|
|
*/
|
|
void idMatX::HouseholderReduction(idVecX & diag, idVecX & subd) {
|
|
int i0, i1, i2, i3;
|
|
float h, f, g, invH, halfFdivH, scale, invScale, sum;
|
|
|
|
assert(numRows == numColumns);
|
|
|
|
diag.SetSize(numRows);
|
|
subd.SetSize(numRows);
|
|
|
|
for (i0 = numRows - 1, i3 = numRows - 2; i0 >= 1; i0--, i3--) {
|
|
h = 0.0f;
|
|
scale = 0.0f;
|
|
|
|
if (i3 > 0) {
|
|
for (i2 = 0; i2 <= i3; i2++) {
|
|
scale += idMath::Fabs((*this)[i0][i2]);
|
|
}
|
|
if (scale == 0) {
|
|
subd[i0] = (*this)[i0][i3];
|
|
}
|
|
else {
|
|
invScale = 1.0f / scale;
|
|
for (i2 = 0; i2 <= i3; i2++)
|
|
{
|
|
(*this)[i0][i2] *= invScale;
|
|
h += (*this)[i0][i2] * (*this)[i0][i2];
|
|
}
|
|
f = (*this)[i0][i3];
|
|
g = idMath::Sqrt(h);
|
|
if (f > 0.0f) {
|
|
g = -g;
|
|
}
|
|
subd[i0] = scale * g;
|
|
h -= f * g;
|
|
(*this)[i0][i3] = f - g;
|
|
f = 0.0f;
|
|
invH = 1.0f / h;
|
|
for (i1 = 0; i1 <= i3; i1++) {
|
|
(*this)[i1][i0] = (*this)[i0][i1] * invH;
|
|
g = 0.0f;
|
|
for (i2 = 0; i2 <= i1; i2++) {
|
|
g += (*this)[i1][i2] * (*this)[i0][i2];
|
|
}
|
|
for (i2 = i1 + 1; i2 <= i3; i2++) {
|
|
g += (*this)[i2][i1] * (*this)[i0][i2];
|
|
}
|
|
subd[i1] = g * invH;
|
|
f += subd[i1] * (*this)[i0][i1];
|
|
}
|
|
halfFdivH = 0.5f * f * invH;
|
|
for (i1 = 0; i1 <= i3; i1++) {
|
|
f = (*this)[i0][i1];
|
|
g = subd[i1] - halfFdivH * f;
|
|
subd[i1] = g;
|
|
for (i2 = 0; i2 <= i1; i2++) {
|
|
(*this)[i1][i2] -= f * subd[i2] + g * (*this)[i0][i2];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else {
|
|
subd[i0] = (*this)[i0][i3];
|
|
}
|
|
|
|
diag[i0] = h;
|
|
}
|
|
|
|
diag[0] = 0.0f;
|
|
subd[0] = 0.0f;
|
|
for (i0 = 0, i3 = -1; i0 <= numRows - 1; i0++, i3++) {
|
|
if (diag[i0]) {
|
|
for (i1 = 0; i1 <= i3; i1++) {
|
|
sum = 0.0f;
|
|
for (i2 = 0; i2 <= i3; i2++) {
|
|
sum += (*this)[i0][i2] * (*this)[i2][i1];
|
|
}
|
|
for (i2 = 0; i2 <= i3; i2++) {
|
|
(*this)[i2][i1] -= sum * (*this)[i2][i0];
|
|
}
|
|
}
|
|
}
|
|
diag[i0] = (*this)[i0][i0];
|
|
(*this)[i0][i0] = 1.0f;
|
|
for (i1 = 0; i1 <= i3; i1++) {
|
|
(*this)[i1][i0] = 0.0f;
|
|
(*this)[i0][i1] = 0.0f;
|
|
}
|
|
}
|
|
|
|
// re-order
|
|
for (i0 = 1, i3 = 0; i0 < numRows; i0++, i3++) {
|
|
subd[i3] = subd[i0];
|
|
}
|
|
subd[numRows - 1] = 0.0f;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::QL
|
|
|
|
QL algorithm with implicit shifts to determine the eigenvalues and eigenvectors of a symmetric tri-diagonal matrix.
|
|
diag contains the diagonal elements of the symmetric tri-diagonal matrix on input and is overwritten with the eigenvalues.
|
|
subd contains the off-diagonal elements of the symmetric tri-diagonal matrix and is destroyed.
|
|
This matrix has to be either the identity matrix to determine the eigenvectors for a symmetric tri-diagonal matrix,
|
|
or the matrix returned by the Householder reduction to determine the eigenvalues for the original symmetric matrix.
|
|
============
|
|
*/
|
|
bool idMatX::QL(idVecX & diag, idVecX & subd) {
|
|
const int maxIter = 32;
|
|
int i0, i1, i2, i3;
|
|
float a, b, f, g, r, p, s, c;
|
|
|
|
assert(numRows == numColumns);
|
|
|
|
for (i0 = 0; i0 < numRows; i0++) {
|
|
for (i1 = 0; i1 < maxIter; i1++) {
|
|
for (i2 = i0; i2 <= numRows - 2; i2++) {
|
|
a = idMath::Fabs(diag[i2]) + idMath::Fabs(diag[i2 + 1]);
|
|
if (idMath::Fabs(subd[i2]) + a == a) {
|
|
break;
|
|
}
|
|
}
|
|
if (i2 == i0) {
|
|
break;
|
|
}
|
|
|
|
g = (diag[i0 + 1] - diag[i0]) / (2.0f * subd[i0]);
|
|
r = idMath::Sqrt(g * g + 1.0f);
|
|
if (g < 0.0f) {
|
|
g = diag[i2] - diag[i0] + subd[i0] / (g - r);
|
|
}
|
|
else {
|
|
g = diag[i2] - diag[i0] + subd[i0] / (g + r);
|
|
}
|
|
s = 1.0f;
|
|
c = 1.0f;
|
|
p = 0.0f;
|
|
for (i3 = i2 - 1; i3 >= i0; i3--) {
|
|
f = s * subd[i3];
|
|
b = c * subd[i3];
|
|
if (idMath::Fabs(f) >= idMath::Fabs(g)) {
|
|
c = g / f;
|
|
r = idMath::Sqrt(c * c + 1.0f);
|
|
subd[i3 + 1] = f * r;
|
|
s = 1.0f / r;
|
|
c *= s;
|
|
}
|
|
else {
|
|
s = f / g;
|
|
r = idMath::Sqrt(s * s + 1.0f);
|
|
subd[i3 + 1] = g * r;
|
|
c = 1.0f / r;
|
|
s *= c;
|
|
}
|
|
g = diag[i3 + 1] - p;
|
|
r = (diag[i3] - g) * s + 2.0f * b * c;
|
|
p = s * r;
|
|
diag[i3 + 1] = g + p;
|
|
g = c * r - b;
|
|
|
|
for (int i4 = 0; i4 < numRows; i4++) {
|
|
f = (*this)[i4][i3 + 1];
|
|
(*this)[i4][i3 + 1] = s * (*this)[i4][i3] + c * f;
|
|
(*this)[i4][i3] = c * (*this)[i4][i3] - s * f;
|
|
}
|
|
}
|
|
diag[i0] -= p;
|
|
subd[i0] = g;
|
|
subd[i2] = 0.0f;
|
|
}
|
|
if (i1 == maxIter) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Eigen_SolveSymmetricTriDiagonal
|
|
|
|
Determine eigen values and eigen vectors for a symmetric tri-diagonal matrix.
|
|
The eigen values are stored in 'eigenValues'.
|
|
Column i of the original matrix will store the eigen vector corresponding to the eigenValues[i].
|
|
The initial matrix has to be symmetric tri-diagonal.
|
|
============
|
|
*/
|
|
bool idMatX::Eigen_SolveSymmetricTriDiagonal(idVecX & eigenValues) {
|
|
int i;
|
|
idVecX subd;
|
|
|
|
assert(numRows == numColumns);
|
|
|
|
subd.SetData(numRows, VECX_ALLOCA(numRows));
|
|
eigenValues.SetSize(numRows);
|
|
|
|
for (i = 0; i < numRows - 1; i++) {
|
|
eigenValues[i] = (*this)[i][i];
|
|
subd[i] = (*this)[i + 1][i];
|
|
}
|
|
eigenValues[numRows - 1] = (*this)[numRows - 1][numRows - 1];
|
|
|
|
Identity();
|
|
|
|
return QL(eigenValues, subd);
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Eigen_SolveSymmetric
|
|
|
|
Determine eigen values and eigen vectors for a symmetric matrix.
|
|
The eigen values are stored in 'eigenValues'.
|
|
Column i of the original matrix will store the eigen vector corresponding to the eigenValues[i].
|
|
The initial matrix has to be symmetric.
|
|
============
|
|
*/
|
|
bool idMatX::Eigen_SolveSymmetric(idVecX & eigenValues) {
|
|
idVecX subd;
|
|
|
|
assert(numRows == numColumns);
|
|
|
|
subd.SetData(numRows, VECX_ALLOCA(numRows));
|
|
eigenValues.SetSize(numRows);
|
|
|
|
HouseholderReduction(eigenValues, subd);
|
|
return QL(eigenValues, subd);
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::HessenbergReduction
|
|
|
|
Reduction to Hessenberg form.
|
|
============
|
|
*/
|
|
void idMatX::HessenbergReduction(idMatX & H) {
|
|
int i, j, m;
|
|
int low = 0;
|
|
int high = numRows - 1;
|
|
float scale, f, g, h;
|
|
idVecX v;
|
|
|
|
v.SetData(numRows, VECX_ALLOCA(numRows));
|
|
|
|
for (m = low + 1; m <= high - 1; m++) {
|
|
|
|
scale = 0.0f;
|
|
for (i = m; i <= high; i++) {
|
|
scale = scale + idMath::Fabs(H[i][m - 1]);
|
|
}
|
|
if (scale != 0.0f) {
|
|
|
|
// compute Householder transformation.
|
|
h = 0.0f;
|
|
for (i = high; i >= m; i--) {
|
|
v[i] = H[i][m - 1] / scale;
|
|
h += v[i] * v[i];
|
|
}
|
|
g = idMath::Sqrt(h);
|
|
if (v[m] > 0.0f) {
|
|
g = -g;
|
|
}
|
|
h = h - v[m] * g;
|
|
v[m] = v[m] - g;
|
|
|
|
// apply Householder similarity transformation
|
|
// H = (I-u*u'/h)*H*(I-u*u')/h)
|
|
for (j = m; j < numRows; j++) {
|
|
f = 0.0f;
|
|
for (i = high; i >= m; i--) {
|
|
f += v[i] * H[i][j];
|
|
}
|
|
f = f / h;
|
|
for (i = m; i <= high; i++) {
|
|
H[i][j] -= f * v[i];
|
|
}
|
|
}
|
|
|
|
for (i = 0; i <= high; i++) {
|
|
f = 0.0f;
|
|
for (j = high; j >= m; j--) {
|
|
f += v[j] * H[i][j];
|
|
}
|
|
f = f / h;
|
|
for (j = m; j <= high; j++) {
|
|
H[i][j] -= f * v[j];
|
|
}
|
|
}
|
|
v[m] = scale * v[m];
|
|
H[m][m - 1] = scale * g;
|
|
}
|
|
}
|
|
|
|
// accumulate transformations
|
|
Identity();
|
|
for (int m = high - 1; m >= low + 1; m--) {
|
|
if (H[m][m - 1] != 0.0f) {
|
|
for (i = m + 1; i <= high; i++) {
|
|
v[i] = H[i][m - 1];
|
|
}
|
|
for (j = m; j <= high; j++) {
|
|
g = 0.0f;
|
|
for (i = m; i <= high; i++) {
|
|
g += v[i] * (*this)[i][j];
|
|
}
|
|
// float division to avoid possible underflow
|
|
g = (g / v[m]) / H[m][m - 1];
|
|
for (i = m; i <= high; i++) {
|
|
(*this)[i][j] += g * v[i];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::ComplexDivision
|
|
|
|
Complex scalar division.
|
|
============
|
|
*/
|
|
void idMatX::ComplexDivision(float xr, float xi, float yr, float yi, float& cdivr, float& cdivi) {
|
|
float r, d;
|
|
if (idMath::Fabs(yr) > idMath::Fabs(yi)) {
|
|
r = yi / yr;
|
|
d = yr + r * yi;
|
|
cdivr = (xr + r * xi) / d;
|
|
cdivi = (xi - r * xr) / d;
|
|
}
|
|
else {
|
|
r = yr / yi;
|
|
d = yi + r * yr;
|
|
cdivr = (r * xr + xi) / d;
|
|
cdivi = (r * xi - xr) / d;
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::HessenbergToRealSchur
|
|
|
|
Reduction from Hessenberg to real Schur form.
|
|
============
|
|
*/
|
|
bool idMatX::HessenbergToRealSchur(idMatX & H, idVecX & realEigenValues, idVecX & imaginaryEigenValues) {
|
|
int i, j, k;
|
|
int n = numRows - 1;
|
|
int low = 0;
|
|
int high = numRows - 1;
|
|
float eps = 2e-16f, exshift = 0.0f;
|
|
float p = 0.0f, q = 0.0f, r = 0.0f, s = 0.0f, z = 0.0f, t, w, x, y;
|
|
|
|
// store roots isolated by balanc and compute matrix norm
|
|
float norm = 0.0f;
|
|
for (i = 0; i < numRows; i++) {
|
|
if (i < low || i > high) {
|
|
realEigenValues[i] = H[i][i];
|
|
imaginaryEigenValues[i] = 0.0f;
|
|
}
|
|
for (j = Max(i - 1, 0); j < numRows; j++) {
|
|
norm = norm + idMath::Fabs(H[i][j]);
|
|
}
|
|
}
|
|
|
|
int iter = 0;
|
|
while (n >= low) {
|
|
|
|
// look for single small sub-diagonal element
|
|
int l = n;
|
|
while (l > low) {
|
|
s = idMath::Fabs(H[l - 1][l - 1]) + idMath::Fabs(H[l][l]);
|
|
if (s == 0.0f) {
|
|
s = norm;
|
|
}
|
|
if (idMath::Fabs(H[l][l - 1]) < eps * s) {
|
|
break;
|
|
}
|
|
l--;
|
|
}
|
|
|
|
// check for convergence
|
|
if (l == n) { // one root found
|
|
H[n][n] = H[n][n] + exshift;
|
|
realEigenValues[n] = H[n][n];
|
|
imaginaryEigenValues[n] = 0.0f;
|
|
n--;
|
|
iter = 0;
|
|
}
|
|
else if (l == n - 1) { // two roots found
|
|
w = H[n][n - 1] * H[n - 1][n];
|
|
p = (H[n - 1][n - 1] - H[n][n]) / 2.0f;
|
|
q = p * p + w;
|
|
z = idMath::Sqrt(idMath::Fabs(q));
|
|
H[n][n] = H[n][n] + exshift;
|
|
H[n - 1][n - 1] = H[n - 1][n - 1] + exshift;
|
|
x = H[n][n];
|
|
|
|
if (q >= 0.0f) { // real pair
|
|
if (p >= 0.0f) {
|
|
z = p + z;
|
|
}
|
|
else {
|
|
z = p - z;
|
|
}
|
|
realEigenValues[n - 1] = x + z;
|
|
realEigenValues[n] = realEigenValues[n - 1];
|
|
if (z != 0.0f) {
|
|
realEigenValues[n] = x - w / z;
|
|
}
|
|
imaginaryEigenValues[n - 1] = 0.0f;
|
|
imaginaryEigenValues[n] = 0.0f;
|
|
x = H[n][n - 1];
|
|
s = idMath::Fabs(x) + idMath::Fabs(z);
|
|
p = x / s;
|
|
q = z / s;
|
|
r = idMath::Sqrt(p * p + q * q);
|
|
p = p / r;
|
|
q = q / r;
|
|
|
|
// modify row
|
|
for (j = n - 1; j < numRows; j++) {
|
|
z = H[n - 1][j];
|
|
H[n - 1][j] = q * z + p * H[n][j];
|
|
H[n][j] = q * H[n][j] - p * z;
|
|
}
|
|
|
|
// modify column
|
|
for (i = 0; i <= n; i++) {
|
|
z = H[i][n - 1];
|
|
H[i][n - 1] = q * z + p * H[i][n];
|
|
H[i][n] = q * H[i][n] - p * z;
|
|
}
|
|
|
|
// accumulate transformations
|
|
for (i = low; i <= high; i++) {
|
|
z = (*this)[i][n - 1];
|
|
(*this)[i][n - 1] = q * z + p * (*this)[i][n];
|
|
(*this)[i][n] = q * (*this)[i][n] - p * z;
|
|
}
|
|
}
|
|
else { // complex pair
|
|
realEigenValues[n - 1] = x + p;
|
|
realEigenValues[n] = x + p;
|
|
imaginaryEigenValues[n - 1] = z;
|
|
imaginaryEigenValues[n] = -z;
|
|
}
|
|
n = n - 2;
|
|
iter = 0;
|
|
|
|
}
|
|
else { // no convergence yet
|
|
|
|
// form shift
|
|
x = H[n][n];
|
|
y = 0.0f;
|
|
w = 0.0f;
|
|
if (l < n) {
|
|
y = H[n - 1][n - 1];
|
|
w = H[n][n - 1] * H[n - 1][n];
|
|
}
|
|
|
|
// Wilkinson's original ad hoc shift
|
|
if (iter == 10) {
|
|
exshift += x;
|
|
for (i = low; i <= n; i++) {
|
|
H[i][i] -= x;
|
|
}
|
|
s = idMath::Fabs(H[n][n - 1]) + idMath::Fabs(H[n - 1][n - 2]);
|
|
x = y = 0.75f * s;
|
|
w = -0.4375f * s * s;
|
|
}
|
|
|
|
// new ad hoc shift
|
|
if (iter == 30) {
|
|
s = (y - x) / 2.0f;
|
|
s = s * s + w;
|
|
if (s > 0) {
|
|
s = idMath::Sqrt(s);
|
|
if (y < x) {
|
|
s = -s;
|
|
}
|
|
s = x - w / ((y - x) / 2.0f + s);
|
|
for (i = low; i <= n; i++) {
|
|
H[i][i] -= s;
|
|
}
|
|
exshift += s;
|
|
x = y = w = 0.964f;
|
|
}
|
|
}
|
|
|
|
iter = iter + 1;
|
|
|
|
// look for two consecutive small sub-diagonal elements
|
|
int m;
|
|
for (m = n - 2; m >= l; m--) {
|
|
z = H[m][m];
|
|
r = x - z;
|
|
s = y - z;
|
|
p = (r * s - w) / H[m + 1][m] + H[m][m + 1];
|
|
q = H[m + 1][m + 1] - z - r - s;
|
|
r = H[m + 2][m + 1];
|
|
s = idMath::Fabs(p) + idMath::Fabs(q) + idMath::Fabs(r);
|
|
p = p / s;
|
|
q = q / s;
|
|
r = r / s;
|
|
if (m == l) {
|
|
break;
|
|
}
|
|
if (idMath::Fabs(H[m][m - 1]) * (idMath::Fabs(q) + idMath::Fabs(r)) <
|
|
eps * (idMath::Fabs(p) * (idMath::Fabs(H[m - 1][m - 1]) + idMath::Fabs(z) + idMath::Fabs(H[m + 1][m + 1])))) {
|
|
break;
|
|
}
|
|
}
|
|
|
|
for (i = m + 2; i <= n; i++) {
|
|
H[i][i - 2] = 0.0f;
|
|
if (i > m + 2) {
|
|
H[i][i - 3] = 0.0f;
|
|
}
|
|
}
|
|
|
|
// double QR step involving rows l:n and columns m:n
|
|
for (k = m; k <= n - 1; k++) {
|
|
bool notlast = (k != n - 1);
|
|
if (k != m) {
|
|
p = H[k][k - 1];
|
|
q = H[k + 1][k - 1];
|
|
r = (notlast ? H[k + 2][k - 1] : 0.0f);
|
|
x = idMath::Fabs(p) + idMath::Fabs(q) + idMath::Fabs(r);
|
|
if (x != 0.0f) {
|
|
p = p / x;
|
|
q = q / x;
|
|
r = r / x;
|
|
}
|
|
}
|
|
if (x == 0.0f) {
|
|
break;
|
|
}
|
|
s = idMath::Sqrt(p * p + q * q + r * r);
|
|
if (p < 0.0f) {
|
|
s = -s;
|
|
}
|
|
if (s != 0.0f) {
|
|
if (k != m) {
|
|
H[k][k - 1] = -s * x;
|
|
}
|
|
else if (l != m) {
|
|
H[k][k - 1] = -H[k][k - 1];
|
|
}
|
|
p = p + s;
|
|
x = p / s;
|
|
y = q / s;
|
|
z = r / s;
|
|
q = q / p;
|
|
r = r / p;
|
|
|
|
// modify row
|
|
for (j = k; j < numRows; j++) {
|
|
p = H[k][j] + q * H[k + 1][j];
|
|
if (notlast) {
|
|
p = p + r * H[k + 2][j];
|
|
H[k + 2][j] = H[k + 2][j] - p * z;
|
|
}
|
|
H[k][j] = H[k][j] - p * x;
|
|
H[k + 1][j] = H[k + 1][j] - p * y;
|
|
}
|
|
|
|
// modify column
|
|
for (i = 0; i <= Min(n, k + 3); i++) {
|
|
p = x * H[i][k] + y * H[i][k + 1];
|
|
if (notlast) {
|
|
p = p + z * H[i][k + 2];
|
|
H[i][k + 2] = H[i][k + 2] - p * r;
|
|
}
|
|
H[i][k] = H[i][k] - p;
|
|
H[i][k + 1] = H[i][k + 1] - p * q;
|
|
}
|
|
|
|
// accumulate transformations
|
|
for (i = low; i <= high; i++) {
|
|
p = x * (*this)[i][k] + y * (*this)[i][k + 1];
|
|
if (notlast) {
|
|
p = p + z * (*this)[i][k + 2];
|
|
(*this)[i][k + 2] = (*this)[i][k + 2] - p * r;
|
|
}
|
|
(*this)[i][k] = (*this)[i][k] - p;
|
|
(*this)[i][k + 1] = (*this)[i][k + 1] - p * q;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// backsubstitute to find vectors of upper triangular form
|
|
if (norm == 0.0f) {
|
|
return false;
|
|
}
|
|
|
|
for (n = numRows - 1; n >= 0; n--) {
|
|
p = realEigenValues[n];
|
|
q = imaginaryEigenValues[n];
|
|
|
|
if (q == 0.0f) { // real vector
|
|
int l = n;
|
|
H[n][n] = 1.0f;
|
|
for (i = n - 1; i >= 0; i--) {
|
|
w = H[i][i] - p;
|
|
r = 0.0f;
|
|
for (j = l; j <= n; j++) {
|
|
r = r + H[i][j] * H[j][n];
|
|
}
|
|
if (imaginaryEigenValues[i] < 0.0f) {
|
|
z = w;
|
|
s = r;
|
|
}
|
|
else {
|
|
l = i;
|
|
if (imaginaryEigenValues[i] == 0.0f) {
|
|
if (w != 0.0f) {
|
|
H[i][n] = -r / w;
|
|
}
|
|
else {
|
|
H[i][n] = -r / (eps * norm);
|
|
}
|
|
}
|
|
else { // solve real equations
|
|
x = H[i][i + 1];
|
|
y = H[i + 1][i];
|
|
q = (realEigenValues[i] - p) * (realEigenValues[i] - p) + imaginaryEigenValues[i] * imaginaryEigenValues[i];
|
|
t = (x * s - z * r) / q;
|
|
H[i][n] = t;
|
|
if (idMath::Fabs(x) > idMath::Fabs(z)) {
|
|
H[i + 1][n] = (-r - w * t) / x;
|
|
}
|
|
else {
|
|
H[i + 1][n] = (-s - y * t) / z;
|
|
}
|
|
}
|
|
|
|
// overflow control
|
|
t = idMath::Fabs(H[i][n]);
|
|
if ((eps * t) * t > 1) {
|
|
for (j = i; j <= n; j++) {
|
|
H[j][n] = H[j][n] / t;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else if (q < 0.0f) { // complex vector
|
|
int l = n - 1;
|
|
|
|
// last vector component imaginary so matrix is triangular
|
|
if (idMath::Fabs(H[n][n - 1]) > idMath::Fabs(H[n - 1][n])) {
|
|
H[n - 1][n - 1] = q / H[n][n - 1];
|
|
H[n - 1][n] = -(H[n][n] - p) / H[n][n - 1];
|
|
}
|
|
else {
|
|
ComplexDivision(0.0f, -H[n - 1][n], H[n - 1][n - 1] - p, q, H[n - 1][n - 1], H[n - 1][n]);
|
|
}
|
|
H[n][n - 1] = 0.0f;
|
|
H[n][n] = 1.0f;
|
|
for (i = n - 2; i >= 0; i--) {
|
|
float ra, sa, vr, vi;
|
|
ra = 0.0f;
|
|
sa = 0.0f;
|
|
for (j = l; j <= n; j++) {
|
|
ra = ra + H[i][j] * H[j][n - 1];
|
|
sa = sa + H[i][j] * H[j][n];
|
|
}
|
|
w = H[i][i] - p;
|
|
|
|
if (imaginaryEigenValues[i] < 0.0f) {
|
|
z = w;
|
|
r = ra;
|
|
s = sa;
|
|
}
|
|
else {
|
|
l = i;
|
|
if (imaginaryEigenValues[i] == 0.0f) {
|
|
ComplexDivision(-ra, -sa, w, q, H[i][n - 1], H[i][n]);
|
|
}
|
|
else {
|
|
// solve complex equations
|
|
x = H[i][i + 1];
|
|
y = H[i + 1][i];
|
|
vr = (realEigenValues[i] - p) * (realEigenValues[i] - p) + imaginaryEigenValues[i] * imaginaryEigenValues[i] - q * q;
|
|
vi = (realEigenValues[i] - p) * 2.0f * q;
|
|
if (vr == 0.0f && vi == 0.0f) {
|
|
vr = eps * norm * (idMath::Fabs(w) + idMath::Fabs(q) + idMath::Fabs(x) + idMath::Fabs(y) + idMath::Fabs(z));
|
|
}
|
|
ComplexDivision(x * r - z * ra + q * sa, x * s - z * sa - q * ra, vr, vi, H[i][n - 1], H[i][n]);
|
|
if (idMath::Fabs(x) > (idMath::Fabs(z) + idMath::Fabs(q))) {
|
|
H[i + 1][n - 1] = (-ra - w * H[i][n - 1] + q * H[i][n]) / x;
|
|
H[i + 1][n] = (-sa - w * H[i][n] - q * H[i][n - 1]) / x;
|
|
}
|
|
else {
|
|
ComplexDivision(-r - y * H[i][n - 1], -s - y * H[i][n], z, q, H[i + 1][n - 1], H[i + 1][n]);
|
|
}
|
|
}
|
|
|
|
// overflow control
|
|
t = Max(idMath::Fabs(H[i][n - 1]), idMath::Fabs(H[i][n]));
|
|
if ((eps * t) * t > 1) {
|
|
for (j = i; j <= n; j++) {
|
|
H[j][n - 1] = H[j][n - 1] / t;
|
|
H[j][n] = H[j][n] / t;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// vectors of isolated roots
|
|
for (i = 0; i < numRows; i++) {
|
|
if (i < low || i > high) {
|
|
for (j = i; j < numRows; j++) {
|
|
(*this)[i][j] = H[i][j];
|
|
}
|
|
}
|
|
}
|
|
|
|
// back transformation to get eigenvectors of original matrix
|
|
for (j = numRows - 1; j >= low; j--) {
|
|
for (i = low; i <= high; i++) {
|
|
z = 0.0f;
|
|
for (k = low; k <= Min(j, high); k++) {
|
|
z = z + (*this)[i][k] * H[k][j];
|
|
}
|
|
(*this)[i][j] = z;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Eigen_Solve
|
|
|
|
Determine eigen values and eigen vectors for a square matrix.
|
|
The eigen values are stored in 'realEigenValues' and 'imaginaryEigenValues'.
|
|
Column i of the original matrix will store the eigen vector corresponding to the realEigenValues[i] and imaginaryEigenValues[i].
|
|
============
|
|
*/
|
|
bool idMatX::Eigen_Solve(idVecX & realEigenValues, idVecX & imaginaryEigenValues) {
|
|
idMatX H;
|
|
|
|
assert(numRows == numColumns);
|
|
|
|
realEigenValues.SetSize(numRows);
|
|
imaginaryEigenValues.SetSize(numRows);
|
|
|
|
H = *this;
|
|
|
|
// reduce to Hessenberg form
|
|
HessenbergReduction(H);
|
|
|
|
// reduce Hessenberg to real Schur form
|
|
return HessenbergToRealSchur(H, realEigenValues, imaginaryEigenValues);
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Eigen_SortIncreasing
|
|
============
|
|
*/
|
|
void idMatX::Eigen_SortIncreasing(idVecX & eigenValues) {
|
|
int i, j, k;
|
|
float min;
|
|
|
|
for (i = 0, j; i <= numRows - 2; i++) {
|
|
j = i;
|
|
min = eigenValues[j];
|
|
for (k = i + 1; k < numRows; k++) {
|
|
if (eigenValues[k] < min) {
|
|
j = k;
|
|
min = eigenValues[j];
|
|
}
|
|
}
|
|
if (j != i) {
|
|
eigenValues.SwapElements(i, j);
|
|
SwapColumns(i, j);
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Eigen_SortDecreasing
|
|
============
|
|
*/
|
|
void idMatX::Eigen_SortDecreasing(idVecX & eigenValues) {
|
|
int i, j, k;
|
|
float max;
|
|
|
|
for (i = 0, j; i <= numRows - 2; i++) {
|
|
j = i;
|
|
max = eigenValues[j];
|
|
for (k = i + 1; k < numRows; k++) {
|
|
if (eigenValues[k] > max) {
|
|
j = k;
|
|
max = eigenValues[j];
|
|
}
|
|
}
|
|
if (j != i) {
|
|
eigenValues.SwapElements(i, j);
|
|
SwapColumns(i, j);
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::DeterminantGeneric
|
|
============
|
|
*/
|
|
float idMatX::DeterminantGeneric(void) const {
|
|
int* index;
|
|
float det;
|
|
idMatX tmp;
|
|
|
|
index = (int*)_alloca16(numRows * sizeof(int));
|
|
tmp.SetData(numRows, numColumns, MATX_ALLOCA(numRows * numColumns));
|
|
tmp = *this;
|
|
|
|
if (!tmp.LU_Factor(index, &det)) {
|
|
return 0.0f;
|
|
}
|
|
|
|
return det;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::InverseSelfGeneric
|
|
============
|
|
*/
|
|
bool idMatX::InverseSelfGeneric(void) {
|
|
int i, j, * index;
|
|
idMatX tmp;
|
|
idVecX x, b;
|
|
|
|
index = (int*)_alloca16(numRows * sizeof(int));
|
|
tmp.SetData(numRows, numColumns, MATX_ALLOCA(numRows * numColumns));
|
|
tmp = *this;
|
|
|
|
if (!tmp.LU_Factor(index)) {
|
|
return false;
|
|
}
|
|
|
|
x.SetData(numRows, VECX_ALLOCA(numRows));
|
|
b.SetData(numRows, VECX_ALLOCA(numRows));
|
|
b.Zero();
|
|
|
|
for (i = 0; i < numRows; i++) {
|
|
|
|
b[i] = 1.0f;
|
|
tmp.LU_Solve(x, b, index);
|
|
for (j = 0; j < numRows; j++) {
|
|
(*this)[j][i] = x[j];
|
|
}
|
|
b[i] = 0.0f;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
/*
|
|
============
|
|
idMatX::Test
|
|
============
|
|
*/
|
|
void idMatX::Test(void) {
|
|
idMatX original, m1, m2, m3, q1, q2, r1, r2;
|
|
idVecX v, w, u, c, d;
|
|
int offset, size, * index1, * index2;
|
|
|
|
size = 6;
|
|
original.Random(size, size, 0);
|
|
original = original * original.Transpose();
|
|
|
|
index1 = (int*)_alloca16((size + 1) * sizeof(index1[0]));
|
|
index2 = (int*)_alloca16((size + 1) * sizeof(index2[0]));
|
|
|
|
/*
|
|
idMatX::LowerTriangularInverse
|
|
*/
|
|
|
|
m1 = original;
|
|
m1.ClearUpperTriangle();
|
|
m2 = m1;
|
|
|
|
m2.InverseSelf();
|
|
m1.LowerTriangularInverse();
|
|
|
|
if (!m1.Compare(m2, 1e-4f)) {
|
|
idLib::common->Warning("idMatX::LowerTriangularInverse failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::UpperTriangularInverse
|
|
*/
|
|
|
|
m1 = original;
|
|
m1.ClearLowerTriangle();
|
|
m2 = m1;
|
|
|
|
m2.InverseSelf();
|
|
m1.UpperTriangularInverse();
|
|
|
|
if (!m1.Compare(m2, 1e-4f)) {
|
|
idLib::common->Warning("idMatX::UpperTriangularInverse failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::Inverse_GaussJordan
|
|
*/
|
|
|
|
m1 = original;
|
|
|
|
m1.Inverse_GaussJordan();
|
|
m1 *= original;
|
|
|
|
if (!m1.IsIdentity(1e-4f)) {
|
|
idLib::common->Warning("idMatX::Inverse_GaussJordan failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::Inverse_UpdateRankOne
|
|
*/
|
|
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
w.Random(size, 1);
|
|
v.Random(size, 2);
|
|
|
|
// invert m1
|
|
m1.Inverse_GaussJordan();
|
|
|
|
// modify and invert m2
|
|
m2.Update_RankOne(v, w, 1.0f);
|
|
if (!m2.Inverse_GaussJordan()) {
|
|
assert(0);
|
|
}
|
|
|
|
// update inverse of m1
|
|
m1.Inverse_UpdateRankOne(v, w, 1.0f);
|
|
|
|
if (!m1.Compare(m2, 1e-4f)) {
|
|
idLib::common->Warning("idMatX::Inverse_UpdateRankOne failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::Inverse_UpdateRowColumn
|
|
*/
|
|
|
|
for (offset = 0; offset < size; offset++) {
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
v.Random(size, 1);
|
|
w.Random(size, 2);
|
|
w[offset] = 0.0f;
|
|
|
|
// invert m1
|
|
m1.Inverse_GaussJordan();
|
|
|
|
// modify and invert m2
|
|
m2.Update_RowColumn(v, w, offset);
|
|
if (!m2.Inverse_GaussJordan()) {
|
|
assert(0);
|
|
}
|
|
|
|
// update inverse of m1
|
|
m1.Inverse_UpdateRowColumn(v, w, offset);
|
|
|
|
if (!m1.Compare(m2, 1e-3f)) {
|
|
idLib::common->Warning("idMatX::Inverse_UpdateRowColumn failed");
|
|
}
|
|
}
|
|
|
|
/*
|
|
idMatX::Inverse_UpdateIncrement
|
|
*/
|
|
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
v.Random(size + 1, 1);
|
|
w.Random(size + 1, 2);
|
|
w[size] = 0.0f;
|
|
|
|
// invert m1
|
|
m1.Inverse_GaussJordan();
|
|
|
|
// modify and invert m2
|
|
m2.Update_Increment(v, w);
|
|
if (!m2.Inverse_GaussJordan()) {
|
|
assert(0);
|
|
}
|
|
|
|
// update inverse of m1
|
|
m1.Inverse_UpdateIncrement(v, w);
|
|
|
|
if (!m1.Compare(m2, 1e-4f)) {
|
|
idLib::common->Warning("idMatX::Inverse_UpdateIncrement failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::Inverse_UpdateDecrement
|
|
*/
|
|
|
|
for (offset = 0; offset < size; offset++) {
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
v.SetSize(6);
|
|
w.SetSize(6);
|
|
for (int i = 0; i < size; i++) {
|
|
v[i] = original[i][offset];
|
|
w[i] = original[offset][i];
|
|
}
|
|
|
|
// invert m1
|
|
m1.Inverse_GaussJordan();
|
|
|
|
// modify and invert m2
|
|
m2.Update_Decrement(offset);
|
|
if (!m2.Inverse_GaussJordan()) {
|
|
assert(0);
|
|
}
|
|
|
|
// update inverse of m1
|
|
m1.Inverse_UpdateDecrement(v, w, offset);
|
|
|
|
if (!m1.Compare(m2, 1e-3f)) {
|
|
idLib::common->Warning("idMatX::Inverse_UpdateDecrement failed");
|
|
}
|
|
}
|
|
|
|
/*
|
|
idMatX::LU_Factor
|
|
*/
|
|
|
|
m1 = original;
|
|
|
|
m1.LU_Factor(NULL); // no pivoting
|
|
m1.LU_UnpackFactors(m2, m3);
|
|
m1 = m2 * m3;
|
|
|
|
if (!original.Compare(m1, 1e-4f)) {
|
|
idLib::common->Warning("idMatX::LU_Factor failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::LU_UpdateRankOne
|
|
*/
|
|
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
w.Random(size, 1);
|
|
v.Random(size, 2);
|
|
|
|
// factor m1
|
|
m1.LU_Factor(index1);
|
|
|
|
// modify and factor m2
|
|
m2.Update_RankOne(v, w, 1.0f);
|
|
if (!m2.LU_Factor(index2)) {
|
|
assert(0);
|
|
}
|
|
m2.LU_MultiplyFactors(m3, index2);
|
|
m2 = m3;
|
|
|
|
// update factored m1
|
|
m1.LU_UpdateRankOne(v, w, 1.0f, index1);
|
|
m1.LU_MultiplyFactors(m3, index1);
|
|
m1 = m3;
|
|
|
|
if (!m1.Compare(m2, 1e-4f)) {
|
|
idLib::common->Warning("idMatX::LU_UpdateRankOne failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::LU_UpdateRowColumn
|
|
*/
|
|
|
|
for (offset = 0; offset < size; offset++) {
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
v.Random(size, 1);
|
|
w.Random(size, 2);
|
|
w[offset] = 0.0f;
|
|
|
|
// factor m1
|
|
m1.LU_Factor(index1);
|
|
|
|
// modify and factor m2
|
|
m2.Update_RowColumn(v, w, offset);
|
|
if (!m2.LU_Factor(index2)) {
|
|
assert(0);
|
|
}
|
|
m2.LU_MultiplyFactors(m3, index2);
|
|
m2 = m3;
|
|
|
|
// update m1
|
|
m1.LU_UpdateRowColumn(v, w, offset, index1);
|
|
m1.LU_MultiplyFactors(m3, index1);
|
|
m1 = m3;
|
|
|
|
if (!m1.Compare(m2, 1e-3f)) {
|
|
idLib::common->Warning("idMatX::LU_UpdateRowColumn failed");
|
|
}
|
|
}
|
|
|
|
/*
|
|
idMatX::LU_UpdateIncrement
|
|
*/
|
|
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
v.Random(size + 1, 1);
|
|
w.Random(size + 1, 2);
|
|
w[size] = 0.0f;
|
|
|
|
// factor m1
|
|
m1.LU_Factor(index1);
|
|
|
|
// modify and factor m2
|
|
m2.Update_Increment(v, w);
|
|
if (!m2.LU_Factor(index2)) {
|
|
assert(0);
|
|
}
|
|
m2.LU_MultiplyFactors(m3, index2);
|
|
m2 = m3;
|
|
|
|
// update factored m1
|
|
m1.LU_UpdateIncrement(v, w, index1);
|
|
m1.LU_MultiplyFactors(m3, index1);
|
|
m1 = m3;
|
|
|
|
if (!m1.Compare(m2, 1e-4f)) {
|
|
idLib::common->Warning("idMatX::LU_UpdateIncrement failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::LU_UpdateDecrement
|
|
*/
|
|
|
|
for (offset = 0; offset < size; offset++) {
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
v.SetSize(6);
|
|
w.SetSize(6);
|
|
for (int i = 0; i < size; i++) {
|
|
v[i] = original[i][offset];
|
|
w[i] = original[offset][i];
|
|
}
|
|
|
|
// factor m1
|
|
m1.LU_Factor(index1);
|
|
|
|
// modify and factor m2
|
|
m2.Update_Decrement(offset);
|
|
if (!m2.LU_Factor(index2)) {
|
|
assert(0);
|
|
}
|
|
m2.LU_MultiplyFactors(m3, index2);
|
|
m2 = m3;
|
|
|
|
u.SetSize(6);
|
|
for (int i = 0; i < size; i++) {
|
|
u[i] = original[index1[offset]][i];
|
|
}
|
|
|
|
// update factors of m1
|
|
m1.LU_UpdateDecrement(v, w, u, offset, index1);
|
|
m1.LU_MultiplyFactors(m3, index1);
|
|
m1 = m3;
|
|
|
|
if (!m1.Compare(m2, 1e-3f)) {
|
|
idLib::common->Warning("idMatX::LU_UpdateDecrement failed");
|
|
}
|
|
}
|
|
|
|
/*
|
|
idMatX::LU_Inverse
|
|
*/
|
|
|
|
m2 = original;
|
|
|
|
m2.LU_Factor(NULL);
|
|
m2.LU_Inverse(m1, NULL);
|
|
m1 *= original;
|
|
|
|
if (!m1.IsIdentity(1e-4f)) {
|
|
idLib::common->Warning("idMatX::LU_Inverse failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::QR_Factor
|
|
*/
|
|
|
|
c.SetSize(size);
|
|
d.SetSize(size);
|
|
|
|
m1 = original;
|
|
|
|
m1.QR_Factor(c, d);
|
|
m1.QR_UnpackFactors(q1, r1, c, d);
|
|
m1 = q1 * r1;
|
|
|
|
if (!original.Compare(m1, 1e-4f)) {
|
|
idLib::common->Warning("idMatX::QR_Factor failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::QR_UpdateRankOne
|
|
*/
|
|
|
|
c.SetSize(size);
|
|
d.SetSize(size);
|
|
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
w.Random(size, 0);
|
|
v = w;
|
|
|
|
// factor m1
|
|
m1.QR_Factor(c, d);
|
|
m1.QR_UnpackFactors(q1, r1, c, d);
|
|
|
|
// modify and factor m2
|
|
m2.Update_RankOne(v, w, 1.0f);
|
|
if (!m2.QR_Factor(c, d)) {
|
|
assert(0);
|
|
}
|
|
m2.QR_UnpackFactors(q2, r2, c, d);
|
|
m2 = q2 * r2;
|
|
|
|
// update factored m1
|
|
q1.QR_UpdateRankOne(r1, v, w, 1.0f);
|
|
m1 = q1 * r1;
|
|
|
|
if (!m1.Compare(m2, 1e-4f)) {
|
|
idLib::common->Warning("idMatX::QR_UpdateRankOne failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::QR_UpdateRowColumn
|
|
*/
|
|
|
|
for (offset = 0; offset < size; offset++) {
|
|
c.SetSize(size);
|
|
d.SetSize(size);
|
|
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
v.Random(size, 1);
|
|
w.Random(size, 2);
|
|
w[offset] = 0.0f;
|
|
|
|
// factor m1
|
|
m1.QR_Factor(c, d);
|
|
m1.QR_UnpackFactors(q1, r1, c, d);
|
|
|
|
// modify and factor m2
|
|
m2.Update_RowColumn(v, w, offset);
|
|
if (!m2.QR_Factor(c, d)) {
|
|
assert(0);
|
|
}
|
|
m2.QR_UnpackFactors(q2, r2, c, d);
|
|
m2 = q2 * r2;
|
|
|
|
// update m1
|
|
q1.QR_UpdateRowColumn(r1, v, w, offset);
|
|
m1 = q1 * r1;
|
|
|
|
if (!m1.Compare(m2, 1e-3f)) {
|
|
idLib::common->Warning("idMatX::QR_UpdateRowColumn failed");
|
|
}
|
|
}
|
|
|
|
/*
|
|
idMatX::QR_UpdateIncrement
|
|
*/
|
|
|
|
c.SetSize(size + 1);
|
|
d.SetSize(size + 1);
|
|
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
v.Random(size + 1, 1);
|
|
w.Random(size + 1, 2);
|
|
w[size] = 0.0f;
|
|
|
|
// factor m1
|
|
m1.QR_Factor(c, d);
|
|
m1.QR_UnpackFactors(q1, r1, c, d);
|
|
|
|
// modify and factor m2
|
|
m2.Update_Increment(v, w);
|
|
if (!m2.QR_Factor(c, d)) {
|
|
assert(0);
|
|
}
|
|
m2.QR_UnpackFactors(q2, r2, c, d);
|
|
m2 = q2 * r2;
|
|
|
|
// update factored m1
|
|
q1.QR_UpdateIncrement(r1, v, w);
|
|
m1 = q1 * r1;
|
|
|
|
if (!m1.Compare(m2, 1e-4f)) {
|
|
idLib::common->Warning("idMatX::QR_UpdateIncrement failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::QR_UpdateDecrement
|
|
*/
|
|
|
|
for (offset = 0; offset < size; offset++) {
|
|
c.SetSize(size + 1);
|
|
d.SetSize(size + 1);
|
|
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
v.SetSize(6);
|
|
w.SetSize(6);
|
|
for (int i = 0; i < size; i++) {
|
|
v[i] = original[i][offset];
|
|
w[i] = original[offset][i];
|
|
}
|
|
|
|
// factor m1
|
|
m1.QR_Factor(c, d);
|
|
m1.QR_UnpackFactors(q1, r1, c, d);
|
|
|
|
// modify and factor m2
|
|
m2.Update_Decrement(offset);
|
|
if (!m2.QR_Factor(c, d)) {
|
|
assert(0);
|
|
}
|
|
m2.QR_UnpackFactors(q2, r2, c, d);
|
|
m2 = q2 * r2;
|
|
|
|
// update factors of m1
|
|
q1.QR_UpdateDecrement(r1, v, w, offset);
|
|
m1 = q1 * r1;
|
|
|
|
if (!m1.Compare(m2, 1e-3f)) {
|
|
idLib::common->Warning("idMatX::QR_UpdateDecrement failed");
|
|
}
|
|
}
|
|
|
|
/*
|
|
idMatX::QR_Inverse
|
|
*/
|
|
|
|
m2 = original;
|
|
|
|
m2.QR_Factor(c, d);
|
|
m2.QR_Inverse(m1, c, d);
|
|
m1 *= original;
|
|
|
|
if (!m1.IsIdentity(1e-4f)) {
|
|
idLib::common->Warning("idMatX::QR_Inverse failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::SVD_Factor
|
|
*/
|
|
|
|
m1 = original;
|
|
m3.Zero(size, size);
|
|
w.Zero(size);
|
|
|
|
m1.SVD_Factor(w, m3);
|
|
m2.Diag(w);
|
|
m3.TransposeSelf();
|
|
m1 = m1 * m2 * m3;
|
|
|
|
if (!original.Compare(m1, 1e-4f)) {
|
|
idLib::common->Warning("idMatX::SVD_Factor failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::SVD_Inverse
|
|
*/
|
|
|
|
m2 = original;
|
|
|
|
m2.SVD_Factor(w, m3);
|
|
m2.SVD_Inverse(m1, w, m3);
|
|
m1 *= original;
|
|
|
|
if (!m1.IsIdentity(1e-4f)) {
|
|
idLib::common->Warning("idMatX::SVD_Inverse failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::Cholesky_Factor
|
|
*/
|
|
|
|
m1 = original;
|
|
|
|
m1.Cholesky_Factor();
|
|
m1.Cholesky_MultiplyFactors(m2);
|
|
|
|
if (!original.Compare(m2, 1e-4f)) {
|
|
idLib::common->Warning("idMatX::Cholesky_Factor failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::Cholesky_UpdateRankOne
|
|
*/
|
|
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
w.Random(size, 0);
|
|
|
|
// factor m1
|
|
m1.Cholesky_Factor();
|
|
m1.ClearUpperTriangle();
|
|
|
|
// modify and factor m2
|
|
m2.Update_RankOneSymmetric(w, 1.0f);
|
|
if (!m2.Cholesky_Factor()) {
|
|
assert(0);
|
|
}
|
|
m2.ClearUpperTriangle();
|
|
|
|
// update factored m1
|
|
m1.Cholesky_UpdateRankOne(w, 1.0f, 0);
|
|
|
|
if (!m1.Compare(m2, 1e-4f)) {
|
|
idLib::common->Warning("idMatX::Cholesky_UpdateRankOne failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::Cholesky_UpdateRowColumn
|
|
*/
|
|
|
|
for (offset = 0; offset < size; offset++) {
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
// factor m1
|
|
m1.Cholesky_Factor();
|
|
m1.ClearUpperTriangle();
|
|
|
|
int pdtable[] = { 1, 0, 1, 0, 0, 0 };
|
|
w.Random(size, pdtable[offset]);
|
|
w *= 0.1f;
|
|
|
|
// modify and factor m2
|
|
m2.Update_RowColumnSymmetric(w, offset);
|
|
if (!m2.Cholesky_Factor()) {
|
|
assert(0);
|
|
}
|
|
m2.ClearUpperTriangle();
|
|
|
|
// update m1
|
|
m1.Cholesky_UpdateRowColumn(w, offset);
|
|
|
|
if (!m1.Compare(m2, 1e-3f)) {
|
|
idLib::common->Warning("idMatX::Cholesky_UpdateRowColumn failed");
|
|
}
|
|
}
|
|
|
|
/*
|
|
idMatX::Cholesky_UpdateIncrement
|
|
*/
|
|
|
|
m1.Random(size + 1, size + 1, 0);
|
|
m3 = m1 * m1.Transpose();
|
|
|
|
m1.SquareSubMatrix(m3, size);
|
|
m2 = m1;
|
|
|
|
w.SetSize(size + 1);
|
|
for (int i = 0; i < size + 1; i++) {
|
|
w[i] = m3[size][i];
|
|
}
|
|
|
|
// factor m1
|
|
m1.Cholesky_Factor();
|
|
|
|
// modify and factor m2
|
|
m2.Update_IncrementSymmetric(w);
|
|
if (!m2.Cholesky_Factor()) {
|
|
assert(0);
|
|
}
|
|
|
|
// update factored m1
|
|
m1.Cholesky_UpdateIncrement(w);
|
|
|
|
m1.ClearUpperTriangle();
|
|
m2.ClearUpperTriangle();
|
|
|
|
if (!m1.Compare(m2, 1e-4f)) {
|
|
idLib::common->Warning("idMatX::Cholesky_UpdateIncrement failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::Cholesky_UpdateDecrement
|
|
*/
|
|
|
|
for (offset = 0; offset < size; offset += size - 1) {
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
v.SetSize(6);
|
|
for (int i = 0; i < size; i++) {
|
|
v[i] = original[i][offset];
|
|
}
|
|
|
|
// factor m1
|
|
m1.Cholesky_Factor();
|
|
|
|
// modify and factor m2
|
|
m2.Update_Decrement(offset);
|
|
if (!m2.Cholesky_Factor()) {
|
|
assert(0);
|
|
}
|
|
|
|
// update factors of m1
|
|
m1.Cholesky_UpdateDecrement(v, offset);
|
|
|
|
if (!m1.Compare(m2, 1e-3f)) {
|
|
idLib::common->Warning("idMatX::Cholesky_UpdateDecrement failed");
|
|
}
|
|
}
|
|
|
|
/*
|
|
idMatX::Cholesky_Inverse
|
|
*/
|
|
|
|
m2 = original;
|
|
|
|
m2.Cholesky_Factor();
|
|
m2.Cholesky_Inverse(m1);
|
|
m1 *= original;
|
|
|
|
if (!m1.IsIdentity(1e-4f)) {
|
|
idLib::common->Warning("idMatX::Cholesky_Inverse failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::LDLT_Factor
|
|
*/
|
|
|
|
m1 = original;
|
|
|
|
m1.LDLT_Factor();
|
|
m1.LDLT_MultiplyFactors(m2);
|
|
|
|
if (!original.Compare(m2, 1e-4f)) {
|
|
idLib::common->Warning("idMatX::LDLT_Factor failed");
|
|
}
|
|
|
|
m1.LDLT_UnpackFactors(m2, m3);
|
|
m2 = m2 * m3 * m2.Transpose();
|
|
|
|
if (!original.Compare(m2, 1e-4f)) {
|
|
idLib::common->Warning("idMatX::LDLT_Factor failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::LDLT_UpdateRankOne
|
|
*/
|
|
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
w.Random(size, 0);
|
|
|
|
// factor m1
|
|
m1.LDLT_Factor();
|
|
m1.ClearUpperTriangle();
|
|
|
|
// modify and factor m2
|
|
m2.Update_RankOneSymmetric(w, 1.0f);
|
|
if (!m2.LDLT_Factor()) {
|
|
assert(0);
|
|
}
|
|
m2.ClearUpperTriangle();
|
|
|
|
// update factored m1
|
|
m1.LDLT_UpdateRankOne(w, 1.0f, 0);
|
|
|
|
if (!m1.Compare(m2, 1e-4f)) {
|
|
idLib::common->Warning("idMatX::LDLT_UpdateRankOne failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::LDLT_UpdateRowColumn
|
|
*/
|
|
|
|
for (offset = 0; offset < size; offset++) {
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
w.Random(size, 0);
|
|
|
|
// factor m1
|
|
m1.LDLT_Factor();
|
|
m1.ClearUpperTriangle();
|
|
|
|
// modify and factor m2
|
|
m2.Update_RowColumnSymmetric(w, offset);
|
|
if (!m2.LDLT_Factor()) {
|
|
assert(0);
|
|
}
|
|
m2.ClearUpperTriangle();
|
|
|
|
// update m1
|
|
m1.LDLT_UpdateRowColumn(w, offset);
|
|
|
|
if (!m1.Compare(m2, 1e-3f)) {
|
|
idLib::common->Warning("idMatX::LDLT_UpdateRowColumn failed");
|
|
}
|
|
}
|
|
|
|
/*
|
|
idMatX::LDLT_UpdateIncrement
|
|
*/
|
|
|
|
m1.Random(size + 1, size + 1, 0);
|
|
m3 = m1 * m1.Transpose();
|
|
|
|
m1.SquareSubMatrix(m3, size);
|
|
m2 = m1;
|
|
|
|
w.SetSize(size + 1);
|
|
for (int i = 0; i < size + 1; i++) {
|
|
w[i] = m3[size][i];
|
|
}
|
|
|
|
// factor m1
|
|
m1.LDLT_Factor();
|
|
|
|
// modify and factor m2
|
|
m2.Update_IncrementSymmetric(w);
|
|
if (!m2.LDLT_Factor()) {
|
|
assert(0);
|
|
}
|
|
|
|
// update factored m1
|
|
m1.LDLT_UpdateIncrement(w);
|
|
|
|
m1.ClearUpperTriangle();
|
|
m2.ClearUpperTriangle();
|
|
|
|
if (!m1.Compare(m2, 1e-4f)) {
|
|
idLib::common->Warning("idMatX::LDLT_UpdateIncrement failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::LDLT_UpdateDecrement
|
|
*/
|
|
|
|
for (offset = 0; offset < size; offset++) {
|
|
m1 = original;
|
|
m2 = original;
|
|
|
|
v.SetSize(6);
|
|
for (int i = 0; i < size; i++) {
|
|
v[i] = original[i][offset];
|
|
}
|
|
|
|
// factor m1
|
|
m1.LDLT_Factor();
|
|
|
|
// modify and factor m2
|
|
m2.Update_Decrement(offset);
|
|
if (!m2.LDLT_Factor()) {
|
|
assert(0);
|
|
}
|
|
|
|
// update factors of m1
|
|
m1.LDLT_UpdateDecrement(v, offset);
|
|
|
|
if (!m1.Compare(m2, 1e-3f)) {
|
|
idLib::common->Warning("idMatX::LDLT_UpdateDecrement failed");
|
|
}
|
|
}
|
|
|
|
/*
|
|
idMatX::LDLT_Inverse
|
|
*/
|
|
|
|
m2 = original;
|
|
|
|
m2.LDLT_Factor();
|
|
m2.LDLT_Inverse(m1);
|
|
m1 *= original;
|
|
|
|
if (!m1.IsIdentity(1e-4f)) {
|
|
idLib::common->Warning("idMatX::LDLT_Inverse failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::Eigen_SolveSymmetricTriDiagonal
|
|
*/
|
|
|
|
m3 = original;
|
|
m3.TriDiagonal_ClearTriangles();
|
|
m1 = m3;
|
|
|
|
v.SetSize(size);
|
|
|
|
m1.Eigen_SolveSymmetricTriDiagonal(v);
|
|
|
|
m3.TransposeMultiply(m2, m1);
|
|
|
|
for (int i = 0; i < size; i++) {
|
|
for (int j = 0; j < size; j++) {
|
|
m1[i][j] *= v[j];
|
|
}
|
|
}
|
|
|
|
if (!m1.Compare(m2, 1e-4f)) {
|
|
idLib::common->Warning("idMatX::Eigen_SolveSymmetricTriDiagonal failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::Eigen_SolveSymmetric
|
|
*/
|
|
|
|
m3 = original;
|
|
m1 = m3;
|
|
|
|
v.SetSize(size);
|
|
|
|
m1.Eigen_SolveSymmetric(v);
|
|
|
|
m3.TransposeMultiply(m2, m1);
|
|
|
|
for (int i = 0; i < size; i++) {
|
|
for (int j = 0; j < size; j++) {
|
|
m1[i][j] *= v[j];
|
|
}
|
|
}
|
|
|
|
if (!m1.Compare(m2, 1e-4f)) {
|
|
idLib::common->Warning("idMatX::Eigen_SolveSymmetric failed");
|
|
}
|
|
|
|
/*
|
|
idMatX::Eigen_Solve
|
|
*/
|
|
|
|
m3 = original;
|
|
m1 = m3;
|
|
|
|
v.SetSize(size);
|
|
w.SetSize(size);
|
|
|
|
m1.Eigen_Solve(v, w);
|
|
|
|
m3.TransposeMultiply(m2, m1);
|
|
|
|
for (int i = 0; i < size; i++) {
|
|
for (int j = 0; j < size; j++) {
|
|
m1[i][j] *= v[j];
|
|
}
|
|
}
|
|
|
|
if (!m1.Compare(m2, 1e-4f)) {
|
|
idLib::common->Warning("idMatX::Eigen_Solve failed");
|
|
}
|
|
}
|