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.cpp574
1 files changed, 0 insertions, 574 deletions
diff --git a/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp b/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp
deleted file mode 100644
index 8b0a834efe..0000000000
--- a/thirdparty/bullet/Bullet3OpenCL/NarrowphaseCollision/b3VoronoiSimplexSolver.cpp
+++ /dev/null
@@ -1,574 +0,0 @@
-
-/*
-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 hasSeparation = closestPtPointTetrahedron(p, a, b, c, d, m_cachedBC);
-
- if (hasSeparation)
- {
- 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-11-01 05:28:16 +0000