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

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

Browse Source
This commit is contained in:
chappuis.daniel 2010-04-06 16:45:48 +00:00
parent 047c2f847f
commit d15751c1de
11 changed files with 332 additions and 143 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

186
sources/reactphysics3d/collision/CollisionDetection.cpp
View File

@ -21,6 +21,7 @@
#include "CollisionDetection.h" #include "CollisionDetection.h"
#include "NoBroadPhaseAlgorithm.h" #include "NoBroadPhaseAlgorithm.h"
#include "SATAlgorithm.h" #include "SATAlgorithm.h"
#include "../body/Body.h"
#include "../body/OBB.h" #include "../body/OBB.h"
#include "../body/RigidBody.h" #include "../body/RigidBody.h"
#include <cassert> #include <cassert>
@ -29,8 +30,9 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
CollisionDetection::CollisionDetection() { CollisionDetection::CollisionDetection(PhysicsWorld* world) {
this->world = world;
// Construct the broad-phase algorithm that will be used (Separating axis with AABB) // Construct the broad-phase algorithm that will be used (Separating axis with AABB)
broadPhaseAlgorithm = new NoBroadPhaseAlgorithm(); broadPhaseAlgorithm = new NoBroadPhaseAlgorithm();
@ -44,9 +46,27 @@ CollisionDetection::~CollisionDetection() {
} }
// Compute the collision detection // Compute the collision detection
bool CollisionDetection::computeCollisionDetection(PhysicsWorld* world) { bool CollisionDetection::computeCollisionDetection() {
bool existsCollision = false; // True if a collision is found in the time interval [0, timeMax] world->removeAllContactConstraints();
possibleCollisionPairs.clear();
contactInfos.clear();
// Compute the broad-phase collision detection
computeBroadPhase();
// Compute the narrow-phase collision detection
computeNarrowPhase();
// Compute all the new contacts
computeAllContacts();
// Return true if at least one contact has been found
return (contactInfos.size() > 0);
}
// Compute the broad-phase collision detection
void CollisionDetection::computeBroadPhase() {
// For each pair of bodies in the physics world // For each pair of bodies in the physics world
for(std::vector<Body*>::const_iterator it1 = world->getBodyListStartIterator(); it1 != world->getBodyListEndIterator(); ++it1) { for(std::vector<Body*>::const_iterator it1 = world->getBodyListStartIterator(); it1 != world->getBodyListEndIterator(); ++it1) {
@ -61,20 +81,154 @@ bool CollisionDetection::computeCollisionDetection(PhysicsWorld* world) {
// Use the broad-phase algorithm to decide if the two bodies can collide // Use the broad-phase algorithm to decide if the two bodies can collide
if(broadPhaseAlgorithm->testCollisionPair(&obb1, &obb2)) { if(broadPhaseAlgorithm->testCollisionPair(&obb1, &obb2)) {
Contact* contact = 0; // If the broad-phase thinks that the two bodies collide, we add the in the possible collision pair list
possibleCollisionPairs.push_back(std::pair<const OBB*, const OBB*>(&obb1, &obb2));
// Use the narrow-phase algorithm to check if the two bodies really collide
if(narrowPhaseAlgorithm->testCollision(&obb1, &obb2, &contact)) {
assert(contact != 0);
existsCollision = true;
// Add the new collision contact into the collision world
world->addConstraint(contact);
}
} }
} }
} }
} }
}
return existsCollision;
// Compute the narrow-phase collision detection
void CollisionDetection::computeNarrowPhase() {
// For each possible collision pair of bodies
for (unsigned int i=0; i<possibleCollisionPairs.size(); i++) {
ContactInfo* contactInfo = 0;
// Use the narrow-phase collision detection algorithm to check if the really are a contact
if (narrowPhaseAlgorithm->testCollision(possibleCollisionPairs.at(i).first, possibleCollisionPairs.at(i).second, contactInfo)) {
assert(contactInfo != 0);
// Add the contact info the current list of collision informations
contactInfos.push_back(contactInfo);
}
}
}
// Compute all the contacts from the contact info list
void CollisionDetection::computeAllContacts() {
// For each possible contact info (computed during narrow-phase collision detection)
for (unsigned int i=0; i<contactInfos.size(); i++) {
ContactInfo* contactInfo = contactInfos.at(i);
assert(contactInfo != 0);
// Compute one or several new contacts and add them into the physics world
computeContact(contactInfo);
}
}
// Compute a contact (and add it to the physics world) for two colliding bodies
void CollisionDetection::computeContact(const ContactInfo* const contactInfo) {
// Extract informations from the contact info structure
const OBB* const obb1 = contactInfo->obb1;
const OBB* const obb2 = contactInfo->obb2;
Vector3D normal = contactInfo->normal;
double penetrationDepth = contactInfo->penetrationDepth;
const std::vector<Vector3D> obb1ExtremePoints = obb1->getExtremeVertices(normal);
const std::vector<Vector3D> obb2ExtremePoints = obb2->getExtremeVertices(normal.getOpposite());
unsigned int nbVerticesExtremeOBB1 = obb1ExtremePoints.size();
unsigned int nbVerticesExtremeOBB2 = obb2ExtremePoints.size();
assert(nbVerticesExtremeOBB1==1 || nbVerticesExtremeOBB1==2 || nbVerticesExtremeOBB1==4);
assert(nbVerticesExtremeOBB2==1 || nbVerticesExtremeOBB2==2 || nbVerticesExtremeOBB2==4);
assert(approxEqual(normal.length(), 1.0));
// 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)));
}
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)));
}
else if (nbVerticesExtremeOBB1 == 2 && nbVerticesExtremeOBB2 == 2) { // If it's an edge-edge contact
// Compute the two vectors of the segment lines
Vector3D d1 = obb1ExtremePoints[1] - obb1ExtremePoints[0];
Vector3D d2 = obb2ExtremePoints[1] - obb2ExtremePoints[0];
double alpha, beta;
std::vector<Vector3D> contactSet;
// If the two edges are parallel
if (d1.isParallelWith(d2)) {
Vector3D contactPointA;
Vector3D contactPointB;
// Compute the intersection between the two edges
computeParallelSegmentsIntersection(obb1ExtremePoints[0], obb1ExtremePoints[1], obb2ExtremePoints[0], obb2ExtremePoints[1],
contactPointA, contactPointB);
// Add the two contact points in the contact set
contactSet.push_back(contactPointA);
contactSet.push_back(contactPointB);
}
else { // If the two edges are not parallel
// Compute the closest two points between the two line segments
closestPointsBetweenTwoLines(obb1ExtremePoints[0], d1, obb2ExtremePoints[0], d2, &alpha, &beta);
Vector3D pointA = obb1ExtremePoints[0] + d1 * alpha;
Vector3D pointB = obb2ExtremePoints[0] + d2 * beta;
// Compute the contact point as halfway between the 2 closest points
Vector3D contactPoint = 0.5 * (pointA + pointB);
// Add the contact point into the contact set
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)));
}
}
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
std::vector<Vector3D> edge = projectPointsOntoPlane(obb1ExtremePoints, obb2ExtremePoints[0], normal);
// Clip the edge of OBB1 using the face of OBB2
std::vector<Vector3D> clippedEdge = clipSegmentWithRectangleInPlane(edge, obb2ExtremePoints);
// Move the clipped edge halway between the edge of OBB1 and the face of OBB2
clippedEdge = movePoints(clippedEdge, penetrationDepth/2.0 * normal.getOpposite());
// 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)));
}
}
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
std::vector<Vector3D> edge = projectPointsOntoPlane(obb2ExtremePoints, obb1ExtremePoints[0], normal);
// Clip the edge of OBB2 using the face of OBB1
std::vector<Vector3D> clippedEdge = clipSegmentWithRectangleInPlane(edge, obb1ExtremePoints);
// Move the clipped edge halfway between the face of OBB1 and the edge of OBB2
clippedEdge = movePoints(clippedEdge, penetrationDepth/2.0 * normal);
// 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)));
}
}
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
std::vector<Vector3D> faceOBB2 = projectPointsOntoPlane(obb2ExtremePoints, obb1ExtremePoints[0], normal);
// Clip the face of OBB2 using the face of OBB1
std::vector<Vector3D> clippedFace = clipPolygonWithRectangleInPlane(faceOBB2, obb1ExtremePoints);
// Move the clipped face halfway between the face of OBB1 and the face of OBB2
clippedFace = movePoints(clippedFace, penetrationDepth/2.0 * normal);
// 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)));
}
}
} }

34
sources/reactphysics3d/collision/CollisionDetection.h
View File

@ -25,6 +25,7 @@
#include "NarrowPhaseAlgorithm.h" #include "NarrowPhaseAlgorithm.h"
#include "../body/Body.h" #include "../body/Body.h"
#include "../engine/PhysicsWorld.h" #include "../engine/PhysicsWorld.h"
#include "ContactInfo.h"
#include <vector> #include <vector>
// ReactPhysics3D namespace // ReactPhysics3D namespace
@ -40,31 +41,26 @@ namespace reactphysics3d {
*/ */
class CollisionDetection { class CollisionDetection {
private : private :
std::vector< std::pair<Body*, Body*> > possibleCollisionPairList; // List that contains the possible collision pairs of bodies PhysicsWorld* world; // Pointer to the physics world
BroadPhaseAlgorithm* broadPhaseAlgorithm; // Broad-phase algorithm std::vector<std::pair<const OBB*, const OBB* > > possibleCollisionPairs; // Possible collision pairs of bodies (computed by broadphase)
NarrowPhaseAlgorithm* narrowPhaseAlgorithm; // Narrow-phase algorithm std::vector<ContactInfo*> contactInfos; // Contact informations (computed by narrowphase)
void addPossibleCollisionPair(Body* body1, Body* body2); // Add a possible collision pair of bodies in the possibleCollisionPairList // TODO : Check if we can avoid pointers for the two following classes (to avoid dynamic alocation)
void initPossibleCollisionPairList(); // Initialize the possibleCollisionPairList BroadPhaseAlgorithm* broadPhaseAlgorithm; // Broad-phase algorithm
NarrowPhaseAlgorithm* narrowPhaseAlgorithm; // Narrow-phase algorithm
void computeBroadPhase(); // Compute the broad-phase collision detection
void computeNarrowPhase(); // Compute the narrow-phase collision detection
void computeAllContacts(); // Compute all the contacts from the collision info list
void computeContact(const ContactInfo* const contactInfo); // Compute a contact (and add it to the physics world) for two colliding bodies
public : public :
CollisionDetection(); // Constructor CollisionDetection(PhysicsWorld* physicsWorld); // Constructor
~CollisionDetection(); // Destructor ~CollisionDetection(); // Destructor
bool computeCollisionDetection(PhysicsWorld* world); // Compute the collision detection bool computeCollisionDetection(); // Compute the collision detection
}; };
// Add a possible collision pair of bodies in the possibleCollisionPairList
inline void CollisionDetection::addPossibleCollisionPair(Body* body1, Body* body2) {
// TODO : Implement this method
}
// Initialize the possibleCollisionPairList
inline void CollisionDetection::initPossibleCollisionPairList() {
// TODO : Implement this method
}
} // End of the ReactPhysics3D namespace } // End of the ReactPhysics3D namespace
#endif #endif

4
sources/reactphysics3d/collision/NarrowPhaseAlgorithm.h
View File

@ -22,7 +22,7 @@
// Libraries // Libraries
#include "../body/BoundingVolume.h" #include "../body/BoundingVolume.h"
#include "../constraint/Contact.h" #include "ContactInfo.h"
// Namespace ReactPhysics3D // Namespace ReactPhysics3D
namespace reactphysics3d { namespace reactphysics3d {
@ -44,7 +44,7 @@ class NarrowPhaseAlgorithm {
NarrowPhaseAlgorithm(); // Constructor NarrowPhaseAlgorithm(); // Constructor
virtual ~NarrowPhaseAlgorithm(); // Destructor virtual ~NarrowPhaseAlgorithm(); // Destructor
virtual bool testCollision(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2, Contact** contact)=0; // Return true and compute a collision contact if the two bounding volume collide virtual bool testCollision(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2, ContactInfo*& contactInfo)=0; // Return true and compute a contact info if the two bounding volume collide
}; };
} // End of reactphysics3d namespace } // End of reactphysics3d namespace

27
sources/reactphysics3d/collision/SATAlgorithm.cpp
View File

@ -42,12 +42,11 @@ SATAlgorithm::~SATAlgorithm() {
} }
// Return true and compute a collision contact if the two bounding volume collide. // Return true and compute a contact info if the two bounding volume collide.
// The method returns false if there is no collision between the two bounding volumes. // The method returns false if there is no collision between the two bounding volumes.
bool SATAlgorithm::testCollision(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2, Contact** contact) { bool SATAlgorithm::testCollision(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2, ContactInfo*& contactInfo) {
assert(boundingVolume1 != boundingVolume2); assert(boundingVolume1 != boundingVolume2);
assert(*contact == 0);
// If the two bounding volumes are OBB // If the two bounding volumes are OBB
const OBB* const obb1 = dynamic_cast<const OBB* const>(boundingVolume1); const OBB* const obb1 = dynamic_cast<const OBB* const>(boundingVolume1);
@ -56,7 +55,7 @@ bool SATAlgorithm::testCollision(const BoundingVolume* const boundingVolume1, co
// If the two bounding volumes are OBB // If the two bounding volumes are OBB
if (obb1 && obb2) { if (obb1 && obb2) {
// Compute the collision test between two OBB // Compute the collision test between two OBB
return computeCollisionTest(obb1, obb2, contact); return computeCollisionTest(obb1, obb2, contactInfo);
} }
else { else {
return false; return false;
@ -64,14 +63,14 @@ bool SATAlgorithm::testCollision(const BoundingVolume* const boundingVolume1, co
} }
// This method returns true and computes a collision contact if the two OBB intersect. // This method returns true and computes a contact info if the two OBB intersect.
// This method implements the separating algorithm between two OBB. The goal of this method is to test if the // This method implements the separating algorithm between two OBB. The goal of this method is to test if the
// two OBBs intersect or not. If they intersect we report a collision contact and the method returns true. If // two OBBs intersect or not. If they intersect we report a contact info and the method returns true. If
// they don't intersect, the method returns false. The separation axis that have to be tested for two // they don't intersect, the method returns false. The separation axis that have to be tested for two
// OBB are the six face normals (3 for each OBB) and the nine vectors V = Ai x Bj where Ai is the ith face normal // OBB are the six face normals (3 for each OBB) and the nine vectors V = Ai x Bj where Ai is the ith face normal
// vector of OBB 1 and Bj is the jth face normal vector of OBB 2. We will use the notation Ai for the ith face // vector of OBB 1 and Bj is the jth face normal vector of OBB 2. We will use the notation Ai for the ith face
// normal of OBB 1 and Bj for the jth face normal of OBB 2. // normal of OBB 1 and Bj for the jth face normal of OBB 2.
bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const obb2, Contact** contact) const { bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const obb2, ContactInfo*& contactInfo) const {
double center; // Center of a projection interval double center; // Center of a projection interval
double radius1; // Radius of projection interval [min1, max1] double radius1; // Radius of projection interval [min1, max1]
@ -240,9 +239,9 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
// There exists a parallel pair of face normals and we have already checked all the face // There exists a parallel pair of face normals and we have already checked all the face
// normals for separation. Therefore the OBBs must intersect // normals for separation. Therefore the OBBs must intersect
// Compute the collision contact // Compute the contact info
computeContact(obb1, obb2, normal.getUnit(), minPenetrationDepth, obb1->getExtremeVertices(normal), obb2->getExtremeVertices(normal.getOpposite()), contact); contactInfo = new ContactInfo(obb1, obb2, normal.getUnit(), minPenetrationDepth);
return true; return true;
} }
@ -408,9 +407,9 @@ bool SATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const
normal = computeContactNormal(obb1->getAxis(2).crossProduct(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign normal = computeContactNormal(obb1->getAxis(2).crossProduct(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign
} }
// Compute the collision contact // Compute the contact info
computeContact(obb1, obb2, normal.getUnit(), minPenetrationDepth, obb1->getExtremeVertices(normal), obb2->getExtremeVertices(normal.getOpposite()), contact); contactInfo = new ContactInfo(obb1, obb2, normal.getUnit(), minPenetrationDepth);
return true; return true;
} }
@ -446,6 +445,7 @@ double SATAlgorithm::computePenetrationDepth(double min1, double max1, double mi
return penetrationDepth; return penetrationDepth;
} }
/* TODO : Remove this following code
// Compute a new collision contact between two OBBs // Compute a new collision contact between two OBBs
void SATAlgorithm::computeContact(const OBB* const obb1, const OBB* const obb2, const Vector3D normal, double penetrationDepth, void SATAlgorithm::computeContact(const OBB* const obb1, const OBB* const obb2, const Vector3D normal, double penetrationDepth,
const std::vector<Vector3D>& obb1ExtremePoints, const std::vector<Vector3D>& obb2ExtremePoints, Contact** contact) const { const std::vector<Vector3D>& obb1ExtremePoints, const std::vector<Vector3D>& obb2ExtremePoints, Contact** contact) const {
@ -544,3 +544,4 @@ void SATAlgorithm::computeContact(const OBB* const obb1, const OBB* const obb2,
assert(*contact != 0); assert(*contact != 0);
} }
*/

13
sources/reactphysics3d/collision/SATAlgorithm.h
View File

@ -44,17 +44,18 @@ namespace reactphysics3d {
*/ */
class SATAlgorithm : public NarrowPhaseAlgorithm { class SATAlgorithm : public NarrowPhaseAlgorithm {
private : private :
bool computeCollisionTest(const OBB* const obb1, const OBB* const obb2, Contact** contact) const; // Return true and compute a collision contact if the two OBB collide bool computeCollisionTest(const OBB* const obb1, const OBB* const obb2, ContactInfo*& contactInfo) const; // Return true and compute a contact info if the two OBB collide
double computePenetrationDepth(double min1, double max1, double min2, double max2, bool& side) const; // Compute the penetration depth of two projection intervals double computePenetrationDepth(double min1, double max1, double min2, double max2, bool& side) const; // Compute the penetration depth of two projection intervals
void computeContact(const OBB* const obb1, const OBB* const obb2, const Vector3D normal, double penetrationDepth, // TODO : Remove the following method
const std::vector<Vector3D>& obb1ExtremePoints, const std::vector<Vector3D>& obb2ExtremePoints, Contact** contact) const; // Compute a new contact // Compute a new collision contact between two projection intervals //void computeContact(const OBB* const obb1, const OBB* const obb2, const Vector3D normal, double penetrationDepth,
Vector3D computeContactNormal(const Vector3D& axis, const Vector3D& distanceOfOBBs) const; // Compute a contact normal // const std::vector<Vector3D>& obb1ExtremePoints, const std::vector<Vector3D>& obb2ExtremePoints, Contact** contact) const; // Compute a new contact // Compute a new collision contact between two projection intervals
Vector3D computeContactNormal(const Vector3D& axis, const Vector3D& distanceOfOBBs) const; // Compute a contact normal
public : public :
SATAlgorithm(); // Constructor SATAlgorithm(); // Constructor
~SATAlgorithm(); // Destructor ~SATAlgorithm(); // Destructor
virtual bool testCollision(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2, Contact** contact); // Return true and compute a collision contact if the two bounding volume collide virtual bool testCollision(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2, ContactInfo*& contactInfo); // Return true and compute a contact info if the two bounding volume collide
}; };
// --- Inlines function --- // // --- Inlines function --- //

46
sources/reactphysics3d/constraint/Constraint.h
View File

@ -60,21 +60,22 @@ class Constraint {
Vector auxErrorValues; // Error values for the auxiliary constraints Vector auxErrorValues; // Error values for the auxiliary constraints
public : public :
Constraint(Body* const body1, Body* const body2, unsigned int nbAuxConstraints, bool active); // Constructor // Constructor Constraint(Body* const body1, Body* const body2, unsigned int nbAuxConstraints, bool active); // Constructor // Constructor
virtual ~Constraint(); // Destructor virtual ~Constraint(); // Destructor
Body* const getBody1() const; // Return the reference to the body 1 Body* const getBody1() const; // Return the reference to the body 1
Body* const getBody2() const; // Return the reference to the body 2 Body* const getBody2() const; // Return the reference to the body 2
virtual void evaluate()=0; // Evaluate the constraint virtual void evaluate()=0; // Evaluate the constraint
bool isActive() const; // Return true if the constraint is active bool isActive() const; // Return true if the constraint is active
Matrix getBody1Jacobian() const; // Return the jacobian matrix of body 1 const Matrix& getBody1Jacobian() const; // Return the jacobian matrix of body 1
Matrix getBody2Jacobian() const; // Return the jacobian matrix of body 2 const Matrix& getBody2Jacobian() const; // Return the jacobian matrix of body 2
unsigned int getNbAuxConstraints() const; // Return the number of auxiliary constraints unsigned int getNbAuxConstraints() const; // Return the number of auxiliary constraints
void getAuxJacobian(Matrix& auxJacobian) const; // Return the jacobian matrix 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 getLowerBound() const; // Return the lower bound value of the constraint
double getUpperBound() const; // Return the upper bound value of the constraint double getUpperBound() const; // Return the upper bound value of the constraint
void getAuxLowerBounds(Vector& auxLowerBounds) const; // Return the vector of lower bounds values const Vector& getAuxLowerBounds() const; // Return the vector of lower bounds values
void getAuxUpperBounds(Vector& auxUpperBounds) const; // Return the vector of the upper 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 double getErrorValue() const; // Return the error value (bias) of the constraint
const Vector& getAuxErrorValues() const; // Return the auxiliary error values
}; };
// Return the reference to the body 1 // Return the reference to the body 1
@ -93,12 +94,12 @@ inline bool Constraint::isActive() const {
} }
// Return the jacobian matrix of body 1 // Return the jacobian matrix of body 1
inline Matrix Constraint::getBody1Jacobian() const { inline const Matrix& Constraint::getBody1Jacobian() const {
return body1Jacobian; return body1Jacobian;
} }
// Return the jacobian matrix of body 2 // Return the jacobian matrix of body 2
inline Matrix Constraint::getBody2Jacobian() const { inline const Matrix& Constraint::getBody2Jacobian() const {
return body2Jacobian; return body2Jacobian;
} }
@ -108,7 +109,7 @@ inline unsigned int Constraint::getNbAuxConstraints() const {
} }
// Return the auxiliary jacobian matrix // Return the auxiliary jacobian matrix
inline void Constraint::getAuxJacobian(Matrix& auxJacobian) const { inline const Matrix& Constraint::getAuxJacobian() const {
auxJacobian = this->auxJacobian; auxJacobian = this->auxJacobian;
} }
@ -123,12 +124,12 @@ inline double Constraint::getUpperBound() const {
} }
// Return the vector of lower bounds values // Return the vector of lower bounds values
inline void Constraint::getAuxLowerBounds(Vector& auxLowerBounds) const { inline const Vector& Constraint::getAuxLowerBounds() const {
auxLowerBounds = this->auxLowerBounds; auxLowerBounds = this->auxLowerBounds;
} }
// Return the vector of the upper bounds values // Return the vector of the upper bounds values
inline void Constraint::getAuxUpperBounds(Vector& auxUpperBounds) const { inline const Vector& Constraint::getAuxUpperBounds() const {
auxUpperBounds = this->auxUpperBounds; auxUpperBounds = this->auxUpperBounds;
} }
@ -137,6 +138,11 @@ inline double Constraint::getErrorValue() const {
return errorValue; return errorValue;
} }
// Return the auxiliary error values
inline const Vector& Constraint::getAuxErrorValues() const {
return auxErrorValues;
}
} // End of the ReactPhysics3D namespace } // End of the ReactPhysics3D namespace
#endif #endif

108
sources/reactphysics3d/engine/ConstraintSolver.cpp
View File

@ -24,10 +24,9 @@
using namespace reactphysics3d; using namespace reactphysics3d;
// Constructor // Constructor
ConstraintSolver::ConstraintSolver() ConstraintSolver::ConstraintSolver(PhysicsWorld& physicsWorld)
:bodies(0), nbBodies(0), bodyMapping(0) { :physicsWorld(physicsWorld), bodyMapping(0) , lcpSolver(LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) {
// Creation of the LCP Solver
lcpSolver = new LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS);
} }
// Destructor // Destructor
@ -36,72 +35,89 @@ ConstraintSolver::~ConstraintSolver() {
} }
// Allocate all the matrices needed to solve the LCP problem // Allocate all the matrices needed to solve the LCP problem
void ConstraintSolver::allocate(std::vector<Constraint*>& constraints, std::vector<Body*>& bodies) { void ConstraintSolver::allocate() {
unsigned int sizeJacobian = 0; unsigned int nbConstraints = 0;
this->bodies = bodies; nbBodies = physicsWorld.getBodies().size();
this->nbBodies = bodies.size();
// TODO : Now we keep every bodies of the physics world in the "bodies" std:vector of the constraint solver. // TODO : Now we keep every bodies of the physics world in the "bodies" std:vector of the constraint solver.
// but maybe we could only keep track of the body that are part of some constraints. // but maybe we could only keep track of the body that are part of some constraints.
// For each constraint // For each constraint
for (unsigned int i=0; i<constraints.size(); ++i) { for (unsigned int i=0; i<physicsWorld.getConstraints().size(); ++i) {
// Evaluate the constraint // Evaluate the constraint
constraints.at(i)->evaluate(dt); physicsWorld.getConstraints().at(i)->evaluate();
// If the constraint is active // If the constraint is active
if (constraints.at(i)->isActive()) { if (physicsWorld.getConstraints().at(i)->isActive()) {
activeConstraints.push_back(constraints.at(i)); activeConstraints.push_back(physicsWorld.getConstraints().at(i));
/Description/ Update the size of the jacobian matrix // Update the size of the jacobian matrix
sizeJacobian += (1 + constraints.at(i)->getNbAuxConstraints()); nbConstraints += (1 + physicsWorld.getConstraints().at(i)->getNbAuxConstraints());
} }
} }
// Allocate all the vectors and matrices bodyMapping = new Body**[nbConstraints];
J_sp = Matrix(sizeJacobian, 12); for (unsigned int i=0; i<nbConstraints; i++) {
bodyMapping[i] = new Body*[2];
bodyMapping = new unsigned int[nbBodies];
for (unsigned int i=0; i<nbBodies; i++) {
bodyMapping[i] = new unsigned int[2];
} }
errorVector = Vector(sizeJacobian); J_sp = Matrix(nbConstraints, 12);
B_sp = Matrix(6*nbBodies, 2; errorValues = Vector(nbConstraints);
b = Vector(totalSize); B_sp = Matrix(12, nbConstraints);
lambda = Vector(totalSize); b = Vector(nbConstraints);
lowLimits = Vector(totalSize); lambda = Vector(nbConstraints);
highLimits = Vector(totalSize); lowerBounds = Vector(nbConstraints);
Minv_sp = Matrix(3*nbBodies, 6*nbBodies); upperBounds = Vector(nbConstraints);
Minv_sp = Matrix(6*nbBodies, 6);
V = Vector(6*nbBodies); V = Vector(6*nbBodies);
Fc = Vector(6*nbBodies); Fext = Vector(6*nbBodies);
} }
// Fill in all the matrices needed to solve the LCP problem // Fill in all the matrices needed to solve the LCP problem
// Notice that all the active constraints should have been evaluated first // Notice that all the active constraints should have been evaluated first
void ConstraintSolver::fillInMatrices() { void ConstraintSolver::fillInMatrices() {
int i,j;
// For each active constraint // For each active constraint
for (unsigned int c=0; c<activeConstraints.size(); ++c) { for (unsigned int c=0; c<activeConstraints.size(); c++) {
i = activeConstraints.at(c)->getJacobianIndex(); Constraint* constraint = activeConstraints.at(c);
//j = i + activeConstraint.at(c)->getNbJacobianRows();
J.fillInSubMatrix(i, 0, activeConstraints.at(c)->getBody1LinearJacobian()); // Fill in the J_sp matrix
J.fillInSubMatrix(i, 3, activeConstraints.at(c)->getBody2LinearJacobian()); J_sp.fillInSubMatrix(c, 0, constraint->getBody1Jacobian());
J.fillInSubMatrix(i, 6, activeConstraints.at(c)->getBody1AngularJacobian()); J_sp.fillInSubMatrix(c, 6, constraint->getBody2Jacobian());
J.fillInSubMatrix(i, 9, activeConstraints.at(c)->getBody2AngularJacobian());
errorVector.fillInSubVector(i, activeConstraints.at(c)->getErrorVector()); // Fill in the body mapping matrix
bodyMapping[c][0] = constraint->getBody1();
bodyMapping[c][1] = constraint->getBody2();
// Fill in the limit vectors for the constraint
lowerBounds.fillInSubVector(c, constraint->getLowerBound());
upperBounds.fillInSubVector(c, constraint->getUpperBound());
// Fill in the error vector
errorValues.fillInSubVector(c, constraint->getErrorValue());
unsigned int nbAuxConstraints = constraint->getNbAuxConstraints();
// If the current constraint has auxiliary constraints
if (nbAuxConstraints > 0) {
// Fill in the J_sp matrix
J_sp.fillInSubMatrix(c+1, 0, constraint->getAuxJacobian());
// For each auxiliary constraints
for (unsigned int i=1; i<nbAuxConstraints; i++) {
// Fill in the body mapping matrix
bodyMapping[c+i][0] = constraint->getBody1();
bodyMapping[c+i][1] = constraint->getBody2();
}
// Fill in the limit vectors for the auxilirary constraints
lowerBounds.fillInSubVector(c+1, constraint->getAuxLowerBounds());
upperBounds.fillInSubVector(c+1, constraint->getAuxUpperBounds());
}
} }
// For each active constraint // For each current body of the physics world
for (unsigned int c=0; c<activeConstraints.size(); ++c) { for (unsigned int b=0; b<physicsWorld->getBodies().size(); b++) {
i = activeConstraints.at(c)->getNbAuxiliaryVars();
// TODO : add activeConstraints.at(c)->getAuxiliaryRowsAndCols(..., ...)
b.fillInSubVector(i, activeConstraints.at(c)->getRightHandSideVector());
}
// For each current body of the simulation
for (unsigned int b=0; b<nbBodies; ++b) {
i = 6*b; i = 6*b;
Minv.fillInSubMatrix(i, 0, bodies.at(b)->getCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3)); Minv.fillInSubMatrix(i, 0, bodies.at(b)->getCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3));
Minv.fillInSubMatrix(i+3, 3, bodies.at(b)->getCurrentBodyState().getInertiaTensorInverse()); Minv.fillInSubMatrix(i+3, 3, bodies.at(b)->getCurrentBodyState().getInertiaTensorInverse());

23
sources/reactphysics3d/engine/ConstraintSolver.h
View File

@ -22,6 +22,8 @@
// Libraries // Libraries
#include "../constraint/Constraint.h" #include "../constraint/Constraint.h"
#include "PhysicsWorld.h"
#include <map>
// ReactPhysics3D namespace // ReactPhysics3D namespace
namespace reactphysics3d { namespace reactphysics3d {
@ -37,11 +39,12 @@ const unsigned int MAX_LCP_ITERATIONS = 10; // Maximum number of iterations
*/ */
class ConstraintSolver { class ConstraintSolver {
protected: protected:
LCPSolver* lcpSolver; // LCP Solver LCPSolver& lcpSolver; // LCP Solver
PhysicsWorld& physicsWorld; // Reference to the physics world
std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world std::vector<Constraint*> activeConstraints; // Current active constraints in the physics world
std::vector<Body*> bodies; // Set of bodies in the physics world unsigned int nbBodies; // Current number of bodies in the physics world
unsigned int nbBodies; // Number of bodies in the physics world std::map<Body*, unsigned int> bodyNumberMapping; // Map a body pointer with its index number
unsigned int** bodyMapping // 2-dimensional array that contains the mapping of body index Body** bodyMapping; // 2-dimensional array that contains the mapping of body reference
// in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains // in the J_sp and B_sp matrices. For instance the cell bodyMapping[i][j] contains
// the integer index of the body that correspond to the 1x6 J_ij matrix in the // the integer index of the body that correspond to the 1x6 J_ij matrix in the
// J_sp matrix. A integer body index refers to its index in the "bodies" std::vector // J_sp matrix. A integer body index refers to its index in the "bodies" std::vector
@ -49,13 +52,13 @@ class ConstraintSolver {
Matrix B_sp; // Useful matrix in sparse representation Matrix B_sp; // Useful matrix in sparse representation
Vector b; // Vector "b" of the LCP problem Vector b; // Vector "b" of the LCP problem
Vector lambda; // Lambda vector of the LCP problem Vector lambda; // Lambda vector of the LCP problem
Vector errorVector; // Error vector of all constraints Vector errorValues; // Error vector of all constraints
Vector lowLimits; // Vector that contains the low limits of the LCP problem Vector lowerBounds; // Vector that contains the low limits for the variables of the LCP problem
Vector highLimits; // Vector that contains the high limits of the LCP problem Vector upperBounds; // Vector that contains the high limits for the variables of the LCP problem
Matrix Minv_sp; // Sparse representation of the Matrix that contains information about mass and inertia of each body Matrix Minv_sp; // Sparse representation of the Matrix that contains information about mass and inertia of each body
Vector V; // Vector that contains internal linear and angular velocities of each body Vector V; // Vector that contains linear and angular velocities of each body
Vector Fc; // Vector that contains internal forces and torques of each body due to constraints Vector Fext; // Vector that contains external forces and torques of each body
void allocate(std::vector<Constraint*>& constraints); // Allocate all the matrices needed to solve the LCP problem void allocate(); // Allocate all the matrices needed to solve the LCP problem
void fillInMatrices(); // Fill in all the matrices needed to solve the LCP problem void fillInMatrices(); // Fill in all the matrices needed to solve the LCP problem
void freeMemory(); // Free the memory that was allocated in the allocate() method void freeMemory(); // Free the memory that was allocated in the allocate() method

7
sources/reactphysics3d/engine/PhysicsEngine.cpp
View File

@ -26,7 +26,7 @@ using namespace reactphysics3d;
// Constructor // Constructor
PhysicsEngine::PhysicsEngine(PhysicsWorld* world, const Time& timeStep) throw (std::invalid_argument) PhysicsEngine::PhysicsEngine(PhysicsWorld* world, const Time& timeStep) throw (std::invalid_argument)
: world(world), timer(Time(0.0), timeStep) { : world(world), timer(Time(0.0), timeStep), collisionDetection(world) {
// Check if the pointer to the world is not NULL // Check if the pointer to the world is not NULL
if (world == 0) { if (world == 0) {
// Throw an exception // Throw an exception
@ -87,11 +87,8 @@ void PhysicsEngine::updateCollision() {
// While the time accumulator is not empty // While the time accumulator is not empty
while(timer.getAccumulator() >= timer.getTimeStep().getValue()) { while(timer.getAccumulator() >= timer.getTimeStep().getValue()) {
// Remove all old collision contact constraints
world->removeAllContactConstraints();
// Compute the collision detection // Compute the collision detection
if (collisionDetection.computeCollisionDetection(world)) { if (collisionDetection.computeCollisionDetection()) {
// TODO : Delete this ---------------------------------------------------------- // TODO : Delete this ----------------------------------------------------------
for (std::vector<Constraint*>::const_iterator it = world->getConstraintListStartIterator(); it != world->getConstraintListEndIterator(); ++it) { for (std::vector<Constraint*>::const_iterator it = world->getConstraintListStartIterator(); it != world->getConstraintListEndIterator(); ++it) {

24
sources/reactphysics3d/engine/PhysicsWorld.h
View File

@ -40,8 +40,8 @@ namespace reactphysics3d {
*/ */
class PhysicsWorld { class PhysicsWorld {
protected : protected :
std::vector<Body*> bodyList; // list that contains all bodies of the physics world std::vector<Body*> bodies; // list that contains all bodies of the physics world
std::vector<Constraint*> constraintList; // List that contains all the current constraints std::vector<Constraint*> constraints; // List that contains all the current constraints
Vector3D gravity; // Gravity vector of the world Vector3D gravity; // Gravity vector of the world
bool isGravityOn; // True if the gravity force is on bool isGravityOn; // True if the gravity force is on
@ -61,6 +61,8 @@ class PhysicsWorld {
void removeAllContactConstraints(); // Remove all collision contacts constraints void removeAllContactConstraints(); // Remove all collision contacts constraints
std::vector<Constraint*>::const_iterator getConstraintListStartIterator() const; // Return a start iterator on the constraint list std::vector<Constraint*>::const_iterator getConstraintListStartIterator() const; // Return a start iterator on the constraint list
std::vector<Constraint*>::const_iterator getConstraintListEndIterator() const; // Return a end iterator on the constraint list std::vector<Constraint*>::const_iterator getConstraintListEndIterator() const; // Return a end iterator on the constraint list
std::vector<Body*>& getBodies() const; // Return a reference to the bodies of the physics world
std::vector<Constraint*>& getConstraints() const; // Return a reference to the constraints of the physics world
}; };
// --- Inline functions --- // // --- Inline functions --- //
@ -73,13 +75,13 @@ inline Vector3D PhysicsWorld::getGravity() const {
// Return a start iterator on the body list // Return a start iterator on the body list
inline std::vector<Body*>::const_iterator PhysicsWorld::getBodyListStartIterator() const { inline std::vector<Body*>::const_iterator PhysicsWorld::getBodyListStartIterator() const {
// Return an iterator on the start of the body list // Return an iterator on the start of the body list
return bodyList.begin(); return bodies.begin();
} }
// Return a end iterator on the body list // Return a end iterator on the body list
inline std::vector<Body*>::const_iterator PhysicsWorld::getBodyListEndIterator() const { inline std::vector<Body*>::const_iterator PhysicsWorld::getBodyListEndIterator() const {
// Return an iterator on the end of the body list // Return an iterator on the end of the body list
return bodyList.end(); return bodies.end();
} }
// Return if the gravity is on // Return if the gravity is on
@ -94,12 +96,22 @@ inline void PhysicsWorld::setIsGratityOn(bool isGravityOn) {
// Return a start iterator on the constraint list // Return a start iterator on the constraint list
inline std::vector<Constraint*>::const_iterator PhysicsWorld::getConstraintListStartIterator() const { inline std::vector<Constraint*>::const_iterator PhysicsWorld::getConstraintListStartIterator() const {
return constraintList.begin(); return constraints.begin();
} }
// Return a end iterator on the constraint list // Return a end iterator on the constraint list
inline std::vector<Constraint*>::const_iterator PhysicsWorld::getConstraintListEndIterator() const { inline std::vector<Constraint*>::const_iterator PhysicsWorld::getConstraintListEndIterator() const {
return constraintList.end(); return constraints.end();
}
// Return a reference to the bodies of the physics world
std::vector<Body*>& PhysicsWorld::getBodies() const {
return bodies;
}
// Return a reference to the constraints of the physics world
std::vector<Constraint*>& PhysicsWorld::getConstraints() const {
return constraints;
} }
} // End of the ReactPhysics3D namespace } // End of the ReactPhysics3D namespace

3
sources/reactphysics3d/engine/Timer.h
View File

@ -28,6 +28,9 @@
// Namespace ReactPhysics3D // Namespace ReactPhysics3D
namespace reactphysics3d { namespace reactphysics3d {
// TODO : Now we are using the "Time" class in this class but we should a
// time class from the standard library
/* ------------------------------------------------------------------- /* -------------------------------------------------------------------
Class Timer : Class Timer :
This class will take care of the time in the physics engine. This class will take care of the time in the physics engine.