From c213ab195c39587bbdeae725a077de1e288b50b7 Mon Sep 17 00:00:00 2001 From: "chappuis.daniel" Date: Fri, 25 Dec 2009 09:39:02 +0000 Subject: [PATCH] git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@239 92aac97c-a6ce-11dd-a772-7fcde58d38e6 --- .../collision/NarrowPhaseSATAlgorithm.cpp | 60 +++++++------------ .../collision/NarrowPhaseSATAlgorithm.h | 6 +- 2 files changed, 26 insertions(+), 40 deletions(-) diff --git a/sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.cpp b/sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.cpp index 07d052d1..ded94899 100644 --- a/sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.cpp +++ b/sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.cpp @@ -23,12 +23,7 @@ #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 #include // TODO : Delete this #include @@ -125,7 +120,7 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const min2 = center - radius2; max2 = center + radius2; bool sideTemp; // TODO : Check if we really need the "side" variable (maybee, we can remove it) - double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); + double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } @@ -136,7 +131,6 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const } // TODO : Delete this - std::cout << "Speed : " << speed << std::endl; std::cout << "min1 : " << min1 << std::endl; std::cout << "max1 : " << max1 << std::endl; std::cout << "min2 : " << min2 << std::endl; @@ -159,7 +153,7 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } @@ -170,7 +164,6 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const } // TODO : Delete this - std::cout << "speed : " << speed << std::endl; std::cout << "min1 : " << min1 << std::endl; std::cout << "max1 : " << max1 << std::endl; std::cout << "min2 : " << min2 << std::endl; @@ -192,7 +185,7 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } @@ -203,7 +196,6 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const } // TODO : Delete this - std::cout << "Speed : " << speed << std::endl; std::cout << "min1 : " << min1 << std::endl; std::cout << "max1 : " << max1 << std::endl; std::cout << "min2 : " << min2 << std::endl; @@ -218,7 +210,7 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } @@ -238,12 +230,11 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const 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; - double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } @@ -262,7 +253,7 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } @@ -284,10 +275,8 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const std::cout << "CONTACT FOUND " << std::endl; // Compute the collision contact - computeContact(obb1, obb2, normal.getUnit(), minPenetrationDepth, obb1->getExtremeVertices(normal), obb2->getExtremeVertices(normal.getOpposite()), contact) + computeContact(obb1, obb2, normal.getUnit(), minPenetrationDepth, obb1->getExtremeVertices(normal), obb2->getExtremeVertices(normal.getOpposite()), contact); - //std::cout << "Contact 1 : " << contact << std::endl; - assert(*contact != 0); return true; } @@ -299,7 +288,7 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } @@ -317,7 +306,7 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } @@ -335,7 +324,7 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } @@ -353,7 +342,7 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } @@ -371,7 +360,7 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } @@ -389,7 +378,7 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } @@ -407,7 +396,7 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } @@ -425,7 +414,7 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } @@ -443,7 +432,7 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); + penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } @@ -456,13 +445,10 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const // 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; + std::cout << "CONTACT FOUND" << std::endl; // Compute the collision contact - computeContact(obb1, obb2, normal.getUnit(), minPenetrationDepth, obb1->getExtremeVertices(normal), obb2->getExtremeVertices(normal.getOpposite()), contact) - - // We have found no separation axis, therefore the two OBBs must collide - assert(*contact != 0); + computeContact(obb1, obb2, normal.getUnit(), minPenetrationDepth, obb1->getExtremeVertices(normal), obb2->getExtremeVertices(normal.getOpposite()), contact); return true; } @@ -478,8 +464,8 @@ double NarrowPhaseSATAlgorithm::computePenetrationDepth(double min1, double max1 double lengthInterval2 = max2 - min2; // Compute the total length of both intervals - double minExtreme = min(min1, min2); - double maxExtreme = max(max1, max2); + double minExtreme = std::min(min1, min2); + double maxExtreme = std::max(max1, max2); double lengthBothIntervals = maxExtreme - minExtreme; // Compute the current penetration depth @@ -513,7 +499,7 @@ void NarrowPhaseSATAlgorithm::computeContact(const OBB* const obb1, const OBB* c // Create a new contact contact = new Contact(obb1, obb2, normal, obb1ExtremePoints); } - else if(nbVerticesExtremmeOBB2 == 1) { // If its a Vertex-Something contact + else if(nbVerticesExtremeOBB2 == 1) { // If its a Vertex-Something contact // Create a new contact contact = new Contact(obb1, obb2, normal, obb2ExtremePoints); } @@ -530,5 +516,5 @@ void NarrowPhaseSATAlgorithm::computeContact(const OBB* const obb1, const OBB* c // TODO : Complete this ... } - assert(*contact == 0); + assert(*contact != 0); } diff --git a/sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.h b/sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.h index 2def697c..02afea69 100644 --- a/sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.h +++ b/sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.h @@ -45,7 +45,7 @@ namespace reactphysics3d { class NarrowPhaseSATAlgorithm : public NarrowPhaseAlgorithm { private : bool computeCollisionTest(const OBB* const obb1, const OBB* const obb2, Contact** contact) const; // Return true and compute a collision contact if the two OBB collide - double computePenetrationDepth(double min1, double max1, double min2, double max2, bool& side); // Compute the penetration depth of two projection intervals + double computePenetrationDepth(double min1, double max1, double min2, double max2, bool& side) const; // Compute the penetration depth of two projection intervals void computeContact(const OBB* const obb1, const OBB* const obb2, const Vector3D normal, double penetrationDepth, const std::vector& obb1ExtremePoints, const std::vector& obb2ExtremePoints, Contact** contact) const; // Compute a new contact // Compute a new collision contact between two projection intervals Vector3D computeContactNormal(const Vector3D& axis, const Vector3D& distanceOfOBBs) const; // Compute a contact normal @@ -61,8 +61,8 @@ class NarrowPhaseSATAlgorithm : public NarrowPhaseAlgorithm { // Return the contact normal with the correct sign (from obb1 toward obb2). "axis" is the axis vector direction where the // collision occur and "distanceOfOBBs" is the vector (obb2.center - obb1.center). -inline Vector3D NarrowPhaseAlgorithm::computeContactNormal(const Vector3D& axis, const Vector3D& distanceOfOBBs) const { - if (distanceOfOBBs.scalarProduct(axis) >= 0) { +inline Vector3D NarrowPhaseSATAlgorithm::computeContactNormal(const Vector3D& axis, const Vector3D& distanceOfOBBs) const { + if (distanceOfOBBs.scalarProduct(axis) >= 0.0) { return axis; } else {