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
MinkowskiDiffgjkepa2_impl::MinkowskiDiff78 MinkowskiDiff()
79 {
80
81 }
82 #ifdef __SPU__
EnableMargingjkepa2_impl::MinkowskiDiff83 void EnableMargin(bool enable)
84 {
85 m_enableMargin = enable;
86 }
Support0gjkepa2_impl::MinkowskiDiff87 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 }
Support1gjkepa2_impl::MinkowskiDiff97 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
EnableMargingjkepa2_impl::MinkowskiDiff108 void EnableMargin(bool enable)
109 {
110 if(enable)
111 Ls=&btConvexShape::localGetSupportVertexNonVirtual;
112 else
113 Ls=&btConvexShape::localGetSupportVertexWithoutMarginNonVirtual;
114 }
Support0gjkepa2_impl::MinkowskiDiff115 inline btVector3 Support0(const btVector3& d) const
116 {
117 return(((m_shapes[0])->*(Ls))(d));
118 }
Support1gjkepa2_impl::MinkowskiDiff119 inline btVector3 Support1(const btVector3& d) const
120 {
121 return(m_toshape0*((m_shapes[1])->*(Ls))(m_toshape1*d));
122 }
123 #endif //__SPU__
124
Supportgjkepa2_impl::MinkowskiDiff125 inline btVector3 Support(const btVector3& d) const
126 {
127 return(Support0(d)-Support1(-d));
128 }
Supportgjkepa2_impl::MinkowskiDiff129 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 */
GJKgjkepa2_impl::GJK171 GJK()
172 {
173 Initialize();
174 }
Initializegjkepa2_impl::GJK175 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 }
Evaluategjkepa2_impl::GJK183 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 }
EncloseOrigingjkepa2_impl::GJK306 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 */
getsupportgjkepa2_impl::GJK372 void getsupport(const btVector3& d,sSV& sv) const
373 {
374 sv.d = d/d.length();
375 sv.w = m_shape.Support(sv.d);
376 }
removeverticegjkepa2_impl::GJK377 void removevertice(sSimplex& simplex)
378 {
379 m_free[m_nfree++]=simplex.c[--simplex.rank];
380 }
appendverticegjkepa2_impl::GJK381 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 }
detgjkepa2_impl::GJK387 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 }
projectorigingjkepa2_impl::GJK393 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 }
projectorigingjkepa2_impl::GJK408 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 }
projectorigingjkepa2_impl::GJK454 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 btScalar p;
515 sSV* c[3];
516 sFace* f[3];
517 sFace* l[2];
518 U1 e[3];
519 U1 pass;
520 };
521 struct sList
522 {
523 sFace* root;
524 U count;
sListgjkepa2_impl::EPA::sList525 sList() : root(0),count(0) {}
526 };
527 struct sHorizon
528 {
529 sFace* cf;
530 sFace* ff;
531 U nf;
sHorizongjkepa2_impl::EPA::sHorizon532 sHorizon() : cf(0),ff(0),nf(0) {}
533 };
534 struct eStatus { enum _ {
535 Valid,
536 Touching,
537 Degenerated,
538 NonConvex,
539 InvalidHull,
540 OutOfFaces,
541 OutOfVertices,
542 AccuraryReached,
543 FallBack,
544 Failed };};
545 /* Fields */
546 eStatus::_ m_status;
547 GJK::sSimplex m_result;
548 btVector3 m_normal;
549 btScalar m_depth;
550 sSV m_sv_store[EPA_MAX_VERTICES];
551 sFace m_fc_store[EPA_MAX_FACES];
552 U m_nextsv;
553 sList m_hull;
554 sList m_stock;
555 /* Methods */
EPAgjkepa2_impl::EPA556 EPA()
557 {
558 Initialize();
559 }
560
561
bindgjkepa2_impl::EPA562 static inline void bind(sFace* fa,U ea,sFace* fb,U eb)
563 {
564 fa->e[ea]=(U1)eb;fa->f[ea]=fb;
565 fb->e[eb]=(U1)ea;fb->f[eb]=fa;
566 }
appendgjkepa2_impl::EPA567 static inline void append(sList& list,sFace* face)
568 {
569 face->l[0] = 0;
570 face->l[1] = list.root;
571 if(list.root) list.root->l[0]=face;
572 list.root = face;
573 ++list.count;
574 }
removegjkepa2_impl::EPA575 static inline void remove(sList& list,sFace* face)
576 {
577 if(face->l[1]) face->l[1]->l[0]=face->l[0];
578 if(face->l[0]) face->l[0]->l[1]=face->l[1];
579 if(face==list.root) list.root=face->l[1];
580 --list.count;
581 }
582
583
Initializegjkepa2_impl::EPA584 void Initialize()
585 {
586 m_status = eStatus::Failed;
587 m_normal = btVector3(0,0,0);
588 m_depth = 0;
589 m_nextsv = 0;
590 for(U i=0;i<EPA_MAX_FACES;++i)
591 {
592 append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
593 }
594 }
Evaluategjkepa2_impl::EPA595 eStatus::_ Evaluate(GJK& gjk,const btVector3& guess)
596 {
597 GJK::sSimplex& simplex=*gjk.m_simplex;
598 if((simplex.rank>1)&&gjk.EncloseOrigin())
599 {
600
601 /* Clean up */
602 while(m_hull.root)
603 {
604 sFace* f = m_hull.root;
605 remove(m_hull,f);
606 append(m_stock,f);
607 }
608 m_status = eStatus::Valid;
609 m_nextsv = 0;
610 /* Orient simplex */
611 if(gjk.det( simplex.c[0]->w-simplex.c[3]->w,
612 simplex.c[1]->w-simplex.c[3]->w,
613 simplex.c[2]->w-simplex.c[3]->w)<0)
614 {
615 btSwap(simplex.c[0],simplex.c[1]);
616 btSwap(simplex.p[0],simplex.p[1]);
617 }
618 /* Build initial hull */
619 sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
620 newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
621 newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
622 newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
623 if(m_hull.count==4)
624 {
625 sFace* best=findbest();
626 sFace outer=*best;
627 U pass=0;
628 U iterations=0;
629 bind(tetra[0],0,tetra[1],0);
630 bind(tetra[0],1,tetra[2],0);
631 bind(tetra[0],2,tetra[3],0);
632 bind(tetra[1],1,tetra[3],2);
633 bind(tetra[1],2,tetra[2],1);
634 bind(tetra[2],2,tetra[3],1);
635 m_status=eStatus::Valid;
636 for(;iterations<EPA_MAX_ITERATIONS;++iterations)
637 {
638 if(m_nextsv<EPA_MAX_VERTICES)
639 {
640 sHorizon horizon;
641 sSV* w=&m_sv_store[m_nextsv++];
642 bool valid=true;
643 best->pass = (U1)(++pass);
644 gjk.getsupport(best->n,*w);
645 const btScalar wdist=btDot(best->n,w->w)-best->d;
646 if(wdist>EPA_ACCURACY)
647 {
648 for(U j=0;(j<3)&&valid;++j)
649 {
650 valid&=expand( pass,w,
651 best->f[j],best->e[j],
652 horizon);
653 }
654 if(valid&&(horizon.nf>=3))
655 {
656 bind(horizon.cf,1,horizon.ff,2);
657 remove(m_hull,best);
658 append(m_stock,best);
659 best=findbest();
660 if(best->p>=outer.p) outer=*best;
661 } else { m_status=eStatus::InvalidHull;break; }
662 } else { m_status=eStatus::AccuraryReached;break; }
663 } else { m_status=eStatus::OutOfVertices;break; }
664 }
665 const btVector3 projection=outer.n*outer.d;
666 m_normal = outer.n;
667 m_depth = outer.d;
668 m_result.rank = 3;
669 m_result.c[0] = outer.c[0];
670 m_result.c[1] = outer.c[1];
671 m_result.c[2] = outer.c[2];
672 m_result.p[0] = btCross( outer.c[1]->w-projection,
673 outer.c[2]->w-projection).length();
674 m_result.p[1] = btCross( outer.c[2]->w-projection,
675 outer.c[0]->w-projection).length();
676 m_result.p[2] = btCross( outer.c[0]->w-projection,
677 outer.c[1]->w-projection).length();
678 const btScalar sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
679 m_result.p[0] /= sum;
680 m_result.p[1] /= sum;
681 m_result.p[2] /= sum;
682 return(m_status);
683 }
684 }
685 /* Fallback */
686 m_status = eStatus::FallBack;
687 m_normal = -guess;
688 const btScalar nl=m_normal.length();
689 if(nl>0)
690 m_normal = m_normal/nl;
691 else
692 m_normal = btVector3(1,0,0);
693 m_depth = 0;
694 m_result.rank=1;
695 m_result.c[0]=simplex.c[0];
696 m_result.p[0]=1;
697 return(m_status);
698 }
newfacegjkepa2_impl::EPA699 sFace* newface(sSV* a,sSV* b,sSV* c,bool forced)
700 {
701 if(m_stock.root)
702 {
703 sFace* face=m_stock.root;
704 remove(m_stock,face);
705 append(m_hull,face);
706 face->pass = 0;
707 face->c[0] = a;
708 face->c[1] = b;
709 face->c[2] = c;
710 face->n = btCross(b->w-a->w,c->w-a->w);
711 const btScalar l=face->n.length();
712 const bool v=l>EPA_ACCURACY;
713 face->p = btMin(btMin(
714 btDot(a->w,btCross(face->n,a->w-b->w)),
715 btDot(b->w,btCross(face->n,b->w-c->w))),
716 btDot(c->w,btCross(face->n,c->w-a->w))) /
717 (v?l:1);
718 face->p = face->p>=-EPA_INSIDE_EPS?0:face->p;
719 if(v)
720 {
721 face->d = btDot(a->w,face->n)/l;
722 face->n /= l;
723 if(forced||(face->d>=-EPA_PLANE_EPS))
724 {
725 return(face);
726 } else m_status=eStatus::NonConvex;
727 } else m_status=eStatus::Degenerated;
728 remove(m_hull,face);
729 append(m_stock,face);
730 return(0);
731 }
732 m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces;
733 return(0);
734 }
findbestgjkepa2_impl::EPA735 sFace* findbest()
736 {
737 sFace* minf=m_hull.root;
738 btScalar mind=minf->d*minf->d;
739 btScalar maxp=minf->p;
740 for(sFace* f=minf->l[1];f;f=f->l[1])
741 {
742 const btScalar sqd=f->d*f->d;
743 if((f->p>=maxp)&&(sqd<mind))
744 {
745 minf=f;
746 mind=sqd;
747 maxp=f->p;
748 }
749 }
750 return(minf);
751 }
expandgjkepa2_impl::EPA752 bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
753 {
754 static const U i1m3[]={1,2,0};
755 static const U i2m3[]={2,0,1};
756 if(f->pass!=pass)
757 {
758 const U e1=i1m3[e];
759 if((btDot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
760 {
761 sFace* nf=newface(f->c[e1],f->c[e],w,false);
762 if(nf)
763 {
764 bind(nf,0,f,e);
765 if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
766 horizon.cf=nf;
767 ++horizon.nf;
768 return(true);
769 }
770 }
771 else
772 {
773 const U e2=i2m3[e];
774 f->pass = (U1)pass;
775 if( expand(pass,w,f->f[e1],f->e[e1],horizon)&&
776 expand(pass,w,f->f[e2],f->e[e2],horizon))
777 {
778 remove(m_hull,f);
779 append(m_stock,f);
780 return(true);
781 }
782 }
783 }
784 return(false);
785 }
786
787 };
788
789 //
Initialize(const btConvexShape * shape0,const btTransform & wtrs0,const btConvexShape * shape1,const btTransform & wtrs1,btGjkEpaSolver2::sResults & results,tShape & shape,bool withmargins)790 static void Initialize( const btConvexShape* shape0,const btTransform& wtrs0,
791 const btConvexShape* shape1,const btTransform& wtrs1,
792 btGjkEpaSolver2::sResults& results,
793 tShape& shape,
794 bool withmargins)
795 {
796 /* Results */
797 results.witnesses[0] =
798 results.witnesses[1] = btVector3(0,0,0);
799 results.status = btGjkEpaSolver2::sResults::Separated;
800 /* Shape */
801 shape.m_shapes[0] = shape0;
802 shape.m_shapes[1] = shape1;
803 shape.m_toshape1 = wtrs1.getBasis().transposeTimes(wtrs0.getBasis());
804 shape.m_toshape0 = wtrs0.inverseTimes(wtrs1);
805 shape.EnableMargin(withmargins);
806 }
807
808 }
809
810 //
811 // Api
812 //
813
814 using namespace gjkepa2_impl;
815
816 //
StackSizeRequirement()817 int btGjkEpaSolver2::StackSizeRequirement()
818 {
819 return(sizeof(GJK)+sizeof(EPA));
820 }
821
822 //
Distance(const btConvexShape * shape0,const btTransform & wtrs0,const btConvexShape * shape1,const btTransform & wtrs1,const btVector3 & guess,sResults & results)823 bool btGjkEpaSolver2::Distance( const btConvexShape* shape0,
824 const btTransform& wtrs0,
825 const btConvexShape* shape1,
826 const btTransform& wtrs1,
827 const btVector3& guess,
828 sResults& results)
829 {
830 tShape shape;
831 Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
832 GJK gjk;
833 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
834 if(gjk_status==GJK::eStatus::Valid)
835 {
836 btVector3 w0=btVector3(0,0,0);
837 btVector3 w1=btVector3(0,0,0);
838 for(U i=0;i<gjk.m_simplex->rank;++i)
839 {
840 const btScalar p=gjk.m_simplex->p[i];
841 w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
842 w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
843 }
844 results.witnesses[0] = wtrs0*w0;
845 results.witnesses[1] = wtrs0*w1;
846 results.normal = w0-w1;
847 results.distance = results.normal.length();
848 results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1;
849 return(true);
850 }
851 else
852 {
853 results.status = gjk_status==GJK::eStatus::Inside?
854 sResults::Penetrating :
855 sResults::GJK_Failed ;
856 return(false);
857 }
858 }
859
860 //
Penetration(const btConvexShape * shape0,const btTransform & wtrs0,const btConvexShape * shape1,const btTransform & wtrs1,const btVector3 & guess,sResults & results,bool usemargins)861 bool btGjkEpaSolver2::Penetration( const btConvexShape* shape0,
862 const btTransform& wtrs0,
863 const btConvexShape* shape1,
864 const btTransform& wtrs1,
865 const btVector3& guess,
866 sResults& results,
867 bool usemargins)
868 {
869 tShape shape;
870 Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,usemargins);
871 GJK gjk;
872 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess);
873 switch(gjk_status)
874 {
875 case GJK::eStatus::Inside:
876 {
877 EPA epa;
878 EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess);
879 if(epa_status!=EPA::eStatus::Failed)
880 {
881 btVector3 w0=btVector3(0,0,0);
882 for(U i=0;i<epa.m_result.rank;++i)
883 {
884 w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
885 }
886 results.status = sResults::Penetrating;
887 results.witnesses[0] = wtrs0*w0;
888 results.witnesses[1] = wtrs0*(w0-epa.m_normal*epa.m_depth);
889 results.normal = -epa.m_normal;
890 results.distance = -epa.m_depth;
891 return(true);
892 } else results.status=sResults::EPA_Failed;
893 }
894 break;
895 case GJK::eStatus::Failed:
896 results.status=sResults::GJK_Failed;
897 break;
898 default:
899 {
900 }
901 }
902 return(false);
903 }
904
905 #ifndef __SPU__
906 //
SignedDistance(const btVector3 & position,btScalar margin,const btConvexShape * shape0,const btTransform & wtrs0,sResults & results)907 btScalar btGjkEpaSolver2::SignedDistance(const btVector3& position,
908 btScalar margin,
909 const btConvexShape* shape0,
910 const btTransform& wtrs0,
911 sResults& results)
912 {
913 tShape shape;
914 btSphereShape shape1(margin);
915 btTransform wtrs1(btQuaternion(0,0,0,1),position);
916 Initialize(shape0,wtrs0,&shape1,wtrs1,results,shape,false);
917 GJK gjk;
918 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,btVector3(1,1,1));
919 if(gjk_status==GJK::eStatus::Valid)
920 {
921 btVector3 w0=btVector3(0,0,0);
922 btVector3 w1=btVector3(0,0,0);
923 for(U i=0;i<gjk.m_simplex->rank;++i)
924 {
925 const btScalar p=gjk.m_simplex->p[i];
926 w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
927 w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
928 }
929 results.witnesses[0] = wtrs0*w0;
930 results.witnesses[1] = wtrs0*w1;
931 const btVector3 delta= results.witnesses[1]-
932 results.witnesses[0];
933 const btScalar margin= shape0->getMarginNonVirtual()+
934 shape1.getMarginNonVirtual();
935 const btScalar length= delta.length();
936 results.normal = delta/length;
937 results.witnesses[0] += results.normal*margin;
938 return(length-margin);
939 }
940 else
941 {
942 if(gjk_status==GJK::eStatus::Inside)
943 {
944 if(Penetration(shape0,wtrs0,&shape1,wtrs1,gjk.m_ray,results))
945 {
946 const btVector3 delta= results.witnesses[0]-
947 results.witnesses[1];
948 const btScalar length= delta.length();
949 if (length >= SIMD_EPSILON)
950 results.normal = delta/length;
951 return(-length);
952 }
953 }
954 }
955 return(SIMD_INFINITY);
956 }
957
958 //
SignedDistance(const btConvexShape * shape0,const btTransform & wtrs0,const btConvexShape * shape1,const btTransform & wtrs1,const btVector3 & guess,sResults & results)959 bool btGjkEpaSolver2::SignedDistance(const btConvexShape* shape0,
960 const btTransform& wtrs0,
961 const btConvexShape* shape1,
962 const btTransform& wtrs1,
963 const btVector3& guess,
964 sResults& results)
965 {
966 if(!Distance(shape0,wtrs0,shape1,wtrs1,guess,results))
967 return(Penetration(shape0,wtrs0,shape1,wtrs1,guess,results,false));
968 else
969 return(true);
970 }
971 #endif //__SPU__
972
973 /* Symbols cleanup */
974
975 #undef GJK_MAX_ITERATIONS
976 #undef GJK_ACCURARY
977 #undef GJK_MIN_DISTANCE
978 #undef GJK_DUPLICATED_EPS
979 #undef GJK_SIMPLEX2_EPS
980 #undef GJK_SIMPLEX3_EPS
981 #undef GJK_SIMPLEX4_EPS
982
983 #undef EPA_MAX_VERTICES
984 #undef EPA_MAX_FACES
985 #undef EPA_MAX_ITERATIONS
986 #undef EPA_ACCURACY
987 #undef EPA_FALLBACK
988 #undef EPA_PLANE_EPS
989 #undef EPA_INSIDE_EPS
990