/** @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 #include #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(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::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