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
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 
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
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
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 
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
243 bool btVoronoiSimplexSolver::closest(btVector3& v)
244 {
245  bool succes = updateClosestVectorAndPoints();
246  v = m_cachedV;
247  return succes;
248 }
249 
250 
251 
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
268 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 
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 
307 void btVoronoiSimplexSolver::backup_closest(btVector3& v)
308 {
309  v = m_cachedV;
310 }
311 
312 
313 bool btVoronoiSimplexSolver::emptySimplex() const
314 {
315  return (numVertices() == 0);
316 
317 }
318 
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 
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 
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 
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 
Generated on Thu Oct 3 2013 18:25:07 for Bullet Collision Detection & Physics Library by   doxygen 1.8.4