Bullet Collision Detection & Physics Library
btGjkEpa2.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
4 
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the
7 use of this software.
8 Permission is granted to anyone to use this software for any purpose,
9 including commercial applications, and to alter it and redistribute it
10 freely,
11 subject to the following restrictions:
12 
13 1. The origin of this software must not be misrepresented; you must not
14 claim that you wrote the original software. If you use this software in a
15 product, an acknowledgment in the product documentation would be appreciated
16 but is not required.
17 2. Altered source versions must be plainly marked as such, and must not be
18 misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20 */
21 
22 /*
23 GJK-EPA collision solver by Nathanael Presson, 2008
24 */
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
67  {
71 #ifdef __SPU__
72  bool m_enableMargin;
73 #else
74  btVector3 (btConvexShape::*Ls)(const btVector3&) const;
75 #endif//__SPU__
76 
77 
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  {
92  } else
93  {
95  }
96  }
97  inline btVector3 Support1(const btVector3& d) const
98  {
99  if (m_enableMargin)
100  {
102  } else
103  {
105  }
106  }
107 #else
108  void EnableMargin(bool enable)
109  {
110  if(enable)
112  else
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 
139 
140 
141  // GJK
142  struct GJK
143  {
144  /* Types */
145  struct sSV
146  {
148  };
149  struct sSimplex
150  {
151  sSV* c[4];
154  };
155  struct eStatus { enum _ {
158  Failed };};
159  /* Fields */
165  sSV* m_free[4];
170  /* Methods */
171  GJK()
172  {
173  Initialize();
174  }
175  void Initialize()
176  {
177  m_ray = btVector3(0,0,0);
178  m_nfree = 0;
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;
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;
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 */
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 */
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 */
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 */
291  break;
292  }
294  } while(m_status==eStatus::Valid);
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  }
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);
319  appendvertice(*m_simplex,-axis);
320  if(EncloseOrigin()) return(true);
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  {
336  if(EncloseOrigin()) return(true);
339  if(EncloseOrigin()) return(true);
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  {
352  if(EncloseOrigin()) return(true);
355  if(EncloseOrigin()) return(true);
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  {
514  sSV* c[3];
515  sFace* f[3];
516  sFace* l[2];
517  U1 e[3];
519  };
520  struct sList
521  {
524  sList() : root(0),count(0) {}
525  };
526  struct sHorizon
527  {
530  U nf;
531  sHorizon() : cf(0),ff(0),nf(0) {}
532  };
533  struct eStatus { enum _ {
543  Failed };};
544  /* Fields */
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  {
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  }
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);
635  for(;iterations<EPA_MAX_ITERATIONS;++iterations)
636  {
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 */
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
767  }
768  else
770 
771  remove(m_hull, face);
772  append(m_stock, face);
773  return 0;
774 
775  }
777  return 0;
778  }
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);
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 //
860 {
861  return(sizeof(GJK)+sizeof(EPA));
862 }
863 
864 //
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 //
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  {
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;
938  results.status=sResults::GJK_Failed;
939  break;
940  default:
941  {
942  }
943  }
944  return(false);
945 }
946 
947 #ifndef __SPU__
948 //
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 //
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
btVector3(btConvexShape::* Ls)(const btVector3 &) const
Definition: btGjkEpa2.cpp:74
static T sum(const btAlignedObjectArray< T > &items)
#define SIMD_EPSILON
Definition: btScalar.h:448
#define GJK_SIMPLEX2_EPS
Definition: btGjkEpa2.cpp:47
btScalar length(const btQuaternion &q)
Return the length of a quaternion.
Definition: btQuaternion.h:835
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, const btVector3 &d, btScalar *w, U &m)
Definition: btGjkEpa2.cpp:454
GJK::sSimplex m_result
Definition: btGjkEpa2.cpp:546
eStatus::_ m_status
Definition: btGjkEpa2.cpp:169
float dist(const Point3 &pnt0, const Point3 &pnt1)
void appendvertice(sSimplex &simplex, const btVector3 &v)
Definition: btGjkEpa2.cpp:381
static void bind(sFace *fa, U ea, sFace *fb, U eb)
Definition: btGjkEpa2.cpp:561
btVector3 Support0(const btVector3 &d) const
Definition: btGjkEpa2.cpp:115
btScalar btSqrt(btScalar y)
Definition: btScalar.h:387
unsigned char U1
Definition: btGjkEpa2.cpp:63
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, const btVector3 &c, btScalar *w, U &m)
Definition: btGjkEpa2.cpp:408
sFace * findbest()
Definition: btGjkEpa2.cpp:779
#define GJK_MIN_DISTANCE
Definition: btGjkEpa2.cpp:45
sFace * newface(sSV *a, sSV *b, sSV *c, bool forced)
Definition: btGjkEpa2.cpp:734
btMatrix3x3 transposeTimes(const btMatrix3x3 &m) const
Definition: btMatrix3x3.h:1037
btVector3 localGetSupportVertexWithoutMarginNonVirtual(const btVector3 &vec) const
bool getedgedist(sFace *face, sSV *a, sSV *b, btScalar &dist)
Definition: btGjkEpa2.cpp:698
The btSphereShape implements an implicit sphere, centered around a local origin with radius...
Definition: btSphereShape.h:22
eStatus::_ Evaluate(GJK &gjk, const btVector3 &guess)
Definition: btGjkEpa2.cpp:594
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
btScalar getMarginNonVirtual() const
btVector3 Support(const btVector3 &d, U index) const
Definition: btGjkEpa2.cpp:129
sSV m_sv_store[EPA_MAX_VERTICES]
Definition: btGjkEpa2.cpp:549
btTransform inverseTimes(const btTransform &t) const
Return the inverse of this transform times the other transform.
Definition: btTransform.h:230
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
eStatus::_ m_status
Definition: btGjkEpa2.cpp:545
The btConvexShape is an abstract shape interface, implemented by all convex shapes such as btBoxShape...
Definition: btConvexShape.h:31
#define EPA_PLANE_EPS
Definition: btGjkEpa2.cpp:57
btVector3 btCross(const btVector3 &v1, const btVector3 &v2)
Return the cross product of two vectors.
Definition: btVector3.h:918
void getsupport(const btVector3 &d, sSV &sv) const
Definition: btGjkEpa2.cpp:372
const btConvexShape * m_shapes[2]
Definition: btGjkEpa2.cpp:68
#define SIMD_INFINITY
Definition: btScalar.h:449
#define GJK_ACCURARY
Definition: btGjkEpa2.cpp:44
float projection(const Point3 &pnt, const Vector3 &unitVec)
void btSwap(T &a, T &b)
Definition: btScalar.h:535
btVector3 Support1(const btVector3 &d) const
Definition: btGjkEpa2.cpp:119
bool expand(U pass, sSV *w, sFace *f, U e, sHorizon &horizon)
Definition: btGjkEpa2.cpp:794
#define GJK_MAX_ITERATIONS
Definition: btGjkEpa2.cpp:43
btVector3 m_normal
Definition: btGjkEpa2.cpp:547
#define GJK_SIMPLEX4_EPS
Definition: btGjkEpa2.cpp:49
const Matrix3 outer(const Vector3 &tfrm0, const Vector3 &tfrm1)
static int StackSizeRequirement()
Definition: btGjkEpa2.cpp:859
btMatrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: btTransform.h:112
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
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
void removevertice(sSimplex &simplex)
Definition: btGjkEpa2.cpp:377
btVector3 localGetSupportVertexNonVirtual(const btVector3 &vec) const
#define EPA_MAX_ITERATIONS
Definition: btGjkEpa2.cpp:54
sFace m_fc_store[EPA_MAX_FACES]
Definition: btGjkEpa2.cpp:550
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
btScalar m_distance
Definition: btGjkEpa2.cpp:162
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btScalar length2() const
Return the length of the vector squared.
Definition: btVector3.h:257
sSimplex * m_simplex
Definition: btGjkEpa2.cpp:168
static bool Distance(const btConvexShape *shape0, const btTransform &wtrs0, const btConvexShape *shape1, const btTransform &wtrs1, const btVector3 &guess, sResults &results)
Definition: btGjkEpa2.cpp:865
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
btVector3 Support(const btVector3 &d) const
Definition: btGjkEpa2.cpp:125
#define GJK_DUPLICATED_EPS
Definition: btGjkEpa2.cpp:46
enum btGjkEpaSolver2::sResults::eStatus status
#define GJK_SIMPLEX3_EPS
Definition: btGjkEpa2.cpp:48
void EnableMargin(bool enable)
Definition: btGjkEpa2.cpp:108
btVector3 witnesses[2]
Definition: btGjkEpa2.h:42
const T & btMax(const T &a, const T &b)
Definition: btMinMax.h:29
static btScalar SignedDistance(const btVector3 &position, btScalar margin, const btConvexShape *shape, const btTransform &wtrs, sResults &results)
Definition: btGjkEpa2.cpp:949
#define EPA_MAX_FACES
Definition: btGjkEpa2.cpp:53
static btScalar projectorigin(const btVector3 &a, const btVector3 &b, btScalar *w, U &m)
Definition: btGjkEpa2.cpp:393
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
static btScalar det(const btVector3 &a, const btVector3 &b, const btVector3 &c)
Definition: btGjkEpa2.cpp:387
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:48
btScalar btDot(const btVector3 &v1, const btVector3 &v2)
Return the dot product between two vectors.
Definition: btVector3.h:888
#define EPA_ACCURACY
Definition: btGjkEpa2.cpp:55
eStatus::_ Evaluate(const tShape &shapearg, const btVector3 &guess)
Definition: btGjkEpa2.cpp:183
unsigned int U
Definition: btGjkEpa2.cpp:62
static void append(sList &list, sFace *face)
Definition: btGjkEpa2.cpp:566
#define EPA_MAX_VERTICES
Definition: btGjkEpa2.cpp:52
MinkowskiDiff tShape
Definition: btGjkEpa2.cpp:138
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:266
sSimplex m_simplices[2]
Definition: btGjkEpa2.cpp:163
btScalar btFabs(btScalar x)
Definition: btScalar.h:407
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579