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

git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@281 92aac97c-a6ce-11dd-a772-7fcde58d38e6

Browse Source
This commit is contained in:
chappuis.daniel 2010-02-18 22:26:48 +00:00
parent 3adbeb65ac
commit 1c6bef5b7e
2 changed files with 113 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

76
sources/reactphysics3d/constraint/Contact.cpp
View File

@ -19,6 +19,7 @@
// 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
@ -27,8 +28,8 @@ using namespace reactphysics3d;
// Constructor
Contact::Contact(Body* const body1, Body* const body2, const Vector3D& normal, double penetrationDepth, const std::vector<Vector3D>& 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<RigidBody*>(body1);
RigidBody* rigidBody2 = dynamic_cast<RigidBody*>(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);

42
sources/reactphysics3d/constraint/Contact.h
View File

@ -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<Vector3D> 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<Vector3D> 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<Vector3D>& points); // Constructor
virtual ~Contact(); // Destructor
Vector3D getNormal() const; // Return the normal vector of the contact
std::vector<Vector3D> getPoints() const; // Return the contact points
Vector3D getNormal() const; // Return the normal vector of the contact
std::vector<Vector3D> 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<Vector3D> 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