git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@303 92aac97c-a6ce-11dd-a772-7fcde58d38e6
This commit is contained in:
chappuis.daniel
parent
047c2f847f
commit
d15751c1de
|
|
@ -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 <cassert>
|
||||
|
|
@ -29,7 +30,8 @@
|
|||
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<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
|
||||
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<const OBB*, const OBB*>(&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; 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)));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -25,6 +25,7 @@
|
|||
#include "NarrowPhaseAlgorithm.h"
|
||||
#include "../body/Body.h"
|
||||
#include "../engine/PhysicsWorld.h"
|
||||
#include "ContactInfo.h"
|
||||
#include <vector>
|
||||
|
||||
// ReactPhysics3D namespace
|
||||
|
|
@ -40,31 +41,26 @@ namespace reactphysics3d {
|
|||
*/
|
||||
class CollisionDetection {
|
||||
private :
|
||||
std::vector< std::pair<Body*, Body*> > possibleCollisionPairList; // List that contains the possible collision pairs of bodies
|
||||
PhysicsWorld* world; // Pointer to the physics world
|
||||
std::vector<std::pair<const OBB*, const OBB* > > possibleCollisionPairs; // Possible collision pairs of bodies (computed by broadphase)
|
||||
std::vector<ContactInfo*> contactInfos; // Contact informations (computed by narrowphase)
|
||||
|
||||
// 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 addPossibleCollisionPair(Body* body1, Body* body2); // Add a possible collision pair of bodies in the possibleCollisionPairList
|
||||
void initPossibleCollisionPairList(); // Initialize the possibleCollisionPairList
|
||||
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(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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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<const OBB* const>(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,8 +239,8 @@ 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,8 +407,8 @@ 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<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);
|
||||
}
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -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
|
||||
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
|
||||
void 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; // Compute a new contact // Compute a new collision contact between 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<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 :
|
||||
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 --- //
|
||||
|
|
|
|||
|
|
@ -66,15 +66,16 @@ class Constraint {
|
|||
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
|
||||
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
|
||||
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 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
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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<Constraint*>& constraints, std::vector<Body*>& 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; i<constraints.size(); ++i) {
|
||||
for (unsigned int i=0; i<physicsWorld.getConstraints().size(); ++i) {
|
||||
// Evaluate the constraint
|
||||
constraints.at(i)->evaluate(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; i<nbBodies; i++) {
|
||||
bodyMapping[i] = new unsigned int[2];
|
||||
bodyMapping = new Body**[nbConstraints];
|
||||
for (unsigned int i=0; i<nbConstraints; i++) {
|
||||
bodyMapping[i] = new Body*[2];
|
||||
}
|
||||
|
||||
errorVector = Vector(sizeJacobian);
|
||||
B_sp = Matrix(6*nbBodies, 2;
|
||||
b = Vector(totalSize);
|
||||
lambda = Vector(totalSize);
|
||||
lowLimits = Vector(totalSize);
|
||||
highLimits = Vector(totalSize);
|
||||
Minv_sp = Matrix(3*nbBodies, 6*nbBodies);
|
||||
J_sp = Matrix(nbConstraints, 12);
|
||||
errorValues = Vector(nbConstraints);
|
||||
B_sp = Matrix(12, nbConstraints);
|
||||
b = Vector(nbConstraints);
|
||||
lambda = Vector(nbConstraints);
|
||||
lowerBounds = Vector(nbConstraints);
|
||||
upperBounds = Vector(nbConstraints);
|
||||
Minv_sp = Matrix(6*nbBodies, 6);
|
||||
V = Vector(6*nbBodies);
|
||||
Fc = Vector(6*nbBodies);
|
||||
Fext = Vector(6*nbBodies);
|
||||
}
|
||||
|
||||
// Fill in all the matrices needed to solve the LCP problem
|
||||
// Notice that all the active constraints should have been evaluated first
|
||||
void ConstraintSolver::fillInMatrices() {
|
||||
int i,j;
|
||||
|
||||
// For each active constraint
|
||||
for (unsigned int c=0; c<activeConstraints.size(); ++c) {
|
||||
i = activeConstraints.at(c)->getJacobianIndex();
|
||||
//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; c<activeConstraints.size(); c++) {
|
||||
Constraint* constraint = activeConstraints.at(c);
|
||||
|
||||
// Fill in the J_sp matrix
|
||||
J_sp.fillInSubMatrix(c, 0, constraint->getBody1Jacobian());
|
||||
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; i<nbAuxConstraints; i++) {
|
||||
// Fill in the body mapping matrix
|
||||
bodyMapping[c+i][0] = constraint->getBody1();
|
||||
bodyMapping[c+i][1] = constraint->getBody2();
|
||||
}
|
||||
|
||||
// For each active constraint
|
||||
for (unsigned int c=0; c<activeConstraints.size(); ++c) {
|
||||
i = activeConstraints.at(c)->getNbAuxiliaryVars();
|
||||
// TODO : add activeConstraints.at(c)->getAuxiliaryRowsAndCols(..., ...)
|
||||
b.fillInSubVector(i, activeConstraints.at(c)->getRightHandSideVector());
|
||||
// Fill in the limit vectors for the auxilirary constraints
|
||||
lowerBounds.fillInSubVector(c+1, constraint->getAuxLowerBounds());
|
||||
upperBounds.fillInSubVector(c+1, constraint->getAuxUpperBounds());
|
||||
}
|
||||
}
|
||||
|
||||
// For each current body of the simulation
|
||||
for (unsigned int b=0; b<nbBodies; ++b) {
|
||||
// For each current body of the physics world
|
||||
for (unsigned int b=0; b<physicsWorld->getBodies().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());
|
||||
|
|
|
|||
|
|
@ -22,6 +22,8 @@
|
|||
|
||||
// Libraries
|
||||
#include "../constraint/Constraint.h"
|
||||
#include "PhysicsWorld.h"
|
||||
#include <map>
|
||||
|
||||
// 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<Constraint*> activeConstraints; // Current active constraints in the physics world
|
||||
std::vector<Body*> 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<Body*, unsigned int> 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<Constraint*>& 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
|
||||
|
||||
|
|
|
|||
|
|
@ -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<Constraint*>::const_iterator it = world->getConstraintListStartIterator(); it != world->getConstraintListEndIterator(); ++it) {
|
||||
|
|
|
|||
|
|
@ -40,8 +40,8 @@ namespace reactphysics3d {
|
|||
*/
|
||||
class PhysicsWorld {
|
||||
protected :
|
||||
std::vector<Body*> bodyList; // list that contains all bodies of the physics world
|
||||
std::vector<Constraint*> constraintList; // List that contains all the current constraints
|
||||
std::vector<Body*> bodies; // list that contains all bodies of the physics world
|
||||
std::vector<Constraint*> 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<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<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 --- //
|
||||
|
|
@ -73,13 +75,13 @@ inline Vector3D PhysicsWorld::getGravity() const {
|
|||
// Return a start iterator on the body list
|
||||
inline std::vector<Body*>::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<Body*>::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<Constraint*>::const_iterator PhysicsWorld::getConstraintListStartIterator() const {
|
||||
return constraintList.begin();
|
||||
return constraints.begin();
|
||||
}
|
||||
|
||||
// Return a end iterator on the constraint list
|
||||
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
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
|
|
|||
Loading…
Reference in New Issue
Block a user