From d15751c1de4c683e43b68a33fb0b0ce66b870171 Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Tue, 6 Apr 2010 16:45:48 +0000 Subject: [PATCH] git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@303 92aac97c-a6ce-11dd-a772-7fcde58d38e6 --- .../collision/CollisionDetection.cpp | 186 ++++++++++++++++-- .../collision/CollisionDetection.h | 34 ++-- .../collision/NarrowPhaseAlgorithm.h | 4 +- .../reactphysics3d/collision/SATAlgorithm.cpp | 27 +-- .../reactphysics3d/collision/SATAlgorithm.h | 13 +- .../reactphysics3d/constraint/Constraint.h | 46 +++-- .../engine/ConstraintSolver.cpp | 108 +++++----- .../reactphysics3d/engine/ConstraintSolver.h | 23 ++- .../reactphysics3d/engine/PhysicsEngine.cpp | 7 +- sources/reactphysics3d/engine/PhysicsWorld.h | 24 ++- sources/reactphysics3d/engine/Timer.h | 3 + 11 files changed, 332 insertions(+), 143 deletions(-) diff --git a/sources/reactphysics3d/collision/CollisionDetection.cpp b/sources/reactphysics3d/collision/CollisionDetection.cpp index dcf4ab20..05dcf859 100644 --- a/sources/reactphysics3d/collision/CollisionDetection.cpp +++ b/sources/reactphysics3d/collision/CollisionDetection.cpp @@ -21,6 +21,7 @@ #include "CollisionDetection.h" #include "NoBroadPhaseAlgorithm.h" #include "SATAlgorithm.h" +#include "../body/Body.h" #include "../body/OBB.h" #include "../body/RigidBody.h" #include @@ -29,8 +30,9 @@ using namespace reactphysics3d; // Constructor -CollisionDetection::CollisionDetection() { - +CollisionDetection::CollisionDetection(PhysicsWorld* world) { + this->world = world; + // Construct the broad-phase algorithm that will be used (Separating axis with AABB) broadPhaseAlgorithm = new NoBroadPhaseAlgorithm(); @@ -44,9 +46,27 @@ CollisionDetection::~CollisionDetection() { } // 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(std::vector::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 if(broadPhaseAlgorithm->testCollisionPair(&obb1, &obb2)) { - Contact* contact = 0; - - // 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); - } + // If the broad-phase thinks that the two bodies collide, we add the in the possible collision pair list + possibleCollisionPairs.push_back(std::pair(&obb1, &obb2)); } } } } - - return existsCollision; +} + +// Compute the narrow-phase collision detection +void CollisionDetection::computeNarrowPhase() { + // For each possible collision pair of bodies + for (unsigned int i=0; itestCollision(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; iobb1; + const OBB* const obb2 = contactInfo->obb2; + Vector3D normal = contactInfo->normal; + double penetrationDepth = contactInfo->penetrationDepth; + + const std::vector obb1ExtremePoints = obb1->getExtremeVertices(normal); + const std::vector 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 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; iaddConstraint(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 edge = projectPointsOntoPlane(obb1ExtremePoints, obb2ExtremePoints[0], normal); + + // Clip the edge of OBB1 using the face of OBB2 + std::vector 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; iaddConstraint(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 edge = projectPointsOntoPlane(obb2ExtremePoints, obb1ExtremePoints[0], normal); + + // Clip the edge of OBB2 using the face of OBB1 + std::vector 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; iaddConstraint(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 faceOBB2 = projectPointsOntoPlane(obb2ExtremePoints, obb1ExtremePoints[0], normal); + + // Clip the face of OBB2 using the face of OBB1 + std::vector 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; iaddConstraint(new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), normal, penetrationDepth, clippedFace.at(i))); + } + } } diff --git a/sources/reactphysics3d/collision/CollisionDetection.h b/sources/reactphysics3d/collision/CollisionDetection.h index cb066b30..5b3fb6b2 100644 --- a/sources/reactphysics3d/collision/CollisionDetection.h +++ b/sources/reactphysics3d/collision/CollisionDetection.h @@ -25,6 +25,7 @@ #include "NarrowPhaseAlgorithm.h" #include "../body/Body.h" #include "../engine/PhysicsWorld.h" +#include "ContactInfo.h" #include // ReactPhysics3D namespace @@ -40,31 +41,26 @@ namespace reactphysics3d { */ class CollisionDetection { private : - std::vector< std::pair > possibleCollisionPairList; // List that contains the possible collision pairs of bodies - BroadPhaseAlgorithm* broadPhaseAlgorithm; // Broad-phase algorithm - NarrowPhaseAlgorithm* narrowPhaseAlgorithm; // Narrow-phase algorithm + PhysicsWorld* world; // Pointer to the physics world + std::vector > possibleCollisionPairs; // Possible collision pairs of bodies (computed by broadphase) + std::vector contactInfos; // Contact informations (computed by narrowphase) - void addPossibleCollisionPair(Body* body1, Body* body2); // Add a possible collision pair of bodies in the possibleCollisionPairList - void initPossibleCollisionPairList(); // Initialize the possibleCollisionPairList + // TODO : Check if we can avoid pointers for the two following classes (to avoid dynamic alocation) + 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 : - CollisionDetection(); // Constructor - ~CollisionDetection(); // Destructor + CollisionDetection(PhysicsWorld* physicsWorld); // Constructor + ~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 #endif diff --git a/sources/reactphysics3d/collision/NarrowPhaseAlgorithm.h b/sources/reactphysics3d/collision/NarrowPhaseAlgorithm.h index d9de9c06..61b6b84f 100644 --- a/sources/reactphysics3d/collision/NarrowPhaseAlgorithm.h +++ b/sources/reactphysics3d/collision/NarrowPhaseAlgorithm.h @@ -22,7 +22,7 @@ // Libraries #include "../body/BoundingVolume.h" -#include "../constraint/Contact.h" +#include "ContactInfo.h" // Namespace ReactPhysics3D namespace reactphysics3d { @@ -44,7 +44,7 @@ class NarrowPhaseAlgorithm { NarrowPhaseAlgorithm(); // Constructor 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 diff --git a/sources/reactphysics3d/collision/SATAlgorithm.cpp b/sources/reactphysics3d/collision/SATAlgorithm.cpp index c442996c..1a75e63f 100644 --- a/sources/reactphysics3d/collision/SATAlgorithm.cpp +++ b/sources/reactphysics3d/collision/SATAlgorithm.cpp @@ -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. -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(*contact == 0); // If the two bounding volumes are OBB const OBB* const obb1 = dynamic_cast(boundingVolume1); @@ -56,7 +55,7 @@ bool SATAlgorithm::testCollision(const BoundingVolume* const boundingVolume1, co // If the two bounding volumes are OBB if (obb1 && obb2) { // Compute the collision test between two OBB - return computeCollisionTest(obb1, obb2, contact); + return computeCollisionTest(obb1, obb2, contactInfo); } else { 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 -// 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 // 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 // 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 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 // normals for separation. Therefore the OBBs must intersect - // Compute the collision contact - computeContact(obb1, obb2, normal.getUnit(), minPenetrationDepth, obb1->getExtremeVertices(normal), obb2->getExtremeVertices(normal.getOpposite()), contact); - + // Compute the contact info + contactInfo = new ContactInfo(obb1, obb2, normal.getUnit(), minPenetrationDepth); + 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 } - // Compute the collision contact - computeContact(obb1, obb2, normal.getUnit(), minPenetrationDepth, obb1->getExtremeVertices(normal), obb2->getExtremeVertices(normal.getOpposite()), contact); - + // Compute the contact info + contactInfo = new ContactInfo(obb1, obb2, normal.getUnit(), minPenetrationDepth); + return true; } @@ -446,6 +445,7 @@ double SATAlgorithm::computePenetrationDepth(double min1, double max1, double mi return penetrationDepth; } +/* TODO : Remove this following code // Compute a new collision contact between two OBBs void SATAlgorithm::computeContact(const OBB* const obb1, const OBB* const obb2, const Vector3D normal, double penetrationDepth, const std::vector& obb1ExtremePoints, const std::vector& obb2ExtremePoints, Contact** contact) const { @@ -544,3 +544,4 @@ void SATAlgorithm::computeContact(const OBB* const obb1, const OBB* const obb2, assert(*contact != 0); } +*/ diff --git a/sources/reactphysics3d/collision/SATAlgorithm.h b/sources/reactphysics3d/collision/SATAlgorithm.h index fcf7842e..79677aed 100644 --- a/sources/reactphysics3d/collision/SATAlgorithm.h +++ b/sources/reactphysics3d/collision/SATAlgorithm.h @@ -44,17 +44,18 @@ namespace reactphysics3d { */ class SATAlgorithm : public NarrowPhaseAlgorithm { 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 - 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, - const std::vector& obb1ExtremePoints, const std::vector& 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 + 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 + // TODO : Remove the following method + //void computeContact(const OBB* const obb1, const OBB* const obb2, const Vector3D normal, double penetrationDepth, + // const std::vector& obb1ExtremePoints, const std::vector& 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 : SATAlgorithm(); // Constructor ~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 --- // diff --git a/sources/reactphysics3d/constraint/Constraint.h b/sources/reactphysics3d/constraint/Constraint.h index 5f61492a..cc651c3f 100644 --- a/sources/reactphysics3d/constraint/Constraint.h +++ b/sources/reactphysics3d/constraint/Constraint.h @@ -60,21 +60,22 @@ class Constraint { Vector auxErrorValues; // Error values for the auxiliary constraints public : - Constraint(Body* const body1, Body* const body2, unsigned int 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 - virtual void evaluate()=0; // Evaluate the constraint - bool isActive() const; // Return true if the constraint is active - Matrix getBody1Jacobian() const; // Return the jacobian matrix of body 1 - Matrix getBody2Jacobian() const; // Return the jacobian matrix of body 2 - unsigned int getNbAuxConstraints() const; // Return the number of auxiliary constraints - void getAuxJacobian(Matrix& auxJacobian) const; // Return the jacobian matrix of auxiliary constraints - double getLowerBound() const; // Return the lower 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 - void getAuxUpperBounds(Vector& auxUpperBounds) const; // Return the vector of the upper bounds values - double getErrorValue() const; // Return the error value (bias) of the constraint + Constraint(Body* const body1, Body* const body2, unsigned int 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 + virtual void evaluate()=0; // Evaluate the constraint + bool isActive() const; // Return true if the constraint is active + const Matrix& getBody1Jacobian() const; // Return the jacobian matrix of body 1 + const Matrix& getBody2Jacobian() const; // Return the jacobian matrix of body 2 + unsigned int getNbAuxConstraints() const; // Return the number 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 getUpperBound() const; // Return the upper bound value of the constraint + const Vector& getAuxLowerBounds() const; // Return the vector of lower 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 + const Vector& getAuxErrorValues() const; // Return the auxiliary error values }; // Return the reference to the body 1 @@ -93,12 +94,12 @@ inline bool Constraint::isActive() const { } // Return the jacobian matrix of body 1 -inline Matrix Constraint::getBody1Jacobian() const { +inline const Matrix& Constraint::getBody1Jacobian() const { return body1Jacobian; } // Return the jacobian matrix of body 2 -inline Matrix Constraint::getBody2Jacobian() const { +inline const Matrix& Constraint::getBody2Jacobian() const { return body2Jacobian; } @@ -108,7 +109,7 @@ inline unsigned int Constraint::getNbAuxConstraints() const { } // Return the auxiliary jacobian matrix -inline void Constraint::getAuxJacobian(Matrix& auxJacobian) const { +inline const Matrix& Constraint::getAuxJacobian() const { auxJacobian = this->auxJacobian; } @@ -123,12 +124,12 @@ inline double Constraint::getUpperBound() const { } // Return the vector of lower bounds values -inline void Constraint::getAuxLowerBounds(Vector& auxLowerBounds) const { +inline const Vector& Constraint::getAuxLowerBounds() const { auxLowerBounds = this->auxLowerBounds; } // Return the vector of the upper bounds values -inline void Constraint::getAuxUpperBounds(Vector& auxUpperBounds) const { +inline const Vector& Constraint::getAuxUpperBounds() const { auxUpperBounds = this->auxUpperBounds; } @@ -137,6 +138,11 @@ inline double Constraint::getErrorValue() const { return errorValue; } +// Return the auxiliary error values +inline const Vector& Constraint::getAuxErrorValues() const { + return auxErrorValues; +} + } // End of the ReactPhysics3D namespace #endif diff --git a/sources/reactphysics3d/engine/ConstraintSolver.cpp b/sources/reactphysics3d/engine/ConstraintSolver.cpp index 43f1fdf2..7a7de234 100644 --- a/sources/reactphysics3d/engine/ConstraintSolver.cpp +++ b/sources/reactphysics3d/engine/ConstraintSolver.cpp @@ -24,10 +24,9 @@ using namespace reactphysics3d; // Constructor -ConstraintSolver::ConstraintSolver() - :bodies(0), nbBodies(0), bodyMapping(0) { - // Creation of the LCP Solver - lcpSolver = new LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS); +ConstraintSolver::ConstraintSolver(PhysicsWorld& physicsWorld) + :physicsWorld(physicsWorld), bodyMapping(0) , lcpSolver(LCPProjectedGaussSeidel(MAX_LCP_ITERATIONS)) { + } // Destructor @@ -36,72 +35,89 @@ ConstraintSolver::~ConstraintSolver() { } // Allocate all the matrices needed to solve the LCP problem -void ConstraintSolver::allocate(std::vector& constraints, std::vector& bodies) { - unsigned int sizeJacobian = 0; - this->bodies = bodies; - this->nbBodies = bodies.size(); +void ConstraintSolver::allocate() { + unsigned int nbConstraints = 0; + nbBodies = physicsWorld.getBodies().size(); // 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. // For each constraint - for (unsigned int i=0; ievaluate(dt); + physicsWorld.getConstraints().at(i)->evaluate(); // If the constraint is active - if (constraints.at(i)->isActive()) { - activeConstraints.push_back(constraints.at(i)); + if (physicsWorld.getConstraints().at(i)->isActive()) { + activeConstraints.push_back(physicsWorld.getConstraints().at(i)); - /Description/ Update the size of the jacobian matrix - sizeJacobian += (1 + constraints.at(i)->getNbAuxConstraints()); + // Update the size of the jacobian matrix + nbConstraints += (1 + physicsWorld.getConstraints().at(i)->getNbAuxConstraints()); } } - // Allocate all the vectors and matrices - J_sp = Matrix(sizeJacobian, 12); - - bodyMapping = new unsigned int[nbBodies]; - for (unsigned int i=0; igetJacobianIndex(); - //j = i + activeConstraint.at(c)->getNbJacobianRows(); - J.fillInSubMatrix(i, 0, activeConstraints.at(c)->getBody1LinearJacobian()); - J.fillInSubMatrix(i, 3, activeConstraints.at(c)->getBody2LinearJacobian()); - J.fillInSubMatrix(i, 6, activeConstraints.at(c)->getBody1AngularJacobian()); - J.fillInSubMatrix(i, 9, activeConstraints.at(c)->getBody2AngularJacobian()); - errorVector.fillInSubVector(i, activeConstraints.at(c)->getErrorVector()); + for (unsigned int c=0; cgetBody1Jacobian()); + J_sp.fillInSubMatrix(c, 6, constraint->getBody2Jacobian()); + + // 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; igetBody1(); + 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 (unsigned int c=0; cgetNbAuxiliaryVars(); - // 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; bgetBodies().size(); b++) { i = 6*b; Minv.fillInSubMatrix(i, 0, bodies.at(b)->getCurrentBodyState().getMassInverse().getValue() * Matrix::identity(3)); Minv.fillInSubMatrix(i+3, 3, bodies.at(b)->getCurrentBodyState().getInertiaTensorInverse()); diff --git a/sources/reactphysics3d/engine/ConstraintSolver.h b/sources/reactphysics3d/engine/ConstraintSolver.h index 078c4bb9..6fa64b74 100644 --- a/sources/reactphysics3d/engine/ConstraintSolver.h +++ b/sources/reactphysics3d/engine/ConstraintSolver.h @@ -22,6 +22,8 @@ // Libraries #include "../constraint/Constraint.h" +#include "PhysicsWorld.h" +#include // ReactPhysics3D namespace namespace reactphysics3d { @@ -37,11 +39,12 @@ const unsigned int MAX_LCP_ITERATIONS = 10; // Maximum number of iterations */ class ConstraintSolver { protected: - LCPSolver* lcpSolver; // LCP Solver + LCPSolver& lcpSolver; // LCP Solver + PhysicsWorld& physicsWorld; // Reference to the physics world std::vector activeConstraints; // Current active constraints in the physics world - std::vector bodies; // Set of bodies in the physics world - unsigned int nbBodies; // Number of bodies in the physics world - unsigned int** bodyMapping // 2-dimensional array that contains the mapping of body index + unsigned int nbBodies; // Current number of bodies in the physics world + std::map bodyNumberMapping; // Map a body pointer with its index number + 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 // 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 @@ -49,13 +52,13 @@ class ConstraintSolver { Matrix B_sp; // Useful matrix in sparse representation Vector b; // Vector "b" of the LCP problem Vector lambda; // Lambda vector of the LCP problem - Vector errorVector; // Error vector of all constraints - Vector lowLimits; // Vector that contains the low limits of the LCP problem - Vector highLimits; // Vector that contains the high limits of the LCP problem + Vector errorValues; // Error vector of all constraints + Vector lowerBounds; // Vector that contains the low limits for the variables 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 - Vector V; // Vector that contains internal linear and angular velocities of each body - Vector Fc; // Vector that contains internal forces and torques of each body due to constraints - void allocate(std::vector& constraints); // Allocate all the matrices needed to solve the LCP problem + Vector V; // Vector that contains linear and angular velocities of each body + Vector Fext; // Vector that contains external forces and torques of each body + 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 freeMemory(); // Free the memory that was allocated in the allocate() method diff --git a/sources/reactphysics3d/engine/PhysicsEngine.cpp b/sources/reactphysics3d/engine/PhysicsEngine.cpp index 4b9d84c8..d9a40cf0 100644 --- a/sources/reactphysics3d/engine/PhysicsEngine.cpp +++ b/sources/reactphysics3d/engine/PhysicsEngine.cpp @@ -26,7 +26,7 @@ using namespace reactphysics3d; // Constructor 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 if (world == 0) { // Throw an exception @@ -87,11 +87,8 @@ void PhysicsEngine::updateCollision() { // While the time accumulator is not empty while(timer.getAccumulator() >= timer.getTimeStep().getValue()) { - // Remove all old collision contact constraints - world->removeAllContactConstraints(); - // Compute the collision detection - if (collisionDetection.computeCollisionDetection(world)) { + if (collisionDetection.computeCollisionDetection()) { // TODO : Delete this ---------------------------------------------------------- for (std::vector::const_iterator it = world->getConstraintListStartIterator(); it != world->getConstraintListEndIterator(); ++it) { diff --git a/sources/reactphysics3d/engine/PhysicsWorld.h b/sources/reactphysics3d/engine/PhysicsWorld.h index fe9dfa9d..4e1ce8ec 100644 --- a/sources/reactphysics3d/engine/PhysicsWorld.h +++ b/sources/reactphysics3d/engine/PhysicsWorld.h @@ -40,8 +40,8 @@ namespace reactphysics3d { */ class PhysicsWorld { protected : - std::vector bodyList; // list that contains all bodies of the physics world - std::vector constraintList; // List that contains all the current constraints + std::vector bodies; // list that contains all bodies of the physics world + std::vector constraints; // List that contains all the current constraints Vector3D gravity; // Gravity vector of the world bool isGravityOn; // True if the gravity force is on @@ -61,6 +61,8 @@ class PhysicsWorld { void removeAllContactConstraints(); // Remove all collision contacts constraints std::vector::const_iterator getConstraintListStartIterator() const; // Return a start iterator on the constraint list std::vector::const_iterator getConstraintListEndIterator() const; // Return a end iterator on the constraint list + std::vector& getBodies() const; // Return a reference to the bodies of the physics world + std::vector& getConstraints() const; // Return a reference to the constraints of the physics world }; // --- Inline functions --- // @@ -73,13 +75,13 @@ inline Vector3D PhysicsWorld::getGravity() const { // Return a start iterator on the body list inline std::vector::const_iterator PhysicsWorld::getBodyListStartIterator() const { // Return an iterator on the start of the body list - return bodyList.begin(); + return bodies.begin(); } // Return a end iterator on the body list inline std::vector::const_iterator PhysicsWorld::getBodyListEndIterator() const { // Return an iterator on the end of the body list - return bodyList.end(); + return bodies.end(); } // Return if the gravity is on @@ -94,12 +96,22 @@ inline void PhysicsWorld::setIsGratityOn(bool isGravityOn) { // Return a start iterator on the constraint list inline std::vector::const_iterator PhysicsWorld::getConstraintListStartIterator() const { - return constraintList.begin(); + return constraints.begin(); } // Return a end iterator on the constraint list inline std::vector::const_iterator PhysicsWorld::getConstraintListEndIterator() const { - return constraintList.end(); + return constraints.end(); +} + +// Return a reference to the bodies of the physics world +std::vector& PhysicsWorld::getBodies() const { + return bodies; +} + +// Return a reference to the constraints of the physics world +std::vector& PhysicsWorld::getConstraints() const { + return constraints; } } // End of the ReactPhysics3D namespace diff --git a/sources/reactphysics3d/engine/Timer.h b/sources/reactphysics3d/engine/Timer.h index 107712a8..76684b9f 100644 --- a/sources/reactphysics3d/engine/Timer.h +++ b/sources/reactphysics3d/engine/Timer.h @@ -28,6 +28,9 @@ // 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 : This class will take care of the time in the physics engine.