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

Changes in the Contact class

Browse Source 
git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@436 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel 2011-08-10 16:49:38 +00:00
parent d9452e727c
commit 03557bbff6
6 changed files with 112 additions and 145 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/collision/CollisionDetection.cpp
View File

@ -120,13 +120,8 @@ void CollisionDetection::computeAllContacts() {
void CollisionDetection::computeContactGJK(const ContactInfo* const contactInfo) {
// TODO : Compute PersisentContact here instead
// Compute the set of contact points
vector<Vector3D> contactPoints;
contactPoints.push_back(contactInfo->point1);
contactPoints.push_back(contactInfo->point2);
// Create a new contact
Contact* contact = new Contact(contactInfo->body1, contactInfo->body2, contactInfo->normal, contactInfo->penetrationDepth, contactPoints);
Contact* contact = new Contact(contactInfo);
// Add the contact to the physics world
world->addConstraint(contact);

2
src/constants.h
View File

@ -47,7 +47,7 @@ const double DEFAULT_TIMESTEP = 0.002;
const double OBJECT_MARGIN = 0.04; // Object margin for collision detection
// Contact constants
const double FRICTION_COEFFICIENT = 0.4; // Friction coefficient
const double FRICTION_COEFFICIENT = 1.0; // Friction coefficient
const double PENETRATION_FACTOR = 0.2; // Penetration factor (between 0 and 1) which specify the importance of the
// penetration depth in order to calculate the correct impulse for the contact

75
src/constraint/Contact.cpp
View File

@ -29,8 +29,9 @@ using namespace reactphysics3d;
using namespace std;
// Constructor
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()) {
Contact::Contact(const ContactInfo* contactInfo)
: Constraint(contactInfo->body1, contactInfo->body2, 3, true), normal(contactInfo->normal), penetrationDepth(contactInfo->penetrationDepth),
pointOnBody1(contactInfo->point1), pointOnBody2(contactInfo->point2) {
// Compute the auxiliary lower and upper bounds
// TODO : Now mC is only the mass of the first body but it is probably wrong
@ -55,28 +56,16 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const {
assert(body1);
assert(body2);
Vector3D r1;
Vector3D r2;
Vector3D r1CrossN;
Vector3D r2CrossN;
Vector3D r1CrossU1;
Vector3D r2CrossU1;
Vector3D r1CrossU2;
Vector3D r2CrossU2;
Vector3D body1Position = body1->getTransform().getPosition();
Vector3D body2Position = body2->getTransform().getPosition();
int currentIndex = noConstraint; // Current constraint index
// For each point in the contact
for (int i=0; i<nbPoints; i++) {
r1 = points[i] - body1Position;
r2 = points[i] - body2Position;
r1CrossN = r1.cross(normal);
r2CrossN = r2.cross(normal);
Vector3D r1 = pointOnBody1 - body1Position;
Vector3D r2 = pointOnBody2 - body2Position;
Vector3D r1CrossN = r1.cross(normal);
Vector3D r2CrossN = r2.cross(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, -normal.getX());
J_sp[currentIndex][0].setValue(1, -normal.getY());
J_sp[currentIndex][0].setValue(2, -normal.getZ());
@ -85,7 +74,6 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const {
J_sp[currentIndex][0].setValue(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, normal.getX());
J_sp[currentIndex][1].setValue(1, normal.getY());
J_sp[currentIndex][1].setValue(2, normal.getZ());
@ -96,11 +84,10 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const {
currentIndex++;
// Compute the jacobian matrix for the body 1 for the first friction constraint
r1CrossU1 = r1.cross(frictionVectors[0]);
r2CrossU1 = r2.cross(frictionVectors[0]);
r1CrossU2 = r1.cross(frictionVectors[1]);
r2CrossU2 = r2.cross(frictionVectors[1]);
//J_sp[currentIndex][0].changeSize(1, 6);
Vector3D r1CrossU1 = r1.cross(frictionVectors[0]);
Vector3D r2CrossU1 = r2.cross(frictionVectors[0]);
Vector3D r1CrossU2 = r1.cross(frictionVectors[1]);
Vector3D 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());
@ -109,7 +96,6 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const {
J_sp[currentIndex][0].setValue(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, frictionVectors[0].getX());
J_sp[currentIndex][1].setValue(1, frictionVectors[0].getY());
J_sp[currentIndex][1].setValue(2, frictionVectors[0].getZ());
@ -120,14 +106,12 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const {
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, -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][1].changeSize(1, 6);
// Compute the jacobian matrix for the body 2 for the second friction constraint
J_sp[currentIndex][1].setValue(0, frictionVectors[1].getX());
@ -136,43 +120,28 @@ void Contact::computeJacobian(int noConstraint, Matrix1x6**& J_sp) const {
J_sp[currentIndex][1].setValue(3, r2CrossU2.getX());
J_sp[currentIndex][1].setValue(4, r2CrossU2.getY());
J_sp[currentIndex][1].setValue(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;
}
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
}
// 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;
}
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
}
// Compute the error values for all the mathematical constraints. The argument
@ -184,7 +153,6 @@ void Contact::computeErrorValue(int noConstraint, Vector& errorValues) const {
RigidBody* rigidBody1 = dynamic_cast<RigidBody*>(body1);
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(body2);
int index = noConstraint;
assert(noConstraint >= 0 && noConstraint + nbConstraints <= errorValues.getNbComponent());
@ -195,10 +163,7 @@ void Contact::computeErrorValue(int noConstraint, Vector& errorValues) const {
double errorValue = restitutionCoeff * (normal.dot(velocity1) - normal.dot(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;
}
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
}

28
src/constraint/Contact.h
View File

@ -27,6 +27,7 @@
// Libraries
#include "Constraint.h"
#include "../collision/ContactInfo.h"
#include "../body/RigidBody.h"
#include "../constants.h"
#include "../mathematics/mathematics.h"
@ -43,8 +44,8 @@ namespace reactphysics3d {
This class represents a collision contact between two bodies in
the physics engine. The contact class inherits from the
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
and contains the two contact points on each body. The contact has 3
mathematical constraints (1 for the contact constraint, and 2
for the friction constraints).
-------------------------------------------------------------------
*/
@ -52,20 +53,20 @@ class Contact : public Constraint {
protected :
const Vector3D normal; // Normal vector of the contact (From body1 toward body2)
const double penetrationDepth; // Penetration depth
const std::vector<Vector3D> points; // Contact points between the two bodies
const int nbPoints; // Number of points in the contact
const Vector3D pointOnBody1; // Contact point on body 1
const Vector3D pointOnBody2; // Contact point on body 2
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 std::vector<Vector3D>& points); // Constructor
Contact(const ContactInfo* contactInfo); // Constructor
virtual ~Contact(); // Destructor
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
Vector3D getPointOnBody1() const; // Return the contact point on body 1
Vector3D getPointOnBody2() const; // Return the contact 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
@ -95,15 +96,14 @@ inline Vector3D Contact::getNormal() const {
return normal;
}
// Return a contact points
inline Vector3D Contact::getPoint(int index) const {
assert(index >= 0 && index < nbPoints);
return points[index];
// Return the contact point on body 1
inline Vector3D Contact::getPointOnBody1() const {
return pointOnBody1;
}
// Return the number of contact points
inline int Contact::getNbPoints() const {
return nbPoints;
// Return the contact point on body 2
inline Vector3D Contact::getPointOnBody2() const {
return pointOnBody2;
}
// Return the penetration depth of the contact

5
src/engine/ConstraintSolver.cpp
View File

@ -353,9 +353,8 @@ void ConstraintSolver::updateContactCache() {
// 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));
}
points.push_back(contact->getPointOnBody1());
points.push_back(contact->getPointOnBody2());
// For each constraint of the contact
for (int i=0; i<contact->getNbConstraints(); i++) {

38
src/engine/ContactCache.cpp
View File

@ -65,30 +65,38 @@ ContactCachingInfo* ContactCache::getContactCachingInfo(Contact* contact) const
assert((*entry).first.first == contact->getBody1());
assert((*entry).first.second == contact->getBody2());
// 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 NULL;
}
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();
posX = contact->getPointOnBody1().getX();
posY = contact->getPointOnBody1().getY();
posZ = contact->getPointOnBody1().getZ();
// Get the position of the old contact
Vector3D& contactPos = contactInfo->positions[i];
Vector3D& contactPos1 = contactInfo->positions[0];
// 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) {
if (posX > contactPos1.getX() + POSITION_TOLERANCE || posX < contactPos1.getX() - POSITION_TOLERANCE ||
posY > contactPos1.getY() + POSITION_TOLERANCE || posY < contactPos1.getY() - POSITION_TOLERANCE ||
posZ > contactPos1.getZ() + POSITION_TOLERANCE || posZ < contactPos1.getZ() - POSITION_TOLERANCE) {
// Return NULL
return NULL;
}
// Get the position of the current contact
posX = contact->getPointOnBody2().getX();
posY = contact->getPointOnBody2().getY();
posZ = contact->getPointOnBody2().getZ();
// Get the position of the old contact
Vector3D& contactPos2 = contactInfo->positions[1];
// If the old contact point doesn't match the current one
if (posX > contactPos2.getX() + POSITION_TOLERANCE || posX < contactPos2.getX() - POSITION_TOLERANCE ||
posY > contactPos2.getY() + POSITION_TOLERANCE || posY < contactPos2.getY() - POSITION_TOLERANCE ||
posZ > contactPos2.getZ() + POSITION_TOLERANCE || posZ < contactPos2.getZ() - POSITION_TOLERANCE) {
// Return NULL
return NULL;
}
// The old contact positions match the current contact, therefore we return the contact caching info