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