1
2 /*
3 Bullet Continuous Collision Detection and Physics Library
4 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
5
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:
11
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.
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
27 #include "btVoronoiSimplexSolver.h"
28
29 #define VERTA 0
30 #define VERTB 1
31 #define VERTC 2
32 #define VERTD 3
33
34 #define CATCH_DEGENERATE_TETRAHEDRON 1
removeVertex(int index)35 void btVoronoiSimplexSolver::removeVertex(int index)
36 {
37
38 btAssert(m_numVertices>0);
39 m_numVertices--;
40 m_simplexVectorW[index] = m_simplexVectorW[m_numVertices];
41 m_simplexPointsP[index] = m_simplexPointsP[m_numVertices];
42 m_simplexPointsQ[index] = m_simplexPointsQ[m_numVertices];
43 }
44
reduceVertices(const btUsageBitfield & usedVerts)45 void btVoronoiSimplexSolver::reduceVertices (const btUsageBitfield& usedVerts)
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
reset()66 void btVoronoiSimplexSolver::reset()
67 {
68 m_cachedValidClosest = false;
69 m_numVertices = 0;
70 m_needsUpdate = true;
71 m_lastW = btVector3(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
72 m_cachedBC.reset();
73 }
74
75
76
77 //add a vertex
addVertex(const btVector3 & w,const btVector3 & p,const btVector3 & q)78 void btVoronoiSimplexSolver::addVertex(const btVector3& w, const btVector3& p, const btVector3& q)
79 {
80 m_lastW = w;
81 m_needsUpdate = true;
82
83 m_simplexVectorW[m_numVertices] = w;
84 m_simplexPointsP[m_numVertices] = p;
85 m_simplexPointsQ[m_numVertices] = q;
86
87 m_numVertices++;
88 }
89
updateClosestVectorAndPoints()90 bool btVoronoiSimplexSolver::updateClosestVectorAndPoints()
91 {
92
93 if (m_needsUpdate)
94 {
95 m_cachedBC.reset();
96
97 m_needsUpdate = false;
98
99 switch (numVertices())
100 {
101 case 0:
102 m_cachedValidClosest = false;
103 break;
104 case 1:
105 {
106 m_cachedP1 = m_simplexPointsP[0];
107 m_cachedP2 = m_simplexPointsQ[0];
108 m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0]
109 m_cachedBC.reset();
110 m_cachedBC.setBarycentricCoordinates(btScalar(1.),btScalar(0.),btScalar(0.),btScalar(0.));
111 m_cachedValidClosest = m_cachedBC.isValid();
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;
131 m_cachedBC.m_usedVertices.usedVertexA = true;
132 m_cachedBC.m_usedVertices.usedVertexB = true;
133 } else {
134 t = 1;
135 diff -= v;
136 //reduce to 1 point
137 m_cachedBC.m_usedVertices.usedVertexB = true;
138 }
139 } else
140 {
141 t = 0;
142 //reduce to 1 point
143 m_cachedBC.m_usedVertices.usedVertexA = true;
144 }
145 m_cachedBC.setBarycentricCoordinates(1-t,t);
146 nearest = from + t*v;
147
148 m_cachedP1 = m_simplexPointsP[0] + t * (m_simplexPointsP[1] - m_simplexPointsP[0]);
149 m_cachedP2 = m_simplexPointsQ[0] + t * (m_simplexPointsQ[1] - m_simplexPointsQ[0]);
150 m_cachedV = m_cachedP1 - m_cachedP2;
151
152 reduceVertices(m_cachedBC.m_usedVertices);
153
154 m_cachedValidClosest = m_cachedBC.isValid();
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
166 closestPtPointTriangle(p,a,b,c,m_cachedBC);
167 m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
168 m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
169 m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2];
170
171 m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
172 m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
173 m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2];
174
175 m_cachedV = m_cachedP1-m_cachedP2;
176
177 reduceVertices (m_cachedBC.m_usedVertices);
178 m_cachedValidClosest = m_cachedBC.isValid();
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
198 m_cachedP1 = m_simplexPointsP[0] * m_cachedBC.m_barycentricCoords[0] +
199 m_simplexPointsP[1] * m_cachedBC.m_barycentricCoords[1] +
200 m_simplexPointsP[2] * m_cachedBC.m_barycentricCoords[2] +
201 m_simplexPointsP[3] * m_cachedBC.m_barycentricCoords[3];
202
203 m_cachedP2 = m_simplexPointsQ[0] * m_cachedBC.m_barycentricCoords[0] +
204 m_simplexPointsQ[1] * m_cachedBC.m_barycentricCoords[1] +
205 m_simplexPointsQ[2] * m_cachedBC.m_barycentricCoords[2] +
206 m_simplexPointsQ[3] * m_cachedBC.m_barycentricCoords[3];
207
208 m_cachedV = m_cachedP1-m_cachedP2;
209 reduceVertices (m_cachedBC.m_usedVertices);
210 } else
211 {
212 // printf("sub distance got penetration\n");
213
214 if (m_cachedBC.m_degenerate)
215 {
216 m_cachedValidClosest = false;
217 } else
218 {
219 m_cachedValidClosest = true;
220 //degenerate case == false, penetration = true + zero
221 m_cachedV.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
222 }
223 break;
224 }
225
226 m_cachedValidClosest = m_cachedBC.isValid();
227
228 //closest point origin from tetrahedron
229 break;
230 }
231 default:
232 {
233 m_cachedValidClosest = false;
234 }
235 };
236 }
237
238 return m_cachedValidClosest;
239
240 }
241
242 //return/calculate the closest vertex
closest(btVector3 & v)243 bool btVoronoiSimplexSolver::closest(btVector3& v)
244 {
245 bool succes = updateClosestVectorAndPoints();
246 v = m_cachedV;
247 return succes;
248 }
249
250
251
maxVertex()252 btScalar btVoronoiSimplexSolver::maxVertex()
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
getSimplex(btVector3 * pBuf,btVector3 * qBuf,btVector3 * yBuf) const268 int btVoronoiSimplexSolver::getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const
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
inSimplex(const btVector3 & w)283 bool btVoronoiSimplexSolver::inSimplex(const btVector3& w)
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 found = true;
298 }
299
300 //check in case lastW is already removed
301 if (w == m_lastW)
302 return true;
303
304 return found;
305 }
306
backup_closest(btVector3 & v)307 void btVoronoiSimplexSolver::backup_closest(btVector3& v)
308 {
309 v = m_cachedV;
310 }
311
312
emptySimplex() const313 bool btVoronoiSimplexSolver::emptySimplex() const
314 {
315 return (numVertices() == 0);
316
317 }
318
compute_points(btVector3 & p1,btVector3 & p2)319 void btVoronoiSimplexSolver::compute_points(btVector3& p1, btVector3& p2)
320 {
321 updateClosestVectorAndPoints();
322 p1 = m_cachedP1;
323 p2 = m_cachedP2;
324
325 }
326
327
328
329
closestPtPointTriangle(const btVector3 & p,const btVector3 & a,const btVector3 & b,const btVector3 & c,btSubSimplexClosestResult & result)330 bool btVoronoiSimplexSolver::closestPtPointTriangle(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c,btSubSimplexClosestResult& result)
331 {
332 result.m_usedVertices.reset();
333
334 // Check if P in vertex region outside A
335 btVector3 ab = b - a;
336 btVector3 ac = c - a;
337 btVector3 ap = p - a;
338 btScalar d1 = ab.dot(ap);
339 btScalar d2 = ac.dot(ap);
340 if (d1 <= btScalar(0.0) && d2 <= btScalar(0.0))
341 {
342 result.m_closestPointOnSimplex = a;
343 result.m_usedVertices.usedVertexA = true;
344 result.setBarycentricCoordinates(1,0,0);
345 return true;// a; // barycentric coordinates (1,0,0)
346 }
347
348 // Check if P in vertex region outside B
349 btVector3 bp = p - b;
350 btScalar d3 = ab.dot(bp);
351 btScalar d4 = ac.dot(bp);
352 if (d3 >= btScalar(0.0) && d4 <= d3)
353 {
354 result.m_closestPointOnSimplex = b;
355 result.m_usedVertices.usedVertexB = true;
356 result.setBarycentricCoordinates(0,1,0);
357
358 return true; // b; // barycentric coordinates (0,1,0)
359 }
360 // Check if P in edge region of AB, if so return projection of P onto AB
361 btScalar vc = d1*d4 - d3*d2;
362 if (vc <= btScalar(0.0) && d1 >= btScalar(0.0) && d3 <= btScalar(0.0)) {
363 btScalar v = d1 / (d1 - d3);
364 result.m_closestPointOnSimplex = a + v * ab;
365 result.m_usedVertices.usedVertexA = true;
366 result.m_usedVertices.usedVertexB = true;
367 result.setBarycentricCoordinates(1-v,v,0);
368 return true;
369 //return a + v * ab; // barycentric coordinates (1-v,v,0)
370 }
371
372 // Check if P in vertex region outside C
373 btVector3 cp = p - c;
374 btScalar d5 = ab.dot(cp);
375 btScalar d6 = ac.dot(cp);
376 if (d6 >= btScalar(0.0) && d5 <= d6)
377 {
378 result.m_closestPointOnSimplex = c;
379 result.m_usedVertices.usedVertexC = true;
380 result.setBarycentricCoordinates(0,0,1);
381 return true;//c; // barycentric coordinates (0,0,1)
382 }
383
384 // Check if P in edge region of AC, if so return projection of P onto AC
385 btScalar vb = d5*d2 - d1*d6;
386 if (vb <= btScalar(0.0) && d2 >= btScalar(0.0) && d6 <= btScalar(0.0)) {
387 btScalar w = d2 / (d2 - d6);
388 result.m_closestPointOnSimplex = a + w * ac;
389 result.m_usedVertices.usedVertexA = true;
390 result.m_usedVertices.usedVertexC = true;
391 result.setBarycentricCoordinates(1-w,0,w);
392 return true;
393 //return a + w * ac; // barycentric coordinates (1-w,0,w)
394 }
395
396 // Check if P in edge region of BC, if so return projection of P onto BC
397 btScalar va = d3*d6 - d5*d4;
398 if (va <= btScalar(0.0) && (d4 - d3) >= btScalar(0.0) && (d5 - d6) >= btScalar(0.0)) {
399 btScalar w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
400
401 result.m_closestPointOnSimplex = b + w * (c - b);
402 result.m_usedVertices.usedVertexB = true;
403 result.m_usedVertices.usedVertexC = true;
404 result.setBarycentricCoordinates(0,1-w,w);
405 return true;
406 // return b + w * (c - b); // barycentric coordinates (0,1-w,w)
407 }
408
409 // P inside face region. Compute Q through its barycentric coordinates (u,v,w)
410 btScalar denom = btScalar(1.0) / (va + vb + vc);
411 btScalar v = vb * denom;
412 btScalar w = vc * denom;
413
414 result.m_closestPointOnSimplex = a + ab * v + ac * w;
415 result.m_usedVertices.usedVertexA = true;
416 result.m_usedVertices.usedVertexB = true;
417 result.m_usedVertices.usedVertexC = true;
418 result.setBarycentricCoordinates(1-v-w,v,w);
419
420 return true;
421 // return a + ab * v + ac * w; // = u*a + v*b + w*c, u = va * denom = btScalar(1.0) - v - w
422
423 }
424
425
426
427
428
429 /// Test if point p and d lie on opposite sides of plane through abc
pointOutsideOfPlane(const btVector3 & p,const btVector3 & a,const btVector3 & b,const btVector3 & c,const btVector3 & d)430 int btVoronoiSimplexSolver::pointOutsideOfPlane(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d)
431 {
432 btVector3 normal = (b-a).cross(c-a);
433
434 btScalar signp = (p - a).dot(normal); // [AP AB AC]
435 btScalar signd = (d - a).dot( normal); // [AD AB AC]
436
437 #ifdef CATCH_DEGENERATE_TETRAHEDRON
438 #ifdef BT_USE_DOUBLE_PRECISION
439 if (signd * signd < (btScalar(1e-8) * btScalar(1e-8)))
440 {
441 return -1;
442 }
443 #else
444 if (signd * signd < (btScalar(1e-4) * btScalar(1e-4)))
445 {
446 // printf("affine dependent/degenerate\n");//
447 return -1;
448 }
449 #endif
450
451 #endif
452 // Points on opposite sides if expression signs are opposite
453 return signp * signd < btScalar(0.);
454 }
455
456
closestPtPointTetrahedron(const btVector3 & p,const btVector3 & a,const btVector3 & b,const btVector3 & c,const btVector3 & d,btSubSimplexClosestResult & finalResult)457 bool btVoronoiSimplexSolver::closestPtPointTetrahedron(const btVector3& p, const btVector3& a, const btVector3& b, const btVector3& c, const btVector3& d, btSubSimplexClosestResult& finalResult)
458 {
459 btSubSimplexClosestResult tempResult;
460
461 // Start out assuming point inside all halfspaces, so closest to itself
462 finalResult.m_closestPointOnSimplex = p;
463 finalResult.m_usedVertices.reset();
464 finalResult.m_usedVertices.usedVertexA = true;
465 finalResult.m_usedVertices.usedVertexB = true;
466 finalResult.m_usedVertices.usedVertexC = true;
467 finalResult.m_usedVertices.usedVertexD = true;
468
469 int pointOutsideABC = pointOutsideOfPlane(p, a, b, c, d);
470 int pointOutsideACD = pointOutsideOfPlane(p, a, c, d, b);
471 int pointOutsideADB = pointOutsideOfPlane(p, a, d, b, c);
472 int pointOutsideBDC = pointOutsideOfPlane(p, b, d, c, a);
473
474 if (pointOutsideABC < 0 || pointOutsideACD < 0 || pointOutsideADB < 0 || pointOutsideBDC < 0)
475 {
476 finalResult.m_degenerate = true;
477 return false;
478 }
479
480 if (!pointOutsideABC && !pointOutsideACD && !pointOutsideADB && !pointOutsideBDC)
481 {
482 return false;
483 }
484
485
486 btScalar bestSqDist = FLT_MAX;
487 // If point outside face abc then compute closest point on abc
488 if (pointOutsideABC)
489 {
490 closestPtPointTriangle(p, a, b, c,tempResult);
491 btVector3 q = tempResult.m_closestPointOnSimplex;
492
493 btScalar sqDist = (q - p).dot( q - p);
494 // Update best closest point if (squared) distance is less than current best
495 if (sqDist < bestSqDist) {
496 bestSqDist = sqDist;
497 finalResult.m_closestPointOnSimplex = q;
498 //convert result bitmask!
499 finalResult.m_usedVertices.reset();
500 finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
501 finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexB;
502 finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
503 finalResult.setBarycentricCoordinates(
504 tempResult.m_barycentricCoords[VERTA],
505 tempResult.m_barycentricCoords[VERTB],
506 tempResult.m_barycentricCoords[VERTC],
507 0
508 );
509
510 }
511 }
512
513
514 // Repeat test for face acd
515 if (pointOutsideACD)
516 {
517 closestPtPointTriangle(p, a, c, d,tempResult);
518 btVector3 q = tempResult.m_closestPointOnSimplex;
519 //convert result bitmask!
520
521 btScalar sqDist = (q - p).dot( q - p);
522 if (sqDist < bestSqDist)
523 {
524 bestSqDist = sqDist;
525 finalResult.m_closestPointOnSimplex = q;
526 finalResult.m_usedVertices.reset();
527 finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
528
529 finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexB;
530 finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexC;
531 finalResult.setBarycentricCoordinates(
532 tempResult.m_barycentricCoords[VERTA],
533 0,
534 tempResult.m_barycentricCoords[VERTB],
535 tempResult.m_barycentricCoords[VERTC]
536 );
537
538 }
539 }
540 // Repeat test for face adb
541
542
543 if (pointOutsideADB)
544 {
545 closestPtPointTriangle(p, a, d, b,tempResult);
546 btVector3 q = tempResult.m_closestPointOnSimplex;
547 //convert result bitmask!
548
549 btScalar sqDist = (q - p).dot( q - p);
550 if (sqDist < bestSqDist)
551 {
552 bestSqDist = sqDist;
553 finalResult.m_closestPointOnSimplex = q;
554 finalResult.m_usedVertices.reset();
555 finalResult.m_usedVertices.usedVertexA = tempResult.m_usedVertices.usedVertexA;
556 finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexC;
557
558 finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
559 finalResult.setBarycentricCoordinates(
560 tempResult.m_barycentricCoords[VERTA],
561 tempResult.m_barycentricCoords[VERTC],
562 0,
563 tempResult.m_barycentricCoords[VERTB]
564 );
565
566 }
567 }
568 // Repeat test for face bdc
569
570
571 if (pointOutsideBDC)
572 {
573 closestPtPointTriangle(p, b, d, c,tempResult);
574 btVector3 q = tempResult.m_closestPointOnSimplex;
575 //convert result bitmask!
576 btScalar sqDist = (q - p).dot( q - p);
577 if (sqDist < bestSqDist)
578 {
579 bestSqDist = sqDist;
580 finalResult.m_closestPointOnSimplex = q;
581 finalResult.m_usedVertices.reset();
582 //
583 finalResult.m_usedVertices.usedVertexB = tempResult.m_usedVertices.usedVertexA;
584 finalResult.m_usedVertices.usedVertexC = tempResult.m_usedVertices.usedVertexC;
585 finalResult.m_usedVertices.usedVertexD = tempResult.m_usedVertices.usedVertexB;
586
587 finalResult.setBarycentricCoordinates(
588 0,
589 tempResult.m_barycentricCoords[VERTA],
590 tempResult.m_barycentricCoords[VERTC],
591 tempResult.m_barycentricCoords[VERTB]
592 );
593
594 }
595 }
596
597 //help! we ended up full !
598
599 if (finalResult.m_usedVertices.usedVertexA &&
600 finalResult.m_usedVertices.usedVertexB &&
601 finalResult.m_usedVertices.usedVertexC &&
602 finalResult.m_usedVertices.usedVertexD)
603 {
604 return true;
605 }
606
607 return true;
608 }
609
610