Bullet Collision Detection & Physics Library: btGjkEpa2.cpp Source File

Bullet Collision Detection & Physics Library

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