mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@319 92aac97c-a6ce-11dd-a772-7fcde58d38e6

Browse Source
This commit is contained in:
chappuis.daniel 2010-05-19 19:46:11 +00:00
parent a4e7560e21
commit d37110fc38
2 changed files with 43 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
sources/reactphysics3d/engine/ConstraintSolver.cpp
View File

@ -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<RigidBody*>(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<activeConstraints.size(); c++) {
// Substract 1.0/dt*J*V to the vector b
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
b -= oneOverDT * (J_sp(c, 0) * V.getSubVector(indexBody1, 6));
b -= oneOverDT * (J_sp(c, 1) * V.getSubVector(indexBody2, 6));
}
b.setValue(c, b.getValue(c) - oneOverDT * (J_sp.getSubMatrix(c, 0, 1, 6) * V.getSubVector(indexBody1*6, 6)).getValue(0,0));
b.setValue(c, b.getValue(c) - oneOverDT * (J_sp.getSubMatrix(c, 6, 1, 6) * V.getSubVector(indexBody2*6, 6)).getValue(0,0));
// TODO : Continue to implement this method ... compute and remove J*Minv*Fext from b
// Substract J*M^-1*F_ext to the vector b
b.setValue(c, b.getValue(c) - ((J_sp.getSubMatrix(c, 0, 1, 6) * Minv_sp.getSubMatrix(indexBody1*6, 0, 6, 6))*Fext.getSubVector(indexBody1*6, 6)
+ (J_sp.getSubMatrix(c, 6, 1, 6) * Minv_sp.getSubMatrix(indexBody2*6, 0, 6, 6))*Fext.getSubVector(indexBody2*6, 6))).getValue(0,0);
}
}
// Compute the matrix B_sp

57
sources/reactphysics3d/engine/ConstraintSolver.h
View File

@ -23,6 +23,7 @@
// Libraries
#include "../typeDefinitions.h"
#include "../constraint/Constraint.h"
#include "../mathematics/lcp/LCPProjectedGaussSeidel.h"
#include "PhysicsWorld.h"
#include <map>
@ -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<Constraint*> activeConstraints; // Current active constraints in the physics world
std::vector<Body*> constraintBodies; // Bodies that are implied in some constraint
uint nbBodies; // Current number of bodies in the physics world
std::map<Body*, uint> 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<Constraint*> activeConstraints; // Current active constraints in the physics world
std::vector<Body*> constraintBodies; // Bodies that are implied in some constraint
uint nbBodies; // Current number of bodies in the physics world
std::map<Body*, uint> 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