Bullet Collision Detection & Physics Library
btVoronoiSimplexSolver.cpp
Go to the documentation of this file.
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
36 {
37 
39  m_numVertices--;
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 {
68  m_cachedValidClosest = false;
69  m_numVertices = 0;
70  m_needsUpdate = true;
72  m_cachedBC.reset();
73 }
74 
75 
76 
77  //add a vertex
79 {
80  m_lastW = w;
81  m_needsUpdate = true;
82 
86 
87  m_numVertices++;
88 }
89 
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  {
108  m_cachedV = m_cachedP1-m_cachedP2; //== m_simplexVectorW[0]
109  m_cachedBC.reset();
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  {
219  m_cachedValidClosest = true;
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 
238  return m_cachedValidClosest;
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  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 
308 {
309  v = m_cachedV;
310 }
311 
312 
314 {
315  return (numVertices() == 0);
316 
317 }
318 
320 {
322  p1 = m_cachedP1;
323  p2 = m_cachedP2;
324 
325 }
326 
327 
328 
329 
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 
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 
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 
unsigned short usedVertexA
#define BT_LARGE_FLOAT
Definition: btScalar.h:268
#define VERTC
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
unsigned short usedVertexD
#define btAssert(x)
Definition: btScalar.h:101
void reduceVertices(const btUsageBitfield &usedVerts)
unsigned short usedVertexB
#define VERTB
btVector3 m_simplexPointsQ[VORONOI_SIMPLEX_MAX_VERTS]
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
bool inSimplex(const btVector3 &w)
#define VERTA
void setBarycentricCoordinates(btScalar a=btScalar(0.), btScalar b=btScalar(0.), btScalar c=btScalar(0.), btScalar d=btScalar(0.))
int getSimplex(btVector3 *pBuf, btVector3 *qBuf, btVector3 *yBuf) const
void addVertex(const btVector3 &w, const btVector3 &p, const btVector3 &q)
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
bool closestPtPointTriangle(const btVector3 &p, const btVector3 &a, const btVector3 &b, const btVector3 &c, btSubSimplexClosestResult &result)
btSubSimplexClosestResult m_cachedBC
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.
static float4 cross(const float4 &a, const float4 &b)
unsigned short usedVertexC
void compute_points(btVector3 &p1, btVector3 &p2)
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:827
btVector3 m_simplexVectorW[VORONOI_SIMPLEX_MAX_VERTS]
btVector3 m_simplexPointsP[VORONOI_SIMPLEX_MAX_VERTS]
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:266
bool closestPtPointTetrahedron(const btVector3 &p, const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &d, btSubSimplexClosestResult &finalResult)