1 /*************************************************************************/
2 /* gjk_epa.cpp */
3 /*************************************************************************/
4 /* This file is part of: */
5 /* GODOT ENGINE */
6 /* https://godotengine.org */
7 /*************************************************************************/
8 /* Copyright (c) 2007-2020 Juan Linietsky, Ariel Manzur. */
9 /* Copyright (c) 2014-2020 Godot Engine contributors (cf. AUTHORS.md). */
10 /* */
11 /* Permission is hereby granted, free of charge, to any person obtaining */
12 /* a copy of this software and associated documentation files (the */
13 /* "Software"), to deal in the Software without restriction, including */
14 /* without limitation the rights to use, copy, modify, merge, publish, */
15 /* distribute, sublicense, and/or sell copies of the Software, and to */
16 /* permit persons to whom the Software is furnished to do so, subject to */
17 /* the following conditions: */
18 /* */
19 /* The above copyright notice and this permission notice shall be */
20 /* included in all copies or substantial portions of the Software. */
21 /* */
22 /* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
23 /* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
24 /* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
25 /* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
26 /* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
27 /* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
28 /* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
29 /*************************************************************************/
30
31 #include "gjk_epa.h"
32
33 /* Disabling formatting for thirdparty code snippet */
34 /* clang-format off */
35
36 /*************** Bullet's GJK-EPA2 IMPLEMENTATION *******************/
37
38 /*
39 Bullet Continuous Collision Detection and Physics Library
40 Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
41
42 This software is provided 'as-is', without any express or implied warranty.
43 In no event will the authors be held liable for any damages arising from the
44 use of this software.
45 Permission is granted to anyone to use this software for any purpose,
46 including commercial applications, and to alter it and redistribute it
47 freely,
48 subject to the following restrictions:
49
50 1. The origin of this software must not be misrepresented; you must not
51 claim that you wrote the original software. If you use this software in a
52 product, an acknowledgment in the product documentation would be appreciated
53 but is not required.
54 2. Altered source versions must be plainly marked as such, and must not be
55 misrepresented as being the original software.
56 3. This notice may not be removed or altered from any source distribution.
57 */
58
59 /*
60 GJK-EPA collision solver by Nathanael Presson, 2008
61 */
62
63 // Config
64
65 /* GJK */
66 #define GJK_MAX_ITERATIONS 128
67 #define GJK_ACCURARY ((real_t)0.0001)
68 #define GJK_MIN_DISTANCE ((real_t)0.0001)
69 #define GJK_DUPLICATED_EPS ((real_t)0.0001)
70 #define GJK_SIMPLEX2_EPS ((real_t)0.0)
71 #define GJK_SIMPLEX3_EPS ((real_t)0.0)
72 #define GJK_SIMPLEX4_EPS ((real_t)0.0)
73
74 /* EPA */
75 #define EPA_MAX_VERTICES 64
76 #define EPA_MAX_FACES (EPA_MAX_VERTICES*2)
77 #define EPA_MAX_ITERATIONS 255
78 #define EPA_ACCURACY ((real_t)0.0001)
79 #define EPA_FALLBACK (10*EPA_ACCURACY)
80 #define EPA_PLANE_EPS ((real_t)0.00001)
81 #define EPA_INSIDE_EPS ((real_t)0.01)
82
83 namespace GjkEpa2 {
84
85
86 struct sResults {
87 enum eStatus {
88 Separated, /* Shapes doesn't penetrate */
89 Penetrating, /* Shapes are penetrating */
90 GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */
91 EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */
92 } status;
93
94 Vector3 witnesses[2];
95 Vector3 normal;
96 real_t distance;
97 };
98
99 // Shorthands
100 typedef unsigned int U;
101 typedef unsigned char U1;
102
103 // MinkowskiDiff
104 struct MinkowskiDiff {
105
106 const ShapeSW* m_shapes[2];
107
108 Transform transform_A;
109 Transform transform_B;
110
111 // i wonder how this could be sped up... if it can
Support0GjkEpa2::MinkowskiDiff112 _FORCE_INLINE_ Vector3 Support0 ( const Vector3& d ) const {
113 return transform_A.xform( m_shapes[0]->get_support( transform_A.basis.xform_inv(d).normalized() ) );
114 }
115
Support1GjkEpa2::MinkowskiDiff116 _FORCE_INLINE_ Vector3 Support1 ( const Vector3& d ) const {
117 return transform_B.xform( m_shapes[1]->get_support( transform_B.basis.xform_inv(d).normalized() ) );
118 }
119
SupportGjkEpa2::MinkowskiDiff120 _FORCE_INLINE_ Vector3 Support ( const Vector3& d ) const {
121 return ( Support0 ( d )-Support1 ( -d ) );
122 }
123
SupportGjkEpa2::MinkowskiDiff124 _FORCE_INLINE_ Vector3 Support ( const Vector3& d,U index ) const
125 {
126 if ( index )
127 return ( Support1 ( d ) );
128 else
129 return ( Support0 ( d ) );
130 }
131 };
132
133 typedef MinkowskiDiff tShape;
134
135
136 // GJK
137 struct GJK
138 {
139 /* Types */
140 struct sSV
141 {
142 Vector3 d,w;
143 };
144 struct sSimplex
145 {
146 sSV* c[4];
147 real_t p[4];
148 U rank;
149 };
150 struct eStatus { enum _ {
151 Valid,
152 Inside,
153 Failed };};
154 /* Fields */
155 tShape m_shape;
156 Vector3 m_ray;
157 real_t m_distance;
158 sSimplex m_simplices[2];
159 sSV m_store[4];
160 sSV* m_free[4];
161 U m_nfree;
162 U m_current;
163 sSimplex* m_simplex;
164 eStatus::_ m_status;
165 /* Methods */
GJKGjkEpa2::GJK166 GJK()
167 {
168 Initialize();
169 }
InitializeGjkEpa2::GJK170 void Initialize()
171 {
172 m_ray = Vector3(0,0,0);
173 m_nfree = 0;
174 m_status = eStatus::Failed;
175 m_current = 0;
176 m_distance = 0;
177 }
EvaluateGjkEpa2::GJK178 eStatus::_ Evaluate(const tShape& shapearg,const Vector3& guess)
179 {
180 U iterations=0;
181 real_t sqdist=0;
182 real_t alpha=0;
183 Vector3 lastw[4];
184 U clastw=0;
185 /* Initialize solver */
186 m_free[0] = &m_store[0];
187 m_free[1] = &m_store[1];
188 m_free[2] = &m_store[2];
189 m_free[3] = &m_store[3];
190 m_nfree = 4;
191 m_current = 0;
192 m_status = eStatus::Valid;
193 m_shape = shapearg;
194 m_distance = 0;
195 /* Initialize simplex */
196 m_simplices[0].rank = 0;
197 m_ray = guess;
198 const real_t sqrl= m_ray.length_squared();
199 appendvertice(m_simplices[0],sqrl>0?-m_ray:Vector3(1,0,0));
200 m_simplices[0].p[0] = 1;
201 m_ray = m_simplices[0].c[0]->w;
202 sqdist = sqrl;
203 lastw[0] =
204 lastw[1] =
205 lastw[2] =
206 lastw[3] = m_ray;
207 /* Loop */
208 do {
209 const U next=1-m_current;
210 sSimplex& cs=m_simplices[m_current];
211 sSimplex& ns=m_simplices[next];
212 /* Check zero */
213 const real_t rl=m_ray.length();
214 if(rl<GJK_MIN_DISTANCE)
215 {/* Touching or inside */
216 m_status=eStatus::Inside;
217 break;
218 }
219 /* Append new vertice in -'v' direction */
220 appendvertice(cs,-m_ray);
221 const Vector3& w=cs.c[cs.rank-1]->w;
222 bool found=false;
223 for(U i=0;i<4;++i)
224 {
225 if((w-lastw[i]).length_squared()<GJK_DUPLICATED_EPS)
226 { found=true;break; }
227 }
228 if(found)
229 {/* Return old simplex */
230 removevertice(m_simplices[m_current]);
231 break;
232 }
233 else
234 {/* Update lastw */
235 lastw[clastw=(clastw+1)&3]=w;
236 }
237 /* Check for termination */
238 const real_t omega=vec3_dot(m_ray,w)/rl;
239 alpha=MAX(omega,alpha);
240 if(((rl-alpha)-(GJK_ACCURARY*rl))<=0)
241 {/* Return old simplex */
242 removevertice(m_simplices[m_current]);
243 break;
244 }
245 /* Reduce simplex */
246 real_t weights[4];
247 U mask=0;
248 switch(cs.rank)
249 {
250 case 2: sqdist=projectorigin( cs.c[0]->w,
251 cs.c[1]->w,
252 weights,mask);break;
253 case 3: sqdist=projectorigin( cs.c[0]->w,
254 cs.c[1]->w,
255 cs.c[2]->w,
256 weights,mask);break;
257 case 4: sqdist=projectorigin( cs.c[0]->w,
258 cs.c[1]->w,
259 cs.c[2]->w,
260 cs.c[3]->w,
261 weights,mask);break;
262 }
263 if(sqdist>=0)
264 {/* Valid */
265 ns.rank = 0;
266 m_ray = Vector3(0,0,0);
267 m_current = next;
268 for(U i=0,ni=cs.rank;i<ni;++i)
269 {
270 if(mask&(1<<i))
271 {
272 ns.c[ns.rank] = cs.c[i];
273 ns.p[ns.rank++] = weights[i];
274 m_ray += cs.c[i]->w*weights[i];
275 }
276 else
277 {
278 m_free[m_nfree++] = cs.c[i];
279 }
280 }
281 if(mask==15) m_status=eStatus::Inside;
282 }
283 else
284 {/* Return old simplex */
285 removevertice(m_simplices[m_current]);
286 break;
287 }
288 m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
289 } while(m_status==eStatus::Valid);
290 m_simplex=&m_simplices[m_current];
291 switch(m_status)
292 {
293 case eStatus::Valid: m_distance=m_ray.length();break;
294 case eStatus::Inside: m_distance=0;break;
295 default: {}
296 }
297 return(m_status);
298 }
EncloseOriginGjkEpa2::GJK299 bool EncloseOrigin()
300 {
301 switch(m_simplex->rank)
302 {
303 case 1:
304 {
305 for(U i=0;i<3;++i)
306 {
307 Vector3 axis=Vector3(0,0,0);
308 axis[i]=1;
309 appendvertice(*m_simplex, axis);
310 if(EncloseOrigin()) return(true);
311 removevertice(*m_simplex);
312 appendvertice(*m_simplex,-axis);
313 if(EncloseOrigin()) return(true);
314 removevertice(*m_simplex);
315 }
316 }
317 break;
318 case 2:
319 {
320 const Vector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w;
321 for(U i=0;i<3;++i)
322 {
323 Vector3 axis=Vector3(0,0,0);
324 axis[i]=1;
325 const Vector3 p=vec3_cross(d,axis);
326 if(p.length_squared()>0)
327 {
328 appendvertice(*m_simplex, p);
329 if(EncloseOrigin()) return(true);
330 removevertice(*m_simplex);
331 appendvertice(*m_simplex,-p);
332 if(EncloseOrigin()) return(true);
333 removevertice(*m_simplex);
334 }
335 }
336 }
337 break;
338 case 3:
339 {
340 const Vector3 n=vec3_cross(m_simplex->c[1]->w-m_simplex->c[0]->w,
341 m_simplex->c[2]->w-m_simplex->c[0]->w);
342 if(n.length_squared()>0)
343 {
344 appendvertice(*m_simplex,n);
345 if(EncloseOrigin()) return(true);
346 removevertice(*m_simplex);
347 appendvertice(*m_simplex,-n);
348 if(EncloseOrigin()) return(true);
349 removevertice(*m_simplex);
350 }
351 }
352 break;
353 case 4:
354 {
355 if(Math::abs(det( m_simplex->c[0]->w-m_simplex->c[3]->w,
356 m_simplex->c[1]->w-m_simplex->c[3]->w,
357 m_simplex->c[2]->w-m_simplex->c[3]->w))>0)
358 return(true);
359 }
360 break;
361 }
362 return(false);
363 }
364 /* Internals */
getsupportGjkEpa2::GJK365 void getsupport(const Vector3& d,sSV& sv) const
366 {
367 sv.d = d/d.length();
368 sv.w = m_shape.Support(sv.d);
369 }
removeverticeGjkEpa2::GJK370 void removevertice(sSimplex& simplex)
371 {
372 m_free[m_nfree++]=simplex.c[--simplex.rank];
373 }
appendverticeGjkEpa2::GJK374 void appendvertice(sSimplex& simplex,const Vector3& v)
375 {
376 simplex.p[simplex.rank]=0;
377 simplex.c[simplex.rank]=m_free[--m_nfree];
378 getsupport(v,*simplex.c[simplex.rank++]);
379 }
detGjkEpa2::GJK380 static real_t det(const Vector3& a,const Vector3& b,const Vector3& c)
381 {
382 return( a.y*b.z*c.x+a.z*b.x*c.y-
383 a.x*b.z*c.y-a.y*b.x*c.z+
384 a.x*b.y*c.z-a.z*b.y*c.x);
385 }
projectoriginGjkEpa2::GJK386 static real_t projectorigin( const Vector3& a,
387 const Vector3& b,
388 real_t* w,U& m)
389 {
390 const Vector3 d=b-a;
391 const real_t l=d.length_squared();
392 if(l>GJK_SIMPLEX2_EPS)
393 {
394 const real_t t(l>0?-vec3_dot(a,d)/l:0);
395 if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length_squared()); }
396 else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length_squared()); }
397 else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length_squared()); }
398 }
399 return(-1);
400 }
projectoriginGjkEpa2::GJK401 static real_t projectorigin( const Vector3& a,
402 const Vector3& b,
403 const Vector3& c,
404 real_t* w,U& m)
405 {
406 static const U imd3[]={1,2,0};
407 const Vector3* vt[]={&a,&b,&c};
408 const Vector3 dl[]={a-b,b-c,c-a};
409 const Vector3 n=vec3_cross(dl[0],dl[1]);
410 const real_t l=n.length_squared();
411 if(l>GJK_SIMPLEX3_EPS)
412 {
413 real_t mindist=-1;
414 real_t subw[2] = { 0 , 0};
415 U subm = 0;
416 for(U i=0;i<3;++i)
417 {
418 if(vec3_dot(*vt[i],vec3_cross(dl[i],n))>0)
419 {
420 const U j=imd3[i];
421 const real_t subd(projectorigin(*vt[i],*vt[j],subw,subm));
422 if((mindist<0)||(subd<mindist))
423 {
424 mindist = subd;
425 m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
426 w[i] = subw[0];
427 w[j] = subw[1];
428 w[imd3[j]] = 0;
429 }
430 }
431 }
432 if(mindist<0)
433 {
434 const real_t d=vec3_dot(a,n);
435 const real_t s=Math::sqrt(l);
436 const Vector3 p=n*(d/l);
437 mindist = p.length_squared();
438 m = 7;
439 w[0] = (vec3_cross(dl[1],b-p)).length()/s;
440 w[1] = (vec3_cross(dl[2],c-p)).length()/s;
441 w[2] = 1-(w[0]+w[1]);
442 }
443 return(mindist);
444 }
445 return(-1);
446 }
projectoriginGjkEpa2::GJK447 static real_t projectorigin( const Vector3& a,
448 const Vector3& b,
449 const Vector3& c,
450 const Vector3& d,
451 real_t* w,U& m)
452 {
453 static const U imd3[]={1,2,0};
454 const Vector3* vt[]={&a,&b,&c,&d};
455 const Vector3 dl[]={a-d,b-d,c-d};
456 const real_t vl=det(dl[0],dl[1],dl[2]);
457 const bool ng=(vl*vec3_dot(a,vec3_cross(b-c,a-b)))<=0;
458 if(ng&&(Math::abs(vl)>GJK_SIMPLEX4_EPS))
459 {
460 real_t mindist=-1;
461 real_t subw[3];
462 U subm=0;
463 for(U i=0;i<3;++i)
464 {
465 const U j=imd3[i];
466 const real_t s=vl*vec3_dot(d,vec3_cross(dl[i],dl[j]));
467 if(s>0)
468 {
469 const real_t subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
470 if((mindist<0)||(subd<mindist))
471 {
472 mindist = subd;
473 m = static_cast<U>((subm&1?1<<i:0)+
474 (subm&2?1<<j:0)+
475 (subm&4?8:0));
476 w[i] = subw[0];
477 w[j] = subw[1];
478 w[imd3[j]] = 0;
479 w[3] = subw[2];
480 }
481 }
482 }
483 if(mindist<0)
484 {
485 mindist = 0;
486 m = 15;
487 w[0] = det(c,b,d)/vl;
488 w[1] = det(a,c,d)/vl;
489 w[2] = det(b,a,d)/vl;
490 w[3] = 1-(w[0]+w[1]+w[2]);
491 }
492 return(mindist);
493 }
494 return(-1);
495 }
496 };
497
498 // EPA
499 struct EPA
500 {
501 /* Types */
502 typedef GJK::sSV sSV;
503 struct sFace
504 {
505 Vector3 n;
506 real_t d;
507 real_t p;
508 sSV* c[3];
509 sFace* f[3];
510 sFace* l[2];
511 U1 e[3];
512 U1 pass;
513 };
514 struct sList
515 {
516 sFace* root;
517 U count;
sListGjkEpa2::EPA::sList518 sList() : root(0),count(0) {}
519 };
520 struct sHorizon
521 {
522 sFace* cf;
523 sFace* ff;
524 U nf;
sHorizonGjkEpa2::EPA::sHorizon525 sHorizon() : cf(0),ff(0),nf(0) {}
526 };
527 struct eStatus { enum _ {
528 Valid,
529 Touching,
530 Degenerated,
531 NonConvex,
532 InvalidHull,
533 OutOfFaces,
534 OutOfVertices,
535 AccuraryReached,
536 FallBack,
537 Failed };};
538 /* Fields */
539 eStatus::_ m_status;
540 GJK::sSimplex m_result;
541 Vector3 m_normal;
542 real_t m_depth;
543 sSV m_sv_store[EPA_MAX_VERTICES];
544 sFace m_fc_store[EPA_MAX_FACES];
545 U m_nextsv;
546 sList m_hull;
547 sList m_stock;
548 /* Methods */
EPAGjkEpa2::EPA549 EPA()
550 {
551 Initialize();
552 }
553
554
bindGjkEpa2::EPA555 static inline void bind(sFace* fa,U ea,sFace* fb,U eb)
556 {
557 fa->e[ea]=(U1)eb;fa->f[ea]=fb;
558 fb->e[eb]=(U1)ea;fb->f[eb]=fa;
559 }
appendGjkEpa2::EPA560 static inline void append(sList& list,sFace* face)
561 {
562 face->l[0] = 0;
563 face->l[1] = list.root;
564 if(list.root) list.root->l[0]=face;
565 list.root = face;
566 ++list.count;
567 }
removeGjkEpa2::EPA568 static inline void remove(sList& list,sFace* face)
569 {
570 if(face->l[1]) face->l[1]->l[0]=face->l[0];
571 if(face->l[0]) face->l[0]->l[1]=face->l[1];
572 if(face==list.root) list.root=face->l[1];
573 --list.count;
574 }
575
576
InitializeGjkEpa2::EPA577 void Initialize()
578 {
579 m_status = eStatus::Failed;
580 m_normal = Vector3(0,0,0);
581 m_depth = 0;
582 m_nextsv = 0;
583 for(U i=0;i<EPA_MAX_FACES;++i)
584 {
585 append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
586 }
587 }
EvaluateGjkEpa2::EPA588 eStatus::_ Evaluate(GJK& gjk,const Vector3& guess)
589 {
590 GJK::sSimplex& simplex=*gjk.m_simplex;
591 if((simplex.rank>1)&&gjk.EncloseOrigin())
592 {
593
594 /* Clean up */
595 while(m_hull.root)
596 {
597 sFace* f = m_hull.root;
598 remove(m_hull,f);
599 append(m_stock,f);
600 }
601 m_status = eStatus::Valid;
602 m_nextsv = 0;
603 /* Orient simplex */
604 if(gjk.det( simplex.c[0]->w-simplex.c[3]->w,
605 simplex.c[1]->w-simplex.c[3]->w,
606 simplex.c[2]->w-simplex.c[3]->w)<0)
607 {
608 SWAP(simplex.c[0],simplex.c[1]);
609 SWAP(simplex.p[0],simplex.p[1]);
610 }
611 /* Build initial hull */
612 sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
613 newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
614 newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
615 newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
616 if(m_hull.count==4)
617 {
618 sFace* best=findbest();
619 sFace outer=*best;
620 U pass=0;
621 U iterations=0;
622 bind(tetra[0],0,tetra[1],0);
623 bind(tetra[0],1,tetra[2],0);
624 bind(tetra[0],2,tetra[3],0);
625 bind(tetra[1],1,tetra[3],2);
626 bind(tetra[1],2,tetra[2],1);
627 bind(tetra[2],2,tetra[3],1);
628 m_status=eStatus::Valid;
629 for(;iterations<EPA_MAX_ITERATIONS;++iterations)
630 {
631 if(m_nextsv<EPA_MAX_VERTICES)
632 {
633 sHorizon horizon;
634 sSV* w=&m_sv_store[m_nextsv++];
635 bool valid=true;
636 best->pass = (U1)(++pass);
637 gjk.getsupport(best->n,*w);
638 const real_t wdist=vec3_dot(best->n,w->w)-best->d;
639 if(wdist>EPA_ACCURACY)
640 {
641 for(U j=0;(j<3)&&valid;++j)
642 {
643 valid&=expand( pass,w,
644 best->f[j],best->e[j],
645 horizon);
646 }
647 if(valid&&(horizon.nf>=3))
648 {
649 bind(horizon.cf,1,horizon.ff,2);
650 remove(m_hull,best);
651 append(m_stock,best);
652 best=findbest();
653 if(best->p>=outer.p) outer=*best;
654 } else { m_status=eStatus::InvalidHull;break; }
655 } else { m_status=eStatus::AccuraryReached;break; }
656 } else { m_status=eStatus::OutOfVertices;break; }
657 }
658 const Vector3 projection=outer.n*outer.d;
659 m_normal = outer.n;
660 m_depth = outer.d;
661 m_result.rank = 3;
662 m_result.c[0] = outer.c[0];
663 m_result.c[1] = outer.c[1];
664 m_result.c[2] = outer.c[2];
665 m_result.p[0] = vec3_cross( outer.c[1]->w-projection,
666 outer.c[2]->w-projection).length();
667 m_result.p[1] = vec3_cross( outer.c[2]->w-projection,
668 outer.c[0]->w-projection).length();
669 m_result.p[2] = vec3_cross( outer.c[0]->w-projection,
670 outer.c[1]->w-projection).length();
671 const real_t sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
672 m_result.p[0] /= sum;
673 m_result.p[1] /= sum;
674 m_result.p[2] /= sum;
675 return(m_status);
676 }
677 }
678 /* Fallback */
679 m_status = eStatus::FallBack;
680 m_normal = -guess;
681 const real_t nl=m_normal.length();
682 if(nl>0)
683 m_normal = m_normal/nl;
684 else
685 m_normal = Vector3(1,0,0);
686 m_depth = 0;
687 m_result.rank=1;
688 m_result.c[0]=simplex.c[0];
689 m_result.p[0]=1;
690 return(m_status);
691 }
newfaceGjkEpa2::EPA692 sFace* newface(sSV* a,sSV* b,sSV* c,bool forced)
693 {
694 if(m_stock.root)
695 {
696 sFace* face=m_stock.root;
697 remove(m_stock,face);
698 append(m_hull,face);
699 face->pass = 0;
700 face->c[0] = a;
701 face->c[1] = b;
702 face->c[2] = c;
703 face->n = vec3_cross(b->w-a->w,c->w-a->w);
704 const real_t l=face->n.length();
705 const bool v=l>EPA_ACCURACY;
706 face->p = MIN(MIN(
707 vec3_dot(a->w,vec3_cross(face->n,a->w-b->w)),
708 vec3_dot(b->w,vec3_cross(face->n,b->w-c->w))),
709 vec3_dot(c->w,vec3_cross(face->n,c->w-a->w))) /
710 (v?l:1);
711 face->p = face->p>=-EPA_INSIDE_EPS?0:face->p;
712 if(v)
713 {
714 face->d = vec3_dot(a->w,face->n)/l;
715 face->n /= l;
716 if(forced||(face->d>=-EPA_PLANE_EPS))
717 {
718 return(face);
719 } else m_status=eStatus::NonConvex;
720 } else m_status=eStatus::Degenerated;
721 remove(m_hull,face);
722 append(m_stock,face);
723 return(0);
724 }
725 // -- GODOT start --
726 //m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces;
727 m_status=eStatus::OutOfFaces;
728 // -- GODOT end --
729 return(0);
730 }
findbestGjkEpa2::EPA731 sFace* findbest()
732 {
733 sFace* minf=m_hull.root;
734 real_t mind=minf->d*minf->d;
735 real_t maxp=minf->p;
736 for(sFace* f=minf->l[1];f;f=f->l[1])
737 {
738 const real_t sqd=f->d*f->d;
739 if((f->p>=maxp)&&(sqd<mind))
740 {
741 minf=f;
742 mind=sqd;
743 maxp=f->p;
744 }
745 }
746 return(minf);
747 }
expandGjkEpa2::EPA748 bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
749 {
750 static const U i1m3[]={1,2,0};
751 static const U i2m3[]={2,0,1};
752 if(f->pass!=pass)
753 {
754 const U e1=i1m3[e];
755 if((vec3_dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
756 {
757 sFace* nf=newface(f->c[e1],f->c[e],w,false);
758 if(nf)
759 {
760 bind(nf,0,f,e);
761 if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf;
762 horizon.cf=nf;
763 ++horizon.nf;
764 return(true);
765 }
766 }
767 else
768 {
769 const U e2=i2m3[e];
770 f->pass = (U1)pass;
771 if( expand(pass,w,f->f[e1],f->e[e1],horizon)&&
772 expand(pass,w,f->f[e2],f->e[e2],horizon))
773 {
774 remove(m_hull,f);
775 append(m_stock,f);
776 return(true);
777 }
778 }
779 }
780 return(false);
781 }
782
783 };
784
785 //
Initialize(const ShapeSW * shape0,const Transform & wtrs0,const ShapeSW * shape1,const Transform & wtrs1,sResults & results,tShape & shape,bool withmargins)786 static void Initialize( const ShapeSW* shape0,const Transform& wtrs0,
787 const ShapeSW* shape1,const Transform& wtrs1,
788 sResults& results,
789 tShape& shape,
790 bool withmargins)
791 {
792 /* Results */
793 results.witnesses[0] =
794 results.witnesses[1] = Vector3(0,0,0);
795 results.status = sResults::Separated;
796 /* Shape */
797 shape.m_shapes[0] = shape0;
798 shape.m_shapes[1] = shape1;
799 shape.transform_A = wtrs0;
800 shape.transform_B = wtrs1;
801
802 }
803
804
805
806 //
807 // Api
808 //
809
810 //
811
812 //
Distance(const ShapeSW * shape0,const Transform & wtrs0,const ShapeSW * shape1,const Transform & wtrs1,const Vector3 & guess,sResults & results)813 bool Distance( const ShapeSW* shape0,
814 const Transform& wtrs0,
815 const ShapeSW* shape1,
816 const Transform& wtrs1,
817 const Vector3& guess,
818 sResults& results)
819 {
820 tShape shape;
821 Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
822 GJK gjk;
823 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
824 if(gjk_status==GJK::eStatus::Valid)
825 {
826 Vector3 w0=Vector3(0,0,0);
827 Vector3 w1=Vector3(0,0,0);
828 for(U i=0;i<gjk.m_simplex->rank;++i)
829 {
830 const real_t p=gjk.m_simplex->p[i];
831 w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
832 w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
833 }
834 results.witnesses[0] = w0;
835 results.witnesses[1] = w1;
836 results.normal = w0-w1;
837 results.distance = results.normal.length();
838 results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1;
839 return(true);
840 }
841 else
842 {
843 results.status = gjk_status==GJK::eStatus::Inside?
844 sResults::Penetrating :
845 sResults::GJK_Failed ;
846 return(false);
847 }
848 }
849
850 //
Penetration(const ShapeSW * shape0,const Transform & wtrs0,const ShapeSW * shape1,const Transform & wtrs1,const Vector3 & guess,sResults & results)851 bool Penetration( const ShapeSW* shape0,
852 const Transform& wtrs0,
853 const ShapeSW* shape1,
854 const Transform& wtrs1,
855 const Vector3& guess,
856 sResults& results
857 )
858 {
859 tShape shape;
860 Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false);
861 GJK gjk;
862 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess);
863 switch(gjk_status)
864 {
865 case GJK::eStatus::Inside:
866 {
867 EPA epa;
868 EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess);
869 if(epa_status!=EPA::eStatus::Failed)
870 {
871 Vector3 w0=Vector3(0,0,0);
872 for(U i=0;i<epa.m_result.rank;++i)
873 {
874 w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
875 }
876 results.status = sResults::Penetrating;
877 results.witnesses[0] = w0;
878 results.witnesses[1] = w0-epa.m_normal*epa.m_depth;
879 results.normal = -epa.m_normal;
880 results.distance = -epa.m_depth;
881 return(true);
882 } else results.status=sResults::EPA_Failed;
883 }
884 break;
885 case GJK::eStatus::Failed:
886 results.status=sResults::GJK_Failed;
887 break;
888 default: {}
889 }
890 return(false);
891 }
892
893
894 /* Symbols cleanup */
895
896 #undef GJK_MAX_ITERATIONS
897 #undef GJK_ACCURARY
898 #undef GJK_MIN_DISTANCE
899 #undef GJK_DUPLICATED_EPS
900 #undef GJK_SIMPLEX2_EPS
901 #undef GJK_SIMPLEX3_EPS
902 #undef GJK_SIMPLEX4_EPS
903
904 #undef EPA_MAX_VERTICES
905 #undef EPA_MAX_FACES
906 #undef EPA_MAX_ITERATIONS
907 #undef EPA_ACCURACY
908 #undef EPA_FALLBACK
909 #undef EPA_PLANE_EPS
910 #undef EPA_INSIDE_EPS
911
912
913 } // end of namespace
914
915 /* clang-format on */
916
gjk_epa_calculate_distance(const ShapeSW * p_shape_A,const Transform & p_transform_A,const ShapeSW * p_shape_B,const Transform & p_transform_B,Vector3 & r_result_A,Vector3 & r_result_B)917 bool gjk_epa_calculate_distance(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B) {
918
919 GjkEpa2::sResults res;
920
921 if (GjkEpa2::Distance(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_transform_B.origin - p_transform_A.origin, res)) {
922
923 r_result_A = res.witnesses[0];
924 r_result_B = res.witnesses[1];
925 return true;
926 }
927
928 return false;
929 }
930
gjk_epa_calculate_penetration(const ShapeSW * p_shape_A,const Transform & p_transform_A,const ShapeSW * p_shape_B,const Transform & p_transform_B,CollisionSolverSW::CallbackResult p_result_callback,void * p_userdata,bool p_swap)931 bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform &p_transform_A, const ShapeSW *p_shape_B, const Transform &p_transform_B, CollisionSolverSW::CallbackResult p_result_callback, void *p_userdata, bool p_swap) {
932
933 GjkEpa2::sResults res;
934
935 if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_transform_B.origin - p_transform_A.origin, res)) {
936 if (p_result_callback) {
937 if (p_swap)
938 p_result_callback(res.witnesses[1], res.witnesses[0], p_userdata);
939 else
940 p_result_callback(res.witnesses[0], res.witnesses[1], p_userdata);
941 }
942 return true;
943 }
944
945 return false;
946 }
947