| |
| /* |
| Bullet Continuous Collision Detection and Physics Library |
| Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ |
| |
| This software is provided 'as-is', without any express or implied warranty. |
| In no event will the authors be held liable for any damages arising from the use of this software. |
| Permission is granted to anyone to use this software for any purpose, |
| including commercial applications, and to alter it and redistribute it freely, |
| subject to the following restrictions: |
| |
| 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. |
| 3. This notice may not be removed or altered from any source distribution. |
| |
| Elsevier CDROM license agreements grants nonexclusive license to use the software |
| for any purpose, commercial or non-commercial as long as the following credit is included |
| identifying the original source of the software: |
| |
| Parts of the source are "from the book Real-Time Collision Detection by |
| Christer Ericson, published by Morgan Kaufmann Publishers, |
| (c) 2005 Elsevier Inc." |
| |
| */ |
| |
| |
| #include "BulletCollision/NarrowPhaseCollision/btVoronoiSimplexSolver.h" |
| |
| #define VERTA 0 |
| #define VERTB 1 |
| #define VERTC 2 |
| #define VERTD 3 |
| |
| #define CATCH_DEGENERATE_TETRAHEDRON 1 |
| void btVoronoiSimplexSolver::removeVertex(int index) |
| { |
| |
| btAssert(m_numVertices>0); |
| m_numVertices--; |
| m_simplexVectorW[index] = m_simplexVectorW[m_numVertices]; |
| m_simplexPointsP[index] = m_simplexPointsP[m_numVertices]; |
| m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices]; |
| } |
| |
| void btVoronoiSimplexSolver::reduceVertices (const btUsageBitfield& usedVerts) |
| { |
| if ((numVertices() >= 4) && (!usedVerts.usedVertexD)) |
| removeVertex(3); |
| |
| if ((numVertices() >= 3) && (!usedVerts.usedVertexC)) |
| removeVertex(2); |
| |
| if ((numVertices() >= 2) && (!usedVerts.usedVertexB)) |
| removeVertex(1); |
| |
| if ((numVertices() >= 1) && (!usedVerts.usedVertexA)) |
| removeVertex(0); |
| |
| } |
| |
| |
| |
| |
| |
| //clear the simplex, remove all the vertices |
| void btVoronoiSimplexSolver::reset() |
| { |
| m_cachedValidClosest = false; |
| m_numVertices = 0; |
| m_needsUpdate = true; |
| m_lastW = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT)); |
| m_cachedBC.reset(); |
| } |
| |
| |
| |
| //add a vertex |
| void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, const btVector3& q) |
| { |
| m_lastW = w; |
| m_needsUpdate = true; |
| |
| m_simplexVectorW[m_numVertices] = w; |
| m_simplexPointsP[m_numVertices] = p; |
| m_simplexPointsQ[m_numVertices] = q; |
| |
| m_numVertices++; |
| } |
| |
| bool btVoronoiSimplexSolver::updateClosestVectorAndPoints() |
| { |
| |
| if (m_needsUpdate) |
| { |
| m_cachedBC.reset(); |
| |
| m_needsUpdate = false; |
| |
| switch (numVertices()) |
| { |
| case 0: |
| m_cachedValidClosest = false; |
| break; |
| case 1: |
| { |
| m_cachedP1 = m_simplexPointsP[0]; |
| m_cachedP2 = m_simplexPointsQ[0]; |
| m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0] |
| m_cachedBC.reset(); |
| m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.)); |
| m_cachedValidClosest = m_cachedBC.isValid(); |
| break; |
| }; |
| case 2: |
| { |
| //closest point origin from line segment |
| const btVector3& from = m_simplexVectorW[0]; |
| const btVector3& to = m_simplexVectorW[1]; |
| btVector3 nearest; |
| |
| btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); |
| btVector3 diff = p - from; |
| btVector3 v = to - from; |
| btScalar t = v.dot(diff); |
| |
| if (t > 0) { |
| btScalar dotVV = v.dot(v); |
| if (t < dotVV) { |
| t /= dotVV; |
| diff -= t*v; |
| m_cachedBC.m_usedVertices.usedVertexA = true; |
| m_cachedBC.m_usedVertices.usedVertexB = true; |
| } else { |
| t = 1; |
| diff -= v; |
| //reduce to 1 point |
| m_cachedBC.m_usedVertices.usedVertexB = true; |
| } |
| } else |
| { |
| t = 0; |
| //reduce to 1 point |
| m_cachedBC.m_usedVertices.usedVertexA = true; |
| } |
| m_cachedBC.setBarycentricCoordinates(1-t,t); |
| nearest = from + t*v; |
| |
| m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]); |
| m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]); |
| m_cachedV = m_cachedP1 - m_cachedP2; |
| |
| reduceVertices(m_cachedBC.m_usedVertices); |
| |
| m_cachedValidClosest = m_cachedBC.isValid(); |
| break; |
| } |
| case 3: |
| { |
| //closest point origin from triangle |
| btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); |
| |
| const btVector3& a = m_simplexVectorW[0]; |
| const btVector3& b = m_simplexVectorW[1]; |
| const btVector3& c = m_simplexVectorW[2]; |
| |
| closestPtPointTriangle(p,a,b,c,m_cachedBC); |
| m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + |
| m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + |
| m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2]; |
| |
| m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + |
| m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + |
| m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2]; |
| |
| m_cachedV = m_cachedP1-m_cachedP2; |
| |
| reduceVertices (m_cachedBC.m_usedVertices); |
| m_cachedValidClosest = m_cachedBC.isValid(); |
| |
| break; |
| } |
| case 4: |
| { |
| |
| |
| btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.)); |
| |
| const btVector3& a = m_simplexVectorW[0]; |
| const btVector3& b = m_simplexVectorW[1]; |
| const btVector3& c = m_simplexVectorW[2]; |
| const btVector3& d = m_simplexVectorW[3]; |
| |
| bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC); |
| |
| if (hasSeperation) |
| { |
| |
| m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] + |
| m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] + |
| m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] + |
| m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3]; |
| |
| m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] + |
| m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] + |
| m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] + |
| m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3]; |
| |
| m_cachedV = m_cachedP1-m_cachedP2; |
| reduceVertices (m_cachedBC.m_usedVertices); |
| } else |
| { |
| // printf("sub distance got penetration\n"); |
| |
| if (m_cachedBC.m_degenerate) |
| { |
| m_cachedValidClosest = false; |
| } else |
| { |
| m_cachedValidClosest = true; |
| //degenerate case == false, penetration = true + zero |
| m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); |
| } |
| break; |
| } |
| |
| m_cachedValidClosest = m_cachedBC.isValid(); |
| |
| //closest point origin from tetrahedron |
| break; |
| } |
| default: |
| { |
| m_cachedValidClosest = false; |
| } |
| }; |
| } |
| |
| return m_cachedValidClosest; |
| |
| } |
| |
| //return/calculate the closest vertex |
| bool btVoronoiSimplexSolver::closest(btVector3& v) |
| { |
| bool succes = updateClosestVectorAndPoints(); |
| v = m_cachedV; |
| return succes; |
| } |
| |
| |
| |
| btScalar btVoronoiSimplexSolver::maxVertex() |
| { |
| int i, numverts = numVertices(); |
| btScalar maxV = btScalar(0.); |
| for (i=0;i<numverts;i++) |
| { |
| btScalar curLen2 = m_simplexVectorW[i].length2(); |
| if (maxV < curLen2) |
| maxV = curLen2; |
| } |
| return maxV; |
| } |
| |
| |
| |
| //return the current simplex |
| int btVoronoiSimplexSolver::getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const |
| { |
| int i; |
| for (i=0;i<numVertices();i++) |
| { |
| yBuf[i] = m_simplexVectorW[i]; |
| pBuf[i] = m_simplexPointsP[i]; |
| qBuf[i] = m_simplexPointsQ[i]; |
| } |
| return numVertices(); |
| } |
| |
| |
| |
| |
| bool btVoronoiSimplexSolver::inSimplex(const btVector3& w) |
| { |
| bool found = false; |
| int i, numverts = numVertices(); |
| //btScalar maxV = btScalar(0.); |
| |
| //w is in the current (reduced) simplex |
| for (i=0;i<numverts;i++) |
| { |
| if (m_simplexVectorW[i] == w) |
| found = true; |
| } |
| |
| //check in case lastW is already removed |
| if (w == m_lastW) |
| return true; |
| |
| return found; |
| } |
| |
| void btVoronoiSimplexSolver::backup_closest(btVector3& v) |
| { |
| v = m_cachedV; |
| } |
| |
| |
| bool btVoronoiSimplexSolver::emptySimplex() const |
| { |
| return (numVertices() == 0); |
| |
| } |
| |
| void btVoronoiSimplexSolver::compute_points(btVector3& p1, btVector3& p2) |
| { |
| updateClosestVectorAndPoints(); |
| p1 = m_cachedP1; |
| p2 = m_cachedP2; |
| |
| } |
| |
| |
| |
| |
| bool btVoronoiSimplexSolver::closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result) |
| { |
| result.m_usedVertices.reset(); |
| |
| // Check if P in vertex region outside A |
| btVector3 ab = b - a; |
| btVector3 ac = c - a; |
| btVector3 ap = p - a; |
| btScalar d1 = ab.dot(ap); |
| btScalar d2 = ac.dot(ap); |
| if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0)) |
| { |
| result.m_closestPointOnSimplex = a; |
| result.m_usedVertices.usedVertexA = true; |
| result.setBarycentricCoordinates(1,0,0); |
| return true;// a; // barycentric coordinates (1,0,0) |
| } |
| |
| // Check if P in vertex region outside B |
| btVector3 bp = p - b; |
| btScalar d3 = ab.dot(bp); |
| btScalar d4 = ac.dot(bp); |
| if (d3 >= btScalar(0.0) && d4 <= d3) |
| { |
| result.m_closestPointOnSimplex = b; |
| result.m_usedVertices.usedVertexB = true; |
| result.setBarycentricCoordinates(0,1,0); |
| |
| return true; // b; // barycentric coordinates (0,1,0) |
| } |
| // Check if P in edge region of AB, if so return projection of P onto AB |
| btScalar vc = d1*d4 - d3*d2; |
| if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) { |
| btScalar v = d1 / (d1 - d3); |
| result.m_closestPointOnSimplex = a + v * ab; |
| result.m_usedVertices.usedVertexA = true; |
| result.m_usedVertices.usedVertexB = true; |
| result.setBarycentricCoordinates(1-v,v,0); |
| return true; |
| //return a + v * ab; // barycentric coordinates (1-v,v,0) |
| } |
| |
| // Check if P in vertex region outside C |
| btVector3 cp = p - c; |
| btScalar d5 = ab.dot(cp); |
| btScalar d6 = ac.dot(cp); |
| if (d6 >= btScalar(0.0) && d5 <= d6) |
| { |
| result.m_closestPointOnSimplex = c; |
| result.m_usedVertices.usedVertexC = true; |
| result.setBarycentricCoordinates(0,0,1); |
| return true;//c; // barycentric coordinates (0,0,1) |
| } |
| |
| // Check if P in edge region of AC, if so return projection of P onto AC |
| btScalar vb = d5*d2 - d1*d6; |
| if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) { |
| btScalar w = d2 / (d2 - d6); |
| result.m_closestPointOnSimplex = a + w * ac; |
| result.m_usedVertices.usedVertexA = true; |
| result.m_usedVertices.usedVertexC = true; |
| result.setBarycentricCoordinates(1-w,0,w); |
| return true; |
| //return a + w * ac; // barycentric coordinates (1-w,0,w) |
| } |
| |
| // Check if P in edge region of BC, if so return projection of P onto BC |
| btScalar va = d3*d6 - d5*d4; |
| if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) { |
| btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); |
| |
| result.m_closestPointOnSimplex = b + w * (c - b); |
| result.m_usedVertices.usedVertexB = true; |
| result.m_usedVertices.usedVertexC = true; |
| result.setBarycentricCoordinates(0,1-w,w); |
| return true; |
| // return b + w * (c - b); // barycentric coordinates (0,1-w,w) |
| } |
| |
| // P inside face region. Compute Q through its barycentric coordinates (u,v,w) |
| btScalar denom = btScalar(1.0) / (va + vb + vc); |
| btScalar v = vb * denom; |
| btScalar w = vc * denom; |
| |
| result.m_closestPointOnSimplex = a + ab * v + ac * w; |
| result.m_usedVertices.usedVertexA = true; |
| result.m_usedVertices.usedVertexB = true; |
| result.m_usedVertices.usedVertexC = true; |
| result.setBarycentricCoordinates(1-v-w,v,w); |
| |
| return true; |
| // return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w |
| |
| } |
| |
| |
| |
| |
| |
| /// Test if point p and d lie on opposite sides of plane through abc |
| int btVoronoiSimplexSolver::pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d) |
| { |
| btVector3 normal = (b-a).cross(c-a); |
| |
| btScalar signp = (p - a).dot(normal); // [AP AB AC] |
| btScalar signd = (d - a).dot( normal); // [AD AB AC] |
| |
| #ifdef CATCH_DEGENERATE_TETRAHEDRON |
| #ifdef BT_USE_DOUBLE_PRECISION |
| if (signd * signd < (btScalar(1e-8) * btScalar(1e-8))) |
| { |
| return -1; |
| } |
| #else |
| if (signd * signd < (btScalar(1e-4) * btScalar(1e-4))) |
| { |
| // printf("affine dependent/degenerate\n");// |
| return -1; |
| } |
| #endif |
| |
| #endif |
| // Points on opposite sides if expression signs are opposite |
| return signp * signd < btScalar(0.); |
| } |
| |
| |
| bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult) |
| { |
| btSubSimplexClosestResult tempResult; |
| |
| // Start out assuming point inside all halfspaces, so closest to itself |
| finalResult.m_closestPointOnSimplex = p; |
| finalResult.m_usedVertices.reset(); |
| finalResult.m_usedVertices.usedVertexA = true; |
| finalResult.m_usedVertices.usedVertexB = true; |
| finalResult.m_usedVertices.usedVertexC = true; |
| finalResult.m_usedVertices.usedVertexD = true; |
| |
| int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d); |
| int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b); |
| int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c); |
| int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a); |
| |
| if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0) |
| { |
| finalResult.m_degenerate = true; |
| return false; |
| } |
| |
| if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC) |
| { |
| return false; |
| } |
| |
| |
| btScalar bestSqDist = FLT_MAX; |
| // If point outside face abc then compute closest point on abc |
| if (pointOutsideABC) |
| { |
| closestPtPointTriangle(p, a, b, c,tempResult); |
| btVector3 q = tempResult.m_closestPointOnSimplex; |
| |
| btScalar sqDist = (q - p).dot( q - p); |
| // Update best closest point if (squared) distance is less than current best |
| if (sqDist < bestSqDist) { |
| bestSqDist = sqDist; |
| finalResult.m_closestPointOnSimplex = q; |
| //convert result bitmask! |
| finalResult.m_usedVertices.reset(); |
| finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; |
| finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB; |
| finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; |
| finalResult.setBarycentricCoordinates( |
| tempResult.m_barycentricCoords[VERTA], |
| tempResult.m_barycentricCoords[VERTB], |
| tempResult.m_barycentricCoords[VERTC], |
| 0 |
| ); |
| |
| } |
| } |
| |
| |
| // Repeat test for face acd |
| if (pointOutsideACD) |
| { |
| closestPtPointTriangle(p, a, c, d,tempResult); |
| btVector3 q = tempResult.m_closestPointOnSimplex; |
| //convert result bitmask! |
| |
| btScalar sqDist = (q - p).dot( q - p); |
| if (sqDist < bestSqDist) |
| { |
| bestSqDist = sqDist; |
| finalResult.m_closestPointOnSimplex = q; |
| finalResult.m_usedVertices.reset(); |
| finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; |
| |
| finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB; |
| finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC; |
| finalResult.setBarycentricCoordinates( |
| tempResult.m_barycentricCoords[VERTA], |
| 0, |
| tempResult.m_barycentricCoords[VERTB], |
| tempResult.m_barycentricCoords[VERTC] |
| ); |
| |
| } |
| } |
| // Repeat test for face adb |
| |
| |
| if (pointOutsideADB) |
| { |
| closestPtPointTriangle(p, a, d, b,tempResult); |
| btVector3 q = tempResult.m_closestPointOnSimplex; |
| //convert result bitmask! |
| |
| btScalar sqDist = (q - p).dot( q - p); |
| if (sqDist < bestSqDist) |
| { |
| bestSqDist = sqDist; |
| finalResult.m_closestPointOnSimplex = q; |
| finalResult.m_usedVertices.reset(); |
| finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA; |
| finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC; |
| |
| finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; |
| finalResult.setBarycentricCoordinates( |
| tempResult.m_barycentricCoords[VERTA], |
| tempResult.m_barycentricCoords[VERTC], |
| 0, |
| tempResult.m_barycentricCoords[VERTB] |
| ); |
| |
| } |
| } |
| // Repeat test for face bdc |
| |
| |
| if (pointOutsideBDC) |
| { |
| closestPtPointTriangle(p, b, d, c,tempResult); |
| btVector3 q = tempResult.m_closestPointOnSimplex; |
| //convert result bitmask! |
| btScalar sqDist = (q - p).dot( q - p); |
| if (sqDist < bestSqDist) |
| { |
| bestSqDist = sqDist; |
| finalResult.m_closestPointOnSimplex = q; |
| finalResult.m_usedVertices.reset(); |
| // |
| finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA; |
| finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC; |
| finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB; |
| |
| finalResult.setBarycentricCoordinates( |
| 0, |
| tempResult.m_barycentricCoords[VERTA], |
| tempResult.m_barycentricCoords[VERTC], |
| tempResult.m_barycentricCoords[VERTB] |
| ); |
| |
| } |
| } |
| |
| //help! we ended up full ! |
| |
| if (finalResult.m_usedVertices.usedVertexA && |
| finalResult.m_usedVertices.usedVertexB && |
| finalResult.m_usedVertices.usedVertexC && |
| finalResult.m_usedVertices.usedVertexD) |
| { |
| return true; |
| } |
| |
| return true; |
| } |
| |