Bullet Collision Detection & Physics Library: btMprPenetration.h Source File

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"
27 #include "LinearMath/btAlignedObjectArray.h"
28
29 //#define MPR_AVERAGE_CONTACT_POSITIONS
30
31
32 struct btMprCollisionDescription
33 {
34 btVector3 m_firstDir;
35 int m_maxGjkIterations;
36 btScalar m_maximumDistanceSquared;
37 btScalar m_gjkRelError2;
38
39 btMprCollisionDescription()
40 : m_firstDir(0,1,0),
41 m_maxGjkIterations(1000),
42 m_maximumDistanceSquared(1e30f),
43 m_gjkRelError2(1.0e-6)
44 {
45 }
46 virtual ~btMprCollisionDescription()
47 {
48 }
49 };
50
51 struct btMprDistanceInfo
52 {
53 btVector3 m_pointOnA;
54 btVector3 m_pointOnB;
55 btVector3 m_normalBtoA;
56 btScalar m_distance;
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
70 struct _btMprSupport_t
71 {
72 btVector3 v;
73 btVector3 v1;
74 btVector3 v2;
75 };
76 typedef struct _btMprSupport_t btMprSupport_t;
77
78 struct _btMprSimplex_t
79 {
80 btMprSupport_t ps[4];
81 int last;
82 };
83 typedef struct _btMprSimplex_t btMprSimplex_t;
84
85 inline btMprSupport_t* btMprSimplexPointW(btMprSimplex_t *s, int idx)
86 {
87 return &s->ps[idx];
88 }
89
90 inline void btMprSimplexSetSize(btMprSimplex_t *s, int size)
91 {
92 s->last = size - 1;
93 }
94
95 #ifdef DEBUG_MPR
96 inline 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
107 inline int btMprSimplexSize(const btMprSimplex_t *s)
108 {
109 return s->last + 1;
110 }
111
112
113 inline 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
119 inline void btMprSupportCopy(btMprSupport_t *d, const btMprSupport_t *s)
120 {
121 *d = *s;
122 }
123
124 inline void btMprSimplexSet(btMprSimplex_t *s, size_t pos, const btMprSupport_t *a)
125 {
126 btMprSupportCopy(s->ps + pos, a);
127 }
128
129
130 inline 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
140 inline int btMprIsZero(float val)
141 {
142 return BT_MPR_FABS(val) < FLT_EPSILON;
143 }
144
145
146
147 inline 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
166 inline 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
183 template <typename btConvexTemplate>
184 inline 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
192 inline void btMprVec3Set(btVector3 *v, float x, float y, float z)
193 {
194 v->setValue(x,y,z);
195 }
196
197 inline void btMprVec3Add(btVector3 *v, const btVector3 *w)
198 {
199 *v += *w;
200 }
201
202 inline void btMprVec3Copy(btVector3 *v, const btVector3 *w)
203 {
204 *v = *w;
205 }
206
207 inline void btMprVec3Scale(btVector3 *d, float k)
208 {
209 *d *= k;
210 }
211
212 inline float btMprVec3Dot(const btVector3 *a, const btVector3 *b)
213 {
214 float dot;
215
216 dot = btDot(*a,*b);
217 return dot;
218 }
219
220
221 inline float btMprVec3Len2(const btVector3 *v)
222 {
223 return btMprVec3Dot(v, v);
224 }
225
226 inline void btMprVec3Normalize(btVector3 *d)
227 {
228 float k = 1.f / BT_MPR_SQRT(btMprVec3Len2(d));
229 btMprVec3Scale(d, k);
230 }
231
232 inline void btMprVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
233 {
234 *d = btCross(*a,*b);
235
236 }
237
238
239 inline void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
240 {
241 *d = *v - *w;
242 }
243
244 inline 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);
253 btMprVec3Normalize(dir);
254 }
255
256
257 inline 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
265 inline 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
289 inline int portalCanEncapsuleOrigin(const btMprSimplex_t *portal,
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
298 inline 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 }
322 template <typename btConvexTemplate>
323 inline 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
339 template <typename btConvexTemplate>
340 static 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
474 template <typename btConvexTemplate>
475 static int btRefinePortal(const btConvexTemplate& a, const btConvexTemplate& b,const btMprCollisionDescription& colDesc,
476 btMprSimplex_t *portal)
477 {
478 btVector3 dir;
479 btMprSupport_t v4;
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
513 static 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
585 inline float btMprVec3Dist2(const btVector3 *a, const btVector3 *b)
586 {
587 btVector3 ab;
588 btMprVec3Sub2(&ab, a, b);
589 return btMprVec3Len2(&ab);
590 }
591
592 inline float _btMprVec3PointSegmentDist2(const btVector3 *P,
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
651 inline 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
734 template <typename btConvexTemplate>
735 static 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;
741 btMprSupport_t v4;
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
785 static 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
804 static void btFindPenetrSegment(btMprSimplex_t *portal,
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));
821 btMprVec3Normalize(dir);
822
823
824 }
825
826
827 template <typename btConvexTemplate>
828 inline 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
886 template<typename btConvexTemplate, typename btMprDistanceTemplate>
887 inline 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
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition: btSoftBodyHelpers.cpp:84
btMprCollisionDescription::m_gjkRelError2
btScalar m_gjkRelError2
Definition: btMprPenetration.h:37
btMprVec3Dot
float btMprVec3Dot(const btVector3 *a, const btVector3 *b)
Definition: btMprPenetration.h:212
btMprVec3Set
void btMprVec3Set(btVector3 *v, float x, float y, float z)
Definition: btMprPenetration.h:192
btMprSimplexPoint
const btMprSupport_t * btMprSimplexPoint(const btMprSimplex_t *s, int idx)
Definition: btMprPenetration.h:113
btExpandPortal
void btExpandPortal(btMprSimplex_t *portal, const btMprSupport_t *v4)
Definition: btMprPenetration.h:298
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:650
btMprEq
int btMprEq(float _a, float _b)
Definition: btMprPenetration.h:147
btDiscoverPortal
static int btDiscoverPortal(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprSimplex_t *portal)
Definition: btMprPenetration.h:340
_btMprSimplex_t
Definition: btMprPenetration.h:78
btMprCollisionDescription::m_maxGjkIterations
int m_maxGjkIterations
Definition: btMprPenetration.h:35
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:735
size
static DBVT_INLINE btScalar size(const btDbvtVolume &a)
Definition: btDbvt.cpp:52
btMprDistanceInfo
Definition: btMprPenetration.h:51
btMprSimplexSetSize
void btMprSimplexSetSize(btMprSimplex_t *s, int size)
Definition: btMprPenetration.h:90
_btMprSupport_t::v
btVector3 v
Support point in minkowski sum.
Definition: btMprPenetration.h:72
btMprVec3Normalize
void btMprVec3Normalize(btVector3 *d)
Definition: btMprPenetration.h:226
btFindPenetrSegment
static void btFindPenetrSegment(btMprSimplex_t *portal, float *depth, btVector3 *dir, btVector3 *pos)
Definition: btMprPenetration.h:804
btMprVec3Eq
int btMprVec3Eq(const btVector3 *a, const btVector3 *b)
Definition: btMprPenetration.h:166
_btMprSupport_t::v1
btVector3 v1
Support point in obj1.
Definition: btMprPenetration.h:73
btTransform.h
btMprVec3Sub2
void btMprVec3Sub2(btVector3 *d, const btVector3 *v, const btVector3 *w)
Definition: btMprPenetration.h:239
btFindOrigin
void btFindOrigin(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprSupport_t *center)
Definition: btMprPenetration.h:184
btRefinePortal
static int btRefinePortal(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprSimplex_t *portal)
Definition: btMprPenetration.h:475
btMprSupport
void btMprSupport(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, const btVector3 &dir, btMprSupport_t *supp)
Definition: btMprPenetration.h:323
btMprPenetration
int btMprPenetration(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, float *depthOut, btVector3 *dirOut, btVector3 *posOut)
Definition: btMprPenetration.h:828
btMprVec3PointTriDist2
float btMprVec3PointTriDist2(const btVector3 *P, const btVector3 *x0, const btVector3 *B, const btVector3 *C, btVector3 *witness)
Definition: btMprPenetration.h:651
btMprIsZero
int btMprIsZero(float val)
Definition: btMprPenetration.h:140
portalCanEncapsuleOrigin
int portalCanEncapsuleOrigin(const btMprSimplex_t *portal, const btMprSupport_t *v4, const btVector3 *dir)
Definition: btMprPenetration.h:289
btCross
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:929
btMprVec3Scale
void btMprVec3Scale(btVector3 *d, float k)
Definition: btMprPenetration.h:207
btMprCollisionDescription
Definition: btMprPenetration.h:32
BT_MPR_MAX_ITERATIONS
#define BT_MPR_MAX_ITERATIONS
Definition: btMprPenetration.h:68
portalReachTolerance
int portalReachTolerance(const btMprSimplex_t *portal, const btMprSupport_t *v4, const btVector3 *dir)
Definition: btMprPenetration.h:265
_btMprSupport_t
Definition: btMprPenetration.h:70
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:585
btMprSupportCopy
void btMprSupportCopy(btMprSupport_t *d, const btMprSupport_t *s)
Definition: btMprPenetration.h:119
btComputeMprPenetration
int btComputeMprPenetration(const btConvexTemplate &a, const btConvexTemplate &b, const btMprCollisionDescription &colDesc, btMprDistanceTemplate *distInfo)
Definition: btMprPenetration.h:887
btFindPos
static void btFindPos(const btMprSimplex_t *portal, btVector3 *pos)
Definition: btMprPenetration.h:513
btMprDistanceInfo::m_normalBtoA
btVector3 m_normalBtoA
Definition: btMprPenetration.h:55
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:587
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:589
btMprSimplexSwap
void btMprSimplexSwap(btMprSimplex_t *s, size_t pos1, size_t pos2)
Definition: btMprPenetration.h:130
btMprVec3Copy
void btMprVec3Copy(btVector3 *v, const btVector3 *w)
Definition: btMprPenetration.h:202
_btMprSupport_t::v2
btVector3 v2
Support point in obj2.
Definition: btMprPenetration.h:74
btMprSimplexSize
int btMprSimplexSize(const btMprSimplex_t *s)
Definition: btMprPenetration.h:107
btMprVec3Dist2
float btMprVec3Dist2(const btVector3 *a, const btVector3 *b)
Definition: btMprPenetration.h:585
btMprVec3Add
void btMprVec3Add(btVector3 *v, const btVector3 *w)
Definition: btMprPenetration.h:197
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
BT_MPR_SQRT
#define BT_MPR_SQRT
Definition: btMprPenetration.h:62
_btMprVec3PointSegmentDist2
float _btMprVec3PointSegmentDist2(const btVector3 *P, const btVector3 *x0, const btVector3 *b, btVector3 *witness)
Definition: btMprPenetration.h:592
btMprDistanceInfo::m_pointOnB
btVector3 m_pointOnB
Definition: btMprPenetration.h:54
btFindPenetrTouch
static void btFindPenetrTouch(btMprSimplex_t *portal, float *depth, btVector3 *dir, btVector3 *pos)
Definition: btMprPenetration.h:785
_btMprSimplex_t::ps
btMprSupport_t ps[4]
Definition: btMprPenetration.h:80
_btMprSimplex_t::last
int last
index of last added point
Definition: btMprPenetration.h:81
btMprSimplexPointW
btMprSupport_t * btMprSimplexPointW(btMprSimplex_t *s, int idx)
Definition: btMprPenetration.h:85
btMprDistanceInfo::m_distance
btScalar m_distance
Definition: btMprPenetration.h:56
btPortalDir
void btPortalDir(const btMprSimplex_t *portal, btVector3 *dir)
Definition: btMprPenetration.h:244
btMprVec3Len2
float btMprVec3Len2(const btVector3 *v)
Definition: btMprPenetration.h:221
btAlignedObjectArray.h
btMprCollisionDescription::btMprCollisionDescription
btMprCollisionDescription()
Definition: btMprPenetration.h:39
btMprDistanceInfo::m_pointOnA
btVector3 m_pointOnA
Definition: btMprPenetration.h:53
btMprCollisionDescription::m_firstDir
btVector3 m_firstDir
Definition: btMprPenetration.h:34
dot
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:848
portalEncapsulesOrigin
int portalEncapsulesOrigin(const btMprSimplex_t *portal, const btVector3 *dir)
Definition: btMprPenetration.h:257
btMprCollisionDescription::~btMprCollisionDescription
virtual ~btMprCollisionDescription()
Definition: btMprPenetration.h:46
btDot
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:899
btMprSimplexSet
void btMprSimplexSet(btMprSimplex_t *s, size_t pos, const btMprSupport_t *a)
Definition: btMprPenetration.h:124
BT_MPR_FMIN
#define BT_MPR_FMIN(x, y)
Definition: btMprPenetration.h:64
BT_MPR_FABS
#define BT_MPR_FABS
Definition: btMprPenetration.h:65
btMprCollisionDescription::m_maximumDistanceSquared
btScalar m_maximumDistanceSquared
Definition: btMprPenetration.h:36
BT_MPR_TOLERANCE
#define BT_MPR_TOLERANCE
Definition: btMprPenetration.h:67
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:279
btMprVec3Cross
void btMprVec3Cross(btVector3 *d, const btVector3 *a, const btVector3 *b)
Definition: btMprPenetration.h:232