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

Optimization of the LCP Solver

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@448 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2011-11-09 20:18:32 +00:00
parent f07bd457f8
commit 9bfb44597f
12 changed files with 393 additions and 333 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/constants.h
View File

@ -42,18 +42,20 @@ const double INFINITY_CONST = std::numeric_limits<double>::infinity(); // I
const double PI = 3.14159265; // Pi constant const double PI = 3.14159265; // Pi constant
// Physics Engine constants // Physics Engine constants
const double DEFAULT_TIMESTEP = 0.002; const double DEFAULT_TIMESTEP = 1.0 / 60.0; // Default internal constant timestep in seconds
// GJK Algorithm parameters // GJK Algorithm parameters
const double OBJECT_MARGIN = 0.04; // Object margin for collision detection const double OBJECT_MARGIN = 0.04; // Object margin for collision detection in cm
// Contact constants // Contact constants
const double FRICTION_COEFFICIENT = 0.4; // Friction coefficient const double FRICTION_COEFFICIENT = 0.4; // Friction coefficient
const double PENETRATION_FACTOR = 0.2; // Penetration factor (between 0 and 1) which specify the importance of the const double PENETRATION_FACTOR = 5.0; // Penetration factor (between 0 and 1) which specify the importance of the
// penetration depth in order to calculate the correct impulse for the contact // penetration depth in order to calculate the correct impulse for the contact
const double PERSISTENT_CONTACT_DIST_THRESHOLD = 0.02; // Distance threshold for two contact points for a valid persistent contact const double PERSISTENT_CONTACT_DIST_THRESHOLD = 0.02; // Distance threshold for two contact points for a valid persistent contact
const int NB_MAX_BODIES = 100000; // Maximum number of bodies
const int NB_MAX_CONTACTS = 100000; // Maximum number of contacts (for memory pool allocation) const int NB_MAX_CONTACTS = 100000; // Maximum number of contacts (for memory pool allocation)
const int NB_MAX_CONSTRAINTS = 100000; // Maximum number of constraints
const int NB_MAX_COLLISION_PAIRS = 10000; // Maximum number of collision pairs of bodies (for memory pool allocation) const int NB_MAX_COLLISION_PAIRS = 10000; // Maximum number of collision pairs of bodies (for memory pool allocation)
// Constraint solver constants // Constraint solver constants

8
src/constraint/Constraint.h
View File

@ -54,10 +54,10 @@ class Constraint {
Body* const getBody1() const; // Return the reference to the body 1 Body* const getBody1() const; // Return the reference to the body 1
Body* const getBody2() const; // Return the reference to the body 2 // Evaluate the constraint Body* const getBody2() const; // Return the reference to the body 2 // Evaluate the constraint
bool isActive() const; // Return true if the constraint is active // Return the jacobian matrix of body 2 bool isActive() const; // Return true if the constraint is active // Return the jacobian matrix of body 2
virtual void computeJacobian(int noConstraint, Matrix1x6**& J_sp) const=0; // Compute the jacobian matrix for all mathematical constraints virtual void computeJacobian(int noConstraint, double J_sp[NB_MAX_CONSTRAINTS][2*6]) const=0; // Compute the jacobian matrix for all mathematical constraints
virtual void computeLowerBound(int noConstraint, Vector& lowerBounds) const=0; // Compute the lowerbounds values for all the mathematical constraints virtual void computeLowerBound(int noConstraint, double lowerBounds[NB_MAX_CONSTRAINTS]) const=0; // Compute the lowerbounds values for all the mathematical constraints
virtual void computeUpperBound(int noConstraint, Vector& upperBounds) const=0; // Compute the upperbounds values for all the mathematical constraints virtual void computeUpperBound(int noConstraint, double upperBounds[NB_MAX_CONSTRAINTS]) const=0; // Compute the upperbounds values for all the mathematical constraints
virtual void computeErrorValue(int noConstraint, Vector& errorValues) const=0; // Compute the error values for all the mathematical constraints virtual void computeErrorValue(int noConstraint, double errorValues[], double penetrationFactor) const=0; // Compute the error values for all the mathematical constraints
unsigned int getNbConstraints() const; // Return the number of mathematical constraints // Return the number of auxiliary constraints unsigned int getNbConstraints() const; // Return the number of mathematical constraints // Return the number of auxiliary constraints
double getCachedLambda(int index) const; // Get one cached lambda value double getCachedLambda(int index) const; // Get one cached lambda value
void setCachedLambda(int index, double lambda); // Set on cached lambda value void setCachedLambda(int index, double lambda); // Set on cached lambda value

106
src/constraint/Contact.cpp
View File

@ -54,7 +54,7 @@ Contact::~Contact() {
// fill in this matrix with all the jacobian matrix of the mathematical constraint // fill in this matrix with all the jacobian matrix of the mathematical constraint
// of the contact. The argument "noConstraint", is the row were the method have // of the contact. The argument "noConstraint", is the row were the method have
// to start to fill in the J_sp matrix. // to start to fill in the J_sp matrix.
void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const { void Contact::computeJacobian(int noConstraint, double J_sp[NB_MAX_CONSTRAINTS][2*6]) const {
assert(body1); assert(body1);
assert(body2); assert(body2);
@ -68,20 +68,20 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const {
Vector3 r2CrossN = r2.cross(normal); Vector3 r2CrossN = r2.cross(normal);
// Compute the jacobian matrix for the body 1 for the contact constraint // Compute the jacobian matrix for the body 1 for the contact constraint
J_sp[currentIndex][0].setValue(0, -normal.getX()); J_sp[currentIndex][0] = -normal.getX();
J_sp[currentIndex][0].setValue(1, -normal.getY()); J_sp[currentIndex][1] = -normal.getY();
J_sp[currentIndex][0].setValue(2, -normal.getZ()); J_sp[currentIndex][2] = -normal.getZ();
J_sp[currentIndex][0].setValue(3, -r1CrossN.getX()); J_sp[currentIndex][3] = -r1CrossN.getX();
J_sp[currentIndex][0].setValue(4, -r1CrossN.getY()); J_sp[currentIndex][4] = -r1CrossN.getY();
J_sp[currentIndex][0].setValue(5, -r1CrossN.getZ()); J_sp[currentIndex][5] = -r1CrossN.getZ();
// Compute the jacobian matrix for the body 2 for the contact constraint // Compute the jacobian matrix for the body 2 for the contact constraint
J_sp[currentIndex][1].setValue(0, normal.getX()); J_sp[currentIndex][6] = normal.getX();
J_sp[currentIndex][1].setValue(1, normal.getY()); J_sp[currentIndex][7] = normal.getY();
J_sp[currentIndex][1].setValue(2, normal.getZ()); J_sp[currentIndex][8] = normal.getZ();
J_sp[currentIndex][1].setValue(3, r2CrossN.getX()); J_sp[currentIndex][9] = r2CrossN.getX();
J_sp[currentIndex][1].setValue(4, r2CrossN.getY()); J_sp[currentIndex][10] = r2CrossN.getY();
J_sp[currentIndex][1].setValue(5, r2CrossN.getZ()); J_sp[currentIndex][11] = r2CrossN.getZ();
currentIndex++; currentIndex++;
@ -90,66 +90,66 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const {
Vector3 r2CrossU1 = r2.cross(frictionVectors[0]); Vector3 r2CrossU1 = r2.cross(frictionVectors[0]);
Vector3 r1CrossU2 = r1.cross(frictionVectors[1]); Vector3 r1CrossU2 = r1.cross(frictionVectors[1]);
Vector3 r2CrossU2 = r2.cross(frictionVectors[1]); Vector3 r2CrossU2 = r2.cross(frictionVectors[1]);
J_sp[currentIndex][0].setValue(0, -frictionVectors[0].getX()); J_sp[currentIndex][0] = -frictionVectors[0].getX();
J_sp[currentIndex][0].setValue(1, -frictionVectors[0].getY()); J_sp[currentIndex][1] = -frictionVectors[0].getY();
J_sp[currentIndex][0].setValue(2, -frictionVectors[0].getZ()); J_sp[currentIndex][2] = -frictionVectors[0].getZ();
J_sp[currentIndex][0].setValue(3, -r1CrossU1.getX()); J_sp[currentIndex][3] = -r1CrossU1.getX();
J_sp[currentIndex][0].setValue(4, -r1CrossU1.getY()); J_sp[currentIndex][4] = -r1CrossU1.getY();
J_sp[currentIndex][0].setValue(5, -r1CrossU1.getZ()); J_sp[currentIndex][5] = -r1CrossU1.getZ();
// Compute the jacobian matrix for the body 2 for the first friction constraint // Compute the jacobian matrix for the body 2 for the first friction constraint
J_sp[currentIndex][1].setValue(0, frictionVectors[0].getX()); J_sp[currentIndex][6] = frictionVectors[0].getX();
J_sp[currentIndex][1].setValue(1, frictionVectors[0].getY()); J_sp[currentIndex][7] = frictionVectors[0].getY();
J_sp[currentIndex][1].setValue(2, frictionVectors[0].getZ()); J_sp[currentIndex][8] = frictionVectors[0].getZ();
J_sp[currentIndex][1].setValue(3, r2CrossU1.getX()); J_sp[currentIndex][9] = r2CrossU1.getX();
J_sp[currentIndex][1].setValue(4, r2CrossU1.getY()); J_sp[currentIndex][10] = r2CrossU1.getY();
J_sp[currentIndex][1].setValue(5, r2CrossU1.getZ()); J_sp[currentIndex][11] = r2CrossU1.getZ();
currentIndex++; currentIndex++;
// Compute the jacobian matrix for the body 1 for the second friction constraint // Compute the jacobian matrix for the body 1 for the second friction constraint
J_sp[currentIndex][0].setValue(0, -frictionVectors[1].getX()); J_sp[currentIndex][0] = -frictionVectors[1].getX();
J_sp[currentIndex][0].setValue(1, -frictionVectors[1].getY()); J_sp[currentIndex][1] = -frictionVectors[1].getY();
J_sp[currentIndex][0].setValue(2, -frictionVectors[1].getZ()); J_sp[currentIndex][2] = -frictionVectors[1].getZ();
J_sp[currentIndex][0].setValue(3, -r1CrossU2.getX()); J_sp[currentIndex][3] = -r1CrossU2.getX();
J_sp[currentIndex][0].setValue(4, -r1CrossU2.getY()); J_sp[currentIndex][4] = -r1CrossU2.getY();
J_sp[currentIndex][0].setValue(5, -r1CrossU2.getZ()); J_sp[currentIndex][5] = -r1CrossU2.getZ();
// Compute the jacobian matrix for the body 2 for the second friction constraint // Compute the jacobian matrix for the body 2 for the second friction constraint
J_sp[currentIndex][1].setValue(0, frictionVectors[1].getX()); J_sp[currentIndex][6] = frictionVectors[1].getX();
J_sp[currentIndex][1].setValue(1, frictionVectors[1].getY()); J_sp[currentIndex][7] = frictionVectors[1].getY();
J_sp[currentIndex][1].setValue(2, frictionVectors[1].getZ()); J_sp[currentIndex][8] = frictionVectors[1].getZ();
J_sp[currentIndex][1].setValue(3, r2CrossU2.getX()); J_sp[currentIndex][9] = r2CrossU2.getX();
J_sp[currentIndex][1].setValue(4, r2CrossU2.getY()); J_sp[currentIndex][10] = r2CrossU2.getY();
J_sp[currentIndex][1].setValue(5, r2CrossU2.getZ()); J_sp[currentIndex][11] = r2CrossU2.getZ();
} }
// Compute the lowerbounds values for all the mathematical constraints. The // Compute the lowerbounds values for all the mathematical constraints. The
// argument "lowerBounds" is the lowerbounds values vector of the constraint solver and // argument "lowerBounds" is the lowerbounds values vector of the constraint solver and
// this methods has to fill in this vector starting from the row "noConstraint" // this methods has to fill in this vector starting from the row "noConstraint"
void Contact::computeLowerBound(int noConstraint, Vector& lowerBounds) const { void Contact::computeLowerBound(int noConstraint, double lowerBounds[NB_MAX_CONSTRAINTS]) const {
assert(noConstraint >= 0 && noConstraint + nbConstraints <= lowerBounds.getNbComponent()); assert(noConstraint >= 0 && noConstraint + nbConstraints <= NB_MAX_CONSTRAINTS);
lowerBounds.setValue(noConstraint, 0.0); // Lower bound for the contact constraint lowerBounds[noConstraint] = 0.0; // Lower bound for the contact constraint
lowerBounds.setValue(noConstraint + 1, -mu_mc_g); // Lower bound for the first friction constraint lowerBounds[noConstraint + 1] = -mu_mc_g; // Lower bound for the first friction constraint
lowerBounds.setValue(noConstraint + 2, -mu_mc_g); // Lower bound for the second friction constraint lowerBounds[noConstraint + 2] = -mu_mc_g; // Lower bound for the second friction constraint
} }
// Compute the upperbounds values for all the mathematical constraints. The // Compute the upperbounds values for all the mathematical constraints. The
// argument "upperBounds" is the upperbounds values vector of the constraint solver and // argument "upperBounds" is the upperbounds values vector of the constraint solver and
// this methods has to fill in this vector starting from the row "noConstraint" // this methods has to fill in this vector starting from the row "noConstraint"
void Contact::computeUpperBound(int noConstraint, Vector& upperBounds) const { void Contact::computeUpperBound(int noConstraint, double upperBounds[NB_MAX_CONSTRAINTS]) const {
assert(noConstraint >= 0 && noConstraint + nbConstraints <= upperBounds.getNbComponent()); assert(noConstraint >= 0 && noConstraint + nbConstraints <= NB_MAX_CONSTRAINTS);
upperBounds.setValue(noConstraint, INFINITY_CONST); // Upper bound for the contact constraint upperBounds[noConstraint] = INFINITY_CONST; // Upper bound for the contact constraint
upperBounds.setValue(noConstraint + 1, mu_mc_g); // Upper bound for the first friction constraint upperBounds[noConstraint + 1] = mu_mc_g; // Upper bound for the first friction constraint
upperBounds.setValue(noConstraint + 2, mu_mc_g); // Upper bound for the second friction constraint upperBounds[noConstraint + 2] = mu_mc_g; // Upper bound for the second friction constraint
} }
// Compute the error values for all the mathematical constraints. The argument // Compute the error values for all the mathematical constraints. The argument
// "errorValues" is the error values vector of the constraint solver and this // "errorValues" is the error values vector of the constraint solver and this
// method has to fill in this vector starting from the row "noConstraint" // method has to fill in this vector starting from the row "noConstraint"
void Contact::computeErrorValue(int noConstraint, Vector& errorValues) const { void Contact::computeErrorValue(int noConstraint, double errorValues[], double penetrationFactor) const {
assert(body1); assert(body1);
assert(body2); assert(body2);
@ -157,16 +157,16 @@ void Contact::computeErrorValue(int noConstraint, Vector& errorValues) const {
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(body1); RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(body1);
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(body2); RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(body2);
assert(noConstraint >= 0 && noConstraint + nbConstraints <= errorValues.getNbComponent()); assert(noConstraint >= 0 && noConstraint + nbConstraints <= NB_MAX_CONSTRAINTS);
// Compute the error value for the contact constraint // Compute the error value for the contact constraint
Vector3 velocity1 = rigidBody1->getLinearVelocity(); Vector3 velocity1 = rigidBody1->getLinearVelocity();
Vector3 velocity2 = rigidBody2->getLinearVelocity(); Vector3 velocity2 = rigidBody2->getLinearVelocity();
double restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution(); double restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution();
double errorValue = restitutionCoeff * (normal.dot(velocity1) - normal.dot(velocity2)) + PENETRATION_FACTOR * penetrationDepth; double errorValue = restitutionCoeff * (normal.dot(velocity1) - normal.dot(velocity2)) + penetrationFactor * penetrationDepth;
// Assign the error value to the vector of error values // Assign the error value to the vector of error values
errorValues.setValue(noConstraint, errorValue); // Error value for contact constraint errorValues[noConstraint] = errorValue; // Error value for contact constraint
errorValues.setValue(noConstraint + 1, 0.0); // Error value for friction constraint errorValues[noConstraint + 1] = 0.0; // Error value for friction constraint
errorValues.setValue(noConstraint + 2, 0.0); // Error value for friction constraint errorValues[noConstraint + 2] = 0.0; // Error value for friction constraint
} }

11
src/constraint/Contact.h
View File

@ -76,17 +76,14 @@ class Contact : public Constraint {
Vector3 getWorldPointOnBody2() const; // Return the contact world point on body 2 Vector3 getWorldPointOnBody2() const; // Return the contact world point on body 2
void setWorldPointOnBody1(const Vector3& worldPoint); // Set the contact world point on body 1 void setWorldPointOnBody1(const Vector3& worldPoint); // Set the contact world point on body 1
void setWorldPointOnBody2(const Vector3& worldPoint); // Set the contact world point on body 2 void setWorldPointOnBody2(const Vector3& worldPoint); // Set the contact world point on body 2
virtual void computeJacobian(int noConstraint, Matrix1x6**& J_SP) const; // Compute the jacobian matrix for all mathematical constraints virtual void computeJacobian(int noConstraint, double J_SP[NB_MAX_CONSTRAINTS][2*6]) const; // Compute the jacobian matrix for all mathematical constraints
virtual void computeLowerBound(int noConstraint, Vector& lowerBounds) const; // Compute the lowerbounds values for all the mathematical constraints virtual void computeLowerBound(int noConstraint, double lowerBounds[NB_MAX_CONSTRAINTS]) const; // Compute the lowerbounds values for all the mathematical constraints
virtual void computeUpperBound(int noConstraint, Vector& upperBounds) const; // Compute the upperbounds values for all the mathematical constraints virtual void computeUpperBound(int noConstraint, double upperBounds[NB_MAX_CONSTRAINTS]) const; // Compute the upperbounds values for all the mathematical constraints
virtual void computeErrorValue(int noConstraint, Vector& errorValues) const; // Compute the error values for all the mathematical constraints virtual void computeErrorValue(int noConstraint, double errorValues[], double penetrationFactor) const; // Compute the error values for all the mathematical constraints
double getPenetrationDepth() const; // Return the penetration depth double getPenetrationDepth() const; // Return the penetration depth
#ifdef VISUAL_DEBUG #ifdef VISUAL_DEBUG
void draw() const; // Draw the contact (for debugging) void draw() const; // Draw the contact (for debugging)
#endif #endif
//void* operator new(size_t, MemoryPool<Contact>& pool) throw(std::bad_alloc); // Overloaded new operator for customized memory allocation
//void operator delete(void* p, MemoryPool<Contact>& pool); // Overloaded delete operator for customized memory allocation
}; };
// Compute the two unit orthogonal vectors "v1" and "v2" that span the tangential friction plane // Compute the two unit orthogonal vectors "v1" and "v2" that span the tangential friction plane

317
src/engine/ConstraintSolver.cpp
View File

@ -1,6 +1,6 @@
/******************************************************************************** /********************************************************************************
* ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ * * ReactPhysics3D physics library, http://code.google.com/p/reactphysics3d/ *
* Copyright (c) 2010 Daniel Chappuis * * Copyright (c) 2011 Daniel Chappuis *
********************************************************************************* *********************************************************************************
* * * *
* Permission is hereby granted, free of charge, to any person obtaining a copy * * Permission is hereby granted, free of charge, to any person obtaining a copy *
@ -30,14 +30,11 @@
using namespace reactphysics3d; using namespace reactphysics3d;
using namespace std; using namespace std;
// TODO : Maybe we could use std::vector instead of Vector to store vector in the constraint solver
// Constructor // Constructor
ConstraintSolver::ConstraintSolver(PhysicsWorld* world) ConstraintSolver::ConstraintSolver(PhysicsWorld* world)
:physicsWorld(world), bodyMapping(0), nbConstraints(0), constraintsCapacity(0), :physicsWorld(world), nbConstraints(0), nbIterationsLCP(MAX_LCP_ITERATIONS),
bodiesCapacity(0), avConstraintsCapacity(0), avBodiesCapacity(0), avBodiesNumber(0), penetrationFactor(10.0) {
avConstraintsNumber(0), avBodiesCounter(0), avConstraintsCounter(0),
lcpSolver(new LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) {
} }
@ -77,106 +74,8 @@ void ConstraintSolver::initialize() {
// Compute the number of bodies that are part of some active constraint // Compute the number of bodies that are part of some active constraint
nbBodies = bodyNumberMapping.size(); nbBodies = bodyNumberMapping.size();
assert(nbConstraints > 0); assert(nbConstraints > 0 && nbConstraints <= NB_MAX_CONSTRAINTS);
assert(nbBodies > 0); assert(nbBodies > 0 && nbBodies <= NB_MAX_BODIES);
// Update the average bodies and constraints capacities
if (avBodiesCounter > AV_COUNTER_LIMIT) {
avBodiesCounter = 0;
avBodiesNumber = 0;
}
if (avConstraintsCounter > AV_COUNTER_LIMIT) {
avConstraintsCounter = 0;
avConstraintsNumber = 0;
}
avBodiesCounter++;
avConstraintsCounter++;
avBodiesNumber += nbBodies;
avConstraintsNumber += nbConstraints;
avBodiesCapacity += (avBodiesNumber / avBodiesCounter);
avConstraintsCapacity += (avConstraintsNumber / avConstraintsCounter);
// Allocate the memory needed for the constraint solver
allocate();
}
// Allocate all the memory needed to solve the LCP problem
// The goal of this method is to avoid to free and allocate the memory
// each time the constraint solver is called but only if the we effectively
// need more memory. Therefore if for instance the number of constraints to
// be solved is smaller than the constraints capacity, we don't free and reallocate
// memory because we don't need to. The problem now is that the constraints capacity
// can grow indefinitely. Therefore we use a way to free and reallocate the memory
// if the average number of constraints currently solved is far less than the current
// constraints capacity
void ConstraintSolver::allocate() {
// If we need to allocate more memory for the bodies
if (nbBodies > bodiesCapacity || avBodiesCapacity < AV_PERCENT_TO_FREE * bodiesCapacity) {
freeMemory(true);
bodiesCapacity = nbBodies;
Minv_sp = new Matrix6x6[nbBodies];
V1 = new Vector6D[nbBodies];
Vconstraint = new Vector6D[nbBodies];
Fext = new Vector6D[nbBodies];
avBodiesNumber = 0;
avBodiesCounter = 0;
}
// If we need to allocate more memory for the constraints
if (nbConstraints > constraintsCapacity || constraintsCapacity < AV_PERCENT_TO_FREE * constraintsCapacity) {
freeMemory(false);
constraintsCapacity = nbConstraints;
bodyMapping = new Body**[nbConstraints];
J_sp = new Matrix1x6*[nbConstraints];
B_sp = new Vector6D*[2];
B_sp[0] = new Vector6D[nbConstraints];
B_sp[1] = new Vector6D[nbConstraints];
for (int i=0; i<nbConstraints; i++) {
bodyMapping[i] = new Body*[2];
J_sp[i] = new Matrix1x6[2];
}
errorValues.changeSize(nbConstraints);
b.changeSize(nbConstraints);
lambda.changeSize(nbConstraints);
lambdaInit.changeSize(nbConstraints);
lowerBounds.changeSize(nbConstraints);
upperBounds.changeSize(nbConstraints);
avConstraintsNumber = 0;
avConstraintsCounter = 0;
}
}
// Free the memory that was allocated in the allocate() method
// If the argument is true the method will free the memory
// associated to the bodies. In the other case, it will free
// the memory associated with the constraints
void ConstraintSolver::freeMemory(bool freeBodiesMemory) {
// If we need to free the bodies memory
if (freeBodiesMemory && bodiesCapacity > 0) {
delete[] Minv_sp;
delete[] V1;
delete[] Vconstraint;
delete[] Fext;
}
else if (constraintsCapacity > 0) { // If we need to free the constraints memory
// Free the bodyMaping array
for (uint i=0; i<constraintsCapacity; i++) {
delete[] bodyMapping[i];
delete[] J_sp[i];
}
delete[] bodyMapping;
delete[] J_sp;
delete[] B_sp[0];
delete[] B_sp[1];
delete[] B_sp;
}
} }
// Fill in all the matrices needed to solve the LCP problem // Fill in all the matrices needed to solve the LCP problem
@ -205,11 +104,11 @@ void ConstraintSolver::fillInMatrices() {
constraint->computeUpperBound(noConstraint, upperBounds); constraint->computeUpperBound(noConstraint, upperBounds);
// Fill in the error vector // Fill in the error vector
constraint->computeErrorValue(noConstraint, errorValues); constraint->computeErrorValue(noConstraint, errorValues, penetrationFactor);
// Get the cached lambda values of the constraint // Get the cached lambda values of the constraint
for (int i=0; i<constraint->getNbConstraints(); i++) { for (int i=0; i<constraint->getNbConstraints(); i++) {
lambdaInit.setValue(noConstraint + i, constraint->getCachedLambda(i)); lambdaInit[noConstraint + i] = constraint->getCachedLambda(i);
} }
noConstraint += constraint->getNbConstraints(); noConstraint += constraint->getNbConstraints();
@ -230,43 +129,42 @@ void ConstraintSolver::fillInMatrices() {
// Compute the vector V1 with initial velocities values // Compute the vector V1 with initial velocities values
Vector3 linearVelocity = rigidBody->getLinearVelocity(); Vector3 linearVelocity = rigidBody->getLinearVelocity();
Vector3 angularVelocity = rigidBody->getAngularVelocity(); Vector3 angularVelocity = rigidBody->getAngularVelocity();
V1[bodyNumber].setValue(0, linearVelocity[0]); int bodyIndexArray = 6 * bodyNumber;
V1[bodyNumber].setValue(1, linearVelocity[1]); V1[bodyIndexArray] = linearVelocity[0];
V1[bodyNumber].setValue(2, linearVelocity[2]); V1[bodyIndexArray + 1] = linearVelocity[1];
V1[bodyNumber].setValue(3, angularVelocity[0]); V1[bodyIndexArray + 2] = linearVelocity[2];
V1[bodyNumber].setValue(4, angularVelocity[1]); V1[bodyIndexArray + 3] = angularVelocity[0];
V1[bodyNumber].setValue(5, angularVelocity[2]); V1[bodyIndexArray + 4] = angularVelocity[1];
V1[bodyIndexArray + 5] = angularVelocity[2];
// Compute the vector Vconstraint with final constraint velocities // Compute the vector Vconstraint with final constraint velocities
Vconstraint[bodyNumber].initWithValue(0.0); Vconstraint[bodyIndexArray] = 0.0;
Vconstraint[bodyIndexArray + 1] = 0.0;
Vconstraint[bodyIndexArray + 2] = 0.0;
Vconstraint[bodyIndexArray + 3] = 0.0;
Vconstraint[bodyIndexArray + 4] = 0.0;
Vconstraint[bodyIndexArray + 5] = 0.0;
// Compute the vector with forces and torques values // Compute the vector with forces and torques values
Vector3 externalForce = rigidBody->getExternalForce(); Vector3 externalForce = rigidBody->getExternalForce();
Vector3 externalTorque = rigidBody->getExternalTorque(); Vector3 externalTorque = rigidBody->getExternalTorque();
Fext[bodyNumber].setValue(0, externalForce[0]); Fext[bodyIndexArray] = externalForce[0];
Fext[bodyNumber].setValue(1, externalForce[1]); Fext[bodyIndexArray + 1] = externalForce[1];
Fext[bodyNumber].setValue(2, externalForce[2]); Fext[bodyIndexArray + 2] = externalForce[2];
Fext[bodyNumber].setValue(3, externalTorque[0]); Fext[bodyIndexArray + 3] = externalTorque[0];
Fext[bodyNumber].setValue(4, externalTorque[1]); Fext[bodyIndexArray + 4] = externalTorque[1];
Fext[bodyNumber].setValue(5, externalTorque[2]); Fext[bodyIndexArray + 5] = externalTorque[2];
// Compute the inverse sparse mass matrix // Initialize the mass and inertia tensor matrices
Minv_sp[bodyNumber].initWithValue(0.0); Minv_sp_inertia[bodyNumber].setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
const Matrix3x3& tensorInv = rigidBody->getInertiaTensorInverseWorld(); Minv_sp_mass_diag[bodyNumber] = 0.0;
// If the motion of the rigid body is enabled
if (rigidBody->getIsMotionEnabled()) { if (rigidBody->getIsMotionEnabled()) {
Minv_sp[bodyNumber].setValue(0, 0, rigidBody->getMassInverse()); Minv_sp_inertia[bodyNumber] = rigidBody->getInertiaTensorInverseWorld();
Minv_sp[bodyNumber].setValue(1, 1, rigidBody->getMassInverse()); Minv_sp_mass_diag[bodyNumber] = rigidBody->getMassInverse();
Minv_sp[bodyNumber].setValue(2, 2, rigidBody->getMassInverse());
Minv_sp[bodyNumber].setValue(3, 3, tensorInv.getValue(0, 0));
Minv_sp[bodyNumber].setValue(3, 4, tensorInv.getValue(0, 1));
Minv_sp[bodyNumber].setValue(3, 5, tensorInv.getValue(0, 2));
Minv_sp[bodyNumber].setValue(4, 3, tensorInv.getValue(1, 0));
Minv_sp[bodyNumber].setValue(4, 4, tensorInv.getValue(1, 1));
Minv_sp[bodyNumber].setValue(4, 5, tensorInv.getValue(1, 2));
Minv_sp[bodyNumber].setValue(5, 3, tensorInv.getValue(2, 0));
Minv_sp[bodyNumber].setValue(5, 4, tensorInv.getValue(2, 1));
Minv_sp[bodyNumber].setValue(5, 5, tensorInv.getValue(2, 2));
} }
} }
} }
@ -275,31 +173,73 @@ void ConstraintSolver::computeVectorB(double dt) {
uint indexBody1, indexBody2; uint indexBody1, indexBody2;
double oneOverDT = 1.0 / dt; double oneOverDT = 1.0 / dt;
b = errorValues * oneOverDT; //b = errorValues * oneOverDT;
for (uint c = 0; c<nbConstraints; c++) { for (uint c = 0; c<nbConstraints; c++) {
b[c] = errorValues[c] * oneOverDT;
// Substract 1.0/dt*J*V to the vector b // Substract 1.0/dt*J*V to the vector b
indexBody1 = bodyNumberMapping[bodyMapping[c][0]]; indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
indexBody2 = bodyNumberMapping[bodyMapping[c][1]]; indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
b.setValue(c, b.getValue(c) - (J_sp[c][0] * V1[indexBody1]) * oneOverDT); double multiplication = 0.0;
b.setValue(c, b.getValue(c) - (J_sp[c][1] * V1[indexBody2]) * oneOverDT); int body1ArrayIndex = 6 * indexBody1;
int body2ArrayIndex = 6 * indexBody2;
for (uint i=0; i<6; i++) {
multiplication += J_sp[c][i] * V1[body1ArrayIndex + i];
multiplication += J_sp[c][6 + i] * V1[body2ArrayIndex + i];
}
b[c] -= multiplication * oneOverDT ;
// Substract J*M^-1*F_ext to the vector b // Substract J*M^-1*F_ext to the vector b
b.setValue(c, b.getValue(c) - ((J_sp[c][0] * Minv_sp[indexBody1]) * Fext[indexBody1] double value1 = 0.0;
+ (J_sp[c][1] * Minv_sp[indexBody2])*Fext[indexBody2])); double value2 = 0.0;
double sum1, sum2;
value1 += J_sp[c][0] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex] +
J_sp[c][1] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex + 1] +
J_sp[c][2] * Minv_sp_mass_diag[indexBody1] * Fext[body1ArrayIndex + 2];
value2 += J_sp[c][6] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex] +
J_sp[c][7] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex + 1] +
J_sp[c][8] * Minv_sp_mass_diag[indexBody2] * Fext[body2ArrayIndex + 2];
for (uint i=0; i<3; i++) {
sum1 = 0.0;
sum2 = 0.0;
for (uint j=0; j<3; j++) {
sum1 += J_sp[c][3 + j] * Minv_sp_inertia[indexBody1].getValue(j, i);
sum2 += J_sp[c][9 + j] * Minv_sp_inertia[indexBody2].getValue(j, i);
}
value1 += sum1 * Fext[body1ArrayIndex + 3 + i];
value2 += sum2 * Fext[body2ArrayIndex + 3 + i];
}
b[c] -= value1 + value2;
} }
} }
// Compute the matrix B_sp // Compute the matrix B_sp
void ConstraintSolver::computeMatrixB_sp() { void ConstraintSolver::computeMatrixB_sp() {
uint indexBody1, indexBody2; uint indexConstraintArray, indexBody1, indexBody2;
// For each constraint // For each constraint
for (uint c = 0; c<nbConstraints; c++) { for (uint c = 0; c<nbConstraints; c++) {
indexConstraintArray = 6 * c;
indexBody1 = bodyNumberMapping[bodyMapping[c][0]]; indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
indexBody2 = bodyNumberMapping[bodyMapping[c][1]]; indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
B_sp[0][c] = Minv_sp[indexBody1] * J_sp[c][0].getTranspose(); B_sp[0][indexConstraintArray] = Minv_sp_mass_diag[indexBody1] * J_sp[c][0];
B_sp[1][c] = Minv_sp[indexBody2] * J_sp[c][1].getTranspose(); B_sp[0][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody1] * J_sp[c][1];
B_sp[0][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody1] * J_sp[c][2];
B_sp[1][indexConstraintArray] = Minv_sp_mass_diag[indexBody2] * J_sp[c][6];
B_sp[1][indexConstraintArray + 1] = Minv_sp_mass_diag[indexBody2] * J_sp[c][7];
B_sp[1][indexConstraintArray + 2] = Minv_sp_mass_diag[indexBody2] * J_sp[c][8];
for (uint i=0; i<3; i++) {
B_sp[0][indexConstraintArray + 3 + i] = 0.0;
B_sp[1][indexConstraintArray + 3 + i] = 0.0;
for (uint j=0; j<3; j++) {
B_sp[0][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody1].getValue(i, j) * J_sp[c][3 + j];
B_sp[1][indexConstraintArray + 3 + i] += Minv_sp_inertia[indexBody2].getValue(i, j) * J_sp[c][9 + j];
}
}
} }
} }
@ -308,18 +248,93 @@ void ConstraintSolver::computeMatrixB_sp() {
// V_constraint = dt * (M^-1 * J^T * lambda) // V_constraint = dt * (M^-1 * J^T * lambda)
// Note that we use the vector V to store both V1 and V_constraint. // Note that we use the vector V to store both V1 and V_constraint.
// Note that M^-1 * J^T = B. // Note that M^-1 * J^T = B.
// This method is called after that the LCP solver have computed lambda // This method is called after that the LCP solver has computed lambda
void ConstraintSolver::computeVectorVconstraint(double dt) { void ConstraintSolver::computeVectorVconstraint(double dt) {
uint indexBody1, indexBody2; uint indexBody1, indexBody2, indexBody1Array, indexBody2Array, indexConstraintArray;
uint j;
// Compute dt * (M^-1 * J^T * lambda // Compute dt * (M^-1 * J^T * lambda
for (uint i=0; i<nbConstraints; i++) { for (uint i=0; i<nbConstraints; i++) {
indexBody1 = bodyNumberMapping[bodyMapping[i][0]]; indexBody1Array = 6 * bodyNumberMapping[bodyMapping[i][0]];
indexBody2 = bodyNumberMapping[bodyMapping[i][1]]; indexBody2Array = 6 * bodyNumberMapping[bodyMapping[i][1]];
Vconstraint[indexBody1] = Vconstraint[indexBody1] + (B_sp[0][i] * lambda.getValue(i)) * dt; indexConstraintArray = 6 * i;
Vconstraint[indexBody2] = Vconstraint[indexBody2] + (B_sp[1][i] * lambda.getValue(i)) * dt; for (j=0; j<6; j++) {
Vconstraint[indexBody1Array + j] += B_sp[0][indexConstraintArray + j] * lambda[i] * dt;
Vconstraint[indexBody2Array + j] += B_sp[1][indexConstraintArray + j] * lambda[i] * dt;
} }
} }
}
// Solve a LCP problem using the Projected-Gauss-Seidel algorithm
// This method outputs the result in the lambda vector
void ConstraintSolver::solveLCP() {
for (uint i=0; i<nbConstraints; i++) {
lambda[i] = lambdaInit[i];
}
uint indexBody1Array, indexBody2Array;
double deltaLambda;
double lambdaTemp;
uint i, iter;
// Compute the vector a
computeVectorA();
// For each constraint
for (i=0; i<nbConstraints; i++) {
uint indexConstraintArray = 6 * i;
d[i] = 0.0;
for (uint j=0; j<6; j++) {
d[i] += J_sp[i][j] * B_sp[0][indexConstraintArray + j] + J_sp[i][6 + j] * B_sp[1][indexConstraintArray + j];
}
}
for(iter=0; iter<nbIterationsLCP; iter++) {
for (i=0; i<nbConstraints; i++) {
indexBody1Array = 6 * bodyNumberMapping[bodyMapping[i][0]];
indexBody2Array = 6 * bodyNumberMapping[bodyMapping[i][1]];
uint indexConstraintArray = 6 * i;
deltaLambda = b[i];
for (uint j=0; j<6; j++) {
deltaLambda -= (J_sp[i][j] * a[indexBody1Array + j] + J_sp[i][6 + j] * a[indexBody2Array + j]);
}
deltaLambda /= d[i];
lambdaTemp = lambda[i];
lambda[i] = std::max(lowerBounds[i], std::min(lambda[i] + deltaLambda, upperBounds[i]));
deltaLambda = lambda[i] - lambdaTemp;
for (uint j=0; j<6; j++) {
a[indexBody1Array + j] += B_sp[0][indexConstraintArray + j] * deltaLambda;
a[indexBody2Array + j] += B_sp[1][indexConstraintArray + j] * deltaLambda;
}
}
}
}
// Compute the vector a used in the solve() method
// Note that a = B * lambda
void ConstraintSolver::computeVectorA() {
uint i;
uint indexBody1Array, indexBody2Array;
// Init the vector a with zero values
for (i=0; i<6*nbBodies; i++) {
a[i] = 0.0;
}
for(i=0; i<nbConstraints; i++) {
indexBody1Array = 6 * bodyNumberMapping[bodyMapping[i][0]];
indexBody2Array = 6 * bodyNumberMapping[bodyMapping[i][1]];
uint indexConstraintArray = 6 * i;
for (uint j=0; j<6; j++) {
a[indexBody1Array + j] += B_sp[0][indexConstraintArray + j] * lambda[i];
a[indexBody2Array + j] += B_sp[1][indexConstraintArray + j] * lambda[i];
}
}
}
// Cache the lambda values in order to reuse them in the next step // Cache the lambda values in order to reuse them in the next step
// to initialize the lambda vector // to initialize the lambda vector
@ -332,7 +347,7 @@ void ConstraintSolver::cacheLambda() {
// For each constraint of the contact // For each constraint of the contact
for (int i=0; i<activeConstraints[c]->getNbConstraints(); i++) { for (int i=0; i<activeConstraints[c]->getNbConstraints(); i++) {
// Get the lambda value that have just been computed // Get the lambda value that have just been computed
activeConstraints[c]->setCachedLambda(i, lambda.getValue(noConstraint + i)); activeConstraints[c]->setCachedLambda(i, lambda[noConstraint + i]);
} }
noConstraint += activeConstraints[c]->getNbConstraints(); noConstraint += activeConstraints[c]->getNbConstraints();

89
src/engine/ConstraintSolver.h
View File

@ -38,60 +38,60 @@
// ReactPhysics3D namespace // ReactPhysics3D namespace
namespace reactphysics3d { namespace reactphysics3d {
/* ------------------------------------------------------------------- /* -------------------------------------------------------------------
Class ConstrainSolver : Class ConstrainSolver :
This class represents the constraint solver. The goal is to This class represents the constraint solver. The constraint solver
solve A constraint-base LCP problem. is based on the theory from the paper "Iterative Dynamics with
Temporal Coherence" from Erin Catto. We keep the same notations as
in the paper. The idea is to construct a LCP problem and then solve
it using a Projected Gauss Seidel (PGS) solver.
------------------------------------------------------------------- -------------------------------------------------------------------
*/ */
class ConstraintSolver { class ConstraintSolver {
protected: private:
PhysicsWorld* physicsWorld; // Reference to the physics world PhysicsWorld* physicsWorld; // Reference to the physics world
LCPSolver* lcpSolver; // LCP Solver
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world
uint nbIterationsLCP; // Number of iterations of the LCP solver
uint nbConstraints; // Total number of constraints (with the auxiliary constraints) uint nbConstraints; // Total number of constraints (with the auxiliary constraints)
uint nbBodies; // Current number of bodies in the physics world uint nbBodies; // Current number of bodies in the physics world
uint constraintsCapacity; // Number of constraints that are currently allocated in memory in the solver double penetrationFactor; // Penetration factor "beta" for penetration correction
uint bodiesCapacity; // Number of bodies that are currently allocated in memory in the solver
uint avConstraintsCapacity; // Average constraint capacity
uint avBodiesCapacity; // Average bodies capacity
uint avBodiesNumber; // Total bodies number for average computation
uint avConstraintsNumber; // Total constraints number for average computation
uint avBodiesCounter; // Counter used to compute the average
uint avConstraintsCounter;
std::set<Body*> constraintBodies; // Bodies that are implied in some constraint std::set<Body*> constraintBodies; // Bodies that are implied in some constraint
std::map<Body*, uint> bodyNumberMapping; // Map a body pointer with its index number 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 Body* bodyMapping[NB_MAX_CONSTRAINTS][2]; // 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 // 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 // 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 // J_sp matrix. An integer body index refers to its index in the "bodies" std::vector
Matrix1x6** J_sp; // 2-dimensional array thar correspond to the sparse representation of the jacobian matrix of all constraints double J_sp[NB_MAX_CONSTRAINTS][2*6]; // 2-dimensional array that correspond to the sparse representation of the jacobian matrix of all constraints
// The dimension of this array is nbConstraints times 2. Each cell will contain // This array contains for each constraint two 1x6 Jacobian matrices (one for each body of the constraint)
// a 1x6 matrix // a 1x6 matrix
Vector6D** B_sp; // 2-dimensional array that correspond to a useful matrix in sparse representation double B_sp[2][6*NB_MAX_CONSTRAINTS]; // 2-dimensional array that correspond to a useful matrix in sparse representation
// The dimension of this array is 2 times nbConstraints. Each cell will contain // This array contains for each constraint two 6x1 matrices (one for each body of the constraint)
// a 6x1 matrix // a 6x1 matrix
Vector b; // Vector "b" of the LCP problem double b[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem
Vector lambda; // Lambda vector of the LCP problem double d[NB_MAX_CONSTRAINTS]; // Vector "d"
Vector lambdaInit; // Lambda init vector for the LCP solver double a[6*NB_MAX_BODIES]; // Vector "a"
Vector errorValues; // Error vector of all constraints double lambda[NB_MAX_CONSTRAINTS]; // Lambda vector of the LCP problem
Vector lowerBounds; // Vector that contains the low limits for the variables of the LCP problem double lambdaInit[NB_MAX_CONSTRAINTS]; // Lambda init vector for the LCP solver
Vector upperBounds; // Vector that contains the high limits for the variables of the LCP problem double errorValues[NB_MAX_CONSTRAINTS]; // Error vector of all constraints
Matrix6x6* Minv_sp; // Sparse representation of the Matrix that contains information about mass and inertia of each body double lowerBounds[NB_MAX_CONSTRAINTS]; // Vector that contains the low limits for the variables of the LCP problem
double upperBounds[NB_MAX_CONSTRAINTS]; // Vector that contains the high limits for the variables of the LCP problem
Matrix3x3 Minv_sp_inertia[NB_MAX_BODIES]; // 3x3 world inertia tensor matrix I for each body (from the Minv_sp matrix)
double Minv_sp_mass_diag[NB_MAX_BODIES]; // Array that contains for each body the inverse of its mass
// This is an array of size nbBodies that contains in each cell a 6x6 matrix // This is an array of size nbBodies that contains in each cell a 6x6 matrix
Vector6D* V1; // Array that contains for each body the Vector that contains linear and angular velocities double V1[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains linear and angular velocities
// Each cell contains a 6x1 vector with linear and angular velocities // Each cell contains a 6x1 vector with linear and angular velocities
Vector6D* Vconstraint; // Same kind of vector as V1 but contains the final constraint velocities double Vconstraint[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities
Vector6D* Fext; // Array that contains for each body the vector that contains external forces and torques double Fext[6*NB_MAX_BODIES]; // Array that contains for each body the 6x1 vector that contains external forces and torques
// Each cell contains a 6x1 vector with external force and torque. // Each cell contains a 6x1 vector with external force and torque.
void initialize(); // Initialize the constraint solver before each solving void initialize(); // Initialize the constraint solver before each solving
void allocate(); // Allocate all the memory needed to solve the LCP problem
void fillInMatrices(); // Fill in all the matrices needed to solve the LCP problem void fillInMatrices(); // Fill in all the matrices needed to solve the LCP problem
void computeVectorB(double dt); // Compute the vector b void computeVectorB(double dt); // Compute the vector b
void computeMatrixB_sp(); // Compute the matrix B_sp void computeMatrixB_sp(); // Compute the matrix B_sp
void computeVectorVconstraint(double dt); // Compute the vector V2 void computeVectorVconstraint(double dt); // Compute the vector V2
void cacheLambda(); // Cache the lambda values in order to reuse them in the next step to initialize the lambda vector void cacheLambda(); // Cache the lambda values in order to reuse them in the next step to initialize the lambda vector
void freeMemory(bool freeBodiesMemory); // Free some memory previously allocated for the constraint solver void computeVectorA(); // Compute the vector a used in the solve() method
void solveLCP(); // Solve a LCP problem using Projected-Gauss-Seidel algorithm
public: public:
ConstraintSolver(PhysicsWorld* world); // Constructor ConstraintSolver(PhysicsWorld* world); // Constructor
@ -100,7 +100,9 @@ class ConstraintSolver {
bool isConstrainedBody(Body* body) const; // Return true if the body is in at least one constraint bool isConstrainedBody(Body* body) const; // Return true if the body is in at least one constraint
Vector3 getConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem Vector3 getConstrainedLinearVelocityOfBody(Body* body); // Return the constrained linear velocity of a body after solving the LCP problem
Vector3 getConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem Vector3 getConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem
void cleanup(); void cleanup(); // Cleanup of the constraint solver
void setPenetrationFactor(double penetrationFactor); // Set the penetration factor
void setNbLCPIterations(uint nbIterations); // Set the number of iterations of the LCP solver
}; };
// Return true if the body is in at least one constraint // Return true if the body is in at least one constraint
@ -114,16 +116,16 @@ inline bool ConstraintSolver::isConstrainedBody(Body* body) const {
// Return the constrained linear velocity of a body after solving the LCP problem // Return the constrained linear velocity of a body after solving the LCP problem
inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(Body* body) { inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(Body* body) {
assert(isConstrainedBody(body)); assert(isConstrainedBody(body));
const Vector6D& vec = Vconstraint[bodyNumberMapping[body]]; uint indexBodyArray = 6 * bodyNumberMapping[body];
return Vector3(vec.getValue(0), vec.getValue(1), vec.getValue(2)); return Vector3(Vconstraint[indexBodyArray], Vconstraint[indexBodyArray + 1], Vconstraint[indexBodyArray + 2]);
} }
// Return the constrained angular velocity of a body after solving the LCP problem // Return the constrained angular velocity of a body after solving the LCP problem
inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body) { inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body) {
assert(isConstrainedBody(body)); assert(isConstrainedBody(body));
const Vector6D& vec = Vconstraint[bodyNumberMapping[body]]; uint indexBodyArray = 6 * bodyNumberMapping[body];
return Vector3(vec.getValue(3), vec.getValue(4), vec.getValue(5)); return Vector3(Vconstraint[indexBodyArray + 3], Vconstraint[indexBodyArray + 4], Vconstraint[indexBodyArray + 5]);
} }
// Cleanup of the constraint solver // Cleanup of the constraint solver
@ -133,6 +135,16 @@ inline void ConstraintSolver::cleanup() {
activeConstraints.clear(); activeConstraints.clear();
} }
// Set the penetration factor
inline void ConstraintSolver::setPenetrationFactor(double factor) {
penetrationFactor = factor;
}
// Set the number of iterations of the LCP solver
inline void ConstraintSolver::setNbLCPIterations(uint nbIterations) {
nbIterationsLCP = nbIterations;
}
// Solve the current LCP problem // Solve the current LCP problem
inline void ConstraintSolver::solve(double dt) { inline void ConstraintSolver::solve(double dt) {
@ -142,7 +154,6 @@ inline void ConstraintSolver::solve(double dt) {
std::cout << "------ START (Constraint Solver) -----" << std::endl; std::cout << "------ START (Constraint Solver) -----" << std::endl;
gettimeofday(&timeValueStart, NULL); gettimeofday(&timeValueStart, NULL);
// Allocate memory for the matrices // Allocate memory for the matrices
initialize(); initialize();
@ -156,8 +167,7 @@ inline void ConstraintSolver::solve(double dt) {
computeMatrixB_sp(); computeMatrixB_sp();
// Solve the LCP problem (computation of lambda) // Solve the LCP problem (computation of lambda)
lcpSolver->setLambdaInit(lambdaInit); solveLCP();
lcpSolver->solve(J_sp, B_sp, nbConstraints, nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda);
// Cache the lambda values in order to use them in the next step // Cache the lambda values in order to use them in the next step
cacheLambda(); cacheLambda();
@ -171,7 +181,6 @@ inline void ConstraintSolver::solve(double dt) {
long double startTime = timeValueStart.tv_sec * 1000000.0 + (timeValueStart.tv_usec); long double startTime = timeValueStart.tv_sec * 1000000.0 + (timeValueStart.tv_usec);
long double endTime = timeValueEnd.tv_sec * 1000000.0 + (timeValueEnd.tv_usec); long double endTime = timeValueEnd.tv_sec * 1000000.0 + (timeValueEnd.tv_usec);
std::cout << "------ END (Constraint Solver) => (" << "time = " << endTime - startTime << " micro sec)-----" << std::endl; std::cout << "------ END (Constraint Solver) => (" << "time = " << endTime - startTime << " micro sec)-----" << std::endl;
} }
} // End of ReactPhysics3D namespace } // End of ReactPhysics3D namespace

12
src/engine/PhysicsEngine.h
View File

@ -61,6 +61,8 @@ public :
void start(); // Start the physics simulation void start(); // Start the physics simulation
void stop(); // Stop the physics simulation void stop(); // Stop the physics simulation
void update(); // Update the physics simulation void update(); // Update the physics simulation
void setPenetrationFactor(double factor); // Set the penetration factor for the constraint solver
void setNbLCPIterations(uint nbIterations); // Set the number of iterations of the LCP solver
}; };
// --- Inline functions --- // // --- Inline functions --- //
@ -74,6 +76,16 @@ inline void PhysicsEngine::stop() {
timer.stop(); timer.stop();
} }
// Set the penetration factor for the constraint solver
inline void PhysicsEngine::setPenetrationFactor(double factor) {
constraintSolver.setPenetrationFactor(factor);
}
// Set the number of iterations of the LCP solver
inline void PhysicsEngine::setNbLCPIterations(uint nbIterations) {
constraintSolver.setNbLCPIterations(nbIterations);
}
} }
#endif #endif

2
src/engine/PhysicsWorld.h
View File

@ -47,7 +47,7 @@ namespace reactphysics3d {
*/ */
class PhysicsWorld { class PhysicsWorld {
protected : protected :
std::vector<Body*> bodies; // list that contains all bodies of the physics world std::vector<Body*> bodies; // All the rigid bodies of the physics world
std::vector<Body*> addedBodies; // Added bodies since last update std::vector<Body*> addedBodies; // Added bodies since last update
std::vector<Body*> removedBodies; // Removed bodies since last update std::vector<Body*> removedBodies; // Removed bodies since last update
std::vector<Constraint*> constraints; // List that contains all the current constraints std::vector<Constraint*> constraints; // List that contains all the current constraints

53
src/mathematics/lcp/LCPProjectedGaussSeidel.cpp
View File

@ -42,11 +42,14 @@ LCPProjectedGaussSeidel::~LCPProjectedGaussSeidel() {
// Solve a LCP problem using the Projected-Gauss-Seidel algorithm // Solve a LCP problem using the Projected-Gauss-Seidel algorithm
// This method outputs the result in the lambda vector // This method outputs the result in the lambda vector
void LCPProjectedGaussSeidel::solve(Matrix1x6** J_sp, Vector6D** B_sp, uint nbConstraints, void LCPProjectedGaussSeidel::solve(double J_sp[NB_MAX_CONSTRAINTS][2*6], double B_sp[2][6*NB_MAX_CONSTRAINTS], uint nbConstraints,
uint nbBodies, Body*** const bodyMapping, map<Body*, uint> bodyNumberMapping, uint nbBodies, Body* bodyMapping[NB_MAX_CONSTRAINTS][2], map<Body*, uint> bodyNumberMapping,
const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const { double b[], double lowLimits[NB_MAX_CONSTRAINTS], double highLimits[NB_MAX_CONSTRAINTS], double lambda[NB_MAX_CONSTRAINTS]) const {
for (uint i=0; i<nbConstraints; i++) {
lambda[i] = lambdaInit[i];
}
lambda = lambdaInit;
double* d = new double[nbConstraints]; // TODO : Avoid those kind of memory allocation here for optimization (allocate once in the object) double* d = new double[nbConstraints]; // TODO : Avoid those kind of memory allocation here for optimization (allocate once in the object)
uint indexBody1, indexBody2; uint indexBody1, indexBody2;
@ -55,26 +58,41 @@ void LCPProjectedGaussSeidel::solve(Matrix1x6** J_sp, Vector6D** B_sp, uint nbCo
uint i, iter; uint i, iter;
Vector6D* a = new Vector6D[nbBodies]; // Array that contains nbBodies vector of dimension 6x1 Vector6D* a = new Vector6D[nbBodies]; // Array that contains nbBodies vector of dimension 6x1
// Compute the vector a // Compute the vector a
computeVectorA(lambda, nbConstraints, bodyMapping, B_sp, bodyNumberMapping, a, nbBodies); computeVectorA(lambda, nbConstraints, bodyMapping, B_sp, bodyNumberMapping, a, nbBodies);
// For each constraint // For each constraint
for (i=0; i<nbConstraints; i++) { for (i=0; i<nbConstraints; i++) {
d[i] = (J_sp[i][0] * B_sp[0][i] + J_sp[i][1] * B_sp[1][i]); uint indexConstraintArray = 6 * i;
d[i] = 0.0;
for (uint j=0; j<6; j++) {
d[i] += J_sp[i][j] * B_sp[0][indexConstraintArray + j] + J_sp[i][6 + j] * B_sp[1][indexConstraintArray + j];
} }
}
for(iter=0; iter<maxIterations; iter++) { for(iter=0; iter<maxIterations; iter++) {
for (i=0; i<nbConstraints; i++) { for (i=0; i<nbConstraints; i++) {
indexBody1 = bodyNumberMapping[bodyMapping[i][0]]; indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
indexBody2 = bodyNumberMapping[bodyMapping[i][1]]; indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
deltaLambda = (b.getValue(i) - (J_sp[i][0] * a[indexBody1]) - (J_sp[i][1] * a[indexBody2])) / d[i]; uint indexConstraintArray = 6 * i;
lambdaTemp = lambda.getValue(i); deltaLambda = b[i];
lambda.setValue(i, std::max(lowLimits.getValue(i), std::min(lambda.getValue(i) + deltaLambda, highLimits.getValue(i)))); for (uint j=0; j<6; j++) {
deltaLambda = lambda.getValue(i) - lambdaTemp; deltaLambda -= (J_sp[i][j] * a[indexBody1].getValue(j) + J_sp[i][6 + j] * a[indexBody2].getValue(j));
a[indexBody1] = a[indexBody1] + (B_sp[0][i] * deltaLambda); }
a[indexBody2] = a[indexBody2] + (B_sp[1][i] * deltaLambda); deltaLambda /= d[i];
lambdaTemp = lambda[i];
lambda[i] = std::max(lowLimits[i], std::min(lambda[i] + deltaLambda, highLimits[i]));
deltaLambda = lambda[i] - lambdaTemp;
for (uint j=0; j<6; j++) {
a[indexBody1].setValue(j, a[indexBody1].getValue(j) + (B_sp[0][indexConstraintArray + j] * deltaLambda));
a[indexBody2].setValue(j, a[indexBody2].getValue(j) + (B_sp[1][indexConstraintArray + j] * deltaLambda));
} }
} }
}
// Clean // Clean
delete[] d; delete[] d;
@ -83,8 +101,8 @@ void LCPProjectedGaussSeidel::solve(Matrix1x6** J_sp, Vector6D** B_sp, uint nbCo
// Compute the vector a used in the solve() method // Compute the vector a used in the solve() method
// Note that a = B * lambda // Note that a = B * lambda
void LCPProjectedGaussSeidel::computeVectorA(const Vector& lambda, uint nbConstraints, Body*** const bodyMapping, void LCPProjectedGaussSeidel::computeVectorA(double lambda[NB_MAX_CONSTRAINTS], uint nbConstraints, Body* bodyMapping[NB_MAX_CONSTRAINTS][2],
Vector6D** B_sp, map<Body*, uint> bodyNumberMapping, double B_sp[2][6*NB_MAX_CONSTRAINTS], map<Body*, uint> bodyNumberMapping,
Vector6D* const a, uint nbBodies) const { Vector6D* const a, uint nbBodies) const {
uint i; uint i;
uint indexBody1, indexBody2; uint indexBody1, indexBody2;
@ -94,11 +112,14 @@ void LCPProjectedGaussSeidel::computeVectorA(const Vector& lambda, uint nbConstr
a[i].initWithValue(0.0); a[i].initWithValue(0.0);
} }
for(i=0; i<nbConstraints; i++) { for(i=0; i<nbConstraints; i++) {
indexBody1 = bodyNumberMapping[bodyMapping[i][0]]; indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
indexBody2 = bodyNumberMapping[bodyMapping[i][1]]; indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
a[indexBody1] = a[indexBody1] + (B_sp[0][i] * lambda.getValue(i)); uint indexConstraintArray = 6 * i;
a[indexBody2] = a[indexBody2] + (B_sp[1][i] * lambda.getValue(i)); for (uint j=0; j<6; j++) {
a[indexBody1].setValue(j, a[indexBody1].getValue(j) + (B_sp[0][indexConstraintArray + j] * lambda[i]));
a[indexBody2].setValue(j, a[indexBody2].getValue(j) + (B_sp[1][indexConstraintArray + j] * lambda[i]));
}
} }
} }

10
src/mathematics/lcp/LCPProjectedGaussSeidel.h
View File

@ -41,16 +41,16 @@ namespace reactphysics3d {
class LCPProjectedGaussSeidel : public LCPSolver { class LCPProjectedGaussSeidel : public LCPSolver {
protected: protected:
void computeVectorA(const Vector& lambda, uint nbConstraints, Body*** const bodyMapping, void computeVectorA(double lambda[NB_MAX_CONSTRAINTS], uint nbConstraints, Body* bodyMapping[NB_MAX_CONSTRAINTS][2],
Vector6D** B_sp, std::map<Body*, uint> bodyNumberMapping, double B_sp[2][6*NB_MAX_CONSTRAINTS], std::map<Body*, uint> bodyNumberMapping,
Vector6D* const a, uint nbBodies) const ; // Compute the vector a used in the solve() method Vector6D* const a, uint nbBodies) const ; // Compute the vector a used in the solve() method
public: public:
LCPProjectedGaussSeidel(uint maxIterations); // Constructor LCPProjectedGaussSeidel(uint maxIterations); // Constructor
virtual ~LCPProjectedGaussSeidel(); // Destructor virtual ~LCPProjectedGaussSeidel(); // Destructor
virtual void solve(Matrix1x6** J_sp, Vector6D** B_sp, uint nbConstraints, virtual void solve(double J_sp[NB_MAX_CONSTRAINTS][2*6], double B_sp[2][6*NB_MAX_CONSTRAINTS], uint nbConstraints,
uint nbBodies, Body*** const bodyMapping, std::map<Body*, uint> bodyNumberMapping, uint nbBodies, Body* bodyMapping[NB_MAX_CONSTRAINTS][2], std::map<Body*, uint> bodyNumberMapping,
const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const; // Solve a LCP problem using Projected-Gauss-Seidel algorithm // Set the initial value for lambda double b[], double lowLimits[NB_MAX_CONSTRAINTS], double highLimits[NB_MAX_CONSTRAINTS], double lambda[NB_MAX_CONSTRAINTS]) const; // Solve a LCP problem using Projected-Gauss-Seidel algorithm // Set the initial value for lambda
}; };
} // End of the ReactPhysics3D namespace } // End of the ReactPhysics3D namespace

17
src/mathematics/lcp/LCPSolver.h
View File

@ -60,22 +60,23 @@ namespace reactphysics3d {
class LCPSolver { class LCPSolver {
protected: protected:
uint maxIterations; // Maximum number of iterations uint maxIterations; // Maximum number of iterations
Vector lambdaInit; // Initial value for lambda at the beginning of the algorithm double lambdaInit[NB_MAX_CONSTRAINTS]; // Initial value for lambda at the beginning of the algorithm
public: public:
LCPSolver(uint maxIterations); // Constructor LCPSolver(uint maxIterations); // Constructor
virtual ~LCPSolver(); // Destructor virtual ~LCPSolver(); // Destructor
virtual void solve(Matrix1x6** J_sp, Vector6D** B_sp, uint nbConstraints, virtual void solve(double J_sp[NB_MAX_CONSTRAINTS][2*6], double B_sp[2][6*NB_MAX_CONSTRAINTS], uint nbConstraints,
uint nbBodies, Body*** const bodyMapping, std::map<Body*, uint> bodyNumberMapping, uint nbBodies, Body* bodyMapping[NB_MAX_CONSTRAINTS][2], std::map<Body*, uint> bodyNumberMapping,
const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const=0; // Solve a LCP problem double b[], double lowLimits[NB_MAX_CONSTRAINTS], double highLimits[NB_MAX_CONSTRAINTS], double lambda[NB_MAX_CONSTRAINTS]) const=0; // Solve a LCP problem
void setLambdaInit(const Vector& lambdaInit); // Set the initial lambda vector void setLambdaInit(double lambdaInit[NB_MAX_CONSTRAINTS], uint nbConstraints); // Set the initial lambda vector
void setMaxIterations(uint maxIterations); // Set the maximum number of iterations void setMaxIterations(uint maxIterations); // Set the maximum number of iterations
}; };
// Set the initial lambda vector // Set the initial lambda vector
inline void LCPSolver::setLambdaInit(const Vector& lambdaInit) { inline void LCPSolver::setLambdaInit(double lambdaInit[NB_MAX_CONSTRAINTS], uint nbConstraints) {
this->lambdaInit.changeSize(lambdaInit.getNbComponent()); for (uint i=0; i<nbConstraints; i++) {
this->lambdaInit = lambdaInit; this->lambdaInit[i] = lambdaInit[i];
}
} }
// Set the maximum number of iterations // Set the maximum number of iterations

3
src/memory/MemoryPool.h
View File

@ -32,6 +32,9 @@
#include <cassert> #include <cassert>
#include <new> #include <new>
// TODO : Check that casting is done correctly in this class using
// C++ cast operator like reinterpret_cast<>, ...
// ReactPhysics3D namespace // ReactPhysics3D namespace
namespace reactphysics3d { namespace reactphysics3d {