3 Bullet Continuous Collision Detection and Physics Library
4 Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
6 This software is provided 'as-is', without any express or implied warranty.
7 In no event will the authors be held liable for any damages arising from the use of this software.
8 Permission is granted to anyone to use this software for any purpose,
9 including commercial applications, and to alter it and redistribute it freely,
10 subject to the following restrictions:
12 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.
13 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
14 3. This notice may not be removed or altered from any source distribution.
16 Elsevier CDROM license agreements grants nonexclusive license to use the software
17 for any purpose, commercial or non-commercial as long as the following credit is included
18 identifying the original source of the software:
20 Parts of the source are "from the book Real-Time Collision Detection by
21 Christer Ericson, published by Morgan Kaufmann Publishers,
22 (c) 2005 Elsevier Inc."
26 #include "btVoronoiSimplexSolver.h"
33 #define CATCH_DEGENERATE_TETRAHEDRON 1
34 void btVoronoiSimplexSolver::removeVertex(int index)
36 btAssert(m_numVertices > 0);
38 m_simplexVectorW[index] = m_simplexVectorW[m_numVertices];
39 m_simplexPointsP[index] = m_simplexPointsP[m_numVertices];
40 m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices];
43 void btVoronoiSimplexSolver::reduceVertices(const btUsageBitfield& usedVerts)
45 if ((numVertices() >= 4) && (!usedVerts.usedVertexD))
48 if ((numVertices() >= 3) && (!usedVerts.usedVertexC))
51 if ((numVertices() >= 2) && (!usedVerts.usedVertexB))
54 if ((numVertices() >= 1) && (!usedVerts.usedVertexA))
58 //clear the simplex, remove all the vertices
59 void btVoronoiSimplexSolver::reset()
61 m_cachedValidClosest = false;
64 m_lastW = btVector3(btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT), btScalar(BT_LARGE_FLOAT));
69 void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, const btVector3& q)
74 m_simplexVectorW[m_numVertices] = w;
75 m_simplexPointsP[m_numVertices] = p;
76 m_simplexPointsQ[m_numVertices] = q;
81 bool btVoronoiSimplexSolver::updateClosestVectorAndPoints()
87 m_needsUpdate = false;
89 switch (numVertices())
92 m_cachedValidClosest = false;
96 m_cachedP1 = m_simplexPointsP[0];
97 m_cachedP2 = m_simplexPointsQ[0];
98 m_cachedV = m_cachedP1 - m_cachedP2; //== m_simplexVectorW[0]
100 m_cachedBC.setBarycentricCoordinates(btScalar(1.), btScalar(0.), btScalar(0.), btScalar(0.));
101 m_cachedValidClosest = m_cachedBC.isValid();
106 //closest point origin from line segment
107 const btVector3& from = m_simplexVectorW[0];
108 const btVector3& to = m_simplexVectorW[1];
111 btVector3 p(btScalar(0.), btScalar(0.), btScalar(0.));
112 btVector3 diff = p - from;
113 btVector3 v = to - from;
114 btScalar t = v.dot(diff);
118 btScalar dotVV = v.dot(v);
123 m_cachedBC.m_usedVertices.usedVertexA = true;
124 m_cachedBC.m_usedVertices.usedVertexB = true;
131 m_cachedBC.m_usedVertices.usedVertexB = true;
138 m_cachedBC.m_usedVertices.usedVertexA = true;
140 m_cachedBC.setBarycentricCoordinates(1 - t, t);
141 nearest = from + t * v;
143 m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
144 m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
145 m_cachedV = m_cachedP1 - m_cachedP2;
147 reduceVertices(m_cachedBC.m_usedVertices);
149 m_cachedValidClosest = m_cachedBC.isValid();
154 //closest point origin from triangle
155 btVector3 p(btScalar(0.), btScalar(0.), btScalar(0.));
157 const btVector3& a = m_simplexVectorW[0];
158 const btVector3& b = m_simplexVectorW[1];
159 const btVector3& c = m_simplexVectorW[2];
161 closestPtPointTriangle(p, a, b, c, m_cachedBC);
162 m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
163 m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
164 m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2];
166 m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
167 m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
168 m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2];
170 m_cachedV = m_cachedP1 - m_cachedP2;
172 reduceVertices(m_cachedBC.m_usedVertices);
173 m_cachedValidClosest = m_cachedBC.isValid();
179 btVector3 p(btScalar(0.), btScalar(0.), btScalar(0.));
181 const btVector3& a = m_simplexVectorW[0];
182 const btVector3& b = m_simplexVectorW[1];
183 const btVector3& c = m_simplexVectorW[2];
184 const btVector3& d = m_simplexVectorW[3];
186 bool hasSeparation = closestPtPointTetrahedron(p, a, b, c, d, m_cachedBC);
190 m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
191 m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
192 m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
193 m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
195 m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
196 m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
197 m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
198 m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
200 m_cachedV = m_cachedP1 - m_cachedP2;
201 reduceVertices(m_cachedBC.m_usedVertices);
205 // printf("sub distance got penetration\n");
207 if (m_cachedBC.m_degenerate)
209 m_cachedValidClosest = false;
213 m_cachedValidClosest = true;
214 //degenerate case == false, penetration = true + zero
215 m_cachedV.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
220 m_cachedValidClosest = m_cachedBC.isValid();
222 //closest point origin from tetrahedron
227 m_cachedValidClosest = false;
232 return m_cachedValidClosest;
235 //return/calculate the closest vertex
236 bool btVoronoiSimplexSolver::closest(btVector3& v)
238 bool succes = updateClosestVectorAndPoints();
243 btScalar btVoronoiSimplexSolver::maxVertex()
245 int i, numverts = numVertices();
246 btScalar maxV = btScalar(0.);
247 for (i = 0; i < numverts; i++)
249 btScalar curLen2 = m_simplexVectorW[i].length2();
256 //return the current simplex
257 int btVoronoiSimplexSolver::getSimplex(btVector3* pBuf, btVector3* qBuf, btVector3* yBuf) const
260 for (i = 0; i < numVertices(); i++)
262 yBuf[i] = m_simplexVectorW[i];
263 pBuf[i] = m_simplexPointsP[i];
264 qBuf[i] = m_simplexPointsQ[i];
266 return numVertices();
269 bool btVoronoiSimplexSolver::inSimplex(const btVector3& w)
272 int i, numverts = numVertices();
273 //btScalar maxV = btScalar(0.);
275 //w is in the current (reduced) simplex
276 for (i = 0; i < numverts; i++)
278 #ifdef BT_USE_EQUAL_VERTEX_THRESHOLD
279 if (m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold)
281 if (m_simplexVectorW[i] == w)
289 //check in case lastW is already removed
296 void btVoronoiSimplexSolver::backup_closest(btVector3& v)
301 bool btVoronoiSimplexSolver::emptySimplex() const
303 return (numVertices() == 0);
306 void btVoronoiSimplexSolver::compute_points(btVector3& p1, btVector3& p2)
308 updateClosestVectorAndPoints();
313 bool btVoronoiSimplexSolver::closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, btSubSimplexClosestResult& result)
315 result.m_usedVertices.reset();
317 // Check if P in vertex region outside A
318 btVector3 ab = b - a;
319 btVector3 ac = c - a;
320 btVector3 ap = p - a;
321 btScalar d1 = ab.dot(ap);
322 btScalar d2 = ac.dot(ap);
323 if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0))
325 result.m_closestPointOnSimplex = a;
326 result.m_usedVertices.usedVertexA = true;
327 result.setBarycentricCoordinates(1, 0, 0);
328 return true; // a; // barycentric coordinates (1,0,0)
331 // Check if P in vertex region outside B
332 btVector3 bp = p - b;
333 btScalar d3 = ab.dot(bp);
334 btScalar d4 = ac.dot(bp);
335 if (d3 >= btScalar(0.0) && d4 <= d3)
337 result.m_closestPointOnSimplex = b;
338 result.m_usedVertices.usedVertexB = true;
339 result.setBarycentricCoordinates(0, 1, 0);
341 return true; // b; // barycentric coordinates (0,1,0)
343 // Check if P in edge region of AB, if so return projection of P onto AB
344 btScalar vc = d1 * d4 - d3 * d2;
345 if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0))
347 btScalar v = d1 / (d1 - d3);
348 result.m_closestPointOnSimplex = a + v * ab;
349 result.m_usedVertices.usedVertexA = true;
350 result.m_usedVertices.usedVertexB = true;
351 result.setBarycentricCoordinates(1 - v, v, 0);
353 //return a + v * ab; // barycentric coordinates (1-v,v,0)
356 // Check if P in vertex region outside C
357 btVector3 cp = p - c;
358 btScalar d5 = ab.dot(cp);
359 btScalar d6 = ac.dot(cp);
360 if (d6 >= btScalar(0.0) && d5 <= d6)
362 result.m_closestPointOnSimplex = c;
363 result.m_usedVertices.usedVertexC = true;
364 result.setBarycentricCoordinates(0, 0, 1);
365 return true; //c; // barycentric coordinates (0,0,1)
368 // Check if P in edge region of AC, if so return projection of P onto AC
369 btScalar vb = d5 * d2 - d1 * d6;
370 if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0))
372 btScalar w = d2 / (d2 - d6);
373 result.m_closestPointOnSimplex = a + w * ac;
374 result.m_usedVertices.usedVertexA = true;
375 result.m_usedVertices.usedVertexC = true;
376 result.setBarycentricCoordinates(1 - w, 0, w);
378 //return a + w * ac; // barycentric coordinates (1-w,0,w)
381 // Check if P in edge region of BC, if so return projection of P onto BC
382 btScalar va = d3 * d6 - d5 * d4;
383 if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0))
385 btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
387 result.m_closestPointOnSimplex = b + w * (c - b);
388 result.m_usedVertices.usedVertexB = true;
389 result.m_usedVertices.usedVertexC = true;
390 result.setBarycentricCoordinates(0, 1 - w, w);
392 // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
395 // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
396 btScalar denom = btScalar(1.0) / (va + vb + vc);
397 btScalar v = vb * denom;
398 btScalar w = vc * denom;
400 result.m_closestPointOnSimplex = a + ab * v + ac * w;
401 result.m_usedVertices.usedVertexA = true;
402 result.m_usedVertices.usedVertexB = true;
403 result.m_usedVertices.usedVertexC = true;
404 result.setBarycentricCoordinates(1 - v - w, v, w);
407 // return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w
410 /// Test if point p and d lie on opposite sides of plane through abc
411 int btVoronoiSimplexSolver::pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d)
413 btVector3 normal = (b - a).cross(c - a);
415 btScalar signp = (p - a).dot(normal); // [AP AB AC]
416 btScalar signd = (d - a).dot(normal); // [AD AB AC]
418 #ifdef CATCH_DEGENERATE_TETRAHEDRON
419 #ifdef BT_USE_DOUBLE_PRECISION
420 if (signd * signd < (btScalar(1e-8) * btScalar(1e-8)))
425 if (signd * signd < (btScalar(1e-4) * btScalar(1e-4)))
427 // printf("affine dependent/degenerate\n");//
433 // Points on opposite sides if expression signs are opposite
434 return signp * signd < btScalar(0.);
437 bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult)
439 btSubSimplexClosestResult tempResult;
441 // Start out assuming point inside all halfspaces, so closest to itself
442 finalResult.m_closestPointOnSimplex = p;
443 finalResult.m_usedVertices.reset();
444 finalResult.m_usedVertices.usedVertexA = true;
445 finalResult.m_usedVertices.usedVertexB = true;
446 finalResult.m_usedVertices.usedVertexC = true;
447 finalResult.m_usedVertices.usedVertexD = true;
449 int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
450 int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b);
451 int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
452 int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);
454 if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
456 finalResult.m_degenerate = true;
460 if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
465 btScalar bestSqDist = FLT_MAX;
466 // If point outside face abc then compute closest point on abc
469 closestPtPointTriangle(p, a, b, c, tempResult);
470 btVector3 q = tempResult.m_closestPointOnSimplex;
472 btScalar sqDist = (q - p).dot(q - p);
473 // Update best closest point if (squared) distance is less than current best
474 if (sqDist < bestSqDist)
477 finalResult.m_closestPointOnSimplex = q;
478 //convert result bitmask!
479 finalResult.m_usedVertices.reset();
480 finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
481 finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB;
482 finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
483 finalResult.setBarycentricCoordinates(
484 tempResult.m_barycentricCoords[VERTA],
485 tempResult.m_barycentricCoords[VERTB],
486 tempResult.m_barycentricCoords[VERTC],
491 // Repeat test for face acd
494 closestPtPointTriangle(p, a, c, d, tempResult);
495 btVector3 q = tempResult.m_closestPointOnSimplex;
496 //convert result bitmask!
498 btScalar sqDist = (q - p).dot(q - p);
499 if (sqDist < bestSqDist)
502 finalResult.m_closestPointOnSimplex = q;
503 finalResult.m_usedVertices.reset();
504 finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
506 finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB;
507 finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC;
508 finalResult.setBarycentricCoordinates(
509 tempResult.m_barycentricCoords[VERTA],
511 tempResult.m_barycentricCoords[VERTB],
512 tempResult.m_barycentricCoords[VERTC]);
515 // Repeat test for face adb
519 closestPtPointTriangle(p, a, d, b, tempResult);
520 btVector3 q = tempResult.m_closestPointOnSimplex;
521 //convert result bitmask!
523 btScalar sqDist = (q - p).dot(q - p);
524 if (sqDist < bestSqDist)
527 finalResult.m_closestPointOnSimplex = q;
528 finalResult.m_usedVertices.reset();
529 finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
530 finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC;
532 finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
533 finalResult.setBarycentricCoordinates(
534 tempResult.m_barycentricCoords[VERTA],
535 tempResult.m_barycentricCoords[VERTC],
537 tempResult.m_barycentricCoords[VERTB]);
540 // Repeat test for face bdc
544 closestPtPointTriangle(p, b, d, c, tempResult);
545 btVector3 q = tempResult.m_closestPointOnSimplex;
546 //convert result bitmask!
547 btScalar sqDist = (q - p).dot(q - p);
548 if (sqDist < bestSqDist)
551 finalResult.m_closestPointOnSimplex = q;
552 finalResult.m_usedVertices.reset();
554 finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA;
555 finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
556 finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
558 finalResult.setBarycentricCoordinates(
560 tempResult.m_barycentricCoords[VERTA],
561 tempResult.m_barycentricCoords[VERTC],
562 tempResult.m_barycentricCoords[VERTB]);
566 //help! we ended up full !
568 if (finalResult.m_usedVertices.usedVertexA &&
569 finalResult.m_usedVertices.usedVertexB &&
570 finalResult.m_usedVertices.usedVertexC &&
571 finalResult.m_usedVertices.usedVertexD)