00001 //---------------------------------------------------------------------------------
00002 // File Name: PhysicsSolver.h
00003 // Description:
00004 //
00005 // Copyright (C) 2004 - 2006 True Axis Pty Ltd, Australia. 
00006 // All Rights Reserved.
00007 //
00008 // History:
00009 //      Created File.
00010 //---------------------------------------------------------------------------------
00011 
00012 #ifndef TA_PHYSICSSOLVER_H
00013 #define TA_PHYSICSSOLVER_H
00014 
00015 #ifndef TA_DEBUG_H
00016 #include "../Common/Debug.h"
00017 #endif // TA_DEBUG_H
00018 
00019 #ifndef TA_TYPES_H
00020 #include "../Common/Types.h"
00021 #endif // TA_TYPES_H
00022 
00023 #ifndef TA_MATRIX_H
00024 #include "../Common/Matrix.h"
00025 #endif // TA_MATRIX_H
00026 
00027 namespace TA
00028 {
00029 
00030 namespace PhysicsSolverHelperClasses
00031 {
00032     class Matrix;
00033     class Vector;
00034 }
00035 
00036 class PhysicsSolver
00037 {
00038 public:
00039     enum ConstraintFlag
00040     {
00041         CONSTRAINT_FLAG_BI_DIRECTIONAL =        0x1,
00042         CONSTRAINT_FLAG_IGNORE =                0x2,
00043         CONSTRAINT_FLAG_JOINT =                 0x4,
00044         CONSTRAINT_FLAG_USE_LINEAR_MATRIX =     0x8,
00045         CONSTRAINT_FLAG_VISITED =               0x10,
00046     };
00047     PhysicsSolver();
00048     ~PhysicsSolver();
00049 
00050     void Initialise(int nMaxNumConstraints, int nMaxNumObjects);
00051     void Finalise();
00052     
00053     void Clear();   
00054     void SetVelocity(int nConstraint, int nRow, float value);
00055     float GetImpulse(int nConstraint, int nRow);
00056     void AddMass(float fInvMass, const Mat33& m33InvRotationalInertialInWorldSpace);
00057     float GetMass(int nIndex) const { return 1.0f / (m_pInvMassList[nIndex].fM * m_pInvMassList[nIndex].fM); }
00058     float GetInvMass(int nIndex) const { return m_pInvMassList[nIndex].fM * m_pInvMassList[nIndex].fM; }
00059     int NewConstraint(  
00060         int nNumRows, 
00061         int nMassNumberA,
00062         int nMassNumberB,
00063         u32 nFlags);
00064     void SetConstraintRow(
00065         int nConstraint,
00066         int nRow,
00067         const Vec3& v3Pos,
00068         const Vec3& v3Rot);
00069     void SetConstraintRow(
00070         int nConstraint,
00071         int nRow,
00072         const Vec3& v3PosA,
00073         const Vec3& v3RotA,
00074         const Vec3& v3PosB,
00075         const Vec3& v3RotB);
00076     void FinishedAddingConstraints();
00077     bool DoSolve();
00078     void Reuse();
00079     int GetMaxNumObjects() const { return m_nMaxNumObjects; }
00080     int GetMaxNumConstraints() const { return m_nMaxSize; }
00081     void SetMassRange();
00082     
00083     static void LDLTDecomposition(
00084         int nSize,
00085         int nMaxSize,
00086         float* pfMatrix);
00087 
00088 private:
00089 
00090     struct InvMass
00091     {
00092         // We will store a Cholesky decomposition of the mass.
00093         // todo: could rearrange for better cache alignment.
00094         //float fM; 
00095         //mat33 m33I;
00096         float fM;
00097         float fm1[1];
00098         float fm2[2];
00099         float fm3[3];
00100     };
00101 
00102     // holds constraints.
00103     // A sparse matrix.
00104     struct Jacobian;
00105     struct ConstraintMgr;
00106     struct ArticulationConstraint;
00107     struct ArticulationMatrix
00108     {
00109         struct Node;
00110 
00111         float* m_pfVelocityIn;
00112         float* m_pfVelocityOffset;
00113         float* m_pfImpulse;
00114 
00115         int m_nNumUngroupedConstraints;
00116         int m_nNumObjects;
00117         int m_nMatrixSize;
00118         int m_nMaxMatrixSize;
00119 
00120         int m_nNumNodes;
00121         int m_nMaxNumNodes;
00122         Node* m_pNodeList;
00123         int m_nOrderCount;
00124         Node** m_ppForwardList;
00125         Node** m_ppBackwardList;
00126         PhysicsSolverHelperClasses::Vector* m_pVTmp;
00127         PhysicsSolverHelperClasses::Matrix* m_pMTmp;
00128 
00129         int m_nNumConstraints;
00130         int m_nMaxNumConstraints;
00131         ArticulationConstraint* m_pJacobian2;
00132 
00133         ArticulationMatrix();
00134         ~ArticulationMatrix() { Finalise(); }
00135 
00136         void Initialise(int nMaxSize, int nMaxNumObjects);
00137         void Finalise();
00138 
00139         void Clear();
00140         void BuildData(ConstraintMgr* pConstraintMgr);
00141         void PreSolve();
00142         void Solve(const float* pfX, float* pfY);
00143         void OrderMatrix(Node* pNode);
00144         void Factor();
00145         void Solve();
00146         void GetJacobian(PhysicsSolverHelperClasses::Matrix& matrix, int nRow, int nCol);
00147         void GetJacobianTranspose(PhysicsSolverHelperClasses::Matrix& matrix, int nRow, int nCol);
00148         void MatrixMinusEqualsJTxDxJ(PhysicsSolverHelperClasses::Matrix& dst, const PhysicsSolverHelperClasses::Matrix& d, const PhysicsSolverHelperClasses::Matrix& j);
00149         void MatrixMult(PhysicsSolverHelperClasses::Matrix& dst, const PhysicsSolverHelperClasses::Matrix& a, const PhysicsSolverHelperClasses::Matrix& b);
00150         void MatrixAequalsBtimeA(PhysicsSolverHelperClasses::Matrix& a, const PhysicsSolverHelperClasses::Matrix& b);
00151         void MatrixMult(PhysicsSolverHelperClasses::Vector& dst, const PhysicsSolverHelperClasses::Matrix& m, const PhysicsSolverHelperClasses::Vector& v);
00152         void MatrixMinusEqualsMTxV(PhysicsSolverHelperClasses::Vector& dst, const PhysicsSolverHelperClasses::Matrix& m, const PhysicsSolverHelperClasses::Vector& v);
00153         void MatrixMinusEqualsMxV(PhysicsSolverHelperClasses::Vector& dst, const PhysicsSolverHelperClasses::Matrix& m, const PhysicsSolverHelperClasses::Vector& v);
00154 
00155         void MultiplyByJacobianTransposeCol(float* pfCol, float* pfTmp, Jacobian* pJacobian, int nCol);
00156         float GetVelocityOffset(Jacobian* pJacobian, int nRow);
00157         void CalculateImpulse(Jacobian* pJacobian, float* pfExternalImpulse);
00158         void PreMultiplyByMass(InvMass* pInvMassList, int nNumObjects);
00159     };
00160 
00161     int m_nSize;
00162     int m_nMaxSize;
00163     int m_nNumObjects;
00164     int m_nMaxNumObjects;
00165     float* m_pfVelocityOut; // Velocity.
00166     float* m_pfDeltaVelocityOut; // Change in velocity.
00167     float* m_pfVelocityIn; // Initial velocity.
00168     float* m_pfImpulse; // Impulse.
00169     float* m_pfDeltaImpulse; // Change in impulse.
00170     int* m_pnFlags;
00171     int* m_pnPreRemapFlags;
00172     float* m_pfMatrixA;
00173     float* m_pfMatrixB;
00174     int* m_pnIndicesB; // The row and column remapping indices for the matrix m_pfMatrixB
00175     int* m_pnTmp;
00176     void* m_pData;
00177 
00178     //Constraint* m_pConstraintList;
00179 
00180     // The inverse mass matrix, just store the masses, not the whole
00181     // matrix because the rest of it is all zeros.
00182     InvMass* m_pInvMassList; 
00183     Jacobian* m_pJacobian;
00184     ArticulationMatrix* m_pArticulationMatrix;
00185     ConstraintMgr* m_pConstraintMgr;
00186     
00187     int m_nNCSize;
00188     int* m_pnNC; // Current unconstrained indices (separating velocity > 0 (Objects are moving away from each other at this point))
00189     int m_nCSize;
00190     int* m_pnC; // Current constrained indices (separating velocity == 0)
00191 
00192     // FDirection data
00193     float* m_pfV;
00194     float* m_pfX;
00195 
00196     int m_nTotalNumberOfDriveToZeroIterations;
00197     
00198     inline float& A(int nRow, int nCol) { return m_pfMatrixA[nRow * m_nSize + nCol]; }
00199 
00200     void SortConstraints();
00201     bool DriveToZero(int nD);
00202     void FDirection(int nD);
00203     void MatrixMultiply(
00204         int nVectorSize,
00205         int nMatrixSize,
00206         const float* pfMatrix, 
00207         const float* pfVector,
00208         float* pfResult);
00209     void MaxStep(int nD, float& fS, int& nJ);
00210     static void GuassianElimination(
00211         int nSize,
00212         float* pfMatrix,
00213         float* pfX,
00214         float* pfY);
00215     void CholeskyDecomposition(
00216         int nSize,
00217         float* pfMatrix,
00218         float* pfX,
00219         float* pfY);
00220     static void LDLTDecomposition(
00221         int nSize,
00222         int nMaxSize,
00223         float* pfMatrix,
00224         float* pfX,
00225         float* pfY);
00226     static void LDLTSolve(
00227         int nSize,
00228         int nMaxSize,
00229         float* pfMatrix,
00230         float* pfX,
00231         float* pfY);
00232     static void LDLTInverse(
00233         int nSize,
00234         int nMaxSize,
00235         float* pfMatrixIn,
00236         float* pfMatrixOut);
00237     void LDLTRemoveRowCol(
00238         int nSize,
00239         int nMaxSize,
00240         float* pfMatrix,
00241         int nRowCol);
00242     bool LDLTAddRowCol(
00243         int nSize,
00244         int nMaxSize,
00245         float* pfAddToMatrix,
00246         const float* pfAddFromMatrix,
00247         int nFromRowCol);
00248 #ifdef _DEBUG
00249     void PhysicsSolver::TestLDLTDecomposition();
00250 #endif
00251 };
00252 
00253 };
00254 
00255 #endif // TA_PHYSICSSOLVER_H