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