Optimization of the LCP Solver
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@448 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel
parent
f07bd457f8
commit
9bfb44597f
|
|
@ -42,18 +42,20 @@ const double INFINITY_CONST = std::numeric_limits<double>::infinity(); // I
|
|||
const double PI = 3.14159265; // Pi constant
|
||||
|
||||
// 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
|
||||
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
|
||||
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
|
||||
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_CONSTRAINTS = 100000; // Maximum number of constraints
|
||||
const int NB_MAX_COLLISION_PAIRS = 10000; // Maximum number of collision pairs of bodies (for memory pool allocation)
|
||||
|
||||
// Constraint solver constants
|
||||
|
|
|
|||
|
|
@ -53,14 +53,14 @@ class Constraint {
|
|||
virtual ~Constraint(); // Destructor
|
||||
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
|
||||
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 computeLowerBound(int noConstraint, Vector& lowerBounds) 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 computeErrorValue(int noConstraint, Vector& errorValues) 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
|
||||
double getCachedLambda(int index) const; // Get one cached lambda value
|
||||
void setCachedLambda(int index, double lambda); // Set on cached lambda value
|
||||
bool isActive() const; // Return true if the constraint is active // Return the jacobian matrix of body 2
|
||||
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, double lowerBounds[NB_MAX_CONSTRAINTS]) const=0; // Compute the lowerbounds 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, 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
|
||||
double getCachedLambda(int index) const; // Get one cached lambda value
|
||||
void setCachedLambda(int index, double lambda); // Set on cached lambda value
|
||||
};
|
||||
|
||||
// Return the reference to the body 1
|
||||
|
|
|
|||
|
|
@ -54,7 +54,7 @@ Contact::~Contact() {
|
|||
// 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
|
||||
// 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(body2);
|
||||
|
||||
|
|
@ -68,20 +68,20 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const {
|
|||
Vector3 r2CrossN = r2.cross(normal);
|
||||
|
||||
// Compute the jacobian matrix for the body 1 for the contact constraint
|
||||
J_sp[currentIndex][0].setValue(0, -normal.getX());
|
||||
J_sp[currentIndex][0].setValue(1, -normal.getY());
|
||||
J_sp[currentIndex][0].setValue(2, -normal.getZ());
|
||||
J_sp[currentIndex][0].setValue(3, -r1CrossN.getX());
|
||||
J_sp[currentIndex][0].setValue(4, -r1CrossN.getY());
|
||||
J_sp[currentIndex][0].setValue(5, -r1CrossN.getZ());
|
||||
J_sp[currentIndex][0] = -normal.getX();
|
||||
J_sp[currentIndex][1] = -normal.getY();
|
||||
J_sp[currentIndex][2] = -normal.getZ();
|
||||
J_sp[currentIndex][3] = -r1CrossN.getX();
|
||||
J_sp[currentIndex][4] = -r1CrossN.getY();
|
||||
J_sp[currentIndex][5] = -r1CrossN.getZ();
|
||||
|
||||
// Compute the jacobian matrix for the body 2 for the contact constraint
|
||||
J_sp[currentIndex][1].setValue(0, normal.getX());
|
||||
J_sp[currentIndex][1].setValue(1, normal.getY());
|
||||
J_sp[currentIndex][1].setValue(2, normal.getZ());
|
||||
J_sp[currentIndex][1].setValue(3, r2CrossN.getX());
|
||||
J_sp[currentIndex][1].setValue(4, r2CrossN.getY());
|
||||
J_sp[currentIndex][1].setValue(5, r2CrossN.getZ());
|
||||
J_sp[currentIndex][6] = normal.getX();
|
||||
J_sp[currentIndex][7] = normal.getY();
|
||||
J_sp[currentIndex][8] = normal.getZ();
|
||||
J_sp[currentIndex][9] = r2CrossN.getX();
|
||||
J_sp[currentIndex][10] = r2CrossN.getY();
|
||||
J_sp[currentIndex][11] = r2CrossN.getZ();
|
||||
|
||||
currentIndex++;
|
||||
|
||||
|
|
@ -90,66 +90,66 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const {
|
|||
Vector3 r2CrossU1 = r2.cross(frictionVectors[0]);
|
||||
Vector3 r1CrossU2 = r1.cross(frictionVectors[1]);
|
||||
Vector3 r2CrossU2 = r2.cross(frictionVectors[1]);
|
||||
J_sp[currentIndex][0].setValue(0, -frictionVectors[0].getX());
|
||||
J_sp[currentIndex][0].setValue(1, -frictionVectors[0].getY());
|
||||
J_sp[currentIndex][0].setValue(2, -frictionVectors[0].getZ());
|
||||
J_sp[currentIndex][0].setValue(3, -r1CrossU1.getX());
|
||||
J_sp[currentIndex][0].setValue(4, -r1CrossU1.getY());
|
||||
J_sp[currentIndex][0].setValue(5, -r1CrossU1.getZ());
|
||||
J_sp[currentIndex][0] = -frictionVectors[0].getX();
|
||||
J_sp[currentIndex][1] = -frictionVectors[0].getY();
|
||||
J_sp[currentIndex][2] = -frictionVectors[0].getZ();
|
||||
J_sp[currentIndex][3] = -r1CrossU1.getX();
|
||||
J_sp[currentIndex][4] = -r1CrossU1.getY();
|
||||
J_sp[currentIndex][5] = -r1CrossU1.getZ();
|
||||
|
||||
// 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][1].setValue(1, frictionVectors[0].getY());
|
||||
J_sp[currentIndex][1].setValue(2, frictionVectors[0].getZ());
|
||||
J_sp[currentIndex][1].setValue(3, r2CrossU1.getX());
|
||||
J_sp[currentIndex][1].setValue(4, r2CrossU1.getY());
|
||||
J_sp[currentIndex][1].setValue(5, r2CrossU1.getZ());
|
||||
J_sp[currentIndex][6] = frictionVectors[0].getX();
|
||||
J_sp[currentIndex][7] = frictionVectors[0].getY();
|
||||
J_sp[currentIndex][8] = frictionVectors[0].getZ();
|
||||
J_sp[currentIndex][9] = r2CrossU1.getX();
|
||||
J_sp[currentIndex][10] = r2CrossU1.getY();
|
||||
J_sp[currentIndex][11] = r2CrossU1.getZ();
|
||||
|
||||
currentIndex++;
|
||||
|
||||
// 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].setValue(1, -frictionVectors[1].getY());
|
||||
J_sp[currentIndex][0].setValue(2, -frictionVectors[1].getZ());
|
||||
J_sp[currentIndex][0].setValue(3, -r1CrossU2.getX());
|
||||
J_sp[currentIndex][0].setValue(4, -r1CrossU2.getY());
|
||||
J_sp[currentIndex][0].setValue(5, -r1CrossU2.getZ());
|
||||
J_sp[currentIndex][0] = -frictionVectors[1].getX();
|
||||
J_sp[currentIndex][1] = -frictionVectors[1].getY();
|
||||
J_sp[currentIndex][2] = -frictionVectors[1].getZ();
|
||||
J_sp[currentIndex][3] = -r1CrossU2.getX();
|
||||
J_sp[currentIndex][4] = -r1CrossU2.getY();
|
||||
J_sp[currentIndex][5] = -r1CrossU2.getZ();
|
||||
|
||||
// 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][1].setValue(1, frictionVectors[1].getY());
|
||||
J_sp[currentIndex][1].setValue(2, frictionVectors[1].getZ());
|
||||
J_sp[currentIndex][1].setValue(3, r2CrossU2.getX());
|
||||
J_sp[currentIndex][1].setValue(4, r2CrossU2.getY());
|
||||
J_sp[currentIndex][1].setValue(5, r2CrossU2.getZ());
|
||||
J_sp[currentIndex][6] = frictionVectors[1].getX();
|
||||
J_sp[currentIndex][7] = frictionVectors[1].getY();
|
||||
J_sp[currentIndex][8] = frictionVectors[1].getZ();
|
||||
J_sp[currentIndex][9] = r2CrossU2.getX();
|
||||
J_sp[currentIndex][10] = r2CrossU2.getY();
|
||||
J_sp[currentIndex][11] = r2CrossU2.getZ();
|
||||
}
|
||||
|
||||
// Compute the lowerbounds values for all the mathematical constraints. The
|
||||
// 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"
|
||||
void Contact::computeLowerBound(int noConstraint, Vector& lowerBounds) const {
|
||||
assert(noConstraint >= 0 && noConstraint + nbConstraints <= lowerBounds.getNbComponent());
|
||||
void Contact::computeLowerBound(int noConstraint, double lowerBounds[NB_MAX_CONSTRAINTS]) const {
|
||||
assert(noConstraint >= 0 && noConstraint + nbConstraints <= NB_MAX_CONSTRAINTS);
|
||||
|
||||
lowerBounds.setValue(noConstraint, 0.0); // Lower bound for the contact constraint
|
||||
lowerBounds.setValue(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] = 0.0; // Lower bound for the contact constraint
|
||||
lowerBounds[noConstraint + 1] = -mu_mc_g; // Lower bound for the first 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
|
||||
// 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"
|
||||
void Contact::computeUpperBound(int noConstraint, Vector& upperBounds) const {
|
||||
assert(noConstraint >= 0 && noConstraint + nbConstraints <= upperBounds.getNbComponent());
|
||||
void Contact::computeUpperBound(int noConstraint, double upperBounds[NB_MAX_CONSTRAINTS]) const {
|
||||
assert(noConstraint >= 0 && noConstraint + nbConstraints <= NB_MAX_CONSTRAINTS);
|
||||
|
||||
upperBounds.setValue(noConstraint, INFINITY_CONST); // Upper bound for the contact constraint
|
||||
upperBounds.setValue(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] = INFINITY_CONST; // Upper bound for the contact constraint
|
||||
upperBounds[noConstraint + 1] = mu_mc_g; // Upper bound for the first 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
|
||||
// "errorValues" is the error values vector of the constraint solver and this
|
||||
// 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(body2);
|
||||
|
||||
|
|
@ -157,16 +157,16 @@ void Contact::computeErrorValue(int noConstraint, Vector& errorValues) const {
|
|||
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(body1);
|
||||
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
|
||||
Vector3 velocity1 = rigidBody1->getLinearVelocity();
|
||||
Vector3 velocity2 = rigidBody2->getLinearVelocity();
|
||||
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
|
||||
errorValues.setValue(noConstraint, errorValue); // Error value for contact constraint
|
||||
errorValues.setValue(noConstraint + 1, 0.0); // Error value for friction constraint
|
||||
errorValues.setValue(noConstraint + 2, 0.0); // Error value for friction constraint
|
||||
errorValues[noConstraint] = errorValue; // Error value for contact constraint
|
||||
errorValues[noConstraint + 1] = 0.0; // Error value for friction constraint
|
||||
errorValues[noConstraint + 2] = 0.0; // Error value for friction constraint
|
||||
}
|
||||
|
|
@ -75,18 +75,15 @@ class Contact : public Constraint {
|
|||
Vector3 getWorldPointOnBody1() const; // Return the contact world point on body 1
|
||||
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 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 computeLowerBound(int noConstraint, Vector& lowerBounds) 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 computeErrorValue(int noConstraint, Vector& errorValues) const; // Compute the error values for all the mathematical constraints
|
||||
double getPenetrationDepth() const; // Return the penetration depth
|
||||
void setWorldPointOnBody2(const Vector3& worldPoint); // Set the contact world point on body 2
|
||||
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, double lowerBounds[NB_MAX_CONSTRAINTS]) const; // Compute the lowerbounds 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, double errorValues[], double penetrationFactor) const; // Compute the error values for all the mathematical constraints
|
||||
double getPenetrationDepth() const; // Return the penetration depth
|
||||
#ifdef VISUAL_DEBUG
|
||||
void draw() const; // Draw the contact (for debugging)
|
||||
void draw() const; // Draw the contact (for debugging)
|
||||
#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
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/********************************************************************************
|
||||
* 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 *
|
||||
|
|
@ -30,14 +30,11 @@
|
|||
using namespace reactphysics3d;
|
||||
using namespace std;
|
||||
|
||||
// TODO : Maybe we could use std::vector instead of Vector to store vector in the constraint solver
|
||||
|
||||
// Constructor
|
||||
ConstraintSolver::ConstraintSolver(PhysicsWorld* world)
|
||||
:physicsWorld(world), bodyMapping(0), nbConstraints(0), constraintsCapacity(0),
|
||||
bodiesCapacity(0), avConstraintsCapacity(0), avBodiesCapacity(0), avBodiesNumber(0),
|
||||
avConstraintsNumber(0), avBodiesCounter(0), avConstraintsCounter(0),
|
||||
lcpSolver(new LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) {
|
||||
:physicsWorld(world), nbConstraints(0), nbIterationsLCP(MAX_LCP_ITERATIONS),
|
||||
penetrationFactor(10.0) {
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -77,106 +74,8 @@ void ConstraintSolver::initialize() {
|
|||
// Compute the number of bodies that are part of some active constraint
|
||||
nbBodies = bodyNumberMapping.size();
|
||||
|
||||
assert(nbConstraints > 0);
|
||||
assert(nbBodies > 0);
|
||||
|
||||
// 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;
|
||||
}
|
||||
assert(nbConstraints > 0 && nbConstraints <= NB_MAX_CONSTRAINTS);
|
||||
assert(nbBodies > 0 && nbBodies <= NB_MAX_BODIES);
|
||||
}
|
||||
|
||||
// Fill in all the matrices needed to solve the LCP problem
|
||||
|
|
@ -205,11 +104,11 @@ void ConstraintSolver::fillInMatrices() {
|
|||
constraint->computeUpperBound(noConstraint, upperBounds);
|
||||
|
||||
// Fill in the error vector
|
||||
constraint->computeErrorValue(noConstraint, errorValues);
|
||||
constraint->computeErrorValue(noConstraint, errorValues, penetrationFactor);
|
||||
|
||||
// Get the cached lambda values of the constraint
|
||||
for (int i=0; i<constraint->getNbConstraints(); i++) {
|
||||
lambdaInit.setValue(noConstraint + i, constraint->getCachedLambda(i));
|
||||
lambdaInit[noConstraint + i] = constraint->getCachedLambda(i);
|
||||
}
|
||||
|
||||
noConstraint += constraint->getNbConstraints();
|
||||
|
|
@ -230,76 +129,117 @@ void ConstraintSolver::fillInMatrices() {
|
|||
// Compute the vector V1 with initial velocities values
|
||||
Vector3 linearVelocity = rigidBody->getLinearVelocity();
|
||||
Vector3 angularVelocity = rigidBody->getAngularVelocity();
|
||||
V1[bodyNumber].setValue(0, linearVelocity[0]);
|
||||
V1[bodyNumber].setValue(1, linearVelocity[1]);
|
||||
V1[bodyNumber].setValue(2, linearVelocity[2]);
|
||||
V1[bodyNumber].setValue(3, angularVelocity[0]);
|
||||
V1[bodyNumber].setValue(4, angularVelocity[1]);
|
||||
V1[bodyNumber].setValue(5, angularVelocity[2]);
|
||||
int bodyIndexArray = 6 * bodyNumber;
|
||||
V1[bodyIndexArray] = linearVelocity[0];
|
||||
V1[bodyIndexArray + 1] = linearVelocity[1];
|
||||
V1[bodyIndexArray + 2] = linearVelocity[2];
|
||||
V1[bodyIndexArray + 3] = angularVelocity[0];
|
||||
V1[bodyIndexArray + 4] = angularVelocity[1];
|
||||
V1[bodyIndexArray + 5] = angularVelocity[2];
|
||||
|
||||
// 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
|
||||
Vector3 externalForce = rigidBody->getExternalForce();
|
||||
Vector3 externalTorque = rigidBody->getExternalTorque();
|
||||
Fext[bodyNumber].setValue(0, externalForce[0]);
|
||||
Fext[bodyNumber].setValue(1, externalForce[1]);
|
||||
Fext[bodyNumber].setValue(2, externalForce[2]);
|
||||
Fext[bodyNumber].setValue(3, externalTorque[0]);
|
||||
Fext[bodyNumber].setValue(4, externalTorque[1]);
|
||||
Fext[bodyNumber].setValue(5, externalTorque[2]);
|
||||
|
||||
// Compute the inverse sparse mass matrix
|
||||
Minv_sp[bodyNumber].initWithValue(0.0);
|
||||
const Matrix3x3& tensorInv = rigidBody->getInertiaTensorInverseWorld();
|
||||
if (rigidBody->getIsMotionEnabled()) {
|
||||
Minv_sp[bodyNumber].setValue(0, 0, rigidBody->getMassInverse());
|
||||
Minv_sp[bodyNumber].setValue(1, 1, 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));
|
||||
Fext[bodyIndexArray] = externalForce[0];
|
||||
Fext[bodyIndexArray + 1] = externalForce[1];
|
||||
Fext[bodyIndexArray + 2] = externalForce[2];
|
||||
Fext[bodyIndexArray + 3] = externalTorque[0];
|
||||
Fext[bodyIndexArray + 4] = externalTorque[1];
|
||||
Fext[bodyIndexArray + 5] = externalTorque[2];
|
||||
|
||||
// Initialize the mass and inertia tensor matrices
|
||||
Minv_sp_inertia[bodyNumber].setAllValues(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
|
||||
Minv_sp_mass_diag[bodyNumber] = 0.0;
|
||||
|
||||
// If the motion of the rigid body is enabled
|
||||
if (rigidBody->getIsMotionEnabled()) {
|
||||
Minv_sp_inertia[bodyNumber] = rigidBody->getInertiaTensorInverseWorld();
|
||||
Minv_sp_mass_diag[bodyNumber] = rigidBody->getMassInverse();
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// Compute the vector b
|
||||
void ConstraintSolver::computeVectorB(double dt) {
|
||||
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++) {
|
||||
b[c] = errorValues[c] * oneOverDT;
|
||||
|
||||
// Substract 1.0/dt*J*V to the vector b
|
||||
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
|
||||
indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
|
||||
b.setValue(c, b.getValue(c) - (J_sp[c][0] * V1[indexBody1]) * oneOverDT);
|
||||
b.setValue(c, b.getValue(c) - (J_sp[c][1] * V1[indexBody2]) * oneOverDT);
|
||||
double multiplication = 0.0;
|
||||
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
|
||||
b.setValue(c, b.getValue(c) - ((J_sp[c][0] * Minv_sp[indexBody1]) * Fext[indexBody1]
|
||||
+ (J_sp[c][1] * Minv_sp[indexBody2])*Fext[indexBody2]));
|
||||
double value1 = 0.0;
|
||||
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
|
||||
void ConstraintSolver::computeMatrixB_sp() {
|
||||
uint indexBody1, indexBody2;
|
||||
|
||||
uint indexConstraintArray, indexBody1, indexBody2;
|
||||
|
||||
// For each constraint
|
||||
for (uint c = 0; c<nbConstraints; c++) {
|
||||
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
|
||||
|
||||
indexConstraintArray = 6 * c;
|
||||
indexBody1 = bodyNumberMapping[bodyMapping[c][0]];
|
||||
indexBody2 = bodyNumberMapping[bodyMapping[c][1]];
|
||||
B_sp[0][c] = Minv_sp[indexBody1] * J_sp[c][0].getTranspose();
|
||||
B_sp[1][c] = Minv_sp[indexBody2] * J_sp[c][1].getTranspose();
|
||||
B_sp[0][indexConstraintArray] = Minv_sp_mass_diag[indexBody1] * J_sp[c][0];
|
||||
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,19 +248,94 @@ void ConstraintSolver::computeMatrixB_sp() {
|
|||
// V_constraint = dt * (M^-1 * J^T * lambda)
|
||||
// Note that we use the vector V to store both V1 and V_constraint.
|
||||
// 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) {
|
||||
uint indexBody1, indexBody2;
|
||||
|
||||
uint indexBody1, indexBody2, indexBody1Array, indexBody2Array, indexConstraintArray;
|
||||
uint j;
|
||||
|
||||
// Compute dt * (M^-1 * J^T * lambda
|
||||
for (uint i=0; i<nbConstraints; i++) {
|
||||
indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
|
||||
indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
|
||||
Vconstraint[indexBody1] = Vconstraint[indexBody1] + (B_sp[0][i] * lambda.getValue(i)) * dt;
|
||||
Vconstraint[indexBody2] = Vconstraint[indexBody2] + (B_sp[1][i] * lambda.getValue(i)) * dt;
|
||||
indexBody1Array = 6 * bodyNumberMapping[bodyMapping[i][0]];
|
||||
indexBody2Array = 6 * bodyNumberMapping[bodyMapping[i][1]];
|
||||
indexConstraintArray = 6 * i;
|
||||
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
|
||||
// to initialize the lambda vector
|
||||
void ConstraintSolver::cacheLambda() {
|
||||
|
|
@ -332,7 +347,7 @@ void ConstraintSolver::cacheLambda() {
|
|||
// For each constraint of the contact
|
||||
for (int i=0; i<activeConstraints[c]->getNbConstraints(); i++) {
|
||||
// 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();
|
||||
|
|
|
|||
|
|
@ -37,70 +37,72 @@
|
|||
|
||||
// ReactPhysics3D namespace
|
||||
namespace reactphysics3d {
|
||||
|
||||
/* -------------------------------------------------------------------
|
||||
|
||||
|
||||
/* -------------------------------------------------------------------
|
||||
Class ConstrainSolver :
|
||||
This class represents the constraint solver. The goal is to
|
||||
solve A constraint-base LCP problem.
|
||||
This class represents the constraint solver. The constraint solver
|
||||
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 {
|
||||
protected:
|
||||
PhysicsWorld* physicsWorld; // Reference to the physics world
|
||||
LCPSolver* lcpSolver; // LCP Solver
|
||||
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world
|
||||
uint nbConstraints; // Total number of constraints (with the auxiliary constraints)
|
||||
uint nbBodies; // Current number of bodies in the physics world
|
||||
uint constraintsCapacity; // Number of constraints that are currently allocated in memory in the solver
|
||||
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::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
|
||||
Matrix1x6** J_sp; // 2-dimensional array thar 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
|
||||
// a 1x6 matrix
|
||||
Vector6D** B_sp; // 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
|
||||
// a 6x1 matrix
|
||||
Vector b; // Vector "b" of the LCP problem
|
||||
Vector lambda; // Lambda vector of the LCP problem
|
||||
Vector lambdaInit; // Lambda init vector for the LCP solver
|
||||
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
|
||||
Matrix6x6* Minv_sp; // Sparse representation of the Matrix that contains information about mass and inertia of each body
|
||||
// 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
|
||||
// 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
|
||||
Vector6D* Fext; // Array that contains for each body the vector that contains external forces and torques
|
||||
// Each cell contains a 6x1 vector with external force and torque.
|
||||
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 computeVectorB(double dt); // Compute the vector b
|
||||
void computeMatrixB_sp(); // Compute the matrix B_sp
|
||||
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 freeMemory(bool freeBodiesMemory); // Free some memory previously allocated for the constraint solver
|
||||
private:
|
||||
PhysicsWorld* physicsWorld; // Reference to 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 nbBodies; // Current number of bodies in the physics world
|
||||
double penetrationFactor; // Penetration factor "beta" for penetration correction
|
||||
std::set<Body*> constraintBodies; // Bodies that are implied in some constraint
|
||||
std::map<Body*, uint> bodyNumberMapping; // Map a body pointer with its index number
|
||||
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
|
||||
// the pointer to the body that correspond to the 1x6 J_ij matrix in the
|
||||
// J_sp matrix. An integer body index refers to its index in the "bodies" std::vector
|
||||
double J_sp[NB_MAX_CONSTRAINTS][2*6]; // 2-dimensional array that correspond to the sparse representation of the jacobian matrix of all constraints
|
||||
// This array contains for each constraint two 1x6 Jacobian matrices (one for each body of the constraint)
|
||||
// a 1x6 matrix
|
||||
double B_sp[2][6*NB_MAX_CONSTRAINTS]; // 2-dimensional array that correspond to a useful matrix in sparse representation
|
||||
// This array contains for each constraint two 6x1 matrices (one for each body of the constraint)
|
||||
// a 6x1 matrix
|
||||
double b[NB_MAX_CONSTRAINTS]; // Vector "b" of the LCP problem
|
||||
double d[NB_MAX_CONSTRAINTS]; // Vector "d"
|
||||
double a[6*NB_MAX_BODIES]; // Vector "a"
|
||||
double lambda[NB_MAX_CONSTRAINTS]; // Lambda vector of the LCP problem
|
||||
double lambdaInit[NB_MAX_CONSTRAINTS]; // Lambda init vector for the LCP solver
|
||||
double errorValues[NB_MAX_CONSTRAINTS]; // Error vector of all constraints
|
||||
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
|
||||
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
|
||||
double Vconstraint[6*NB_MAX_BODIES]; // Same kind of vector as V1 but contains the final constraint velocities
|
||||
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.
|
||||
void initialize(); // Initialize the constraint solver before each solving
|
||||
void fillInMatrices(); // Fill in all the matrices needed to solve the LCP problem
|
||||
void computeVectorB(double dt); // Compute the vector b
|
||||
void computeMatrixB_sp(); // Compute the matrix B_sp
|
||||
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 computeVectorA(); // Compute the vector a used in the solve() method
|
||||
void solveLCP(); // Solve a LCP problem using Projected-Gauss-Seidel algorithm
|
||||
|
||||
public:
|
||||
ConstraintSolver(PhysicsWorld* world); // Constructor
|
||||
virtual ~ConstraintSolver(); // Destructor
|
||||
void solve(double dt); // Solve the current LCP problem
|
||||
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 getConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem
|
||||
void cleanup();
|
||||
ConstraintSolver(PhysicsWorld* world); // Constructor
|
||||
virtual ~ConstraintSolver(); // Destructor
|
||||
void solve(double dt); // Solve the current LCP problem
|
||||
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 getConstrainedAngularVelocityOfBody(Body* body); // Return the constrained angular velocity of a body after solving the LCP problem
|
||||
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
|
||||
|
|
@ -114,16 +116,16 @@ inline bool ConstraintSolver::isConstrainedBody(Body* body) const {
|
|||
// Return the constrained linear velocity of a body after solving the LCP problem
|
||||
inline Vector3 ConstraintSolver::getConstrainedLinearVelocityOfBody(Body* body) {
|
||||
assert(isConstrainedBody(body));
|
||||
const Vector6D& vec = Vconstraint[bodyNumberMapping[body]];
|
||||
return Vector3(vec.getValue(0), vec.getValue(1), vec.getValue(2));
|
||||
|
||||
uint indexBodyArray = 6 * bodyNumberMapping[body];
|
||||
return Vector3(Vconstraint[indexBodyArray], Vconstraint[indexBodyArray + 1], Vconstraint[indexBodyArray + 2]);
|
||||
}
|
||||
|
||||
|
||||
// Return the constrained angular velocity of a body after solving the LCP problem
|
||||
inline Vector3 ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body) {
|
||||
assert(isConstrainedBody(body));
|
||||
const Vector6D& vec = Vconstraint[bodyNumberMapping[body]];
|
||||
return Vector3(vec.getValue(3), vec.getValue(4), vec.getValue(5));
|
||||
uint indexBodyArray = 6 * bodyNumberMapping[body];
|
||||
return Vector3(Vconstraint[indexBodyArray + 3], Vconstraint[indexBodyArray + 4], Vconstraint[indexBodyArray + 5]);
|
||||
}
|
||||
|
||||
// Cleanup of the constraint solver
|
||||
|
|
@ -133,16 +135,25 @@ inline void ConstraintSolver::cleanup() {
|
|||
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
|
||||
inline void ConstraintSolver::solve(double dt) {
|
||||
|
||||
|
||||
// TODO : Remove the following timing code
|
||||
timeval timeValueStart;
|
||||
timeval timeValueEnd;
|
||||
std::cout << "------ START (Constraint Solver) -----" << std::endl;
|
||||
gettimeofday(&timeValueStart, NULL);
|
||||
|
||||
|
||||
// Allocate memory for the matrices
|
||||
initialize();
|
||||
|
||||
|
|
@ -156,8 +167,7 @@ inline void ConstraintSolver::solve(double dt) {
|
|||
computeMatrixB_sp();
|
||||
|
||||
// Solve the LCP problem (computation of lambda)
|
||||
lcpSolver->setLambdaInit(lambdaInit);
|
||||
lcpSolver->solve(J_sp, B_sp, nbConstraints, nbBodies, bodyMapping, bodyNumberMapping, b, lowerBounds, upperBounds, lambda);
|
||||
solveLCP();
|
||||
|
||||
// Cache the lambda values in order to use them in the next step
|
||||
cacheLambda();
|
||||
|
|
@ -171,7 +181,6 @@ inline void ConstraintSolver::solve(double dt) {
|
|||
long double startTime = timeValueStart.tv_sec * 1000000.0 + (timeValueStart.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;
|
||||
|
||||
}
|
||||
|
||||
} // End of ReactPhysics3D namespace
|
||||
|
|
|
|||
|
|
@ -61,6 +61,8 @@ public :
|
|||
void start(); // Start the physics simulation
|
||||
void stop(); // Stop 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 --- //
|
||||
|
|
@ -72,7 +74,17 @@ inline void PhysicsEngine::start() {
|
|||
|
||||
inline void PhysicsEngine::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);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -47,7 +47,7 @@ namespace reactphysics3d {
|
|||
*/
|
||||
class PhysicsWorld {
|
||||
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*> removedBodies; // Removed bodies since last update
|
||||
std::vector<Constraint*> constraints; // List that contains all the current constraints
|
||||
|
|
@ -56,11 +56,11 @@ class PhysicsWorld {
|
|||
long unsigned int currentBodyID; // Current body ID
|
||||
|
||||
void addBody(Body* body); // Add a body to the physics world
|
||||
void removeBody(Body* body); // Remove a body from the physics world
|
||||
void removeBody(Body* body); // Remove a body from the physics world
|
||||
|
||||
public :
|
||||
PhysicsWorld(const Vector3& gravity); // Constructor
|
||||
virtual ~PhysicsWorld(); // Destructor
|
||||
virtual ~PhysicsWorld(); // Destructor
|
||||
|
||||
RigidBody* createRigidBody(const Transform& transform, double mass,
|
||||
const Matrix3x3& inertiaTensorLocal, Shape* shape); // Create a rigid body into the physics world
|
||||
|
|
|
|||
|
|
@ -42,11 +42,14 @@ LCPProjectedGaussSeidel::~LCPProjectedGaussSeidel() {
|
|||
|
||||
// Solve a LCP problem using the Projected-Gauss-Seidel algorithm
|
||||
// This method outputs the result in the lambda vector
|
||||
void LCPProjectedGaussSeidel::solve(Matrix1x6** J_sp, Vector6D** B_sp, uint nbConstraints,
|
||||
uint nbBodies, Body*** const bodyMapping, map<Body*, uint> bodyNumberMapping,
|
||||
const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const {
|
||||
void LCPProjectedGaussSeidel::solve(double J_sp[NB_MAX_CONSTRAINTS][2*6], double B_sp[2][6*NB_MAX_CONSTRAINTS], uint nbConstraints,
|
||||
uint nbBodies, Body* bodyMapping[NB_MAX_CONSTRAINTS][2], map<Body*, uint> bodyNumberMapping,
|
||||
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)
|
||||
uint indexBody1, indexBody2;
|
||||
|
|
@ -55,27 +58,42 @@ void LCPProjectedGaussSeidel::solve(Matrix1x6** J_sp, Vector6D** B_sp, uint nbCo
|
|||
uint i, iter;
|
||||
Vector6D* a = new Vector6D[nbBodies]; // Array that contains nbBodies vector of dimension 6x1
|
||||
|
||||
|
||||
// Compute the vector a
|
||||
computeVectorA(lambda, nbConstraints, bodyMapping, B_sp, bodyNumberMapping, a, nbBodies);
|
||||
|
||||
|
||||
// For each constraint
|
||||
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 (i=0; i<nbConstraints; i++) {
|
||||
indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
|
||||
indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
|
||||
deltaLambda = (b.getValue(i) - (J_sp[i][0] * a[indexBody1]) - (J_sp[i][1] * a[indexBody2])) / d[i];
|
||||
lambdaTemp = lambda.getValue(i);
|
||||
lambda.setValue(i, std::max(lowLimits.getValue(i), std::min(lambda.getValue(i) + deltaLambda, highLimits.getValue(i))));
|
||||
deltaLambda = lambda.getValue(i) - lambdaTemp;
|
||||
a[indexBody1] = a[indexBody1] + (B_sp[0][i] * deltaLambda);
|
||||
a[indexBody2] = a[indexBody2] + (B_sp[1][i] * deltaLambda);
|
||||
uint indexConstraintArray = 6 * i;
|
||||
deltaLambda = b[i];
|
||||
for (uint j=0; j<6; j++) {
|
||||
deltaLambda -= (J_sp[i][j] * a[indexBody1].getValue(j) + J_sp[i][6 + j] * a[indexBody2].getValue(j));
|
||||
}
|
||||
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
|
||||
delete[] d;
|
||||
delete[] a;
|
||||
|
|
@ -83,8 +101,8 @@ void LCPProjectedGaussSeidel::solve(Matrix1x6** J_sp, Vector6D** B_sp, uint nbCo
|
|||
|
||||
// Compute the vector a used in the solve() method
|
||||
// Note that a = B * lambda
|
||||
void LCPProjectedGaussSeidel::computeVectorA(const Vector& lambda, uint nbConstraints, Body*** const bodyMapping,
|
||||
Vector6D** B_sp, map<Body*, uint> bodyNumberMapping,
|
||||
void LCPProjectedGaussSeidel::computeVectorA(double lambda[NB_MAX_CONSTRAINTS], uint nbConstraints, Body* bodyMapping[NB_MAX_CONSTRAINTS][2],
|
||||
double B_sp[2][6*NB_MAX_CONSTRAINTS], map<Body*, uint> bodyNumberMapping,
|
||||
Vector6D* const a, uint nbBodies) const {
|
||||
uint i;
|
||||
uint indexBody1, indexBody2;
|
||||
|
|
@ -94,11 +112,14 @@ void LCPProjectedGaussSeidel::computeVectorA(const Vector& lambda, uint nbConstr
|
|||
a[i].initWithValue(0.0);
|
||||
}
|
||||
|
||||
|
||||
for(i=0; i<nbConstraints; i++) {
|
||||
indexBody1 = bodyNumberMapping[bodyMapping[i][0]];
|
||||
indexBody2 = bodyNumberMapping[bodyMapping[i][1]];
|
||||
a[indexBody1] = a[indexBody1] + (B_sp[0][i] * lambda.getValue(i));
|
||||
a[indexBody2] = a[indexBody2] + (B_sp[1][i] * lambda.getValue(i));
|
||||
uint indexConstraintArray = 6 * 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]));
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -41,16 +41,16 @@ namespace reactphysics3d {
|
|||
class LCPProjectedGaussSeidel : public LCPSolver {
|
||||
protected:
|
||||
|
||||
void computeVectorA(const Vector& lambda, uint nbConstraints, Body*** const bodyMapping,
|
||||
Vector6D** B_sp, std::map<Body*, uint> bodyNumberMapping,
|
||||
void computeVectorA(double lambda[NB_MAX_CONSTRAINTS], uint nbConstraints, Body* bodyMapping[NB_MAX_CONSTRAINTS][2],
|
||||
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
|
||||
|
||||
public:
|
||||
LCPProjectedGaussSeidel(uint maxIterations); // Constructor
|
||||
virtual ~LCPProjectedGaussSeidel(); // Destructor
|
||||
virtual void solve(Matrix1x6** J_sp, Vector6D** B_sp, uint nbConstraints,
|
||||
uint nbBodies, Body*** const bodyMapping, 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
|
||||
virtual void solve(double J_sp[NB_MAX_CONSTRAINTS][2*6], double B_sp[2][6*NB_MAX_CONSTRAINTS], uint nbConstraints,
|
||||
uint nbBodies, Body* bodyMapping[NB_MAX_CONSTRAINTS][2], std::map<Body*, uint> bodyNumberMapping,
|
||||
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
|
||||
|
|
|
|||
|
|
@ -59,23 +59,24 @@ namespace reactphysics3d {
|
|||
*/
|
||||
class LCPSolver {
|
||||
protected:
|
||||
uint maxIterations; // Maximum number of iterations
|
||||
Vector lambdaInit; // Initial value for lambda at the beginning of the algorithm
|
||||
uint maxIterations; // Maximum number of iterations
|
||||
double lambdaInit[NB_MAX_CONSTRAINTS]; // Initial value for lambda at the beginning of the algorithm
|
||||
|
||||
public:
|
||||
LCPSolver(uint maxIterations); // Constructor
|
||||
virtual ~LCPSolver(); // Destructor
|
||||
virtual void solve(Matrix1x6** J_sp, Vector6D** B_sp, uint nbConstraints,
|
||||
uint nbBodies, Body*** const bodyMapping, std::map<Body*, uint> bodyNumberMapping,
|
||||
const Vector& b, const Vector& lowLimits, const Vector& highLimits, Vector& lambda) const=0; // Solve a LCP problem
|
||||
void setLambdaInit(const Vector& lambdaInit); // Set the initial lambda vector
|
||||
virtual void solve(double J_sp[NB_MAX_CONSTRAINTS][2*6], double B_sp[2][6*NB_MAX_CONSTRAINTS], uint nbConstraints,
|
||||
uint nbBodies, Body* bodyMapping[NB_MAX_CONSTRAINTS][2], std::map<Body*, uint> bodyNumberMapping,
|
||||
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(double lambdaInit[NB_MAX_CONSTRAINTS], uint nbConstraints); // Set the initial lambda vector
|
||||
void setMaxIterations(uint maxIterations); // Set the maximum number of iterations
|
||||
};
|
||||
|
||||
// Set the initial lambda vector
|
||||
inline void LCPSolver::setLambdaInit(const Vector& lambdaInit) {
|
||||
this->lambdaInit.changeSize(lambdaInit.getNbComponent());
|
||||
this->lambdaInit = lambdaInit;
|
||||
inline void LCPSolver::setLambdaInit(double lambdaInit[NB_MAX_CONSTRAINTS], uint nbConstraints) {
|
||||
for (uint i=0; i<nbConstraints; i++) {
|
||||
this->lambdaInit[i] = lambdaInit[i];
|
||||
}
|
||||
}
|
||||
|
||||
// Set the maximum number of iterations
|
||||
|
|
|
|||
|
|
@ -32,6 +32,9 @@
|
|||
#include <cassert>
|
||||
#include <new>
|
||||
|
||||
// TODO : Check that casting is done correctly in this class using
|
||||
// C++ cast operator like reinterpret_cast<>, ...
|
||||
|
||||
// ReactPhysics3D namespace
|
||||
namespace reactphysics3d {
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user