mrq/reactphysics3d
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

git-svn-id: https://reactphysics3d.googlecode.com/svn/trunk@226 92aac97c-a6ce-11dd-a772-7fcde58d38e6

Browse Source
This commit is contained in:
chappuis.daniel 2009-12-12 21:27:43 +00:00
parent efb07cade3
commit c049fdd855
2 changed files with 129 additions and 155 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

256
sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.cpp
View File

@ -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 // 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 // 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. // 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 center; // Center of a projection interval
double radius1; // Radius of projection interval [min1, max1] 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 min2; // Minimm of interval 2
double max2; // Maximum of interval 2 double max2; // Maximum of interval 2
Vector3D normal; // Contact normal (correspond to the separation axis with the smallest positive penetration depth) Vector3D normal; // Contact normal (correspond to the separation axis with the smallest positive penetration depth)
//  // The contact normal point out of OBB1 toward OBB2
ContactType contactType; // Current contact type of the contact found so far 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 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 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. 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; max1 = radius1;
min2 = center - radius2; min2 = center - radius2;
max2 = 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 if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false; 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 // TODO : Delete this
std::cout << "Speed : " << speed << std::endl; std::cout << "Speed : " << speed << std::endl;
@ -151,10 +157,15 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const
max1 = radius1; max1 = radius1;
min2 = center - radius2; min2 = center - radius2;
max2 = 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 if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false; 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 // TODO : Delete this
std::cout << "speed : " << speed << std::endl; std::cout << "speed : " << speed << std::endl;
@ -179,10 +190,15 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const
max1 = radius1; max1 = radius1;
min2 = center - radius2; min2 = center - radius2;
max2 = 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 if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false; 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 // TODO : Delete this
std::cout << "Speed : " << speed << std::endl; std::cout << "Speed : " << speed << std::endl;
@ -200,10 +216,15 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const
max1 = radius1; max1 = radius1;
min2 = center - radius2; min2 = center - radius2;
max2 = 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 if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false; 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 // Axis B1
//std::cout << "----- AXIS B1 -----" << std::endl; //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 << "max1 : " << max1 << std::endl;
std::cout << "min2 : " << min2 << std::endl; std::cout << "min2 : " << min2 << std::endl;
std::cout << "max2 : " << max2 << 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 if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false; 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 // Axis B2
udc2[2] = obb2->getAxis(2).scalarProduct(boxDistance); udc2[2] = obb2->getAxis(2).scalarProduct(boxDistance);
@ -235,10 +260,15 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const
max1 = radius1; max1 = radius1;
min2 = center - radius2; min2 = center - radius2;
max2 = 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 if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false; 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 // If there exists a parallel pair of face normals
@ -267,11 +297,15 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const
max1 = radius1; max1 = radius1;
min2 = center - radius2; min2 = center - radius2;
max2 = center + radius2; max2 = center + radius2;
//Vector3D axis = obb1->getAxis(0).crossProduct(obb2->getAxis(0)); double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp);
penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false; 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 // Axis A0 x B1
center = udc1[2] * c[1][1] - udc1[1] * c[2][1]; 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; max1 = radius1;
min2 = center - radius2; min2 = center - radius2;
max2 = center + radius2; max2 = center + radius2;
//axis = obb1->getAxis(0).crossProduct(obb2->getAxis(1)); double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp);
penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false; 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 // Axis A0 x B2
center = udc1[2] * c[1][2] - udc1[1] * c[2][2]; 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; max1 = radius1;
min2 = center - radius2; min2 = center - radius2;
max2 = center + radius2; max2 = center + radius2;
//axis = obb1->getAxis(0).crossProduct(obb2->getAxis(2)); double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp);
penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false; 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 // Axis A1 x B0
center = udc1[0] * c[2][0] - udc1[2] * c[0][0]; 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; max1 = radius1;
min2 = center - radius2; min2 = center - radius2;
max2 = center + radius2; max2 = center + radius2;
//axis = obb1->getAxis(1).crossProduct(obb2->getAxis(0)); double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp);
penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false; 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 // Axis A1 x B1
center = udc1[0] * c[2][1] - udc1[2] * c[0][1]; 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; max1 = radius1;
min2 = center - radius2; min2 = center - radius2;
max2 = center + radius2; max2 = center + radius2;
//axis = obb1->getAxis(1).crossProduct(obb2->getAxis(1)); double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp);
penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false; 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 // Axis A1 x B2
center = udc1[0] * c[2][2] - udc1[2] * c[0][2]; 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; max1 = radius1;
min2 = center - radius2; min2 = center - radius2;
max2 = center + radius2; max2 = center + radius2;
//axis = obb1->getAxis(1).crossProduct(obb2->getAxis(2)); double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp);
penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false; 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 // Axis A2 x B0
center = udc1[1] * c[0][0] - udc1[0] * c[1][0]; 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; max1 = radius1;
min2 = center - radius2; min2 = center - radius2;
max2 = center + radius2; max2 = center + radius2;
//axis = obb1->getAxis(2).crossProduct(obb2->getAxis(0)); double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp);
penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false; 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 // Axis A2 x B1
center = udc1[1] * c[0][1] - udc1[0] * c[1][1]; 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; max1 = radius1;
min2 = center - radius2; min2 = center - radius2;
max2 = center + radius2; max2 = center + radius2;
//axis = obb1->getAxis(2).crossProduct(obb2->getAxis(1)); double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp);
penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false; 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 // Axis A2 x B2
center = udc1[1] * c[0][2] - udc1[0] * c[1][2]; 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; max1 = radius1;
min2 = center - radius2; min2 = center - radius2;
max2 = center + radius2; max2 = center + radius2;
//axis = obb1->getAxis(2).crossProduct(obb2->getAxis(2)); double penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth, sideTemp);
penetrationDepth = computePenetrationDepth(min1, max1, min2, max2, minPenetrationDepth);
if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide if (penetrationDepth < 0) { // We have found a separation axis, therefore the two OBBs don't collide
return false; 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 // TODO : Delete this
//(*contact) = new Contact(obb1->getBodyPointer(), obb2->getBodyPointer(), Vector3D(1,0,0), timeFirst); //(*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 // Compute the collision contact
// TODO : The normal has to be unit before passing it to the computeContact() method // 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 // We have found no separation axis, therefore the two OBBs must collide
assert(*contact != 0); assert(*contact != 0);
@ -400,12 +466,11 @@ bool NarrowPhaseSATAlgorithm::computeCollisionTest(const OBB* const obb1, const
return true; return true;
} }
// This method computes penetration depth between two intervals and update the minimum penetration // This method computes and returns the penetration depth between two intervals. This method returns the computed
// depth found so far if the computed penetration depth is positive (the penetration depth in case // penetration depth (note that it could return a negative penetration depth if the intervals are separated. This
// of collision has to be positive) and smaller than the current minimum penetration // method also find which interval is at the left of the other in order to know which extreme of interval 1 collides with
// depth. This method returns the computed penetration depth (note that it could return a negative // which extreme of interval 2 if a collision occur.
// penetration depth if the intervals are separated. double NarrowPhaseSATAlgorithm::computePenetrationDepth(double min1, double max1, double min2, double max2, bool& side) const {
double NarrowPhaseSATAlgorithm::computePenetrationDepth(double min1, double max1, double min2, double max2, double& minPenetrationDepth) {
// Compute the length of both intervals // Compute the length of both intervals
double lengthInterval1 = max1 - min1; double lengthInterval1 = max1 - min1;
@ -419,122 +484,25 @@ double NarrowPhaseSATAlgorithm::computePenetrationDepth(double min1, double max1
// Compute the current penetration depth // Compute the current penetration depth
double penetrationDepth = (lengthInterval1 + lengthInterval2) - lengthBothIntervals; double penetrationDepth = (lengthInterval1 + lengthInterval2) - lengthBothIntervals;
// If the current penetration depth is smaller than the minimum penetration depth // Find which interval is at the left of the other
if (penetrationDepth < minPenetrationDepth && penetrationDepth >= 0) { if (abs(max1-min2) <= abs(max2-min1)) {
minPenetrationDepth = penetrationDepth; // Right of interval 1 collides with the left of interval 2
side = currentSide; side = true;
}
else {
// Right of interval 2 collides with the left of interval 1
side = false;
} }
// Return the computed penetration depth // Return the computed penetration depth
return penetrationDepth; return penetrationDepth;
} }
// Compute a new collision contact between two projection intervals. // Compute a new collision contact between two OBBs
// Warning : If the side value is true the max of interval1 collides with the min of interval2. If the void NarrowPhaseSATAlgorithm::computeContact(const OBB* const obb1, const OBB* const obb2, const Vector3D normal, double penetrationDepth,
// side value is false the max value of interval2 collides with the min value of interval1. const std::vector<Vector3D>& obb1Extremepoints, const std::vector<Vector3D>& obb2ExtremePoints, Contact** contact) const {
void NarrowPhaseSATAlgorithm::computeContact(const ProjectionInterval& interval1, const ProjectionInterval& interval2,
bool side, Contact** contact) {
assert(*contact == 0); assert(*contact == 0);
ProjectionInterval intervalLeft = (side) ? interval1 : interval2; // To complete ...
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<Vector3D> leftExtremePointsAtContact = movePoints(intervalLeft.getMaxProjectedPoints(), velocityLeft * time.getValue());
//std::vector<Vector3D> rightExtremePointsAtContact = movePoints(intervalRight.getMinProjectedPoints(), velocityRight * time.getValue());
// TODO : ADD THE BODY ADRESS INTO THE CONTACT HERE
// Get the rigid bodies
//RigidBody* body1 = dynamic_cast<RigidBody*>(intervalLeft.getBoundingVolumePointer()->getBodyPointer());
//RigidBody* body2 = dynamic_cast<RigidBody*>(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;
}
*/
} }

28
sources/reactphysics3d/collision/NarrowPhaseSATAlgorithm.h
View File

@ -25,13 +25,6 @@
#include "../constraint/Contact.h" #include "../constraint/Contact.h"
#include "../body/OBB.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 // ReactPhysics3D namespace
namespace reactphysics3d { namespace reactphysics3d {
@ -51,11 +44,11 @@ namespace reactphysics3d {
*/ */
class NarrowPhaseSATAlgorithm : public NarrowPhaseAlgorithm { class NarrowPhaseSATAlgorithm : public NarrowPhaseAlgorithm {
private : 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 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, double& minPenetrationDepth); // Compute the penetration depth of two projection intervals 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, 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<Vector3D>& obb1Extremepoints, const std::vector<Vector3D>& 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 : public :
NarrowPhaseSATAlgorithm(); // Constructor NarrowPhaseSATAlgorithm(); // Constructor
~NarrowPhaseSATAlgorithm(); // Destructor ~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 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 } // End of the ReactPhysics3D namespace
#endif #endif