Bullet Collision Detection & Physics Library
btVoronoiSimplexSolver.cpp
Go to the documentation of this file.
1
2/*
3Bullet Continuous Collision Detection and Physics Library
4Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
5
6This software is provided 'as-is', without any express or implied warranty.
7In no event will the authors be held liable for any damages arising from the use of this software.
8Permission is granted to anyone to use this software for any purpose,
9including commercial applications, and to alter it and redistribute it freely,
10subject to the following restrictions:
11
121. 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.
132. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
143. This notice may not be removed or altered from any source distribution.
15
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:
19
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."
23
24*/
25
26
28
29#define VERTA 0
30#define VERTB 1
31#define VERTC 2
32#define VERTD 3
33
34#define CATCH_DEGENERATE_TETRAHEDRON 1
36{
37
43}
44
46{
47 if ((numVertices() >= 4) && (!usedVerts.usedVertexD))
48 removeVertex(3);
49
50 if ((numVertices() >= 3) && (!usedVerts.usedVertexC))
51 removeVertex(2);
52
53 if ((numVertices() >= 2) && (!usedVerts.usedVertexB))
54 removeVertex(1);
55
56 if ((numVertices() >= 1) && (!usedVerts.usedVertexA))
57 removeVertex(0);
58
59}
60
61
62
63
64
65//clear the simplex, remove all the vertices
67{
69 m_numVertices = 0;
70 m_needsUpdate = true;
73}
74
75
76
77 //add a vertex
79{
80 m_lastW = w;
81 m_needsUpdate = true;
82
86
88}
89
91{
92
93 if (m_needsUpdate)
94 {
96
97 m_needsUpdate = false;
98
99 switch (numVertices())
100 {
101 case 0:
102 m_cachedValidClosest = false;
103 break;
104 case 1:
105 {
108 m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0]
112 break;
113 };
114 case 2:
115 {
116 //closest point origin from line segment
117 const btVector3& from = m_simplexVectorW[0];
118 const btVector3& to = m_simplexVectorW[1];
119 btVector3 nearest;
120
121 btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
122 btVector3 diff = p - from;
123 btVector3 v = to - from;
124 btScalar t = v.dot(diff);
125
126 if (t > 0) {
127 btScalar dotVV = v.dot(v);
128 if (t < dotVV) {
129 t /= dotVV;
130 diff -= t*v;
133 } else {
134 t = 1;
135 diff -= v;
136 //reduce to 1 point
138 }
139 } else
140 {
141 t = 0;
142 //reduce to 1 point
144 }
146 nearest = from + t*v;
147
151
153
155 break;
156 }
157 case 3:
158 {
159 //closest point origin from triangle
160 btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
161
162 const btVector3& a = m_simplexVectorW[0];
163 const btVector3& b = m_simplexVectorW[1];
164 const btVector3& c = m_simplexVectorW[2];
165
170
174
176
179
180 break;
181 }
182 case 4:
183 {
184
185
186 btVector3 p (btScalar(0.),btScalar(0.),btScalar(0.));
187
188 const btVector3& a = m_simplexVectorW[0];
189 const btVector3& b = m_simplexVectorW[1];
190 const btVector3& c = m_simplexVectorW[2];
191 const btVector3& d = m_simplexVectorW[3];
192
193 bool hasSeperation = closestPtPointTetrahedron(p,a,b,c,d,m_cachedBC);
194
195 if (hasSeperation)
196 {
197
202
207
210 } else
211 {
212// printf("sub distance got penetration\n");
213
215 {
216 m_cachedValidClosest = false;
217 } else
218 {
220 //degenerate case == false, penetration = true + zero
222 }
223 break;
224 }
225
227
228 //closest point origin from tetrahedron
229 break;
230 }
231 default:
232 {
233 m_cachedValidClosest = false;
234 }
235 };
236 }
237
239
240}
241
242//return/calculate the closest vertex
244{
245 bool succes = updateClosestVectorAndPoints();
246 v = m_cachedV;
247 return succes;
248}
249
250
251
253{
254 int i, numverts = numVertices();
255 btScalar maxV = btScalar(0.);
256 for (i=0;i<numverts;i++)
257 {
258 btScalar curLen2 = m_simplexVectorW[i].length2();
259 if (maxV < curLen2)
260 maxV = curLen2;
261 }
262 return maxV;
263}
264
265
266
267 //return the current simplex
269{
270 int i;
271 for (i=0;i<numVertices();i++)
272 {
273 yBuf[i] = m_simplexVectorW[i];
274 pBuf[i] = m_simplexPointsP[i];
275 qBuf[i] = m_simplexPointsQ[i];
276 }
277 return numVertices();
278}
279
280
281
282
284{
285 bool found = false;
286 int i, numverts = numVertices();
287 //btScalar maxV = btScalar(0.);
288
289 //w is in the current (reduced) simplex
290 for (i=0;i<numverts;i++)
291 {
292#ifdef BT_USE_EQUAL_VERTEX_THRESHOLD
293 if ( m_simplexVectorW[i].distance2(w) <= m_equalVertexThreshold)
294#else
295 if (m_simplexVectorW[i] == w)
296#endif
297 {
298 found = true;
299 break;
300 }
301 }
302
303 //check in case lastW is already removed
304 if (w == m_lastW)
305 return true;
306
307 return found;
308}
309
311{
312 v = m_cachedV;
313}
314
315
317{
318 return (numVertices() == 0);
319
320}
321
323{
325 p1 = m_cachedP1;
326 p2 = m_cachedP2;
327
328}
329
330
331
332
334{
335 result.m_usedVertices.reset();
336
337 // Check if P in vertex region outside A
338 btVector3 ab = b - a;
339 btVector3 ac = c - a;
340 btVector3 ap = p - a;
341 btScalar d1 = ab.dot(ap);
342 btScalar d2 = ac.dot(ap);
343 if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0))
344 {
345 result.m_closestPointOnSimplex = a;
346 result.m_usedVertices.usedVertexA = true;
347 result.setBarycentricCoordinates(1,0,0);
348 return true;// a; // barycentric coordinates (1,0,0)
349 }
350
351 // Check if P in vertex region outside B
352 btVector3 bp = p - b;
353 btScalar d3 = ab.dot(bp);
354 btScalar d4 = ac.dot(bp);
355 if (d3 >= btScalar(0.0) && d4 <= d3)
356 {
357 result.m_closestPointOnSimplex = b;
358 result.m_usedVertices.usedVertexB = true;
359 result.setBarycentricCoordinates(0,1,0);
360
361 return true; // b; // barycentric coordinates (0,1,0)
362 }
363 // Check if P in edge region of AB, if so return projection of P onto AB
364 btScalar vc = d1*d4 - d3*d2;
365 if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) {
366 btScalar v = d1 / (d1 - d3);
367 result.m_closestPointOnSimplex = a + v * ab;
368 result.m_usedVertices.usedVertexA = true;
369 result.m_usedVertices.usedVertexB = true;
370 result.setBarycentricCoordinates(1-v,v,0);
371 return true;
372 //return a + v * ab; // barycentric coordinates (1-v,v,0)
373 }
374
375 // Check if P in vertex region outside C
376 btVector3 cp = p - c;
377 btScalar d5 = ab.dot(cp);
378 btScalar d6 = ac.dot(cp);
379 if (d6 >= btScalar(0.0) && d5 <= d6)
380 {
381 result.m_closestPointOnSimplex = c;
382 result.m_usedVertices.usedVertexC = true;
383 result.setBarycentricCoordinates(0,0,1);
384 return true;//c; // barycentric coordinates (0,0,1)
385 }
386
387 // Check if P in edge region of AC, if so return projection of P onto AC
388 btScalar vb = d5*d2 - d1*d6;
389 if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) {
390 btScalar w = d2 / (d2 - d6);
391 result.m_closestPointOnSimplex = a + w * ac;
392 result.m_usedVertices.usedVertexA = true;
393 result.m_usedVertices.usedVertexC = true;
394 result.setBarycentricCoordinates(1-w,0,w);
395 return true;
396 //return a + w * ac; // barycentric coordinates (1-w,0,w)
397 }
398
399 // Check if P in edge region of BC, if so return projection of P onto BC
400 btScalar va = d3*d6 - d5*d4;
401 if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) {
402 btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
403
404 result.m_closestPointOnSimplex = b + w * (c - b);
405 result.m_usedVertices.usedVertexB = true;
406 result.m_usedVertices.usedVertexC = true;
407 result.setBarycentricCoordinates(0,1-w,w);
408 return true;
409 // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
410 }
411
412 // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
413 btScalar denom = btScalar(1.0) / (va + vb + vc);
414 btScalar v = vb * denom;
415 btScalar w = vc * denom;
416
417 result.m_closestPointOnSimplex = a + ab * v + ac * w;
418 result.m_usedVertices.usedVertexA = true;
419 result.m_usedVertices.usedVertexB = true;
420 result.m_usedVertices.usedVertexC = true;
421 result.setBarycentricCoordinates(1-v-w,v,w);
422
423 return true;
424// return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w
425
426}
427
428
429
430
431
434{
435 btVector3 normal = (b-a).cross(c-a);
436
437 btScalar signp = (p - a).dot(normal); // [AP AB AC]
438 btScalar signd = (d - a).dot( normal); // [AD AB AC]
439
440#ifdef CATCH_DEGENERATE_TETRAHEDRON
441#ifdef BT_USE_DOUBLE_PRECISION
442if (signd * signd < (btScalar(1e-8) * btScalar(1e-8)))
443 {
444 return -1;
445 }
446#else
447 if (signd * signd < (btScalar(1e-4) * btScalar(1e-4)))
448 {
449// printf("affine dependent/degenerate\n");//
450 return -1;
451 }
452#endif
453
454#endif
455 // Points on opposite sides if expression signs are opposite
456 return signp * signd < btScalar(0.);
457}
458
459
461{
462 btSubSimplexClosestResult tempResult;
463
464 // Start out assuming point inside all halfspaces, so closest to itself
465 finalResult.m_closestPointOnSimplex = p;
466 finalResult.m_usedVertices.reset();
467 finalResult.m_usedVertices.usedVertexA = true;
468 finalResult.m_usedVertices.usedVertexB = true;
469 finalResult.m_usedVertices.usedVertexC = true;
470 finalResult.m_usedVertices.usedVertexD = true;
471
472 int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
473 int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b);
474 int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
475 int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);
476
477 if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
478 {
479 finalResult.m_degenerate = true;
480 return false;
481 }
482
483 if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
484 {
485 return false;
486 }
487
488
489 btScalar bestSqDist = FLT_MAX;
490 // If point outside face abc then compute closest point on abc
491 if (pointOutsideABC)
492 {
493 closestPtPointTriangle(p, a, b, c,tempResult);
494 btVector3 q = tempResult.m_closestPointOnSimplex;
495
496 btScalar sqDist = (q - p).dot( q - p);
497 // Update best closest point if (squared) distance is less than current best
498 if (sqDist < bestSqDist) {
499 bestSqDist = sqDist;
500 finalResult.m_closestPointOnSimplex = q;
501 //convert result bitmask!
502 finalResult.m_usedVertices.reset();
503 finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
504 finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB;
505 finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
506 finalResult.setBarycentricCoordinates(
507 tempResult.m_barycentricCoords[VERTA],
508 tempResult.m_barycentricCoords[VERTB],
509 tempResult.m_barycentricCoords[VERTC],
510 0
511 );
512
513 }
514 }
515
516
517 // Repeat test for face acd
518 if (pointOutsideACD)
519 {
520 closestPtPointTriangle(p, a, c, d,tempResult);
521 btVector3 q = tempResult.m_closestPointOnSimplex;
522 //convert result bitmask!
523
524 btScalar sqDist = (q - p).dot( q - p);
525 if (sqDist < bestSqDist)
526 {
527 bestSqDist = sqDist;
528 finalResult.m_closestPointOnSimplex = q;
529 finalResult.m_usedVertices.reset();
530 finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
531
532 finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB;
533 finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC;
534 finalResult.setBarycentricCoordinates(
535 tempResult.m_barycentricCoords[VERTA],
536 0,
537 tempResult.m_barycentricCoords[VERTB],
538 tempResult.m_barycentricCoords[VERTC]
539 );
540
541 }
542 }
543 // Repeat test for face adb
544
545
546 if (pointOutsideADB)
547 {
548 closestPtPointTriangle(p, a, d, b,tempResult);
549 btVector3 q = tempResult.m_closestPointOnSimplex;
550 //convert result bitmask!
551
552 btScalar sqDist = (q - p).dot( q - p);
553 if (sqDist < bestSqDist)
554 {
555 bestSqDist = sqDist;
556 finalResult.m_closestPointOnSimplex = q;
557 finalResult.m_usedVertices.reset();
558 finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
559 finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC;
560
561 finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
562 finalResult.setBarycentricCoordinates(
563 tempResult.m_barycentricCoords[VERTA],
564 tempResult.m_barycentricCoords[VERTC],
565 0,
566 tempResult.m_barycentricCoords[VERTB]
567 );
568
569 }
570 }
571 // Repeat test for face bdc
572
573
574 if (pointOutsideBDC)
575 {
576 closestPtPointTriangle(p, b, d, c,tempResult);
577 btVector3 q = tempResult.m_closestPointOnSimplex;
578 //convert result bitmask!
579 btScalar sqDist = (q - p).dot( q - p);
580 if (sqDist < bestSqDist)
581 {
582 bestSqDist = sqDist;
583 finalResult.m_closestPointOnSimplex = q;
584 finalResult.m_usedVertices.reset();
585 //
586 finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA;
587 finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
588 finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
589
590 finalResult.setBarycentricCoordinates(
591 0,
592 tempResult.m_barycentricCoords[VERTA],
593 tempResult.m_barycentricCoords[VERTC],
594 tempResult.m_barycentricCoords[VERTB]
595 );
596
597 }
598 }
599
600 //help! we ended up full !
601
602 if (finalResult.m_usedVertices.usedVertexA &&
603 finalResult.m_usedVertices.usedVertexB &&
604 finalResult.m_usedVertices.usedVertexC &&
605 finalResult.m_usedVertices.usedVertexD)
606 {
607 return true;
608 }
609
610 return true;
611}
612
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:878
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
#define BT_LARGE_FLOAT
Definition: btScalar.h:294
#define btAssert(x)
Definition: btScalar.h:131
#define VERTA
#define VERTB
#define VERTC
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
void reduceVertices(const btUsageBitfield &usedVerts)
btVector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS]
btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS]
btSubSimplexClosestResult m_cachedBC
int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const
void addVertex(const btVector3 &w, const btVector3 &p, const btVector3 &q)
bool closestPtPointTetrahedron(const btVector3 &p, const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &d, btSubSimplexClosestResult &finalResult)
bool inSimplex(const btVector3 &w)
int pointOutsideOfPlane(const btVector3 &p, const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &d)
Test if point p and d lie on opposite sides of plane through abc.
void compute_points(btVector3 &p1, btVector3 &p2)
bool closestPtPointTriangle(const btVector3 &p, const btVector3 &a, const btVector3 &b, const btVector3 &c, btSubSimplexClosestResult &result)
btVector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS]
void setBarycentricCoordinates(btScalar a=btScalar(0.), btScalar b=btScalar(0.), btScalar c=btScalar(0.), btScalar d=btScalar(0.))
unsigned short usedVertexC
unsigned short usedVertexB
unsigned short usedVertexA
unsigned short usedVertexD