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