From bcdf9e92ddb16fa15d04b1546efa4128a5dc4b8b Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Wed, 17 Mar 2010 19:19:11 +0000 Subject: [PATCH] git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@291 92aac97c-a6ce-11dd-a772-7fcde58d38e6 --- .../engine/ConstraintSolver.cpp | 112 ++++++++++++++---- .../reactphysics3d/engine/ConstraintSolver.h | 32 ++++- 2 files changed, 116 insertions(+), 28 deletions(-) diff --git a/sources/reactphysics3d/engine/ConstraintSolver.cpp b/sources/reactphysics3d/engine/ConstraintSolver.cpp index f897de9a..3df8c140 100644 --- a/sources/reactphysics3d/engine/ConstraintSolver.cpp +++ b/sources/reactphysics3d/engine/ConstraintSolver.cpp @@ -19,58 +19,124 @@ // Libraries #include "ConstraintSolver.h" +#include "LCPProjectedGaussSeidel.h" + +using namespace reactphysics3d; // Constructor -ConstraintSolver::ConstraintSolver() { - +ConstraintSolver::ConstraintSolver() + :bodies(0), nbBodies(0), bodyMapping(0) { + // Creation of the LCP Solver + lcpSolver = new LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS); } // Destructor -ConstraintSolver~ConstraintSolver() { +ConstraintSolver::~ConstraintSolver() { } // Allocate all the matrices needed to solve the LCP problem -void ConstraintSolver::allocate(std::vector& constraints, double dt) { - std::vector activeConstraints; +void ConstraintSolver::allocate(std::vector& constraints, std::vector& bodies) { unsigned int sizeJacobian = 0; - unsigned int nbAuxiliaryVars = 0; + this->bodies = bodies; + this->nbBodies = bodies.size(); // For each constraint for (unsigned int i=0; ievaluate(dt); + // If the constraint is active if (constraints.at(i)->isActive()) { activeConstraints.push_back(constraints.at(i)); - // Evaluate the constraint - constraints.at(i)->evaluate(dt); - - // Set the current jacobian index to the constraint - constraints.at(i)->setJacobianIndex(sizeJacobian); - - // Update the size of the jacobian matrix - sizeJacobian += constraints.at(i)->getNbJacobianRows(); + /Description/ Update the size of the jacobian matrix + sizeJacobian += (1 + constraints.at(i)->getNbAuxConstraints()); } } - // For each active constraint - for (unsigned int i=0; isetAuxiliaryIndex(sizeJacobian + nbAuxiliaryVars); + // Allocate all the vectors and matrices + J_sp = Matrix(sizeJacobian, 12); - // Update the number of auxiliary variables - nbAuxiliaryVars += activeConstraints.at(i)->getNbAuxiliaryVars(); + bodyMapping = new unsigned int[nbBodies]; + for (unsigned int i=0; i constraints) { +// Notice that all the active constraints should have been evaluated first +void ConstraintSolver::fillInMatrices() { + int i,j; + // For each active constraint + for (unsigned int c=0; cgetJacobianIndex(); + //j = i + activeConstraint.at(c)->getNbJacobianRows(); + J.fillInSubMatrix(i, 0, activeConstraints.at(c)->getBody1LinearJacobian()); + J.fillInSubMatrix(i, 3, activeConstraints.at(c)->getBody2LinearJacobian()); + J.fillInSubMatrix(i, 6, activeConstraints.at(c)->getBody1AngularJacobian()); + J.fillInSubMatrix(i, 9, activeConstraints.at(c)->getBody2AngularJacobian()); + errorVector.fillInSubVector(i, activeConstraints.at(c)->getErrorVector()); + } + + // For each active constraint + for (unsigned int c=0; cgetNbAuxiliaryVars(); + // TODO : add activeConstraints.at(c)->getAuxiliaryRowsAndCols(..., ...) + b.fillInSubVector(i, activeConstraints.at(c)->getRightHandSideVector()); + } + + // For each current body of the simulation + for (unsigned int b=0; bgetCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3)); + Minv.fillInSubMatrix(i+3, 3, bodies.at(b)->getCurrentBodyState().getInertiaTensorInverse()); + u.fillInSubVector(i, bodies.at(b)->getCurrentBodyState().getLinearVelocity()); + u.fillInSubVector(i+3, bodies.at(b)->getCurrentBodyState().getAngularVelocity()); + fExt.fillInSubVector(i, bodies.at(b)->getCurrentBodyState().getExternalForce()); + Vector3D externalTorque = bodies.at(b)->getCurrentBodyState().getExternalTorque(); + Matrix3x3 inertia = bodies.at(b)->getInertiaTensor(); + Vector3D angularVelocity = bodies.at(b)->getCurrentBodyState().getAngularVelocity(); + fExt.fillInSubVector(i+3, externalTorque - (angularVelocity.crossProduct(inertia*angularVelocity))); + } +} + +// Free the memory that was allocated in the allocate() method +void ConstraintSolver::freeMemory() { + // Free the bodyMaping array + for (unsigned int i=0; i constraints, double dt) { +void ConstraintSolver::solve(std::vector& constraints, std::vector* bodies, double dt) { + // Update the current set of bodies in the physics world + this->bodies = bodies; + // Delete all the current actice constraints + activeConstraints.clear(); + + // Allocate memory for the matrices + allocate(constraints, dt); + + // Fill-in all the matrices needed to solve the LCP problem + fillInMatrices(); + + // Solve the LCP problem (computation of lambda) + lcpSolver.solve(A, b, lowLimits, highLimits, lambda); + + // TODO : Implement this method ... } diff --git a/sources/reactphysics3d/engine/ConstraintSolver.h b/sources/reactphysics3d/engine/ConstraintSolver.h index 9c5e9ad4..078c4bb9 100644 --- a/sources/reactphysics3d/engine/ConstraintSolver.h +++ b/sources/reactphysics3d/engine/ConstraintSolver.h @@ -26,6 +26,9 @@ // ReactPhysics3D namespace namespace reactphysics3d { +// Constants +const unsigned int MAX_LCP_ITERATIONS = 10; // Maximum number of iterations when solving a LCP problem + /* ------------------------------------------------------------------- Class ConstrainSolver : This class represents the constraint solver. The goal is to @@ -34,13 +37,32 @@ namespace reactphysics3d { */ class ConstraintSolver { protected: - void allocate(std::vector& constraints, double dt); // Allocate all the matrices needed to solve the LCP problem - void fillInMatrices(std::vector constraints); // Fill in all the matrices needed to solve the LCP problem + LCPSolver* lcpSolver; // LCP Solver + std::vector activeConstraints; // Current active constraints in the physics world + std::vector bodies; // Set of bodies in the physics world + unsigned int nbBodies; // Number of bodies in the physics world + unsigned int** bodyMapping // 2-dimensional array that contains the mapping of body index + // in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains + // the integer index of 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 errorVector; // Error vector of all constraints + Vector lowLimits; // Vector that contains the low limits of the LCP problem + Vector highLimits; // Vector that contains the high limits 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 internal linear and angular velocities of each body + Vector Fc; // Vector that contains internal forces and torques of each body due to constraints + void allocate(std::vector& constraints); // 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 public: - ConstraintSolver(); // Constructor - virtual ~ConstraintSolver(); // Destructor - void solve(std::vector& constraints, double dt); // Solve the current LCP problem + ConstraintSolver(); // Constructor + virtual ~ConstraintSolver(); // Destructor + void solve(std::vector& constraints, std::vector* bodies, double dt); // Solve the current LCP problem }; } // End of ReactPhysics3D namespace