From 951725a401fd4c759e37a4fa29d63f4e6cd53a86 Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Tue, 21 Jul 2009 20:29:37 +0000 Subject: [PATCH] git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@177 92aac97c-a6ce-11dd-a772-7fcde58d38e6 --- .../collision/BroadPhaseAlgorithm.h | 2 +- .../collision/CollisionDetection.cpp | 45 +- .../collision/CollisionDetection.h | 2 +- .../collision/NarrowPhaseAlgorithm.h | 5 +- .../collision/SeparatingAxisAABB.cpp | 5 +- .../collision/SeparatingAxisAABB.h | 10 +- .../collision/SeparatingAxisOBB.cpp | 456 +++++++++++++++++- .../collision/SeparatingAxisOBB.h | 24 +- 8 files changed, 538 insertions(+), 11 deletions(-) diff --git a/sources/reactphysics3d/collision/BroadPhaseAlgorithm.h b/sources/reactphysics3d/collision/BroadPhaseAlgorithm.h index 909fd5be..9d3961e3 100644 --- a/sources/reactphysics3d/collision/BroadPhaseAlgorithm.h +++ b/sources/reactphysics3d/collision/BroadPhaseAlgorithm.h @@ -46,7 +46,7 @@ class BroadPhaseAlgorithm { BroadPhaseAlgorithm(); // Constructor virtual ~BroadPhaseAlgorithm(); // Destructor - virtual bool testCollisionPair(const BoundingVolume& boundingVolume1, const BoundingVolume& boundingVolume2)=0; // Return true if the two bounding volume can collide + virtual bool testCollisionPair(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2)=0; // Return true if the two bounding volume can collide }; } // End of reactphysics3d namespace diff --git a/sources/reactphysics3d/collision/CollisionDetection.cpp b/sources/reactphysics3d/collision/CollisionDetection.cpp index 866ab0c6..82bfe28b 100644 --- a/sources/reactphysics3d/collision/CollisionDetection.cpp +++ b/sources/reactphysics3d/collision/CollisionDetection.cpp @@ -21,6 +21,9 @@ #include "CollisionDetection.h" #include "SeparatingAxisAABB.h" #include "SeparatingAxisOBB.h" +#include "../body/OBB.h" +#include "../body/RigidBody.h" +#include // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; @@ -50,7 +53,43 @@ void CollisionDetection::computeCollisionContacts() { // TODO : Implement this method } -// Compute the collision detection -void CollisionDetection::computeCollisionDetection(CollisionWorld& collisionWorld) { - // TODO : Implement this method +// Compute the collision detection for the time interval [0, timeMax] +// The method returns true if a collision occurs in the time interval [0, timeMax] +bool CollisionDetection::computeCollisionDetection(CollisionWorld* collisionWorld, const Time& timeMax, Time& timeFirst, Time& timeLast) { + + bool existsCollision = false; // True if a collision is found in the time interval [0, timeMax] + + // For each pair of bodies in the collisionWorld + for(std::vector::const_iterator it1 = collisionWorld->getBodyListStartIterator(); it1 != collisionWorld->getBodyListEndIterator(); ++it1) { + for(std::vector::const_iterator it2 = it1; it2 != collisionWorld->getBodyListEndIterator(); ++it2) { + // If both bodies are RigidBody and are different + RigidBody* rigidBody1 = dynamic_cast(*it1); + RigidBody* rigidBody2 = dynamic_cast(*it2); + if(rigidBody1 && rigidBody2 && rigidBody1 != rigidBody2) { + // Get the oriented bounding boxes of the two bodies + OBB obb1 = rigidBody1->getOBB(); + OBB obb2 = rigidBody2->getOBB(); + + // Use the broad-phase algorithm to decide if the two bodies can collide + if(broadPhaseAlgorithm->testCollisionPair(&obb1, &obb2)) { + Contact* contact = 0; + + // Get the velocities of both bodies + Vector3D velocity1 = rigidBody1->getInterpolatedState().getLinearVelocity(); + Vector3D velocity2 = rigidBody2->getInterpolatedState().getLinearVelocity(); + + // Use the narrow-phase algorithm to check if the two bodies really collide + if(narrowPhaseAlgorithm->testCollision(&obb1, &obb2, &contact, velocity1, velocity2, timeMax, timeFirst, timeLast)) { + assert(contact != 0); + existsCollision = true; + + // Add the new collision contact into the collision world + collisionWorld->addConstraint(contact); + } + } + } + } + } + + return existsCollision; } diff --git a/sources/reactphysics3d/collision/CollisionDetection.h b/sources/reactphysics3d/collision/CollisionDetection.h index eaa44625..cf8ebdd7 100644 --- a/sources/reactphysics3d/collision/CollisionDetection.h +++ b/sources/reactphysics3d/collision/CollisionDetection.h @@ -53,7 +53,7 @@ class CollisionDetection { CollisionDetection(); // Constructor ~CollisionDetection(); // Destructor - void computeCollisionDetection(CollisionWorld& collisionWorld); // Compute the collision detection + bool computeCollisionDetection(CollisionWorld* collisionWorld, const Time& timeMax, Time& timeFirst, Time& timeLast); // Compute the collision detection }; // Add a possible collision pair of bodies in the possibleCollisionPairList diff --git a/sources/reactphysics3d/collision/NarrowPhaseAlgorithm.h b/sources/reactphysics3d/collision/NarrowPhaseAlgorithm.h index 53093cda..727cd300 100644 --- a/sources/reactphysics3d/collision/NarrowPhaseAlgorithm.h +++ b/sources/reactphysics3d/collision/NarrowPhaseAlgorithm.h @@ -41,7 +41,10 @@ class NarrowPhaseAlgorithm { public : NarrowPhaseAlgorithm(); // Constructor virtual ~NarrowPhaseAlgorithm(); // Destructor - virtual bool testCollision(const BoundingVolume& boundingVolume1, const BoundingVolume& boundingVolume2, Contact* const 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, + Contact** contact, const Vector3D& velocity1, const Vector3D& velocity2, + const Time& timeMax, Time& timeFirst, Time& timeLast)=0; // Return true and compute a collision contact and collision time if the two bounding volume collide }; } // End of reactphysics3d namespace diff --git a/sources/reactphysics3d/collision/SeparatingAxisAABB.cpp b/sources/reactphysics3d/collision/SeparatingAxisAABB.cpp index fda69dc4..722d44da 100644 --- a/sources/reactphysics3d/collision/SeparatingAxisAABB.cpp +++ b/sources/reactphysics3d/collision/SeparatingAxisAABB.cpp @@ -34,7 +34,10 @@ SeparatingAxisAABB::~SeparatingAxisAABB() { } // Return true if the two AABB of the bodies intersect -bool SeparatingAxisAABB::testCollisionPair(const BoundingVolume& boundingVolume1, const BoundingVolume& boundingVolume2) { +bool SeparatingAxisAABB::testCollisionPair(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2) { // TODO : Implement this method + + // At this time this method returns true to indicate that all body pairs can collide + return true; } diff --git a/sources/reactphysics3d/collision/SeparatingAxisAABB.h b/sources/reactphysics3d/collision/SeparatingAxisAABB.h index 7c9a6ab5..057e1947 100644 --- a/sources/reactphysics3d/collision/SeparatingAxisAABB.h +++ b/sources/reactphysics3d/collision/SeparatingAxisAABB.h @@ -27,6 +27,14 @@ // ReactPhysics3D namespace namespace reactphysics3d { +/* ------------------------------------------------------------------- + Class SeparatingAxisAABB : + This class implements a broad-phase collision detection + algorithm. This algorithm is uses a separating axis technique + with axis aligned bounding box (AABB) to check if two bounding + volumes can colide or not. + ------------------------------------------------------------------- +*/ class SeparatingAxisAABB : public BroadPhaseAlgorithm { private : @@ -34,7 +42,7 @@ class SeparatingAxisAABB : public BroadPhaseAlgorithm { SeparatingAxisAABB(); // Constructor ~SeparatingAxisAABB(); // Destructor - virtual bool testCollisionPair(const BoundingVolume& boundingVolume1, const BoundingVolume& boundingVolume2); // Return true if the two AABB of the bodies intersect + virtual bool testCollisionPair(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2); // Return true if the two AABB of the bodies intersect }; } // End of the ReactPhysics3D namespace diff --git a/sources/reactphysics3d/collision/SeparatingAxisOBB.cpp b/sources/reactphysics3d/collision/SeparatingAxisOBB.cpp index 6a9970d8..d98b76b0 100644 --- a/sources/reactphysics3d/collision/SeparatingAxisOBB.cpp +++ b/sources/reactphysics3d/collision/SeparatingAxisOBB.cpp @@ -20,6 +20,10 @@ // Libraries #include "SeparatingAxisOBB.h" #include "../body/OBB.h" +#include "../constraint/Contact.h" +#include +#include // TODO : Delete this +#include // We want to use the ReactPhysics3D namespace using namespace reactphysics3d; @@ -36,6 +40,454 @@ SeparatingAxisOBB::~SeparatingAxisOBB() { // Return true and compute a collision contact if the two bounding volume collide. // The method returns false if there is no collision between the two bounding volumes. -bool SeparatingAxisOBB::testCollision(const BoundingVolume& boundingVolume1, const BoundingVolume& boundingVolume2, Contact* const contact) { - // TODO : Implement this method +bool SeparatingAxisOBB::testCollision(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2, Contact** contact, + const Vector3D& velocity1, const Vector3D& velocity2, const Time& timeMax, Time& timeFirst, Time& timeLast) { + assert(boundingVolume1 != boundingVolume2); + + // If the two bounding volumes are OBB + const OBB* const obb1 = dynamic_cast(boundingVolume1); + const OBB* const obb2 = dynamic_cast(boundingVolume2); + + // If the two bounding volumes are OBB + if (obb1 && obb2) { + // Compute the collision test between two OBB + return computeCollisionTest(obb1, obb2, contact, velocity1, velocity2, timeMax, timeFirst, timeLast); + } + else { + return false; + } +} + +// Return true and compute a collision contact if the two OBB collide. +// This method implements the separating algorithm between two OBB. The goal of this method is to compute the +// time (in the interval [0, timeMax] at wich the two bodies will collide if they will collide. If they will +// collide we report a collision contact. "velocity1" and "velocity2" are the velocity vectors of the two bodies. +// If they collide, timeFirst will contain the first collision time of the two bodies and timeLast will contain +// the time when the two bodies separate after the collision. 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 SeparatingAxisOBB::computeCollisionTest(const OBB* const obb1, const OBB* const obb2, Contact** contact, + const Vector3D& velocity1, const Vector3D& velocity2, const Time& timeMax, + Time& timeFirst, Time& timeLast) { + + double center; // Center + double speed; // Relavtive speed of the projection intervals (dotProduct(SeparatingAxis, deltaVelocity)) + double radius1; // Radius of projection interval [min1, max1] + double radius2; // Radius of projection interval [min2, max2] + double min1; // Minimum of interval 1 + double max1; // Maximum of interval 1 + double min2; // Minimm of interval 2 + double max2; // Maximum of interval 2 + const double cutoff = 0.999999; // Cutoff for cosine of angles between box axes + bool existsParallelPair = false; // True if there exists two face normals that are parallel. + // This is used because if a parallel pair exists, it is sufficient + // to test only the face normals of the OBBs for separation. Two nearly + // parallel faces can lead to all face normal tests reporting no separation + // along those directions. The cross product directions are tested next, but + // Ai x Bj is nearly the zero vector and can cause a report that the two OBBs + // are not intersecting when in fact they are. + double c[3][3]; // c[i][j] = DotProduct(obb1.Ai, obb2.Bj) + double absC[3][3]; // absC[i][j] = abs(DotProduct(obb1.Ai, obb2.Bj)) + double udc1[3]; // DotProduct(obb1.Ai, obb2.center - obb1.center) + double udv1[3]; // DotProduct(obb1.Ai, velocity2 - velocity1) + double udc2[3]; // DotProduct(obb2.Ai, obb2.center - obb1.center) + double udv2[3]; // DotProduct(obb2.Ai, velocity2 - velocity1) + + Vector3D deltaVelocity = velocity2-velocity1; // Difference of box center velocities + Vector3D boxDistance = obb2->getCenter() - obb1->getCenter(); // Distance between the centers of the OBBs + timeFirst.setValue(0.0); // timeFirst = 0 + timeLast.setValue(DBL_MAX); // timeLast = infinity + + // Axis A0 + for (int i=0; i<3; ++i) { + c[0][i] = obb1->getAxis(0).scalarProduct(obb2->getAxis(i)); + absC[0][i] = fabs(c[0][i]); + if (absC[0][i] > cutoff) { + existsParallelPair = true; + } + } + udc1[0] = obb1->getAxis(0).scalarProduct(boxDistance); + udv1[0] = obb1->getAxis(0).scalarProduct(deltaVelocity); + center = udc1[0]; + speed = udv1[0]; + radius1 = obb1->getExtent(0); + radius2 = obb2->getExtent(0)*absC[0][0] + obb2->getExtent(1)*absC[0][1] + obb2->getExtent(2) * absC[0][2]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + std::cout << "Speed : " << speed << std::endl; + std::cout << "min1 : " << min1 << std::endl; + std::cout << "max1 : " << max1 << std::endl; + std::cout << "min2 : " << min2 << std::endl; + std::cout << "max2 : " << max2 << std::endl; + if(!computeIntervalsIntersectionTime(timeMax, speed, min1, max1, min2, max2, timeFirst, timeLast)) { + // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + + // Axis A1 + for (int i=0; i<3; ++i) { + c[1][i] = obb1->getAxis(1).scalarProduct(obb2->getAxis(i)); + absC[1][i] = fabs(c[1][i]); + if (absC[1][i] > cutoff) { + existsParallelPair = true; + } + } + udc1[1] = obb1->getAxis(1).scalarProduct(boxDistance); + udv1[1] = obb1->getAxis(1).scalarProduct(deltaVelocity); + center = udc1[1]; + std::cout << "Axis - X : " << obb1->getAxis(1).getX() << "Y : " << obb1->getAxis(1).getY() << "Z : " << obb1->getAxis(1).getZ() << std::endl; + std::cout << "Distance - X : " << boxDistance.getX() << "Y : " << boxDistance.getY() << "Z : " << boxDistance.getZ() << std::endl; + std::cout << "Center : " << center << std::endl; + speed = udv1[1]; + radius1 = obb1->getExtent(1); + radius2 = obb2->getExtent(0)*absC[1][0] + obb2->getExtent(1)*absC[1][1] + obb2->getExtent(2) * absC[1][2]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + std::cout << "Speed : " << speed << std::endl; + std::cout << "min1 : " << min1 << std::endl; + std::cout << "max1 : " << max1 << std::endl; + std::cout << "min2 : " << min2 << std::endl; + std::cout << "max2 : " << max2 << std::endl; + if(!computeIntervalsIntersectionTime(timeMax, speed, min1, max1, min2, max2, timeFirst, timeLast)) { + // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + + // Axis A2 + for (int i=0; i<3; ++i) { + c[2][i] = obb1->getAxis(2).scalarProduct(obb2->getAxis(i)); + absC[2][i] = fabs(c[2][i]); + if (absC[2][i] > cutoff) { + existsParallelPair = true; + } + } + udc1[2] = obb1->getAxis(2).scalarProduct(boxDistance); + udv1[2] = obb1->getAxis(2).scalarProduct(deltaVelocity); + center = udc1[2]; + speed = udv1[2]; + radius1 = obb1->getExtent(2); + radius2 = obb2->getExtent(0)*absC[2][0] + obb2->getExtent(1)*absC[2][1] + obb2->getExtent(2) * absC[2][2]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + std::cout << "Speed : " << speed << std::endl; + std::cout << "min1 : " << min1 << std::endl; + std::cout << "max1 : " << max1 << std::endl; + std::cout << "min2 : " << min2 << std::endl; + std::cout << "max2 : " << max2 << std::endl; + if(!computeIntervalsIntersectionTime(timeMax, speed, min1, max1, min2, max2, timeFirst, timeLast)) { + // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + + // Axis B0 + udc2[0] = obb2->getAxis(0).scalarProduct(boxDistance); + udv2[0] = obb2->getAxis(0).scalarProduct(deltaVelocity); + center = udc2[0]; + speed = udv2[0]; + radius1 = obb1->getExtent(0)*absC[0][0] + obb1->getExtent(1)*absC[1][0] + obb1->getExtent(2) * absC[1][0]; + radius2 = obb2->getExtent(0); + min1 = center - radius1; + max1 = center + radius1; + min2 = -radius2; + max2 = radius2; + if(!computeIntervalsIntersectionTime(timeMax, speed, min1, max1, min2, max2, timeFirst, timeLast)) { + // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + + // Axis B1 + udc2[1] = obb2->getAxis(1).scalarProduct(boxDistance); + udv2[1] = obb2->getAxis(1).scalarProduct(deltaVelocity); + center = udc2[1]; + speed = udv2[1]; + radius1 = obb1->getExtent(0)*absC[0][1] + obb1->getExtent(1)*absC[1][1] + obb1->getExtent(2) * absC[1][1]; + radius2 = obb2->getExtent(1); + min1 = center - radius1; + max1 = center + radius1; + min2 = -radius2; + max2 = radius2; + if(!computeIntervalsIntersectionTime(timeMax, speed, min1, max1, min2, max2, timeFirst, timeLast)) { + // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + + // Axis B2 + udc2[2] = obb2->getAxis(2).scalarProduct(boxDistance); + udv2[2] = obb2->getAxis(2).scalarProduct(deltaVelocity); + center = udc2[2]; + speed = udv2[2]; + radius1 = obb1->getExtent(0)*absC[0][2] + obb1->getExtent(1)*absC[1][2] + obb1->getExtent(2) * absC[1][2]; + radius2 = obb2->getExtent(2); + min1 = center - radius1; + max1 = center + radius1; + min2 = -radius2; + max2 = radius2; + if(!computeIntervalsIntersectionTime(timeMax, speed, min1, max1, min2, max2, timeFirst, timeLast)) { + // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + + // If there exists a parallel pair of face normals + if (existsParallelPair) { + // There exists a parallel pair of face normals and we have already checked all the face + // normals for separation. Therefore the OBBs must intersect + + // TODO : Delete this + (*contact) = new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), Vector3D(1,0,0)); + std::cout << "Contact : " << contact << std::endl; + + return true; + } + + // Axis A0 x B0 + center = udc1[2] * c[1][0] - udc1[1] * c[2][0]; + speed = udv1[2] * c[1][0] - udv1[1] * c[2][0]; + radius1 = obb1->getExtent(1) * absC[2][0] + obb1->getExtent(2) * absC[1][0]; + radius2 = obb2->getExtent(1) * absC[0][2] + obb2->getExtent(2) * absC[0][1]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + if(!computeIntervalsIntersectionTime(timeMax, speed, min1, max1, min2, max2, timeFirst, timeLast)) { + // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + + // Axis A0 x B1 + center = udc1[2] * c[1][1] - udc1[1] * c[2][1]; + speed = udv1[2] * c[1][1] - udv1[1] * c[2][1]; + radius1 = obb1->getExtent(1) * absC[2][1] + obb1->getExtent(2) * absC[1][1]; + radius2 = obb2->getExtent(0) * absC[0][2] + obb2->getExtent(2) * absC[0][0]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + if(!computeIntervalsIntersectionTime(timeMax, speed, min1, max1, min2, max2, timeFirst, timeLast)) { + // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + + // Axis A0 x B2 + center = udc1[2] * c[1][2] - udc1[1] * c[2][2]; + speed = udv1[2] * c[1][2] - udv1[1] * c[2][2]; + radius1 = obb1->getExtent(1) * absC[2][2] + obb1->getExtent(2) * absC[1][2]; + radius2 = obb2->getExtent(0) * absC[0][1] + obb2->getExtent(1) * absC[0][0]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + if(!computeIntervalsIntersectionTime(timeMax, speed, min1, max1, min2, max2, timeFirst, timeLast)) { + // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + + // Axis A1 x B0 + center = udc1[0] * c[2][0] - udc1[2] * c[0][0]; + speed = udv1[0] * c[2][0] - udv1[2] * c[0][0]; + radius1 = obb1->getExtent(0) * absC[2][0] + obb1->getExtent(2) * absC[0][0]; + radius2 = obb2->getExtent(1) * absC[1][2] + obb2->getExtent(2) * absC[1][1]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + if(!computeIntervalsIntersectionTime(timeMax, speed, min1, max1, min2, max2, timeFirst, timeLast)) { + // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + + // Axis A1 x B1 + center = udc1[0] * c[2][1] - udc1[2] * c[0][1]; + speed = udv1[0] * c[2][1] - udv1[2] * c[0][1]; + radius1 = obb1->getExtent(0) * absC[2][1] + obb1->getExtent(2) * absC[0][1]; + radius2 = obb2->getExtent(0) * absC[1][2] + obb2->getExtent(2) * absC[1][0]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + if(!computeIntervalsIntersectionTime(timeMax, speed, min1, max1, min2, max2, timeFirst, timeLast)) { + // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + + // Axis A1 x B2 + center = udc1[0] * c[2][2] - udc1[2] * c[0][2]; + speed = udv1[0] * c[2][2] - udv1[2] * c[0][2]; + radius1 = obb1->getExtent(0) * absC[2][2] + obb1->getExtent(2) * absC[0][2]; + radius2 = obb2->getExtent(0) * absC[1][1] + obb2->getExtent(1) * absC[1][0]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + if(!computeIntervalsIntersectionTime(timeMax, speed, min1, max1, min2, max2, timeFirst, timeLast)) { + // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + + // Axis A2 x B0 + center = udc1[1] * c[0][0] - udc1[0] * c[1][0]; + speed = udv1[1] * c[0][0] - udv1[0] * c[1][0]; + radius1 = obb1->getExtent(0) * absC[1][0] + obb1->getExtent(1) * absC[0][0]; + radius2 = obb2->getExtent(1) * absC[2][2] + obb2->getExtent(2) * absC[2][1]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + if(!computeIntervalsIntersectionTime(timeMax, speed, min1, max1, min2, max2, timeFirst, timeLast)) { + // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + + // Axis A2 x B1 + center = udc1[1] * c[0][1] - udc1[0] * c[1][1]; + speed = udv1[1] * c[0][1] - udv1[0] * c[1][1]; + radius1 = obb1->getExtent(0) * absC[1][1] + obb1->getExtent(1) * absC[0][1]; + radius2 = obb2->getExtent(0) * absC[2][2] + obb2->getExtent(2) * absC[2][0]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + if(!computeIntervalsIntersectionTime(timeMax, speed, min1, max1, min2, max2, timeFirst, timeLast)) { + // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + + // Axis A2 x B2 + center = udc1[1] * c[0][2] - udc1[0] * c[1][2]; + speed = udv1[1] * c[0][2] - udv1[0] * c[1][2]; + radius1 = obb1->getExtent(0) * absC[1][2] + obb1->getExtent(1) * absC[0][2]; + radius2 = obb2->getExtent(0) * absC[2][1] + obb2->getExtent(1) * absC[2][0]; + min1 = -radius1; + max1 = radius1; + min2 = center - radius2; + max2 = center + radius2; + if(!computeIntervalsIntersectionTime(timeMax, speed, min1, max1, min2, max2, timeFirst, timeLast)) { + // We have found a separation axis, therefore the two OBBs don't collide + return false; + } + + // TODO : Delete this + (*contact) = new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), Vector3D(1,0,0)); + std::cout << "Contact2 : " << contact << std::endl; + + // We have found no separation axis, therefore the two OBBs must collide + return true; +} + +// This method computes the intersection time of two projection intervals. +// This method takes two projection intervals [min1, max1] and [min2, max2] and computes (if the +// two intervals intersect) in the time interval [0, timeMax] the time timeFirst where the two bodies +// enter in collision and the time timeLast where the two bodies separate themself from the collision. +// We consider that the interval 2 move at the speed "speed" and the interval 1 don't move. +// The method returns true if the two projection intervals intersect and false if they move appart. +// This method will be called for each separation axis. +bool SeparatingAxisOBB::computeIntervalsIntersectionTime(const Time& timeMax, double speed, double min1, double max1, + double min2, double max2, Time& timeFirst, Time& timeLast) { + double speedInverse = 1.0/speed; + double t; + + // If the interval [min0, max0] is on right of interval [min1, max1] + if (max2 < min1) { + // If the two intervals move apart they will not intersect + if (speed <= 0) { + return false; + } + + // Compute the time t when the two intervals enter in contact + t = (min1-max2) * speedInverse; + + // If we found a later collision time, we update the first collision time + if (t > timeFirst.getValue()) { + timeFirst.setValue(t); + } + + // If the first collision time is outside of the time interval [0, timeMax] + if(timeFirst.getValue() > timeMax.getValue()) { + return false; + } + + // Compute the time t when the two intervals separate from a contact + t = (max1 - min2) * speedInverse; + + // If we found a earlier separated collision time, we update the last collision time + if (t < timeLast.getValue()) { + + timeLast.setValue(t); + } + + // If the first collision time occurs after the last collision time + if (timeFirst.getValue() > timeLast.getValue()) { + return false; + } + } + else if (max1 < min2) { // If the interval [min0, max0] is on left of interval [min1, max1] + // If the two intervals move apart they will not intersect + if (speed >= 0) { + return false; + } + + // Compute the time t when the two intervals enter in contact + t = (max1 - min2) * speedInverse; + + // If we found a later collision time + if (t > timeFirst.getValue()) { + timeFirst.setValue(t); + } + + // If the first collision time is outside of the time interval [0, timeMax] + if(timeFirst.getValue() > timeMax.getValue()) { + return false; + } + + // Compute the time t when the two intervals separate from a contact + t = (min1 - max2) * speedInverse; + + // If we found a earlier separated collision time + if (t < timeLast.getValue()) { + timeLast.setValue(t); + } + + // If the first collision time occurs after the last collision time + if (timeFirst.getValue() > timeLast.getValue()) { + return false; + } + } + else { // If the two intervals overlap + if (speed > 0) { + // Compute the time t when the two intervals separate from a contact + t = (max1 - min2) * speedInverse; + + // If we found a earlier separated collision time + if (t < timeLast.getValue()) { + timeLast.setValue(t); + } + + // If the first collision time occurs after the last collision time + if (timeFirst.getValue() > timeLast.getValue()) { + return false; + } + } + else if (speed < 0) { + // Compute the time t when the two intervals separate from a contact + t = (min1 - max2) * speedInverse; + + // If we found a earlier separated collision time + if (t < timeLast.getValue()) { + timeLast.setValue(t); + } + + // If the first collision time occurs after the last collision time + if (timeFirst.getValue() > timeLast.getValue()) { + return false; + } + } + } + + return true; } diff --git a/sources/reactphysics3d/collision/SeparatingAxisOBB.h b/sources/reactphysics3d/collision/SeparatingAxisOBB.h index af6ab52a..b4b653e3 100644 --- a/sources/reactphysics3d/collision/SeparatingAxisOBB.h +++ b/sources/reactphysics3d/collision/SeparatingAxisOBB.h @@ -23,18 +23,40 @@ // Libraries #include "NarrowPhaseAlgorithm.h" #include "../constraint/Contact.h" +#include "../body/OBB.h" // ReactPhysics3D namespace namespace reactphysics3d { +/* ------------------------------------------------------------------- + Class SeparatingAxisOBB : + This class implements a narrow-phase algorithm. This algorithm + uses a separating axis technique with oriented bounding box + (OBB) to check if two bounding volumes collide or not. If the + two bounding volumes collide we have to create a contact object + to describe the collision contact. The idea is to check if there + exists an axis where, if we project the two bounding volumes on + this axis, the two projections are separated. If we find at + least an axis where the projections of the two bounding volumes + are separated then we know that the two bounding volumes don't + intersect. + ------------------------------------------------------------------- +*/ class SeparatingAxisOBB : public NarrowPhaseAlgorithm { private : + bool computeCollisionTest(const OBB* const obb1, const OBB* const obb2, Contact** contact, + const Vector3D& velocity1, const Vector3D& velocity2, + const Time& timeMax, Time& timeFirst, Time& timeLast); // Return true and compute a collision contact if the two OBB collide + bool computeIntervalsIntersectionTime(const Time& timeMax, double speed, double min0, double max0, + double min1, double max1, Time& timeFirst, Time& timeLast); // Compute the intersection time of two projection intervals public : SeparatingAxisOBB(); // Constructor ~SeparatingAxisOBB(); // Destructor - virtual bool testCollision(const BoundingVolume& boundingVolume1, const BoundingVolume& boundingVolume2, Contact* const 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, + Contact** contact, const Vector3D& velocity1, const Vector3D& velocity2, const Time& timeMax, + Time& timeFirst, Time& timeLast); // Return true and compute a collision contact if the two bounding volume collide };