diff --git a/sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.cpp b/sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.cpp index d4cd7401..c570ccff 100644 --- a/sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.cpp +++ b/sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.cpp @@ -78,7 +78,7 @@ bool NarrowPhaseSATAlgorithm::testCollision(const BoundingVolume* const bounding // 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) { +bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const OBB* const obb2, Contact** contact) const { double center; // Center of a projection interval double radius1; // Radius of projection interval [min1, max1] @@ -88,8 +88,8 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const double min2; // Minimm of interval 2 double max2; // Maximum of interval 2 Vector3D normal; // Contact normal (correspond to the separation axis with the smallest positive penetration depth) - //  - ContactType contactType; // Current contact type of the contact found so far + // The contact normal point out of OBB1 toward OBB2 + bool side; // True if the interval 1 is at the left of interval 2 if a collision occurs and false otherwise double minPenetrationDepth = 0.0; // Minimum penetration depth detected among all separated axis 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. @@ -122,10 +122,16 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth); + bool sideTemp; + double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + side = sideTemp; + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb1->getAxis(0), boxDistance); // Compute the contact normal with the correct sign + } // TODO : Delete this std::cout << "Speed : " << speed << std::endl; @@ -151,10 +157,15 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth); + double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + side = sideTemp; + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb1->getAxis(1), boxDistance); // Compute the contact normal with the correct sign + } // TODO : Delete this std::cout << "speed : " << speed << std::endl; @@ -179,10 +190,15 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth); + double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + side = sideTemp; + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb1->getAxis(2), boxDistance); // Compute the contact normal with the correct sign + } // TODO : Delete this std::cout << "Speed : " << speed << std::endl; @@ -200,10 +216,15 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth); + double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + side = sideTemp; + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb2->getAxis(0), boxDistance); // Compute the contact normal with the correct sign + } // Axis B1 //std::cout << "----- AXIS B1 -----" << std::endl; @@ -220,11 +241,15 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const std::cout << "max1 : " << max1 << std::endl; std::cout << "min2 : " << min2 << std::endl; std::cout << "max2 : " << max2 << std::endl; - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth); + double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } - + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + side = sideTemp; + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb2->getAxis(1), boxDistance); // Compute the contact normal with the correct sign + } // Axis B2 udc2[2] = obb2->getAxis(2).scalarProduct(boxDistance); @@ -235,10 +260,15 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth); + double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + side = sideTemp; + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb2->getAxis(2), boxDistance); // Compute the contact normal with the correct sign + } // If there exists a parallel pair of face normals @@ -267,11 +297,15 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - //Vector3D axis = obb1->getAxis(0).crossProduct(obb2->getAxis(0)); - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth); + double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + side = sideTemp; + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb1->getAxis(0).crossProduct(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign + } // Axis A0 x B1 center = udc1[2] * c[1][1] - udc1[1] * c[2][1]; @@ -281,11 +315,15 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - //axis = obb1->getAxis(0).crossProduct(obb2->getAxis(1)); - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth); + double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + side = sideTemp; + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb1->getAxis(0).crossProduct(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign + } // Axis A0 x B2 center = udc1[2] * c[1][2] - udc1[1] * c[2][2]; @@ -295,11 +333,15 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - //axis = obb1->getAxis(0).crossProduct(obb2->getAxis(2)); - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth); + double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + side = sideTemp; + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb1->getAxis(0).crossProduct(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign + } // Axis A1 x B0 center = udc1[0] * c[2][0] - udc1[2] * c[0][0]; @@ -309,11 +351,15 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - //axis = obb1->getAxis(1).crossProduct(obb2->getAxis(0)); - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth); + double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + side = sideTemp; + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb1->getAxis(1).crossProduct(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign + } // Axis A1 x B1 center = udc1[0] * c[2][1] - udc1[2] * c[0][1]; @@ -323,11 +369,15 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - //axis = obb1->getAxis(1).crossProduct(obb2->getAxis(1)); - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth); + double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + side = sideTemp; + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb1->getAxis(1).crossProduct(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign + } // Axis A1 x B2 center = udc1[0] * c[2][2] - udc1[2] * c[0][2]; @@ -337,11 +387,15 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - //axis = obb1->getAxis(1).crossProduct(obb2->getAxis(2)); - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth); + double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + side = sideTemp; + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb1->getAxis(1).crossProduct(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign + } // Axis A2 x B0 center = udc1[1] * c[0][0] - udc1[0] * c[1][0]; @@ -351,11 +405,15 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - //axis = obb1->getAxis(2).crossProduct(obb2->getAxis(0)); - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth); + double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + side = sideTemp; + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb1->getAxis(2).crossProduct(obb2->getAxis(0)), boxDistance); // Compute the contact normal with the correct sign + } // Axis A2 x B1 center = udc1[1] * c[0][1] - udc1[0] * c[1][1]; @@ -365,11 +423,15 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - //axis = obb1->getAxis(2).crossProduct(obb2->getAxis(1)); - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth); + double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + side = sideTemp; + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb1->getAxis(2).crossProduct(obb2->getAxis(1)), boxDistance); // Compute the contact normal with the correct sign + } // Axis A2 x B2 center = udc1[1] * c[0][2] - udc1[0] * c[1][2]; @@ -379,11 +441,15 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const max1 = radius1; min2 = center - radius2; max2 = center + radius2; - //axis = obb1->getAxis(2).crossProduct(obb2->getAxis(2)); - penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth); + double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp); if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide return false; } + else if (penetrationDepth < minPenetrationDepth) { // Interval 1 and 2 overlap with a smaller penetration depth on this axis + side = sideTemp; + minPenetrationDepth = penetrationDepth; // Update the minimum penetration depth + normal = computeContactNormal(obb1->getAxis(2).crossProduct(obb2->getAxis(2)), boxDistance); // Compute the contact normal with the correct sign + } // TODO : Delete this //(*contact) = new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), Vector3D(1,0,0), timeFirst); @@ -392,7 +458,7 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const // Compute the collision contact // TODO : The normal has to be unit before passing it to the computeContact() method - computeContact(contact); + computeContact(obb1, obb2, normal.getUnit(), minPenetrationDepth, getExtremeVertices(???), getExtremeVertices(???), Contact** contact) // We have found no separation axis, therefore the two OBBs must collide assert(*contact != 0); @@ -400,12 +466,11 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const return true; } -// This method computes penetration depth between two intervals and update the minimum penetration -// depth found so far if the computed penetration depth is positive (the penetration depth in case -// of collision has to be positive) and smaller than the current minimum penetration -// depth. This method returns the computed penetration depth (note that it could return a negative -// penetration depth if the intervals are separated. -double NarrowPhaseSATAlgorithm::computePenetrationDepth(double min1, double max1, double min2, double max2, double& minPenetrationDepth) { +// This method computes and returns the penetration depth between two intervals. This method returns the computed +// penetration depth (note that it could return a negative penetration depth if the intervals are separated. This +// method also find which interval is at the left of the other in order to know which extreme of interval 1 collides with +// which extreme of interval 2 if a collision occur. +double NarrowPhaseSATAlgorithm::computePenetrationDepth(double min1, double max1, double min2, double max2, bool& side) const { // Compute the length of both intervals double lengthInterval1 = max1 - min1; @@ -419,122 +484,25 @@ double NarrowPhaseSATAlgorithm::computePenetrationDepth(double min1, double max1 // Compute the current penetration depth double penetrationDepth = (lengthInterval1 + lengthInterval2) - lengthBothIntervals; - // If the current penetration depth is smaller than the minimum penetration depth - if (penetrationDepth < minPenetrationDepth && penetrationDepth >= 0) { - minPenetrationDepth = penetrationDepth; - side = currentSide; + // Find which interval is at the left of the other + if (abs(max1-min2) <= abs(max2-min1)) { + // Right of interval 1 collides with the left of interval 2 + side = true; + } + else { + // Right of interval 2 collides with the left of interval 1 + side = false; } // Return the computed penetration depth return penetrationDepth; } -// 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, - bool side, Contact** contact) { +// Compute a new collision contact between two OBBs +void NarrowPhaseSATAlgorithm::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 { 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; - } - */ + // To complete ... } diff --git a/sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.h b/sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.h index 031f90d1..5301c89b 100644 --- a/sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.h +++ b/sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.h @@ -25,13 +25,6 @@ #include "../constraint/Contact.h" #include "../body/OBB.h" -// Enumeration for the contact type -enum ContactType { - EDGE_EDGE, // Contact between an edge of OBB1 and an edge of OBB2 - FACEOBB1_SOMETHING, // Contact between a face of OBB1 and eiter a vertex, an edge or a face of OBB2 - FACEOBB2_SOMETHING // Contact between a face of OBB2 and either a vertex, an edge or a face of OBB1 -} - // ReactPhysics3D namespace namespace reactphysics3d { @@ -51,11 +44,11 @@ namespace reactphysics3d { */ class NarrowPhaseSATAlgorithm : public NarrowPhaseAlgorithm { private : - bool computeCollisionTest(const OBB* const obb1, const OBB* const obb2, Contact** contact); // Return true and compute a collision contact if the two OBB collide - double computePenetrationDepth(double min1, double max1, double min2, double max2, double& minPenetrationDepth); // Compute the penetration depth of two projection intervals + 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 void computeContact(const OBB* const obb1, const OBB* const obb2, const Vector3D normal, double penetrationDepth, - ContactType contactType,Contact** contact); // Compute a new collision contact between two projection intervals - + 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 public : NarrowPhaseSATAlgorithm(); // Constructor ~NarrowPhaseSATAlgorithm(); // Destructor @@ -63,6 +56,19 @@ class NarrowPhaseSATAlgorithm : public NarrowPhaseAlgorithm { virtual bool testCollision(const BoundingVolume* const boundingVolume1, const BoundingVolume* const boundingVolume2, Contact** contact); // Return true and compute a collision contact if the two bounding volume collide }; +// --- Inlines function --- // + +// 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) { + return axis; + } + else { + return axis.getOpposite(); + } +} + } // End of the ReactPhysics3D namespace #endif