1/**************************************************************************/
2/* gjk_epa.cpp */
3/**************************************************************************/
4/* This file is part of: */
5/* GODOT ENGINE */
6/* https://godotengine.org */
7/**************************************************************************/
8/* Copyright (c) 2014-present Godot Engine contributors (see AUTHORS.md). */
9/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */
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/*
39Bullet Continuous Collision Detection and Physics Library
40Copyright (c) 2003-2008 Erwin Coumans http://continuousphysics.com/Bullet/
41
42This software is provided 'as-is', without any express or implied warranty.
43In no event will the authors be held liable for any damages arising from the
44use of this software.
45Permission is granted to anyone to use this software for any purpose,
46including commercial applications, and to alter it and redistribute it
47freely,
48subject to the following restrictions:
49
501. The origin of this software must not be misrepresented; you must not
51claim that you wrote the original software. If you use this software in a
52product, an acknowledgment in the product documentation would be appreciated
53but is not required.
542. Altered source versions must be plainly marked as such, and must not be
55misrepresented as being the original software.
563. This notice may not be removed or altered from any source distribution.
57*/
58
59/*
60GJK-EPA collision solver by Nathanael Presson, 2008
61*/
62
63 // Config
64
65/* GJK */
66#define GJK_MAX_ITERATIONS 128
67#define GJK_ACCURACY ((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 128
76#define EPA_MAX_FACES (EPA_MAX_VERTICES*2)
77#define EPA_MAX_ITERATIONS 255
78// -- GODOT start --
79//#define EPA_ACCURACY ((real_t)0.0001)
80#define EPA_ACCURACY ((real_t)0.00001)
81// -- GODOT end --
82#define EPA_FALLBACK (10*EPA_ACCURACY)
83#define EPA_PLANE_EPS ((real_t)0.00001)
84#define EPA_INSIDE_EPS ((real_t)0.01)
85
86namespace GjkEpa2 {
87
88
89struct sResults {
90 enum eStatus {
91 Separated, /* Shapes doesn't penetrate */
92 Penetrating, /* Shapes are penetrating */
93 GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */
94 EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */
95 } status;
96
97 Vector3 witnesses[2];
98 Vector3 normal;
99 real_t distance = 0.0;
100};
101
102// Shorthands
103typedef unsigned int U;
104typedef unsigned char U1;
105
106// MinkowskiDiff
107struct MinkowskiDiff {
108 const GodotShape3D* m_shapes[2];
109
110 Transform3D transform_A;
111 Transform3D transform_B;
112
113 real_t margin_A = 0.0;
114 real_t margin_B = 0.0;
115
116 Vector3 (*get_support)(const GodotShape3D*, const Vector3&, real_t) = nullptr;
117
118 void Initialize(const GodotShape3D* shape0, const Transform3D& wtrs0, const real_t margin0,
119 const GodotShape3D* shape1, const Transform3D& wtrs1, const real_t margin1) {
120 m_shapes[0] = shape0;
121 m_shapes[1] = shape1;
122 transform_A = wtrs0;
123 transform_B = wtrs1;
124 margin_A = margin0;
125 margin_B = margin1;
126
127 if ((margin0 > 0.0) || (margin1 > 0.0)) {
128 get_support = get_support_with_margin;
129 } else {
130 get_support = get_support_without_margin;
131 }
132 }
133
134 static Vector3 get_support_without_margin(const GodotShape3D* p_shape, const Vector3& p_dir, real_t p_margin) {
135 return p_shape->get_support(p_dir.normalized());
136 }
137
138 static Vector3 get_support_with_margin(const GodotShape3D* p_shape, const Vector3& p_dir, real_t p_margin) {
139 Vector3 local_dir_norm = p_dir;
140 if (local_dir_norm.length_squared() < CMP_EPSILON2) {
141 local_dir_norm = Vector3(-1.0, -1.0, -1.0);
142 }
143 local_dir_norm.normalize();
144
145 return p_shape->get_support(local_dir_norm) + p_margin * local_dir_norm;
146 }
147
148 // i wonder how this could be sped up... if it can
149 _FORCE_INLINE_ Vector3 Support0(const Vector3& d) const {
150 return transform_A.xform(get_support(m_shapes[0], transform_A.basis.xform_inv(d), margin_A));
151 }
152
153 _FORCE_INLINE_ Vector3 Support1(const Vector3& d) const {
154 return transform_B.xform(get_support(m_shapes[1], transform_B.basis.xform_inv(d), margin_B));
155 }
156
157 _FORCE_INLINE_ Vector3 Support (const Vector3& d) const {
158 return (Support0(d) - Support1(-d));
159 }
160
161 _FORCE_INLINE_ Vector3 Support(const Vector3& d, U index) const {
162 if (index) {
163 return Support1(d);
164 } else {
165 return Support0(d);
166 }
167 }
168};
169
170typedef MinkowskiDiff tShape;
171
172
173// GJK
174struct GJK
175{
176 /* Types */
177 struct sSV
178 {
179 Vector3 d,w;
180 };
181 struct sSimplex
182 {
183 sSV* c[4];
184 real_t p[4];
185 U rank;
186 };
187 struct eStatus { enum _ {
188 Valid,
189 Inside,
190 Failed };};
191 /* Fields */
192 tShape m_shape;
193 Vector3 m_ray;
194 real_t m_distance = 0.0f;
195 sSimplex m_simplices[2];
196 sSV m_store[4];
197 sSV* m_free[4];
198 U m_nfree = 0;
199 U m_current = 0;
200 sSimplex* m_simplex = nullptr;
201 eStatus::_ m_status;
202 /* Methods */
203 GJK()
204 {
205 Initialize();
206 }
207 void Initialize()
208 {
209 m_ray = Vector3(0,0,0);
210 m_nfree = 0;
211 m_status = eStatus::Failed;
212 m_current = 0;
213 m_distance = 0;
214 }
215 eStatus::_ Evaluate(const tShape& shapearg,const Vector3& guess)
216 {
217 U iterations=0;
218 real_t sqdist=0;
219 real_t alpha=0;
220 Vector3 lastw[4];
221 U clastw=0;
222 /* Initialize solver */
223 m_free[0] = &m_store[0];
224 m_free[1] = &m_store[1];
225 m_free[2] = &m_store[2];
226 m_free[3] = &m_store[3];
227 m_nfree = 4;
228 m_current = 0;
229 m_status = eStatus::Valid;
230 m_shape = shapearg;
231 m_distance = 0;
232 /* Initialize simplex */
233 m_simplices[0].rank = 0;
234 m_ray = guess;
235 const real_t sqrl= m_ray.length_squared();
236 appendvertice(m_simplices[0],sqrl>0?-m_ray:Vector3(1,0,0));
237 m_simplices[0].p[0] = 1;
238 m_ray = m_simplices[0].c[0]->w;
239 sqdist = sqrl;
240 lastw[0] =
241 lastw[1] =
242 lastw[2] =
243 lastw[3] = m_ray;
244 /* Loop */
245 do {
246 const U next=1-m_current;
247 sSimplex& cs=m_simplices[m_current];
248 sSimplex& ns=m_simplices[next];
249 /* Check zero */
250 const real_t rl=m_ray.length();
251 if(rl<GJK_MIN_DISTANCE)
252 {/* Touching or inside */
253 m_status=eStatus::Inside;
254 break;
255 }
256 /* Append new vertice in -'v' direction */
257 appendvertice(cs,-m_ray);
258 const Vector3& w=cs.c[cs.rank-1]->w;
259 bool found=false;
260 for(U i=0;i<4;++i)
261 {
262 if((w-lastw[i]).length_squared()<GJK_DUPLICATED_EPS)
263 { found=true;break; }
264 }
265 if(found)
266 {/* Return old simplex */
267 removevertice(m_simplices[m_current]);
268 break;
269 }
270 else
271 {/* Update lastw */
272 lastw[clastw=(clastw+1)&3]=w;
273 }
274 /* Check for termination */
275 const real_t omega=vec3_dot(m_ray,w)/rl;
276 alpha=MAX(omega,alpha);
277 if(((rl-alpha)-(GJK_ACCURACY*rl))<=0)
278 {/* Return old simplex */
279 removevertice(m_simplices[m_current]);
280 break;
281 }
282 /* Reduce simplex */
283 real_t weights[4];
284 U mask=0;
285 switch(cs.rank)
286 {
287 case 2: sqdist=projectorigin( cs.c[0]->w,
288 cs.c[1]->w,
289 weights,mask);break;
290 case 3: sqdist=projectorigin( cs.c[0]->w,
291 cs.c[1]->w,
292 cs.c[2]->w,
293 weights,mask);break;
294 case 4: sqdist=projectorigin( cs.c[0]->w,
295 cs.c[1]->w,
296 cs.c[2]->w,
297 cs.c[3]->w,
298 weights,mask);break;
299 }
300 if(sqdist>=0)
301 {/* Valid */
302 ns.rank = 0;
303 m_ray = Vector3(0,0,0);
304 m_current = next;
305 for(U i=0,ni=cs.rank;i<ni;++i)
306 {
307 if(mask&(1<<i))
308 {
309 ns.c[ns.rank] = cs.c[i];
310 ns.p[ns.rank++] = weights[i];
311 m_ray += cs.c[i]->w*weights[i];
312 }
313 else
314 {
315 m_free[m_nfree++] = cs.c[i];
316 }
317 }
318 if(mask==15) { m_status=eStatus::Inside;
319}
320 }
321 else
322 {/* Return old simplex */
323 removevertice(m_simplices[m_current]);
324 break;
325 }
326 m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed;
327 } while(m_status==eStatus::Valid);
328 m_simplex=&m_simplices[m_current];
329 switch(m_status)
330 {
331 case eStatus::Valid: m_distance=m_ray.length();break;
332 case eStatus::Inside: m_distance=0;break;
333 default: {}
334 }
335 return(m_status);
336 }
337 bool EncloseOrigin()
338 {
339 switch(m_simplex->rank)
340 {
341 case 1:
342 {
343 for(U i=0;i<3;++i)
344 {
345 Vector3 axis=Vector3(0,0,0);
346 axis[i]=1;
347 appendvertice(*m_simplex, axis);
348 if(EncloseOrigin()) { return(true);
349}
350 removevertice(*m_simplex);
351 appendvertice(*m_simplex,-axis);
352 if(EncloseOrigin()) { return(true);
353}
354 removevertice(*m_simplex);
355 }
356 }
357 break;
358 case 2:
359 {
360 const Vector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w;
361 for(U i=0;i<3;++i)
362 {
363 Vector3 axis=Vector3(0,0,0);
364 axis[i]=1;
365 const Vector3 p=vec3_cross(d,axis);
366 if(p.length_squared()>0)
367 {
368 appendvertice(*m_simplex, p);
369 if(EncloseOrigin()) { return(true);
370}
371 removevertice(*m_simplex);
372 appendvertice(*m_simplex,-p);
373 if(EncloseOrigin()) { return(true);
374}
375 removevertice(*m_simplex);
376 }
377 }
378 }
379 break;
380 case 3:
381 {
382 const Vector3 n=vec3_cross(m_simplex->c[1]->w-m_simplex->c[0]->w,
383 m_simplex->c[2]->w-m_simplex->c[0]->w);
384 if(n.length_squared()>0)
385 {
386 appendvertice(*m_simplex,n);
387 if(EncloseOrigin()) { return(true);
388}
389 removevertice(*m_simplex);
390 appendvertice(*m_simplex,-n);
391 if(EncloseOrigin()) { return(true);
392}
393 removevertice(*m_simplex);
394 }
395 }
396 break;
397 case 4:
398 {
399 if(Math::abs(det( m_simplex->c[0]->w-m_simplex->c[3]->w,
400 m_simplex->c[1]->w-m_simplex->c[3]->w,
401 m_simplex->c[2]->w-m_simplex->c[3]->w))>0) {
402 return(true);
403}
404 }
405 break;
406 }
407 return(false);
408 }
409 /* Internals */
410 void getsupport(const Vector3& d,sSV& sv) const
411 {
412 sv.d = d/d.length();
413 sv.w = m_shape.Support(sv.d);
414 }
415 void removevertice(sSimplex& simplex)
416 {
417 m_free[m_nfree++]=simplex.c[--simplex.rank];
418 }
419 void appendvertice(sSimplex& simplex,const Vector3& v)
420 {
421 simplex.p[simplex.rank]=0;
422 simplex.c[simplex.rank]=m_free[--m_nfree];
423 getsupport(v,*simplex.c[simplex.rank++]);
424 }
425 static real_t det(const Vector3& a,const Vector3& b,const Vector3& c)
426 {
427 return( a.y*b.z*c.x+a.z*b.x*c.y-
428 a.x*b.z*c.y-a.y*b.x*c.z+
429 a.x*b.y*c.z-a.z*b.y*c.x);
430 }
431 static real_t projectorigin( const Vector3& a,
432 const Vector3& b,
433 real_t* w,U& m)
434 {
435 const Vector3 d=b-a;
436 const real_t l=d.length_squared();
437 if(l>GJK_SIMPLEX2_EPS)
438 {
439 const real_t t(l>0?-vec3_dot(a,d)/l:0);
440 if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length_squared()); }
441 else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length_squared()); }
442 else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length_squared()); }
443 }
444 return(-1);
445 }
446 static real_t projectorigin( const Vector3& a,
447 const Vector3& b,
448 const Vector3& c,
449 real_t* w,U& m)
450 {
451 static const U imd3[]={1,2,0};
452 const Vector3* vt[]={&a,&b,&c};
453 const Vector3 dl[]={a-b,b-c,c-a};
454 const Vector3 n=vec3_cross(dl[0],dl[1]);
455 const real_t l=n.length_squared();
456 if(l>GJK_SIMPLEX3_EPS)
457 {
458 real_t mindist=-1;
459 real_t subw[2] = { 0 , 0};
460 U subm = 0;
461 for(U i=0;i<3;++i)
462 {
463 if(vec3_dot(*vt[i],vec3_cross(dl[i],n))>0)
464 {
465 const U j=imd3[i];
466 const real_t subd(projectorigin(*vt[i],*vt[j],subw,subm));
467 if((mindist<0)||(subd<mindist))
468 {
469 mindist = subd;
470 m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0));
471 w[i] = subw[0];
472 w[j] = subw[1];
473 w[imd3[j]] = 0;
474 }
475 }
476 }
477 if(mindist<0)
478 {
479 const real_t d=vec3_dot(a,n);
480 const real_t s=Math::sqrt(l);
481 const Vector3 p=n*(d/l);
482 mindist = p.length_squared();
483 m = 7;
484 w[0] = (vec3_cross(dl[1],b-p)).length()/s;
485 w[1] = (vec3_cross(dl[2],c-p)).length()/s;
486 w[2] = 1-(w[0]+w[1]);
487 }
488 return(mindist);
489 }
490 return(-1);
491 }
492 static real_t projectorigin( const Vector3& a,
493 const Vector3& b,
494 const Vector3& c,
495 const Vector3& d,
496 real_t* w,U& m)
497 {
498 static const U imd3[]={1,2,0};
499 const Vector3* vt[]={&a,&b,&c,&d};
500 const Vector3 dl[]={a-d,b-d,c-d};
501 const real_t vl=det(dl[0],dl[1],dl[2]);
502 const bool ng=(vl*vec3_dot(a,vec3_cross(b-c,a-b)))<=0;
503 if(ng&&(Math::abs(vl)>GJK_SIMPLEX4_EPS))
504 {
505 real_t mindist=-1;
506 real_t subw[3] = {0.f, 0.f, 0.f};
507 U subm=0;
508 for(U i=0;i<3;++i)
509 {
510 const U j=imd3[i];
511 const real_t s=vl*vec3_dot(d,vec3_cross(dl[i],dl[j]));
512 if(s>0)
513 {
514 const real_t subd=projectorigin(*vt[i],*vt[j],d,subw,subm);
515 if((mindist<0)||(subd<mindist))
516 {
517 mindist = subd;
518 m = static_cast<U>((subm&1?1<<i:0)+
519 (subm&2?1<<j:0)+
520 (subm&4?8:0));
521 w[i] = subw[0];
522 w[j] = subw[1];
523 w[imd3[j]] = 0;
524 w[3] = subw[2];
525 }
526 }
527 }
528 if(mindist<0)
529 {
530 mindist = 0;
531 m = 15;
532 w[0] = det(c,b,d)/vl;
533 w[1] = det(a,c,d)/vl;
534 w[2] = det(b,a,d)/vl;
535 w[3] = 1-(w[0]+w[1]+w[2]);
536 }
537 return(mindist);
538 }
539 return(-1);
540 }
541};
542
543 // EPA
544 struct EPA
545 {
546 /* Types */
547 typedef GJK::sSV sSV;
548 struct sFace
549 {
550 Vector3 n;
551 real_t d = 0.0f;
552 sSV* c[3];
553 sFace* f[3];
554 sFace* l[2];
555 U1 e[3];
556 U1 pass = 0;
557 };
558 struct sList
559 {
560 sFace* root = nullptr;
561 U count = 0;
562 sList() {}
563 };
564 struct sHorizon
565 {
566 sFace* cf = nullptr;
567 sFace* ff = nullptr;
568 U nf = 0;
569 sHorizon() {}
570 };
571 struct eStatus { enum _ {
572 Valid,
573 Touching,
574 Degenerated,
575 NonConvex,
576 InvalidHull,
577 OutOfFaces,
578 OutOfVertices,
579 AccuraryReached,
580 FallBack,
581 Failed };};
582 /* Fields */
583 eStatus::_ m_status;
584 GJK::sSimplex m_result;
585 Vector3 m_normal;
586 real_t m_depth = 0.0f;
587 sSV m_sv_store[EPA_MAX_VERTICES];
588 sFace m_fc_store[EPA_MAX_FACES];
589 U m_nextsv = 0;
590 sList m_hull;
591 sList m_stock;
592 /* Methods */
593 EPA()
594 {
595 Initialize();
596 }
597
598
599 static inline void bind(sFace* fa,U ea,sFace* fb,U eb)
600 {
601 fa->e[ea]=(U1)eb;fa->f[ea]=fb;
602 fb->e[eb]=(U1)ea;fb->f[eb]=fa;
603 }
604 static inline void append(sList& list,sFace* face)
605 {
606 face->l[0] = nullptr;
607 face->l[1] = list.root;
608 if(list.root) { list.root->l[0]=face;
609}
610 list.root = face;
611 ++list.count;
612 }
613 static inline void remove(sList& list,sFace* face)
614 {
615 if(face->l[1]) { face->l[1]->l[0]=face->l[0];
616}
617 if(face->l[0]) { face->l[0]->l[1]=face->l[1];
618}
619 if(face==list.root) { list.root=face->l[1];
620}
621 --list.count;
622 }
623
624
625 void Initialize()
626 {
627 m_status = eStatus::Failed;
628 m_normal = Vector3(0,0,0);
629 m_depth = 0;
630 m_nextsv = 0;
631 for(U i=0;i<EPA_MAX_FACES;++i)
632 {
633 append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]);
634 }
635 }
636 eStatus::_ Evaluate(GJK& gjk,const Vector3& guess)
637 {
638 GJK::sSimplex& simplex=*gjk.m_simplex;
639 if((simplex.rank>1)&&gjk.EncloseOrigin())
640 {
641 /* Clean up */
642 while(m_hull.root)
643 {
644 sFace* f = m_hull.root;
645 remove(m_hull,f);
646 append(m_stock,f);
647 }
648 m_status = eStatus::Valid;
649 m_nextsv = 0;
650 /* Orient simplex */
651 if(gjk.det( simplex.c[0]->w-simplex.c[3]->w,
652 simplex.c[1]->w-simplex.c[3]->w,
653 simplex.c[2]->w-simplex.c[3]->w)<0)
654 {
655 SWAP(simplex.c[0],simplex.c[1]);
656 SWAP(simplex.p[0],simplex.p[1]);
657 }
658 /* Build initial hull */
659 sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true),
660 newface(simplex.c[1],simplex.c[0],simplex.c[3],true),
661 newface(simplex.c[2],simplex.c[1],simplex.c[3],true),
662 newface(simplex.c[0],simplex.c[2],simplex.c[3],true)};
663 if(m_hull.count==4)
664 {
665 sFace* best=findbest();
666 sFace outer=*best;
667 U pass=0;
668 U iterations=0;
669 bind(tetra[0],0,tetra[1],0);
670 bind(tetra[0],1,tetra[2],0);
671 bind(tetra[0],2,tetra[3],0);
672 bind(tetra[1],1,tetra[3],2);
673 bind(tetra[1],2,tetra[2],1);
674 bind(tetra[2],2,tetra[3],1);
675 m_status=eStatus::Valid;
676 for(;iterations<EPA_MAX_ITERATIONS;++iterations)
677 {
678 if(m_nextsv<EPA_MAX_VERTICES)
679 {
680 sHorizon horizon;
681 sSV* w=&m_sv_store[m_nextsv++];
682 bool valid=true;
683 best->pass = (U1)(++pass);
684 gjk.getsupport(best->n,*w);
685 const real_t wdist=vec3_dot(best->n,w->w)-best->d;
686 if(wdist>EPA_ACCURACY)
687 {
688 for(U j=0;(j<3)&&valid;++j)
689 {
690 valid&=expand( pass,w,
691 best->f[j],best->e[j],
692 horizon);
693 }
694 if(valid&&(horizon.nf>=3))
695 {
696 bind(horizon.cf,1,horizon.ff,2);
697 remove(m_hull,best);
698 append(m_stock,best);
699 best=findbest();
700 outer=*best;
701 } else { m_status=eStatus::InvalidHull;break; }
702 } else { m_status=eStatus::AccuraryReached;break; }
703 } else { m_status=eStatus::OutOfVertices;break; }
704 }
705 const Vector3 projection=outer.n*outer.d;
706 m_normal = outer.n;
707 m_depth = outer.d;
708 m_result.rank = 3;
709 m_result.c[0] = outer.c[0];
710 m_result.c[1] = outer.c[1];
711 m_result.c[2] = outer.c[2];
712 m_result.p[0] = vec3_cross( outer.c[1]->w-projection,
713 outer.c[2]->w-projection).length();
714 m_result.p[1] = vec3_cross( outer.c[2]->w-projection,
715 outer.c[0]->w-projection).length();
716 m_result.p[2] = vec3_cross( outer.c[0]->w-projection,
717 outer.c[1]->w-projection).length();
718 const real_t sum=m_result.p[0]+m_result.p[1]+m_result.p[2];
719 m_result.p[0] /= sum;
720 m_result.p[1] /= sum;
721 m_result.p[2] /= sum;
722 return(m_status);
723 }
724 }
725 /* Fallback */
726 m_status = eStatus::FallBack;
727 m_normal = -guess;
728 const real_t nl = m_normal.length();
729 if (nl > 0) {
730 m_normal = m_normal/nl;
731 } else {
732 m_normal = Vector3(1,0,0);
733 }
734 m_depth = 0;
735 m_result.rank=1;
736 m_result.c[0]=simplex.c[0];
737 m_result.p[0]=1;
738 return(m_status);
739 }
740
741 bool getedgedist(sFace* face, sSV* a, sSV* b, real_t& dist)
742 {
743 const Vector3 ba = b->w - a->w;
744 const Vector3 n_ab = vec3_cross(ba, face->n); // Outward facing edge normal direction, on triangle plane
745 const real_t a_dot_nab = vec3_dot(a->w, n_ab); // Only care about the sign to determine inside/outside, so not normalization required
746
747 if (a_dot_nab < 0) {
748 // Outside of edge a->b
749 const real_t ba_l2 = ba.length_squared();
750 const real_t a_dot_ba = vec3_dot(a->w, ba);
751 const real_t b_dot_ba = vec3_dot(b->w, ba);
752
753 if (a_dot_ba > 0) {
754 // Pick distance vertex a
755 dist = a->w.length();
756 } else if (b_dot_ba < 0) {
757 // Pick distance vertex b
758 dist = b->w.length();
759 } else {
760 // Pick distance to edge a->b
761 const real_t a_dot_b = vec3_dot(a->w, b->w);
762 dist = Math::sqrt(MAX((a->w.length_squared() * b->w.length_squared() - a_dot_b * a_dot_b) / ba_l2, 0.0));
763 }
764
765 return true;
766 }
767
768 return false;
769 }
770
771 sFace* newface(sSV* a,sSV* b,sSV* c,bool forced)
772 {
773 if (m_stock.root) {
774 sFace* face=m_stock.root;
775 remove(m_stock,face);
776 append(m_hull,face);
777 face->pass = 0;
778 face->c[0] = a;
779 face->c[1] = b;
780 face->c[2] = c;
781 face->n = vec3_cross(b->w-a->w,c->w-a->w);
782 const real_t l=face->n.length();
783 const bool v=l>EPA_ACCURACY;
784 if (v) {
785 if (!(getedgedist(face, a, b, face->d) ||
786 getedgedist(face, b, c, face->d) ||
787 getedgedist(face, c, a, face->d))) {
788 // Origin projects to the interior of the triangle
789 // Use distance to triangle plane
790 face->d = vec3_dot(a->w, face->n) / l;
791 }
792 face->n /= l;
793 if (forced||(face->d>=-EPA_PLANE_EPS)) {
794 return(face);
795 } else {
796 m_status=eStatus::NonConvex;
797 }
798 } else {
799 m_status=eStatus::Degenerated;
800 }
801 remove(m_hull,face);
802 append(m_stock,face);
803 return(nullptr);
804 }
805 // -- GODOT start --
806 //m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces;
807 m_status=eStatus::OutOfFaces;
808 // -- GODOT end --
809 return(nullptr);
810 }
811 sFace* findbest()
812 {
813 sFace* minf=m_hull.root;
814 real_t mind=minf->d*minf->d;
815 for(sFace* f=minf->l[1];f;f=f->l[1])
816 {
817 const real_t sqd=f->d*f->d;
818 if(sqd<mind)
819 {
820 minf=f;
821 mind=sqd;
822 }
823 }
824 return(minf);
825 }
826 bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon)
827 {
828 static const U i1m3[]={1,2,0};
829 static const U i2m3[]={2,0,1};
830 if(f->pass!=pass)
831 {
832 const U e1=i1m3[e];
833 if((vec3_dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS)
834 {
835 sFace* nf=newface(f->c[e1],f->c[e],w,false);
836 if(nf)
837 {
838 bind(nf,0,f,e);
839 if(horizon.cf) { bind(horizon.cf,1,nf,2); } else { horizon.ff=nf;
840}
841 horizon.cf=nf;
842 ++horizon.nf;
843 return(true);
844 }
845 }
846 else
847 {
848 const U e2=i2m3[e];
849 f->pass = (U1)pass;
850 if( expand(pass,w,f->f[e1],f->e[e1],horizon)&&
851 expand(pass,w,f->f[e2],f->e[e2],horizon))
852 {
853 remove(m_hull,f);
854 append(m_stock,f);
855 return(true);
856 }
857 }
858 }
859 return(false);
860 }
861
862 };
863
864 //
865 static void Initialize( const GodotShape3D* shape0, const Transform3D& wtrs0, real_t margin0,
866 const GodotShape3D* shape1, const Transform3D& wtrs1, real_t margin1,
867 sResults& results,
868 tShape& shape)
869 {
870 /* Results */
871 results.witnesses[0] = Vector3(0,0,0);
872 results.witnesses[1] = Vector3(0,0,0);
873 results.status = sResults::Separated;
874 /* Shape */
875 shape.Initialize(shape0, wtrs0, margin0, shape1, wtrs1, margin1);
876 }
877
878
879
880//
881// Api
882//
883
884//
885
886//
887bool Distance( const GodotShape3D* shape0,
888 const Transform3D& wtrs0,
889 real_t margin0,
890 const GodotShape3D* shape1,
891 const Transform3D& wtrs1,
892 real_t margin1,
893 const Vector3& guess,
894 sResults& results)
895{
896 tShape shape;
897 Initialize(shape0, wtrs0, margin0, shape1, wtrs1, margin1, results, shape);
898 GJK gjk;
899 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess);
900 if(gjk_status==GJK::eStatus::Valid)
901 {
902 Vector3 w0=Vector3(0,0,0);
903 Vector3 w1=Vector3(0,0,0);
904 for(U i=0;i<gjk.m_simplex->rank;++i)
905 {
906 const real_t p=gjk.m_simplex->p[i];
907 w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p;
908 w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p;
909 }
910 results.witnesses[0] = w0;
911 results.witnesses[1] = w1;
912 results.normal = w0-w1;
913 results.distance = results.normal.length();
914 results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1;
915 return(true);
916 }
917 else
918 {
919 results.status = gjk_status==GJK::eStatus::Inside?
920 sResults::Penetrating :
921 sResults::GJK_Failed;
922 return(false);
923 }
924}
925
926
927//
928bool Penetration( const GodotShape3D* shape0,
929 const Transform3D& wtrs0,
930 real_t margin0,
931 const GodotShape3D* shape1,
932 const Transform3D& wtrs1,
933 real_t margin1,
934 const Vector3& guess,
935 sResults& results
936 )
937{
938 tShape shape;
939 Initialize(shape0, wtrs0, margin0, shape1, wtrs1, margin1, results, shape);
940 GJK gjk;
941 GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess);
942 switch(gjk_status)
943 {
944 case GJK::eStatus::Inside:
945 {
946 EPA epa;
947 EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess);
948 if(epa_status!=EPA::eStatus::Failed)
949 {
950 Vector3 w0=Vector3(0,0,0);
951 for(U i=0;i<epa.m_result.rank;++i)
952 {
953 w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i];
954 }
955 results.status = sResults::Penetrating;
956 results.witnesses[0] = w0;
957 results.witnesses[1] = w0-epa.m_normal*epa.m_depth;
958 results.normal = -epa.m_normal;
959 results.distance = -epa.m_depth;
960 return(true);
961 } else { results.status=sResults::EPA_Failed;
962}
963 }
964 break;
965 case GJK::eStatus::Failed:
966 results.status=sResults::GJK_Failed;
967 break;
968 default: {}
969 }
970 return(false);
971}
972
973
974
975/* Symbols cleanup */
976
977#undef GJK_MAX_ITERATIONS
978#undef GJK_ACCURARY
979#undef GJK_MIN_DISTANCE
980#undef GJK_DUPLICATED_EPS
981#undef GJK_SIMPLEX2_EPS
982#undef GJK_SIMPLEX3_EPS
983#undef GJK_SIMPLEX4_EPS
984
985#undef EPA_MAX_VERTICES
986#undef EPA_MAX_FACES
987#undef EPA_MAX_ITERATIONS
988#undef EPA_ACCURACY
989#undef EPA_FALLBACK
990#undef EPA_PLANE_EPS
991#undef EPA_INSIDE_EPS
992} // end of namespace
993
994/* clang-format on */
995
996bool gjk_epa_calculate_distance(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, Vector3 &r_result_A, Vector3 &r_result_B) {
997 GjkEpa2::sResults res;
998
999 if (GjkEpa2::Distance(p_shape_A, p_transform_A, 0.0, p_shape_B, p_transform_B, 0.0, p_transform_B.origin - p_transform_A.origin, res)) {
1000 r_result_A = res.witnesses[0];
1001 r_result_B = res.witnesses[1];
1002 return true;
1003 }
1004
1005 return false;
1006}
1007
1008bool gjk_epa_calculate_penetration(const GodotShape3D *p_shape_A, const Transform3D &p_transform_A, const GodotShape3D *p_shape_B, const Transform3D &p_transform_B, GodotCollisionSolver3D::CallbackResult p_result_callback, void *p_userdata, bool p_swap, real_t p_margin_A, real_t p_margin_B) {
1009 GjkEpa2::sResults res;
1010
1011 if (GjkEpa2::Penetration(p_shape_A, p_transform_A, p_margin_A, p_shape_B, p_transform_B, p_margin_B, p_transform_B.origin - p_transform_A.origin, res)) {
1012 if (p_result_callback) {
1013 if (p_swap) {
1014 Vector3 normal = (res.witnesses[1] - res.witnesses[0]).normalized();
1015 p_result_callback(res.witnesses[1], 0, res.witnesses[0], 0, normal, p_userdata);
1016 } else {
1017 Vector3 normal = (res.witnesses[0] - res.witnesses[1]).normalized();
1018 p_result_callback(res.witnesses[0], 0, res.witnesses[1], 0, normal, p_userdata);
1019 }
1020 }
1021 return true;
1022 }
1023
1024 return false;
1025}
1026