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 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;
sListgjkepa2_impl::EPA::sList524 sList() : root(0),count(0) {}
525 };
526 struct sHorizon
527 {
528 sFace* cf;
529 sFace* ff;
530 U nf;
sHorizongjkepa2_impl::EPA::sHorizon531 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 */
EPAgjkepa2_impl::EPA555 EPA()
556 {
557 Initialize();
558 }
559
560
bindgjkepa2_impl::EPA561 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 }
appendgjkepa2_impl::EPA566 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 }
removegjkepa2_impl::EPA574 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
Initializegjkepa2_impl::EPA583 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 }
Evaluategjkepa2_impl::EPA594 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 }
getedgedistgjkepa2_impl::EPA698 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 }
newfacegjkepa2_impl::EPA734 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 }
findbestgjkepa2_impl::EPA779 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 }
expandgjkepa2_impl::EPA794 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 //
Initialize(const btConvexShape * shape0,const btTransform & wtrs0,const btConvexShape * shape1,const btTransform & wtrs1,btGjkEpaSolver2::sResults & results,tShape & shape,bool withmargins)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 //
StackSizeRequirement()859 int btGjkEpaSolver2::StackSizeRequirement()
860 {
861 return(sizeof(GJK)+sizeof(EPA));
862 }
863
864 //
Distance(const btConvexShape * shape0,const btTransform & wtrs0,const btConvexShape * shape1,const btTransform & wtrs1,const btVector3 & guess,sResults & results)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 //
Penetration(const btConvexShape * shape0,const btTransform & wtrs0,const btConvexShape * shape1,const btTransform & wtrs1,const btVector3 & guess,sResults & results,bool usemargins)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 //
SignedDistance(const btVector3 & position,btScalar margin,const btConvexShape * shape0,const btTransform & wtrs0,sResults & results)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 //
SignedDistance(const btConvexShape * shape0,const btTransform & wtrs0,const btConvexShape * shape1,const btTransform & wtrs1,const btVector3 & guess,sResults & results)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
1032