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

Bullet Collision Detection & Physics Library

btGjkEpa3.h
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2014 Erwin Coumans http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the
7 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
10 freely,
11 subject to the following restrictions:
12
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software in a
15 product, an acknowledgment in the product documentation would be appreciated
16 but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20 */
21
22 /*
23 Initial GJK-EPA collision solver by Nathanael Presson, 2008
24 Improvements and refactoring by Erwin Coumans, 2008-2014
25 */
26 #ifndef BT_GJK_EPA3_H
27 #define BT_GJK_EPA3_H
28
29 #include "LinearMath/btTransform.h"
30 #include "btGjkCollisionDescription.h"
31
32
33
34 struct btGjkEpaSolver3
35 {
36 struct sResults
37 {
38 enum eStatus
39 {
40 Separated, /* Shapes doesnt penetrate */
41 Penetrating, /* Shapes are penetrating */
42 GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */
43 EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */
44 } status;
45 btVector3 witnesses[2];
46 btVector3 normal;
47 btScalar distance;
48 };
49
50
51 };
52
53
54
55 #if defined(DEBUG) || defined (_DEBUG)
56 #include <stdio.h> //for debug printf
57 #ifdef __SPU__
58 #include <spu_printf.h>
59 #define printf spu_printf
60 #endif //__SPU__
61 #endif
62
63
64
65 // Config
66
67 /* GJK */
68 #define GJK_MAX_ITERATIONS 128
69 #define GJK_ACCURARY ((btScalar)0.0001)
70 #define GJK_MIN_DISTANCE ((btScalar)0.0001)
71 #define GJK_DUPLICATED_EPS ((btScalar)0.0001)
72 #define GJK_SIMPLEX2_EPS ((btScalar)0.0)
73 #define GJK_SIMPLEX3_EPS ((btScalar)0.0)
74 #define GJK_SIMPLEX4_EPS ((btScalar)0.0)
75
76 /* EPA */
77 #define EPA_MAX_VERTICES 64
78 #define EPA_MAX_FACES (EPA_MAX_VERTICES*2)
79 #define EPA_MAX_ITERATIONS 255
80 #define EPA_ACCURACY ((btScalar)0.0001)
81 #define EPA_FALLBACK (10*EPA_ACCURACY)
82 #define EPA_PLANE_EPS ((btScalar)0.00001)
83 #define EPA_INSIDE_EPS ((btScalar)0.01)
84
85
86 // Shorthands
87 typedef unsigned int U;
88 typedef unsigned char U1;
89
90 // MinkowskiDiff
91 template <typename btConvexTemplate>
92 struct MinkowskiDiff
93 {
94 const btConvexTemplate* m_convexAPtr;
95 const btConvexTemplate* m_convexBPtr;
96
97 btMatrix3x3 m_toshape1;
98 btTransform m_toshape0;
99
100 bool m_enableMargin;
101
102
103 MinkowskiDiff(const btConvexTemplate& a, const btConvexTemplate& b)
104 :m_convexAPtr(&a),
105 m_convexBPtr(&b)
106 {
107 }
108
109 void EnableMargin(bool enable)
110 {
111 m_enableMargin = enable;
112 }
113 inline btVector3 Support0(const btVector3& d) const
114 {
115 return m_convexAPtr->getLocalSupportWithMargin(d);
116 }
117 inline btVector3 Support1(const btVector3& d) const
118 {
119 return m_toshape0*m_convexBPtr->getLocalSupportWithMargin(m_toshape1*d);
120 }
121
122
123 inline btVector3 Support(const btVector3& d) const
124 {
125 return(Support0(d)-Support1(-d));
126 }
127 btVector3 Support(const btVector3& d,U index) const
128 {
129 if(index)
130 return(Support1(d));
131 else
132 return(Support0(d));
133 }
134 };
135
136 enum eGjkStatus
137 {
138 eGjkValid,
139 eGjkInside,
140 eGjkFailed
141 };
142
143 // GJK
144 template <typename btConvexTemplate>
145 struct GJK
146 {
147 /* Types */
148 struct sSV
149 {
150 btVector3 d,w;
151 };
152 struct sSimplex
153 {
154 sSV* c[4];
155 btScalar p[4];
156 U rank;
157 };
158
159 /* Fields */
160
161 MinkowskiDiff<btConvexTemplate> m_shape;
162 btVector3 m_ray;
163 btScalar m_distance;
164 sSimplex m_simplices[2];
165 sSV m_store[4];
166 sSV* m_free[4];
167 U m_nfree;
168 U m_current;
169 sSimplex* m_simplex;
170 eGjkStatus m_status;
171 /* Methods */
172
173 GJK(const btConvexTemplate& a, const btConvexTemplate& b)
174 :m_shape(a,b)
175 {
176 Initialize();
177 }
178 void Initialize()
179 {
180 m_ray = btVector3(0,0,0);
181 m_nfree = 0;
182 m_status = eGjkFailed;
183 m_current = 0;
184 m_distance = 0;
185 }
186 eGjkStatus Evaluate(const MinkowskiDiff<btConvexTemplate>& shapearg,const btVector3& guess)
187 {
188 U iterations=0;
189 btScalar sqdist=0;
190 btScalar alpha=0;
191 btVector3 lastw[4];
192 U clastw=0;
193 /* Initialize solver */
194 m_free[0] = &m_store[0];
195 m_free[1] = &m_store[1];
196 m_free[2] = &m_store[2];
197 m_free[3] = &m_store[3];
198 m_nfree = 4;
199 m_current = 0;
200 m_status = eGjkValid;
201 m_shape = shapearg;
202 m_distance = 0;
203 /* Initialize simplex */
204 m_simplices[0].rank = 0;
205 m_ray = guess;
206 const btScalar sqrl= m_ray.length2();
207 appendvertice(m_simplices[0],sqrl>0?-m_ray:btVector3(1,0,0));
208 m_simplices[0].p[0] = 1;
209 m_ray = m_simplices[0].c[0]->w;
210 sqdist = sqrl;
211 lastw[0] =
212 lastw[1] =
213 lastw[2] =
214 lastw[3] = m_ray;
215 /* Loop */
216 do {
217 const U next=1-m_current;
218 sSimplex& cs=m_simplices[m_current];
219 sSimplex& ns=m_simplices[next];
220 /* Check zero */
221 const btScalar rl=m_ray.length();
222 if(rl<GJK_MIN_DISTANCE)
223 {/* Touching or inside */
224 m_status=eGjkInside;
225 break;
226 }
227 /* Append new vertice in -'v' direction */
228 appendvertice(cs,-m_ray);
229 const btVector3& w=cs.c[cs.rank-1]->w;
230 bool found=false;
231 for(U i=0;i<4;++i)
232 {
233 if((w-lastw[i]).length2()<GJK_DUPLICATED_EPS)
234 { found=true;break; }
235 }
236 if(found)
237 {/* Return old simplex */
238 removevertice(m_simplices[m_current]);
239 break;
240 }
241 else
242 {/* Update lastw */
243 lastw[clastw=(clastw+1)&3]=w;
244 }
245 /* Check for termination */
246 const btScalar omega=btDot(m_ray,w)/rl;
247 alpha=btMax(omega,alpha);
248 if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
249 {/* Return old simplex */
250 removevertice(m_simplices[m_current]);
251 break;
252 }
253 /* Reduce simplex */
254 btScalar weights[4];
255 U mask=0;
256 switch(cs.rank)
257 {
258 case 2: sqdist=projectorigin( cs.c[0]->w,
259 cs.c[1]->w,
260 weights,mask);break;
261 case 3: sqdist=projectorigin( cs.c[0]->w,
262 cs.c[1]->w,
263 cs.c[2]->w,
264 weights,mask);break;
265 case 4: sqdist=projectorigin( cs.c[0]->w,
266 cs.c[1]->w,
267 cs.c[2]->w,
268 cs.c[3]->w,
269 weights,mask);break;
270 }
271 if(sqdist>=0)
272 {/* Valid */
273 ns.rank = 0;
274 m_ray = btVector3(0,0,0);
275 m_current = next;
276 for(U i=0,ni=cs.rank;i<ni;++i)
277 {
278 if(mask&(1<<i))
279 {
280 ns.c[ns.rank] = cs.c[i];
281 ns.p[ns.rank++] = weights[i];
282 m_ray += cs.c[i]->w*weights[i];
283 }
284 else
285 {
286 m_free[m_nfree++] = cs.c[i];
287 }
288 }
289 if(mask==15) m_status=eGjkInside;
290 }
291 else
292 {/* Return old simplex */
293 removevertice(m_simplices[m_current]);
294 break;
295 }
296 m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eGjkFailed;
297 } while(m_status==eGjkValid);
298 m_simplex=&m_simplices[m_current];
299 switch(m_status)
300 {
301 case eGjkValid: m_distance=m_ray.length();break;
302 case eGjkInside: m_distance=0;break;
303 default:
304 {
305 }
306 }
307 return(m_status);
308 }
309 bool EncloseOrigin()
310 {
311 switch(m_simplex->rank)
312 {
313 case 1:
314 {
315 for(U i=0;i<3;++i)
316 {
317 btVector3 axis=btVector3(0,0,0);
318 axis[i]=1;
319 appendvertice(*m_simplex, axis);
320 if(EncloseOrigin()) return(true);
321 removevertice(*m_simplex);
322 appendvertice(*m_simplex,-axis);
323 if(EncloseOrigin()) return(true);
324 removevertice(*m_simplex);
325 }
326 }
327 break;
328 case 2:
329 {
330 const btVector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w;
331 for(U i=0;i<3;++i)
332 {
333 btVector3 axis=btVector3(0,0,0);
334 axis[i]=1;
335 const btVector3 p=btCross(d,axis);
336 if(p.length2()>0)
337 {
338 appendvertice(*m_simplex, p);
339 if(EncloseOrigin()) return(true);
340 removevertice(*m_simplex);
341 appendvertice(*m_simplex,-p);
342 if(EncloseOrigin()) return(true);
343 removevertice(*m_simplex);
344 }
345 }
346 }
347 break;
348 case 3:
349 {
350 const btVector3 n=btCross(m_simplex->c[1]->w-m_simplex->c[0]->w,
351 m_simplex->c[2]->w-m_simplex->c[0]->w);
352 if(n.length2()>0)
353 {
354 appendvertice(*m_simplex,n);
355 if(EncloseOrigin()) return(true);
356 removevertice(*m_simplex);
357 appendvertice(*m_simplex,-n);
358 if(EncloseOrigin()) return(true);
359 removevertice(*m_simplex);
360 }
361 }
362 break;
363 case 4:
364 {
365 if(btFabs(det( m_simplex->c[0]->w-m_simplex->c[3]->w,
366 m_simplex->c[1]->w-m_simplex->c[3]->w,
367 m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
368 return(true);
369 }
370 break;
371 }
372 return(false);
373 }
374 /* Internals */
375 void getsupport(const btVector3& d,sSV& sv) const
376 {
377 sv.d = d/d.length();
378 sv.w = m_shape.Support(sv.d);
379 }
380 void removevertice(sSimplex& simplex)
381 {
382 m_free[m_nfree++]=simplex.c[--simplex.rank];
383 }
384 void appendvertice(sSimplex& simplex,const btVector3& v)
385 {
386 simplex.p[simplex.rank]=0;
387 simplex.c[simplex.rank]=m_free[--m_nfree];
388 getsupport(v,*simplex.c[simplex.rank++]);
389 }
390 static btScalar det(const btVector3& a,const btVector3& b,const btVector3& c)
391 {
392 return( a.y()*b.z()*c.x()+a.z()*b.x()*c.y()-
393 a.x()*b.z()*c.y()-a.y()*b.x()*c.z()+
394 a.x()*b.y()*c.z()-a.z()*b.y()*c.x());
395 }
396 static btScalar projectorigin( const btVector3& a,
397 const btVector3& b,
398 btScalar* w,U& m)
399 {
400 const btVector3 d=b-a;
401 const btScalar l=d.length2();
402 if(l>GJK_SIMPLEX2_EPS)
403 {
404 const btScalar t(l>0?-btDot(a,d)/l:0);
405 if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length2()); }
406 else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length2()); }
407 else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length2()); }
408 }
409 return(-1);
410 }
411 static btScalar projectorigin( const btVector3& a,
412 const btVector3& b,
413 const btVector3& c,
414 btScalar* w,U& m)
415 {
416 static const U imd3[]={1,2,0};
417 const btVector3* vt[]={&a,&b,&c};
418 const btVector3 dl[]={a-b,b-c,c-a};
419 const btVector3 n=btCross(dl[0],dl[1]);
420 const btScalar l=n.length2();
421 if(l>GJK_SIMPLEX3_EPS)
422 {
423 btScalar mindist=-1;
424 btScalar subw[2]={0.f,0.f};
425 U subm(0);
426 for(U i=0;i<3;++i)
427 {
428 if(btDot(*vt[i],btCross(dl[i],n))>0)
429 {
430 const U j=imd3[i];
431 const btScalar subd(projectorigin(*vt[i],*vt[j],subw,subm));
432 if((mindist<0)||(subd<mindist))
433 {
434 mindist = subd;
435 m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
436 w[i] = subw[0];
437 w[j] = subw[1];
438 w[imd3[j]] = 0;
439 }
440 }
441 }
442 if(mindist<0)
443 {
444 const btScalar d=btDot(a,n);
445 const btScalar s=btSqrt(l);
446 const btVector3 p=n*(d/l);
447 mindist = p.length2();
448 m = 7;
449 w[0] = (btCross(dl[1],b-p)).length()/s;
450 w[1] = (btCross(dl[2],c-p)).length()/s;
451 w[2] = 1-(w[0]+w[1]);
452 }
453 return(mindist);
454 }
455 return(-1);
456 }
457 static btScalar projectorigin( const btVector3& a,
458 const btVector3& b,
459 const btVector3& c,
460 const btVector3& d,
461 btScalar* w,U& m)
462 {
463 static const U imd3[]={1,2,0};
464 const btVector3* vt[]={&a,&b,&c,&d};
465 const btVector3 dl[]={a-d,b-d,c-d};
466 const btScalar vl=det(dl[0],dl[1],dl[2]);
467 const bool ng=(vl*btDot(a,btCross(b-c,a-b)))<=0;
468 if(ng&&(btFabs(vl)>GJK_SIMPLEX4_EPS))
469 {
470 btScalar mindist=-1;
471 btScalar subw[3]={0.f,0.f,0.f};
472 U subm(0);
473 for(U i=0;i<3;++i)
474 {
475 const U j=imd3[i];
476 const btScalar s=vl*btDot(d,btCross(dl[i],dl[j]));
477 if(s>0)
478 {
479 const btScalar subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
480 if((mindist<0)||(subd<mindist))
481 {
482 mindist = subd;
483 m = static_cast<U>((subm&1?1<<i:0)+
484 (subm&2?1<<j:0)+
485 (subm&4?8:0));
486 w[i] = subw[0];
487 w[j] = subw[1];
488 w[imd3[j]] = 0;
489 w[3] = subw[2];
490 }
491 }
492 }
493 if(mindist<0)
494 {
495 mindist = 0;
496 m = 15;
497 w[0] = det(c,b,d)/vl;
498 w[1] = det(a,c,d)/vl;
499 w[2] = det(b,a,d)/vl;
500 w[3] = 1-(w[0]+w[1]+w[2]);
501 }
502 return(mindist);
503 }
504 return(-1);
505 }
506 };
507
508
509 enum eEpaStatus
510 {
511 eEpaValid,
512 eEpaTouching,
513 eEpaDegenerated,
514 eEpaNonConvex,
515 eEpaInvalidHull,
516 eEpaOutOfFaces,
517 eEpaOutOfVertices,
518 eEpaAccuraryReached,
519 eEpaFallBack,
520 eEpaFailed
521 };
522
523
524 // EPA
525 template <typename btConvexTemplate>
526 struct EPA
527 {
528 /* Types */
529
530 struct sFace
531 {
532 btVector3 n;
533 btScalar d;
534 typename GJK<btConvexTemplate>::sSV* c[3];
535 sFace* f[3];
536 sFace* l[2];
537 U1 e[3];
538 U1 pass;
539 };
540 struct sList
541 {
542 sFace* root;
543 U count;
544 sList() : root(0),count(0) {}
545 };
546 struct sHorizon
547 {
548 sFace* cf;
549 sFace* ff;
550 U nf;
551 sHorizon() : cf(0),ff(0),nf(0) {}
552 };
553
554 /* Fields */
555 eEpaStatus m_status;
556 typename GJK<btConvexTemplate>::sSimplex m_result;
557 btVector3 m_normal;
558 btScalar m_depth;
559 typename GJK<btConvexTemplate>::sSV m_sv_store[EPA_MAX_VERTICES];
560 sFace m_fc_store[EPA_MAX_FACES];
561 U m_nextsv;
562 sList m_hull;
563 sList m_stock;
564 /* Methods */
565 EPA()
566 {
567 Initialize();
568 }
569
570
571 static inline void bind(sFace* fa,U ea,sFace* fb,U eb)
572 {
573 fa->e[ea]=(U1)eb;fa->f[ea]=fb;
574 fb->e[eb]=(U1)ea;fb->f[eb]=fa;
575 }
576 static inline void append(sList& list,sFace* face)
577 {
578 face->l[0] = 0;
579 face->l[1] = list.root;
580 if(list.root) list.root->l[0]=face;
581 list.root = face;
582 ++list.count;
583 }
584 static inline void remove(sList& list,sFace* face)
585 {
586 if(face->l[1]) face->l[1]->l[0]=face->l[0];
587 if(face->l[0]) face->l[0]->l[1]=face->l[1];
588 if(face==list.root) list.root=face->l[1];
589 --list.count;
590 }
591
592
593 void Initialize()
594 {
595 m_status = eEpaFailed;
596 m_normal = btVector3(0,0,0);
597 m_depth = 0;
598 m_nextsv = 0;
599 for(U i=0;i<EPA_MAX_FACES;++i)
600 {
601 append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
602 }
603 }
604 eEpaStatus Evaluate(GJK<btConvexTemplate>& gjk,const btVector3& guess)
605 {
606 typename GJK<btConvexTemplate>::sSimplex& simplex=*gjk.m_simplex;
607 if((simplex.rank>1)&&gjk.EncloseOrigin())
608 {
609
610 /* Clean up */
611 while(m_hull.root)
612 {
613 sFace* f = m_hull.root;
614 remove(m_hull,f);
615 append(m_stock,f);
616 }
617 m_status = eEpaValid;
618 m_nextsv = 0;
619 /* Orient simplex */
620 if(gjk.det( simplex.c[0]->w-simplex.c[3]->w,
621 simplex.c[1]->w-simplex.c[3]->w,
622 simplex.c[2]->w-simplex.c[3]->w)<0)
623 {
624 btSwap(simplex.c[0],simplex.c[1]);
625 btSwap(simplex.p[0],simplex.p[1]);
626 }
627 /* Build initial hull */
628 sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
629 newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
630 newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
631 newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
632 if(m_hull.count==4)
633 {
634 sFace* best=findbest();
635 sFace outer=*best;
636 U pass=0;
637 U iterations=0;
638 bind(tetra[0],0,tetra[1],0);
639 bind(tetra[0],1,tetra[2],0);
640 bind(tetra[0],2,tetra[3],0);
641 bind(tetra[1],1,tetra[3],2);
642 bind(tetra[1],2,tetra[2],1);
643 bind(tetra[2],2,tetra[3],1);
644 m_status=eEpaValid;
645 for(;iterations<EPA_MAX_ITERATIONS;++iterations)
646 {
647 if(m_nextsv<EPA_MAX_VERTICES)
648 {
649 sHorizon horizon;
650 typename GJK<btConvexTemplate>::sSV* w=&m_sv_store[m_nextsv++];
651 bool valid=true;
652 best->pass = (U1)(++pass);
653 gjk.getsupport(best->n,*w);
654 const btScalar wdist=btDot(best->n,w->w)-best->d;
655 if(wdist>EPA_ACCURACY)
656 {
657 for(U j=0;(j<3)&&valid;++j)
658 {
659 valid&=expand( pass,w,
660 best->f[j],best->e[j],
661 horizon);
662 }
663 if(valid&&(horizon.nf>=3))
664 {
665 bind(horizon.cf,1,horizon.ff,2);
666 remove(m_hull,best);
667 append(m_stock,best);
668 best=findbest();
669 outer=*best;
670 } else { m_status=eEpaInvalidHull;break; }
671 } else { m_status=eEpaAccuraryReached;break; }
672 } else { m_status=eEpaOutOfVertices;break; }
673 }
674 const btVector3 projection=outer.n*outer.d;
675 m_normal = outer.n;
676 m_depth = outer.d;
677 m_result.rank = 3;
678 m_result.c[0] = outer.c[0];
679 m_result.c[1] = outer.c[1];
680 m_result.c[2] = outer.c[2];
681 m_result.p[0] = btCross( outer.c[1]->w-projection,
682 outer.c[2]->w-projection).length();
683 m_result.p[1] = btCross( outer.c[2]->w-projection,
684 outer.c[0]->w-projection).length();
685 m_result.p[2] = btCross( outer.c[0]->w-projection,
686 outer.c[1]->w-projection).length();
687 const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
688 m_result.p[0] /= sum;
689 m_result.p[1] /= sum;
690 m_result.p[2] /= sum;
691 return(m_status);
692 }
693 }
694 /* Fallback */
695 m_status = eEpaFallBack;
696 m_normal = -guess;
697 const btScalar nl=m_normal.length();
698 if(nl>0)
699 m_normal = m_normal/nl;
700 else
701 m_normal = btVector3(1,0,0);
702 m_depth = 0;
703 m_result.rank=1;
704 m_result.c[0]=simplex.c[0];
705 m_result.p[0]=1;
706 return(m_status);
707 }
708 bool getedgedist(sFace* face, typename GJK<btConvexTemplate>::sSV* a, typename GJK<btConvexTemplate>::sSV* b, btScalar& dist)
709 {
710 const btVector3 ba = b->w - a->w;
711 const btVector3 n_ab = btCross(ba, face->n); // Outward facing edge normal direction, on triangle plane
712 const btScalar a_dot_nab = btDot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required
713
714 if(a_dot_nab < 0)
715 {
716 // Outside of edge a->b
717
718 const btScalar ba_l2 = ba.length2();
719 const btScalar a_dot_ba = btDot(a->w, ba);
720 const btScalar b_dot_ba = btDot(b->w, ba);
721
722 if(a_dot_ba > 0)
723 {
724 // Pick distance vertex a
725 dist = a->w.length();
726 }
727 else if(b_dot_ba < 0)
728 {
729 // Pick distance vertex b
730 dist = b->w.length();
731 }
732 else
733 {
734 // Pick distance to edge a->b
735 const btScalar a_dot_b = btDot(a->w, b->w);
736 dist = btSqrt(btMax((a->w.length2() * b->w.length2() - a_dot_b * a_dot_b) / ba_l2, (btScalar)0));
737 }
738
739 return true;
740 }
741
742 return false;
743 }
744 sFace* newface(typename GJK<btConvexTemplate>::sSV* a,typename GJK<btConvexTemplate>::sSV* b,typename GJK<btConvexTemplate>::sSV* c,bool forced)
745 {
746 if(m_stock.root)
747 {
748 sFace* face=m_stock.root;
749 remove(m_stock,face);
750 append(m_hull,face);
751 face->pass = 0;
752 face->c[0] = a;
753 face->c[1] = b;
754 face->c[2] = c;
755 face->n = btCross(b->w-a->w,c->w-a->w);
756 const btScalar l=face->n.length();
757 const bool v=l>EPA_ACCURACY;
758
759 if(v)
760 {
761 if(!(getedgedist(face, a, b, face->d) ||
762 getedgedist(face, b, c, face->d) ||
763 getedgedist(face, c, a, face->d)))
764 {
765 // Origin projects to the interior of the triangle
766 // Use distance to triangle plane
767 face->d = btDot(a->w, face->n) / l;
768 }
769
770 face->n /= l;
771 if(forced || (face->d >= -EPA_PLANE_EPS))
772 {
773 return face;
774 }
775 else
776 m_status=eEpaNonConvex;
777 }
778 else
779 m_status=eEpaDegenerated;
780
781 remove(m_hull, face);
782 append(m_stock, face);
783 return 0;
784
785 }
786 m_status = m_stock.root ? eEpaOutOfVertices : eEpaOutOfFaces;
787 return 0;
788 }
789 sFace* findbest()
790 {
791 sFace* minf=m_hull.root;
792 btScalar mind=minf->d*minf->d;
793 for(sFace* f=minf->l[1];f;f=f->l[1])
794 {
795 const btScalar sqd=f->d*f->d;
796 if(sqd<mind)
797 {
798 minf=f;
799 mind=sqd;
800 }
801 }
802 return(minf);
803 }
804 bool expand(U pass,typename GJK<btConvexTemplate>::sSV* w,sFace* f,U e,sHorizon& horizon)
805 {
806 static const U i1m3[]={1,2,0};
807 static const U i2m3[]={2,0,1};
808 if(f->pass!=pass)
809 {
810 const U e1=i1m3[e];
811 if((btDot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
812 {
813 sFace* nf=newface(f->c[e1],f->c[e],w,false);
814 if(nf)
815 {
816 bind(nf,0,f,e);
817 if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
818 horizon.cf=nf;
819 ++horizon.nf;
820 return(true);
821 }
822 }
823 else
824 {
825 const U e2=i2m3[e];
826 f->pass = (U1)pass;
827 if( expand(pass,w,f->f[e1],f->e[e1],horizon)&&
828 expand(pass,w,f->f[e2],f->e[e2],horizon))
829 {
830 remove(m_hull,f);
831 append(m_stock,f);
832 return(true);
833 }
834 }
835 }
836 return(false);
837 }
838
839 };
840
841 template <typename btConvexTemplate>
842 static void Initialize( const btConvexTemplate& a, const btConvexTemplate& b,
843 btGjkEpaSolver3::sResults& results,
844 MinkowskiDiff<btConvexTemplate>& shape)
845 {
846 /* Results */
847 results.witnesses[0] =
848 results.witnesses[1] = btVector3(0,0,0);
849 results.status = btGjkEpaSolver3::sResults::Separated;
850 /* Shape */
851
852 shape.m_toshape1 = b.getWorldTransform().getBasis().transposeTimes(a.getWorldTransform().getBasis());
853 shape.m_toshape0 = a.getWorldTransform().inverseTimes(b.getWorldTransform());
854
855 }
856
857
858 //
859 // Api
860 //
861
862
863
864 //
865 template <typename btConvexTemplate>
866 bool btGjkEpaSolver3_Distance(const btConvexTemplate& a, const btConvexTemplate& b,
867 const btVector3& guess,
868 btGjkEpaSolver3::sResults& results)
869 {
870 MinkowskiDiff<btConvexTemplate> shape(a,b);
871 Initialize(a,b,results,shape);
872 GJK<btConvexTemplate> gjk(a,b);
873 eGjkStatus gjk_status=gjk.Evaluate(shape,guess);
874 if(gjk_status==eGjkValid)
875 {
876 btVector3 w0=btVector3(0,0,0);
877 btVector3 w1=btVector3(0,0,0);
878 for(U i=0;i<gjk.m_simplex->rank;++i)
879 {
880 const btScalar p=gjk.m_simplex->p[i];
881 w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
882 w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
883 }
884 results.witnesses[0] = a.getWorldTransform()*w0;
885 results.witnesses[1] = a.getWorldTransform()*w1;
886 results.normal = w0-w1;
887 results.distance = results.normal.length();
888 results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1;
889 return(true);
890 }
891 else
892 {
893 results.status = gjk_status==eGjkInside?
894 btGjkEpaSolver3::sResults::Penetrating :
895 btGjkEpaSolver3::sResults::GJK_Failed ;
896 return(false);
897 }
898 }
899
900
901 template <typename btConvexTemplate>
902 bool btGjkEpaSolver3_Penetration(const btConvexTemplate& a,
903 const btConvexTemplate& b,
904 const btVector3& guess,
905 btGjkEpaSolver3::sResults& results)
906 {
907 MinkowskiDiff<btConvexTemplate> shape(a,b);
908 Initialize(a,b,results,shape);
909 GJK<btConvexTemplate> gjk(a,b);
910 eGjkStatus gjk_status=gjk.Evaluate(shape,-guess);
911 switch(gjk_status)
912 {
913 case eGjkInside:
914 {
915 EPA<btConvexTemplate> epa;
916 eEpaStatus epa_status=epa.Evaluate(gjk,-guess);
917 if(epa_status!=eEpaFailed)
918 {
919 btVector3 w0=btVector3(0,0,0);
920 for(U i=0;i<epa.m_result.rank;++i)
921 {
922 w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
923 }
924 results.status = btGjkEpaSolver3::sResults::Penetrating;
925 results.witnesses[0] = a.getWorldTransform()*w0;
926 results.witnesses[1] = a.getWorldTransform()*(w0-epa.m_normal*epa.m_depth);
927 results.normal = -epa.m_normal;
928 results.distance = -epa.m_depth;
929 return(true);
930 } else results.status=btGjkEpaSolver3::sResults::EPA_Failed;
931 }
932 break;
933 case eGjkFailed:
934 results.status=btGjkEpaSolver3::sResults::GJK_Failed;
935 break;
936 default:
937 {
938 }
939 }
940 return(false);
941 }
942
943 #if 0
944 int btComputeGjkEpaPenetration2(const btCollisionDescription& colDesc, btDistanceInfo* distInfo)
945 {
946 btGjkEpaSolver3::sResults results;
947 btVector3 guess = colDesc.m_firstDir;
948
949 bool res = btGjkEpaSolver3::Penetration(colDesc.m_objA,colDesc.m_objB,
950 colDesc.m_transformA,colDesc.m_transformB,
951 colDesc.m_localSupportFuncA,colDesc.m_localSupportFuncB,
952 guess,
953 results);
954 if (res)
955 {
956 if ((results.status==btGjkEpaSolver3::sResults::Penetrating) || results.status==GJK::eStatus::Inside)
957 {
958 //normal could be 'swapped'
959
960 distInfo->m_distance = results.distance;
961 distInfo->m_normalBtoA = results.normal;
962 btVector3 tmpNormalInB = results.witnesses[1]-results.witnesses[0];
963 btScalar lenSqr = tmpNormalInB.length2();
964 if (lenSqr <= (SIMD_EPSILON*SIMD_EPSILON))
965 {
966 tmpNormalInB = results.normal;
967 lenSqr = results.normal.length2();
968 }
969
970 if (lenSqr > (SIMD_EPSILON*SIMD_EPSILON))
971 {
972 tmpNormalInB /= btSqrt(lenSqr);
973 btScalar distance2 = -(results.witnesses[0]-results.witnesses[1]).length();
974 //only replace valid penetrations when the result is deeper (check)
975 //if ((distance2 < results.distance))
976 {
977 distInfo->m_distance = distance2;
978 distInfo->m_pointOnA= results.witnesses[0];
979 distInfo->m_pointOnB= results.witnesses[1];
980 distInfo->m_normalBtoA= tmpNormalInB;
981 return 0;
982 }
983 }
984 }
985
986 }
987
988 return -1;
989 }
990 #endif
991
992 template <typename btConvexTemplate, typename btDistanceInfoTemplate>
993 int btComputeGjkDistance(const btConvexTemplate& a, const btConvexTemplate& b,
994 const btGjkCollisionDescription& colDesc, btDistanceInfoTemplate* distInfo)
995 {
996 btGjkEpaSolver3::sResults results;
997 btVector3 guess = colDesc.m_firstDir;
998
999 bool isSeparated = btGjkEpaSolver3_Distance( a,b,
1000 guess,
1001 results);
1002 if (isSeparated)
1003 {
1004 distInfo->m_distance = results.distance;
1005 distInfo->m_pointOnA= results.witnesses[0];
1006 distInfo->m_pointOnB= results.witnesses[1];
1007 distInfo->m_normalBtoA= results.normal;
1008 return 0;
1009 }
1010
1011 return -1;
1012 }
1013
1014 /* Symbols cleanup */
1015
1016 #undef GJK_MAX_ITERATIONS
1017 #undef GJK_ACCURARY
1018 #undef GJK_MIN_DISTANCE
1019 #undef GJK_DUPLICATED_EPS
1020 #undef GJK_SIMPLEX2_EPS
1021 #undef GJK_SIMPLEX3_EPS
1022 #undef GJK_SIMPLEX4_EPS
1023
1024 #undef EPA_MAX_VERTICES
1025 #undef EPA_MAX_FACES
1026 #undef EPA_MAX_ITERATIONS
1027 #undef EPA_ACCURACY
1028 #undef EPA_FALLBACK
1029 #undef EPA_PLANE_EPS
1030 #undef EPA_INSIDE_EPS
1031
1032
1033
1034 #endif //BT_GJK_EPA3_H
1035
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition: btSoftBodyHelpers.cpp:84
eEpaTouching
Definition: btGjkEpa3.h:512
btGjkEpaSolver3_Penetration
bool btGjkEpaSolver3_Penetration(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
Definition: btGjkEpa3.h:902
GJK_SIMPLEX4_EPS
#define GJK_SIMPLEX4_EPS
Definition: btGjkEpa3.h:74
EPA::findbest
sFace * findbest()
Definition: btGjkEpa3.h:789
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:521
GJK
Definition: btGjkEpa3.h:145
EPA::m_hull
sList m_hull
Definition: btGjkEpa3.h:562
GJK::EncloseOrigin
bool EncloseOrigin()
Definition: btGjkEpa3.h:309
length
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:886
MinkowskiDiff::Support
btVector3 Support(const btVector3 &d) const
Definition: btGjkEpa3.h:123
EPA::sFace::e
U1 e[3]
Definition: btGjkEpa3.h:537
EPA::m_status
eEpaStatus m_status
Definition: btGjkEpa3.h:555
EPA::Evaluate
eEpaStatus Evaluate(GJK< btConvexTemplate > &gjk, const btVector3 &guess)
Definition: btGjkEpa3.h:604
EPA::m_nextsv
U m_nextsv
Definition: btGjkEpa3.h:561
btComputeGjkDistance
int btComputeGjkDistance(const btConvexTemplate &a, const btConvexTemplate &b, const btGjkCollisionDescription &colDesc, btDistanceInfoTemplate *distInfo)
Definition: btGjkEpa3.h:993
GJK::sSimplex::rank
U rank
Definition: btGjkEpa3.h:156
GJK_MAX_ITERATIONS
#define GJK_MAX_ITERATIONS
Definition: btGjkEpa3.h:68
btGjkEpaSolver3
Definition: btGjkEpa3.h:34
eEpaDegenerated
Definition: btGjkEpa3.h:513
btGjkEpaSolver3::sResults::Separated
Definition: btGjkEpa3.h:40
EPA_MAX_VERTICES
#define EPA_MAX_VERTICES
Definition: btGjkEpa3.h:77
EPA::sHorizon
Definition: btGjkEpa3.h:546
GJK_SIMPLEX2_EPS
#define GJK_SIMPLEX2_EPS
Definition: btGjkEpa3.h:72
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
EPA::sList::root
sFace * root
Definition: btGjkEpa3.h:542
MinkowskiDiff::m_convexBPtr
const btConvexTemplate * m_convexBPtr
Definition: btGjkEpa3.h:95
GJK::m_status
eGjkStatus m_status
Definition: btGjkEpa3.h:170
btGjkCollisionDescription::m_firstDir
btVector3 m_firstDir
Definition: btGjkCollisionDescription.h:24
EPA::sFace::d
btScalar d
Definition: btGjkEpa3.h:533
btSqrt
btScalar btSqrt(btScalar y)
Definition: btScalar.h:444
MinkowskiDiff::m_convexAPtr
const btConvexTemplate * m_convexAPtr
Definition: btGjkEpa3.h:94
EPA::sHorizon::ff
sFace * ff
Definition: btGjkEpa3.h:549
EPA::sFace::n
btVector3 n
Definition: btGjkEpa3.h:532
eEpaNonConvex
Definition: btGjkEpa3.h:514
btTransform.h
GJK::m_distance
btScalar m_distance
Definition: btGjkEpa3.h:163
EPA::append
static void append(sList &list, sFace *face)
Definition: btGjkEpa3.h:576
MinkowskiDiff::m_toshape1
btMatrix3x3 m_toshape1
Definition: btGjkEpa3.h:97
GJK_SIMPLEX3_EPS
#define GJK_SIMPLEX3_EPS
Definition: btGjkEpa3.h:73
eEpaAccuraryReached
Definition: btGjkEpa3.h:518
btGjkEpaSolver3::sResults::witnesses
btVector3 witnesses[2]
Definition: btGjkEpa3.h:45
EPA::sFace::l
sFace * l[2]
Definition: btGjkEpa3.h:536
GJK::projectorigin
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, btScalar *w, U &m)
Definition: btGjkEpa3.h:396
btGjkCollisionDescription
Definition: btGjkCollisionDescription.h:22
eEpaOutOfFaces
Definition: btGjkEpa3.h:516
btGjkEpaSolver3::sResults::EPA_Failed
Definition: btGjkEpa3.h:43
GJK::m_ray
btVector3 m_ray
Definition: btGjkEpa3.h:162
btCross
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:931
btGjkEpaSolver3::sResults::distance
btScalar distance
Definition: btGjkEpa3.h:47
btGjkEpaSolver3::sResults
Definition: btGjkEpa3.h:36
eEpaInvalidHull
Definition: btGjkEpa3.h:515
GJK::sSimplex::c
sSV * c[4]
Definition: btGjkEpa3.h:154
Initialize
static void Initialize(const btConvexTemplate &a, const btConvexTemplate &b, btGjkEpaSolver3::sResults &results, MinkowskiDiff< btConvexTemplate > &shape)
Definition: btGjkEpa3.h:842
U1
unsigned char U1
Definition: btGjkEpa3.h:88
GJK_DUPLICATED_EPS
#define GJK_DUPLICATED_EPS
Definition: btGjkEpa3.h:71
btMatrix3x3::transposeTimes
btMatrix3x3 transposeTimes(const btMatrix3x3 &m) const
Definition: btMatrix3x3.h:1016
GJK::m_simplex
sSimplex * m_simplex
Definition: btGjkEpa3.h:169
MinkowskiDiff::EnableMargin
void EnableMargin(bool enable)
Definition: btGjkEpa3.h:109
GJK::projectorigin
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, btScalar *w, U &m)
Definition: btGjkEpa3.h:411
EPA::sHorizon::sHorizon
sHorizon()
Definition: btGjkEpa3.h:551
GJK::Evaluate
eGjkStatus Evaluate(const MinkowskiDiff< btConvexTemplate > &shapearg, const btVector3 &guess)
Definition: btGjkEpa3.h:186
EPA::sHorizon::cf
sFace * cf
Definition: btGjkEpa3.h:548
MinkowskiDiff::m_enableMargin
bool m_enableMargin
Definition: btGjkEpa3.h:100
btVector3::x
const btScalar & x() const
Return the x value.
Definition: btVector3.h:587
EPA_ACCURACY
#define EPA_ACCURACY
Definition: btGjkEpa3.h:80
EPA::Initialize
void Initialize()
Definition: btGjkEpa3.h:593
btGjkEpaSolver3::sResults::status
enum btGjkEpaSolver3::sResults::eStatus status
EPA::m_stock
sList m_stock
Definition: btGjkEpa3.h:563
btSwap
void btSwap(T &a, T &b)
Definition: btScalar.h:621
GJK::sSV::w
btVector3 w
Definition: btGjkEpa3.h:150
EPA::sList
Definition: btGjkEpa3.h:540
btTransform::inverseTimes
btTransform inverseTimes(const btTransform &t) const
Return the inverse of this transform times the other transform.
Definition: btTransform.h:230
btGjkEpaSolver3::sResults::Penetrating
Definition: btGjkEpa3.h:41
MinkowskiDiff
Definition: btGjkEpa3.h:92
EPA::sList::count
U count
Definition: btGjkEpa3.h:543
MinkowskiDiff::MinkowskiDiff
MinkowskiDiff(const btConvexTemplate &a, const btConvexTemplate &b)
Definition: btGjkEpa3.h:103
GJK::projectorigin
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &d, btScalar *w, U &m)
Definition: btGjkEpa3.h:457
btVector3::y
const btScalar & y() const
Return the y value.
Definition: btVector3.h:589
btVector3::z
const btScalar & z() const
Return the z value.
Definition: btVector3.h:591
EPA::sFace
Definition: btGjkEpa3.h:530
MinkowskiDiff::Support1
btVector3 Support1(const btVector3 &d) const
Definition: btGjkEpa3.h:117
EPA::expand
bool expand(U pass, typename GJK< btConvexTemplate >::sSV *w, sFace *f, U e, sHorizon &horizon)
Definition: btGjkEpa3.h:804
btGjkEpaSolver3::sResults::eStatus
eStatus
Definition: btGjkEpa3.h:38
EPA::sFace::c
GJK< btConvexTemplate >::sSV * c[3]
Definition: btGjkEpa3.h:534
EPA::sHorizon::nf
U nf
Definition: btGjkEpa3.h:550
EPA::sList::sList
sList()
Definition: btGjkEpa3.h:544
GJK::sSimplex
Definition: btGjkEpa3.h:152
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
GJK::m_shape
MinkowskiDiff< btConvexTemplate > m_shape
Definition: btGjkEpa3.h:161
EPA::sFace::pass
U1 pass
Definition: btGjkEpa3.h:538
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
MinkowskiDiff::m_toshape0
btTransform m_toshape0
Definition: btGjkEpa3.h:98
EPA::sFace::f
sFace * f[3]
Definition: btGjkEpa3.h:535
MinkowskiDiff::Support
btVector3 Support(const btVector3 &d, U index) const
Definition: btGjkEpa3.h:127
GJK::det
static btScalar det(const btVector3 &a, const btVector3 &b, const btVector3 &c)
Definition: btGjkEpa3.h:390
EPA::newface
sFace * newface(typename GJK< btConvexTemplate >::sSV *a, typename GJK< btConvexTemplate >::sSV *b, typename GJK< btConvexTemplate >::sSV *c, bool forced)
Definition: btGjkEpa3.h:744
EPA::m_normal
btVector3 m_normal
Definition: btGjkEpa3.h:557
btGjkEpaSolver3_Distance
bool btGjkEpaSolver3_Distance(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
Definition: btGjkEpa3.h:866
eGjkInside
Definition: btGjkEpa3.h:139
btGjkCollisionDescription.h
EPA::bind
static void bind(sFace *fa, U ea, sFace *fb, U eb)
Definition: btGjkEpa3.h:571
MinkowskiDiff::Support0
btVector3 Support0(const btVector3 &d) const
Definition: btGjkEpa3.h:113
GJK::getsupport
void getsupport(const btVector3 &d, sSV &sv) const
Definition: btGjkEpa3.h:375
eEpaValid
Definition: btGjkEpa3.h:511
GJK::appendvertice
void appendvertice(sSimplex &simplex, const btVector3 &v)
Definition: btGjkEpa3.h:384
eEpaFailed
Definition: btGjkEpa3.h:520
GJK::removevertice
void removevertice(sSimplex &simplex)
Definition: btGjkEpa3.h:380
eGjkFailed
Definition: btGjkEpa3.h:140
GJK_MIN_DISTANCE
#define GJK_MIN_DISTANCE
Definition: btGjkEpa3.h:70
GJK::sSV::d
btVector3 d
Definition: btGjkEpa3.h:150
btGjkEpaSolver3::sResults::GJK_Failed
Definition: btGjkEpa3.h:42
btMax
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:29
GJK_ACCURARY
#define GJK_ACCURARY
Definition: btGjkEpa3.h:69
EPA::m_result
GJK< btConvexTemplate >::sSimplex m_result
Definition: btGjkEpa3.h:556
EPA_MAX_ITERATIONS
#define EPA_MAX_ITERATIONS
Definition: btGjkEpa3.h:79
EPA::getedgedist
bool getedgedist(sFace *face, typename GJK< btConvexTemplate >::sSV *a, typename GJK< btConvexTemplate >::sSV *b, btScalar &dist)
Definition: btGjkEpa3.h:708
GJK::sSV
Definition: btGjkEpa3.h:148
U
unsigned int U
Definition: btGjkEpa3.h:87
GJK::m_nfree
U m_nfree
Definition: btGjkEpa3.h:167
EPA_MAX_FACES
#define EPA_MAX_FACES
Definition: btGjkEpa3.h:78
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
EPA_PLANE_EPS
#define EPA_PLANE_EPS
Definition: btGjkEpa3.h:82
eEpaFallBack
Definition: btGjkEpa3.h:519
btDot
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:901
EPA::m_depth
btScalar m_depth
Definition: btGjkEpa3.h:558
eEpaOutOfVertices
Definition: btGjkEpa3.h:517
GJK::sSimplex::p
btScalar p[4]
Definition: btGjkEpa3.h:155
EPA
Definition: btGjkEpa3.h:526
GJK::Initialize
void Initialize()
Definition: btGjkEpa3.h:178
btGjkEpaSolver3::sResults::normal
btVector3 normal
Definition: btGjkEpa3.h:46
GJK::GJK
GJK(const btConvexTemplate &a, const btConvexTemplate &b)
Definition: btGjkEpa3.h:173
eGjkValid
Definition: btGjkEpa3.h:138
eEpaStatus
eEpaStatus
Definition: btGjkEpa3.h:509
GJK::m_current
U m_current
Definition: btGjkEpa3.h:168
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
EPA::EPA
EPA()
Definition: btGjkEpa3.h:565
btVector3::length
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
eGjkStatus
eGjkStatus
Definition: btGjkEpa3.h:136
btFabs
btScalar btFabs(btScalar x)
Definition: btScalar.h:475