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

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

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