Some optimizations in the constraint solver have been made
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@386 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel
parent
b9ddb0e2f1
commit
f61a9eeff7
|
|
@ -25,8 +25,7 @@ using namespace reactphysics3d;
|
|||
|
||||
// Constructor
|
||||
Constraint::Constraint(Body* const body1, Body* const body2, uint nbAuxConstraints, bool active)
|
||||
:body1(body1), body2(body2), active(active), nbAuxConstraints(nbAuxConstraints),
|
||||
lowerBound(lowerBound), upperBound(upperBound) {
|
||||
:body1(body1), body2(body2), active(active), nbAuxConstraints(nbAuxConstraints) {
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -44,38 +44,26 @@ namespace reactphysics3d {
|
|||
*/
|
||||
class Constraint {
|
||||
protected :
|
||||
Body* const body1; // Pointer to the first body of the constraint
|
||||
Body* const body2; // Pointer to the second body of the constraint
|
||||
bool active; // True if the constraint is active
|
||||
Matrix body1Jacobian; // Jacobian matrix of the constraint for body1 (dimension 1x6)
|
||||
Matrix body2Jacobian; // Jacobian matrix of the constraint for body2 (dimension 1x6)
|
||||
Matrix auxJacobian; // Jacobian matrix for all the auxiliary constraints jacobian associated with this constraint
|
||||
// (dimension nx12 where n is the number of auxiliary constraints)
|
||||
uint nbAuxConstraints; // Number of auxiliary constraints associated with this constraint
|
||||
double lowerBound; // Lower bound of the constraint
|
||||
double upperBound; // Upper bound of the constraint
|
||||
Vector auxLowerBounds; // Vector that contains all the lower bounds of the auxiliary constraints
|
||||
Vector auxUpperBounds; // Vector that contains all the upper bounds of the auxiliary constraints
|
||||
double errorValue; // Error value (bias) of the constraint
|
||||
Vector auxErrorValues; // Error values for the auxiliary constraints
|
||||
Body* const body1; // Pointer to the first body of the constraint
|
||||
Body* const body2; // Pointer to the second body of the constraint
|
||||
bool active; // True if the constraint is active
|
||||
uint nbAuxConstraints; // Number of auxiliary constraints associated with this constraint
|
||||
|
||||
public :
|
||||
Constraint(Body* const body1, Body* const body2, uint nbAuxConstraints, bool active); // Constructor // Constructor
|
||||
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
|
||||
virtual void evaluate()=0; // Evaluate the constraint
|
||||
bool isActive() const; // Return true if the constraint is active
|
||||
const Matrix& getBody1Jacobian() const; // Return the jacobian matrix of body 1
|
||||
const Matrix& getBody2Jacobian() const; // Return the jacobian matrix of body 2
|
||||
unsigned int getNbAuxConstraints() const; // Return the number of auxiliary constraints
|
||||
const Matrix& getAuxJacobian() const; // Return the jacobian matrix of auxiliary constraints
|
||||
double getLowerBound() const; // Return the lower bound value of the constraint
|
||||
double getUpperBound() const; // Return the upper bound value of the constraint
|
||||
const Vector& getAuxLowerBounds() const; // Return the vector of lower bounds values
|
||||
const Vector& getAuxUpperBounds() const; // Return the vector of the upper bounds values
|
||||
double getErrorValue() const; // Return the error value (bias) of the constraint
|
||||
const Vector& getAuxErrorValues() const; // Return the auxiliary error values
|
||||
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 noBody, Matrix& jacobian) const=0; // Compute a part of the jacobian for a given body
|
||||
virtual void computeAuxJacobian(int noBody, int noAuxConstraint, Matrix& auxJacobian) const=0; // Compute a part of the jacobian for an auxiliary constraint
|
||||
virtual double computeLowerBound() const=0; // Compute the lowerbound of the constraint
|
||||
virtual double computeUpperBound() const=0; // Compute the upperbound of the constraint
|
||||
virtual void computeAuxLowerBounds(int beginIndex, Vector& auxLowerBounds) const=0; // Compute lowerbounds for the auxiliary constraints
|
||||
virtual void computeAuxUpperBounds(int beginIndex, Vector& auxUpperBounds) const=0; // Compute upperbounds for the auxiliary constraints
|
||||
virtual double computeErrorValue() const=0; // Compute the error value for the constraint
|
||||
virtual void computeAuxErrorValues(int beginIndex, Vector& errorValues) const=0; // Compute the errors values of the auxiliary constraints
|
||||
unsigned int getNbAuxConstraints() const; // Return the number of auxiliary constraints // Return the number of auxiliary constraints
|
||||
};
|
||||
|
||||
// Return the reference to the body 1
|
||||
|
|
@ -93,56 +81,11 @@ inline bool Constraint::isActive() const {
|
|||
return active;
|
||||
}
|
||||
|
||||
// Return the jacobian matrix of body 1
|
||||
inline const Matrix& Constraint::getBody1Jacobian() const {
|
||||
return body1Jacobian;
|
||||
}
|
||||
|
||||
// Return the jacobian matrix of body 2
|
||||
inline const Matrix& Constraint::getBody2Jacobian() const {
|
||||
return body2Jacobian;
|
||||
}
|
||||
|
||||
// Return the number auxiliary constraints
|
||||
inline uint Constraint::getNbAuxConstraints() const {
|
||||
return nbAuxConstraints;
|
||||
}
|
||||
|
||||
// Return the auxiliary jacobian matrix
|
||||
inline const Matrix& Constraint::getAuxJacobian() const {
|
||||
return auxJacobian;
|
||||
}
|
||||
|
||||
// Return the lower bound value of the constraint
|
||||
inline double Constraint::getLowerBound() const {
|
||||
return lowerBound;
|
||||
}
|
||||
|
||||
// Return the upper bound value of the constraint
|
||||
inline double Constraint::getUpperBound() const {
|
||||
return upperBound;
|
||||
}
|
||||
|
||||
// Return the vector of lower bounds values
|
||||
inline const Vector& Constraint::getAuxLowerBounds() const {
|
||||
return auxLowerBounds;
|
||||
}
|
||||
|
||||
// Return the vector of the upper bounds values
|
||||
inline const Vector& Constraint::getAuxUpperBounds() const {
|
||||
return auxUpperBounds;
|
||||
}
|
||||
|
||||
// Return the error value (bias) of the constraint
|
||||
inline double Constraint::getErrorValue() const {
|
||||
return errorValue;
|
||||
}
|
||||
|
||||
// Return the auxiliary error values
|
||||
inline const Vector& Constraint::getAuxErrorValues() const {
|
||||
return auxErrorValues;
|
||||
}
|
||||
|
||||
} // End of the ReactPhysics3D namespace
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -19,9 +19,6 @@
|
|||
|
||||
// Libraries
|
||||
#include "Contact.h"
|
||||
#include "../body/RigidBody.h"
|
||||
#include <GL/freeglut.h> // TODO : Remove this in the final version
|
||||
#include <GL/gl.h> // TODO : Remove this in the final version
|
||||
|
||||
// We want to use the ReactPhysics3D namespace
|
||||
using namespace reactphysics3d;
|
||||
|
|
@ -29,115 +26,17 @@ using namespace reactphysics3d;
|
|||
// Constructor
|
||||
Contact::Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const Vector3D& point)
|
||||
:Constraint(body1, body2, 2, true), normal(normal), penetrationDepth(penetrationDepth), point(point) {
|
||||
body1Jacobian.changeSize(1,6);
|
||||
body2Jacobian.changeSize(1,6);
|
||||
auxJacobian.changeSize(nbAuxConstraints, 12);
|
||||
auxLowerBounds.changeSize(nbAuxConstraints);
|
||||
auxUpperBounds.changeSize(nbAuxConstraints);
|
||||
auxErrorValues.changeSize(nbAuxConstraints);
|
||||
|
||||
body1Jacobian = Matrix(1, 6);
|
||||
body2Jacobian = Matrix(1, 6);
|
||||
auxJacobian = Matrix(nbAuxConstraints, 12);
|
||||
auxLowerBounds = Vector(nbAuxConstraints);
|
||||
auxUpperBounds = Vector(nbAuxConstraints);
|
||||
auxErrorValues = Vector(nbAuxConstraints);
|
||||
// Compute the auxiliary lower and upper bounds
|
||||
// TODO : Now mC is only the mass of the first body but it is probably wrong
|
||||
// TODO : Now g is 9.81 but we should use the true gravity value of the physics world.
|
||||
mu_mc_g = FRICTION_COEFFICIENT * body1->getMass() * 9.81;
|
||||
|
||||
// Compute the friction vectors that span the tangential friction plane
|
||||
computeFrictionVectors();
|
||||
}
|
||||
|
||||
// Destructor
|
||||
Contact::~Contact() {
|
||||
|
||||
}
|
||||
|
||||
// Evaluate the constraint
|
||||
// This method computes the jacobian matrices of the constraint
|
||||
void Contact::evaluate() {
|
||||
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(body1);
|
||||
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(body2);
|
||||
|
||||
assert(rigidBody1 != 0);
|
||||
assert(rigidBody2 != 0);
|
||||
|
||||
// Compute the friction vectors that span the tangential friction plane
|
||||
computeFrictionVectors();
|
||||
|
||||
Vector3D r1 = point - rigidBody1->getPosition();
|
||||
Vector3D r2 = point - rigidBody2->getPosition();
|
||||
Vector3D r1CrossN = r1.crossProduct(normal);
|
||||
Vector3D r2CrossN = r2.crossProduct(normal);
|
||||
|
||||
// Compute the jacobian matrix for the body 1
|
||||
body1Jacobian.setValue(0, 0, -normal.getX());
|
||||
body1Jacobian.setValue(0, 1, -normal.getY());
|
||||
body1Jacobian.setValue(0, 2, -normal.getZ());
|
||||
body1Jacobian.setValue(0, 3, -r1CrossN.getX());
|
||||
body1Jacobian.setValue(0, 4, -r1CrossN.getY());
|
||||
body1Jacobian.setValue(0, 5, -r1CrossN.getZ());
|
||||
|
||||
// Compute the jacobian matrix for the body 2
|
||||
body2Jacobian.setValue(0, 0, normal.getX());
|
||||
body2Jacobian.setValue(0, 1, normal.getY());
|
||||
body2Jacobian.setValue(0, 2, normal.getZ());
|
||||
body2Jacobian.setValue(0, 3, r2CrossN.getX());
|
||||
body2Jacobian.setValue(0, 4, r2CrossN.getY());
|
||||
body2Jacobian.setValue(0, 5, r2CrossN.getZ());
|
||||
|
||||
// Compute the lower and upper bounds values
|
||||
lowerBound = 0.0;
|
||||
upperBound = INFINITY_CONST;
|
||||
|
||||
// Compute the error value of the constraint
|
||||
Vector3D velocity1 = rigidBody1->getLinearVelocity();
|
||||
Vector3D velocity2 = rigidBody2->getLinearVelocity();
|
||||
double restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution();
|
||||
errorValue = restitutionCoeff * (normal.scalarProduct(velocity1) - normal.scalarProduct(velocity2)) + PENETRATION_FACTOR * penetrationDepth;
|
||||
|
||||
// Compute the auxiliary jacobian matrix (this corresponds to the friction constraint)
|
||||
Vector3D r1CrossU1 = r1.crossProduct(frictionVectors[0]);
|
||||
Vector3D r2CrossU1 = r2.crossProduct(frictionVectors[0]);
|
||||
Vector3D r1CrossU2 = r1.crossProduct(frictionVectors[1]);
|
||||
Vector3D r2CrossU2 = r2.crossProduct(frictionVectors[1]);
|
||||
auxJacobian.setValue(0, 0, -frictionVectors[0].getX());
|
||||
auxJacobian.setValue(0, 1, -frictionVectors[0].getY());
|
||||
auxJacobian.setValue(0, 2, -frictionVectors[0].getZ());
|
||||
auxJacobian.setValue(0, 3, -r1CrossU1.getX());
|
||||
auxJacobian.setValue(0, 4, -r1CrossU1.getY());
|
||||
auxJacobian.setValue(0, 5, -r1CrossU1.getZ());
|
||||
auxJacobian.setValue(0, 6, frictionVectors[0].getX());
|
||||
auxJacobian.setValue(0, 7, frictionVectors[0].getY());
|
||||
auxJacobian.setValue(0, 8, frictionVectors[0].getZ());
|
||||
auxJacobian.setValue(0, 9, r2CrossU1.getX());
|
||||
auxJacobian.setValue(0, 10, r2CrossU1.getY());
|
||||
auxJacobian.setValue(0, 11, r2CrossU1.getZ());
|
||||
auxJacobian.setValue(1, 0, -frictionVectors[1].getX());
|
||||
auxJacobian.setValue(1, 1, -frictionVectors[1].getY());
|
||||
auxJacobian.setValue(1, 2, -frictionVectors[1].getZ());
|
||||
auxJacobian.setValue(1, 3, -r1CrossU2.getX());
|
||||
auxJacobian.setValue(1, 4, -r1CrossU2.getY());
|
||||
auxJacobian.setValue(1, 5, -r1CrossU2.getZ());
|
||||
auxJacobian.setValue(1, 6, frictionVectors[1].getX());
|
||||
auxJacobian.setValue(1, 7, frictionVectors[1].getY());
|
||||
auxJacobian.setValue(1, 8, frictionVectors[1].getZ());
|
||||
auxJacobian.setValue(1, 9, r2CrossU2.getX());
|
||||
auxJacobian.setValue(1, 10, r2CrossU2.getY());
|
||||
auxJacobian.setValue(1, 11, r2CrossU2.getZ());
|
||||
|
||||
// Compute the auxiliary lower and upper bounds
|
||||
// TODO : Now mC is only the mass of the first body but it is probably wrong
|
||||
// TODO : Now g is 9.81 but we should use the true gravity value of the physics world.
|
||||
double mu_mc_g = FRICTION_COEFFICIENT * rigidBody1->getMass() * 9.81;
|
||||
auxLowerBounds.setValue(0, -mu_mc_g);
|
||||
auxLowerBounds.setValue(1, -mu_mc_g);
|
||||
auxUpperBounds.setValue(0, mu_mc_g);
|
||||
auxUpperBounds.setValue(1, mu_mc_g);
|
||||
|
||||
// Compute the error auxiliary values
|
||||
auxErrorValues.setValue(0, 0.0);
|
||||
auxErrorValues.setValue(1, 0.0);
|
||||
;}
|
||||
|
||||
// TODO : Delete this (Used to debug collision detection)
|
||||
void Contact::draw() const {
|
||||
glColor3f(1.0, 0.0, 0.0);
|
||||
glutSolidSphere(0.5, 20, 20);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -23,7 +23,10 @@
|
|||
// Libraries
|
||||
#include "../typeDefinitions.h"
|
||||
#include "Constraint.h"
|
||||
#include "../body/RigidBody.h"
|
||||
#include "../mathematics/mathematics.h"
|
||||
#include <GL/freeglut.h> // TODO : Remove this in the final version
|
||||
#include <GL/gl.h> // TODO : Remove this in the final version
|
||||
|
||||
// ReactPhysics3D namespace
|
||||
namespace reactphysics3d {
|
||||
|
|
@ -50,20 +53,27 @@ class Contact : public Constraint {
|
|||
const double penetrationDepth; // Penetration depth
|
||||
const Vector3D point; // Contact point
|
||||
std::vector<Vector3D> frictionVectors; // Two orthogonal vectors that span the tangential friction plane
|
||||
|
||||
double mu_mc_g;
|
||||
|
||||
void computeFrictionVectors(); // Compute the two friction vectors that span the tangential friction plane
|
||||
|
||||
public :
|
||||
Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const Vector3D& point); // Constructor
|
||||
virtual ~Contact(); // Destructor
|
||||
|
||||
Vector3D getNormal() const; // Return the normal vector of the contact
|
||||
Vector3D getPoint() const; // Return the contact point
|
||||
virtual void evaluate(); // Evaluate the constraint
|
||||
uint getNbAuxConstraints() const; // Return the number of auxiliary constraints
|
||||
|
||||
double getPenetrationDepth() const; // Return the penetration depth
|
||||
void draw() const; // TODO : Delete this (Used to debug collision detection)
|
||||
Vector3D getNormal() const; // Return the normal vector of the contact
|
||||
Vector3D getPoint() const; // Return the contact point
|
||||
virtual void computeJacobian(int noBody, Matrix& jacobian) const; // Compute a part of the jacobian for a given body
|
||||
virtual void computeAuxJacobian(int noBody, int noAuxConstraint, Matrix& auxJacobian) const; // Compute a part of the jacobian for an auxiliary constraint
|
||||
virtual double computeLowerBound() const; // Compute the lowerbound of the constraint
|
||||
virtual double computeUpperBound() const; // Compute the upperbound of the constraint
|
||||
virtual void computeAuxLowerBounds(int beginIndex, Vector& auxLowerBounds) const; // Compute the lowerbounds for the auxiliary constraints
|
||||
virtual void computeAuxUpperBounds(int beginIndex, Vector& auxLowerBounds) const; // Compute the upperbounds for the auxiliary constraints
|
||||
virtual double computeErrorValue() const; // Compute the error value for the constraint
|
||||
virtual void computeAuxErrorValues(int beginIndex, Vector& errorValues) const; // Compute the errors values of the auxiliary constraints
|
||||
uint getNbAuxConstraints() const; // Return the number of auxiliary constraints
|
||||
double getPenetrationDepth() const; // Return the penetration depth
|
||||
void draw() const; // TODO : Delete this (Used to debug collision detection)
|
||||
};
|
||||
|
||||
// Compute the two unit orthogonal vectors "v1" and "v2" that span the tangential friction plane
|
||||
|
|
@ -90,10 +100,142 @@ inline Vector3D Contact::getPoint() const {
|
|||
return point;
|
||||
}
|
||||
|
||||
// Return the penetration depth of the contact
|
||||
inline double Contact::getPenetrationDepth() const {
|
||||
return penetrationDepth;
|
||||
}
|
||||
|
||||
// This method computes a part of the jacobian matrix for a given body.
|
||||
// The argument "noBody" is 1 or 2 and corresponds to which one of the two
|
||||
// bodies of the constraint we will compute the jacobian part. The argument
|
||||
// "jacobian" is a 1x6 jacobian matrix of the constraint corresponding to one of
|
||||
// the two bodies of the constraint.
|
||||
inline void Contact::computeJacobian(int noBody, Matrix& jacobian) const {
|
||||
RigidBody* rigidBody;
|
||||
Vector3D rCrossN;
|
||||
Vector3D r;
|
||||
Vector3D norm = normal;
|
||||
|
||||
assert(noBody == 1 || noBody == 2);
|
||||
assert(jacobian.getNbRow() == 1 && jacobian.getNbColumn() == 6);
|
||||
|
||||
if (noBody == 1) {
|
||||
rigidBody = dynamic_cast<RigidBody*>(body1);
|
||||
assert(rigidBody);
|
||||
r = point - rigidBody->getPosition();
|
||||
rCrossN = r.crossProduct(normal).getOpposite();
|
||||
norm = normal.getOpposite();
|
||||
}
|
||||
else {
|
||||
rigidBody = dynamic_cast<RigidBody*>(body2);
|
||||
assert(rigidBody);
|
||||
r = point - rigidBody->getPosition();
|
||||
rCrossN = r.crossProduct(normal);
|
||||
}
|
||||
|
||||
// Compute the jacobian matrix for the body 1
|
||||
jacobian.setValue(0, 0, norm.getX());
|
||||
jacobian.setValue(0, 1, norm.getY());
|
||||
jacobian.setValue(0, 2, norm.getZ());
|
||||
jacobian.setValue(0, 3, rCrossN.getX());
|
||||
jacobian.setValue(0, 4, rCrossN.getY());
|
||||
jacobian.setValue(0, 5, rCrossN.getZ());
|
||||
}
|
||||
|
||||
// Compute a part of the jacobian matrix for an auxiliary constraint (given by "noAuxConstraint")
|
||||
// and one of the two bodies (given by "noBody") of the contact. The argument "noBody" is 1 or 2 and
|
||||
// argument auxJacobian is a 1x6 matrix.
|
||||
inline void Contact::computeAuxJacobian(int noBody, int noAuxConstraint, Matrix& auxJacobian) const {
|
||||
Vector3D r;
|
||||
Vector3D rCrossU;
|
||||
RigidBody* rigidBody;
|
||||
double sign;
|
||||
|
||||
assert(noBody == 1 || noBody == 2);
|
||||
assert(noAuxConstraint == 1 || noAuxConstraint == 2);
|
||||
assert(auxJacobian.getNbRow() == 1 && auxJacobian.getNbColumn() == 6);
|
||||
|
||||
if (noBody == 1) {
|
||||
rigidBody = dynamic_cast<RigidBody*>(body1);
|
||||
assert(rigidBody);
|
||||
r = point - rigidBody->getPosition();
|
||||
sign = -1.0;
|
||||
}
|
||||
else {
|
||||
rigidBody = dynamic_cast<RigidBody*>(body2);
|
||||
assert(rigidBody);
|
||||
r = point - rigidBody->getPosition();
|
||||
sign = 1.0;
|
||||
}
|
||||
|
||||
rCrossU = r.crossProduct(frictionVectors[noAuxConstraint-1]);
|
||||
|
||||
auxJacobian.setValue(0, 0, sign * frictionVectors[noAuxConstraint-1].getX());
|
||||
auxJacobian.setValue(0, 1, sign * frictionVectors[noAuxConstraint-1].getY());
|
||||
auxJacobian.setValue(0, 2, sign * frictionVectors[noAuxConstraint-1].getZ());
|
||||
auxJacobian.setValue(0, 3, sign * rCrossU.getX());
|
||||
auxJacobian.setValue(0, 4, sign * rCrossU.getY());
|
||||
auxJacobian.setValue(0, 5, sign * rCrossU.getZ());
|
||||
}
|
||||
|
||||
// Compute the lowerbounds for the auxiliary constraints
|
||||
inline double Contact::computeLowerBound() const {
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
// Compute the upperbounds for the auxiliary constraints
|
||||
inline double Contact::computeUpperBound() const {
|
||||
return INFINITY_CONST;
|
||||
}
|
||||
|
||||
// Compute the lowerbounds for the auxiliary constraints. This method fills the "auxLowerBounds"
|
||||
// vector starting at the index "beginIndex" in this vector.
|
||||
inline void Contact::computeAuxLowerBounds(int beginIndex, Vector& auxLowerBounds) const {
|
||||
assert(beginIndex + nbAuxConstraints <= auxLowerBounds.getNbComponent());
|
||||
|
||||
auxLowerBounds.setValue(beginIndex, -mu_mc_g);
|
||||
auxLowerBounds.setValue(beginIndex + 1, -mu_mc_g);
|
||||
|
||||
}
|
||||
|
||||
// Compute the upperbounds for the auxiliary constraints. This method fills the "auxUpperBounds"
|
||||
// vector starting at the index "beginIndex" in this vector.
|
||||
inline void Contact::computeAuxUpperBounds(int beginIndex, Vector& auxUpperBounds) const {
|
||||
assert(beginIndex + nbAuxConstraints <= auxUpperBounds.getNbComponent());
|
||||
|
||||
auxUpperBounds.setValue(beginIndex, mu_mc_g);
|
||||
auxUpperBounds.setValue(beginIndex + 1, mu_mc_g);
|
||||
}
|
||||
|
||||
// Compute the error value for the constraint
|
||||
inline double Contact::computeErrorValue() const {
|
||||
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(body1);
|
||||
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(body2);
|
||||
|
||||
assert(rigidBody1);
|
||||
assert(rigidBody2);
|
||||
|
||||
Vector3D velocity1 = rigidBody1->getLinearVelocity();
|
||||
Vector3D velocity2 = rigidBody2->getLinearVelocity();
|
||||
double restitutionCoeff = rigidBody1->getRestitution() * rigidBody2->getRestitution();
|
||||
double errorValue = restitutionCoeff * (normal.scalarProduct(velocity1) - normal.scalarProduct(velocity2)) + PENETRATION_FACTOR * penetrationDepth;
|
||||
return errorValue;
|
||||
}
|
||||
|
||||
// Compute the errors values of the auxiliary constraints
|
||||
inline void Contact::computeAuxErrorValues(int beginIndex, Vector& errorValues) const {
|
||||
assert(beginIndex + nbAuxConstraints <= errorValues.getNbComponent());
|
||||
|
||||
errorValues.setValue(beginIndex, 0.0);
|
||||
errorValues.setValue(beginIndex + 1, 0.0);
|
||||
}
|
||||
|
||||
// TODO : Delete this (Used to debug collision detection)
|
||||
inline void Contact::draw() const {
|
||||
glColor3f(1.0, 0.0, 0.0);
|
||||
glutSolidSphere(0.3, 20, 20);
|
||||
}
|
||||
|
||||
} // End of the ReactPhysics3D namespace
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -50,9 +50,6 @@ void ConstraintSolver::initialize() {
|
|||
for (it = physicsWorld->getConstraintsBeginIterator(); it != physicsWorld->getConstraintsEndIterator(); it++) {
|
||||
constraint = *it;
|
||||
|
||||
// Evaluate the constraint
|
||||
constraint->evaluate();
|
||||
|
||||
// If the constraint is active
|
||||
if (constraint->isActive()) {
|
||||
activeConstraints.push_back(constraint);
|
||||
|
|
@ -178,36 +175,39 @@ void ConstraintSolver::freeMemory(bool freeBodiesMemory) {
|
|||
// Fill in all the matrices needed to solve the LCP problem
|
||||
// Notice that all the active constraints should have been evaluated first
|
||||
void ConstraintSolver::fillInMatrices() {
|
||||
Constraint* constraint;
|
||||
Contact* contact;
|
||||
ContactCachingInfo* contactInfo;
|
||||
|
||||
// For each active constraint
|
||||
uint noConstraint = 0;
|
||||
uint nbAuxConstraints = 0;
|
||||
for (uint c=0; c<activeConstraints.size(); c++) {
|
||||
|
||||
Constraint* constraint = activeConstraints.at(c);
|
||||
constraint = activeConstraints.at(c);
|
||||
|
||||
// Fill in the J_sp matrix
|
||||
J_sp[noConstraint][0].changeSize(1, 6);
|
||||
J_sp[noConstraint][1].changeSize(1, 6);
|
||||
J_sp[noConstraint][0] = constraint->getBody1Jacobian();
|
||||
J_sp[noConstraint][1] = constraint->getBody2Jacobian();
|
||||
constraint->computeJacobian(1, J_sp[noConstraint][0]);
|
||||
constraint->computeJacobian(2, J_sp[noConstraint][1]);
|
||||
|
||||
// Fill in the body mapping matrix
|
||||
bodyMapping[noConstraint][0] = constraint->getBody1();
|
||||
bodyMapping[noConstraint][1] = constraint->getBody2();
|
||||
|
||||
// Fill in the limit vectors for the constraint
|
||||
lowerBounds.setValue(noConstraint, constraint->getLowerBound());
|
||||
upperBounds.setValue(noConstraint, constraint->getUpperBound());
|
||||
lowerBounds.setValue(noConstraint, constraint->computeLowerBound());
|
||||
upperBounds.setValue(noConstraint, constraint->computeUpperBound());
|
||||
|
||||
// Fill in the error vector
|
||||
errorValues.setValue(noConstraint, constraint->getErrorValue());
|
||||
errorValues.setValue(noConstraint, constraint->computeErrorValue());
|
||||
|
||||
// If it's a contact constraint
|
||||
Contact* contact = dynamic_cast<Contact*>(constraint);
|
||||
contact = dynamic_cast<Contact*>(constraint);
|
||||
if (contact) {
|
||||
// Get the lambda init value from the cache if exists
|
||||
ContactCachingInfo* contactInfo = contactCache.getContactCachingInfo(contact->getBody1(), contact->getBody2(), contact->getPoint());
|
||||
contactInfo = contactCache.getContactCachingInfo(contact->getBody1(), contact->getBody2(), contact->getPoint());
|
||||
if (contactInfo) {
|
||||
// The last lambda init value was in the cache
|
||||
lambdaInit.setValue(noConstraint, contactInfo->lambda);
|
||||
|
|
@ -232,8 +232,8 @@ void ConstraintSolver::fillInMatrices() {
|
|||
// Fill in the J_sp matrix
|
||||
J_sp[noConstraint+i][0].changeSize(1, 6);
|
||||
J_sp[noConstraint+i][1].changeSize(1, 6);
|
||||
J_sp[noConstraint+i][0] = constraint->getAuxJacobian().getSubMatrix(i-1, 0, 1, 6);
|
||||
J_sp[noConstraint+i][1] = constraint->getAuxJacobian().getSubMatrix(i-1, 6, 1, 6);
|
||||
constraint->computeAuxJacobian(1, i, J_sp[noConstraint+i][0]);
|
||||
constraint->computeAuxJacobian(2, i, J_sp[noConstraint+i][1]);
|
||||
|
||||
// Fill in the body mapping matrix
|
||||
bodyMapping[noConstraint+i][0] = constraint->getBody1();
|
||||
|
|
@ -244,8 +244,11 @@ void ConstraintSolver::fillInMatrices() {
|
|||
}
|
||||
|
||||
// Fill in the limit vectors for the auxilirary constraints
|
||||
lowerBounds.fillInSubVector(noConstraint+1, constraint->getAuxLowerBounds());
|
||||
upperBounds.fillInSubVector(noConstraint+1, constraint->getAuxUpperBounds());
|
||||
constraint->computeAuxLowerBounds(noConstraint+1, lowerBounds);
|
||||
constraint->computeAuxUpperBounds(noConstraint+1, upperBounds);
|
||||
|
||||
// Fill in the errorValues vector for the auxiliary constraints
|
||||
constraint->computeAuxErrorValues(noConstraint+1, errorValues);
|
||||
}
|
||||
|
||||
noConstraint += 1 + nbAuxConstraints;
|
||||
|
|
@ -256,6 +259,8 @@ void ConstraintSolver::fillInMatrices() {
|
|||
Body* body;
|
||||
Vector v(6);
|
||||
Vector f(6);
|
||||
Matrix identity = Matrix::identity(3);
|
||||
Matrix mInv(6,6);
|
||||
uint b=0;
|
||||
for (set<Body*>::iterator it = constraintBodies.begin(); it != constraintBodies.end(); it++, b++) {
|
||||
body = *it;
|
||||
|
|
@ -282,10 +287,9 @@ void ConstraintSolver::fillInMatrices() {
|
|||
Fext[bodyNumber] = f;
|
||||
|
||||
// Compute the inverse sparse mass matrix
|
||||
Matrix mInv(6,6);
|
||||
mInv.initWithValue(0.0);
|
||||
if (rigidBody->getIsMotionEnabled()) {
|
||||
mInv.fillInSubMatrix(0, 0, rigidBody->getMassInverse() * Matrix::identity(3));
|
||||
mInv.fillInSubMatrix(0, 0, rigidBody->getMassInverse() * identity);
|
||||
mInv.fillInSubMatrix(3, 3, rigidBody->getInertiaTensorInverseWorld());
|
||||
}
|
||||
Minv_sp[bodyNumber].changeSize(6, 6);
|
||||
|
|
@ -348,6 +352,9 @@ void ConstraintSolver::computeVectorVconstraint(double dt) {
|
|||
|
||||
// Clear and Fill in the contact cache with the new lambda values
|
||||
void ConstraintSolver::updateContactCache() {
|
||||
Contact* contact;
|
||||
ContactCachingInfo* contactInfo;
|
||||
|
||||
// Clear the contact cache
|
||||
contactCache.clear();
|
||||
|
||||
|
|
@ -356,10 +363,10 @@ void ConstraintSolver::updateContactCache() {
|
|||
for (uint c=0; c<activeConstraints.size(); c++) {
|
||||
|
||||
// If it's a contact constraint
|
||||
Contact* contact = dynamic_cast<Contact*>(activeConstraints.at(c));
|
||||
contact = dynamic_cast<Contact*>(activeConstraints.at(c));
|
||||
if (contact) {
|
||||
// Create a new ContactCachingInfo
|
||||
ContactCachingInfo* contactInfo = new ContactCachingInfo(contact->getBody1(), contact->getBody2(), contact->getPoint(), lambda.getValue(noConstraint));
|
||||
contactInfo = new ContactCachingInfo(contact->getBody1(), contact->getBody2(), contact->getPoint(), lambda.getValue(noConstraint));
|
||||
|
||||
// Add it to the contact cache
|
||||
contactCache.addContactCachingInfo(contactInfo);
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user