diff options
author | maximius <none@none> | 2009-10-17 15:35:07 -0700 |
---|---|---|
committer | maximius <none@none> | 2009-10-17 15:35:07 -0700 |
commit | 26b5e033ffde3d161382fc9addbfa99738379641 (patch) | |
tree | a344f369ca32945f787a02dee35c3dbe342bed7e /dep/src/g3dlite/Matrix3.cpp | |
parent | f21f47005dcb6b76e1abc9f35fbcd03eed191bff (diff) |
*Massive cleanup (\n\n -> \n, *\n -> \n, cleanup for(...) to for (...), and some other cleanups by hand)
*Fix a possible crash in Spell::DoAllEffectOnTarget
--HG--
branch : trunk
Diffstat (limited to 'dep/src/g3dlite/Matrix3.cpp')
-rw-r--r-- | dep/src/g3dlite/Matrix3.cpp | 295 |
1 files changed, 0 insertions, 295 deletions
diff --git a/dep/src/g3dlite/Matrix3.cpp b/dep/src/g3dlite/Matrix3.cpp index 630c1883c0b..717939c8809 100644 --- a/dep/src/g3dlite/Matrix3.cpp +++ b/dep/src/g3dlite/Matrix3.cpp @@ -1,14 +1,10 @@ /** @file Matrix3.cpp - 3x3 matrix class - @author Morgan McGuire, graphics3d.com - @created 2001-06-02 @edited 2006-04-06 */ - #include "G3D/platform.h" #include "G3D/format.h" #include <memory.h> @@ -16,28 +12,21 @@ #include "G3D/Matrix3.h" #include "G3D/g3dmath.h" #include "G3D/Quat.h" - namespace G3D { - const float Matrix3::EPSILON = 1e-06f; - 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; } - // Deprecated. const Matrix3 Matrix3::ZERO(0, 0, 0, 0, 0, 0, 0, 0, 0); const Matrix3 Matrix3::IDENTITY(1, 0, 0, 0, 1, 0, 0, 0, 1); - const float Matrix3::ms_fSvdEpsilon = 1e-04f; const int Matrix3::ms_iSvdMaxIterations = 32; - bool Matrix3::fuzzyEq(const Matrix3& b) const { for (int r = 0; r < 3; ++r) { for (int c = 0; c < 3; ++c) { @@ -49,12 +38,10 @@ bool Matrix3::fuzzyEq(const Matrix3& b) const { return true; } - bool Matrix3::isOrthonormal() const { Vector3 X = getColumn(0); Vector3 Y = getColumn(1); Vector3 Z = getColumn(2); - return (G3D::fuzzyEq(X.dot(Y), 0.0f) && G3D::fuzzyEq(Y.dot(Z), 0.0f) && @@ -63,7 +50,6 @@ bool Matrix3::isOrthonormal() const { G3D::fuzzyEq(Y.squaredMagnitude(), 1.0f) && G3D::fuzzyEq(Z.squaredMagnitude(), 1.0f)); } - //---------------------------------------------------------------------------- Matrix3::Matrix3(const Quat& _q) { // Implementation from Watt and Watt, pg 362 @@ -73,30 +59,23 @@ Matrix3::Matrix3(const Quat& _q) { 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, @@ -106,12 +85,10 @@ Matrix3::Matrix3( 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; @@ -123,18 +100,15 @@ void Matrix3::set( elt[2][2] = fEntry22; } - //---------------------------------------------------------------------------- Vector3 Matrix3::getColumn (int iCol) const { assert((0 <= iCol) && (iCol < 3)); return Vector3(elt[0][iCol], elt[1][iCol], elt[2][iCol]); } - Vector3 Matrix3::getRow (int iRow) const { return Vector3(elt[iRow][0], elt[iRow][1], elt[iRow][2]); } - void Matrix3::setColumn(int iCol, const Vector3 &vector) { debugAssert((iCol >= 0) && (iCol < 3)); elt[0][iCol] = vector.x; @@ -142,7 +116,6 @@ void Matrix3::setColumn(int iCol, const Vector3 &vector) { elt[2][iCol] = vector.z; } - void Matrix3::setRow(int iRow, const Vector3 &vector) { debugAssert((iRow >= 0) && (iRow < 3)); elt[iRow][0] = vector.x; @@ -150,7 +123,6 @@ void Matrix3::setRow(int iRow, const Vector3 &vector) { elt[iRow][2] = vector.z; } - //---------------------------------------------------------------------------- bool Matrix3::operator== (const Matrix3& rkMatrix) const { for (int iRow = 0; iRow < 3; iRow++) { @@ -159,47 +131,37 @@ bool Matrix3::operator== (const Matrix3& rkMatrix) const { 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] = @@ -208,30 +170,24 @@ Matrix3 Matrix3::operator* (const Matrix3& rkMatrix) const { 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++) { @@ -242,76 +198,60 @@ Matrix3& Matrix3::operator*= (const Matrix3& rkMatrix) { 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 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] = 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] - @@ -330,32 +270,25 @@ bool Matrix3::inverse (Matrix3& rkInverse, float fTolerance) const { 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.0 / 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] - @@ -364,33 +297,27 @@ float Matrix3::determinant () const { 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.0 : -1.0); fT1 = kA[0][0] + fSign * fLength; fInvT1 = 1.0 / fT1; afV[1] = kA[1][0] * fInvT1; afV[2] = kA[2][0] * fInvT1; - fT2 = -2.0 / (1.0 + 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]); @@ -402,7 +329,6 @@ void Matrix3::bidiagonalize (Matrix3& kA, Matrix3& kL, 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.0 + fT2; kL[0][1] = kL[1][0] = fT2 * afV[1]; kL[0][2] = kL[2][0] = fT2 * afV[2]; @@ -414,15 +340,12 @@ void Matrix3::bidiagonalize (Matrix3& kA, Matrix3& kL, 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.0 : -1.0); fT1 = kA[0][1] + fSign * fLength; afV[2] = kA[0][2] / fT1; - fT2 = -2.0 / (1.0 + 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]); @@ -432,7 +355,6 @@ void Matrix3::bidiagonalize (Matrix3& kA, Matrix3& kL, kA[1][2] += afW[1] * afV[2]; kA[2][1] += afW[2]; kA[2][2] += afW[2] * afV[2]; - kR[0][0] = 1.0; kR[0][1] = kR[1][0] = 0.0; kR[0][2] = kR[2][0] = 0.0; @@ -442,26 +364,21 @@ void Matrix3::bidiagonalize (Matrix3& kA, Matrix3& kL, } 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.0 : -1.0); fT1 = kA[1][1] + fSign * fLength; afV[2] = kA[2][1] / fT1; - fT2 = -2.0 / (1.0 + 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.0 + fT2; float fB = fT2 * afV[2]; float fC = 1.0 + fB * afV[2]; - if ( bIdentity ) { kL[0][0] = 1.0; kL[0][1] = kL[1][0] = 0.0; @@ -479,7 +396,6 @@ void Matrix3::bidiagonalize (Matrix3& kA, Matrix3& kL, } } } - //---------------------------------------------------------------------------- void Matrix3::golubKahanStep (Matrix3& kA, Matrix3& kL, Matrix3& kR) { @@ -491,7 +407,6 @@ void Matrix3::golubKahanStep (Matrix3& kA, Matrix3& kL, float fDiscr = sqrt(fDiff * fDiff + 4.0 * fT12 * fT12); float fRoot1 = 0.5 * (fTrace + fDiscr); float fRoot2 = 0.5 * (fTrace - fDiscr); - // adjust right float fY = kA[0][0] - (G3D::abs(fRoot1 - fT22) <= G3D::abs(fRoot2 - fT22) ? fRoot1 : fRoot2); @@ -499,110 +414,69 @@ void Matrix3::golubKahanStep (Matrix3& kA, Matrix3& kL, float fInvLength = 1.0 / 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.0 / 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.0 / 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.0 / 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]; @@ -610,25 +484,20 @@ void Matrix3::golubKahanStep (Matrix3& kA, Matrix3& kL, 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]; @@ -642,25 +511,21 @@ void Matrix3::singularValueDecomposition (Matrix3& kL, Vector3& kS, fTan0 = 0.5 * (fTmp + sqrt(fTmp * fTmp + 4.0)); fCos0 = 1.0 / sqrt(1.0 + 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.0 / sqrt(1.0 + 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]); @@ -676,25 +541,21 @@ void Matrix3::singularValueDecomposition (Matrix3& kL, Vector3& kS, fTan0 = 0.5 * ( -fTmp + sqrt(fTmp * fTmp + 4.0)); fCos0 = 1.0 / sqrt(1.0 + 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.0 / sqrt(1.0 + 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] + @@ -706,41 +567,34 @@ void Matrix3::singularValueDecomposition (Matrix3& kL, Vector3& kS, } } } - // 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 @@ -752,58 +606,46 @@ void Matrix3::orthonormalize () { // // where |V| indicates length of vector V and A*B indicates dot // product of vectors A and B. - // compute q0 float fInvLength = 1.0 / 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.0 / 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.0 / 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 { @@ -826,14 +668,11 @@ void Matrix3::qDUDecomposition (Matrix3& kQ, // // 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.0 / sqrt(elt[0][0] * elt[0][0] + elt[1][0] * elt[1][0] + @@ -841,7 +680,6 @@ void Matrix3::qDUDecomposition (Matrix3& kQ, 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]; @@ -852,7 +690,6 @@ void Matrix3::qDUDecomposition (Matrix3& kQ, 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]; @@ -868,134 +705,96 @@ void Matrix3::qDUDecomposition (Matrix3& kQ, 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.0 / kD[0]; - kU[0] = kR[0][1] * fInvD0; - kU[1] = kR[0][2] * fInvD0; - kU[2] = kR[1][2] / kD[1]; } - //---------------------------------------------------------------------------- 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 = G3D::abs(afCoeff[0]); float fTmp = 1.0 + G3D::abs(afCoeff[1]); - if ( fTmp > fX ) fX = fTmp; - fTmp = 1.0 + 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.0 / 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]) + @@ -1004,12 +803,10 @@ float Matrix3::spectralNorm () const { 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; } - //---------------------------------------------------------------------------- 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. @@ -1033,11 +830,9 @@ void Matrix3::toAxisAngle (Vector3& rkAxis, float& rfRadians) const { // 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.5 * (fTrace - 1.0); rfRadians = G3D::aCos(fCos); // in [0,PI] - if ( rfRadians > 0.0 ) { if ( rfRadians < G3D_PI ) { rkAxis.x = elt[2][1] - elt[1][2]; @@ -1047,7 +842,6 @@ void Matrix3::toAxisAngle (Vector3& rkAxis, float& rfRadians) const { } else { // angle is PI float fHalfInverse; - if ( elt[0][0] >= elt[1][1] ) { // r00 >= r11 if ( elt[0][0] >= elt[2][2] ) { @@ -1092,11 +886,9 @@ void Matrix3::toAxisAngle (Vector3& rkAxis, float& rfRadians) const { rkAxis.z = 0.0; } } - //---------------------------------------------------------------------------- Matrix3 Matrix3::fromAxisAngle (const Vector3& rkAxis, float fRadians) { Matrix3 m; - float fCos = cos(fRadians); float fSin = sin(fRadians); float fOneMinusCos = 1.0 - fCos; @@ -1109,7 +901,6 @@ Matrix3 Matrix3::fromAxisAngle (const Vector3& rkAxis, float fRadians) { float fXSin = rkAxis.x * fSin; float fYSin = rkAxis.y * fSin; float fZSin = rkAxis.z * fSin; - m.elt[0][0] = fX2 * fOneMinusCos + fCos; m.elt[0][1] = fXYM - fZSin; m.elt[0][2] = fXZM + fYSin; @@ -1119,17 +910,14 @@ Matrix3 Matrix3::fromAxisAngle (const Vector3& rkAxis, float fRadians) { 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 = G3D::aTan2( -elt[1][2], elt[2][2]); @@ -1151,14 +939,12 @@ bool Matrix3::toEulerAnglesXYZ (float& rfXAngle, float& rfYAngle, 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 = G3D::aTan2(elt[2][1], elt[1][1]); @@ -1180,14 +966,12 @@ bool Matrix3::toEulerAnglesXZY (float& rfXAngle, float& rfZAngle, 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 = G3D::aTan2(elt[0][2], elt[2][2]); @@ -1209,14 +993,12 @@ bool Matrix3::toEulerAnglesYXZ (float& rfYAngle, float& rfXAngle, 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 = G3D::aTan2( -elt[2][0], elt[0][0]); @@ -1238,14 +1020,12 @@ bool Matrix3::toEulerAnglesYZX (float& rfYAngle, float& rfZAngle, 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 = G3D::aTan2( -elt[0][1], elt[1][1]); @@ -1267,14 +1047,12 @@ bool Matrix3::toEulerAnglesZXY (float& rfZAngle, float& rfXAngle, 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]); @@ -1296,134 +1074,100 @@ bool Matrix3::toEulerAnglesZYX (float& rfZAngle, float& rfYAngle, 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 @@ -1433,17 +1177,14 @@ void Matrix3::tridiagonal (float afDiag[3], float afSubDiag[3]) { // 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.0 / fLength; @@ -1479,49 +1220,35 @@ void Matrix3::tridiagonal (float afDiag[3], float afSubDiag[3]) { 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 = 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.0 * afSubDiag[i0]); - float fTmp1 = sqrt(fTmp0 * fTmp0 + 1.0); - 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.0); @@ -1535,13 +1262,11 @@ bool Matrix3::qLAlgorithm (float afDiag[3], float afSubDiag[3]) { fCos = 1.0 / fTmp1; fSin *= fCos; } - fTmp0 = afDiag[i2 + 1] - fTmp2; fTmp1 = (afDiag[i2] - fTmp0) * fSin + 2.0 * 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] + @@ -1550,21 +1275,17 @@ bool Matrix3::qLAlgorithm (float afDiag[3], float afSubDiag[3]) { 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 { @@ -1572,25 +1293,20 @@ void Matrix3::eigenSolveSymmetric (float afEigenvalue[3], 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) { @@ -1600,9 +1316,7 @@ void Matrix3::tensorProduct (const Vector3& rkU, const Vector3& rkV, } } } - //---------------------------------------------------------------------------- - // Runs in 52 cycles on AMD, 76 cycles on Intel Centrino // // The loop unrolling is necessary for performance. @@ -1625,10 +1339,8 @@ void Matrix3::_mul(const Matrix3& A, const Matrix3& B, Matrix3& out) { 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] + @@ -1641,10 +1353,8 @@ void Matrix3::_mul(const Matrix3& A, const Matrix3& B, Matrix3& out) { 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] + @@ -1658,7 +1368,6 @@ void Matrix3::_mul(const Matrix3& A, const Matrix3& B, Matrix3& out) { 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]; @@ -1671,7 +1380,6 @@ void Matrix3::_transpose(const Matrix3& A, Matrix3& out) { 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]", @@ -1680,8 +1388,5 @@ std::string Matrix3::toString() const { elt[2][0], elt[2][1], elt[2][2]); } - - } // namespace - |