From d37110fc38a40070b1858ed7776adf4c824d4b76 Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Wed, 19 May 2010 19:46:11 +0000 Subject: [PATCH] git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@319 92aac97c-a6ce-11dd-a772-7fcde58d38e6 --- .../engine/ConstraintSolver.cpp | 25 ++++---- .../reactphysics3d/engine/ConstraintSolver.h | 57 ++++++++++--------- 2 files changed, 43 insertions(+), 39 deletions(-) diff --git a/sources/reactphysics3d/engine/ConstraintSolver.cpp b/sources/reactphysics3d/engine/ConstraintSolver.cpp index 56ec4257..07a6e276 100644 --- a/sources/reactphysics3d/engine/ConstraintSolver.cpp +++ b/sources/reactphysics3d/engine/ConstraintSolver.cpp @@ -19,7 +19,8 @@ // Libraries #include "ConstraintSolver.h" -#include "LCPProjectedGaussSeidel.h" +#include "../mathematics/lcp/LCPProjectedGaussSeidel.h" +#include "../body/RigidBody.h" using namespace reactphysics3d; @@ -134,14 +135,14 @@ void ConstraintSolver::fillInMatrices() { // TODO : Use polymorphism and remove this casting RigidBody* rigidBody = dynamic_cast(body); assert(rigidBody != 0); - + // Compute the vector with velocities values - V.fillInSubVector(bodyNumber*6, rigidBody->getCurrentBodyState()->getLinearVelocity()); - V.fillInSubVector(bodyNumber*6+3, rigidBody->getCurrentBodyState()->getAngularVelocity()); + V.fillInSubVector(bodyNumber*6, rigidBody->getCurrentBodyState().getLinearVelocity()); + V.fillInSubVector(bodyNumber*6+3, rigidBody->getCurrentBodyState().getAngularVelocity()); // Compute the vector with forces and torques values - Fext.fillInSubVector(bodyNumber*6, rigidBody->getCurrentBodyState()->getExternalForce()); - Fext.fillInSubVector(bodyNumber*6+3, rigidBody->getCurrentBodyState()->getExternalTorque()); + Fext.fillInSubVector(bodyNumber*6, rigidBody->getCurrentBodyState().getExternalForce()); + Fext.fillInSubVector(bodyNumber*6+3, rigidBody->getCurrentBodyState().getExternalTorque()); // Compute the inverse sparse mass matrix Minv_sp.fillInSubMatrix(b*6, 0, rigidBody->getCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3)); @@ -169,15 +170,17 @@ void ConstraintSolver::computeVectorB(double dt) { b = errorValues * oneOverDT; - // Substract 1.0/dt*J*V to the vector b for (uint c = 0; c @@ -40,37 +41,37 @@ const uint MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when sol */ class ConstraintSolver { protected: - LCPSolver& lcpSolver; // LCP Solver - PhysicsWorld& physicsWorld; // Reference to the physics world - std::vector activeConstraints; // Current active constraints in the physics world - std::vector constraintBodies; // Bodies that are implied in some constraint - uint nbBodies; // Current number of bodies in the physics world - std::map bodyNumberMapping; // Map a body pointer with its index number - Body*** bodyMapping; // 2-dimensional array that contains the mapping of body reference - // in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains - // the pointer to the body that correspond to the 1x6 J_ij matrix in the - // J_sp matrix. A integer body index refers to its index in the "bodies" std::vector - Matrix J_sp; // Sparse representation of the jacobian matrix of all constraints - Matrix B_sp; // Useful matrix in sparse representation - Vector b; // Vector "b" of the LCP problem - Vector lambda; // Lambda vector of the LCP problem - Vector errorValues; // Error vector of all constraints - Vector lowerBounds; // Vector that contains the low limits for the variables of the LCP problem - Vector upperBounds; // Vector that contains the high limits for the variables of the LCP problem - Matrix Minv_sp; // Sparse representation of the Matrix that contains information about mass and inertia of each body - Vector V; // Vector that contains linear and angular velocities of each body - Vector Fext; // Vector that contains external forces and torques of each body + LCPSolver& lcpSolver; // LCP Solver + PhysicsWorld& physicsWorld; // Reference to the physics world + std::vector activeConstraints; // Current active constraints in the physics world + std::vector constraintBodies; // Bodies that are implied in some constraint + uint nbBodies; // Current number of bodies in the physics world + std::map bodyNumberMapping; // Map a body pointer with its index number + Body*** bodyMapping; // 2-dimensional array that contains the mapping of body reference + // in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains + // the pointer to the body that correspond to the 1x6 J_ij matrix in the + // J_sp matrix. A integer body index refers to its index in the "bodies" std::vector + Matrix J_sp; // Sparse representation of the jacobian matrix of all constraints + Matrix B_sp; // Useful matrix in sparse representation + Vector b; // Vector "b" of the LCP problem + Vector lambda; // Lambda vector of the LCP problem + Vector errorValues; // Error vector of all constraints + Vector lowerBounds; // Vector that contains the low limits for the variables of the LCP problem + Vector upperBounds; // Vector that contains the high limits for the variables of the LCP problem + Matrix Minv_sp; // Sparse representation of the Matrix that contains information about mass and inertia of each body + Vector V; // Vector that contains linear and angular velocities of each body + Vector Fext; // Vector that contains external forces and torques of each body - void allocate(); // Allocate all the matrices needed to solve the LCP problem - void fillInMatrices(); // Fill in all the matrices needed to solve the LCP problem - void freeMemory(); // Free the memory that was allocated in the allocate() method - void computeVectorB(double dt); // Compute the vector b - void computeMatrixB_sp(); // Compute the matrix B_sp + void allocate(); // Allocate all the matrices needed to solve the LCP problem + void fillInMatrices(); // Fill in all the matrices needed to solve the LCP problem + void freeMemory(); // Free the memory that was allocated in the allocate() method + void computeVectorB(double dt); // Compute the vector b + void computeMatrixB_sp(); // Compute the matrix B_sp public: - ConstraintSolver(); // Constructor - virtual ~ConstraintSolver(); // Destructor - void solve(double dt); // Solve the current LCP problem + ConstraintSolver(); // Constructor + virtual ~ConstraintSolver(); // Destructor + void solve(double dt); // Solve the current LCP problem }; } // End of ReactPhysics3D namespace