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