diff --git a/sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.cpp b/sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.cpp
new file mode 100644
index 00000000..a19909e9
--- /dev/null
+++ b/sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.cpp
@@ -0,0 +1,743 @@
+/***************************************************************************
+* Copyright (C) 2009 Daniel Chappuis *
+****************************************************************************
+* This file is part of ReactPhysics3D. *
+* *
+* ReactPhysics3D is free software: you can redistribute it and/or modify *
+* it under the terms of the GNU Lesser General Public License as published *
+* by the Free Software Foundation, either version 3 of the License, or *
+* (at your option) any later version. *
+* *
+* ReactPhysics3D is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+* GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public License *
+* along with ReactPhysics3D. If not, see . *
+***************************************************************************/
+
+// Libraries
+#include "NarrowPhaseSATAlgorithm.h"
+#include "ProjectionInterval.h"
+#include "../body/OBB.h"
+#include "../body/RigidBody.h"
+#include "../constraint/Contact.h"
+#include "../constraint/VertexVertexContact.h"
+#include "../constraint/EdgeEdgeContact.h"
+#include "../constraint/FaceFaceContact.h"
+#include "../constraint/EdgeVertexContact.h"
+#include "../constraint/FaceEdgeContact.h"
+#include "../constraint/FaceVertexContact.h"
+#include
+#include // TODO : Delete this
+#include
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+NarrowPhaseSATAlgorithm::NarrowPhaseSATAlgorithm() {
+
+}
+
+// Destructor
+NarrowPhaseSATAlgorithm::~NarrowPhaseSATAlgorithm() {
+
+}
+
+// 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 NarrowPhaseSATAlgorithm::testCollision(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2, Contact** contact,
+ const Vector3D& velocity1, const Vector3D& velocity2, const Time& timeMax) {
+ assert(boundingVolume1 != boundingVolume2);
+ assert(*contact == 0);
+
+ // 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);
+ }
+ 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 NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const obb2, Contact** contact,
+ const Vector3D& velocity1, const Vector3D& velocity2, const Time& timeMax) {
+
+ 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
+ ProjectionInterval currentInterval1; // Current projection interval 1 (correspond to the first collision)
+ ProjectionInterval currentInterval2; // Current projection interval 2 (correspond to the first collision)
+ bool side; // True if the collision is between max1 and min2 and false if it's between max2 and min1
+ 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
+ Time timeFirst(0.0); // timeFirst = 0
+ Time timeLast(DBL_MAX); // timeLast = infinity (time when two colliding bodies separates)I
+
+ // 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;
+ ProjectionInterval interval1 = computeProjectionInterval(min1, max1, obb1, obb1->getAxis(0));
+ ProjectionInterval interval2 = computeProjectionInterval(min2, max2, obb2, obb1->getAxis(0));
+ /*
+ 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, currentInterval1, currentInterval2, interval1, interval2, timeFirst, timeLast, side)) {
+ // We have found a separation axis, therefore the two OBBs don't collide
+ //std::cout << "SEPARATION AXIS : A0 " << std::endl;
+
+ return false;
+ }
+
+ // Axis A1
+ //std::cout << "----- AXIS A1 -----" << std::endl;
+ 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];
+ 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;
+ interval1 = computeProjectionInterval(min1, max1, obb1, obb1->getAxis(1));
+ interval2 = computeProjectionInterval(min2, max2, obb2, obb1->getAxis(1));
+ /*
+ 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, currentInterval1, currentInterval2, interval1, interval2, timeFirst, timeLast, side)) {
+ // We have found a separation axis, therefore the two OBBs don't collide
+ //std::cout << "SEPARATION AXIS : A1 " << std::endl;
+
+ 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;
+ interval1 = computeProjectionInterval(min1, max1, obb1, obb1->getAxis(2));
+ interval2 = computeProjectionInterval(min2, max2, obb2, obb1->getAxis(2));
+ /*
+ 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, currentInterval1, currentInterval2, interval1, interval2, timeFirst, timeLast, side)) {
+ // We have found a separation axis, therefore the two OBBs don't collide
+ //std::cout << "SEPARATION AXIS : A2 " << std::endl;
+
+ 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[2][0];
+ radius2 = obb2->getExtent(0);
+ min1 = -radius1;
+ max1 = radius1;
+ min2 = center - radius2;
+ max2 = center + radius2;
+ interval1 = computeProjectionInterval(min1, max1, obb1, obb2->getAxis(0));
+ interval2 = computeProjectionInterval(min2, max2, obb2, obb2->getAxis(0));
+ if(!computeIntervalsIntersectionTime(timeMax, speed, currentInterval1, currentInterval2, interval1, interval2, timeFirst, timeLast, side)) {
+ // We have found a separation axis, therefore the two OBBs don't collide
+ //std::cout << "SEPARATION AXIS : B0 " << std::endl;
+
+ return false;
+ }
+
+ // Axis B1
+ //std::cout << "----- AXIS B1 -----" << std::endl;
+ 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[2][1];
+ radius2 = obb2->getExtent(1);
+ min1 = - radius1;
+ max1 = radius1;
+ min2 = center - radius2;
+ max2 = center + radius2;
+ interval1 = computeProjectionInterval(min1, max1, obb1, obb2->getAxis(1));
+ interval2 = computeProjectionInterval(min2, max2, obb2, obb2->getAxis(1));
+ 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, currentInterval1, currentInterval2, interval1, interval2, timeFirst, timeLast, side)) {
+ // We have found a separation axis, therefore the two OBBs don't collide
+ //std::cout << "SEPARATION AXIS : B1 " << std::endl;
+
+ return false;
+ }
+ //std::cout << "----- FIN AXIS B1 -----" << std::endl;
+
+
+ // 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[2][2];
+ radius2 = obb2->getExtent(2);
+ min1 = - radius1;
+ max1 = radius1;
+ min2 = center - radius2;
+ max2 = center + radius2;
+ interval1 = computeProjectionInterval(min1, max1, obb1, obb2->getAxis(2));
+ interval2 = computeProjectionInterval(min2, max2, obb2, obb2->getAxis(2));
+ if(!computeIntervalsIntersectionTime(timeMax, speed, currentInterval1, currentInterval2, interval1, interval2, timeFirst, timeLast, side)) {
+ // We have found a separation axis, therefore the two OBBs don't collide
+ //std::cout << "SEPARATION AXIS : B2 " << std::endl;
+
+ 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
+ //std::cout << "PARALLEL PAIR" << std::endl;
+ //std::cout << "Current -- 1 -- MIN Points : " << currentInterval1.getMinProjectedPoints().size() << " MAX : " << currentInterval1.getMaxProjectedPoints().size() << std::endl;
+ //std::cout << "Current -- 1 -- min : " << currentInterval1.getMin() << std::endl;
+ //std::cout << "Timefirst : " << timeFirst.getValue() << std::endl;
+ std::cout << "CONTACT FOUND AND TIMEFIRST IS " << timeFirst.getValue() << std::endl;
+
+ // TODO : Construct a face-face contact here
+ //(*contact) = new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), Vector3D(1,0,0), timeFirst);
+
+ computeContact(currentInterval1, currentInterval2, velocity1, velocity2, timeFirst, side, contact);
+
+ //std::cout << "Contact 1 : " << contact << std::endl;
+ assert(*contact != 0);
+ 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;
+ Vector3D axis = obb1->getAxis(0).crossProduct(obb2->getAxis(0));
+ interval1 = computeProjectionInterval(min1, max1, obb1, axis);
+ interval2 = computeProjectionInterval(min2, max2, obb2, axis);
+ if(!computeIntervalsIntersectionTime(timeMax, speed, currentInterval1, currentInterval2, interval1, interval2, timeFirst, timeLast, side)) {
+ // 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;
+ axis = obb1->getAxis(0).crossProduct(obb2->getAxis(1));
+ interval1 = computeProjectionInterval(min1, max1, obb1, axis);
+ interval2 = computeProjectionInterval(min2, max2, obb2, axis);
+ if(!computeIntervalsIntersectionTime(timeMax, speed, currentInterval1, currentInterval2, interval1, interval2, timeFirst, timeLast, side)) {
+ // 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;
+ axis = obb1->getAxis(0).crossProduct(obb2->getAxis(2));
+ interval1 = computeProjectionInterval(min1, max1, obb1, axis);
+ interval2 = computeProjectionInterval(min2, max2, obb2, axis);
+ if(!computeIntervalsIntersectionTime(timeMax, speed, currentInterval1, currentInterval2, interval1, interval2, timeFirst, timeLast, side)) {
+ // 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;
+ axis = obb1->getAxis(1).crossProduct(obb2->getAxis(0));
+ interval1 = computeProjectionInterval(min1, max1, obb1, axis);
+ interval2 = computeProjectionInterval(min2, max2, obb2, axis);
+ if(!computeIntervalsIntersectionTime(timeMax, speed, currentInterval1, currentInterval2, interval1, interval2, timeFirst, timeLast, side)) {
+ // 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;
+ axis = obb1->getAxis(1).crossProduct(obb2->getAxis(1));
+ interval1 = computeProjectionInterval(min1, max1, obb1, axis);
+ interval2 = computeProjectionInterval(min2, max2, obb2, axis);
+ if(!computeIntervalsIntersectionTime(timeMax, speed, currentInterval1, currentInterval2, interval1, interval2, timeFirst, timeLast, side)) {
+ // 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;
+ axis = obb1->getAxis(1).crossProduct(obb2->getAxis(2));
+ interval1 = computeProjectionInterval(min1, max1, obb1, axis);
+ interval2 = computeProjectionInterval(min2, max2, obb2, axis);
+ if(!computeIntervalsIntersectionTime(timeMax, speed, currentInterval1, currentInterval2, interval1, interval2, timeFirst, timeLast, side)) {
+ // 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;
+ axis = obb1->getAxis(2).crossProduct(obb2->getAxis(0));
+ interval1 = computeProjectionInterval(min1, max1, obb1, axis);
+ interval2 = computeProjectionInterval(min2, max2, obb2, axis);
+ if(!computeIntervalsIntersectionTime(timeMax, speed, currentInterval1, currentInterval2, interval1, interval2, timeFirst, timeLast, side)) {
+ // 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;
+ axis = obb1->getAxis(2).crossProduct(obb2->getAxis(1));
+ interval1 = computeProjectionInterval(min1, max1, obb1, axis);
+ interval2 = computeProjectionInterval(min2, max2, obb2, axis);
+ if(!computeIntervalsIntersectionTime(timeMax, speed, currentInterval1, currentInterval2, interval1, interval2, timeFirst, timeLast, side)) {
+ // 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;
+ axis = obb1->getAxis(2).crossProduct(obb2->getAxis(2));
+ interval1 = computeProjectionInterval(min1, max1, obb1, axis);
+ interval2 = computeProjectionInterval(min2, max2, obb2, axis);
+ if(!computeIntervalsIntersectionTime(timeMax, speed, currentInterval1, currentInterval2, interval1, interval2, timeFirst, timeLast, side)) {
+ // 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), timeFirst);
+ std::cout << "Contact2 : " << contact << std::endl;
+ std::cout << "CONTACT FOUND AND TIMEFIRST IS " << timeFirst.getValue() << std::endl;
+
+ // Compute the collision contact
+ computeContact(currentInterval1, currentInterval2, velocity1, velocity2, timeFirst, side, contact);
+
+ // We have found no separation axis, therefore the two OBBs must collide
+ assert(*contact != 0);
+ 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 NarrowPhaseSATAlgorithm::computeIntervalsIntersectionTime(const Time& timeMax, double speed, ProjectionInterval& currentInterval1,
+ ProjectionInterval& currentInterval2, const ProjectionInterval& interval1,
+ const ProjectionInterval& interval2, Time& timeFirst, Time& timeLast, bool& side) {
+ double speedInverse = 1.0/speed; // TODO : Test if the speed could be zero at this place
+ double t;
+ double min1 = interval1.getMin();
+ double max1 = interval1.getMax();
+ double min2 = interval2.getMin();
+ double max2 = interval2.getMax();
+
+ // If the interval [min1, max1] is on right of interval [min2, max2]
+ 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);
+ side = false;
+ currentInterval1 = interval1;
+ currentInterval2 = interval2;
+
+ //std::cout << "Curent1 = Interval1 : min " << interval1.getMin() << std::endl;
+ //std::cout << "Curent2 = Interval2 : min " << interval2.getMin() << std::endl;
+ }
+
+ // 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;
+
+ //std::cout << "Separate time : " << t << std::endl;
+ //std::cout << "timeLast : " << timeLast.getValue() << std::endl;
+
+ // 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 [min1, max1] is on left of interval [min2, max2]
+ // If the two intervals move apart they will not intersect
+ if (speed >= 0) {
+ //std::cout << "Move appart" << std::endl;
+ 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);
+ side = true;
+ currentInterval1 = interval1;
+ currentInterval2 = interval2;
+ }
+
+ // 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;
+ }
+ }
+
+ // TODO : Are we sure that we don't have to create a contact when both intervals
+ // overlaps ?
+
+ // TODO : Because we are approximating collision detection using constant linear
+ // velocity and no angular velocity. Some errors can occur. For instance,
+ // it's possible that we obtain a penetration of two objects. We have to
+ // find a way to consider thoses errors.
+ }
+
+ return true;
+}
+
+// Compute a new collision contact between two projection intervals.
+// Warning : If the side value is true the max of interval1 collides with the min of interval2. If the
+// side value is false the max value of interval2 collides with the min value of interval1.
+void NarrowPhaseSATAlgorithm::computeContact(const ProjectionInterval& interval1, const ProjectionInterval& interval2,
+ const Vector3D& velocity1, const Vector3D& velocity2, const Time& time, bool side, Contact** contact) {
+
+ std::cout << "COMPUTE CONTACT and timeFirst is " << time.getValue() << std::endl;
+ assert(*contact == 0);
+
+ ProjectionInterval intervalLeft = (side) ? interval1 : interval2;
+ ProjectionInterval intervalRight = (!side) ? interval2 : interval1;
+
+ Vector3D velocityLeft = (side) ? velocity1 : velocity2;
+ Vector3D velocityRight = (!side) ? velocity2 : velocity1;
+
+ // Compute the extreme points of the two intervals at the instant of contact
+ std::vector leftExtremePointsAtContact = movePoints(intervalLeft.getMaxProjectedPoints(), velocityLeft * time.getValue());
+ std::vector rightExtremePointsAtContact = movePoints(intervalRight.getMinProjectedPoints(), velocityRight * time.getValue());
+
+ // TODO : ADD THE BODY ADRESS INTO THE CONTACT HERE
+ // Get the rigid bodies
+ //RigidBody* body1 = dynamic_cast(intervalLeft.getBoundingVolumePointer()->getBodyPointer());
+ //RigidBody* body2 = dynamic_cast(intervalRight.getBoundingVolumePointer()->getBodyPointer());
+
+ //assert(body1 != 0 && body2 != 0);
+ RigidBody* body1 = 0;
+ RigidBody* body2 = 0; // TODO : DELETE THIS
+
+ // Compute the normal vector of the contact
+ // TODO : Compute the normal vector of the contact
+ Vector3D normalVector(0.0, 1.0, 0.0);
+
+ switch(intervalLeft.getMaxType()) {
+ case VERTEX : if (intervalRight.getMinType() == VERTEX) {
+ // Construct a new Vertex-Vertex contact
+ *contact = new VertexVertexContact(body1, body2, normalVector, time, intervalLeft.getMaxProjectedPoints()[0]);
+ }
+ else if (intervalRight.getMinType() == EDGE) {
+ // Construct a new Edge-Vertex contact
+ *contact = new EdgeVertexContact(body1, body2, normalVector, time, intervalLeft.getMaxProjectedPoints()[0]);
+ }
+ else if (intervalRight.getMinType() == FACE) {
+ // Construct a new Face-Vertex contact
+ *contact = new FaceVertexContact(body1, body2, normalVector, time, intervalLeft.getMaxProjectedPoints()[0]);
+ }
+ break;
+
+ case EDGE: if (intervalRight.getMinType() == VERTEX) {
+ // Construct a new Edge-Vertex contact
+ *contact = new EdgeVertexContact(body1, body2, normalVector, time, intervalRight.getMinProjectedPoints()[0]);
+ }
+ else if (intervalRight.getMinType() == EDGE) {
+ // Compute the intersection between the two edges
+ Segment3D edge1(intervalLeft.getMaxProjectedPoints()[0], intervalLeft.getMaxProjectedPoints()[1]);
+ Segment3D edge2(intervalRight.getMinProjectedPoints()[0], intervalRight.getMinProjectedPoints()[1]);
+ Segment3D intersectionSegment = computeSegmentSegmentIntersection(edge1, edge2);
+
+ // Construct a new Edge-Edge contact
+ *contact = new EdgeEdgeContact(body1, body2, normalVector, time, intersectionSegment);
+ }
+ else if (intervalRight.getMinType() == FACE) {
+ // Compute the intersection between the edge and the face
+ Segment3D edge(intervalLeft.getMaxProjectedPoints()[0], intervalLeft.getMaxProjectedPoints()[1]);
+ Polygon3D face(intervalRight.getMinProjectedPoints());
+ Segment3D intersectionSegment = computeSegmentPolygonIntersection(edge, face);
+
+ // TODO : Warning : At this moment the set of vertices of the contact is not sorted. We will have to
+ // find a way to sort it because the constructor of the Polygon3D class needs a set where vertices are
+ // sorted in order to have a correct polygon.
+
+ // Construct a new Face-Edge contact
+ *contact = new FaceEdgeContact(body1, body2, normalVector, time, intersectionSegment);
+ }
+ break;
+
+ case FACE: if (intervalRight.getMinType() == VERTEX) {
+ // Construct a new Face-Vertex contact
+ *contact = new FaceVertexContact(body1, body2, normalVector, time, intervalRight.getMinProjectedPoints()[0]);
+ }
+ else if (intervalRight.getMinType() == EDGE) {
+ // Compute the intersection between the edge and the face
+ Polygon3D face(intervalLeft.getMaxProjectedPoints());
+ Segment3D edge(intervalRight.getMinProjectedPoints()[0], intervalRight.getMinProjectedPoints()[1]);
+ Segment3D intersectionSegment = computeSegmentPolygonIntersection(edge, face);
+
+ // TODO : Warning : At this moment the set of vertices of the contact is not sorted. We will have to
+ // find a way to sort it because the constructor of the Polygon3D class needs a set where vertices are
+ // sorted in order to have a correct polygon.
+
+ // TODO : Here we will have to compute the Segment intersection between the edge and the face
+ *contact = new FaceEdgeContact(body1, body2, normalVector, time, intersectionSegment);
+ }
+ else if (intervalRight.getMinType() == FACE) {
+ // Compute the intersection between the two faces
+ Polygon3D face1(intervalLeft.getMaxProjectedPoints());
+ Polygon3D face2(intervalRight.getMinProjectedPoints());
+ Polygon3D intersectionPolygon = computePolygonPolygonIntersection(face1, face2);
+
+ // TODO : Warning : At this moment the set of vertices of the contact is not sorted. We will have to
+ // find a way to sort it because the constructor of the Polygon3D class needs a set where vertices are
+ // sorted in order to have a correct polygon.
+
+ // Construct a new Face-Face contact
+ *contact = new FaceFaceContact(body1, body2, normalVector, time, intersectionPolygon);
+ }
+ break;
+ }
+}
+
+// Compute a new projection interval
+ProjectionInterval NarrowPhaseSATAlgorithm::computeProjectionInterval(double min, double max, const OBB* const obb, const Vector3D& axis) const {
+ ExtremeType minExtremeType;
+ ExtremeType maxExtremeType;
+ std::vector minProjectedVertices; // Vertices of the OBB that are projected on the minimum of an interval
+ std::vector maxProjectedVertices; // Vertices of the OBB that are projected on the minimum of an interval
+
+ // Compute the extreme vertices of the OBB that are projected at the extreme of the interval
+ int nbExtremeVerticesMin = obb->getExtremeVertices(axis.getOpposite(), minProjectedVertices);
+ int nbExtremeVerticesMax = obb->getExtremeVertices(axis, maxProjectedVertices);
+
+ // Compute the type of the extremes of the interval
+ switch(nbExtremeVerticesMin) {
+ case 1 : minExtremeType = VERTEX; break;
+ case 2 : minExtremeType = EDGE; break;
+ case 4 : minExtremeType = FACE; break;
+ }
+ switch(nbExtremeVerticesMax) {
+ case 1 : maxExtremeType = VERTEX; break;
+ case 2 : maxExtremeType = EDGE; break;
+ case 4 : maxExtremeType = FACE; break;
+ }
+
+ // Compute and return a projection interval
+ return ProjectionInterval(obb, min, max, minExtremeType, maxExtremeType, minProjectedVertices, maxProjectedVertices);
+}
diff --git a/sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.h b/sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.h
new file mode 100644
index 00000000..9f2d2715
--- /dev/null
+++ b/sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.h
@@ -0,0 +1,68 @@
+/***************************************************************************
+* Copyright (C) 2009 Daniel Chappuis *
+****************************************************************************
+* This file is part of ReactPhysics3D. *
+* *
+* ReactPhysics3D is free software: you can redistribute it and/or modify *
+* it under the terms of the GNU Lesser General Public License as published *
+* by the Free Software Foundation, either version 3 of the License, or *
+* (at your option) any later version. *
+* *
+* ReactPhysics3D is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+* GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public License *
+* along with ReactPhysics3D. If not, see . *
+***************************************************************************/
+
+#ifndef NARROWPHASESATALGORITHM_H
+#define NARROWPHASESATALGORITHM_H
+
+// Libraries
+#include "NarrowPhaseAlgorithm.h"
+#include "ProjectionInterval.h"
+#include "../constraint/Contact.h"
+#include "../body/OBB.h"
+
+// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+/* -------------------------------------------------------------------
+ Class NarrowPhaseSATAlgorithm :
+ This class implements a narrow-phase algorithm. This algorithm
+ uses a separating axis theorem (SAT) 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 NarrowPhaseSATAlgorithm : public NarrowPhaseAlgorithm {
+ private :
+ bool computeCollisionTest(const OBB* const obb1, const OBB* const obb2, Contact** contact,
+ const Vector3D& velocity1, const Vector3D& velocity2, const Time& timeMax); // Return true and compute a collision contact if the two OBB collide
+ bool computeIntervalsIntersectionTime(const Time& timeMax, double speed, ProjectionInterval& currentInterval1,
+ ProjectionInterval& currentInterval2, const ProjectionInterval& interval1,
+ const ProjectionInterval& interval2, Time& timeFirst, Time& timeLast, bool& side); // Compute the intersection time of two projection intervals
+ void computeContact(const ProjectionInterval& interval1, const ProjectionInterval& interval2,
+ const Vector3D& velocity1, const Vector3D& velocity2, const Time& time, bool side, Contact** contact); // Compute a new collision contact between two projection intervals
+ ProjectionInterval computeProjectionInterval(double min, double max, const OBB* const obb, const Vector3D& axis) const; // Compute a new projection interval
+
+ public :
+ NarrowPhaseSATAlgorithm(); // Constructor
+ ~NarrowPhaseSATAlgorithm(); // Destructor
+
+ virtual bool testCollision(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2, Contact** contact,
+ const Vector3D& velocity1, const Vector3D& velocity2, const Time& timeMax); // Return true and compute a collision contact if the two bounding volume collide
+
+};
+
+} // End of the ReactPhysics3D namespace
+
+#endif
diff --git a/sources/reactphysics3d/collision/ProjectionInterval.cpp b/sources/reactphysics3d/collision/ProjectionInterval.cpp
new file mode 100644
index 00000000..2b96dafb
--- /dev/null
+++ b/sources/reactphysics3d/collision/ProjectionInterval.cpp
@@ -0,0 +1,44 @@
+/****************************************************************************
+* Copyright (C) 2009 Daniel Chappuis *
+****************************************************************************
+* This file is part of ReactPhysics3D. *
+* *
+* ReactPhysics3D is free software: you can redistribute it and/or modify *
+* it under the terms of the GNU Lesser General Public License as published *
+* by the Free Software Foundation, either version 3 of the License, or *
+* (at your option) any later version. *
+* *
+* ReactPhysics3D is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+* GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public License *
+* along with ReactPhysics3D. If not, see . *
+***************************************************************************/
+
+// Libraries
+#include "ProjectionInterval.h"
+#include
+
+// We want to use the ReactPhysics3D namespace
+using namespace reactphysics3d;
+
+// Constructor
+ProjectionInterval::ProjectionInterval()
+ :minType(VERTEX), maxType(VERTEX) {
+ boundingVolume = 0;
+ min = 0;
+ max = 0;
+}
+
+// Constructor
+ProjectionInterval::ProjectionInterval(const BoundingVolume* const boudingVolume, double min, double max, ExtremeType minType, ExtremeType maxType, std::vector minProjectedPoints, std::vector maxProjectedPoints)
+ :min(min), max(max), minType(minType), maxType(maxType), minProjectedPoints(minProjectedPoints), maxProjectedPoints(maxProjectedPoints) {
+ this->boundingVolume = boundingVolume;
+}
+
+// Destructor
+ProjectionInterval::~ProjectionInterval() {
+
+}
diff --git a/sources/reactphysics3d/collision/ProjectionInterval.h b/sources/reactphysics3d/collision/ProjectionInterval.h
new file mode 100644
index 00000000..0e5aca4c
--- /dev/null
+++ b/sources/reactphysics3d/collision/ProjectionInterval.h
@@ -0,0 +1,103 @@
+/****************************************************************************
+* Copyright (C) 2009 Daniel Chappuis *
+****************************************************************************
+* This file is part of ReactPhysics3D. *
+* *
+* ReactPhysics3D is free software: you can redistribute it and/or modify *
+* it under the terms of the GNU Lesser General Public License as published *
+* by the Free Software Foundation, either version 3 of the License, or *
+* (at your option) any later version. *
+* *
+* ReactPhysics3D is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+* GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public License *
+* along with ReactPhysics3D. If not, see . *
+***************************************************************************/
+
+#ifndef PROJECTIONINTERVAL_H
+#define PROJECTIONINTERVAL_H
+
+// Libraries
+#include
+#include "../body/BoundingVolume.h"
+#include "../mathematics/mathematics.h"
+
+// ReactPhysics3D namespace
+namespace reactphysics3d {
+
+// Type of the extreme of an interval. For instance if a extreme of an
+// interval is the result of the projection of an edge, the type will be
+// EDGE.
+enum ExtremeType {VERTEX, EDGE, FACE};
+
+/* -------------------------------------------------------------------
+ Class ProjectionInterval :
+ This class represents an projection interval of an bounding
+ volume onto a separation axis.
+ -------------------------------------------------------------------
+*/
+class ProjectionInterval {
+ private :
+ BoundingVolume* boundingVolume; // Pointer on the bounding volume corresponding to this projection interval
+ double min; // Minimum of the interval
+ double max; // Maximum of the interval
+ ExtremeType minType; // Type of the extreme of the projection interval
+ ExtremeType maxType; // Type of the extreme of the projection interval
+ std::vector minProjectedPoints; // Projected points onto the minimum of the interval
+ std::vector maxProjectedPoints; // Projected points onto the maximum of the interval
+
+ public :
+ ProjectionInterval(); // Constructor
+ ProjectionInterval(const BoundingVolume* const boudingVolume, double min, double max, ExtremeType minType, ExtremeType maxType, std::vector minProjectedPoints, std::vector maxProjectedPoints); // Constructor
+ ~ProjectionInterval(); // Destructor
+
+ BoundingVolume* getBoundingVolumePointer() const; // Return the pointer on the bounding volume
+ double getMin() const; // Return the minimum of the interval
+ double getMax() const; // Return the maximum of the interval
+ ExtremeType getMinType() const; // Return the type of the minimum extreme
+ ExtremeType getMaxType() const; // Return the type of the maximum extreme
+ std::vector getMinProjectedPoints() const; // Return the projected points onto the minimum extreme
+ std::vector getMaxProjectedPoints() const; // Return the projected points onto the maximum extreme
+};
+
+// Return the pointer on the bounding volume
+inline BoundingVolume* ProjectionInterval::getBoundingVolumePointer() const {
+ return boundingVolume;
+}
+
+// Return the minimum of the interval
+inline double ProjectionInterval::getMin() const {
+ return min;
+}
+
+// Return the maximum of the interval
+inline double ProjectionInterval::getMax() const {
+ return max;
+}
+
+// Return the type of the minimum extreme
+inline ExtremeType ProjectionInterval::getMinType() const {
+ return minType;
+}
+
+// Return the type of the maximum extreme
+inline ExtremeType ProjectionInterval::getMaxType() const {
+ return maxType;
+}
+
+// Return the projected points onto the minimum extreme
+inline std::vector ProjectionInterval::getMinProjectedPoints() const {
+ return minProjectedPoints;
+}
+
+// Return the projected points onto the maximum extreme
+inline std::vector ProjectionInterval::getMaxProjectedPoints() const {
+ return maxProjectedPoints;
+}
+
+} // End of the ReactPhysics3D namespace
+
+#endif