summaryrefslogtreecommitdiff
path: root/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp')
-rw-r--r--thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp609
1 files changed, 609 insertions, 0 deletions
diff --git a/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp b/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp
new file mode 100644
index 0000000000..cf3d5ef49d
--- /dev/null
+++ b/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp
@@ -0,0 +1,609 @@
+
+/*
+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 "b3VoronoiSimplexSolver.h"
+
+#define VERTA 0
+#define VERTB 1
+#define VERTC 2
+#define VERTD 3
+
+#define B3_CATCH_DEGENERATE_TETRAHEDRON 1
+void b3VoronoiSimplexSolver::removeVertex(int index)
+{
+
+ b3Assert(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 b3VoronoiSimplexSolver::reduceVertices (const b3UsageBitfield& 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 b3VoronoiSimplexSolver::reset()
+{
+ m_cachedValidClosest = false;
+ m_numVertices = 0;
+ m_needsUpdate = true;
+ m_lastW = b3MakeVector3(b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT),b3Scalar(B3_LARGE_FLOAT));
+ m_cachedBC.reset();
+}
+
+
+
+ //add a vertex
+void b3VoronoiSimplexSolver::addVertex(const b3Vector3& w, const b3Vector3& p, const b3Vector3& 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 b3VoronoiSimplexSolver::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(b3Scalar(1.),b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
+ m_cachedValidClosest = m_cachedBC.isValid();
+ break;
+ };
+ case 2:
+ {
+ //closest point origin from line segment
+ const b3Vector3& from = m_simplexVectorW[0];
+ const b3Vector3& to = m_simplexVectorW[1];
+ b3Vector3 nearest;
+
+ b3Vector3 p =b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
+ b3Vector3 diff = p - from;
+ b3Vector3 v = to - from;
+ b3Scalar t = v.dot(diff);
+
+ if (t > 0) {
+ b3Scalar 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
+ b3Vector3 p =b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
+
+ const b3Vector3& a = m_simplexVectorW[0];
+ const b3Vector3& b = m_simplexVectorW[1];
+ const b3Vector3& 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:
+ {
+
+
+ b3Vector3 p =b3MakeVector3(b3Scalar(0.),b3Scalar(0.),b3Scalar(0.));
+
+ const b3Vector3& a = m_simplexVectorW[0];
+ const b3Vector3& b = m_simplexVectorW[1];
+ const b3Vector3& c = m_simplexVectorW[2];
+ const b3Vector3& 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(b3Scalar(0.),b3Scalar(0.),b3Scalar(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 b3VoronoiSimplexSolver::closest(b3Vector3& v)
+{
+ bool succes = updateClosestVectorAndPoints();
+ v = m_cachedV;
+ return succes;
+}
+
+
+
+b3Scalar b3VoronoiSimplexSolver::maxVertex()
+{
+ int i, numverts = numVertices();
+ b3Scalar maxV = b3Scalar(0.);
+ for (i=0;i<numverts;i++)
+ {
+ b3Scalar curLen2 = m_simplexVectorW[i].length2();
+ if (maxV < curLen2)
+ maxV = curLen2;
+ }
+ return maxV;
+}
+
+
+
+ //return the current simplex
+int b3VoronoiSimplexSolver::getSimplex(b3Vector3 *pBuf, b3Vector3 *qBuf, b3Vector3 *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 b3VoronoiSimplexSolver::inSimplex(const b3Vector3& w)
+{
+ bool found = false;
+ int i, numverts = numVertices();
+ //b3Scalar maxV = b3Scalar(0.);
+
+ //w is in the current (reduced) simplex
+ for (i=0;i<numverts;i++)
+ {
+#ifdef BT_USE_EQUAL_VERTEX_THRESHOLD
+ if ( m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold)
+#else
+ if (m_simplexVectorW[i] == w)
+#endif
+ found = true;
+ }
+
+ //check in case lastW is already removed
+ if (w == m_lastW)
+ return true;
+
+ return found;
+}
+
+void b3VoronoiSimplexSolver::backup_closest(b3Vector3& v)
+{
+ v = m_cachedV;
+}
+
+
+bool b3VoronoiSimplexSolver::emptySimplex() const
+{
+ return (numVertices() == 0);
+
+}
+
+void b3VoronoiSimplexSolver::compute_points(b3Vector3& p1, b3Vector3& p2)
+{
+ updateClosestVectorAndPoints();
+ p1 = m_cachedP1;
+ p2 = m_cachedP2;
+
+}
+
+
+
+
+bool b3VoronoiSimplexSolver::closestPtPointTriangle(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c,b3SubSimplexClosestResult& result)
+{
+ result.m_usedVertices.reset();
+
+ // Check if P in vertex region outside A
+ b3Vector3 ab = b - a;
+ b3Vector3 ac = c - a;
+ b3Vector3 ap = p - a;
+ b3Scalar d1 = ab.dot(ap);
+ b3Scalar d2 = ac.dot(ap);
+ if (d1 <= b3Scalar(0.0) && d2 <= b3Scalar(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
+ b3Vector3 bp = p - b;
+ b3Scalar d3 = ab.dot(bp);
+ b3Scalar d4 = ac.dot(bp);
+ if (d3 >= b3Scalar(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
+ b3Scalar vc = d1*d4 - d3*d2;
+ if (vc <= b3Scalar(0.0) && d1 >= b3Scalar(0.0) && d3 <= b3Scalar(0.0)) {
+ b3Scalar 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
+ b3Vector3 cp = p - c;
+ b3Scalar d5 = ab.dot(cp);
+ b3Scalar d6 = ac.dot(cp);
+ if (d6 >= b3Scalar(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
+ b3Scalar vb = d5*d2 - d1*d6;
+ if (vb <= b3Scalar(0.0) && d2 >= b3Scalar(0.0) && d6 <= b3Scalar(0.0)) {
+ b3Scalar 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
+ b3Scalar va = d3*d6 - d5*d4;
+ if (va <= b3Scalar(0.0) && (d4 - d3) >= b3Scalar(0.0) && (d5 - d6) >= b3Scalar(0.0)) {
+ b3Scalar 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)
+ b3Scalar denom = b3Scalar(1.0) / (va + vb + vc);
+ b3Scalar v = vb * denom;
+ b3Scalar 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 = b3Scalar(1.0) - v - w
+
+}
+
+
+
+
+
+/// Test if point p and d lie on opposite sides of plane through abc
+int b3VoronoiSimplexSolver::pointOutsideOfPlane(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, const b3Vector3& d)
+{
+ b3Vector3 normal = (b-a).cross(c-a);
+
+ b3Scalar signp = (p - a).dot(normal); // [AP AB AC]
+ b3Scalar signd = (d - a).dot( normal); // [AD AB AC]
+
+#ifdef B3_CATCH_DEGENERATE_TETRAHEDRON
+#ifdef BT_USE_DOUBLE_PRECISION
+if (signd * signd < (b3Scalar(1e-8) * b3Scalar(1e-8)))
+ {
+ return -1;
+ }
+#else
+ if (signd * signd < (b3Scalar(1e-4) * b3Scalar(1e-4)))
+ {
+// printf("affine dependent/degenerate\n");//
+ return -1;
+ }
+#endif
+
+#endif
+ // Points on opposite sides if expression signs are opposite
+ return signp * signd < b3Scalar(0.);
+}
+
+
+bool b3VoronoiSimplexSolver::closestPtPointTetrahedron(const b3Vector3& p, const b3Vector3& a, const b3Vector3& b, const b3Vector3& c, const b3Vector3& d, b3SubSimplexClosestResult& finalResult)
+{
+ b3SubSimplexClosestResult 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;
+ }
+
+
+ b3Scalar bestSqDist = FLT_MAX;
+ // If point outside face abc then compute closest point on abc
+ if (pointOutsideABC)
+ {
+ closestPtPointTriangle(p, a, b, c,tempResult);
+ b3Vector3 q = tempResult.m_closestPointOnSimplex;
+
+ b3Scalar 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);
+ b3Vector3 q = tempResult.m_closestPointOnSimplex;
+ //convert result bitmask!
+
+ b3Scalar 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);
+ b3Vector3 q = tempResult.m_closestPointOnSimplex;
+ //convert result bitmask!
+
+ b3Scalar 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);
+ b3Vector3 q = tempResult.m_closestPointOnSimplex;
+ //convert result bitmask!
+ b3Scalar 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;
+}
+
generated by cgit v1.2.3 (git 2.25.1) at 2024-12-22 12:20:14 +0000