From 47f4cc6e6e53b781fd8d8f64d9ac7c396cbc1f4c Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Thu, 8 Oct 2009 21:56:58 +0000 Subject: [PATCH] git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@203 92aac97c-a6ce-11dd-a772-7fcde58d38e6 --- .../collision/NarrowPhaseSATAlgorithm.cpp | 743 ++++++++++++++++++ .../collision/NarrowPhaseSATAlgorithm.h | 68 ++ .../collision/ProjectionInterval.cpp | 44 ++ .../collision/ProjectionInterval.h | 103 +++ 4 files changed, 958 insertions(+) create mode 100644 sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.cpp create mode 100644 sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.h create mode 100644 sources/reactphysics3d/collision/ProjectionInterval.cpp create mode 100644 sources/reactphysics3d/collision/ProjectionInterval.h 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