1943 lines
59 KiB
C++
1943 lines
59 KiB
C++
/**
|
|
@file Matrix3.cpp
|
|
|
|
3x3 matrix class
|
|
|
|
@author Morgan McGuire, graphics3d.com
|
|
|
|
@created 2001-06-02
|
|
@edited 2010-08-15
|
|
|
|
Copyright 2000-2012, Morgan McGuire.
|
|
All rights reserved.
|
|
*/
|
|
|
|
#include "G3D/platform.h"
|
|
#include <memory.h>
|
|
#include <assert.h>
|
|
#include "G3D/Matrix3.h"
|
|
#include "G3D/g3dmath.h"
|
|
#include "G3D/BinaryInput.h"
|
|
#include "G3D/BinaryOutput.h"
|
|
#include "G3D/Quat.h"
|
|
#include "G3D/Any.h"
|
|
|
|
namespace G3D {
|
|
|
|
const float Matrix3::EPSILON = 1e-06f;
|
|
|
|
Matrix3::Matrix3(const Any& any) {
|
|
any.verifyName("Matrix3");
|
|
any.verifyType(Any::ARRAY);
|
|
|
|
if (any.nameEquals("Matrix3::fromAxisAngle")) {
|
|
any.verifySize(2);
|
|
*this = fromAxisAngle(Vector3(any[0]), any[1].floatValue());
|
|
} else if (any.nameEquals("Matrix3::diagonal")) {
|
|
any.verifySize(3);
|
|
*this = diagonal(any[0], any[1], any[2]);
|
|
} else if (any.nameEquals("Matrix3::identity")) {
|
|
*this = identity();
|
|
} else {
|
|
any.verifySize(9);
|
|
|
|
for (int r = 0; r < 3; ++r) {
|
|
for (int c = 0; c < 3; ++c) {
|
|
elt[r][c] = any[r * 3 + c];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
Any Matrix3::toAny() const {
|
|
Any any(Any::ARRAY, "Matrix3");
|
|
any.resize(9);
|
|
for (int r = 0; r < 3; ++r) {
|
|
for (int c = 0; c < 3; ++c) {
|
|
any[r * 3 + c] = elt[r][c];
|
|
}
|
|
}
|
|
|
|
return any;
|
|
}
|
|
|
|
const Matrix3& Matrix3::zero() {
|
|
static Matrix3 m(0, 0, 0, 0, 0, 0, 0, 0, 0);
|
|
return m;
|
|
}
|
|
|
|
const Matrix3& Matrix3::identity() {
|
|
static Matrix3 m(1, 0, 0, 0, 1, 0, 0, 0, 1);
|
|
return m;
|
|
}
|
|
|
|
|
|
const float Matrix3::ms_fSvdEpsilon = 1e-04f;
|
|
const int Matrix3::ms_iSvdMaxIterations = 32;
|
|
|
|
Matrix3::Matrix3(BinaryInput& b) {
|
|
deserialize(b);
|
|
}
|
|
|
|
bool Matrix3::fuzzyEq(const Matrix3& b) const {
|
|
for (int r = 0; r < 3; ++r) {
|
|
for (int c = 0; c < 3; ++c) {
|
|
if (! G3D::fuzzyEq(elt[r][c], b[r][c])) {
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
|
|
bool Matrix3::isRightHanded() const{
|
|
|
|
const Vector3& X = column(0);
|
|
const Vector3& Y = column(1);
|
|
const Vector3& Z = column(2);
|
|
|
|
const Vector3& W = X.cross(Y);
|
|
|
|
return W.dot(Z) > 0.0f;
|
|
}
|
|
|
|
|
|
bool Matrix3::isOrthonormal() const {
|
|
const Vector3& X = column(0);
|
|
const Vector3& Y = column(1);
|
|
const Vector3& Z = column(2);
|
|
|
|
return
|
|
(G3D::fuzzyEq(X.dot(Y), 0.0f) &&
|
|
G3D::fuzzyEq(Y.dot(Z), 0.0f) &&
|
|
G3D::fuzzyEq(X.dot(Z), 0.0f) &&
|
|
G3D::fuzzyEq(X.squaredMagnitude(), 1.0f) &&
|
|
G3D::fuzzyEq(Y.squaredMagnitude(), 1.0f) &&
|
|
G3D::fuzzyEq(Z.squaredMagnitude(), 1.0f));
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
Matrix3::Matrix3(const Quat& _q) {
|
|
// Implementation from Watt and Watt, pg 362
|
|
// See also http://www.flipcode.com/documents/matrfaq.html#Q54
|
|
Quat q = _q;
|
|
q.unitize();
|
|
float xx = 2.0f * q.x * q.x;
|
|
float xy = 2.0f * q.x * q.y;
|
|
float xz = 2.0f * q.x * q.z;
|
|
float xw = 2.0f * q.x * q.w;
|
|
|
|
float yy = 2.0f * q.y * q.y;
|
|
float yz = 2.0f * q.y * q.z;
|
|
float yw = 2.0f * q.y * q.w;
|
|
|
|
float zz = 2.0f * q.z * q.z;
|
|
float zw = 2.0f * q.z * q.w;
|
|
|
|
set(1.0f - yy - zz, xy - zw, xz + yw,
|
|
xy + zw, 1.0f - xx - zz, yz - xw,
|
|
xz - yw, yz + xw, 1.0f - xx - yy);
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
|
|
Matrix3::Matrix3 (const float aafEntry[3][3]) {
|
|
memcpy(elt, aafEntry, 9*sizeof(float));
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
Matrix3::Matrix3 (const Matrix3& rkMatrix) {
|
|
memcpy(elt, rkMatrix.elt, 9*sizeof(float));
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
Matrix3::Matrix3(
|
|
float fEntry00, float fEntry01, float fEntry02,
|
|
float fEntry10, float fEntry11, float fEntry12,
|
|
float fEntry20, float fEntry21, float fEntry22) {
|
|
set(fEntry00, fEntry01, fEntry02,
|
|
fEntry10, fEntry11, fEntry12,
|
|
fEntry20, fEntry21, fEntry22);
|
|
}
|
|
|
|
void Matrix3::set(
|
|
float fEntry00, float fEntry01, float fEntry02,
|
|
float fEntry10, float fEntry11, float fEntry12,
|
|
float fEntry20, float fEntry21, float fEntry22) {
|
|
|
|
elt[0][0] = fEntry00;
|
|
elt[0][1] = fEntry01;
|
|
elt[0][2] = fEntry02;
|
|
elt[1][0] = fEntry10;
|
|
elt[1][1] = fEntry11;
|
|
elt[1][2] = fEntry12;
|
|
elt[2][0] = fEntry20;
|
|
elt[2][1] = fEntry21;
|
|
elt[2][2] = fEntry22;
|
|
}
|
|
|
|
|
|
void Matrix3::deserialize(BinaryInput& b) {
|
|
int r,c;
|
|
for (c = 0; c < 3; ++c) {
|
|
for (r = 0; r < 3; ++r) {
|
|
elt[r][c] = b.readFloat32();
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void Matrix3::serialize(BinaryOutput& b) const {
|
|
int r,c;
|
|
for (c = 0; c < 3; ++c) {
|
|
for (r = 0; r < 3; ++r) {
|
|
b.writeFloat32(elt[r][c]);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
Vector3 Matrix3::column (int iCol) const {
|
|
assert((0 <= iCol) && (iCol < 3));
|
|
return Vector3(elt[0][iCol], elt[1][iCol],
|
|
elt[2][iCol]);
|
|
}
|
|
|
|
|
|
const Vector3& Matrix3::row (int iRow) const {
|
|
assert((0 <= iRow) && (iRow < 3));
|
|
return *reinterpret_cast<const Vector3*>(elt[iRow]);
|
|
}
|
|
|
|
|
|
void Matrix3::setColumn(int iCol, const Vector3 &vector) {
|
|
debugAssert((iCol >= 0) && (iCol < 3));
|
|
elt[0][iCol] = vector.x;
|
|
elt[1][iCol] = vector.y;
|
|
elt[2][iCol] = vector.z;
|
|
}
|
|
|
|
|
|
void Matrix3::setRow(int iRow, const Vector3 &vector) {
|
|
debugAssert((iRow >= 0) && (iRow < 3));
|
|
elt[iRow][0] = vector.x;
|
|
elt[iRow][1] = vector.y;
|
|
elt[iRow][2] = vector.z;
|
|
}
|
|
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool Matrix3::operator== (const Matrix3& rkMatrix) const {
|
|
for (int iRow = 0; iRow < 3; ++iRow) {
|
|
for (int iCol = 0; iCol < 3; ++iCol) {
|
|
if ( elt[iRow][iCol] != rkMatrix.elt[iRow][iCol] )
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool Matrix3::operator!= (const Matrix3& rkMatrix) const {
|
|
return !operator==(rkMatrix);
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
Matrix3 Matrix3::operator+ (const Matrix3& rkMatrix) const {
|
|
Matrix3 kSum;
|
|
|
|
for (int iRow = 0; iRow < 3; ++iRow) {
|
|
for (int iCol = 0; iCol < 3; ++iCol) {
|
|
kSum.elt[iRow][iCol] = elt[iRow][iCol] +
|
|
rkMatrix.elt[iRow][iCol];
|
|
}
|
|
}
|
|
|
|
return kSum;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
Matrix3 Matrix3::operator- (const Matrix3& rkMatrix) const {
|
|
Matrix3 kDiff;
|
|
|
|
for (int iRow = 0; iRow < 3; ++iRow) {
|
|
for (int iCol = 0; iCol < 3; ++iCol) {
|
|
kDiff.elt[iRow][iCol] = elt[iRow][iCol] -
|
|
rkMatrix.elt[iRow][iCol];
|
|
}
|
|
}
|
|
|
|
return kDiff;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
Matrix3 Matrix3::operator* (const Matrix3& rkMatrix) const {
|
|
Matrix3 kProd;
|
|
|
|
for (int iRow = 0; iRow < 3; ++iRow) {
|
|
for (int iCol = 0; iCol < 3; ++iCol) {
|
|
kProd.elt[iRow][iCol] =
|
|
elt[iRow][0] * rkMatrix.elt[0][iCol] +
|
|
elt[iRow][1] * rkMatrix.elt[1][iCol] +
|
|
elt[iRow][2] * rkMatrix.elt[2][iCol];
|
|
}
|
|
}
|
|
|
|
return kProd;
|
|
}
|
|
|
|
Matrix3& Matrix3::operator+= (const Matrix3& rkMatrix) {
|
|
for (int iRow = 0; iRow < 3; ++iRow) {
|
|
for (int iCol = 0; iCol < 3; ++iCol) {
|
|
elt[iRow][iCol] = elt[iRow][iCol] + rkMatrix.elt[iRow][iCol];
|
|
}
|
|
}
|
|
|
|
return *this;
|
|
}
|
|
|
|
Matrix3& Matrix3::operator-= (const Matrix3& rkMatrix) {
|
|
for (int iRow = 0; iRow < 3; ++iRow) {
|
|
for (int iCol = 0; iCol < 3; ++iCol) {
|
|
elt[iRow][iCol] = elt[iRow][iCol] - rkMatrix.elt[iRow][iCol];
|
|
}
|
|
}
|
|
|
|
return *this;
|
|
}
|
|
|
|
Matrix3& Matrix3::operator*= (const Matrix3& rkMatrix) {
|
|
Matrix3 mulMat;
|
|
for (int iRow = 0; iRow < 3; ++iRow) {
|
|
for (int iCol = 0; iCol < 3; ++iCol) {
|
|
mulMat.elt[iRow][iCol] =
|
|
elt[iRow][0] * rkMatrix.elt[0][iCol] +
|
|
elt[iRow][1] * rkMatrix.elt[1][iCol] +
|
|
elt[iRow][2] * rkMatrix.elt[2][iCol];
|
|
}
|
|
}
|
|
|
|
*this = mulMat;
|
|
return *this;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
Matrix3 Matrix3::operator- () const {
|
|
Matrix3 kNeg;
|
|
|
|
for (int iRow = 0; iRow < 3; ++iRow) {
|
|
for (int iCol = 0; iCol < 3; ++iCol) {
|
|
kNeg[iRow][iCol] = -elt[iRow][iCol];
|
|
}
|
|
}
|
|
|
|
return kNeg;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
Matrix3 Matrix3::operator* (float fScalar) const {
|
|
Matrix3 kProd;
|
|
|
|
for (int iRow = 0; iRow < 3; ++iRow) {
|
|
for (int iCol = 0; iCol < 3; ++iCol) {
|
|
kProd[iRow][iCol] = fScalar * elt[iRow][iCol];
|
|
}
|
|
}
|
|
|
|
return kProd;
|
|
}
|
|
|
|
Matrix3& Matrix3::operator/= (float fScalar) {
|
|
return *this *= (1.0f / fScalar);
|
|
}
|
|
|
|
Matrix3& Matrix3::operator*= (float fScalar) {
|
|
|
|
for (int iRow = 0; iRow < 3; ++iRow) {
|
|
for (int iCol = 0; iCol < 3; ++iCol) {
|
|
elt[iRow][iCol] *= fScalar;
|
|
}
|
|
}
|
|
|
|
return *this;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
Matrix3 operator* (double fScalar, const Matrix3& rkMatrix) {
|
|
Matrix3 kProd;
|
|
|
|
for (int iRow = 0; iRow < 3; ++iRow) {
|
|
for (int iCol = 0; iCol < 3; ++iCol) {
|
|
kProd[iRow][iCol] = (float)fScalar * rkMatrix.elt[iRow][iCol];
|
|
}
|
|
}
|
|
|
|
return kProd;
|
|
}
|
|
|
|
Matrix3 operator* (float fScalar, const Matrix3& rkMatrix) {
|
|
return (double)fScalar * rkMatrix;
|
|
}
|
|
|
|
|
|
Matrix3 operator* (int fScalar, const Matrix3& rkMatrix) {
|
|
return (double)fScalar * rkMatrix;
|
|
}
|
|
//----------------------------------------------------------------------------
|
|
Matrix3 Matrix3::transpose () const {
|
|
Matrix3 kTranspose;
|
|
|
|
for (int iRow = 0; iRow < 3; ++iRow) {
|
|
for (int iCol = 0; iCol < 3; ++iCol) {
|
|
kTranspose[iRow][iCol] = elt[iCol][iRow];
|
|
}
|
|
}
|
|
|
|
return kTranspose;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool Matrix3::inverse (Matrix3& rkInverse, float fTolerance) const {
|
|
// Invert a 3x3 using cofactors. This is about 8 times faster than
|
|
// the Numerical Recipes code which uses Gaussian elimination.
|
|
|
|
rkInverse[0][0] = elt[1][1] * elt[2][2] -
|
|
elt[1][2] * elt[2][1];
|
|
rkInverse[0][1] = elt[0][2] * elt[2][1] -
|
|
elt[0][1] * elt[2][2];
|
|
rkInverse[0][2] = elt[0][1] * elt[1][2] -
|
|
elt[0][2] * elt[1][1];
|
|
rkInverse[1][0] = elt[1][2] * elt[2][0] -
|
|
elt[1][0] * elt[2][2];
|
|
rkInverse[1][1] = elt[0][0] * elt[2][2] -
|
|
elt[0][2] * elt[2][0];
|
|
rkInverse[1][2] = elt[0][2] * elt[1][0] -
|
|
elt[0][0] * elt[1][2];
|
|
rkInverse[2][0] = elt[1][0] * elt[2][1] -
|
|
elt[1][1] * elt[2][0];
|
|
rkInverse[2][1] = elt[0][1] * elt[2][0] -
|
|
elt[0][0] * elt[2][1];
|
|
rkInverse[2][2] = elt[0][0] * elt[1][1] -
|
|
elt[0][1] * elt[1][0];
|
|
|
|
float fDet =
|
|
elt[0][0] * rkInverse[0][0] +
|
|
elt[0][1] * rkInverse[1][0] +
|
|
elt[0][2] * rkInverse[2][0];
|
|
|
|
if ( G3D::abs(fDet) <= fTolerance )
|
|
return false;
|
|
|
|
float fInvDet = 1.0f / fDet;
|
|
|
|
for (int iRow = 0; iRow < 3; ++iRow) {
|
|
for (int iCol = 0; iCol < 3; ++iCol)
|
|
rkInverse[iRow][iCol] *= fInvDet;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
Matrix3 Matrix3::inverse (float fTolerance) const {
|
|
Matrix3 kInverse = Matrix3::zero();
|
|
inverse(kInverse, fTolerance);
|
|
return kInverse;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
float Matrix3::determinant () const {
|
|
float fCofactor00 = elt[1][1] * elt[2][2] -
|
|
elt[1][2] * elt[2][1];
|
|
float fCofactor10 = elt[1][2] * elt[2][0] -
|
|
elt[1][0] * elt[2][2];
|
|
float fCofactor20 = elt[1][0] * elt[2][1] -
|
|
elt[1][1] * elt[2][0];
|
|
|
|
float fDet =
|
|
elt[0][0] * fCofactor00 +
|
|
elt[0][1] * fCofactor10 +
|
|
elt[0][2] * fCofactor20;
|
|
|
|
return fDet;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
void Matrix3::bidiagonalize (Matrix3& kA, Matrix3& kL,
|
|
Matrix3& kR) {
|
|
float afV[3], afW[3];
|
|
float fLength, fSign, fT1, fInvT1, fT2;
|
|
bool bIdentity;
|
|
|
|
// map first column to (*,0,0)
|
|
fLength = sqrt(kA[0][0] * kA[0][0] + kA[1][0] * kA[1][0] +
|
|
kA[2][0] * kA[2][0]);
|
|
|
|
if ( fLength > 0.0 ) {
|
|
fSign = (kA[0][0] > 0.0 ? 1.0f : -1.0f);
|
|
fT1 = (float)kA[0][0] + fSign * fLength;
|
|
fInvT1 = 1.0f / fT1;
|
|
afV[1] = kA[1][0] * fInvT1;
|
|
afV[2] = kA[2][0] * fInvT1;
|
|
|
|
fT2 = -2.0f / (1.0f + afV[1] * afV[1] + afV[2] * afV[2]);
|
|
afW[0] = fT2 * (kA[0][0] + kA[1][0] * afV[1] + kA[2][0] * afV[2]);
|
|
afW[1] = fT2 * (kA[0][1] + kA[1][1] * afV[1] + kA[2][1] * afV[2]);
|
|
afW[2] = fT2 * (kA[0][2] + kA[1][2] * afV[1] + kA[2][2] * afV[2]);
|
|
kA[0][0] += afW[0];
|
|
kA[0][1] += afW[1];
|
|
kA[0][2] += afW[2];
|
|
kA[1][1] += afV[1] * afW[1];
|
|
kA[1][2] += afV[1] * afW[2];
|
|
kA[2][1] += afV[2] * afW[1];
|
|
kA[2][2] += afV[2] * afW[2];
|
|
|
|
kL[0][0] = 1.0f + fT2;
|
|
kL[0][1] = kL[1][0] = fT2 * afV[1];
|
|
kL[0][2] = kL[2][0] = fT2 * afV[2];
|
|
kL[1][1] = 1.0f + fT2 * afV[1] * afV[1];
|
|
kL[1][2] = kL[2][1] = fT2 * afV[1] * afV[2];
|
|
kL[2][2] = 1.0f + fT2 * afV[2] * afV[2];
|
|
bIdentity = false;
|
|
} else {
|
|
kL = Matrix3::identity();
|
|
bIdentity = true;
|
|
}
|
|
|
|
// map first row to (*,*,0)
|
|
fLength = sqrt(kA[0][1] * kA[0][1] + kA[0][2] * kA[0][2]);
|
|
|
|
if ( fLength > 0.0 ) {
|
|
fSign = (kA[0][1] > 0.0 ? 1.0f : -1.0f);
|
|
fT1 = kA[0][1] + fSign * fLength;
|
|
afV[2] = kA[0][2] / fT1;
|
|
|
|
fT2 = -2.0f / (1.0f + afV[2] * afV[2]);
|
|
afW[0] = fT2 * (kA[0][1] + kA[0][2] * afV[2]);
|
|
afW[1] = fT2 * (kA[1][1] + kA[1][2] * afV[2]);
|
|
afW[2] = fT2 * (kA[2][1] + kA[2][2] * afV[2]);
|
|
kA[0][1] += afW[0];
|
|
kA[1][1] += afW[1];
|
|
kA[1][2] += afW[1] * afV[2];
|
|
kA[2][1] += afW[2];
|
|
kA[2][2] += afW[2] * afV[2];
|
|
|
|
kR[0][0] = 1.0f;
|
|
kR[0][1] = kR[1][0] = 0.0f;
|
|
kR[0][2] = kR[2][0] = 0.0f;
|
|
kR[1][1] = 1.0f + fT2;
|
|
kR[1][2] = kR[2][1] = fT2 * afV[2];
|
|
kR[2][2] = 1.0f + fT2 * afV[2] * afV[2];
|
|
} else {
|
|
kR = Matrix3::identity();
|
|
}
|
|
|
|
// map second column to (*,*,0)
|
|
fLength = sqrt(kA[1][1] * kA[1][1] + kA[2][1] * kA[2][1]);
|
|
|
|
if ( fLength > 0.0 ) {
|
|
fSign = (kA[1][1] > 0.0 ? 1.0f : -1.0f);
|
|
fT1 = kA[1][1] + fSign * fLength;
|
|
afV[2] = kA[2][1] / fT1;
|
|
|
|
fT2 = -2.0f / (1.0f + afV[2] * afV[2]);
|
|
afW[1] = fT2 * (kA[1][1] + kA[2][1] * afV[2]);
|
|
afW[2] = fT2 * (kA[1][2] + kA[2][2] * afV[2]);
|
|
kA[1][1] += afW[1];
|
|
kA[1][2] += afW[2];
|
|
kA[2][2] += afV[2] * afW[2];
|
|
|
|
float fA = 1.0f + fT2;
|
|
float fB = fT2 * afV[2];
|
|
float fC = 1.0f + fB * afV[2];
|
|
|
|
if ( bIdentity ) {
|
|
kL[0][0] = 1.0;
|
|
kL[0][1] = kL[1][0] = 0.0;
|
|
kL[0][2] = kL[2][0] = 0.0;
|
|
kL[1][1] = fA;
|
|
kL[1][2] = kL[2][1] = fB;
|
|
kL[2][2] = fC;
|
|
} else {
|
|
for (int iRow = 0; iRow < 3; ++iRow) {
|
|
float fTmp0 = kL[iRow][1];
|
|
float fTmp1 = kL[iRow][2];
|
|
kL[iRow][1] = fA * fTmp0 + fB * fTmp1;
|
|
kL[iRow][2] = fB * fTmp0 + fC * fTmp1;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
void Matrix3::golubKahanStep (Matrix3& kA, Matrix3& kL,
|
|
Matrix3& kR) {
|
|
float fT11 = kA[0][1] * kA[0][1] + kA[1][1] * kA[1][1];
|
|
float fT22 = kA[1][2] * kA[1][2] + kA[2][2] * kA[2][2];
|
|
float fT12 = kA[1][1] * kA[1][2];
|
|
float fTrace = fT11 + fT22;
|
|
float fDiff = fT11 - fT22;
|
|
float fDiscr = sqrt(fDiff * fDiff + 4.0f * fT12 * fT12);
|
|
float fRoot1 = 0.5f * (fTrace + fDiscr);
|
|
float fRoot2 = 0.5f * (fTrace - fDiscr);
|
|
|
|
// adjust right
|
|
float fY = kA[0][0] - (G3D::abs(fRoot1 - fT22) <=
|
|
G3D::abs(fRoot2 - fT22) ? fRoot1 : fRoot2);
|
|
float fZ = kA[0][1];
|
|
float fInvLength = 1.0f / sqrt(fY * fY + fZ * fZ);
|
|
float fSin = fZ * fInvLength;
|
|
float fCos = -fY * fInvLength;
|
|
|
|
float fTmp0 = kA[0][0];
|
|
float fTmp1 = kA[0][1];
|
|
kA[0][0] = fCos * fTmp0 - fSin * fTmp1;
|
|
kA[0][1] = fSin * fTmp0 + fCos * fTmp1;
|
|
kA[1][0] = -fSin * kA[1][1];
|
|
kA[1][1] *= fCos;
|
|
|
|
int iRow;
|
|
|
|
for (iRow = 0; iRow < 3; ++iRow) {
|
|
fTmp0 = kR[0][iRow];
|
|
fTmp1 = kR[1][iRow];
|
|
kR[0][iRow] = fCos * fTmp0 - fSin * fTmp1;
|
|
kR[1][iRow] = fSin * fTmp0 + fCos * fTmp1;
|
|
}
|
|
|
|
// adjust left
|
|
fY = kA[0][0];
|
|
|
|
fZ = kA[1][0];
|
|
|
|
fInvLength = 1.0f / sqrt(fY * fY + fZ * fZ);
|
|
|
|
fSin = fZ * fInvLength;
|
|
|
|
fCos = -fY * fInvLength;
|
|
|
|
kA[0][0] = fCos * kA[0][0] - fSin * kA[1][0];
|
|
|
|
fTmp0 = kA[0][1];
|
|
|
|
fTmp1 = kA[1][1];
|
|
|
|
kA[0][1] = fCos * fTmp0 - fSin * fTmp1;
|
|
|
|
kA[1][1] = fSin * fTmp0 + fCos * fTmp1;
|
|
|
|
kA[0][2] = -fSin * kA[1][2];
|
|
|
|
kA[1][2] *= fCos;
|
|
|
|
int iCol;
|
|
|
|
for (iCol = 0; iCol < 3; ++iCol) {
|
|
fTmp0 = kL[iCol][0];
|
|
fTmp1 = kL[iCol][1];
|
|
kL[iCol][0] = fCos * fTmp0 - fSin * fTmp1;
|
|
kL[iCol][1] = fSin * fTmp0 + fCos * fTmp1;
|
|
}
|
|
|
|
// adjust right
|
|
fY = kA[0][1];
|
|
|
|
fZ = kA[0][2];
|
|
|
|
fInvLength = 1.0f / sqrt(fY * fY + fZ * fZ);
|
|
|
|
fSin = fZ * fInvLength;
|
|
|
|
fCos = -fY * fInvLength;
|
|
|
|
kA[0][1] = fCos * kA[0][1] - fSin * kA[0][2];
|
|
|
|
fTmp0 = kA[1][1];
|
|
|
|
fTmp1 = kA[1][2];
|
|
|
|
kA[1][1] = fCos * fTmp0 - fSin * fTmp1;
|
|
|
|
kA[1][2] = fSin * fTmp0 + fCos * fTmp1;
|
|
|
|
kA[2][1] = -fSin * kA[2][2];
|
|
|
|
kA[2][2] *= fCos;
|
|
|
|
for (iRow = 0; iRow < 3; ++iRow) {
|
|
fTmp0 = kR[1][iRow];
|
|
fTmp1 = kR[2][iRow];
|
|
kR[1][iRow] = fCos * fTmp0 - fSin * fTmp1;
|
|
kR[2][iRow] = fSin * fTmp0 + fCos * fTmp1;
|
|
}
|
|
|
|
// adjust left
|
|
fY = kA[1][1];
|
|
|
|
fZ = kA[2][1];
|
|
|
|
fInvLength = 1.0f / sqrt(fY * fY + fZ * fZ);
|
|
|
|
fSin = fZ * fInvLength;
|
|
|
|
fCos = -fY * fInvLength;
|
|
|
|
kA[1][1] = fCos * kA[1][1] - fSin * kA[2][1];
|
|
|
|
fTmp0 = kA[1][2];
|
|
|
|
fTmp1 = kA[2][2];
|
|
|
|
kA[1][2] = fCos * fTmp0 - fSin * fTmp1;
|
|
|
|
kA[2][2] = fSin * fTmp0 + fCos * fTmp1;
|
|
|
|
for (iCol = 0; iCol < 3; ++iCol) {
|
|
fTmp0 = kL[iCol][1];
|
|
fTmp1 = kL[iCol][2];
|
|
kL[iCol][1] = fCos * fTmp0 - fSin * fTmp1;
|
|
kL[iCol][2] = fSin * fTmp0 + fCos * fTmp1;
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
void Matrix3::singularValueDecomposition (Matrix3& kL, Vector3& kS,
|
|
Matrix3& kR) const {
|
|
int iRow, iCol;
|
|
|
|
Matrix3 kA = *this;
|
|
bidiagonalize(kA, kL, kR);
|
|
|
|
for (int i = 0; i < ms_iSvdMaxIterations; ++i) {
|
|
float fTmp, fTmp0, fTmp1;
|
|
float fSin0, fCos0, fTan0;
|
|
float fSin1, fCos1, fTan1;
|
|
|
|
bool bTest1 = (G3D::abs(kA[0][1]) <=
|
|
ms_fSvdEpsilon * (G3D::abs(kA[0][0]) + G3D::abs(kA[1][1])));
|
|
bool bTest2 = (G3D::abs(kA[1][2]) <=
|
|
ms_fSvdEpsilon * (G3D::abs(kA[1][1]) + G3D::abs(kA[2][2])));
|
|
|
|
if ( bTest1 ) {
|
|
if ( bTest2 ) {
|
|
kS[0] = kA[0][0];
|
|
kS[1] = kA[1][1];
|
|
kS[2] = kA[2][2];
|
|
break;
|
|
} else {
|
|
// 2x2 closed form factorization
|
|
fTmp = (kA[1][1] * kA[1][1] - kA[2][2] * kA[2][2] +
|
|
kA[1][2] * kA[1][2]) / (kA[1][2] * kA[2][2]);
|
|
fTan0 = 0.5f * (fTmp + sqrt(fTmp * fTmp + 4.0f));
|
|
fCos0 = 1.0f / sqrt(1.0f + fTan0 * fTan0);
|
|
fSin0 = fTan0 * fCos0;
|
|
|
|
for (iCol = 0; iCol < 3; ++iCol) {
|
|
fTmp0 = kL[iCol][1];
|
|
fTmp1 = kL[iCol][2];
|
|
kL[iCol][1] = fCos0 * fTmp0 - fSin0 * fTmp1;
|
|
kL[iCol][2] = fSin0 * fTmp0 + fCos0 * fTmp1;
|
|
}
|
|
|
|
fTan1 = (kA[1][2] - kA[2][2] * fTan0) / kA[1][1];
|
|
fCos1 = 1.0f / sqrt(1.0f + fTan1 * fTan1);
|
|
fSin1 = -fTan1 * fCos1;
|
|
|
|
for (iRow = 0; iRow < 3; ++iRow) {
|
|
fTmp0 = kR[1][iRow];
|
|
fTmp1 = kR[2][iRow];
|
|
kR[1][iRow] = fCos1 * fTmp0 - fSin1 * fTmp1;
|
|
kR[2][iRow] = fSin1 * fTmp0 + fCos1 * fTmp1;
|
|
}
|
|
|
|
kS[0] = kA[0][0];
|
|
kS[1] = fCos0 * fCos1 * kA[1][1] -
|
|
fSin1 * (fCos0 * kA[1][2] - fSin0 * kA[2][2]);
|
|
kS[2] = fSin0 * fSin1 * kA[1][1] +
|
|
fCos1 * (fSin0 * kA[1][2] + fCos0 * kA[2][2]);
|
|
break;
|
|
}
|
|
} else {
|
|
if ( bTest2 ) {
|
|
// 2x2 closed form factorization
|
|
fTmp = (kA[0][0] * kA[0][0] + kA[1][1] * kA[1][1] -
|
|
kA[0][1] * kA[0][1]) / (kA[0][1] * kA[1][1]);
|
|
fTan0 = 0.5f * ( -fTmp + sqrt(fTmp * fTmp + 4.0f));
|
|
fCos0 = 1.0f / sqrt(1.0f + fTan0 * fTan0);
|
|
fSin0 = fTan0 * fCos0;
|
|
|
|
for (iCol = 0; iCol < 3; ++iCol) {
|
|
fTmp0 = kL[iCol][0];
|
|
fTmp1 = kL[iCol][1];
|
|
kL[iCol][0] = fCos0 * fTmp0 - fSin0 * fTmp1;
|
|
kL[iCol][1] = fSin0 * fTmp0 + fCos0 * fTmp1;
|
|
}
|
|
|
|
fTan1 = (kA[0][1] - kA[1][1] * fTan0) / kA[0][0];
|
|
fCos1 = 1.0f / sqrt(1.0f + fTan1 * fTan1);
|
|
fSin1 = -fTan1 * fCos1;
|
|
|
|
for (iRow = 0; iRow < 3; ++iRow) {
|
|
fTmp0 = kR[0][iRow];
|
|
fTmp1 = kR[1][iRow];
|
|
kR[0][iRow] = fCos1 * fTmp0 - fSin1 * fTmp1;
|
|
kR[1][iRow] = fSin1 * fTmp0 + fCos1 * fTmp1;
|
|
}
|
|
|
|
kS[0] = fCos0 * fCos1 * kA[0][0] -
|
|
fSin1 * (fCos0 * kA[0][1] - fSin0 * kA[1][1]);
|
|
kS[1] = fSin0 * fSin1 * kA[0][0] +
|
|
fCos1 * (fSin0 * kA[0][1] + fCos0 * kA[1][1]);
|
|
kS[2] = kA[2][2];
|
|
break;
|
|
} else {
|
|
golubKahanStep(kA, kL, kR);
|
|
}
|
|
}
|
|
}
|
|
|
|
// positize diagonal
|
|
for (iRow = 0; iRow < 3; ++iRow) {
|
|
if ( kS[iRow] < 0.0 ) {
|
|
kS[iRow] = -kS[iRow];
|
|
|
|
for (iCol = 0; iCol < 3; ++iCol)
|
|
kR[iRow][iCol] = -kR[iRow][iCol];
|
|
}
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
void Matrix3::singularValueComposition (const Matrix3& kL,
|
|
const Vector3& kS, const Matrix3& kR) {
|
|
int iRow, iCol;
|
|
Matrix3 kTmp;
|
|
|
|
// product S*R
|
|
for (iRow = 0; iRow < 3; ++iRow) {
|
|
for (iCol = 0; iCol < 3; ++iCol)
|
|
kTmp[iRow][iCol] = kS[iRow] * kR[iRow][iCol];
|
|
}
|
|
|
|
// product L*S*R
|
|
for (iRow = 0; iRow < 3; ++iRow) {
|
|
for (iCol = 0; iCol < 3; ++iCol) {
|
|
elt[iRow][iCol] = 0.0;
|
|
|
|
for (int iMid = 0; iMid < 3; ++iMid)
|
|
elt[iRow][iCol] += kL[iRow][iMid] * kTmp[iMid][iCol];
|
|
}
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
void Matrix3::orthonormalize () {
|
|
// Algorithm uses Gram-Schmidt orthogonalization. If 'this' matrix is
|
|
// M = [m0|m1|m2], then orthonormal output matrix is Q = [q0|q1|q2],
|
|
//
|
|
// q0 = m0/|m0|
|
|
// q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
|
|
// q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1|
|
|
//
|
|
// where |V| indicates length of vector V and A*B indicates dot
|
|
// product of vectors A and B.
|
|
|
|
// compute q0
|
|
float fInvLength = 1.0f / sqrt(elt[0][0] * elt[0][0]
|
|
+ elt[1][0] * elt[1][0] +
|
|
elt[2][0] * elt[2][0]);
|
|
|
|
elt[0][0] *= fInvLength;
|
|
elt[1][0] *= fInvLength;
|
|
elt[2][0] *= fInvLength;
|
|
|
|
// compute q1
|
|
float fDot0 =
|
|
elt[0][0] * elt[0][1] +
|
|
elt[1][0] * elt[1][1] +
|
|
elt[2][0] * elt[2][1];
|
|
|
|
elt[0][1] -= fDot0 * elt[0][0];
|
|
elt[1][1] -= fDot0 * elt[1][0];
|
|
elt[2][1] -= fDot0 * elt[2][0];
|
|
|
|
fInvLength = 1.0f / sqrt(elt[0][1] * elt[0][1] +
|
|
elt[1][1] * elt[1][1] +
|
|
elt[2][1] * elt[2][1]);
|
|
|
|
elt[0][1] *= fInvLength;
|
|
elt[1][1] *= fInvLength;
|
|
elt[2][1] *= fInvLength;
|
|
|
|
// compute q2
|
|
float fDot1 =
|
|
elt[0][1] * elt[0][2] +
|
|
elt[1][1] * elt[1][2] +
|
|
elt[2][1] * elt[2][2];
|
|
|
|
fDot0 =
|
|
elt[0][0] * elt[0][2] +
|
|
elt[1][0] * elt[1][2] +
|
|
elt[2][0] * elt[2][2];
|
|
|
|
elt[0][2] -= fDot0 * elt[0][0] + fDot1 * elt[0][1];
|
|
elt[1][2] -= fDot0 * elt[1][0] + fDot1 * elt[1][1];
|
|
elt[2][2] -= fDot0 * elt[2][0] + fDot1 * elt[2][1];
|
|
|
|
fInvLength = 1.0f / sqrt(elt[0][2] * elt[0][2] +
|
|
elt[1][2] * elt[1][2] +
|
|
elt[2][2] * elt[2][2]);
|
|
|
|
elt[0][2] *= fInvLength;
|
|
elt[1][2] *= fInvLength;
|
|
elt[2][2] *= fInvLength;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
void Matrix3::qDUDecomposition (Matrix3& kQ,
|
|
Vector3& kD, Vector3& kU) const {
|
|
// Factor M = QR = QDU where Q is orthogonal, D is diagonal,
|
|
// and U is upper triangular with ones on its diagonal. Algorithm uses
|
|
// Gram-Schmidt orthogonalization (the QR algorithm).
|
|
//
|
|
// If M = [ m0 | m1 | m2 ] and Q = [ q0 | q1 | q2 ], then
|
|
//
|
|
// q0 = m0/|m0|
|
|
// q1 = (m1-(q0*m1)q0)/|m1-(q0*m1)q0|
|
|
// q2 = (m2-(q0*m2)q0-(q1*m2)q1)/|m2-(q0*m2)q0-(q1*m2)q1|
|
|
//
|
|
// where |V| indicates length of vector V and A*B indicates dot
|
|
// product of vectors A and B. The matrix R has entries
|
|
//
|
|
// r00 = q0*m0 r01 = q0*m1 r02 = q0*m2
|
|
// r10 = 0 r11 = q1*m1 r12 = q1*m2
|
|
// r20 = 0 r21 = 0 r22 = q2*m2
|
|
//
|
|
// so D = diag(r00,r11,r22) and U has entries u01 = r01/r00,
|
|
// u02 = r02/r00, and u12 = r12/r11.
|
|
|
|
// Q = rotation
|
|
// D = scaling
|
|
// U = shear
|
|
|
|
// D stores the three diagonal entries r00, r11, r22
|
|
// U stores the entries U[0] = u01, U[1] = u02, U[2] = u12
|
|
|
|
// build orthogonal matrix Q
|
|
float fInvLength = 1.0f / sqrt(elt[0][0] * elt[0][0]
|
|
+ elt[1][0] * elt[1][0] +
|
|
elt[2][0] * elt[2][0]);
|
|
kQ[0][0] = elt[0][0] * fInvLength;
|
|
kQ[1][0] = elt[1][0] * fInvLength;
|
|
kQ[2][0] = elt[2][0] * fInvLength;
|
|
|
|
float fDot = kQ[0][0] * elt[0][1] + kQ[1][0] * elt[1][1] +
|
|
kQ[2][0] * elt[2][1];
|
|
kQ[0][1] = elt[0][1] - fDot * kQ[0][0];
|
|
kQ[1][1] = elt[1][1] - fDot * kQ[1][0];
|
|
kQ[2][1] = elt[2][1] - fDot * kQ[2][0];
|
|
fInvLength = 1.0f / sqrt(kQ[0][1] * kQ[0][1] + kQ[1][1] * kQ[1][1] +
|
|
kQ[2][1] * kQ[2][1]);
|
|
kQ[0][1] *= fInvLength;
|
|
kQ[1][1] *= fInvLength;
|
|
kQ[2][1] *= fInvLength;
|
|
|
|
fDot = kQ[0][0] * elt[0][2] + kQ[1][0] * elt[1][2] +
|
|
kQ[2][0] * elt[2][2];
|
|
kQ[0][2] = elt[0][2] - fDot * kQ[0][0];
|
|
kQ[1][2] = elt[1][2] - fDot * kQ[1][0];
|
|
kQ[2][2] = elt[2][2] - fDot * kQ[2][0];
|
|
fDot = kQ[0][1] * elt[0][2] + kQ[1][1] * elt[1][2] +
|
|
kQ[2][1] * elt[2][2];
|
|
kQ[0][2] -= fDot * kQ[0][1];
|
|
kQ[1][2] -= fDot * kQ[1][1];
|
|
kQ[2][2] -= fDot * kQ[2][1];
|
|
fInvLength = 1.0f / sqrt(kQ[0][2] * kQ[0][2] + kQ[1][2] * kQ[1][2] +
|
|
kQ[2][2] * kQ[2][2]);
|
|
kQ[0][2] *= fInvLength;
|
|
kQ[1][2] *= fInvLength;
|
|
kQ[2][2] *= fInvLength;
|
|
|
|
// guarantee that orthogonal matrix has determinant 1 (no reflections)
|
|
float fDet = kQ[0][0] * kQ[1][1] * kQ[2][2] + kQ[0][1] * kQ[1][2] * kQ[2][0] +
|
|
kQ[0][2] * kQ[1][0] * kQ[2][1] - kQ[0][2] * kQ[1][1] * kQ[2][0] -
|
|
kQ[0][1] * kQ[1][0] * kQ[2][2] - kQ[0][0] * kQ[1][2] * kQ[2][1];
|
|
|
|
if ( fDet < 0.0 ) {
|
|
for (int iRow = 0; iRow < 3; ++iRow)
|
|
for (int iCol = 0; iCol < 3; ++iCol)
|
|
kQ[iRow][iCol] = -kQ[iRow][iCol];
|
|
}
|
|
|
|
// build "right" matrix R
|
|
Matrix3 kR;
|
|
|
|
kR[0][0] = kQ[0][0] * elt[0][0] + kQ[1][0] * elt[1][0] +
|
|
kQ[2][0] * elt[2][0];
|
|
|
|
kR[0][1] = kQ[0][0] * elt[0][1] + kQ[1][0] * elt[1][1] +
|
|
kQ[2][0] * elt[2][1];
|
|
|
|
kR[1][1] = kQ[0][1] * elt[0][1] + kQ[1][1] * elt[1][1] +
|
|
kQ[2][1] * elt[2][1];
|
|
|
|
kR[0][2] = kQ[0][0] * elt[0][2] + kQ[1][0] * elt[1][2] +
|
|
kQ[2][0] * elt[2][2];
|
|
|
|
kR[1][2] = kQ[0][1] * elt[0][2] + kQ[1][1] * elt[1][2] +
|
|
kQ[2][1] * elt[2][2];
|
|
|
|
kR[2][2] = kQ[0][2] * elt[0][2] + kQ[1][2] * elt[1][2] +
|
|
kQ[2][2] * elt[2][2];
|
|
|
|
// the scaling component
|
|
kD[0] = kR[0][0];
|
|
|
|
kD[1] = kR[1][1];
|
|
|
|
kD[2] = kR[2][2];
|
|
|
|
// the shear component
|
|
float fInvD0 = 1.0f / kD[0];
|
|
|
|
kU[0] = kR[0][1] * fInvD0;
|
|
|
|
kU[1] = kR[0][2] * fInvD0;
|
|
|
|
kU[2] = kR[1][2] / kD[1];
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
void Matrix3::polarDecomposition(Matrix3 &R, Matrix3 &S) const{
|
|
/*
|
|
Polar decomposition of a matrix. Based on pseudocode from
|
|
Nicholas J Higham, "Computing the Polar Decomposition -- with
|
|
Applications Siam Journal of Science and Statistical Computing, Vol 7, No. 4,
|
|
October 1986.
|
|
|
|
Decomposes A into R*S, where R is orthogonal and S is symmetric.
|
|
|
|
Ken Shoemake's "Matrix animation and polar decomposition"
|
|
in Proceedings of the conference on Graphics interface '92
|
|
seems to be better known in the world of graphics, but Higham's version
|
|
uses a scaling constant that can lead to faster convergence than
|
|
Shoemake's when the initial matrix is far from orthogonal.
|
|
*/
|
|
|
|
Matrix3 X = *this;
|
|
Matrix3 tmp = X.inverse();
|
|
Matrix3 Xit = tmp.transpose();
|
|
int iter = 0;
|
|
|
|
const int MAX_ITERS = 100;
|
|
|
|
const double eps = 50 * std::numeric_limits<float>::epsilon();
|
|
const float BigEps = 50.0f * (float)eps;
|
|
|
|
/* Higham suggests using OneNorm(Xit-X) < eps * OneNorm(X)
|
|
* as the convergence criterion, but OneNorm(X) should quickly
|
|
* settle down to something between 1 and 1.7, so just comparing
|
|
* with eps seems sufficient.
|
|
*--------------------------------------------------------------- */
|
|
|
|
double resid = X.diffOneNorm(Xit);
|
|
while (resid > eps && iter < MAX_ITERS) {
|
|
|
|
tmp = X.inverse();
|
|
Xit = tmp.transpose();
|
|
|
|
if (resid < BigEps) {
|
|
// close enough use simple iteration
|
|
X += Xit;
|
|
X *= 0.5f;
|
|
}
|
|
else {
|
|
// not close to convergence, compute acceleration factor
|
|
float gamma = sqrt( sqrt(
|
|
(Xit.l1Norm()* Xit.lInfNorm())/(X.l1Norm()*X.lInfNorm()) ) );
|
|
|
|
X *= 0.5f * gamma;
|
|
tmp = Xit;
|
|
tmp *= 0.5f / gamma;
|
|
X += tmp;
|
|
}
|
|
|
|
resid = X.diffOneNorm(Xit);
|
|
++iter;
|
|
}
|
|
|
|
R = X;
|
|
tmp = R.transpose();
|
|
|
|
S = tmp * (*this);
|
|
|
|
// S := (S + S^t)/2 one more time to make sure it is symmetric
|
|
tmp = S.transpose();
|
|
|
|
S += tmp;
|
|
S *= 0.5f;
|
|
|
|
#ifdef G3D_DEBUG
|
|
// Check iter limit
|
|
assert(iter < MAX_ITERS);
|
|
|
|
// Check A = R*S
|
|
tmp = R*S;
|
|
resid = tmp.diffOneNorm(*this);
|
|
assert(resid < eps);
|
|
|
|
// Check R is orthogonal
|
|
tmp = R*R.transpose();
|
|
resid = tmp.diffOneNorm(Matrix3::identity());
|
|
assert(resid < eps);
|
|
|
|
// Check that S is symmetric
|
|
tmp = S.transpose();
|
|
resid = tmp.diffOneNorm(S);
|
|
assert(resid < eps);
|
|
#endif
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
float Matrix3::maxCubicRoot (float afCoeff[3]) {
|
|
// Spectral norm is for A^T*A, so characteristic polynomial
|
|
// P(x) = c[0]+c[1]*x+c[2]*x^2+x^3 has three positive float roots.
|
|
// This yields the assertions c[0] < 0 and c[2]*c[2] >= 3*c[1].
|
|
|
|
// quick out for uniform scale (triple root)
|
|
const float fOneThird = 1.0f / 3.0f;
|
|
const float fEpsilon = 1e-06f;
|
|
float fDiscr = afCoeff[2] * afCoeff[2] - 3.0f * afCoeff[1];
|
|
|
|
if ( fDiscr <= fEpsilon )
|
|
return -fOneThird*afCoeff[2];
|
|
|
|
// Compute an upper bound on roots of P(x). This assumes that A^T*A
|
|
// has been scaled by its largest entry.
|
|
float fX = 1.0f;
|
|
|
|
float fPoly = afCoeff[0] + fX * (afCoeff[1] + fX * (afCoeff[2] + fX));
|
|
|
|
if ( fPoly < 0.0f ) {
|
|
// uses a matrix norm to find an upper bound on maximum root
|
|
fX = (float)G3D::abs(afCoeff[0]);
|
|
float fTmp = 1.0f + (float)G3D::abs(afCoeff[1]);
|
|
|
|
if ( fTmp > fX )
|
|
fX = fTmp;
|
|
|
|
fTmp = 1.0f + (float)G3D::abs(afCoeff[2]);
|
|
|
|
if ( fTmp > fX )
|
|
fX = fTmp;
|
|
}
|
|
|
|
// Newton's method to find root
|
|
float fTwoC2 = 2.0f * afCoeff[2];
|
|
|
|
for (int i = 0; i < 16; ++i) {
|
|
fPoly = afCoeff[0] + fX * (afCoeff[1] + fX * (afCoeff[2] + fX));
|
|
|
|
if ( G3D::abs(fPoly) <= fEpsilon )
|
|
return fX;
|
|
|
|
float fDeriv = afCoeff[1] + fX * (fTwoC2 + 3.0f * fX);
|
|
|
|
fX -= fPoly / fDeriv;
|
|
}
|
|
|
|
return fX;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
float Matrix3::spectralNorm () const {
|
|
Matrix3 kP;
|
|
int iRow, iCol;
|
|
float fPmax = 0.0;
|
|
|
|
for (iRow = 0; iRow < 3; ++iRow) {
|
|
for (iCol = 0; iCol < 3; ++iCol) {
|
|
kP[iRow][iCol] = 0.0;
|
|
|
|
for (int iMid = 0; iMid < 3; ++iMid) {
|
|
kP[iRow][iCol] +=
|
|
elt[iMid][iRow] * elt[iMid][iCol];
|
|
}
|
|
|
|
if ( kP[iRow][iCol] > fPmax )
|
|
fPmax = kP[iRow][iCol];
|
|
}
|
|
}
|
|
|
|
float fInvPmax = 1.0f / fPmax;
|
|
|
|
for (iRow = 0; iRow < 3; ++iRow) {
|
|
for (iCol = 0; iCol < 3; ++iCol)
|
|
kP[iRow][iCol] *= fInvPmax;
|
|
}
|
|
|
|
float afCoeff[3];
|
|
afCoeff[0] = -(kP[0][0] * (kP[1][1] * kP[2][2] - kP[1][2] * kP[2][1]) +
|
|
kP[0][1] * (kP[2][0] * kP[1][2] - kP[1][0] * kP[2][2]) +
|
|
kP[0][2] * (kP[1][0] * kP[2][1] - kP[2][0] * kP[1][1]));
|
|
afCoeff[1] = kP[0][0] * kP[1][1] - kP[0][1] * kP[1][0] +
|
|
kP[0][0] * kP[2][2] - kP[0][2] * kP[2][0] +
|
|
kP[1][1] * kP[2][2] - kP[1][2] * kP[2][1];
|
|
afCoeff[2] = -(kP[0][0] + kP[1][1] + kP[2][2]);
|
|
|
|
float fRoot = maxCubicRoot(afCoeff);
|
|
float fNorm = sqrt(fPmax * fRoot);
|
|
return fNorm;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
float Matrix3::squaredFrobeniusNorm() const {
|
|
float norm2 = 0;
|
|
const float* e = &elt[0][0];
|
|
|
|
for (int i = 0; i < 9; ++i){
|
|
norm2 += (*e) * (*e);
|
|
}
|
|
|
|
return norm2;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
float Matrix3::frobeniusNorm() const {
|
|
return sqrtf(squaredFrobeniusNorm());
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
float Matrix3::l1Norm() const {
|
|
// The one norm of a matrix is the max column sum in absolute value.
|
|
float oneNorm = 0;
|
|
for (int c = 0; c < 3; ++c) {
|
|
|
|
float f = fabs(elt[0][c])+ fabs(elt[1][c]) + fabs(elt[2][c]);
|
|
|
|
if (f > oneNorm) {
|
|
oneNorm = f;
|
|
}
|
|
}
|
|
return oneNorm;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
float Matrix3::lInfNorm() const {
|
|
// The infinity norm of a matrix is the max row sum in absolute value.
|
|
float infNorm = 0;
|
|
|
|
for (int r = 0; r < 3; ++r) {
|
|
|
|
float f = fabs(elt[r][0]) + fabs(elt[r][1])+ fabs(elt[r][2]);
|
|
|
|
if (f > infNorm) {
|
|
infNorm = f;
|
|
}
|
|
}
|
|
return infNorm;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
float Matrix3::diffOneNorm(const Matrix3 &y) const{
|
|
float oneNorm = 0;
|
|
|
|
for (int c = 0; c < 3; ++c){
|
|
|
|
float f = fabs(elt[0][c] - y[0][c]) + fabs(elt[1][c] - y[1][c])
|
|
+ fabs(elt[2][c] - y[2][c]);
|
|
|
|
if (f > oneNorm) {
|
|
oneNorm = f;
|
|
}
|
|
}
|
|
return oneNorm;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
void Matrix3::toAxisAngle (Vector3& rkAxis, float& rfRadians) const {
|
|
//
|
|
// Let (x,y,z) be the unit-length axis and let A be an angle of rotation.
|
|
// The rotation matrix is R = I + sin(A)*P + (1-cos(A))*P^2 (Rodrigues' formula) where
|
|
// I is the identity and
|
|
//
|
|
// +- -+
|
|
// P = | 0 -z +y |
|
|
// | +z 0 -x |
|
|
// | -y +x 0 |
|
|
// +- -+
|
|
//
|
|
// If A > 0, R represents a counterclockwise rotation about the axis in
|
|
// the sense of looking from the tip of the axis vector towards the
|
|
// origin. Some algebra will show that
|
|
//
|
|
// cos(A) = (trace(R)-1)/2 and R - R^t = 2*sin(A)*P
|
|
//
|
|
// In the event that A = pi, R-R^t = 0 which prevents us from extracting
|
|
// the axis through P. Instead note that R = I+2*P^2 when A = pi, so
|
|
// P^2 = (R-I)/2. The diagonal entries of P^2 are x^2-1, y^2-1, and
|
|
// z^2-1. We can solve these for axis (x,y,z). Because the angle is pi,
|
|
// it does not matter which sign you choose on the square roots.
|
|
|
|
float fTrace = elt[0][0] + elt[1][1] + elt[2][2];
|
|
float fCos = 0.5f * (fTrace - 1.0f);
|
|
rfRadians = (float)G3D::aCos(fCos); // in [0,PI]
|
|
|
|
if ( rfRadians > 0.0 ) {
|
|
if ( rfRadians < pi() ) {
|
|
rkAxis.x = elt[2][1] - elt[1][2];
|
|
rkAxis.y = elt[0][2] - elt[2][0];
|
|
rkAxis.z = elt[1][0] - elt[0][1];
|
|
rkAxis = rkAxis.direction();
|
|
} else {
|
|
// angle is PI
|
|
float fHalfInverse;
|
|
|
|
if ( elt[0][0] >= elt[1][1] ) {
|
|
// r00 >= r11
|
|
if ( elt[0][0] >= elt[2][2] ) {
|
|
// r00 is maximum diagonal term
|
|
rkAxis.x = 0.5f * sqrt(elt[0][0] -
|
|
elt[1][1] - elt[2][2] + 1.0f);
|
|
fHalfInverse = 0.5f / rkAxis.x;
|
|
rkAxis.y = fHalfInverse * elt[0][1];
|
|
rkAxis.z = fHalfInverse * elt[0][2];
|
|
} else {
|
|
// r22 is maximum diagonal term
|
|
rkAxis.z = 0.5f * sqrt(elt[2][2] -
|
|
elt[0][0] - elt[1][1] + 1.0f);
|
|
fHalfInverse = 0.5f / rkAxis.z;
|
|
rkAxis.x = fHalfInverse * elt[0][2];
|
|
rkAxis.y = fHalfInverse * elt[1][2];
|
|
}
|
|
} else {
|
|
// r11 > r00
|
|
if ( elt[1][1] >= elt[2][2] ) {
|
|
// r11 is maximum diagonal term
|
|
rkAxis.y = 0.5f * sqrt(elt[1][1] -
|
|
elt[0][0] - elt[2][2] + 1.0f);
|
|
fHalfInverse = 0.5f / rkAxis.y;
|
|
rkAxis.x = fHalfInverse * elt[0][1];
|
|
rkAxis.z = fHalfInverse * elt[1][2];
|
|
} else {
|
|
// r22 is maximum diagonal term
|
|
rkAxis.z = 0.5f * sqrt(elt[2][2] -
|
|
elt[0][0] - elt[1][1] + 1.0f);
|
|
fHalfInverse = 0.5f / rkAxis.z;
|
|
rkAxis.x = fHalfInverse * elt[0][2];
|
|
rkAxis.y = fHalfInverse * elt[1][2];
|
|
}
|
|
}
|
|
}
|
|
} else {
|
|
// The angle is 0 and the matrix is the identity. Any axis will
|
|
// work, so just use the x-axis.
|
|
rkAxis.x = 1.0;
|
|
rkAxis.y = 0.0;
|
|
rkAxis.z = 0.0;
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
Matrix3 Matrix3::fromAxisAngle (const Vector3& _axis, float fRadians) {
|
|
return fromUnitAxisAngle(_axis.direction(), fRadians);
|
|
}
|
|
|
|
Matrix3 Matrix3::fromUnitAxisAngle (const Vector3& axis, float fRadians) {
|
|
debugAssertM(axis.isUnit(), "Matrix3::fromUnitAxisAngle requires ||axis|| = 1");
|
|
|
|
Matrix3 m;
|
|
float fCos = cos(fRadians);
|
|
float fSin = sin(fRadians);
|
|
float fOneMinusCos = 1.0f - fCos;
|
|
float fX2 = square(axis.x);
|
|
float fY2 = square(axis.y);
|
|
float fZ2 = square(axis.z);
|
|
float fXYM = axis.x * axis.y * fOneMinusCos;
|
|
float fXZM = axis.x * axis.z * fOneMinusCos;
|
|
float fYZM = axis.y * axis.z * fOneMinusCos;
|
|
float fXSin = axis.x * fSin;
|
|
float fYSin = axis.y * fSin;
|
|
float fZSin = axis.z * fSin;
|
|
|
|
m.elt[0][0] = fX2 * fOneMinusCos + fCos;
|
|
m.elt[0][1] = fXYM - fZSin;
|
|
m.elt[0][2] = fXZM + fYSin;
|
|
|
|
m.elt[1][0] = fXYM + fZSin;
|
|
m.elt[1][1] = fY2 * fOneMinusCos + fCos;
|
|
m.elt[1][2] = fYZM - fXSin;
|
|
|
|
m.elt[2][0] = fXZM - fYSin;
|
|
m.elt[2][1] = fYZM + fXSin;
|
|
m.elt[2][2] = fZ2 * fOneMinusCos + fCos;
|
|
|
|
return m;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool Matrix3::toEulerAnglesXYZ (float& rfXAngle, float& rfYAngle,
|
|
float& rfZAngle) const {
|
|
// rot = cy*cz -cy*sz sy
|
|
// cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
|
|
// -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
|
|
|
|
if ( elt[0][2] < 1.0f ) {
|
|
if ( elt[0][2] > -1.0f ) {
|
|
rfXAngle = (float) G3D::aTan2( -elt[1][2], elt[2][2]);
|
|
rfYAngle = (float) G3D::aSin(elt[0][2]);
|
|
rfZAngle = (float) G3D::aTan2( -elt[0][1], elt[0][0]);
|
|
return true;
|
|
} else {
|
|
// WARNING. Not unique. XA - ZA = -atan2(r10,r11)
|
|
rfXAngle = -(float)G3D::aTan2(elt[1][0], elt[1][1]);
|
|
rfYAngle = -(float)halfPi();
|
|
rfZAngle = 0.0f;
|
|
return false;
|
|
}
|
|
} else {
|
|
// WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
|
|
rfXAngle = (float)G3D::aTan2(elt[1][0], elt[1][1]);
|
|
rfYAngle = (float)halfPi();
|
|
rfZAngle = 0.0f;
|
|
return false;
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool Matrix3::toEulerAnglesXZY (float& rfXAngle, float& rfZAngle,
|
|
float& rfYAngle) const {
|
|
// rot = cy*cz -sz cz*sy
|
|
// sx*sy+cx*cy*sz cx*cz -cy*sx+cx*sy*sz
|
|
// -cx*sy+cy*sx*sz cz*sx cx*cy+sx*sy*sz
|
|
|
|
if ( elt[0][1] < 1.0f ) {
|
|
if ( elt[0][1] > -1.0f ) {
|
|
rfXAngle = (float) G3D::aTan2(elt[2][1], elt[1][1]);
|
|
rfZAngle = (float) asin( -elt[0][1]);
|
|
rfYAngle = (float) G3D::aTan2(elt[0][2], elt[0][0]);
|
|
return true;
|
|
} else {
|
|
// WARNING. Not unique. XA - YA = atan2(r20,r22)
|
|
rfXAngle = (float)G3D::aTan2(elt[2][0], elt[2][2]);
|
|
rfZAngle = (float)halfPi();
|
|
rfYAngle = 0.0f;
|
|
return false;
|
|
}
|
|
} else {
|
|
// WARNING. Not unique. XA + YA = atan2(-r20,r22)
|
|
rfXAngle = (float)G3D::aTan2( -elt[2][0], elt[2][2]);
|
|
rfZAngle = -(float)halfPi();
|
|
rfYAngle = 0.0f;
|
|
return false;
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool Matrix3::toEulerAnglesYXZ (float& rfYAngle, float& rfXAngle,
|
|
float& rfZAngle) const {
|
|
// rot = cy*cz+sx*sy*sz cz*sx*sy-cy*sz cx*sy
|
|
// cx*sz cx*cz -sx
|
|
// -cz*sy+cy*sx*sz cy*cz*sx+sy*sz cx*cy
|
|
|
|
if ( elt[1][2] < 1.0 ) {
|
|
if ( elt[1][2] > -1.0 ) {
|
|
rfYAngle = (float) G3D::aTan2(elt[0][2], elt[2][2]);
|
|
rfXAngle = (float) asin( -elt[1][2]);
|
|
rfZAngle = (float) G3D::aTan2(elt[1][0], elt[1][1]);
|
|
return true;
|
|
} else {
|
|
// WARNING. Not unique. YA - ZA = atan2(r01,r00)
|
|
rfYAngle = (float)G3D::aTan2(elt[0][1], elt[0][0]);
|
|
rfXAngle = (float)halfPi();
|
|
rfZAngle = 0.0f;
|
|
return false;
|
|
}
|
|
} else {
|
|
// WARNING. Not unique. YA + ZA = atan2(-r01,r00)
|
|
rfYAngle = (float)G3D::aTan2( -elt[0][1], elt[0][0]);
|
|
rfXAngle = -(float)halfPi();
|
|
rfZAngle = 0.0f;
|
|
return false;
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool Matrix3::toEulerAnglesYZX (float& rfYAngle, float& rfZAngle,
|
|
float& rfXAngle) const {
|
|
// rot = cy*cz sx*sy-cx*cy*sz cx*sy+cy*sx*sz
|
|
// sz cx*cz -cz*sx
|
|
// -cz*sy cy*sx+cx*sy*sz cx*cy-sx*sy*sz
|
|
|
|
if ( elt[1][0] < 1.0 ) {
|
|
if ( elt[1][0] > -1.0 ) {
|
|
rfYAngle = (float) G3D::aTan2( -elt[2][0], elt[0][0]);
|
|
rfZAngle = (float) asin(elt[1][0]);
|
|
rfXAngle = (float) G3D::aTan2( -elt[1][2], elt[1][1]);
|
|
return true;
|
|
} else {
|
|
// WARNING. Not unique. YA - XA = -atan2(r21,r22);
|
|
rfYAngle = -(float)G3D::aTan2(elt[2][1], elt[2][2]);
|
|
rfZAngle = -(float)halfPi();
|
|
rfXAngle = 0.0f;
|
|
return false;
|
|
}
|
|
} else {
|
|
// WARNING. Not unique. YA + XA = atan2(r21,r22)
|
|
rfYAngle = (float)G3D::aTan2(elt[2][1], elt[2][2]);
|
|
rfZAngle = (float)halfPi();
|
|
rfXAngle = 0.0f;
|
|
return false;
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool Matrix3::toEulerAnglesZXY (float& rfZAngle, float& rfXAngle,
|
|
float& rfYAngle) const {
|
|
// rot = cy*cz-sx*sy*sz -cx*sz cz*sy+cy*sx*sz
|
|
// cz*sx*sy+cy*sz cx*cz -cy*cz*sx+sy*sz
|
|
// -cx*sy sx cx*cy
|
|
|
|
if ( elt[2][1] < 1.0 ) {
|
|
if ( elt[2][1] > -1.0 ) {
|
|
rfZAngle = (float) G3D::aTan2( -elt[0][1], elt[1][1]);
|
|
rfXAngle = (float) asin(elt[2][1]);
|
|
rfYAngle = (float) G3D::aTan2( -elt[2][0], elt[2][2]);
|
|
return true;
|
|
} else {
|
|
// WARNING. Not unique. ZA - YA = -atan(r02,r00)
|
|
rfZAngle = -(float)G3D::aTan2(elt[0][2], elt[0][0]);
|
|
rfXAngle = -(float)halfPi();
|
|
rfYAngle = 0.0f;
|
|
return false;
|
|
}
|
|
} else {
|
|
// WARNING. Not unique. ZA + YA = atan2(r02,r00)
|
|
rfZAngle = (float)G3D::aTan2(elt[0][2], elt[0][0]);
|
|
rfXAngle = (float)halfPi();
|
|
rfYAngle = 0.0f;
|
|
return false;
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool Matrix3::toEulerAnglesZYX (float& rfZAngle, float& rfYAngle,
|
|
float& rfXAngle) const {
|
|
// rot = cy*cz cz*sx*sy-cx*sz cx*cz*sy+sx*sz
|
|
// cy*sz cx*cz+sx*sy*sz -cz*sx+cx*sy*sz
|
|
// -sy cy*sx cx*cy
|
|
|
|
if ( elt[2][0] < 1.0 ) {
|
|
if ( elt[2][0] > -1.0 ) {
|
|
rfZAngle = atan2f(elt[1][0], elt[0][0]);
|
|
rfYAngle = asinf(-elt[2][0]);
|
|
rfXAngle = atan2f(elt[2][1], elt[2][2]);
|
|
return true;
|
|
} else {
|
|
// WARNING. Not unique. ZA - XA = -atan2(r01,r02)
|
|
rfZAngle = -(float)G3D::aTan2(elt[0][1], elt[0][2]);
|
|
rfYAngle = (float)halfPi();
|
|
rfXAngle = 0.0f;
|
|
return false;
|
|
}
|
|
} else {
|
|
// WARNING. Not unique. ZA + XA = atan2(-r01,-r02)
|
|
rfZAngle = (float)G3D::aTan2( -elt[0][1], -elt[0][2]);
|
|
rfYAngle = -(float)halfPi();
|
|
rfXAngle = 0.0f;
|
|
return false;
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
Matrix3 Matrix3::fromEulerAnglesXYZ (float fYAngle, float fPAngle,
|
|
float fRAngle) {
|
|
float fCos, fSin;
|
|
|
|
fCos = cosf(fYAngle);
|
|
fSin = sinf(fYAngle);
|
|
Matrix3 kXMat(1.0f, 0.0f, 0.0f, 0.0f, fCos, -fSin, 0.0, fSin, fCos);
|
|
|
|
fCos = cosf(fPAngle);
|
|
fSin = sinf(fPAngle);
|
|
Matrix3 kYMat(fCos, 0.0f, fSin, 0.0f, 1.0f, 0.0f, -fSin, 0.0f, fCos);
|
|
|
|
fCos = cosf(fRAngle);
|
|
fSin = sinf(fRAngle);
|
|
Matrix3 kZMat(fCos, -fSin, 0.0f, fSin, fCos, 0.0f, 0.0f, 0.0f, 1.0f);
|
|
|
|
return kXMat * (kYMat * kZMat);
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
Matrix3 Matrix3::fromEulerAnglesXZY (float fYAngle, float fPAngle,
|
|
float fRAngle) {
|
|
|
|
float fCos, fSin;
|
|
|
|
fCos = cosf(fYAngle);
|
|
fSin = sinf(fYAngle);
|
|
Matrix3 kXMat(1.0, 0.0, 0.0, 0.0, fCos, -fSin, 0.0, fSin, fCos);
|
|
|
|
fCos = cosf(fPAngle);
|
|
fSin = sinf(fPAngle);
|
|
Matrix3 kZMat(fCos, -fSin, 0.0, fSin, fCos, 0.0, 0.0, 0.0, 1.0);
|
|
|
|
fCos = cosf(fRAngle);
|
|
fSin = sinf(fRAngle);
|
|
Matrix3 kYMat(fCos, 0.0, fSin, 0.0, 1.0, 0.0, -fSin, 0.0, fCos);
|
|
|
|
return kXMat * (kZMat * kYMat);
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
Matrix3 Matrix3::fromEulerAnglesYXZ(
|
|
float fYAngle,
|
|
float fPAngle,
|
|
float fRAngle) {
|
|
|
|
float fCos, fSin;
|
|
|
|
fCos = cos(fYAngle);
|
|
fSin = sin(fYAngle);
|
|
Matrix3 kYMat(fCos, 0.0f, fSin, 0.0f, 1.0f, 0.0f, -fSin, 0.0f, fCos);
|
|
|
|
fCos = cos(fPAngle);
|
|
fSin = sin(fPAngle);
|
|
Matrix3 kXMat(1.0f, 0.0f, 0.0f, 0.0f, fCos, -fSin, 0.0f, fSin, fCos);
|
|
|
|
fCos = cos(fRAngle);
|
|
fSin = sin(fRAngle);
|
|
Matrix3 kZMat(fCos, -fSin, 0.0f, fSin, fCos, 0.0f, 0.0f, 0.0f, 1.0f);
|
|
|
|
return kYMat * (kXMat * kZMat);
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
Matrix3 Matrix3::fromEulerAnglesYZX(
|
|
float fYAngle,
|
|
float fPAngle,
|
|
float fRAngle) {
|
|
|
|
float fCos, fSin;
|
|
|
|
fCos = cos(fYAngle);
|
|
fSin = sin(fYAngle);
|
|
Matrix3 kYMat(fCos, 0.0f, fSin, 0.0f, 1.0f, 0.0f, -fSin, 0.0f, fCos);
|
|
|
|
fCos = cos(fPAngle);
|
|
fSin = sin(fPAngle);
|
|
Matrix3 kZMat(fCos, -fSin, 0.0f, fSin, fCos, 0.0f, 0.0f, 0.0f, 1.0f);
|
|
|
|
fCos = cos(fRAngle);
|
|
fSin = sin(fRAngle);
|
|
Matrix3 kXMat(1.0f, 0.0f, 0.0f, 0.0f, fCos, -fSin, 0.0f, fSin, fCos);
|
|
|
|
return kYMat * (kZMat * kXMat);
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
Matrix3 Matrix3::fromEulerAnglesZXY (float fYAngle, float fPAngle,
|
|
float fRAngle) {
|
|
float fCos, fSin;
|
|
|
|
fCos = cos(fYAngle);
|
|
fSin = sin(fYAngle);
|
|
Matrix3 kZMat(fCos, -fSin, 0.0, fSin, fCos, 0.0, 0.0, 0.0, 1.0);
|
|
|
|
fCos = cos(fPAngle);
|
|
fSin = sin(fPAngle);
|
|
Matrix3 kXMat(1.0, 0.0, 0.0, 0.0, fCos, -fSin, 0.0, fSin, fCos);
|
|
|
|
fCos = cos(fRAngle);
|
|
fSin = sin(fRAngle);
|
|
Matrix3 kYMat(fCos, 0.0, fSin, 0.0, 1.0, 0.0, -fSin, 0.0, fCos);
|
|
|
|
return kZMat * (kXMat * kYMat);
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
Matrix3 Matrix3::fromEulerAnglesZYX (float fYAngle, float fPAngle,
|
|
float fRAngle) {
|
|
float fCos, fSin;
|
|
|
|
fCos = cos(fYAngle);
|
|
fSin = sin(fYAngle);
|
|
Matrix3 kZMat(fCos, -fSin, 0.0, fSin, fCos, 0.0, 0.0, 0.0, 1.0);
|
|
|
|
fCos = cos(fPAngle);
|
|
fSin = sin(fPAngle);
|
|
Matrix3 kYMat(fCos, 0.0, fSin, 0.0, 1.0, 0.0, -fSin, 0.0, fCos);
|
|
|
|
fCos = cos(fRAngle);
|
|
fSin = sin(fRAngle);
|
|
Matrix3 kXMat(1.0, 0.0, 0.0, 0.0, fCos, -fSin, 0.0, fSin, fCos);
|
|
|
|
return kZMat * (kYMat * kXMat);
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
void Matrix3::tridiagonal (float afDiag[3], float afSubDiag[3]) {
|
|
// Householder reduction T = Q^t M Q
|
|
// Input:
|
|
// mat, symmetric 3x3 matrix M
|
|
// Output:
|
|
// mat, orthogonal matrix Q
|
|
// diag, diagonal entries of T
|
|
// subd, subdiagonal entries of T (T is symmetric)
|
|
|
|
float fA = elt[0][0];
|
|
float fB = elt[0][1];
|
|
float fC = elt[0][2];
|
|
float fD = elt[1][1];
|
|
float fE = elt[1][2];
|
|
float fF = elt[2][2];
|
|
|
|
afDiag[0] = fA;
|
|
afSubDiag[2] = 0.0;
|
|
|
|
if ( G3D::abs(fC) >= EPSILON ) {
|
|
float fLength = sqrt(fB * fB + fC * fC);
|
|
float fInvLength = 1.0f / fLength;
|
|
fB *= fInvLength;
|
|
fC *= fInvLength;
|
|
float fQ = 2.0f * fB * fE + fC * (fF - fD);
|
|
afDiag[1] = fD + fC * fQ;
|
|
afDiag[2] = fF - fC * fQ;
|
|
afSubDiag[0] = fLength;
|
|
afSubDiag[1] = fE - fB * fQ;
|
|
elt[0][0] = 1.0;
|
|
elt[0][1] = 0.0;
|
|
elt[0][2] = 0.0;
|
|
elt[1][0] = 0.0;
|
|
elt[1][1] = fB;
|
|
elt[1][2] = fC;
|
|
elt[2][0] = 0.0;
|
|
elt[2][1] = fC;
|
|
elt[2][2] = -fB;
|
|
} else {
|
|
afDiag[1] = fD;
|
|
afDiag[2] = fF;
|
|
afSubDiag[0] = fB;
|
|
afSubDiag[1] = fE;
|
|
elt[0][0] = 1.0;
|
|
elt[0][1] = 0.0;
|
|
elt[0][2] = 0.0;
|
|
elt[1][0] = 0.0;
|
|
elt[1][1] = 1.0;
|
|
elt[1][2] = 0.0;
|
|
elt[2][0] = 0.0;
|
|
elt[2][1] = 0.0;
|
|
elt[2][2] = 1.0;
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
bool Matrix3::qLAlgorithm (float afDiag[3], float afSubDiag[3]) {
|
|
// QL iteration with implicit shifting to reduce matrix from tridiagonal
|
|
// to diagonal
|
|
|
|
for (int i0 = 0; i0 < 3; ++i0) {
|
|
const int iMaxIter = 32;
|
|
int iIter;
|
|
|
|
for (iIter = 0; iIter < iMaxIter; ++iIter) {
|
|
int i1;
|
|
|
|
for (i1 = i0; i1 <= 1; ++i1) {
|
|
float fSum = float(G3D::abs(afDiag[i1]) +
|
|
G3D::abs(afDiag[i1 + 1]));
|
|
|
|
if ( G3D::abs(afSubDiag[i1]) + fSum == fSum )
|
|
break;
|
|
}
|
|
|
|
if ( i1 == i0 )
|
|
break;
|
|
|
|
float fTmp0 = (afDiag[i0 + 1] - afDiag[i0]) / (2.0f * afSubDiag[i0]);
|
|
|
|
float fTmp1 = sqrt(fTmp0 * fTmp0 + 1.0f);
|
|
|
|
if ( fTmp0 < 0.0 )
|
|
fTmp0 = afDiag[i1] - afDiag[i0] + afSubDiag[i0] / (fTmp0 - fTmp1);
|
|
else
|
|
fTmp0 = afDiag[i1] - afDiag[i0] + afSubDiag[i0] / (fTmp0 + fTmp1);
|
|
|
|
float fSin = 1.0;
|
|
|
|
float fCos = 1.0;
|
|
|
|
float fTmp2 = 0.0;
|
|
|
|
for (int i2 = i1 - 1; i2 >= i0; i2--) {
|
|
float fTmp3 = fSin * afSubDiag[i2];
|
|
float fTmp4 = fCos * afSubDiag[i2];
|
|
|
|
if (G3D::abs(fTmp3) >= G3D::abs(fTmp0)) {
|
|
fCos = fTmp0 / fTmp3;
|
|
fTmp1 = sqrt(fCos * fCos + 1.0f);
|
|
afSubDiag[i2 + 1] = fTmp3 * fTmp1;
|
|
fSin = 1.0f / fTmp1;
|
|
fCos *= fSin;
|
|
} else {
|
|
fSin = fTmp3 / fTmp0;
|
|
fTmp1 = sqrt(fSin * fSin + 1.0f);
|
|
afSubDiag[i2 + 1] = fTmp0 * fTmp1;
|
|
fCos = 1.0f / fTmp1;
|
|
fSin *= fCos;
|
|
}
|
|
|
|
fTmp0 = afDiag[i2 + 1] - fTmp2;
|
|
fTmp1 = (afDiag[i2] - fTmp0) * fSin + 2.0f * fTmp4 * fCos;
|
|
fTmp2 = fSin * fTmp1;
|
|
afDiag[i2 + 1] = fTmp0 + fTmp2;
|
|
fTmp0 = fCos * fTmp1 - fTmp4;
|
|
|
|
for (int iRow = 0; iRow < 3; ++iRow) {
|
|
fTmp3 = elt[iRow][i2 + 1];
|
|
elt[iRow][i2 + 1] = fSin * elt[iRow][i2] +
|
|
fCos * fTmp3;
|
|
elt[iRow][i2] = fCos * elt[iRow][i2] -
|
|
fSin * fTmp3;
|
|
}
|
|
}
|
|
|
|
afDiag[i0] -= fTmp2;
|
|
afSubDiag[i0] = fTmp0;
|
|
afSubDiag[i1] = 0.0;
|
|
}
|
|
|
|
if ( iIter == iMaxIter ) {
|
|
// should not get here under normal circumstances
|
|
return false;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
void Matrix3::eigenSolveSymmetric (float afEigenvalue[3],
|
|
Vector3 akEigenvector[3]) const {
|
|
Matrix3 kMatrix = *this;
|
|
float afSubDiag[3];
|
|
kMatrix.tridiagonal(afEigenvalue, afSubDiag);
|
|
kMatrix.qLAlgorithm(afEigenvalue, afSubDiag);
|
|
|
|
for (int i = 0; i < 3; ++i) {
|
|
akEigenvector[i][0] = kMatrix[0][i];
|
|
akEigenvector[i][1] = kMatrix[1][i];
|
|
akEigenvector[i][2] = kMatrix[2][i];
|
|
}
|
|
|
|
// make eigenvectors form a right--handed system
|
|
Vector3 kCross = akEigenvector[1].cross(akEigenvector[2]);
|
|
|
|
float fDet = akEigenvector[0].dot(kCross);
|
|
|
|
if ( fDet < 0.0 ) {
|
|
akEigenvector[2][0] = - akEigenvector[2][0];
|
|
akEigenvector[2][1] = - akEigenvector[2][1];
|
|
akEigenvector[2][2] = - akEigenvector[2][2];
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
void Matrix3::tensorProduct (const Vector3& rkU, const Vector3& rkV,
|
|
Matrix3& rkProduct) {
|
|
for (int iRow = 0; iRow < 3; ++iRow) {
|
|
for (int iCol = 0; iCol < 3; ++iCol) {
|
|
rkProduct[iRow][iCol] = rkU[iRow] * rkV[iCol];
|
|
}
|
|
}
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
|
|
// Runs in 52 cycles on AMD, 76 cycles on Intel Centrino
|
|
//
|
|
// The loop unrolling is necessary for performance.
|
|
// I was unable to improve performance further by flattening the matrices
|
|
// into float*'s instead of 2D arrays.
|
|
//
|
|
// -morgan
|
|
void Matrix3::_mul(const Matrix3& A, const Matrix3& B, Matrix3& out) {
|
|
const float* ARowPtr = A.elt[0];
|
|
float* outRowPtr = out.elt[0];
|
|
outRowPtr[0] =
|
|
ARowPtr[0] * B.elt[0][0] +
|
|
ARowPtr[1] * B.elt[1][0] +
|
|
ARowPtr[2] * B.elt[2][0];
|
|
outRowPtr[1] =
|
|
ARowPtr[0] * B.elt[0][1] +
|
|
ARowPtr[1] * B.elt[1][1] +
|
|
ARowPtr[2] * B.elt[2][1];
|
|
outRowPtr[2] =
|
|
ARowPtr[0] * B.elt[0][2] +
|
|
ARowPtr[1] * B.elt[1][2] +
|
|
ARowPtr[2] * B.elt[2][2];
|
|
|
|
ARowPtr = A.elt[1];
|
|
outRowPtr = out.elt[1];
|
|
|
|
outRowPtr[0] =
|
|
ARowPtr[0] * B.elt[0][0] +
|
|
ARowPtr[1] * B.elt[1][0] +
|
|
ARowPtr[2] * B.elt[2][0];
|
|
outRowPtr[1] =
|
|
ARowPtr[0] * B.elt[0][1] +
|
|
ARowPtr[1] * B.elt[1][1] +
|
|
ARowPtr[2] * B.elt[2][1];
|
|
outRowPtr[2] =
|
|
ARowPtr[0] * B.elt[0][2] +
|
|
ARowPtr[1] * B.elt[1][2] +
|
|
ARowPtr[2] * B.elt[2][2];
|
|
|
|
ARowPtr = A.elt[2];
|
|
outRowPtr = out.elt[2];
|
|
|
|
outRowPtr[0] =
|
|
ARowPtr[0] * B.elt[0][0] +
|
|
ARowPtr[1] * B.elt[1][0] +
|
|
ARowPtr[2] * B.elt[2][0];
|
|
outRowPtr[1] =
|
|
ARowPtr[0] * B.elt[0][1] +
|
|
ARowPtr[1] * B.elt[1][1] +
|
|
ARowPtr[2] * B.elt[2][1];
|
|
outRowPtr[2] =
|
|
ARowPtr[0] * B.elt[0][2] +
|
|
ARowPtr[1] * B.elt[1][2] +
|
|
ARowPtr[2] * B.elt[2][2];
|
|
}
|
|
|
|
//----------------------------------------------------------------------------
|
|
void Matrix3::_transpose(const Matrix3& A, Matrix3& out) {
|
|
out[0][0] = A.elt[0][0];
|
|
out[0][1] = A.elt[1][0];
|
|
out[0][2] = A.elt[2][0];
|
|
out[1][0] = A.elt[0][1];
|
|
out[1][1] = A.elt[1][1];
|
|
out[1][2] = A.elt[2][1];
|
|
out[2][0] = A.elt[0][2];
|
|
out[2][1] = A.elt[1][2];
|
|
out[2][2] = A.elt[2][2];
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
std::string Matrix3::toString() const {
|
|
return G3D::format("[%g, %g, %g; %g, %g, %g; %g, %g, %g]",
|
|
elt[0][0], elt[0][1], elt[0][2],
|
|
elt[1][0], elt[1][1], elt[1][2],
|
|
elt[2][0], elt[2][1], elt[2][2]);
|
|
}
|
|
|
|
|
|
|
|
} // namespace
|
|
|