Now we can use a single Contact for several contact points between two bodies
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@404 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel
parent
47c302fe56
commit
9fd3d8b598
|
|
@ -131,11 +131,11 @@ void CollisionDetection::computeContact(const ContactInfo* const contactInfo) {
|
|||
// If it's a Vertex-Something contact
|
||||
if (nbVerticesExtremeOBB1 == 1) {
|
||||
// Create a new contact and add it to the physics world
|
||||
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, obb1ExtremePoints.at(0)));
|
||||
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, obb1ExtremePoints));
|
||||
}
|
||||
else if(nbVerticesExtremeOBB2 == 1) { // If its a Vertex-Something contact
|
||||
// Create a new contact and add it to the physics world
|
||||
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, obb2ExtremePoints.at(0)));
|
||||
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, obb2ExtremePoints));
|
||||
}
|
||||
else if (nbVerticesExtremeOBB1 == 2 && nbVerticesExtremeOBB2 == 2) { // If it's an edge-edge contact
|
||||
// Compute the two vectors of the segment lines
|
||||
|
|
@ -171,11 +171,8 @@ void CollisionDetection::computeContact(const ContactInfo* const contactInfo) {
|
|||
contactSet.push_back(contactPoint);
|
||||
}
|
||||
|
||||
// For each point of the set of contact points
|
||||
for (unsigned int i=0; i<contactSet.size(); i++) {
|
||||
// Create a new contact and add it to the physics world
|
||||
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, contactSet.at(i)));
|
||||
}
|
||||
// Create a new contact and add it to the physics world
|
||||
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, contactSet));
|
||||
}
|
||||
else if(nbVerticesExtremeOBB1 == 2 && nbVerticesExtremeOBB2 == 4) { // If it's an edge-face contact
|
||||
// Compute the projection of the edge of OBB1 onto the same plane of the face of OBB2
|
||||
|
|
@ -199,11 +196,8 @@ void CollisionDetection::computeContact(const ContactInfo* const contactInfo) {
|
|||
|
||||
assert(clippedEdge.size() == 2);
|
||||
|
||||
// For each point of the contact set
|
||||
for (unsigned int i=0; i<clippedEdge.size(); i++) {
|
||||
// Create a new contact and add it to the physics world
|
||||
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedEdge.at(i)));
|
||||
}
|
||||
// Create a new contact and add it to the physics world
|
||||
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedEdge));
|
||||
}
|
||||
else if(nbVerticesExtremeOBB1 == 4 && nbVerticesExtremeOBB2 == 2) { // If it's an edge-face contact
|
||||
// Compute the projection of the edge of OBB2 onto the same plane of the face of OBB1
|
||||
|
|
@ -227,11 +221,8 @@ void CollisionDetection::computeContact(const ContactInfo* const contactInfo) {
|
|||
|
||||
assert(clippedEdge.size() == 2);
|
||||
|
||||
// For each point of the contact set
|
||||
for (unsigned int i=0; i<clippedEdge.size(); i++) {
|
||||
// Create a new contact and add it to the physics world
|
||||
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedEdge.at(i)));
|
||||
}
|
||||
// Create a new contact and add it to the physics world
|
||||
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedEdge));
|
||||
}
|
||||
else { // If it's a face-face contact
|
||||
// Compute the projection of the face vertices of OBB2 onto the plane of the face of OBB1
|
||||
|
|
@ -244,10 +235,7 @@ void CollisionDetection::computeContact(const ContactInfo* const contactInfo) {
|
|||
clippedFace = movePoints(clippedFace, penetrationDepth/2.0 * normal);
|
||||
assert(clippedFace.size() >= 3);
|
||||
|
||||
// For each point of the contact set
|
||||
for (unsigned int i=0; i<clippedFace.size(); i++) {
|
||||
// Create a new contact and add it to the physics world
|
||||
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedFace.at(i)));
|
||||
}
|
||||
// Create a new contact and add it to the physics world
|
||||
world->addConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedFace));
|
||||
}
|
||||
}
|
||||
|
|
@ -29,8 +29,8 @@
|
|||
using namespace reactphysics3d;
|
||||
|
||||
// Constructor
|
||||
Constraint::Constraint(Body* const body1, Body* const body2, uint nbAuxConstraints, bool active)
|
||||
:body1(body1), body2(body2), active(active), nbAuxConstraints(nbAuxConstraints) {
|
||||
Constraint::Constraint(Body* const body1, Body* const body2, uint nbConstraints, bool active)
|
||||
:body1(body1), body2(body2), active(active), nbConstraints(nbConstraints) {
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -36,15 +36,8 @@ namespace reactphysics3d {
|
|||
Class Constraint :
|
||||
This abstract class represents a constraint in the physics engine.
|
||||
A constraint can be a collision contact or a joint for
|
||||
instance. Each constraint have a jacobian matrix associated with
|
||||
the first body of the constraint and a jacobian matrix associated
|
||||
with the second body. Each constraint can also have some auxiliary
|
||||
constraints. Those auxiliary constraint are all represented in the
|
||||
auxiliary Jacobian matrix. Auxiliary constraints represents some
|
||||
constraints associated with a main constraint. For instance, a
|
||||
contact constraint between two bodies can have two associated
|
||||
auxiliary constraints to represent the frictions force constraints
|
||||
in two directions.
|
||||
instance. Each constraint can be made of several "mathematical
|
||||
constraints" needed to represent the main constraint.
|
||||
-------------------------------------------------------------------
|
||||
*/
|
||||
class Constraint {
|
||||
|
|
@ -52,23 +45,19 @@ class Constraint {
|
|||
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
|
||||
|
||||
uint nbConstraints; // Number mathematical 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 // 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
|
||||
Constraint(Body* const body1, Body* const body2, uint nbConstraints, 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 // 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, Matrix**& 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
|
||||
};
|
||||
|
||||
// Return the reference to the body 1
|
||||
|
|
@ -87,8 +76,8 @@ inline bool Constraint::isActive() const {
|
|||
}
|
||||
|
||||
// Return the number auxiliary constraints
|
||||
inline uint Constraint::getNbAuxConstraints() const {
|
||||
return nbAuxConstraints;
|
||||
inline uint Constraint::getNbConstraints() const {
|
||||
return nbConstraints;
|
||||
}
|
||||
|
||||
} // End of the ReactPhysics3D namespace
|
||||
|
|
|
|||
|
|
@ -25,12 +25,12 @@
|
|||
// Libraries
|
||||
#include "Contact.h"
|
||||
|
||||
// We want to use the ReactPhysics3D namespace
|
||||
using namespace reactphysics3d;
|
||||
using namespace std;
|
||||
|
||||
// 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) {
|
||||
Contact::Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const vector<Vector3D>& points)
|
||||
:Constraint(body1, body2, 3*points.size(), true), normal(normal), penetrationDepth(penetrationDepth), points(points), nbPoints(points.size()) {
|
||||
|
||||
// Compute the auxiliary lower and upper bounds
|
||||
// TODO : Now mC is only the mass of the first body but it is probably wrong
|
||||
|
|
@ -45,3 +45,161 @@ Contact::Contact(Body* const body1, Body* const body2, const Vector3D& normal, d
|
|||
Contact::~Contact() {
|
||||
|
||||
}
|
||||
|
||||
// This method computes the jacobian matrix for all mathematical constraints
|
||||
// The argument "J_sp" is the jacobian matrix of the constraint solver. This method
|
||||
// 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, Matrix**& J_sp) const {
|
||||
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(body1);
|
||||
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(body2);
|
||||
Vector3D r1;
|
||||
Vector3D r2;
|
||||
Vector3D r1CrossN;
|
||||
Vector3D r2CrossN;
|
||||
Vector3D r1CrossU1;
|
||||
Vector3D r2CrossU1;
|
||||
Vector3D r1CrossU2;
|
||||
Vector3D r2CrossU2;
|
||||
Vector3D body1Position = rigidBody1->getPosition();
|
||||
Vector3D body2Position = rigidBody2->getPosition();
|
||||
int currentIndex = noConstraint; // Current constraint index
|
||||
|
||||
assert(rigidBody1 != 0);
|
||||
assert(rigidBody2 != 0);
|
||||
|
||||
// For each point in the contact
|
||||
for (int i=0; i<nbPoints; i++) {
|
||||
|
||||
r1 = points[i] - body1Position;
|
||||
r2 = points[i] - body2Position;
|
||||
r1CrossN = r1.crossProduct(normal);
|
||||
r2CrossN = r2.crossProduct(normal);
|
||||
|
||||
// Compute the jacobian matrix for the body 1 for the contact constraint
|
||||
J_sp[currentIndex][0].changeSize(1, 6);
|
||||
J_sp[currentIndex][0].setValue(0, 0, -normal.getX());
|
||||
J_sp[currentIndex][0].setValue(0, 1, -normal.getY());
|
||||
J_sp[currentIndex][0].setValue(0, 2, -normal.getZ());
|
||||
J_sp[currentIndex][0].setValue(0, 3, -r1CrossN.getX());
|
||||
J_sp[currentIndex][0].setValue(0, 4, -r1CrossN.getY());
|
||||
J_sp[currentIndex][0].setValue(0, 5, -r1CrossN.getZ());
|
||||
|
||||
// Compute the jacobian matrix for the body 2 for the contact constraint
|
||||
J_sp[currentIndex][1].changeSize(1, 6);
|
||||
J_sp[currentIndex][1].setValue(0, 0, normal.getX());
|
||||
J_sp[currentIndex][1].setValue(0, 1, normal.getY());
|
||||
J_sp[currentIndex][1].setValue(0, 2, normal.getZ());
|
||||
J_sp[currentIndex][1].setValue(0, 3, r2CrossN.getX());
|
||||
J_sp[currentIndex][1].setValue(0, 4, r2CrossN.getY());
|
||||
J_sp[currentIndex][1].setValue(0, 5, r2CrossN.getZ());
|
||||
|
||||
currentIndex++;
|
||||
|
||||
// Compute the jacobian matrix for the body 1 for the first friction constraint
|
||||
r1CrossU1 = r1.crossProduct(frictionVectors[0]);
|
||||
r2CrossU1 = r2.crossProduct(frictionVectors[0]);
|
||||
r1CrossU2 = r1.crossProduct(frictionVectors[1]);
|
||||
r2CrossU2 = r2.crossProduct(frictionVectors[1]);
|
||||
J_sp[currentIndex][0].changeSize(1, 6);
|
||||
J_sp[currentIndex][0].setValue(0, 0, -frictionVectors[0].getX());
|
||||
J_sp[currentIndex][0].setValue(0, 1, -frictionVectors[0].getY());
|
||||
J_sp[currentIndex][0].setValue(0, 2, -frictionVectors[0].getZ());
|
||||
J_sp[currentIndex][0].setValue(0, 3, -r1CrossU1.getX());
|
||||
J_sp[currentIndex][0].setValue(0, 4, -r1CrossU1.getY());
|
||||
J_sp[currentIndex][0].setValue(0, 5, -r1CrossU1.getZ());
|
||||
|
||||
// Compute the jacobian matrix for the body 2 for the first friction constraint
|
||||
J_sp[currentIndex][1].changeSize(1, 6);
|
||||
J_sp[currentIndex][1].setValue(0, 0, frictionVectors[0].getX());
|
||||
J_sp[currentIndex][1].setValue(0, 1, frictionVectors[0].getY());
|
||||
J_sp[currentIndex][1].setValue(0, 2, frictionVectors[0].getZ());
|
||||
J_sp[currentIndex][1].setValue(0, 3, r2CrossU1.getX());
|
||||
J_sp[currentIndex][1].setValue(0, 4, r2CrossU1.getY());
|
||||
J_sp[currentIndex][1].setValue(0, 5, r2CrossU1.getZ());
|
||||
|
||||
currentIndex++;
|
||||
|
||||
// Compute the jacobian matrix for the body 1 for the second friction constraint
|
||||
J_sp[currentIndex][0].changeSize(1, 6);
|
||||
J_sp[currentIndex][0].setValue(0, 0, -frictionVectors[1].getX());
|
||||
J_sp[currentIndex][0].setValue(0, 1, -frictionVectors[1].getY());
|
||||
J_sp[currentIndex][0].setValue(0, 2, -frictionVectors[1].getZ());
|
||||
J_sp[currentIndex][0].setValue(0, 3, -r1CrossU2.getX());
|
||||
J_sp[currentIndex][0].setValue(0, 4, -r1CrossU2.getY());
|
||||
J_sp[currentIndex][0].setValue(0, 5, -r1CrossU2.getZ());
|
||||
J_sp[currentIndex][1].changeSize(1, 6);
|
||||
|
||||
// Compute the jacobian matrix for the body 2 for the second friction constraint
|
||||
J_sp[currentIndex][1].setValue(0, 0, frictionVectors[1].getX());
|
||||
J_sp[currentIndex][1].setValue(0, 1, frictionVectors[1].getY());
|
||||
J_sp[currentIndex][1].setValue(0, 2, frictionVectors[1].getZ());
|
||||
J_sp[currentIndex][1].setValue(0, 3, r2CrossU2.getX());
|
||||
J_sp[currentIndex][1].setValue(0, 4, r2CrossU2.getY());
|
||||
J_sp[currentIndex][1].setValue(0, 5, r2CrossU2.getZ());
|
||||
|
||||
currentIndex++;
|
||||
}
|
||||
}
|
||||
|
||||
// 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 {
|
||||
int index = noConstraint;
|
||||
|
||||
assert(noConstraint >= 0 && noConstraint + nbConstraints <= lowerBounds.getNbComponent());
|
||||
|
||||
// For each constraint
|
||||
for (int i=0; i<nbPoints; i++) {
|
||||
lowerBounds.setValue(index, 0.0); // Lower bound for the contact constraint
|
||||
lowerBounds.setValue(index + 1, -mu_mc_g); // Lower bound for the first friction constraint
|
||||
lowerBounds.setValue(index + 2, -mu_mc_g); // Lower bound for the second friction constraint
|
||||
index += 3;
|
||||
}
|
||||
}
|
||||
|
||||
// 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 {
|
||||
int index = noConstraint;
|
||||
|
||||
assert(noConstraint >= 0 && noConstraint + nbConstraints <= upperBounds.getNbComponent());
|
||||
|
||||
// For each constraint
|
||||
for (int i=0; i<nbPoints; i++) {
|
||||
upperBounds.setValue(index, INFINITY_CONST); // Upper bound for the contact constraint
|
||||
upperBounds.setValue(index + 1, mu_mc_g); // Upper bound for the first friction constraint
|
||||
upperBounds.setValue(index + 2, mu_mc_g); // Upper bound for the second friction constraint
|
||||
index += 3;
|
||||
}
|
||||
}
|
||||
|
||||
// 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 {
|
||||
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(body1);
|
||||
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(body2);
|
||||
int index = noConstraint;
|
||||
|
||||
assert(rigidBody1);
|
||||
assert(rigidBody2);
|
||||
assert(noConstraint >= 0 && noConstraint + nbConstraints <= errorValues.getNbComponent());
|
||||
|
||||
// Compute the error value for the contact constraint
|
||||
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;
|
||||
|
||||
// Assign the error value to the vector of error values
|
||||
for (int i=0; i<nbPoints; i++) {
|
||||
errorValues.setValue(index, errorValue); // Error value for contact constraint
|
||||
errorValues.setValue(index + 1, 0.0); // Error value for friction constraint
|
||||
errorValues.setValue(index + 2, 0.0); // Error value for friction constraint
|
||||
index += 3;
|
||||
}
|
||||
}
|
||||
|
|
@ -45,40 +45,36 @@ const double PENETRATION_FACTOR = 0.0; // Penetration factor (between 0 and
|
|||
Class Contact :
|
||||
This class represents a collision contact between two bodies in
|
||||
the physics engine. The contact class inherits from the
|
||||
Constraint class. The collision detection system computes
|
||||
contact informations that will be used to perform the collision
|
||||
response. Each contact contains only one contact point. A contact
|
||||
constraint has two auxiliary constraints in order two represent
|
||||
the two friction forces at the contact surface.
|
||||
Constraint class. Each Contact represent a contact between two bodies
|
||||
and can have several contact points. The Contact will have 3 mathematical
|
||||
constraints for each contact point (1 for the contact constraint, and 2
|
||||
for the friction constraints).
|
||||
-------------------------------------------------------------------
|
||||
*/
|
||||
class Contact : public Constraint {
|
||||
protected :
|
||||
const Vector3D normal; // Normal vector of the contact (From body1 toward body2)
|
||||
const double penetrationDepth; // Penetration depth
|
||||
const Vector3D point; // Contact point
|
||||
const std::vector<Vector3D> points; // Contact points between the two bodies
|
||||
const int nbPoints; // Number of points in the contact
|
||||
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
|
||||
Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const std::vector<Vector3D>& points); // Constructor
|
||||
virtual ~Contact(); // Destructor
|
||||
|
||||
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)
|
||||
Vector3D getNormal() const; // Return the normal vector of the contact
|
||||
Vector3D getPoint(int index) const; // Return a contact point
|
||||
int getNbPoints() const; // Return the number of contact points
|
||||
virtual void computeJacobian(int noConstraint, Matrix**& 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 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
|
||||
|
|
@ -100,9 +96,15 @@ inline Vector3D Contact::getNormal() const {
|
|||
return normal;
|
||||
}
|
||||
|
||||
// Return the contact points
|
||||
inline Vector3D Contact::getPoint() const {
|
||||
return point;
|
||||
// Return a contact points
|
||||
inline Vector3D Contact::getPoint(int index) const {
|
||||
assert(index >= 0 && index < nbPoints);
|
||||
return points[index];
|
||||
}
|
||||
|
||||
// Return the number of contact points
|
||||
inline int Contact::getNbPoints() const {
|
||||
return nbPoints;
|
||||
}
|
||||
|
||||
// Return the penetration depth of the contact
|
||||
|
|
@ -110,131 +112,6 @@ 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);
|
||||
|
|
|
|||
|
|
@ -68,7 +68,7 @@ void ConstraintSolver::initialize() {
|
|||
bodyNumberMapping.insert(pair<Body*, unsigned int>(constraint->getBody2(), bodyNumberMapping.size()));
|
||||
|
||||
// Update the size of the jacobian matrix
|
||||
nbConstraints += (1 + constraint->getNbAuxConstraints());
|
||||
nbConstraints += constraint->getNbConstraints();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -132,7 +132,7 @@ void ConstraintSolver::allocate() {
|
|||
B_sp = new Matrix*[2];
|
||||
B_sp[0] = new Matrix[nbConstraints];
|
||||
B_sp[1] = new Matrix[nbConstraints];
|
||||
for (uint i=0; i<nbConstraints; i++) {
|
||||
for (int i=0; i<nbConstraints; i++) {
|
||||
bodyMapping[i] = new Body*[2];
|
||||
J_sp[i] = new Matrix[2];
|
||||
}
|
||||
|
|
@ -185,78 +185,47 @@ void ConstraintSolver::fillInMatrices() {
|
|||
ContactCachingInfo* contactInfo;
|
||||
|
||||
// For each active constraint
|
||||
uint noConstraint = 0;
|
||||
uint nbAuxConstraints = 0;
|
||||
for (uint c=0; c<activeConstraints.size(); c++) {
|
||||
int noConstraint = 0;
|
||||
//uint nbAuxConstraints = 0;
|
||||
|
||||
for (int c=0; c<activeConstraints.size(); 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);
|
||||
constraint->computeJacobian(1, J_sp[noConstraint][0]);
|
||||
constraint->computeJacobian(2, J_sp[noConstraint][1]);
|
||||
constraint->computeJacobian(noConstraint, J_sp);
|
||||
//constraint->computeJacobian(noConstraint, J_sp);
|
||||
|
||||
// Fill in the body mapping matrix
|
||||
bodyMapping[noConstraint][0] = constraint->getBody1();
|
||||
bodyMapping[noConstraint][1] = constraint->getBody2();
|
||||
for(int i=0; i<constraint->getNbConstraints(); i++) {
|
||||
bodyMapping[noConstraint+i][0] = constraint->getBody1();
|
||||
bodyMapping[noConstraint+i][1] = constraint->getBody2();
|
||||
}
|
||||
|
||||
// Fill in the limit vectors for the constraint
|
||||
lowerBounds.setValue(noConstraint, constraint->computeLowerBound());
|
||||
upperBounds.setValue(noConstraint, constraint->computeUpperBound());
|
||||
constraint->computeLowerBound(noConstraint, lowerBounds);
|
||||
constraint->computeUpperBound(noConstraint, upperBounds);
|
||||
|
||||
// Fill in the error vector
|
||||
errorValues.setValue(noConstraint, constraint->computeErrorValue());
|
||||
constraint->computeErrorValue(noConstraint, errorValues);
|
||||
|
||||
// If it's a contact constraint
|
||||
// Set the init lambda values
|
||||
contact = dynamic_cast<Contact*>(constraint);
|
||||
contactInfo = 0;
|
||||
if (contact) {
|
||||
// Get the lambda init value from the cache if exists
|
||||
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);
|
||||
contactInfo = contactCache.getContactCachingInfo(contact);
|
||||
}
|
||||
for (int i=0; i<constraint->getNbConstraints(); i++) {
|
||||
if (contactInfo) { // If the last lambda init value is in the cache
|
||||
lambdaInit.setValue(noConstraint + i, contactInfo->lambdas[i]);
|
||||
}
|
||||
else {
|
||||
// The las lambda init value was not in the cache
|
||||
lambdaInit.setValue(noConstraint, 0.0);
|
||||
else { // The las lambda init value was not in the cache
|
||||
lambdaInit.setValue(noConstraint + i, 0.0);
|
||||
}
|
||||
}
|
||||
else {
|
||||
// Set the lambda init value
|
||||
lambdaInit.setValue(noConstraint, 0.0);
|
||||
}
|
||||
|
||||
nbAuxConstraints = constraint->getNbAuxConstraints();
|
||||
|
||||
// If the current constraint has auxiliary constraints
|
||||
if (nbAuxConstraints > 0) {
|
||||
|
||||
// For each auxiliary constraints
|
||||
for (uint i=1; i<=nbAuxConstraints; i++) {
|
||||
// Fill in the J_sp matrix
|
||||
J_sp[noConstraint+i][0].changeSize(1, 6);
|
||||
J_sp[noConstraint+i][1].changeSize(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();
|
||||
bodyMapping[noConstraint+i][1] = constraint->getBody2();
|
||||
|
||||
// Fill in the init lambda value for the constraint
|
||||
lambdaInit.setValue(noConstraint+i, 0.0);
|
||||
}
|
||||
|
||||
// Fill in the limit vectors for the auxilirary constraints
|
||||
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;
|
||||
noConstraint += constraint->getNbConstraints();
|
||||
}
|
||||
|
||||
// For each current body that is implied in some constraint
|
||||
|
|
@ -359,24 +328,40 @@ void ConstraintSolver::computeVectorVconstraint(double dt) {
|
|||
void ConstraintSolver::updateContactCache() {
|
||||
Contact* contact;
|
||||
ContactCachingInfo* contactInfo;
|
||||
int index;
|
||||
|
||||
// Clear the contact cache
|
||||
contactCache.clear();
|
||||
|
||||
// For each active constraint
|
||||
uint noConstraint = 0;
|
||||
for (uint c=0; c<activeConstraints.size(); c++) {
|
||||
int noConstraint = 0;
|
||||
for (int c=0; c<activeConstraints.size(); c++) {
|
||||
index = noConstraint;
|
||||
|
||||
// If it's a contact constraint
|
||||
contact = dynamic_cast<Contact*>(activeConstraints.at(c));
|
||||
if (contact) {
|
||||
|
||||
// Get all the contact points of the contact
|
||||
vector<Vector3D> points;
|
||||
vector<double> lambdas;
|
||||
for (int i=0; i<contact->getNbPoints(); i++) {
|
||||
points.push_back(contact->getPoint(i));
|
||||
}
|
||||
|
||||
// For each constraint of the contact
|
||||
for (int i=0; i<contact->getNbConstraints(); i++) {
|
||||
// Get the lambda value that have just been computed
|
||||
lambdas.push_back(lambda.getValue(noConstraint + i));
|
||||
}
|
||||
|
||||
// Create a new ContactCachingInfo
|
||||
contactInfo = new ContactCachingInfo(contact->getBody1(), contact->getBody2(), contact->getPoint(), lambda.getValue(noConstraint));
|
||||
contactInfo = new ContactCachingInfo(contact->getBody1(), contact->getBody2(), points, lambdas);
|
||||
|
||||
// Add it to the contact cache
|
||||
contactCache.addContactCachingInfo(contactInfo);
|
||||
}
|
||||
|
||||
noConstraint += 1 + activeConstraints.at(c)->getNbAuxConstraints();
|
||||
noConstraint += activeConstraints.at(c)->getNbConstraints();
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -134,6 +134,13 @@ inline Vector3D ConstraintSolver::getConstrainedAngularVelocityOfBody(Body* body
|
|||
return Vector3D(vec.getValue(0), vec.getValue(1), vec.getValue(2));
|
||||
}
|
||||
|
||||
// Cleanup of the constraint solver
|
||||
inline void ConstraintSolver::cleanup() {
|
||||
bodyNumberMapping.clear();
|
||||
constraintBodies.clear();
|
||||
activeConstraints.clear();
|
||||
}
|
||||
|
||||
// Solve the current LCP problem
|
||||
inline void ConstraintSolver::solve(double dt) {
|
||||
// Allocate memory for the matrices
|
||||
|
|
@ -159,13 +166,6 @@ inline void ConstraintSolver::solve(double dt) {
|
|||
computeVectorVconstraint(dt);
|
||||
}
|
||||
|
||||
// Cleanup of the constraint solver
|
||||
inline void ConstraintSolver::cleanup() {
|
||||
bodyNumberMapping.clear();
|
||||
constraintBodies.clear();
|
||||
activeConstraints.clear();
|
||||
}
|
||||
|
||||
} // End of ReactPhysics3D namespace
|
||||
|
||||
#endif
|
||||
|
|
@ -46,44 +46,53 @@ void ContactCache::clear() {
|
|||
|
||||
// Add a new contact caching info in the cache
|
||||
void ContactCache::addContactCachingInfo(ContactCachingInfo* info) {
|
||||
// Check if there is already an entry for that pair of body in the cache
|
||||
map<pair<Body*, Body*>, vector<ContactCachingInfo*> >::iterator entry = cache.find(make_pair(info->body1, info->body2));
|
||||
if (entry != cache.end()) {
|
||||
(*entry).second.push_back(info);
|
||||
}
|
||||
else {
|
||||
// Add a new entry in the cache
|
||||
vector<ContactCachingInfo*> vec;
|
||||
vec.push_back(info);
|
||||
cache.insert(make_pair(make_pair(info->body1, info->body2), vec));
|
||||
}
|
||||
// Add a new entry in the cache
|
||||
cache.insert(make_pair(make_pair(info->body1, info->body2), info));
|
||||
}
|
||||
|
||||
// Return the ContactCachingInfo (if exists) corresponding to the arguments
|
||||
// If the contact pair between the two bodies in argument doesn't exist this or there is no ContactCachingInfo
|
||||
// compatible (with approximatively the same position), this method returns NULL.
|
||||
ContactCachingInfo* ContactCache::getContactCachingInfo(Body* body1, Body* body2, const Vector3D& position) {
|
||||
// Check if there is an entry for that pair of body in the cache
|
||||
map<pair<Body*, Body*>, vector<ContactCachingInfo*> >::iterator entry = cache.find(make_pair(body1, body2));
|
||||
ContactCachingInfo* ContactCache::getContactCachingInfo(Contact* contact) const {
|
||||
double posX, posY, posZ;
|
||||
|
||||
// Check if there is an entry for that pair of body in the cache
|
||||
map<pair<Body*, Body*>, ContactCachingInfo* >::const_iterator entry = cache.find(make_pair(contact->getBody1(), contact->getBody2()));
|
||||
if (entry != cache.end()) {
|
||||
vector<ContactCachingInfo*> vec = (*entry).second;
|
||||
assert((*entry).first.first == body1);
|
||||
assert((*entry).first.second == body2);
|
||||
ContactCachingInfo* contactInfo = (*entry).second;
|
||||
|
||||
double posX = position.getX();
|
||||
double posY = position.getY();
|
||||
double posZ = position.getZ();
|
||||
assert(contactInfo);
|
||||
assert((*entry).first.first == contact->getBody1());
|
||||
assert((*entry).first.second == contact->getBody2());
|
||||
|
||||
// Check among all the old contacts for this pair of body if there is an approximative match for the contact position
|
||||
for(vector<ContactCachingInfo*>::iterator it = vec.begin(); it != vec.end(); it++) {
|
||||
Vector3D& contactPos = (*it)->position;
|
||||
if (posX <= contactPos.getX() + POSITION_TOLERANCE && posX >= contactPos.getX() - POSITION_TOLERANCE &&
|
||||
posY <= contactPos.getY() + POSITION_TOLERANCE && posY >= contactPos.getY() - POSITION_TOLERANCE &&
|
||||
posZ <= contactPos.getZ() + POSITION_TOLERANCE && posZ >= contactPos.getZ() - POSITION_TOLERANCE) {
|
||||
// Return the ContactCachingInfo
|
||||
return *it;
|
||||
// If the new contact and the contact caching info doesn't have the same number of contact points
|
||||
if (contact->getNbPoints() != contactInfo->positions.size()) {
|
||||
// We return NULL because, the contact doesn't match
|
||||
return 0;
|
||||
}
|
||||
|
||||
for (int i=0; i<contactInfo->positions.size(); i++) {
|
||||
|
||||
// Get the position of the current contact
|
||||
posX = contact->getPoint(i).getX();
|
||||
posY = contact->getPoint(i).getY();
|
||||
posZ = contact->getPoint(i).getZ();
|
||||
|
||||
// Get the position of the old contact
|
||||
Vector3D& contactPos = contactInfo->positions[i];
|
||||
|
||||
// If the old contact point doesn't match the current one
|
||||
if (posX > contactPos.getX() + POSITION_TOLERANCE || posX < contactPos.getX() - POSITION_TOLERANCE ||
|
||||
posY > contactPos.getY() + POSITION_TOLERANCE || posY < contactPos.getY() - POSITION_TOLERANCE ||
|
||||
posZ > contactPos.getZ() + POSITION_TOLERANCE || posZ < contactPos.getZ() - POSITION_TOLERANCE) {
|
||||
|
||||
// Return NULL
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
// The old contact positions match the current contact, therefore we return the contact caching info
|
||||
return contactInfo;
|
||||
}
|
||||
else {
|
||||
return 0;
|
||||
|
|
|
|||
|
|
@ -30,12 +30,13 @@
|
|||
#include <vector>
|
||||
#include <utility>
|
||||
#include "ContactCachingInfo.h"
|
||||
#include "../constraint/Contact.h"
|
||||
|
||||
// ReactPhysics3D namespace
|
||||
namespace reactphysics3d {
|
||||
|
||||
// Constant
|
||||
const double POSITION_TOLERANCE = 0.01 ; // Tolerance used to consider two similar positions to be considered as the same
|
||||
const double POSITION_TOLERANCE = 0.01; // Tolerance used to consider two similar positions to be considered as the same
|
||||
|
||||
/* -------------------------------------------------------------------
|
||||
Class ContactCache :
|
||||
|
|
@ -47,14 +48,15 @@ const double POSITION_TOLERANCE = 0.01 ; // Tolerance used to consider two
|
|||
*/
|
||||
class ContactCache {
|
||||
private:
|
||||
std::map<std::pair<Body*, Body*>, std::vector<ContactCachingInfo*> > cache;
|
||||
std::map<std::pair<Body*, Body*>, ContactCachingInfo* > cache;
|
||||
|
||||
public:
|
||||
ContactCache(); // Constructor
|
||||
~ContactCache(); // Destructor
|
||||
void clear(); // Remove all the contact caching info of the cache
|
||||
void addContactCachingInfo(ContactCachingInfo* contactCachingInfo); // Add a new contact caching info in the cache
|
||||
ContactCachingInfo* getContactCachingInfo(Body* body1, Body* body2, const Vector3D& position); // Return the ContactCachingInfo (if exists) corresponding to the arguments
|
||||
ContactCache(); // Constructor
|
||||
~ContactCache(); // Destructor
|
||||
void clear(); // Remove all the contact caching info of the cache
|
||||
ContactCachingInfo* getContactCachingInfo(Contact* contact) const;
|
||||
void addContactCachingInfo(ContactCachingInfo* contactCachingInfo); // Add a new contact caching info in the cache
|
||||
|
||||
};
|
||||
|
||||
} // End of the ReactPhysics3D namespace
|
||||
|
|
|
|||
|
|
@ -26,9 +26,10 @@
|
|||
#include "ContactCachingInfo.h"
|
||||
|
||||
using namespace reactphysics3d;
|
||||
using namespace std;
|
||||
|
||||
// Constructor
|
||||
ContactCachingInfo::ContactCachingInfo(Body* body1, Body* body2, const Vector3D& position, double lambda)
|
||||
: body1(body1), body2(body2), position(position), lambda(lambda) {
|
||||
ContactCachingInfo::ContactCachingInfo(Body* body1, Body* body2, const vector<Vector3D>& positions, const vector<double>& lambdas)
|
||||
: body1(body1), body2(body2), positions(positions), lambdas(lambdas) {
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -41,13 +41,12 @@ namespace reactphysics3d {
|
|||
*/
|
||||
struct ContactCachingInfo {
|
||||
public:
|
||||
// TODO : Use polymorphism here (change OBB into BoundingVolume to be more general)
|
||||
Body* body1; // Body pointer of the first bounding volume
|
||||
Body* body2; // Body pointer of the second bounding volume
|
||||
Vector3D position; // Contact point
|
||||
double lambda; // Last lambda value for the constraint
|
||||
std::vector<Vector3D> positions; // Positions of the contact points
|
||||
std::vector<double> lambdas; // Last lambdas value for the constraint
|
||||
|
||||
ContactCachingInfo(Body* body1, Body* body2, const Vector3D& position, double lambda); // Constructor
|
||||
ContactCachingInfo(Body* body1, Body* body2, const std::vector<Vector3D>& positions, const std::vector<double>& lambdas); // Constructor
|
||||
};
|
||||
|
||||
} // End of the ReactPhysics3D namespace
|
||||
|
|
|
|||
|
|
@ -470,6 +470,7 @@ Matrix Matrix::operator*(const Matrix& matrix2) const throw(MathematicsException
|
|||
|
||||
// Overloaded operator for multiplication with a vector
|
||||
Matrix Matrix::operator*(const Vector& vector) const throw(MathematicsException) {
|
||||
|
||||
// Check the sizes of the matrices
|
||||
if (nbColumn == vector.getNbComponent()) {
|
||||
Matrix result(nbRow, 1);
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user