Bullet Collision Detection & Physics Library
btMprPenetration.h
Go to the documentation of this file.
1 
2 /***
3  * ---------------------------------
4  * Copyright (c)2012 Daniel Fiser <danfis@danfis.cz>
5  *
6  * This file was ported from mpr.c file, part of libccd.
7  * The Minkoski Portal Refinement implementation was ported
8  * to OpenCL by Erwin Coumans for the Bullet 3 Physics library.
9  * The original MPR idea and implementation is by Gary Snethen
10  * in XenoCollide, see http://github.com/erwincoumans/xenocollide
11  *
12  * Distributed under the OSI-approved BSD License (the "License");
13  * see <http://www.opensource.org/licenses/bsd-license.php>.
14  * This software is distributed WITHOUT ANY WARRANTY; without even the
15  * implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
16  * See the License for more information.
17  */
18 
20 
21 #ifndef BT_MPR_PENETRATION_H
22 #define BT_MPR_PENETRATION_H
23 
24 #define BT_DEBUG_MPR1
25 
26 #include "LinearMath/btTransform.h"
28 
29 //#define MPR_AVERAGE_CONTACT_POSITIONS
30 
32 {
37 
39  : m_firstDir(0, 1, 0),
40  m_maxGjkIterations(1000),
42  m_gjkRelError2(1.0e-6)
43  {
44  }
46  {
47  }
48 };
49 
50 struct btMprDistanceInfo
51 {
56 };
57 
58 #ifdef __cplusplus
59 #define BT_MPR_SQRT sqrtf
60 #else
61 #define BT_MPR_SQRT sqrt
62 #endif
63 #define BT_MPR_FMIN(x, y) ((x) < (y) ? (x) : (y))
64 #define BT_MPR_FABS fabs
65 
66 #define BT_MPR_TOLERANCE 1E-6f
67 #define BT_MPR_MAX_ITERATIONS 1000
68 
69 struct _btMprSupport_t
70 {
73  btVector3 v2;
74 };
75 typedef struct _btMprSupport_t btMprSupport_t;
76 
77 struct _btMprSimplex_t
78 {
80  int last;
81 };
82 typedef struct _btMprSimplex_t btMprSimplex_t;
83 
85 {
86  return &s->ps[idx];
87 }
88 
89 inline void btMprSimplexSetSize(btMprSimplex_t *s, int size)
90 {
91  s->last = size - 1;
92 }
93 
94 #ifdef DEBUG_MPR
95 inline void btPrintPortalVertex(_btMprSimplex_t *portal, int index)
96 {
97  printf("portal[%d].v = %f,%f,%f, v1=%f,%f,%f, v2=%f,%f,%f\n", index, portal->ps[index].v.x(), portal->ps[index].v.y(), portal->ps[index].v.z(),
98  portal->ps[index].v1.x(), portal->ps[index].v1.y(), portal->ps[index].v1.z(),
99  portal->ps[index].v2.x(), portal->ps[index].v2.y(), portal->ps[index].v2.z());
100 }
101 #endif //DEBUG_MPR
102 
103 inline int btMprSimplexSize(const btMprSimplex_t *s)
104 {
105  return s->last + 1;
106 }
107 
108 inline const btMprSupport_t *btMprSimplexPoint(const btMprSimplex_t *s, int idx)
109 {
110  // here is no check on boundaries
111  return &s->ps[idx];
112 }
113 
114 inline void btMprSupportCopy(btMprSupport_t *d, const btMprSupport_t *s)
115 {
116  *d = *s;
117 }
118 
119 inline void btMprSimplexSet(btMprSimplex_t *s, size_t pos, const btMprSupport_t *a)
120 {
121  btMprSupportCopy(s->ps + pos, a);
122 }
123 
124 inline void btMprSimplexSwap(btMprSimplex_t *s, size_t pos1, size_t pos2)
125 {
126  btMprSupport_t supp;
127 
128  btMprSupportCopy(&supp, &s->ps[pos1]);
129  btMprSupportCopy(&s->ps[pos1], &s->ps[pos2]);
130  btMprSupportCopy(&s->ps[pos2], &supp);
131 }
132 
133 inline int btMprIsZero(float val)
134 {
135  return BT_MPR_FABS(val) < FLT_EPSILON;
136 }
137 
138 inline int btMprEq(float _a, float _b)
139 {
140  float ab;
141  float a, b;
142 
143  ab = BT_MPR_FABS(_a - _b);
144  if (BT_MPR_FABS(ab) < FLT_EPSILON)
145  return 1;
146 
147  a = BT_MPR_FABS(_a);
148  b = BT_MPR_FABS(_b);
149  if (b > a)
150  {
151  return ab < FLT_EPSILON * b;
152  }
153  else
154  {
155  return ab < FLT_EPSILON * a;
156  }
157 }
158 
159 inline int btMprVec3Eq(const btVector3 *a, const btVector3 *b)
160 {
161  return btMprEq((*a).x(), (*b).x()) && btMprEq((*a).y(), (*b).y()) && btMprEq((*a).z(), (*b).z());
162 }
163 
164 template <typename btConvexTemplate>
165 inline void btFindOrigin(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprSupport_t *center)
166 {
167  center->v1 = a.getObjectCenterInWorld();
168  center->v2 = b.getObjectCenterInWorld();
169  center->v = center->v1 - center->v2;
170 }
171 
172 inline void btMprVec3Set(btVector3 *v, float x, float y, float z)
173 {
174  v->setValue(x, y, z);
175 }
176 
177 inline void btMprVec3Add(btVector3 *v, const btVector3 *w)
178 {
179  *v += *w;
180 }
181 
182 inline void btMprVec3Copy(btVector3 *v, const btVector3 *w)
183 {
184  *v = *w;
185 }
186 
187 inline void btMprVec3Scale(btVector3 *d, float k)
188 {
189  *d *= k;
190 }
191 
192 inline float btMprVec3Dot(const btVector3 *a, const btVector3 *b)
193 {
194  float dot;
195 
196  dot = btDot(*a, *b);
197  return dot;
198 }
199 
200 inline float btMprVec3Len2(const btVector3 *v)
201 {
202  return btMprVec3Dot(v, v);
203 }
204 
205 inline void btMprVec3Normalize(btVector3 *d)
206 {
207  float k = 1.f / BT_MPR_SQRT(btMprVec3Len2(d));
208  btMprVec3Scale(d, k);
209 }
210 
211 inline void btMprVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
212 {
213  *d = btCross(*a, *b);
214 }
215 
216 inline void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
217 {
218  *d = *v - *w;
219 }
220 
221 inline void btPortalDir(const btMprSimplex_t *portal, btVector3 *dir)
222 {
223  btVector3 v2v1, v3v1;
224 
225  btMprVec3Sub2(&v2v1, &btMprSimplexPoint(portal, 2)->v,
226  &btMprSimplexPoint(portal, 1)->v);
227  btMprVec3Sub2(&v3v1, &btMprSimplexPoint(portal, 3)->v,
228  &btMprSimplexPoint(portal, 1)->v);
229  btMprVec3Cross(dir, &v2v1, &v3v1);
230  btMprVec3Normalize(dir);
231 }
232 
233 inline int portalEncapsulesOrigin(const btMprSimplex_t *portal,
234  const btVector3 *dir)
235 {
236  float dot;
237  dot = btMprVec3Dot(dir, &btMprSimplexPoint(portal, 1)->v);
238  return btMprIsZero(dot) || dot > 0.f;
239 }
240 
241 inline int portalReachTolerance(const btMprSimplex_t *portal,
242  const btMprSupport_t *v4,
243  const btVector3 *dir)
244 {
245  float dv1, dv2, dv3, dv4;
246  float dot1, dot2, dot3;
247 
248  // find the smallest dot product of dir and {v1-v4, v2-v4, v3-v4}
249 
250  dv1 = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, dir);
251  dv2 = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, dir);
252  dv3 = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, dir);
253  dv4 = btMprVec3Dot(&v4->v, dir);
254 
255  dot1 = dv4 - dv1;
256  dot2 = dv4 - dv2;
257  dot3 = dv4 - dv3;
258 
259  dot1 = BT_MPR_FMIN(dot1, dot2);
260  dot1 = BT_MPR_FMIN(dot1, dot3);
261 
262  return btMprEq(dot1, BT_MPR_TOLERANCE) || dot1 < BT_MPR_TOLERANCE;
263 }
264 
265 inline int portalCanEncapsuleOrigin(const btMprSimplex_t *portal,
266  const btMprSupport_t *v4,
267  const btVector3 *dir)
268 {
269  float dot;
270  dot = btMprVec3Dot(&v4->v, dir);
271  return btMprIsZero(dot) || dot > 0.f;
272 }
273 
274 inline void btExpandPortal(btMprSimplex_t *portal,
275  const btMprSupport_t *v4)
276 {
277  float dot;
278  btVector3 v4v0;
279 
280  btMprVec3Cross(&v4v0, &v4->v, &btMprSimplexPoint(portal, 0)->v);
281  dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &v4v0);
282  if (dot > 0.f)
283  {
284  dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &v4v0);
285  if (dot > 0.f)
286  {
287  btMprSimplexSet(portal, 1, v4);
288  }
289  else
290  {
291  btMprSimplexSet(portal, 3, v4);
292  }
293  }
294  else
295  {
296  dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &v4v0);
297  if (dot > 0.f)
298  {
299  btMprSimplexSet(portal, 2, v4);
300  }
301  else
302  {
303  btMprSimplexSet(portal, 1, v4);
304  }
305  }
306 }
307 template <typename btConvexTemplate>
308 inline void btMprSupport(const btConvexTemplate &a, const btConvexTemplate &b,
309  const btMprCollisionDescription &colDesc,
310  const btVector3 &dir, btMprSupport_t *supp)
311 {
312  btVector3 separatingAxisInA = dir * a.getWorldTransform().getBasis();
313  btVector3 separatingAxisInB = -dir * b.getWorldTransform().getBasis();
314 
315  btVector3 pInA = a.getLocalSupportWithMargin(separatingAxisInA);
316  btVector3 qInB = b.getLocalSupportWithMargin(separatingAxisInB);
317 
318  supp->v1 = a.getWorldTransform()(pInA);
319  supp->v2 = b.getWorldTransform()(qInB);
320  supp->v = supp->v1 - supp->v2;
321 }
322 
323 template <typename btConvexTemplate>
324 static int btDiscoverPortal(const btConvexTemplate &a, const btConvexTemplate &b,
325  const btMprCollisionDescription &colDesc,
326  btMprSimplex_t *portal)
327 {
328  btVector3 dir, va, vb;
329  float dot;
330  int cont;
331 
332  // vertex 0 is center of portal
333  btFindOrigin(a, b, colDesc, btMprSimplexPointW(portal, 0));
334 
335  // vertex 0 is center of portal
336  btMprSimplexSetSize(portal, 1);
337 
338  btVector3 zero = btVector3(0, 0, 0);
339  btVector3 *org = &zero;
340 
341  if (btMprVec3Eq(&btMprSimplexPoint(portal, 0)->v, org))
342  {
343  // Portal's center lies on origin (0,0,0) => we know that objects
344  // intersect but we would need to know penetration info.
345  // So move center little bit...
346  btMprVec3Set(&va, FLT_EPSILON * 10.f, 0.f, 0.f);
347  btMprVec3Add(&btMprSimplexPointW(portal, 0)->v, &va);
348  }
349 
350  // vertex 1 = support in direction of origin
351  btMprVec3Copy(&dir, &btMprSimplexPoint(portal, 0)->v);
352  btMprVec3Scale(&dir, -1.f);
353  btMprVec3Normalize(&dir);
354 
355  btMprSupport(a, b, colDesc, dir, btMprSimplexPointW(portal, 1));
356 
357  btMprSimplexSetSize(portal, 2);
358 
359  // test if origin isn't outside of v1
360  dot = btMprVec3Dot(&btMprSimplexPoint(portal, 1)->v, &dir);
361 
362  if (btMprIsZero(dot) || dot < 0.f)
363  return -1;
364 
365  // vertex 2
366  btMprVec3Cross(&dir, &btMprSimplexPoint(portal, 0)->v,
367  &btMprSimplexPoint(portal, 1)->v);
368  if (btMprIsZero(btMprVec3Len2(&dir)))
369  {
370  if (btMprVec3Eq(&btMprSimplexPoint(portal, 1)->v, org))
371  {
372  // origin lies on v1
373  return 1;
374  }
375  else
376  {
377  // origin lies on v0-v1 segment
378  return 2;
379  }
380  }
381 
382  btMprVec3Normalize(&dir);
383  btMprSupport(a, b, colDesc, dir, btMprSimplexPointW(portal, 2));
384 
385  dot = btMprVec3Dot(&btMprSimplexPoint(portal, 2)->v, &dir);
386  if (btMprIsZero(dot) || dot < 0.f)
387  return -1;
388 
389  btMprSimplexSetSize(portal, 3);
390 
391  // vertex 3 direction
392  btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v,
393  &btMprSimplexPoint(portal, 0)->v);
394  btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v,
395  &btMprSimplexPoint(portal, 0)->v);
396  btMprVec3Cross(&dir, &va, &vb);
397  btMprVec3Normalize(&dir);
398 
399  // it is better to form portal faces to be oriented "outside" origin
400  dot = btMprVec3Dot(&dir, &btMprSimplexPoint(portal, 0)->v);
401  if (dot > 0.f)
402  {
403  btMprSimplexSwap(portal, 1, 2);
404  btMprVec3Scale(&dir, -1.f);
405  }
406 
407  while (btMprSimplexSize(portal) < 4)
408  {
409  btMprSupport(a, b, colDesc, dir, btMprSimplexPointW(portal, 3));
410 
411  dot = btMprVec3Dot(&btMprSimplexPoint(portal, 3)->v, &dir);
412  if (btMprIsZero(dot) || dot < 0.f)
413  return -1;
414 
415  cont = 0;
416 
417  // test if origin is outside (v1, v0, v3) - set v2 as v3 and
418  // continue
419  btMprVec3Cross(&va, &btMprSimplexPoint(portal, 1)->v,
420  &btMprSimplexPoint(portal, 3)->v);
421  dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v);
422  if (dot < 0.f && !btMprIsZero(dot))
423  {
424  btMprSimplexSet(portal, 2, btMprSimplexPoint(portal, 3));
425  cont = 1;
426  }
427 
428  if (!cont)
429  {
430  // test if origin is outside (v3, v0, v2) - set v1 as v3 and
431  // continue
432  btMprVec3Cross(&va, &btMprSimplexPoint(portal, 3)->v,
433  &btMprSimplexPoint(portal, 2)->v);
434  dot = btMprVec3Dot(&va, &btMprSimplexPoint(portal, 0)->v);
435  if (dot < 0.f && !btMprIsZero(dot))
436  {
437  btMprSimplexSet(portal, 1, btMprSimplexPoint(portal, 3));
438  cont = 1;
439  }
440  }
441 
442  if (cont)
443  {
444  btMprVec3Sub2(&va, &btMprSimplexPoint(portal, 1)->v,
445  &btMprSimplexPoint(portal, 0)->v);
446  btMprVec3Sub2(&vb, &btMprSimplexPoint(portal, 2)->v,
447  &btMprSimplexPoint(portal, 0)->v);
448  btMprVec3Cross(&dir, &va, &vb);
449  btMprVec3Normalize(&dir);
450  }
451  else
452  {
453  btMprSimplexSetSize(portal, 4);
454  }
455  }
456 
457  return 0;
458 }
459 
460 template <typename btConvexTemplate>
461 static int btRefinePortal(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc,
462  btMprSimplex_t *portal)
463 {
464  btVector3 dir;
465  btMprSupport_t v4;
466 
467  for (int i = 0; i < BT_MPR_MAX_ITERATIONS; i++)
468  //while (1)
469  {
470  // compute direction outside the portal (from v0 through v1,v2,v3
471  // face)
472  btPortalDir(portal, &dir);
473 
474  // test if origin is inside the portal
475  if (portalEncapsulesOrigin(portal, &dir))
476  return 0;
477 
478  // get next support point
479 
480  btMprSupport(a, b, colDesc, dir, &v4);
481 
482  // test if v4 can expand portal to contain origin and if portal
483  // expanding doesn't reach given tolerance
484  if (!portalCanEncapsuleOrigin(portal, &v4, &dir) || portalReachTolerance(portal, &v4, &dir))
485  {
486  return -1;
487  }
488 
489  // v1-v2-v3 triangle must be rearranged to face outside Minkowski
490  // difference (direction from v0).
491  btExpandPortal(portal, &v4);
492  }
493 
494  return -1;
495 }
496 
497 static void btFindPos(const btMprSimplex_t *portal, btVector3 *pos)
498 {
499  btVector3 zero = btVector3(0, 0, 0);
500  btVector3 *origin = &zero;
501 
502  btVector3 dir;
503  size_t i;
504  float b[4], sum, inv;
505  btVector3 vec, p1, p2;
506 
507  btPortalDir(portal, &dir);
508 
509  // use barycentric coordinates of tetrahedron to find origin
510  btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v,
511  &btMprSimplexPoint(portal, 2)->v);
512  b[0] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v);
513 
514  btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v,
515  &btMprSimplexPoint(portal, 2)->v);
516  b[1] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v);
517 
518  btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 0)->v,
519  &btMprSimplexPoint(portal, 1)->v);
520  b[2] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 3)->v);
521 
522  btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v,
523  &btMprSimplexPoint(portal, 1)->v);
524  b[3] = btMprVec3Dot(&vec, &btMprSimplexPoint(portal, 0)->v);
525 
526  sum = b[0] + b[1] + b[2] + b[3];
527 
528  if (btMprIsZero(sum) || sum < 0.f)
529  {
530  b[0] = 0.f;
531 
532  btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 2)->v,
533  &btMprSimplexPoint(portal, 3)->v);
534  b[1] = btMprVec3Dot(&vec, &dir);
535  btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 3)->v,
536  &btMprSimplexPoint(portal, 1)->v);
537  b[2] = btMprVec3Dot(&vec, &dir);
538  btMprVec3Cross(&vec, &btMprSimplexPoint(portal, 1)->v,
539  &btMprSimplexPoint(portal, 2)->v);
540  b[3] = btMprVec3Dot(&vec, &dir);
541 
542  sum = b[1] + b[2] + b[3];
543  }
544 
545  inv = 1.f / sum;
546 
547  btMprVec3Copy(&p1, origin);
548  btMprVec3Copy(&p2, origin);
549  for (i = 0; i < 4; i++)
550  {
551  btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v1);
552  btMprVec3Scale(&vec, b[i]);
553  btMprVec3Add(&p1, &vec);
554 
555  btMprVec3Copy(&vec, &btMprSimplexPoint(portal, i)->v2);
556  btMprVec3Scale(&vec, b[i]);
557  btMprVec3Add(&p2, &vec);
558  }
559  btMprVec3Scale(&p1, inv);
560  btMprVec3Scale(&p2, inv);
561 #ifdef MPR_AVERAGE_CONTACT_POSITIONS
562  btMprVec3Copy(pos, &p1);
563  btMprVec3Add(pos, &p2);
564  btMprVec3Scale(pos, 0.5);
565 #else
566  btMprVec3Copy(pos, &p2);
567 #endif //MPR_AVERAGE_CONTACT_POSITIONS
568 }
569 
570 inline float btMprVec3Dist2(const btVector3 *a, const btVector3 *b)
571 {
572  btVector3 ab;
573  btMprVec3Sub2(&ab, a, b);
574  return btMprVec3Len2(&ab);
575 }
576 
577 inline float _btMprVec3PointSegmentDist2(const btVector3 *P,
578  const btVector3 *x0,
579  const btVector3 *b,
580  btVector3 *witness)
581 {
582  // The computation comes from solving equation of segment:
583  // S(t) = x0 + t.d
584  // where - x0 is initial point of segment
585  // - d is direction of segment from x0 (|d| > 0)
586  // - t belongs to <0, 1> interval
587  //
588  // Than, distance from a segment to some point P can be expressed:
589  // D(t) = |x0 + t.d - P|^2
590  // which is distance from any point on segment. Minimization
591  // of this function brings distance from P to segment.
592  // Minimization of D(t) leads to simple quadratic equation that's
593  // solving is straightforward.
594  //
595  // Bonus of this method is witness point for free.
596 
597  float dist, t;
598  btVector3 d, a;
599 
600  // direction of segment
601  btMprVec3Sub2(&d, b, x0);
602 
603  // precompute vector from P to x0
604  btMprVec3Sub2(&a, x0, P);
605 
606  t = -1.f * btMprVec3Dot(&a, &d);
607  t /= btMprVec3Len2(&d);
608 
609  if (t < 0.f || btMprIsZero(t))
610  {
611  dist = btMprVec3Dist2(x0, P);
612  if (witness)
613  btMprVec3Copy(witness, x0);
614  }
615  else if (t > 1.f || btMprEq(t, 1.f))
616  {
617  dist = btMprVec3Dist2(b, P);
618  if (witness)
619  btMprVec3Copy(witness, b);
620  }
621  else
622  {
623  if (witness)
624  {
625  btMprVec3Copy(witness, &d);
626  btMprVec3Scale(witness, t);
627  btMprVec3Add(witness, x0);
628  dist = btMprVec3Dist2(witness, P);
629  }
630  else
631  {
632  // recycling variables
633  btMprVec3Scale(&d, t);
634  btMprVec3Add(&d, &a);
635  dist = btMprVec3Len2(&d);
636  }
637  }
638 
639  return dist;
640 }
641 
642 inline float btMprVec3PointTriDist2(const btVector3 *P,
643  const btVector3 *x0, const btVector3 *B,
644  const btVector3 *C,
645  btVector3 *witness)
646 {
647  // Computation comes from analytic expression for triangle (x0, B, C)
648  // T(s, t) = x0 + s.d1 + t.d2, where d1 = B - x0 and d2 = C - x0 and
649  // Then equation for distance is:
650  // D(s, t) = | T(s, t) - P |^2
651  // This leads to minimization of quadratic function of two variables.
652  // The solution from is taken only if s is between 0 and 1, t is
653  // between 0 and 1 and t + s < 1, otherwise distance from segment is
654  // computed.
655 
656  btVector3 d1, d2, a;
657  float u, v, w, p, q, r;
658  float s, t, dist, dist2;
659  btVector3 witness2;
660 
661  btMprVec3Sub2(&d1, B, x0);
662  btMprVec3Sub2(&d2, C, x0);
663  btMprVec3Sub2(&a, x0, P);
664 
665  u = btMprVec3Dot(&a, &a);
666  v = btMprVec3Dot(&d1, &d1);
667  w = btMprVec3Dot(&d2, &d2);
668  p = btMprVec3Dot(&a, &d1);
669  q = btMprVec3Dot(&a, &d2);
670  r = btMprVec3Dot(&d1, &d2);
671 
672  btScalar div = (w * v - r * r);
673  if (btMprIsZero(div))
674  {
675  s = -1;
676  }
677  else
678  {
679  s = (q * r - w * p) / div;
680  t = (-s * r - q) / w;
681  }
682 
683  if ((btMprIsZero(s) || s > 0.f) && (btMprEq(s, 1.f) || s < 1.f) && (btMprIsZero(t) || t > 0.f) && (btMprEq(t, 1.f) || t < 1.f) && (btMprEq(t + s, 1.f) || t + s < 1.f))
684  {
685  if (witness)
686  {
687  btMprVec3Scale(&d1, s);
688  btMprVec3Scale(&d2, t);
689  btMprVec3Copy(witness, x0);
690  btMprVec3Add(witness, &d1);
691  btMprVec3Add(witness, &d2);
692 
693  dist = btMprVec3Dist2(witness, P);
694  }
695  else
696  {
697  dist = s * s * v;
698  dist += t * t * w;
699  dist += 2.f * s * t * r;
700  dist += 2.f * s * p;
701  dist += 2.f * t * q;
702  dist += u;
703  }
704  }
705  else
706  {
707  dist = _btMprVec3PointSegmentDist2(P, x0, B, witness);
708 
709  dist2 = _btMprVec3PointSegmentDist2(P, x0, C, &witness2);
710  if (dist2 < dist)
711  {
712  dist = dist2;
713  if (witness)
714  btMprVec3Copy(witness, &witness2);
715  }
716 
717  dist2 = _btMprVec3PointSegmentDist2(P, B, C, &witness2);
718  if (dist2 < dist)
719  {
720  dist = dist2;
721  if (witness)
722  btMprVec3Copy(witness, &witness2);
723  }
724  }
725 
726  return dist;
727 }
728 
729 template <typename btConvexTemplate>
730 static void btFindPenetr(const btConvexTemplate &a, const btConvexTemplate &b,
731  const btMprCollisionDescription &colDesc,
732  btMprSimplex_t *portal,
733  float *depth, btVector3 *pdir, btVector3 *pos)
734 {
735  btVector3 dir;
736  btMprSupport_t v4;
737  unsigned long iterations;
738 
739  btVector3 zero = btVector3(0, 0, 0);
740  btVector3 *origin = &zero;
741 
742  iterations = 1UL;
743  for (int i = 0; i < BT_MPR_MAX_ITERATIONS; i++)
744  //while (1)
745  {
746  // compute portal direction and obtain next support point
747  btPortalDir(portal, &dir);
748 
749  btMprSupport(a, b, colDesc, dir, &v4);
750 
751  // reached tolerance -> find penetration info
752  if (portalReachTolerance(portal, &v4, &dir) || iterations == BT_MPR_MAX_ITERATIONS)
753  {
754  *depth = btMprVec3PointTriDist2(origin, &btMprSimplexPoint(portal, 1)->v, &btMprSimplexPoint(portal, 2)->v, &btMprSimplexPoint(portal, 3)->v, pdir);
755  *depth = BT_MPR_SQRT(*depth);
756 
757  if (btMprIsZero((*pdir).x()) && btMprIsZero((*pdir).y()) && btMprIsZero((*pdir).z()))
758  {
759  *pdir = dir;
760  }
761  btMprVec3Normalize(pdir);
762 
763  // barycentric coordinates:
764  btFindPos(portal, pos);
765 
766  return;
767  }
768 
769  btExpandPortal(portal, &v4);
770 
771  iterations++;
772  }
773 }
774 
775 static void btFindPenetrTouch(btMprSimplex_t *portal, float *depth, btVector3 *dir, btVector3 *pos)
776 {
777  // Touching contact on portal's v1 - so depth is zero and direction
778  // is unimportant and pos can be guessed
779  *depth = 0.f;
780  btVector3 zero = btVector3(0, 0, 0);
781  btVector3 *origin = &zero;
782 
783  btMprVec3Copy(dir, origin);
784 #ifdef MPR_AVERAGE_CONTACT_POSITIONS
785  btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1);
786  btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2);
787  btMprVec3Scale(pos, 0.5);
788 #else
789  btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2);
790 #endif
791 }
792 
793 static void btFindPenetrSegment(btMprSimplex_t *portal,
794  float *depth, btVector3 *dir, btVector3 *pos)
795 {
796  // Origin lies on v0-v1 segment.
797  // Depth is distance to v1, direction also and position must be
798  // computed
799 #ifdef MPR_AVERAGE_CONTACT_POSITIONS
800  btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v1);
801  btMprVec3Add(pos, &btMprSimplexPoint(portal, 1)->v2);
802  btMprVec3Scale(pos, 0.5f);
803 #else
804  btMprVec3Copy(pos, &btMprSimplexPoint(portal, 1)->v2);
805 #endif //MPR_AVERAGE_CONTACT_POSITIONS
806 
807  btMprVec3Copy(dir, &btMprSimplexPoint(portal, 1)->v);
808  *depth = BT_MPR_SQRT(btMprVec3Len2(dir));
809  btMprVec3Normalize(dir);
810 }
811 
812 template <typename btConvexTemplate>
813 inline int btMprPenetration(const btConvexTemplate &a, const btConvexTemplate &b,
814  const btMprCollisionDescription &colDesc,
815  float *depthOut, btVector3 *dirOut, btVector3 *posOut)
816 {
817  btMprSimplex_t portal;
818 
819  // Phase 1: Portal discovery
820  int result = btDiscoverPortal(a, b, colDesc, &portal);
821 
822  //sepAxis[pairIndex] = *pdir;//or -dir?
823 
824  switch (result)
825  {
826  case 0:
827  {
828  // Phase 2: Portal refinement
829 
830  result = btRefinePortal(a, b, colDesc, &portal);
831  if (result < 0)
832  return -1;
833 
834  // Phase 3. Penetration info
835  btFindPenetr(a, b, colDesc, &portal, depthOut, dirOut, posOut);
836 
837  break;
838  }
839  case 1:
840  {
841  // Touching contact on portal's v1.
842  btFindPenetrTouch(&portal, depthOut, dirOut, posOut);
843  result = 0;
844  break;
845  }
846  case 2:
847  {
848  btFindPenetrSegment(&portal, depthOut, dirOut, posOut);
849  result = 0;
850  break;
851  }
852  default:
853  {
854  //if (res < 0)
855  //{
856  // Origin isn't inside portal - no collision.
857  result = -1;
858  //}
859  }
860  };
861 
862  return result;
863 };
864 
865 template <typename btConvexTemplate, typename btMprDistanceTemplate>
866 inline int btComputeMprPenetration(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprDistanceTemplate *distInfo)
867 {
868  btVector3 dir, pos;
869  float depth;
870 
871  int res = btMprPenetration(a, b, colDesc, &depth, &dir, &pos);
872  if (res == 0)
873  {
874  distInfo->m_distance = -depth;
875  distInfo->m_pointOnB = pos;
876  distInfo->m_normalBtoA = -dir;
877  distInfo->m_pointOnA = pos - distInfo->m_distance * dir;
878  return 0;
879  }
880 
881  return -1;
882 }
883 
884 #endif //BT_MPR_PENETRATION_H
BT_MPR_FMIN
#define BT_MPR_FMIN(x, y)
Definition: btMprPenetration.h:62
btMprDistanceInfo::m_pointOnA
btVector3 m_pointOnA
Definition: btMprPenetration.h:51
_btMprSupport_t::v2
btVector3 v2
Support point in obj2.
Definition: btMprPenetration.h:72
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:888
btMprVec3Cross
void btMprVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
Definition: btMprPenetration.h:210
portalReachTolerance
int portalReachTolerance(const btMprSimplex_t *portal, const btMprSupport_t *v4, const btVector3 *dir)
Definition: btMprPenetration.h:240
btMprVec3Eq
int btMprVec3Eq(const btVector3 *a, const btVector3 *b)
Definition: btMprPenetration.h:158
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btMprVec3Sub2
void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
Definition: btMprPenetration.h:215
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
_btMprSupport_t::v
btVector3 v
Support point in minkowski sum.
Definition: btMprPenetration.h:70
btDiscoverPortal
static int btDiscoverPortal(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprSimplex_t *portal)
Definition: btMprPenetration.h:323
btMprCollisionDescription::~btMprCollisionDescription
virtual ~btMprCollisionDescription()
Definition: btMprPenetration.h:44
_btMprSimplex_t::last
int last
index of last added point
Definition: btMprPenetration.h:79
btMprDistanceInfo
Definition: btMprPenetration.h:49
btMprSupportCopy
void btMprSupportCopy(btMprSupport_t *d, const btMprSupport_t *s)
Definition: btMprPenetration.h:113
BT_MPR_TOLERANCE
#define BT_MPR_TOLERANCE
Definition: btMprPenetration.h:65
btComputeMprPenetration
int btComputeMprPenetration(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprDistanceTemplate *distInfo)
Definition: btMprPenetration.h:865
btMprCollisionDescription
Definition: btMprPenetration.h:30
btMprSimplexPointW
btMprSupport_t * btMprSimplexPointW(btMprSimplex_t *s, int idx)
Definition: btMprPenetration.h:83
_btMprVec3PointSegmentDist2
float _btMprVec3PointSegmentDist2(const btVector3 *P, const btVector3 *x0, const btVector3 *b, btVector3 *witness)
Definition: btMprPenetration.h:576
btCross
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:918
btMprDistanceInfo::m_distance
btScalar m_distance
Definition: btMprPenetration.h:54
BT_MPR_FABS
#define BT_MPR_FABS
Definition: btMprPenetration.h:63
btExpandPortal
void btExpandPortal(btMprSimplex_t *portal, const btMprSupport_t *v4)
Definition: btMprPenetration.h:273
btPortalDir
void btPortalDir(const btMprSimplex_t *portal, btVector3 *dir)
Definition: btMprPenetration.h:220
btMprCollisionDescription::btMprCollisionDescription
btMprCollisionDescription()
Definition: btMprPenetration.h:37
btMprSimplexSet
void btMprSimplexSet(btMprSimplex_t *s, size_t pos, const btMprSupport_t *a)
Definition: btMprPenetration.h:118
btMprVec3Dist2
float btMprVec3Dist2(const btVector3 *a, const btVector3 *b)
Definition: btMprPenetration.h:569
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btFindPenetr
static void btFindPenetr(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprSimplex_t *portal, float *depth, btVector3 *pdir, btVector3 *pos)
Definition: btMprPenetration.h:729
_btMprSupport_t
Definition: btMprPenetration.h:68
_btMprSimplex_t::ps
btMprSupport_t ps[4]
Definition: btMprPenetration.h:78
btMprVec3PointTriDist2
float btMprVec3PointTriDist2(const btVector3 *P, const btVector3 *x0, const btVector3 *B, const btVector3 *C, btVector3 *witness)
Definition: btMprPenetration.h:641
btMprSimplexSize
int btMprSimplexSize(const btMprSimplex_t *s)
Definition: btMprPenetration.h:102
btMprVec3Len2
float btMprVec3Len2(const btVector3 *v)
Definition: btMprPenetration.h:199
btMprSimplexSetSize
void btMprSimplexSetSize(btMprSimplex_t *s, int size)
Definition: btMprPenetration.h:88
btMprSupport
void btMprSupport(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, const btVector3 &dir, btMprSupport_t *supp)
Definition: btMprPenetration.h:307
BT_MPR_MAX_ITERATIONS
#define BT_MPR_MAX_ITERATIONS
Definition: btMprPenetration.h:66
btMprCollisionDescription::m_maximumDistanceSquared
btScalar m_maximumDistanceSquared
Definition: btMprPenetration.h:34
btTransform.h
btMprVec3Normalize
void btMprVec3Normalize(btVector3 *d)
Definition: btMprPenetration.h:204
portalCanEncapsuleOrigin
int portalCanEncapsuleOrigin(const btMprSimplex_t *portal, const btMprSupport_t *v4, const btVector3 *dir)
Definition: btMprPenetration.h:264
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btMprSimplexSwap
void btMprSimplexSwap(btMprSimplex_t *s, size_t pos1, size_t pos2)
Definition: btMprPenetration.h:123
btMprCollisionDescription::m_firstDir
btVector3 m_firstDir
Definition: btMprPenetration.h:32
btFindPenetrTouch
static void btFindPenetrTouch(btMprSimplex_t *portal, float *depth, btVector3 *dir, btVector3 *pos)
Definition: btMprPenetration.h:774
btMprEq
int btMprEq(float _a, float _b)
Definition: btMprPenetration.h:137
btMprSimplexPoint
const btMprSupport_t * btMprSimplexPoint(const btMprSimplex_t *s, int idx)
Definition: btMprPenetration.h:107
btFindPos
static void btFindPos(const btMprSimplex_t *portal, btVector3 *pos)
Definition: btMprPenetration.h:496
btDot
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:890
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
_btMprSimplex_t
Definition: btMprPenetration.h:76
portalEncapsulesOrigin
int portalEncapsulesOrigin(const btMprSimplex_t *portal, const btVector3 *dir)
Definition: btMprPenetration.h:232
btMprPenetration
int btMprPenetration(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, float *depthOut, btVector3 *dirOut, btVector3 *posOut)
Definition: btMprPenetration.h:812
btFindOrigin
void btFindOrigin(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprSupport_t *center)
Definition: btMprPenetration.h:164
btMprVec3Dot
float btMprVec3Dot(const btVector3 *a, const btVector3 *b)
Definition: btMprPenetration.h:191
btRefinePortal
static int btRefinePortal(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprSimplex_t *portal)
Definition: btMprPenetration.h:460
btMprCollisionDescription::m_gjkRelError2
btScalar m_gjkRelError2
Definition: btMprPenetration.h:35
btMprDistanceInfo::m_pointOnB
btVector3 m_pointOnB
Definition: btMprPenetration.h:52
btAlignedObjectArray.h
btMprIsZero
int btMprIsZero(float val)
Definition: btMprPenetration.h:132
btMprVec3Set
void btMprVec3Set(btVector3 *v, float x, float y, float z)
Definition: btMprPenetration.h:171
btMprVec3Add
void btMprVec3Add(btVector3 *v, const btVector3 *w)
Definition: btMprPenetration.h:176
BT_MPR_SQRT
#define BT_MPR_SQRT
Definition: btMprPenetration.h:60
btFindPenetrSegment
static void btFindPenetrSegment(btMprSimplex_t *portal, float *depth, btVector3 *dir, btVector3 *pos)
Definition: btMprPenetration.h:792
btMprVec3Scale
void btMprVec3Scale(btVector3 *d, float k)
Definition: btMprPenetration.h:186
btMprDistanceInfo::m_normalBtoA
btVector3 m_normalBtoA
Definition: btMprPenetration.h:53
size
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition: btSoftBodyHelpers.cpp:94
_btMprSupport_t::v1
btVector3 v1
Support point in obj1.
Definition: btMprPenetration.h:71
btMprCollisionDescription::m_maxGjkIterations
int m_maxGjkIterations
Definition: btMprPenetration.h:33
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
btMprVec3Copy
void btMprVec3Copy(btVector3 *v, const btVector3 *w)
Definition: btMprPenetration.h:181