|
|
@@ -1,1510 +0,0 @@
|
|
|
-/*
|
|
|
------------------------------------------------------------------------------
|
|
|
-This source file is part of OGRE
|
|
|
- (Object-oriented Graphics Rendering Engine)
|
|
|
-For the latest info, see http://www.ogre3d.org/
|
|
|
-
|
|
|
-Copyright (c) 2000-2011 Torus Knot Software Ltd
|
|
|
-
|
|
|
-Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
|
-of this software and associated documentation files (the "Software"), to deal
|
|
|
-in the Software without restriction, including without limitation the rights
|
|
|
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
|
-copies of the Software, and to permit persons to whom the Software is
|
|
|
-furnished to do so, subject to the following conditions:
|
|
|
-
|
|
|
-The above copyright notice and this permission notice shall be included in
|
|
|
-all copies or substantial portions of the Software.
|
|
|
-
|
|
|
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
|
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
|
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
|
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
|
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
|
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
|
|
-THE SOFTWARE.
|
|
|
------------------------------------------------------------------------------
|
|
|
-*/
|
|
|
-#include "OgreMatrix3.h"
|
|
|
-
|
|
|
-#include "OgreMath.h"
|
|
|
-
|
|
|
-// Adapted from Matrix math by Wild Magic http://www.geometrictools.com/
|
|
|
-
|
|
|
-namespace CamelotEngine
|
|
|
-{
|
|
|
- const Real Matrix3::EPSILON = 1e-06f;
|
|
|
- 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 Real Matrix3::ms_fSvdEpsilon = 1e-04f;
|
|
|
- const unsigned int Matrix3::ms_iSvdMaxIterations = 32;
|
|
|
-
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- Vector3 Matrix3::GetColumn (size_t iCol) const
|
|
|
- {
|
|
|
- assert( 0 <= iCol && iCol < 3 );
|
|
|
- return Vector3(m[0][iCol],m[1][iCol],
|
|
|
- m[2][iCol]);
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- void Matrix3::SetColumn(size_t iCol, const Vector3& vec)
|
|
|
- {
|
|
|
- assert( 0 <= iCol && iCol < 3 );
|
|
|
- m[0][iCol] = vec.x;
|
|
|
- m[1][iCol] = vec.y;
|
|
|
- m[2][iCol] = vec.z;
|
|
|
-
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- void Matrix3::FromAxes(const Vector3& xAxis, const Vector3& yAxis, const Vector3& zAxis)
|
|
|
- {
|
|
|
- SetColumn(0,xAxis);
|
|
|
- SetColumn(1,yAxis);
|
|
|
- SetColumn(2,zAxis);
|
|
|
-
|
|
|
- }
|
|
|
-
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- bool Matrix3::operator== (const Matrix3& rkMatrix) const
|
|
|
- {
|
|
|
- for (size_t iRow = 0; iRow < 3; iRow++)
|
|
|
- {
|
|
|
- for (size_t iCol = 0; iCol < 3; iCol++)
|
|
|
- {
|
|
|
- if ( m[iRow][iCol] != rkMatrix.m[iRow][iCol] )
|
|
|
- return false;
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- return true;
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- Matrix3 Matrix3::operator+ (const Matrix3& rkMatrix) const
|
|
|
- {
|
|
|
- Matrix3 kSum;
|
|
|
- for (size_t iRow = 0; iRow < 3; iRow++)
|
|
|
- {
|
|
|
- for (size_t iCol = 0; iCol < 3; iCol++)
|
|
|
- {
|
|
|
- kSum.m[iRow][iCol] = m[iRow][iCol] +
|
|
|
- rkMatrix.m[iRow][iCol];
|
|
|
- }
|
|
|
- }
|
|
|
- return kSum;
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- Matrix3 Matrix3::operator- (const Matrix3& rkMatrix) const
|
|
|
- {
|
|
|
- Matrix3 kDiff;
|
|
|
- for (size_t iRow = 0; iRow < 3; iRow++)
|
|
|
- {
|
|
|
- for (size_t iCol = 0; iCol < 3; iCol++)
|
|
|
- {
|
|
|
- kDiff.m[iRow][iCol] = m[iRow][iCol] -
|
|
|
- rkMatrix.m[iRow][iCol];
|
|
|
- }
|
|
|
- }
|
|
|
- return kDiff;
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- Matrix3 Matrix3::operator* (const Matrix3& rkMatrix) const
|
|
|
- {
|
|
|
- Matrix3 kProd;
|
|
|
- for (size_t iRow = 0; iRow < 3; iRow++)
|
|
|
- {
|
|
|
- for (size_t iCol = 0; iCol < 3; iCol++)
|
|
|
- {
|
|
|
- kProd.m[iRow][iCol] =
|
|
|
- m[iRow][0]*rkMatrix.m[0][iCol] +
|
|
|
- m[iRow][1]*rkMatrix.m[1][iCol] +
|
|
|
- m[iRow][2]*rkMatrix.m[2][iCol];
|
|
|
- }
|
|
|
- }
|
|
|
- return kProd;
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- Vector3 Matrix3::operator* (const Vector3& rkPoint) const
|
|
|
- {
|
|
|
- Vector3 kProd;
|
|
|
- for (size_t iRow = 0; iRow < 3; iRow++)
|
|
|
- {
|
|
|
- kProd[iRow] =
|
|
|
- m[iRow][0]*rkPoint[0] +
|
|
|
- m[iRow][1]*rkPoint[1] +
|
|
|
- m[iRow][2]*rkPoint[2];
|
|
|
- }
|
|
|
- return kProd;
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- Vector3 operator* (const Vector3& rkPoint, const Matrix3& rkMatrix)
|
|
|
- {
|
|
|
- Vector3 kProd;
|
|
|
- for (size_t iRow = 0; iRow < 3; iRow++)
|
|
|
- {
|
|
|
- kProd[iRow] =
|
|
|
- rkPoint[0]*rkMatrix.m[0][iRow] +
|
|
|
- rkPoint[1]*rkMatrix.m[1][iRow] +
|
|
|
- rkPoint[2]*rkMatrix.m[2][iRow];
|
|
|
- }
|
|
|
- return kProd;
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- Matrix3 Matrix3::operator- () const
|
|
|
- {
|
|
|
- Matrix3 kNeg;
|
|
|
- for (size_t iRow = 0; iRow < 3; iRow++)
|
|
|
- {
|
|
|
- for (size_t iCol = 0; iCol < 3; iCol++)
|
|
|
- kNeg[iRow][iCol] = -m[iRow][iCol];
|
|
|
- }
|
|
|
- return kNeg;
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- Matrix3 Matrix3::operator* (Real fScalar) const
|
|
|
- {
|
|
|
- Matrix3 kProd;
|
|
|
- for (size_t iRow = 0; iRow < 3; iRow++)
|
|
|
- {
|
|
|
- for (size_t iCol = 0; iCol < 3; iCol++)
|
|
|
- kProd[iRow][iCol] = fScalar*m[iRow][iCol];
|
|
|
- }
|
|
|
- return kProd;
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- Matrix3 operator* (Real fScalar, const Matrix3& rkMatrix)
|
|
|
- {
|
|
|
- Matrix3 kProd;
|
|
|
- for (size_t iRow = 0; iRow < 3; iRow++)
|
|
|
- {
|
|
|
- for (size_t iCol = 0; iCol < 3; iCol++)
|
|
|
- kProd[iRow][iCol] = fScalar*rkMatrix.m[iRow][iCol];
|
|
|
- }
|
|
|
- return kProd;
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- Matrix3 Matrix3::Transpose () const
|
|
|
- {
|
|
|
- Matrix3 kTranspose;
|
|
|
- for (size_t iRow = 0; iRow < 3; iRow++)
|
|
|
- {
|
|
|
- for (size_t iCol = 0; iCol < 3; iCol++)
|
|
|
- kTranspose[iRow][iCol] = m[iCol][iRow];
|
|
|
- }
|
|
|
- return kTranspose;
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- bool Matrix3::Inverse (Matrix3& rkInverse, Real 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] = m[1][1]*m[2][2] -
|
|
|
- m[1][2]*m[2][1];
|
|
|
- rkInverse[0][1] = m[0][2]*m[2][1] -
|
|
|
- m[0][1]*m[2][2];
|
|
|
- rkInverse[0][2] = m[0][1]*m[1][2] -
|
|
|
- m[0][2]*m[1][1];
|
|
|
- rkInverse[1][0] = m[1][2]*m[2][0] -
|
|
|
- m[1][0]*m[2][2];
|
|
|
- rkInverse[1][1] = m[0][0]*m[2][2] -
|
|
|
- m[0][2]*m[2][0];
|
|
|
- rkInverse[1][2] = m[0][2]*m[1][0] -
|
|
|
- m[0][0]*m[1][2];
|
|
|
- rkInverse[2][0] = m[1][0]*m[2][1] -
|
|
|
- m[1][1]*m[2][0];
|
|
|
- rkInverse[2][1] = m[0][1]*m[2][0] -
|
|
|
- m[0][0]*m[2][1];
|
|
|
- rkInverse[2][2] = m[0][0]*m[1][1] -
|
|
|
- m[0][1]*m[1][0];
|
|
|
-
|
|
|
- Real fDet =
|
|
|
- m[0][0]*rkInverse[0][0] +
|
|
|
- m[0][1]*rkInverse[1][0]+
|
|
|
- m[0][2]*rkInverse[2][0];
|
|
|
-
|
|
|
- if ( Math::Abs(fDet) <= fTolerance )
|
|
|
- return false;
|
|
|
-
|
|
|
- Real fInvDet = 1.0f/fDet;
|
|
|
- for (size_t iRow = 0; iRow < 3; iRow++)
|
|
|
- {
|
|
|
- for (size_t iCol = 0; iCol < 3; iCol++)
|
|
|
- rkInverse[iRow][iCol] *= fInvDet;
|
|
|
- }
|
|
|
-
|
|
|
- return true;
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- Matrix3 Matrix3::Inverse (Real fTolerance) const
|
|
|
- {
|
|
|
- Matrix3 kInverse = Matrix3::ZERO;
|
|
|
- Inverse(kInverse,fTolerance);
|
|
|
- return kInverse;
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- Real Matrix3::Determinant () const
|
|
|
- {
|
|
|
- Real fCofactor00 = m[1][1]*m[2][2] -
|
|
|
- m[1][2]*m[2][1];
|
|
|
- Real fCofactor10 = m[1][2]*m[2][0] -
|
|
|
- m[1][0]*m[2][2];
|
|
|
- Real fCofactor20 = m[1][0]*m[2][1] -
|
|
|
- m[1][1]*m[2][0];
|
|
|
-
|
|
|
- Real fDet =
|
|
|
- m[0][0]*fCofactor00 +
|
|
|
- m[0][1]*fCofactor10 +
|
|
|
- m[0][2]*fCofactor20;
|
|
|
-
|
|
|
- return fDet;
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- void Matrix3::Bidiagonalize (Matrix3& kA, Matrix3& kL,
|
|
|
- Matrix3& kR)
|
|
|
- {
|
|
|
- Real afV[3], afW[3];
|
|
|
- Real fLength, fSign, fT1, fInvT1, fT2;
|
|
|
- bool bIdentity;
|
|
|
-
|
|
|
- // map first column to (*,0,0)
|
|
|
- fLength = Math::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.0f ? 1.0f : -1.0f);
|
|
|
- fT1 = 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 = Math::Sqrt(kA[0][1]*kA[0][1]+kA[0][2]*kA[0][2]);
|
|
|
- if ( fLength > 0.0 )
|
|
|
- {
|
|
|
- fSign = (kA[0][1] > 0.0f ? 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.0;
|
|
|
- kR[0][1] = kR[1][0] = 0.0;
|
|
|
- kR[0][2] = kR[2][0] = 0.0;
|
|
|
- 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 = Math::Sqrt(kA[1][1]*kA[1][1]+kA[2][1]*kA[2][1]);
|
|
|
- if ( fLength > 0.0 )
|
|
|
- {
|
|
|
- fSign = (kA[1][1] > 0.0f ? 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];
|
|
|
-
|
|
|
- Real fA = 1.0f+fT2;
|
|
|
- Real fB = fT2*afV[2];
|
|
|
- Real 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++)
|
|
|
- {
|
|
|
- Real fTmp0 = kL[iRow][1];
|
|
|
- Real 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)
|
|
|
- {
|
|
|
- Real fT11 = kA[0][1]*kA[0][1]+kA[1][1]*kA[1][1];
|
|
|
- Real fT22 = kA[1][2]*kA[1][2]+kA[2][2]*kA[2][2];
|
|
|
- Real fT12 = kA[1][1]*kA[1][2];
|
|
|
- Real fTrace = fT11+fT22;
|
|
|
- Real fDiff = fT11-fT22;
|
|
|
- Real fDiscr = Math::Sqrt(fDiff*fDiff+4.0f*fT12*fT12);
|
|
|
- Real fRoot1 = 0.5f*(fTrace+fDiscr);
|
|
|
- Real fRoot2 = 0.5f*(fTrace-fDiscr);
|
|
|
-
|
|
|
- // adjust right
|
|
|
- Real fY = kA[0][0] - (Math::Abs(fRoot1-fT22) <=
|
|
|
- Math::Abs(fRoot2-fT22) ? fRoot1 : fRoot2);
|
|
|
- Real fZ = kA[0][1];
|
|
|
- Real fInvLength = Math::InvSqrt(fY*fY+fZ*fZ);
|
|
|
- Real fSin = fZ*fInvLength;
|
|
|
- Real fCos = -fY*fInvLength;
|
|
|
-
|
|
|
- Real fTmp0 = kA[0][0];
|
|
|
- Real 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;
|
|
|
-
|
|
|
- size_t 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 = Math::InvSqrt(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;
|
|
|
-
|
|
|
- size_t 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 = Math::InvSqrt(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 = Math::InvSqrt(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
|
|
|
- {
|
|
|
- // temas: currently unused
|
|
|
- //const int iMax = 16;
|
|
|
- size_t iRow, iCol;
|
|
|
-
|
|
|
- Matrix3 kA = *this;
|
|
|
- Bidiagonalize(kA,kL,kR);
|
|
|
-
|
|
|
- for (unsigned int i = 0; i < ms_iSvdMaxIterations; i++)
|
|
|
- {
|
|
|
- Real fTmp, fTmp0, fTmp1;
|
|
|
- Real fSin0, fCos0, fTan0;
|
|
|
- Real fSin1, fCos1, fTan1;
|
|
|
-
|
|
|
- bool bTest1 = (Math::Abs(kA[0][1]) <=
|
|
|
- ms_fSvdEpsilon*(Math::Abs(kA[0][0])+Math::Abs(kA[1][1])));
|
|
|
- bool bTest2 = (Math::Abs(kA[1][2]) <=
|
|
|
- ms_fSvdEpsilon*(Math::Abs(kA[1][1])+Math::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+Math::Sqrt(fTmp*fTmp + 4.0f));
|
|
|
- fCos0 = Math::InvSqrt(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 = Math::InvSqrt(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+Math::Sqrt(fTmp*fTmp + 4.0f));
|
|
|
- fCos0 = Math::InvSqrt(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 = Math::InvSqrt(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)
|
|
|
- {
|
|
|
- size_t 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++)
|
|
|
- {
|
|
|
- m[iRow][iCol] = 0.0;
|
|
|
- for (int iMid = 0; iMid < 3; iMid++)
|
|
|
- m[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
|
|
|
- Real fInvLength = Math::InvSqrt(m[0][0]*m[0][0]
|
|
|
- + m[1][0]*m[1][0] +
|
|
|
- m[2][0]*m[2][0]);
|
|
|
-
|
|
|
- m[0][0] *= fInvLength;
|
|
|
- m[1][0] *= fInvLength;
|
|
|
- m[2][0] *= fInvLength;
|
|
|
-
|
|
|
- // compute q1
|
|
|
- Real fDot0 =
|
|
|
- m[0][0]*m[0][1] +
|
|
|
- m[1][0]*m[1][1] +
|
|
|
- m[2][0]*m[2][1];
|
|
|
-
|
|
|
- m[0][1] -= fDot0*m[0][0];
|
|
|
- m[1][1] -= fDot0*m[1][0];
|
|
|
- m[2][1] -= fDot0*m[2][0];
|
|
|
-
|
|
|
- fInvLength = Math::InvSqrt(m[0][1]*m[0][1] +
|
|
|
- m[1][1]*m[1][1] +
|
|
|
- m[2][1]*m[2][1]);
|
|
|
-
|
|
|
- m[0][1] *= fInvLength;
|
|
|
- m[1][1] *= fInvLength;
|
|
|
- m[2][1] *= fInvLength;
|
|
|
-
|
|
|
- // compute q2
|
|
|
- Real fDot1 =
|
|
|
- m[0][1]*m[0][2] +
|
|
|
- m[1][1]*m[1][2] +
|
|
|
- m[2][1]*m[2][2];
|
|
|
-
|
|
|
- fDot0 =
|
|
|
- m[0][0]*m[0][2] +
|
|
|
- m[1][0]*m[1][2] +
|
|
|
- m[2][0]*m[2][2];
|
|
|
-
|
|
|
- m[0][2] -= fDot0*m[0][0] + fDot1*m[0][1];
|
|
|
- m[1][2] -= fDot0*m[1][0] + fDot1*m[1][1];
|
|
|
- m[2][2] -= fDot0*m[2][0] + fDot1*m[2][1];
|
|
|
-
|
|
|
- fInvLength = Math::InvSqrt(m[0][2]*m[0][2] +
|
|
|
- m[1][2]*m[1][2] +
|
|
|
- m[2][2]*m[2][2]);
|
|
|
-
|
|
|
- m[0][2] *= fInvLength;
|
|
|
- m[1][2] *= fInvLength;
|
|
|
- m[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
|
|
|
- Real fInvLength = Math::InvSqrt(m[0][0]*m[0][0]
|
|
|
- + m[1][0]*m[1][0] +
|
|
|
- m[2][0]*m[2][0]);
|
|
|
- kQ[0][0] = m[0][0]*fInvLength;
|
|
|
- kQ[1][0] = m[1][0]*fInvLength;
|
|
|
- kQ[2][0] = m[2][0]*fInvLength;
|
|
|
-
|
|
|
- Real fDot = kQ[0][0]*m[0][1] + kQ[1][0]*m[1][1] +
|
|
|
- kQ[2][0]*m[2][1];
|
|
|
- kQ[0][1] = m[0][1]-fDot*kQ[0][0];
|
|
|
- kQ[1][1] = m[1][1]-fDot*kQ[1][0];
|
|
|
- kQ[2][1] = m[2][1]-fDot*kQ[2][0];
|
|
|
- fInvLength = Math::InvSqrt(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]*m[0][2] + kQ[1][0]*m[1][2] +
|
|
|
- kQ[2][0]*m[2][2];
|
|
|
- kQ[0][2] = m[0][2]-fDot*kQ[0][0];
|
|
|
- kQ[1][2] = m[1][2]-fDot*kQ[1][0];
|
|
|
- kQ[2][2] = m[2][2]-fDot*kQ[2][0];
|
|
|
- fDot = kQ[0][1]*m[0][2] + kQ[1][1]*m[1][2] +
|
|
|
- kQ[2][1]*m[2][2];
|
|
|
- kQ[0][2] -= fDot*kQ[0][1];
|
|
|
- kQ[1][2] -= fDot*kQ[1][1];
|
|
|
- kQ[2][2] -= fDot*kQ[2][1];
|
|
|
- fInvLength = Math::InvSqrt(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)
|
|
|
- Real 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 (size_t iRow = 0; iRow < 3; iRow++)
|
|
|
- for (size_t iCol = 0; iCol < 3; iCol++)
|
|
|
- kQ[iRow][iCol] = -kQ[iRow][iCol];
|
|
|
- }
|
|
|
-
|
|
|
- // build "right" matrix R
|
|
|
- Matrix3 kR;
|
|
|
- kR[0][0] = kQ[0][0]*m[0][0] + kQ[1][0]*m[1][0] +
|
|
|
- kQ[2][0]*m[2][0];
|
|
|
- kR[0][1] = kQ[0][0]*m[0][1] + kQ[1][0]*m[1][1] +
|
|
|
- kQ[2][0]*m[2][1];
|
|
|
- kR[1][1] = kQ[0][1]*m[0][1] + kQ[1][1]*m[1][1] +
|
|
|
- kQ[2][1]*m[2][1];
|
|
|
- kR[0][2] = kQ[0][0]*m[0][2] + kQ[1][0]*m[1][2] +
|
|
|
- kQ[2][0]*m[2][2];
|
|
|
- kR[1][2] = kQ[0][1]*m[0][2] + kQ[1][1]*m[1][2] +
|
|
|
- kQ[2][1]*m[2][2];
|
|
|
- kR[2][2] = kQ[0][2]*m[0][2] + kQ[1][2]*m[1][2] +
|
|
|
- kQ[2][2]*m[2][2];
|
|
|
-
|
|
|
- // the scaling component
|
|
|
- kD[0] = kR[0][0];
|
|
|
- kD[1] = kR[1][1];
|
|
|
- kD[2] = kR[2][2];
|
|
|
-
|
|
|
- // the shear component
|
|
|
- Real fInvD0 = 1.0f/kD[0];
|
|
|
- kU[0] = kR[0][1]*fInvD0;
|
|
|
- kU[1] = kR[0][2]*fInvD0;
|
|
|
- kU[2] = kR[1][2]/kD[1];
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- Real Matrix3::MaxCubicRoot (Real 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 real roots.
|
|
|
- // This yields the assertions c[0] < 0 and c[2]*c[2] >= 3*c[1].
|
|
|
-
|
|
|
- // quick out for uniform scale (triple root)
|
|
|
- const Real fOneThird = 1.0f/3.0f;
|
|
|
- const Real fEpsilon = 1e-06f;
|
|
|
- Real 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.
|
|
|
- Real fX = 1.0;
|
|
|
- Real fPoly = afCoeff[0]+fX*(afCoeff[1]+fX*(afCoeff[2]+fX));
|
|
|
- if ( fPoly < 0.0 )
|
|
|
- {
|
|
|
- // uses a matrix norm to find an upper bound on maximum root
|
|
|
- fX = Math::Abs(afCoeff[0]);
|
|
|
- Real fTmp = 1.0f+Math::Abs(afCoeff[1]);
|
|
|
- if ( fTmp > fX )
|
|
|
- fX = fTmp;
|
|
|
- fTmp = 1.0f+Math::Abs(afCoeff[2]);
|
|
|
- if ( fTmp > fX )
|
|
|
- fX = fTmp;
|
|
|
- }
|
|
|
-
|
|
|
- // Newton's method to find root
|
|
|
- Real fTwoC2 = 2.0f*afCoeff[2];
|
|
|
- for (int i = 0; i < 16; i++)
|
|
|
- {
|
|
|
- fPoly = afCoeff[0]+fX*(afCoeff[1]+fX*(afCoeff[2]+fX));
|
|
|
- if ( Math::Abs(fPoly) <= fEpsilon )
|
|
|
- return fX;
|
|
|
-
|
|
|
- Real fDeriv = afCoeff[1]+fX*(fTwoC2+3.0f*fX);
|
|
|
- fX -= fPoly/fDeriv;
|
|
|
- }
|
|
|
-
|
|
|
- return fX;
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- Real Matrix3::SpectralNorm () const
|
|
|
- {
|
|
|
- Matrix3 kP;
|
|
|
- size_t iRow, iCol;
|
|
|
- Real 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] +=
|
|
|
- m[iMid][iRow]*m[iMid][iCol];
|
|
|
- }
|
|
|
- if ( kP[iRow][iCol] > fPmax )
|
|
|
- fPmax = kP[iRow][iCol];
|
|
|
- }
|
|
|
- }
|
|
|
-
|
|
|
- Real fInvPmax = 1.0f/fPmax;
|
|
|
- for (iRow = 0; iRow < 3; iRow++)
|
|
|
- {
|
|
|
- for (iCol = 0; iCol < 3; iCol++)
|
|
|
- kP[iRow][iCol] *= fInvPmax;
|
|
|
- }
|
|
|
-
|
|
|
- Real 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]);
|
|
|
-
|
|
|
- Real fRoot = MaxCubicRoot(afCoeff);
|
|
|
- Real fNorm = Math::Sqrt(fPmax*fRoot);
|
|
|
- return fNorm;
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- void Matrix3::ToAxisAngle (Vector3& rkAxis, Radian& 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 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.
|
|
|
-
|
|
|
- Real fTrace = m[0][0] + m[1][1] + m[2][2];
|
|
|
- Real fCos = 0.5f*(fTrace-1.0f);
|
|
|
- rfRadians = Math::ACos(fCos); // in [0,PI]
|
|
|
-
|
|
|
- if ( rfRadians > Radian(0.0) )
|
|
|
- {
|
|
|
- if ( rfRadians < Radian(Math::PI) )
|
|
|
- {
|
|
|
- rkAxis.x = m[2][1]-m[1][2];
|
|
|
- rkAxis.y = m[0][2]-m[2][0];
|
|
|
- rkAxis.z = m[1][0]-m[0][1];
|
|
|
- rkAxis.normalise();
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- // angle is PI
|
|
|
- float fHalfInverse;
|
|
|
- if ( m[0][0] >= m[1][1] )
|
|
|
- {
|
|
|
- // r00 >= r11
|
|
|
- if ( m[0][0] >= m[2][2] )
|
|
|
- {
|
|
|
- // r00 is maximum diagonal term
|
|
|
- rkAxis.x = 0.5f*Math::Sqrt(m[0][0] -
|
|
|
- m[1][1] - m[2][2] + 1.0f);
|
|
|
- fHalfInverse = 0.5f/rkAxis.x;
|
|
|
- rkAxis.y = fHalfInverse*m[0][1];
|
|
|
- rkAxis.z = fHalfInverse*m[0][2];
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- // r22 is maximum diagonal term
|
|
|
- rkAxis.z = 0.5f*Math::Sqrt(m[2][2] -
|
|
|
- m[0][0] - m[1][1] + 1.0f);
|
|
|
- fHalfInverse = 0.5f/rkAxis.z;
|
|
|
- rkAxis.x = fHalfInverse*m[0][2];
|
|
|
- rkAxis.y = fHalfInverse*m[1][2];
|
|
|
- }
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- // r11 > r00
|
|
|
- if ( m[1][1] >= m[2][2] )
|
|
|
- {
|
|
|
- // r11 is maximum diagonal term
|
|
|
- rkAxis.y = 0.5f*Math::Sqrt(m[1][1] -
|
|
|
- m[0][0] - m[2][2] + 1.0f);
|
|
|
- fHalfInverse = 0.5f/rkAxis.y;
|
|
|
- rkAxis.x = fHalfInverse*m[0][1];
|
|
|
- rkAxis.z = fHalfInverse*m[1][2];
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- // r22 is maximum diagonal term
|
|
|
- rkAxis.z = 0.5f*Math::Sqrt(m[2][2] -
|
|
|
- m[0][0] - m[1][1] + 1.0f);
|
|
|
- fHalfInverse = 0.5f/rkAxis.z;
|
|
|
- rkAxis.x = fHalfInverse*m[0][2];
|
|
|
- rkAxis.y = fHalfInverse*m[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;
|
|
|
- }
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- void Matrix3::FromAxisAngle (const Vector3& rkAxis, const Radian& fRadians)
|
|
|
- {
|
|
|
- Real fCos = Math::Cos(fRadians);
|
|
|
- Real fSin = Math::Sin(fRadians);
|
|
|
- Real fOneMinusCos = 1.0f-fCos;
|
|
|
- Real fX2 = rkAxis.x*rkAxis.x;
|
|
|
- Real fY2 = rkAxis.y*rkAxis.y;
|
|
|
- Real fZ2 = rkAxis.z*rkAxis.z;
|
|
|
- Real fXYM = rkAxis.x*rkAxis.y*fOneMinusCos;
|
|
|
- Real fXZM = rkAxis.x*rkAxis.z*fOneMinusCos;
|
|
|
- Real fYZM = rkAxis.y*rkAxis.z*fOneMinusCos;
|
|
|
- Real fXSin = rkAxis.x*fSin;
|
|
|
- Real fYSin = rkAxis.y*fSin;
|
|
|
- Real fZSin = rkAxis.z*fSin;
|
|
|
-
|
|
|
- m[0][0] = fX2*fOneMinusCos+fCos;
|
|
|
- m[0][1] = fXYM-fZSin;
|
|
|
- m[0][2] = fXZM+fYSin;
|
|
|
- m[1][0] = fXYM+fZSin;
|
|
|
- m[1][1] = fY2*fOneMinusCos+fCos;
|
|
|
- m[1][2] = fYZM-fXSin;
|
|
|
- m[2][0] = fXZM-fYSin;
|
|
|
- m[2][1] = fYZM+fXSin;
|
|
|
- m[2][2] = fZ2*fOneMinusCos+fCos;
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- bool Matrix3::ToEulerAnglesXYZ (Radian& rfYAngle, Radian& rfPAngle,
|
|
|
- Radian& rfRAngle) 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
|
|
|
-
|
|
|
- rfPAngle = Radian(Math::ASin(m[0][2]));
|
|
|
- if ( rfPAngle < Radian(Math::HALF_PI) )
|
|
|
- {
|
|
|
- if ( rfPAngle > Radian(-Math::HALF_PI) )
|
|
|
- {
|
|
|
- rfYAngle = Math::ATan2(-m[1][2],m[2][2]);
|
|
|
- rfRAngle = Math::ATan2(-m[0][1],m[0][0]);
|
|
|
- return true;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- // WARNING. Not a unique solution.
|
|
|
- Radian fRmY = Math::ATan2(m[1][0],m[1][1]);
|
|
|
- rfRAngle = Radian(0.0); // any angle works
|
|
|
- rfYAngle = rfRAngle - fRmY;
|
|
|
- return false;
|
|
|
- }
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- // WARNING. Not a unique solution.
|
|
|
- Radian fRpY = Math::ATan2(m[1][0],m[1][1]);
|
|
|
- rfRAngle = Radian(0.0); // any angle works
|
|
|
- rfYAngle = fRpY - rfRAngle;
|
|
|
- return false;
|
|
|
- }
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- bool Matrix3::ToEulerAnglesXZY (Radian& rfYAngle, Radian& rfPAngle,
|
|
|
- Radian& rfRAngle) 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
|
|
|
-
|
|
|
- rfPAngle = Math::ASin(-m[0][1]);
|
|
|
- if ( rfPAngle < Radian(Math::HALF_PI) )
|
|
|
- {
|
|
|
- if ( rfPAngle > Radian(-Math::HALF_PI) )
|
|
|
- {
|
|
|
- rfYAngle = Math::ATan2(m[2][1],m[1][1]);
|
|
|
- rfRAngle = Math::ATan2(m[0][2],m[0][0]);
|
|
|
- return true;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- // WARNING. Not a unique solution.
|
|
|
- Radian fRmY = Math::ATan2(-m[2][0],m[2][2]);
|
|
|
- rfRAngle = Radian(0.0); // any angle works
|
|
|
- rfYAngle = rfRAngle - fRmY;
|
|
|
- return false;
|
|
|
- }
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- // WARNING. Not a unique solution.
|
|
|
- Radian fRpY = Math::ATan2(-m[2][0],m[2][2]);
|
|
|
- rfRAngle = Radian(0.0); // any angle works
|
|
|
- rfYAngle = fRpY - rfRAngle;
|
|
|
- return false;
|
|
|
- }
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- bool Matrix3::ToEulerAnglesYXZ (Radian& rfYAngle, Radian& rfPAngle,
|
|
|
- Radian& rfRAngle) 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
|
|
|
-
|
|
|
- rfPAngle = Math::ASin(-m[1][2]);
|
|
|
- if ( rfPAngle < Radian(Math::HALF_PI) )
|
|
|
- {
|
|
|
- if ( rfPAngle > Radian(-Math::HALF_PI) )
|
|
|
- {
|
|
|
- rfYAngle = Math::ATan2(m[0][2],m[2][2]);
|
|
|
- rfRAngle = Math::ATan2(m[1][0],m[1][1]);
|
|
|
- return true;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- // WARNING. Not a unique solution.
|
|
|
- Radian fRmY = Math::ATan2(-m[0][1],m[0][0]);
|
|
|
- rfRAngle = Radian(0.0); // any angle works
|
|
|
- rfYAngle = rfRAngle - fRmY;
|
|
|
- return false;
|
|
|
- }
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- // WARNING. Not a unique solution.
|
|
|
- Radian fRpY = Math::ATan2(-m[0][1],m[0][0]);
|
|
|
- rfRAngle = Radian(0.0); // any angle works
|
|
|
- rfYAngle = fRpY - rfRAngle;
|
|
|
- return false;
|
|
|
- }
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- bool Matrix3::ToEulerAnglesYZX (Radian& rfYAngle, Radian& rfPAngle,
|
|
|
- Radian& rfRAngle) 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
|
|
|
-
|
|
|
- rfPAngle = Math::ASin(m[1][0]);
|
|
|
- if ( rfPAngle < Radian(Math::HALF_PI) )
|
|
|
- {
|
|
|
- if ( rfPAngle > Radian(-Math::HALF_PI) )
|
|
|
- {
|
|
|
- rfYAngle = Math::ATan2(-m[2][0],m[0][0]);
|
|
|
- rfRAngle = Math::ATan2(-m[1][2],m[1][1]);
|
|
|
- return true;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- // WARNING. Not a unique solution.
|
|
|
- Radian fRmY = Math::ATan2(m[2][1],m[2][2]);
|
|
|
- rfRAngle = Radian(0.0); // any angle works
|
|
|
- rfYAngle = rfRAngle - fRmY;
|
|
|
- return false;
|
|
|
- }
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- // WARNING. Not a unique solution.
|
|
|
- Radian fRpY = Math::ATan2(m[2][1],m[2][2]);
|
|
|
- rfRAngle = Radian(0.0); // any angle works
|
|
|
- rfYAngle = fRpY - rfRAngle;
|
|
|
- return false;
|
|
|
- }
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- bool Matrix3::ToEulerAnglesZXY (Radian& rfYAngle, Radian& rfPAngle,
|
|
|
- Radian& rfRAngle) 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
|
|
|
-
|
|
|
- rfPAngle = Math::ASin(m[2][1]);
|
|
|
- if ( rfPAngle < Radian(Math::HALF_PI) )
|
|
|
- {
|
|
|
- if ( rfPAngle > Radian(-Math::HALF_PI) )
|
|
|
- {
|
|
|
- rfYAngle = Math::ATan2(-m[0][1],m[1][1]);
|
|
|
- rfRAngle = Math::ATan2(-m[2][0],m[2][2]);
|
|
|
- return true;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- // WARNING. Not a unique solution.
|
|
|
- Radian fRmY = Math::ATan2(m[0][2],m[0][0]);
|
|
|
- rfRAngle = Radian(0.0); // any angle works
|
|
|
- rfYAngle = rfRAngle - fRmY;
|
|
|
- return false;
|
|
|
- }
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- // WARNING. Not a unique solution.
|
|
|
- Radian fRpY = Math::ATan2(m[0][2],m[0][0]);
|
|
|
- rfRAngle = Radian(0.0); // any angle works
|
|
|
- rfYAngle = fRpY - rfRAngle;
|
|
|
- return false;
|
|
|
- }
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- bool Matrix3::ToEulerAnglesZYX (Radian& rfYAngle, Radian& rfPAngle,
|
|
|
- Radian& rfRAngle) 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
|
|
|
-
|
|
|
- rfPAngle = Math::ASin(-m[2][0]);
|
|
|
- if ( rfPAngle < Radian(Math::HALF_PI) )
|
|
|
- {
|
|
|
- if ( rfPAngle > Radian(-Math::HALF_PI) )
|
|
|
- {
|
|
|
- rfYAngle = Math::ATan2(m[1][0],m[0][0]);
|
|
|
- rfRAngle = Math::ATan2(m[2][1],m[2][2]);
|
|
|
- return true;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- // WARNING. Not a unique solution.
|
|
|
- Radian fRmY = Math::ATan2(-m[0][1],m[0][2]);
|
|
|
- rfRAngle = Radian(0.0); // any angle works
|
|
|
- rfYAngle = rfRAngle - fRmY;
|
|
|
- return false;
|
|
|
- }
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- // WARNING. Not a unique solution.
|
|
|
- Radian fRpY = Math::ATan2(-m[0][1],m[0][2]);
|
|
|
- rfRAngle = Radian(0.0); // any angle works
|
|
|
- rfYAngle = fRpY - rfRAngle;
|
|
|
- return false;
|
|
|
- }
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- void Matrix3::FromEulerAnglesXYZ (const Radian& fYAngle, const Radian& fPAngle,
|
|
|
- const Radian& fRAngle)
|
|
|
- {
|
|
|
- Real fCos, fSin;
|
|
|
-
|
|
|
- fCos = Math::Cos(fYAngle);
|
|
|
- fSin = Math::Sin(fYAngle);
|
|
|
- Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);
|
|
|
-
|
|
|
- fCos = Math::Cos(fPAngle);
|
|
|
- fSin = Math::Sin(fPAngle);
|
|
|
- Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);
|
|
|
-
|
|
|
- fCos = Math::Cos(fRAngle);
|
|
|
- fSin = Math::Sin(fRAngle);
|
|
|
- Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);
|
|
|
-
|
|
|
- *this = kXMat*(kYMat*kZMat);
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- void Matrix3::FromEulerAnglesXZY (const Radian& fYAngle, const Radian& fPAngle,
|
|
|
- const Radian& fRAngle)
|
|
|
- {
|
|
|
- Real fCos, fSin;
|
|
|
-
|
|
|
- fCos = Math::Cos(fYAngle);
|
|
|
- fSin = Math::Sin(fYAngle);
|
|
|
- Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);
|
|
|
-
|
|
|
- fCos = Math::Cos(fPAngle);
|
|
|
- fSin = Math::Sin(fPAngle);
|
|
|
- Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);
|
|
|
-
|
|
|
- fCos = Math::Cos(fRAngle);
|
|
|
- fSin = Math::Sin(fRAngle);
|
|
|
- Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);
|
|
|
-
|
|
|
- *this = kXMat*(kZMat*kYMat);
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- void Matrix3::FromEulerAnglesYXZ (const Radian& fYAngle, const Radian& fPAngle,
|
|
|
- const Radian& fRAngle)
|
|
|
- {
|
|
|
- Real fCos, fSin;
|
|
|
-
|
|
|
- fCos = Math::Cos(fYAngle);
|
|
|
- fSin = Math::Sin(fYAngle);
|
|
|
- Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);
|
|
|
-
|
|
|
- fCos = Math::Cos(fPAngle);
|
|
|
- fSin = Math::Sin(fPAngle);
|
|
|
- Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);
|
|
|
-
|
|
|
- fCos = Math::Cos(fRAngle);
|
|
|
- fSin = Math::Sin(fRAngle);
|
|
|
- Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);
|
|
|
-
|
|
|
- *this = kYMat*(kXMat*kZMat);
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- void Matrix3::FromEulerAnglesYZX (const Radian& fYAngle, const Radian& fPAngle,
|
|
|
- const Radian& fRAngle)
|
|
|
- {
|
|
|
- Real fCos, fSin;
|
|
|
-
|
|
|
- fCos = Math::Cos(fYAngle);
|
|
|
- fSin = Math::Sin(fYAngle);
|
|
|
- Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);
|
|
|
-
|
|
|
- fCos = Math::Cos(fPAngle);
|
|
|
- fSin = Math::Sin(fPAngle);
|
|
|
- Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);
|
|
|
-
|
|
|
- fCos = Math::Cos(fRAngle);
|
|
|
- fSin = Math::Sin(fRAngle);
|
|
|
- Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);
|
|
|
-
|
|
|
- *this = kYMat*(kZMat*kXMat);
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- void Matrix3::FromEulerAnglesZXY (const Radian& fYAngle, const Radian& fPAngle,
|
|
|
- const Radian& fRAngle)
|
|
|
- {
|
|
|
- Real fCos, fSin;
|
|
|
-
|
|
|
- fCos = Math::Cos(fYAngle);
|
|
|
- fSin = Math::Sin(fYAngle);
|
|
|
- Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);
|
|
|
-
|
|
|
- fCos = Math::Cos(fPAngle);
|
|
|
- fSin = Math::Sin(fPAngle);
|
|
|
- Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);
|
|
|
-
|
|
|
- fCos = Math::Cos(fRAngle);
|
|
|
- fSin = Math::Sin(fRAngle);
|
|
|
- Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);
|
|
|
-
|
|
|
- *this = kZMat*(kXMat*kYMat);
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- void Matrix3::FromEulerAnglesZYX (const Radian& fYAngle, const Radian& fPAngle,
|
|
|
- const Radian& fRAngle)
|
|
|
- {
|
|
|
- Real fCos, fSin;
|
|
|
-
|
|
|
- fCos = Math::Cos(fYAngle);
|
|
|
- fSin = Math::Sin(fYAngle);
|
|
|
- Matrix3 kZMat(fCos,-fSin,0.0,fSin,fCos,0.0,0.0,0.0,1.0);
|
|
|
-
|
|
|
- fCos = Math::Cos(fPAngle);
|
|
|
- fSin = Math::Sin(fPAngle);
|
|
|
- Matrix3 kYMat(fCos,0.0,fSin,0.0,1.0,0.0,-fSin,0.0,fCos);
|
|
|
-
|
|
|
- fCos = Math::Cos(fRAngle);
|
|
|
- fSin = Math::Sin(fRAngle);
|
|
|
- Matrix3 kXMat(1.0,0.0,0.0,0.0,fCos,-fSin,0.0,fSin,fCos);
|
|
|
-
|
|
|
- *this = kZMat*(kYMat*kXMat);
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- void Matrix3::Tridiagonal (Real afDiag[3], Real 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)
|
|
|
-
|
|
|
- Real fA = m[0][0];
|
|
|
- Real fB = m[0][1];
|
|
|
- Real fC = m[0][2];
|
|
|
- Real fD = m[1][1];
|
|
|
- Real fE = m[1][2];
|
|
|
- Real fF = m[2][2];
|
|
|
-
|
|
|
- afDiag[0] = fA;
|
|
|
- afSubDiag[2] = 0.0;
|
|
|
- if ( Math::Abs(fC) >= EPSILON )
|
|
|
- {
|
|
|
- Real fLength = Math::Sqrt(fB*fB+fC*fC);
|
|
|
- Real fInvLength = 1.0f/fLength;
|
|
|
- fB *= fInvLength;
|
|
|
- fC *= fInvLength;
|
|
|
- Real 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;
|
|
|
- m[0][0] = 1.0;
|
|
|
- m[0][1] = 0.0;
|
|
|
- m[0][2] = 0.0;
|
|
|
- m[1][0] = 0.0;
|
|
|
- m[1][1] = fB;
|
|
|
- m[1][2] = fC;
|
|
|
- m[2][0] = 0.0;
|
|
|
- m[2][1] = fC;
|
|
|
- m[2][2] = -fB;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- afDiag[1] = fD;
|
|
|
- afDiag[2] = fF;
|
|
|
- afSubDiag[0] = fB;
|
|
|
- afSubDiag[1] = fE;
|
|
|
- m[0][0] = 1.0;
|
|
|
- m[0][1] = 0.0;
|
|
|
- m[0][2] = 0.0;
|
|
|
- m[1][0] = 0.0;
|
|
|
- m[1][1] = 1.0;
|
|
|
- m[1][2] = 0.0;
|
|
|
- m[2][0] = 0.0;
|
|
|
- m[2][1] = 0.0;
|
|
|
- m[2][2] = 1.0;
|
|
|
- }
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
- bool Matrix3::QLAlgorithm (Real afDiag[3], Real afSubDiag[3])
|
|
|
- {
|
|
|
- // QL iteration with implicit shifting to reduce matrix from tridiagonal
|
|
|
- // to diagonal
|
|
|
-
|
|
|
- for (int i0 = 0; i0 < 3; i0++)
|
|
|
- {
|
|
|
- const unsigned int iMaxIter = 32;
|
|
|
- unsigned int iIter;
|
|
|
- for (iIter = 0; iIter < iMaxIter; iIter++)
|
|
|
- {
|
|
|
- int i1;
|
|
|
- for (i1 = i0; i1 <= 1; i1++)
|
|
|
- {
|
|
|
- Real fSum = Math::Abs(afDiag[i1]) +
|
|
|
- Math::Abs(afDiag[i1+1]);
|
|
|
- if ( Math::Abs(afSubDiag[i1]) + fSum == fSum )
|
|
|
- break;
|
|
|
- }
|
|
|
- if ( i1 == i0 )
|
|
|
- break;
|
|
|
-
|
|
|
- Real fTmp0 = (afDiag[i0+1]-afDiag[i0])/(2.0f*afSubDiag[i0]);
|
|
|
- Real fTmp1 = Math::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);
|
|
|
- Real fSin = 1.0;
|
|
|
- Real fCos = 1.0;
|
|
|
- Real fTmp2 = 0.0;
|
|
|
- for (int i2 = i1-1; i2 >= i0; i2--)
|
|
|
- {
|
|
|
- Real fTmp3 = fSin*afSubDiag[i2];
|
|
|
- Real fTmp4 = fCos*afSubDiag[i2];
|
|
|
- if ( Math::Abs(fTmp3) >= Math::Abs(fTmp0) )
|
|
|
- {
|
|
|
- fCos = fTmp0/fTmp3;
|
|
|
- fTmp1 = Math::Sqrt(fCos*fCos+1.0f);
|
|
|
- afSubDiag[i2+1] = fTmp3*fTmp1;
|
|
|
- fSin = 1.0f/fTmp1;
|
|
|
- fCos *= fSin;
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
- fSin = fTmp3/fTmp0;
|
|
|
- fTmp1 = Math::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 = m[iRow][i2+1];
|
|
|
- m[iRow][i2+1] = fSin*m[iRow][i2] +
|
|
|
- fCos*fTmp3;
|
|
|
- m[iRow][i2] = fCos*m[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 (Real afEigenvalue[3],
|
|
|
- Vector3 akEigenvector[3]) const
|
|
|
- {
|
|
|
- Matrix3 kMatrix = *this;
|
|
|
- Real afSubDiag[3];
|
|
|
- kMatrix.Tridiagonal(afEigenvalue,afSubDiag);
|
|
|
- kMatrix.QLAlgorithm(afEigenvalue,afSubDiag);
|
|
|
-
|
|
|
- for (size_t 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].crossProduct(akEigenvector[2]);
|
|
|
- Real fDet = akEigenvector[0].dotProduct(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 (size_t iRow = 0; iRow < 3; iRow++)
|
|
|
- {
|
|
|
- for (size_t iCol = 0; iCol < 3; iCol++)
|
|
|
- rkProduct[iRow][iCol] = rkU[iRow]*rkV[iCol];
|
|
|
- }
|
|
|
- }
|
|
|
- //-----------------------------------------------------------------------
|
|
|
-}
|