From 1c6bef5b7e0a156d56cde8c0473d608b98a801c1 Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Thu, 18 Feb 2010 22:26:48 +0000 Subject: [PATCH] git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@281 92aac97c-a6ce-11dd-a772-7fcde58d38e6 --- sources/reactphysics3d/constraint/Contact.cpp | 76 ++++++++++++++++++- sources/reactphysics3d/constraint/Contact.h | 42 +++++++++- 2 files changed, 113 insertions(+), 5 deletions(-) diff --git a/sources/reactphysics3d/constraint/Contact.cpp b/sources/reactphysics3d/constraint/Contact.cpp index c504962d..980e098a 100644 --- a/sources/reactphysics3d/constraint/Contact.cpp +++ b/sources/reactphysics3d/constraint/Contact.cpp @@ -19,6 +19,7 @@ // Libraries #include "Contact.h" +#include "../body/RigidBody.h" #include // TODO : Remove this in the final version #include // TODO : Remove this in the final version @@ -27,8 +28,8 @@ using namespace reactphysics3d; // Constructor Contact::Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const std::vector& points) - :Constraint(body1, body2), normal(normal), penetrationDepth(penetrationDepth), points(points){ - + :Constraint(body1, body2), normal(normal), penetrationDepth(penetrationDepth), points(points), nbFrictionVectors(NB_FRICTION_VECTORS) { + active = true; } // Destructor @@ -36,6 +37,77 @@ Contact::~Contact() { } +// Evaluate the constraint +// This method computes the jacobian matrices of the constraint +// The argument "dt" is the time step of the physics simulation +void Contact::evaluate(double dt) { + RigidBody* rigidBody1 = dynamic_cast(body1); + RigidBody* rigidBody2 = dynamic_cast(body2); + + assert(frictionVectors.size() == NB_FRICTION_VECTORS); + assert(rigidBody1 != 0); + assert(rigidBody2 != 0); + + // Computation of the linear jacobian for body 1 + body1LinearJacobian = Matrix(1 + NB_FRICTION_VECTORS, 3); + body1LinearJacobian.setValue(0, 0, -normal.getX()); + body1LinearJacobian.setValue(0, 1, -normal.getY()); + body1LinearJacobian.setValue(0, 2, -normal.getZ()); + for (unsigned int i=1; i<=NB_FRICTION_VECTORS; ++i) { + body1LinearJacobian.setValue(i, 0, -frictionVectors.at(i-1).getX()); + body1LinearJacobian.setValue(i, 1, -frictionVectors.at(i-1).getY()); + body1LinearJacobian.setValue(i, 2, -frictionVectors.at(i-1).getZ()); + } + + // Computation of the linear jacobian for body 2 + body2LinearJacobian = Matrix(1 + NB_FRICTION_VECTORS, 3); + body2LinearJacobian.setValue(0, 0, normal.getX()); + body2LinearJacobian.setValue(0, 1, normal.getY()); + body2LinearJacobian.setValue(0, 2, normal.getZ()); + for (unsigned int i=1; i<=NB_FRICTION_VECTORS; ++i) { + body2LinearJacobian.setValue(i, 0, frictionVectors.at(i-1).getX()); + body2LinearJacobian.setValue(i, 1, frictionVectors.at(i-1).getY()); + body2LinearJacobian.setValue(i, 2, frictionVectors.at(i-1).getZ()); + } + + // Computation of the angular jacobian for body 1 + Vector3D r1 = rigidBody1->getCurrentBodyState().getPosition(); + Vector3D r1CrossNormal = r1.crossProduct(normal); + body1AngularJacobian = Matrix(1 + NB_FRICTION_VECTORS, 3); + body1AngularJacobian.setValue(0, 0, -r1CrossNormal.getX()); + body1AngularJacobian.setValue(0, 1, -r1CrossNormal.getY()); + body1AngularJacobian.setValue(0, 2, -r1CrossNormal.getZ()); + for (unsigned int i=1; i<=NB_FRICTION_VECTORS; ++i) { + Vector3D r1CrossFrictionVector = r1.crossProduct(frictionVectors.at(i-1)); + body1AngularJacobian.setValue(i, 0, -r1CrossFrictionVector.getX()); + body1AngularJacobian.setValue(i, 1, -r1CrossFrictionVector.getY()); + body1AngularJacobian.setValue(i, 2, -r1CrossFrictionVector.getZ()); + } + + // Computation of the angular jacobian for body 2 + Vector3D r2 = rigidBody2->getCurrentBodyState().getPosition(); + Vector3D r2CrossNormal = r2.crossProduct(normal); + body2AngularJacobian = Matrix(1 + NB_FRICTION_VECTORS, 3); + body2AngularJacobian.setValue(0, 0, r2CrossNormal.getX()); + body2AngularJacobian.setValue(0, 1, r2CrossNormal.getY()); + body2AngularJacobian.setValue(0, 2, r2CrossNormal.getZ()); + for (unsigned int i=1; i<=NB_FRICTION_VECTORS; ++i) { + Vector3D r2CrossFrictionVector = r2.crossProduct(frictionVectors.at(i-1)); + body2AngularJacobian.setValue(i, 0, r2CrossFrictionVector.getX()); + body2AngularJacobian.setValue(i, 1, r2CrossFrictionVector.getY()); + body2AngularJacobian.setValue(i, 2, r2CrossFrictionVector.getZ()); + } + + // Computation of the error vector + double kFps = 1.0 / dt; + double kCorrection = kErp * kFps; // Computation of the error coefficient + errorVector = Vector(1+NB_FRICTION_VECTORS); + errorVector.setValue(0, kCorrection * penetrationDepth); + for (unsigned int i=1; i<=NB_FRICTION_VECTORS; ++i) { + errorVector.setValue(i, 0.0); + } +} + // TODO : Delete this (Used to debug collision detection) void Contact::draw() const { glColor3f(1.0, 0.0, 0.0); diff --git a/sources/reactphysics3d/constraint/Contact.h b/sources/reactphysics3d/constraint/Contact.h index c6de5569..12840496 100644 --- a/sources/reactphysics3d/constraint/Contact.h +++ b/sources/reactphysics3d/constraint/Contact.h @@ -27,6 +27,9 @@ // ReactPhysics3D namespace namespace reactphysics3d { +// Constants +const unsigned int NB_FRICTION_VECTORS = 4; // Number of vectors used to describe the contact friction + /* ------------------------------------------------------------------- Class Contact : This class represents a collision contact between two bodies in @@ -37,21 +40,44 @@ namespace reactphysics3d { ------------------------------------------------------------------- */ class Contact : public Constraint { - private : + protected : Vector3D normal; // Normal vector of the contact (From body1 toward body2) double penetrationDepth; // Penetration depth std::vector points; // Contact points + unsigned int nbFrictionVectors; // Number of vectors used to describe the friction + // TODO : Implement the computation of the frictionVectors in the collision detection + std::vector frictionVectors; // Set of vectors that span the tangential friction plane + + void computeFrictionVectors(const Vector3D& vector1, const Vector3D& vector2); // Compute all the friction vectors from two vectors that span the friction plane public : Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const std::vector& points); // Constructor virtual ~Contact(); // Destructor - Vector3D getNormal() const; // Return the normal vector of the contact - std::vector getPoints() const; // Return the contact points + Vector3D getNormal() const; // Return the normal vector of the contact + std::vector getPoints() const; // Return the contact points + virtual unsigned int getNbJacobianRows() const; // Return the number of rows of the Jacobian matrix + virtual void evaluate(double dt); // Evaluate the constraint + virtual int getNbAuxiliaryVars() const; // Return the number of auxiliary variables void draw() const; // TODO : Delete this (Used to debug collision detection) }; +// Compute all the friction vectors from two vectors that span the friction cone +// The vectors "vector1" and "vector2" are two vectors that span the friction cone +inline void Contact::computeFrictionVectors(const Vector3D& vector1, const Vector3D& vector2) { + // Delete the current friction vectors + frictionVectors.clear(); + + Vector3D vector; + + // Compute each friction vector + for (unsigned int i=1; i<=NB_FRICTION_VECTORS; ++i) { + vector = cos((2.0 * (i-1) * PI) / NB_FRICTION_VECTORS) * vector1 + sin((2.0 * (i-1) * PI) / NB_FRICTION_VECTORS) * vector2; + frictionVectors.push_back(vector); + } +} + // Return the normal vector of the contact inline Vector3D Contact::getNormal() const { return normal; @@ -62,6 +88,16 @@ inline std::vector Contact::getPoints() const { return points; } +// Return the number of rows of the Jacobian matrix +inline unsigned int Contact::getNbJacobianRows() const { + return (1+nbFrictionVectors); +} + +// Return the number of auxiliary variables +inline int Contact::getNbAuxiliaryVars() const { + return nbFrictionVectors; +} + } // End of the ReactPhysics3D namespace