Bullet Collision Detection & Physics Library
btGjkEpa3.h
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2014 Erwin Coumans http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the
7use of this software.
8Permission is granted to anyone to use this software for any purpose,
9including commercial applications, and to alter it and redistribute it
10freely,
11subject to the following restrictions:
12
131. The origin of this software must not be misrepresented; you must not
14claim that you wrote the original software. If you use this software in a
15product, an acknowledgment in the product documentation would be appreciated
16but is not required.
172. Altered source versions must be plainly marked as such, and must not be
18misrepresented as being the original software.
193. This notice may not be removed or altered from any source distribution.
20*/
21
22/*
23Initial GJK-EPA collision solver by Nathanael Presson, 2008
24Improvements 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
34struct btGjkEpaSolver3
35{
36struct 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
136enum 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
509enum 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
525template <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//
865template <typename btConvexTemplate>
866bool 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
901template <typename btConvexTemplate>
902bool 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
944int 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
992template <typename btConvexTemplate, typename btDistanceInfoTemplate>
993int 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
EPA_ACCURACY
#define EPA_ACCURACY
Definition btGjkEpa3.h:80
btComputeGjkDistance
int btComputeGjkDistance(const btConvexTemplate &a, const btConvexTemplate &b, const btGjkCollisionDescription &colDesc, btDistanceInfoTemplate *distInfo)
Definition btGjkEpa3.h:993
U
unsigned int U
Definition btGjkEpa3.h:87
GJK_ACCURARY
#define GJK_ACCURARY
Definition btGjkEpa3.h:69
EPA_MAX_FACES
#define EPA_MAX_FACES
Definition btGjkEpa3.h:78
Initialize
static void Initialize(const btConvexTemplate &a, const btConvexTemplate &b, btGjkEpaSolver3::sResults &results, MinkowskiDiff< btConvexTemplate > &shape)
Definition btGjkEpa3.h:842
GJK_MIN_DISTANCE
#define GJK_MIN_DISTANCE
Definition btGjkEpa3.h:70
GJK_SIMPLEX4_EPS
#define GJK_SIMPLEX4_EPS
Definition btGjkEpa3.h:74
EPA_PLANE_EPS
#define EPA_PLANE_EPS
Definition btGjkEpa3.h:82
GJK_DUPLICATED_EPS
#define GJK_DUPLICATED_EPS
Definition btGjkEpa3.h:71
btGjkEpaSolver3_Penetration
bool btGjkEpaSolver3_Penetration(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
Definition btGjkEpa3.h:902
eEpaStatus
eEpaStatus
Definition btGjkEpa3.h:510
eEpaFailed
@ eEpaFailed
Definition btGjkEpa3.h:520
eEpaFallBack
@ eEpaFallBack
Definition btGjkEpa3.h:519
eEpaDegenerated
@ eEpaDegenerated
Definition btGjkEpa3.h:513
eEpaInvalidHull
@ eEpaInvalidHull
Definition btGjkEpa3.h:515
eEpaNonConvex
@ eEpaNonConvex
Definition btGjkEpa3.h:514
eEpaOutOfVertices
@ eEpaOutOfVertices
Definition btGjkEpa3.h:517
eEpaTouching
@ eEpaTouching
Definition btGjkEpa3.h:512
eEpaAccuraryReached
@ eEpaAccuraryReached
Definition btGjkEpa3.h:518
eEpaValid
@ eEpaValid
Definition btGjkEpa3.h:511
eEpaOutOfFaces
@ eEpaOutOfFaces
Definition btGjkEpa3.h:516
EPA_MAX_VERTICES
#define EPA_MAX_VERTICES
Definition btGjkEpa3.h:77
GJK_SIMPLEX2_EPS
#define GJK_SIMPLEX2_EPS
Definition btGjkEpa3.h:72
GJK_MAX_ITERATIONS
#define GJK_MAX_ITERATIONS
Definition btGjkEpa3.h:68
GJK_SIMPLEX3_EPS
#define GJK_SIMPLEX3_EPS
Definition btGjkEpa3.h:73
btGjkEpaSolver3_Distance
bool btGjkEpaSolver3_Distance(const btConvexTemplate &a, const btConvexTemplate &b, const btVector3 &guess, btGjkEpaSolver3::sResults &results)
Definition btGjkEpa3.h:866
U1
unsigned char U1
Definition btGjkEpa3.h:88
eGjkStatus
eGjkStatus
Definition btGjkEpa3.h:137
eGjkValid
@ eGjkValid
Definition btGjkEpa3.h:138
eGjkInside
@ eGjkInside
Definition btGjkEpa3.h:139
eGjkFailed
@ eGjkFailed
Definition btGjkEpa3.h:140
EPA_MAX_ITERATIONS
#define EPA_MAX_ITERATIONS
Definition btGjkEpa3.h:79
btMax
const T & btMax(const T &a, const T &b)
Definition btMinMax.h:29
length
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition btQuaternion.h:886
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:292
btSqrt
btScalar btSqrt(btScalar y)
Definition btScalar.h:444
btFabs
btScalar btFabs(btScalar x)
Definition btScalar.h:475
SIMD_EPSILON
#define SIMD_EPSILON
Definition btScalar.h:521
btSwap
void btSwap(T &a, T &b)
Definition btScalar.h:621
sum
static T sum(const btAlignedObjectArray< T > &items)
Definition btSoftBodyHelpers.cpp:84
btDot
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition btVector3.h:901
btCross
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition btVector3.h:931
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition btMatrix3x3.h:48
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition btTransform.h:34
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition btVector3.h:84
btVector3::w
const btScalar & w() const
Return the w value.
Definition btVector3.h:593
btVector3::z
const btScalar & z() const
Return the z value.
Definition btVector3.h:591
btVector3::length
btScalar length() const
Return the length of the vector.
Definition btVector3.h:263
btVector3::length2
btScalar length2() const
Return the length of the vector squared.
Definition btVector3.h:257
btVector3::x
const btScalar & x() const
Return the x value.
Definition btVector3.h:587
btVector3::y
const btScalar & y() const
Return the y value.
Definition btVector3.h:589
EPA::sFace::l
sFace * l[2]
Definition btGjkEpa3.h:536
EPA::sFace::f
sFace * f[3]
Definition btGjkEpa3.h:535
EPA::sFace::c
GJK< btConvexTemplate >::sSV * c[3]
Definition btGjkEpa3.h:534
EPA::sFace::d
btScalar d
Definition btGjkEpa3.h:533
EPA::sFace::n
btVector3 n
Definition btGjkEpa3.h:532
EPA::sList::root
sFace * root
Definition btGjkEpa3.h:542
EPA::getedgedist
bool getedgedist(sFace *face, typename GJK< btConvexTemplate >::sSV *a, typename GJK< btConvexTemplate >::sSV *b, btScalar &dist)
Definition btGjkEpa3.h:708
EPA::expand
bool expand(U pass, typename GJK< btConvexTemplate >::sSV *w, sFace *f, U e, sHorizon &horizon)
Definition btGjkEpa3.h:804
EPA::append
static void append(sList &list, sFace *face)
Definition btGjkEpa3.h:576
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_result
GJK< btConvexTemplate >::sSimplex m_result
Definition btGjkEpa3.h:556
EPA::m_sv_store
GJK< btConvexTemplate >::sSV m_sv_store[EPA_MAX_VERTICES]
Definition btGjkEpa3.h:559
EPA::m_stock
sList m_stock
Definition btGjkEpa3.h:563
EPA::remove
static void remove(sList &list, sFace *face)
Definition btGjkEpa3.h:584
EPA::Evaluate
eEpaStatus Evaluate(GJK< btConvexTemplate > &gjk, const btVector3 &guess)
Definition btGjkEpa3.h:604
EPA::m_depth
btScalar m_depth
Definition btGjkEpa3.h:558
EPA::m_nextsv
U m_nextsv
Definition btGjkEpa3.h:561
EPA::m_hull
sList m_hull
Definition btGjkEpa3.h:562
EPA::bind
static void bind(sFace *fa, U ea, sFace *fb, U eb)
Definition btGjkEpa3.h:571
EPA::m_normal
btVector3 m_normal
Definition btGjkEpa3.h:557
EPA::m_fc_store
sFace m_fc_store[EPA_MAX_FACES]
Definition btGjkEpa3.h:560
EPA::Initialize
void Initialize()
Definition btGjkEpa3.h:593
EPA::m_status
eEpaStatus m_status
Definition btGjkEpa3.h:555
EPA::EPA
EPA()
Definition btGjkEpa3.h:565
EPA::findbest
sFace * findbest()
Definition btGjkEpa3.h:789
GJK::sSV::w
btVector3 w
Definition btGjkEpa3.h:150
GJK::sSV::d
btVector3 d
Definition btGjkEpa3.h:150
GJK::sSimplex::c
sSV * c[4]
Definition btGjkEpa3.h:154
GJK::sSimplex::p
btScalar p[4]
Definition btGjkEpa3.h:155
GJK::m_nfree
U m_nfree
Definition btGjkEpa3.h:167
GJK::Evaluate
eGjkStatus Evaluate(const MinkowskiDiff< btConvexTemplate > &shapearg, const btVector3 &guess)
Definition btGjkEpa3.h:186
GJK::getsupport
void getsupport(const btVector3 &d, sSV &sv) const
Definition btGjkEpa3.h:375
GJK::m_shape
MinkowskiDiff< btConvexTemplate > m_shape
Definition btGjkEpa3.h:161
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
GJK::m_current
U m_current
Definition btGjkEpa3.h:168
GJK::EncloseOrigin
bool EncloseOrigin()
Definition btGjkEpa3.h:309
GJK::m_free
sSV * m_free[4]
Definition btGjkEpa3.h:166
GJK::projectorigin
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, btScalar *w, U &m)
Definition btGjkEpa3.h:396
GJK::m_store
sSV m_store[4]
Definition btGjkEpa3.h:165
GJK::m_simplex
sSimplex * m_simplex
Definition btGjkEpa3.h:169
GJK::det
static btScalar det(const btVector3 &a, const btVector3 &b, const btVector3 &c)
Definition btGjkEpa3.h:390
GJK::Initialize
void Initialize()
Definition btGjkEpa3.h:178
GJK::GJK
GJK(const btConvexTemplate &a, const btConvexTemplate &b)
Definition btGjkEpa3.h:173
GJK::m_simplices
sSimplex m_simplices[2]
Definition btGjkEpa3.h:164
GJK::m_ray
btVector3 m_ray
Definition btGjkEpa3.h:162
GJK::projectorigin
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, btScalar *w, U &m)
Definition btGjkEpa3.h:411
GJK::removevertice
void removevertice(sSimplex &simplex)
Definition btGjkEpa3.h:380
GJK::m_status
eGjkStatus m_status
Definition btGjkEpa3.h:170
GJK::m_distance
btScalar m_distance
Definition btGjkEpa3.h:163
GJK::appendvertice
void appendvertice(sSimplex &simplex, const btVector3 &v)
Definition btGjkEpa3.h:384
MinkowskiDiff::Support1
btVector3 Support1(const btVector3 &d) const
Definition btGjkEpa3.h:117
MinkowskiDiff::Support0
btVector3 Support0(const btVector3 &d) const
Definition btGjkEpa3.h:113
MinkowskiDiff::EnableMargin
void EnableMargin(bool enable)
Definition btGjkEpa3.h:109
MinkowskiDiff::Support
btVector3 Support(const btVector3 &d) const
Definition btGjkEpa3.h:123
MinkowskiDiff::Support
btVector3 Support(const btVector3 &d, U index) const
Definition btGjkEpa3.h:127
MinkowskiDiff::MinkowskiDiff
MinkowskiDiff(const btConvexTemplate &a, const btConvexTemplate &b)
Definition btGjkEpa3.h:103
MinkowskiDiff::m_toshape0
btTransform m_toshape0
Definition btGjkEpa3.h:98
MinkowskiDiff::m_convexBPtr
const btConvexTemplate * m_convexBPtr
Definition btGjkEpa3.h:95
MinkowskiDiff::m_convexAPtr
const btConvexTemplate * m_convexAPtr
Definition btGjkEpa3.h:94
MinkowskiDiff::m_enableMargin
bool m_enableMargin
Definition btGjkEpa3.h:100
MinkowskiDiff::m_toshape1
btMatrix3x3 m_toshape1
Definition btGjkEpa3.h:97
btGjkEpaSolver3::sResults::status
enum btGjkEpaSolver3::sResults::eStatus status
Generated by doxygen 1.10.0