| 1 | /* |
|---|
| 2 | Bullet Continuous Collision Detection and Physics Library |
|---|
| 3 | Copyright (c) 2003-2006 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 |
|---|
| 24 | Nov.2006 |
|---|
| 25 | */ |
|---|
| 26 | |
|---|
| 27 | #include "btGjkEpa.h" |
|---|
| 28 | #include <string.h> //for memset |
|---|
| 29 | #include "LinearMath/btStackAlloc.h" |
|---|
| 30 | |
|---|
| 31 | #if defined(DEBUG) || defined (_DEBUG) |
|---|
| 32 | #include <stdio.h> //for debug printf |
|---|
| 33 | #ifdef __SPU__ |
|---|
| 34 | #include <spu_printf.h> |
|---|
| 35 | #define printf spu_printf |
|---|
| 36 | #endif //__SPU__ |
|---|
| 37 | #endif |
|---|
| 38 | |
|---|
| 39 | namespace gjkepa_impl |
|---|
| 40 | { |
|---|
| 41 | |
|---|
| 42 | // |
|---|
| 43 | // Port. typedefs |
|---|
| 44 | // |
|---|
| 45 | |
|---|
| 46 | typedef btScalar F; |
|---|
| 47 | typedef bool Z; |
|---|
| 48 | typedef int I; |
|---|
| 49 | typedef unsigned int U; |
|---|
| 50 | typedef unsigned char U1; |
|---|
| 51 | typedef unsigned short U2; |
|---|
| 52 | |
|---|
| 53 | typedef btVector3 Vector3; |
|---|
| 54 | typedef btMatrix3x3 Rotation; |
|---|
| 55 | |
|---|
| 56 | // |
|---|
| 57 | // Config |
|---|
| 58 | // |
|---|
| 59 | |
|---|
| 60 | #if 0 |
|---|
| 61 | #define BTLOCALSUPPORT localGetSupportingVertexWithoutMargin |
|---|
| 62 | #else |
|---|
| 63 | #define BTLOCALSUPPORT localGetSupportingVertex |
|---|
| 64 | #endif |
|---|
| 65 | |
|---|
| 66 | // |
|---|
| 67 | // Const |
|---|
| 68 | // |
|---|
| 69 | |
|---|
| 70 | |
|---|
| 71 | #define cstInf SIMD_INFINITY |
|---|
| 72 | #define cstPi SIMD_PI |
|---|
| 73 | #define cst2Pi SIMD_2_PI |
|---|
| 74 | #define GJK_maxiterations (128) |
|---|
| 75 | #define GJK_hashsize (1<<6) |
|---|
| 76 | #define GJK_hashmask (GJK_hashsize-1) |
|---|
| 77 | #define GJK_insimplex_eps F(0.0001) |
|---|
| 78 | #define GJK_sqinsimplex_eps (GJK_insimplex_eps*GJK_insimplex_eps) |
|---|
| 79 | #define EPA_maxiterations 256 |
|---|
| 80 | #define EPA_inface_eps F(0.01) |
|---|
| 81 | #define EPA_accuracy F(0.001) |
|---|
| 82 | |
|---|
| 83 | // |
|---|
| 84 | // Utils |
|---|
| 85 | // |
|---|
| 86 | |
|---|
| 87 | static inline F Abs(F v) { return(v<0?-v:v); } |
|---|
| 88 | static inline F Sign(F v) { return(F(v<0?-1:1)); } |
|---|
| 89 | template <typename T> static inline void Swap(T& a,T& b) { T |
|---|
| 90 | t(a);a=b;b=t; } |
|---|
| 91 | template <typename T> static inline T Min(const T& a,const T& b) { |
|---|
| 92 | return(a<b?a:b); } |
|---|
| 93 | template <typename T> static inline T Max(const T& a,const T& b) { |
|---|
| 94 | return(a>b?a:b); } |
|---|
| 95 | static inline void ClearMemory(void* p,U sz) { memset(p,0,(size_t)sz); |
|---|
| 96 | } |
|---|
| 97 | #if 0 |
|---|
| 98 | template <typename T> static inline void Raise(const T& object) { |
|---|
| 99 | throw(object); } |
|---|
| 100 | #else |
|---|
| 101 | template <typename T> static inline void Raise(const T&) {} |
|---|
| 102 | #endif |
|---|
| 103 | |
|---|
| 104 | |
|---|
| 105 | |
|---|
| 106 | // |
|---|
| 107 | // GJK |
|---|
| 108 | // |
|---|
| 109 | struct GJK |
|---|
| 110 | { |
|---|
| 111 | struct Mkv |
|---|
| 112 | { |
|---|
| 113 | Vector3 w; /* Minkowski vertice */ |
|---|
| 114 | Vector3 r; /* Ray */ |
|---|
| 115 | }; |
|---|
| 116 | struct He |
|---|
| 117 | { |
|---|
| 118 | Vector3 v; |
|---|
| 119 | He* n; |
|---|
| 120 | }; |
|---|
| 121 | btStackAlloc* sa; |
|---|
| 122 | btBlock* sablock; |
|---|
| 123 | He* table[GJK_hashsize]; |
|---|
| 124 | Rotation wrotations[2]; |
|---|
| 125 | Vector3 positions[2]; |
|---|
| 126 | const btConvexShape* shapes[2]; |
|---|
| 127 | Mkv simplex[5]; |
|---|
| 128 | Vector3 ray; |
|---|
| 129 | U order; |
|---|
| 130 | U iterations; |
|---|
| 131 | F margin; |
|---|
| 132 | Z failed; |
|---|
| 133 | // |
|---|
| 134 | GJK(btStackAlloc* psa, |
|---|
| 135 | const Rotation& wrot0,const Vector3& pos0,const btConvexShape* shape0, |
|---|
| 136 | const Rotation& wrot1,const Vector3& pos1,const btConvexShape* shape1, |
|---|
| 137 | F pmargin=0) |
|---|
| 138 | { |
|---|
| 139 | wrotations[0]=wrot0;positions[0]=pos0;shapes[0]=shape0; |
|---|
| 140 | wrotations[1]=wrot1;positions[1]=pos1;shapes[1]=shape1; |
|---|
| 141 | sa =psa; |
|---|
| 142 | sablock =sa->beginBlock(); |
|---|
| 143 | margin =pmargin; |
|---|
| 144 | failed =false; |
|---|
| 145 | } |
|---|
| 146 | // |
|---|
| 147 | ~GJK() |
|---|
| 148 | { |
|---|
| 149 | sa->endBlock(sablock); |
|---|
| 150 | } |
|---|
| 151 | // vdh : very dumm hash |
|---|
| 152 | static inline U Hash(const Vector3& v) |
|---|
| 153 | { |
|---|
| 154 | //this doesn't compile under GCC 3.3.5, so add the ()... |
|---|
| 155 | //const U h(U(v[0]*15461)^U(v[1]*83003)^U(v[2]*15473)); |
|---|
| 156 | //return(((*((const U*)&h))*169639)&GJK_hashmask); |
|---|
| 157 | const U h((U)(v[0]*15461)^(U)(v[1]*83003)^(U)(v[2]*15473)); |
|---|
| 158 | return(((*((const U*)&h))*169639)&GJK_hashmask); |
|---|
| 159 | } |
|---|
| 160 | // |
|---|
| 161 | inline Vector3 LocalSupport(const Vector3& d,U i) const |
|---|
| 162 | { |
|---|
| 163 | return(wrotations[i]*shapes[i]->BTLOCALSUPPORT(d*wrotations[i])+positions[i]); |
|---|
| 164 | } |
|---|
| 165 | // |
|---|
| 166 | inline void Support(const Vector3& d,Mkv& v) const |
|---|
| 167 | { |
|---|
| 168 | v.r = d; |
|---|
| 169 | v.w = LocalSupport(d,0)-LocalSupport(-d,1)+d*margin; |
|---|
| 170 | } |
|---|
| 171 | #define SPX(_i_) simplex[_i_] |
|---|
| 172 | #define SPXW(_i_) simplex[_i_].w |
|---|
| 173 | // |
|---|
| 174 | inline Z FetchSupport() |
|---|
| 175 | { |
|---|
| 176 | const U h(Hash(ray)); |
|---|
| 177 | He* e = (He*)(table[h]); |
|---|
| 178 | while(e) { if(e->v==ray) { --order;return(false); } else e=e->n; } |
|---|
| 179 | e=(He*)sa->allocate(sizeof(He));e->v=ray;e->n=table[h];table[h]=e; |
|---|
| 180 | Support(ray,simplex[++order]); |
|---|
| 181 | return(ray.dot(SPXW(order))>0); |
|---|
| 182 | } |
|---|
| 183 | // |
|---|
| 184 | inline Z SolveSimplex2(const Vector3& ao,const Vector3& ab) |
|---|
| 185 | { |
|---|
| 186 | if(ab.dot(ao)>=0) |
|---|
| 187 | { |
|---|
| 188 | const Vector3 cabo(cross(ab,ao)); |
|---|
| 189 | if(cabo.length2()>GJK_sqinsimplex_eps) |
|---|
| 190 | { ray=cross(cabo,ab); } |
|---|
| 191 | else |
|---|
| 192 | { return(true); } |
|---|
| 193 | } |
|---|
| 194 | else |
|---|
| 195 | { order=0;SPX(0)=SPX(1);ray=ao; } |
|---|
| 196 | return(false); |
|---|
| 197 | } |
|---|
| 198 | // |
|---|
| 199 | inline Z SolveSimplex3(const Vector3& ao,const Vector3& ab,const Vector3& |
|---|
| 200 | ac) |
|---|
| 201 | { |
|---|
| 202 | return(SolveSimplex3a(ao,ab,ac,cross(ab,ac))); |
|---|
| 203 | } |
|---|
| 204 | // |
|---|
| 205 | inline Z SolveSimplex3a(const Vector3& ao,const Vector3& ab,const Vector3& |
|---|
| 206 | ac,const Vector3& cabc) |
|---|
| 207 | { |
|---|
| 208 | if((cross(cabc,ab)).dot(ao)<-GJK_insimplex_eps) |
|---|
| 209 | { order=1;SPX(0)=SPX(1);SPX(1)=SPX(2);return(SolveSimplex2(ao,ab)); } |
|---|
| 210 | else if((cross(cabc,ac)).dot(ao)>+GJK_insimplex_eps) |
|---|
| 211 | { order=1;SPX(1)=SPX(2);return(SolveSimplex2(ao,ac)); } |
|---|
| 212 | else |
|---|
| 213 | { |
|---|
| 214 | const F d(cabc.dot(ao)); |
|---|
| 215 | if(Abs(d)>GJK_insimplex_eps) |
|---|
| 216 | { |
|---|
| 217 | if(d>0) |
|---|
| 218 | { ray=cabc; } |
|---|
| 219 | else |
|---|
| 220 | { ray=-cabc;Swap(SPX(0),SPX(1)); } |
|---|
| 221 | return(false); |
|---|
| 222 | } else return(true); |
|---|
| 223 | } |
|---|
| 224 | } |
|---|
| 225 | // |
|---|
| 226 | inline Z SolveSimplex4(const Vector3& ao,const Vector3& ab,const Vector3& |
|---|
| 227 | ac,const Vector3& ad) |
|---|
| 228 | { |
|---|
| 229 | Vector3 crs; |
|---|
| 230 | if((crs=cross(ab,ac)).dot(ao)>GJK_insimplex_eps) |
|---|
| 231 | { |
|---|
| 232 | order=2;SPX(0)=SPX(1);SPX(1)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ab,ac,crs)); |
|---|
| 233 | } |
|---|
| 234 | else if((crs=cross(ac,ad)).dot(ao)>GJK_insimplex_eps) |
|---|
| 235 | { order=2;SPX(2)=SPX(3);return(SolveSimplex3a(ao,ac,ad,crs)); } |
|---|
| 236 | else if((crs=cross(ad,ab)).dot(ao)>GJK_insimplex_eps) |
|---|
| 237 | { |
|---|
| 238 | order=2;SPX(1)=SPX(0);SPX(0)=SPX(2);SPX(2)=SPX(3);return(SolveSimplex3a(ao,ad,ab,crs)); |
|---|
| 239 | } |
|---|
| 240 | else return(true); |
|---|
| 241 | } |
|---|
| 242 | // |
|---|
| 243 | inline Z SearchOrigin(const Vector3& initray=Vector3(1,0,0)) |
|---|
| 244 | { |
|---|
| 245 | iterations = 0; |
|---|
| 246 | order = (U)-1; |
|---|
| 247 | failed = false; |
|---|
| 248 | ray = initray.normalized(); |
|---|
| 249 | ClearMemory(table,sizeof(void*)*GJK_hashsize); |
|---|
| 250 | FetchSupport(); |
|---|
| 251 | ray = -SPXW(0); |
|---|
| 252 | for(;iterations<GJK_maxiterations;++iterations) |
|---|
| 253 | { |
|---|
| 254 | const F rl(ray.length()); |
|---|
| 255 | ray/=rl>0?rl:1; |
|---|
| 256 | if(FetchSupport()) |
|---|
| 257 | { |
|---|
| 258 | Z found(false); |
|---|
| 259 | switch(order) |
|---|
| 260 | { |
|---|
| 261 | case 1: found=SolveSimplex2(-SPXW(1),SPXW(0)-SPXW(1));break; |
|---|
| 262 | case 2: found=SolveSimplex3(-SPXW(2),SPXW(1)-SPXW(2),SPXW(0)-SPXW(2));break; |
|---|
| 263 | case 3: found=SolveSimplex4(-SPXW(3),SPXW(2)-SPXW(3),SPXW(1)-SPXW(3),SPXW(0)-SPXW(3));break; |
|---|
| 264 | } |
|---|
| 265 | if(found) return(true); |
|---|
| 266 | } else return(false); |
|---|
| 267 | } |
|---|
| 268 | failed=true; |
|---|
| 269 | return(false); |
|---|
| 270 | } |
|---|
| 271 | // |
|---|
| 272 | inline Z EncloseOrigin() |
|---|
| 273 | { |
|---|
| 274 | switch(order) |
|---|
| 275 | { |
|---|
| 276 | /* Point */ |
|---|
| 277 | case 0: break; |
|---|
| 278 | /* Line */ |
|---|
| 279 | case 1: |
|---|
| 280 | { |
|---|
| 281 | const Vector3 ab(SPXW(1)-SPXW(0)); |
|---|
| 282 | const Vector3 b[]={ cross(ab,Vector3(1,0,0)), |
|---|
| 283 | cross(ab,Vector3(0,1,0)), |
|---|
| 284 | cross(ab,Vector3(0,0,1))}; |
|---|
| 285 | const F m[]={b[0].length2(),b[1].length2(),b[2].length2()}; |
|---|
| 286 | const Rotation r(btQuaternion(ab.normalized(),cst2Pi/3)); |
|---|
| 287 | Vector3 w(b[m[0]>m[1]?m[0]>m[2]?0:2:m[1]>m[2]?1:2]); |
|---|
| 288 | Support(w.normalized(),simplex[4]);w=r*w; |
|---|
| 289 | Support(w.normalized(),simplex[2]);w=r*w; |
|---|
| 290 | Support(w.normalized(),simplex[3]);w=r*w; |
|---|
| 291 | order=4; |
|---|
| 292 | return(true); |
|---|
| 293 | } |
|---|
| 294 | break; |
|---|
| 295 | /* Triangle */ |
|---|
| 296 | case 2: |
|---|
| 297 | { |
|---|
| 298 | const |
|---|
| 299 | Vector3 n(cross((SPXW(1)-SPXW(0)),(SPXW(2)-SPXW(0))).normalized()); |
|---|
| 300 | Support( n,simplex[3]); |
|---|
| 301 | Support(-n,simplex[4]); |
|---|
| 302 | order=4; |
|---|
| 303 | return(true); |
|---|
| 304 | } |
|---|
| 305 | break; |
|---|
| 306 | /* Tetrahedron */ |
|---|
| 307 | case 3: return(true); |
|---|
| 308 | /* Hexahedron */ |
|---|
| 309 | case 4: return(true); |
|---|
| 310 | } |
|---|
| 311 | return(false); |
|---|
| 312 | } |
|---|
| 313 | #undef SPX |
|---|
| 314 | #undef SPXW |
|---|
| 315 | }; |
|---|
| 316 | |
|---|
| 317 | // |
|---|
| 318 | // EPA |
|---|
| 319 | // |
|---|
| 320 | struct EPA |
|---|
| 321 | { |
|---|
| 322 | // |
|---|
| 323 | struct Face |
|---|
| 324 | { |
|---|
| 325 | const GJK::Mkv* v[3]; |
|---|
| 326 | Face* f[3]; |
|---|
| 327 | U e[3]; |
|---|
| 328 | Vector3 n; |
|---|
| 329 | F d; |
|---|
| 330 | U mark; |
|---|
| 331 | Face* prev; |
|---|
| 332 | Face* next; |
|---|
| 333 | Face() {} |
|---|
| 334 | }; |
|---|
| 335 | // |
|---|
| 336 | GJK* gjk; |
|---|
| 337 | btStackAlloc* sa; |
|---|
| 338 | Face* root; |
|---|
| 339 | U nfaces; |
|---|
| 340 | U iterations; |
|---|
| 341 | Vector3 features[2][3]; |
|---|
| 342 | Vector3 nearest[2]; |
|---|
| 343 | Vector3 normal; |
|---|
| 344 | F depth; |
|---|
| 345 | Z failed; |
|---|
| 346 | // |
|---|
| 347 | EPA(GJK* pgjk) |
|---|
| 348 | { |
|---|
| 349 | gjk = pgjk; |
|---|
| 350 | sa = pgjk->sa; |
|---|
| 351 | } |
|---|
| 352 | // |
|---|
| 353 | ~EPA() |
|---|
| 354 | { |
|---|
| 355 | } |
|---|
| 356 | // |
|---|
| 357 | inline Vector3 GetCoordinates(const Face* face) const |
|---|
| 358 | { |
|---|
| 359 | const Vector3 o(face->n*-face->d); |
|---|
| 360 | const F a[]={ cross(face->v[0]->w-o,face->v[1]->w-o).length(), |
|---|
| 361 | cross(face->v[1]->w-o,face->v[2]->w-o).length(), |
|---|
| 362 | cross(face->v[2]->w-o,face->v[0]->w-o).length()}; |
|---|
| 363 | const F sm(a[0]+a[1]+a[2]); |
|---|
| 364 | return(Vector3(a[1],a[2],a[0])/(sm>0?sm:1)); |
|---|
| 365 | } |
|---|
| 366 | // |
|---|
| 367 | inline Face* FindBest() const |
|---|
| 368 | { |
|---|
| 369 | Face* bf = 0; |
|---|
| 370 | if(root) |
|---|
| 371 | { |
|---|
| 372 | Face* cf = root; |
|---|
| 373 | F bd(cstInf); |
|---|
| 374 | do { |
|---|
| 375 | if(cf->d<bd) { bd=cf->d;bf=cf; } |
|---|
| 376 | } while(0!=(cf=cf->next)); |
|---|
| 377 | } |
|---|
| 378 | return(bf); |
|---|
| 379 | } |
|---|
| 380 | // |
|---|
| 381 | inline Z Set(Face* f,const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv* |
|---|
| 382 | c) const |
|---|
| 383 | { |
|---|
| 384 | const Vector3 nrm(cross(b->w-a->w,c->w-a->w)); |
|---|
| 385 | const F len(nrm.length()); |
|---|
| 386 | const Z valid( (cross(a->w,b->w).dot(nrm)>=-EPA_inface_eps)&& |
|---|
| 387 | (cross(b->w,c->w).dot(nrm)>=-EPA_inface_eps)&& |
|---|
| 388 | (cross(c->w,a->w).dot(nrm)>=-EPA_inface_eps)); |
|---|
| 389 | f->v[0] = a; |
|---|
| 390 | f->v[1] = b; |
|---|
| 391 | f->v[2] = c; |
|---|
| 392 | f->mark = 0; |
|---|
| 393 | f->n = nrm/(len>0?len:cstInf); |
|---|
| 394 | f->d = Max<F>(0,-f->n.dot(a->w)); |
|---|
| 395 | return(valid); |
|---|
| 396 | } |
|---|
| 397 | // |
|---|
| 398 | inline Face* NewFace(const GJK::Mkv* a,const GJK::Mkv* b,const GJK::Mkv* c) |
|---|
| 399 | { |
|---|
| 400 | Face* pf = (Face*)sa->allocate(sizeof(Face)); |
|---|
| 401 | if(Set(pf,a,b,c)) |
|---|
| 402 | { |
|---|
| 403 | if(root) root->prev=pf; |
|---|
| 404 | pf->prev=0; |
|---|
| 405 | pf->next=root; |
|---|
| 406 | root =pf; |
|---|
| 407 | ++nfaces; |
|---|
| 408 | } |
|---|
| 409 | else |
|---|
| 410 | { |
|---|
| 411 | pf->prev=pf->next=0; |
|---|
| 412 | } |
|---|
| 413 | return(pf); |
|---|
| 414 | } |
|---|
| 415 | // |
|---|
| 416 | inline void Detach(Face* face) |
|---|
| 417 | { |
|---|
| 418 | if(face->prev||face->next) |
|---|
| 419 | { |
|---|
| 420 | --nfaces; |
|---|
| 421 | if(face==root) |
|---|
| 422 | { root=face->next;root->prev=0; } |
|---|
| 423 | else |
|---|
| 424 | { |
|---|
| 425 | if(face->next==0) |
|---|
| 426 | { face->prev->next=0; } |
|---|
| 427 | else |
|---|
| 428 | { face->prev->next=face->next;face->next->prev=face->prev; } |
|---|
| 429 | } |
|---|
| 430 | face->prev=face->next=0; |
|---|
| 431 | } |
|---|
| 432 | } |
|---|
| 433 | // |
|---|
| 434 | inline void Link(Face* f0,U e0,Face* f1,U e1) const |
|---|
| 435 | { |
|---|
| 436 | f0->f[e0]=f1;f1->e[e1]=e0; |
|---|
| 437 | f1->f[e1]=f0;f0->e[e0]=e1; |
|---|
| 438 | } |
|---|
| 439 | // |
|---|
| 440 | GJK::Mkv* Support(const Vector3& w) const |
|---|
| 441 | { |
|---|
| 442 | GJK::Mkv* v =(GJK::Mkv*)sa->allocate(sizeof(GJK::Mkv)); |
|---|
| 443 | gjk->Support(w,*v); |
|---|
| 444 | return(v); |
|---|
| 445 | } |
|---|
| 446 | // |
|---|
| 447 | U BuildHorizon(U markid,const GJK::Mkv* w,Face& f,U e,Face*& cf,Face*& |
|---|
| 448 | ff) |
|---|
| 449 | { |
|---|
| 450 | static const U mod3[]={0,1,2,0,1}; |
|---|
| 451 | U ne(0); |
|---|
| 452 | if(f.mark!=markid) |
|---|
| 453 | { |
|---|
| 454 | const U e1(mod3[e+1]); |
|---|
| 455 | if((f.n.dot(w->w)+f.d)>0) |
|---|
| 456 | { |
|---|
| 457 | Face* nf = NewFace(f.v[e1],f.v[e],w); |
|---|
| 458 | Link(nf,0,&f,e); |
|---|
| 459 | if(cf) Link(cf,1,nf,2); else ff=nf; |
|---|
| 460 | cf=nf;ne=1; |
|---|
| 461 | } |
|---|
| 462 | else |
|---|
| 463 | { |
|---|
| 464 | const U e2(mod3[e+2]); |
|---|
| 465 | Detach(&f); |
|---|
| 466 | f.mark = markid; |
|---|
| 467 | ne += BuildHorizon(markid,w,*f.f[e1],f.e[e1],cf,ff); |
|---|
| 468 | ne += BuildHorizon(markid,w,*f.f[e2],f.e[e2],cf,ff); |
|---|
| 469 | } |
|---|
| 470 | } |
|---|
| 471 | return(ne); |
|---|
| 472 | } |
|---|
| 473 | // |
|---|
| 474 | inline F EvaluatePD(F accuracy=EPA_accuracy) |
|---|
| 475 | { |
|---|
| 476 | btBlock* sablock = sa->beginBlock(); |
|---|
| 477 | Face* bestface = 0; |
|---|
| 478 | U markid(1); |
|---|
| 479 | depth = -cstInf; |
|---|
| 480 | normal = Vector3(0,0,0); |
|---|
| 481 | root = 0; |
|---|
| 482 | nfaces = 0; |
|---|
| 483 | iterations = 0; |
|---|
| 484 | failed = false; |
|---|
| 485 | /* Prepare hull */ |
|---|
| 486 | if(gjk->EncloseOrigin()) |
|---|
| 487 | { |
|---|
| 488 | const U* pfidx = 0; |
|---|
| 489 | U nfidx= 0; |
|---|
| 490 | const U* peidx = 0; |
|---|
| 491 | U neidx = 0; |
|---|
| 492 | GJK::Mkv* basemkv[5]; |
|---|
| 493 | Face* basefaces[6]; |
|---|
| 494 | switch(gjk->order) |
|---|
| 495 | { |
|---|
| 496 | /* Tetrahedron */ |
|---|
| 497 | case 3: { |
|---|
| 498 | static const U fidx[4][3]={{2,1,0},{3,0,1},{3,1,2},{3,2,0}}; |
|---|
| 499 | static const |
|---|
| 500 | U eidx[6][4]={{0,0,2,1},{0,1,1,1},{0,2,3,1},{1,0,3,2},{2,0,1,2},{3,0,2,2}}; |
|---|
| 501 | pfidx=(const U*)fidx;nfidx=4;peidx=(const U*)eidx;neidx=6; |
|---|
| 502 | } break; |
|---|
| 503 | /* Hexahedron */ |
|---|
| 504 | case 4: { |
|---|
| 505 | static const |
|---|
| 506 | U fidx[6][3]={{2,0,4},{4,1,2},{1,4,0},{0,3,1},{0,2,3},{1,3,2}}; |
|---|
| 507 | static const |
|---|
| 508 | U eidx[9][4]={{0,0,4,0},{0,1,2,1},{0,2,1,2},{1,1,5,2},{1,0,2,0},{2,2,3,2},{3,1,5,0},{3,0,4,2},{5,1,4,1}}; |
|---|
| 509 | pfidx=(const U*)fidx;nfidx=6;peidx=(const U*)eidx;neidx=9; |
|---|
| 510 | } break; |
|---|
| 511 | } |
|---|
| 512 | U i; |
|---|
| 513 | |
|---|
| 514 | for( i=0;i<=gjk->order;++i) { |
|---|
| 515 | basemkv[i]=(GJK::Mkv*)sa->allocate(sizeof(GJK::Mkv));*basemkv[i]=gjk->simplex[i]; |
|---|
| 516 | } |
|---|
| 517 | for( i=0;i<nfidx;++i,pfidx+=3) { |
|---|
| 518 | basefaces[i]=NewFace(basemkv[pfidx[0]],basemkv[pfidx[1]],basemkv[pfidx[2]]); |
|---|
| 519 | } |
|---|
| 520 | for( i=0;i<neidx;++i,peidx+=4) { |
|---|
| 521 | Link(basefaces[peidx[0]],peidx[1],basefaces[peidx[2]],peidx[3]); } |
|---|
| 522 | } |
|---|
| 523 | if(0==nfaces) |
|---|
| 524 | { |
|---|
| 525 | sa->endBlock(sablock); |
|---|
| 526 | return(depth); |
|---|
| 527 | } |
|---|
| 528 | /* Expand hull */ |
|---|
| 529 | for(;iterations<EPA_maxiterations;++iterations) |
|---|
| 530 | { |
|---|
| 531 | Face* bf = FindBest(); |
|---|
| 532 | if(bf) |
|---|
| 533 | { |
|---|
| 534 | GJK::Mkv* w = Support(-bf->n); |
|---|
| 535 | const F d(bf->n.dot(w->w)+bf->d); |
|---|
| 536 | bestface = bf; |
|---|
| 537 | if(d<-accuracy) |
|---|
| 538 | { |
|---|
| 539 | Face* cf =0; |
|---|
| 540 | Face* ff =0; |
|---|
| 541 | U nf = 0; |
|---|
| 542 | Detach(bf); |
|---|
| 543 | bf->mark=++markid; |
|---|
| 544 | for(U i=0;i<3;++i) { |
|---|
| 545 | nf+=BuildHorizon(markid,w,*bf->f[i],bf->e[i],cf,ff); } |
|---|
| 546 | if(nf<=2) { break; } |
|---|
| 547 | Link(cf,1,ff,2); |
|---|
| 548 | } else break; |
|---|
| 549 | } else break; |
|---|
| 550 | } |
|---|
| 551 | /* Extract contact */ |
|---|
| 552 | if(bestface) |
|---|
| 553 | { |
|---|
| 554 | const Vector3 b(GetCoordinates(bestface)); |
|---|
| 555 | normal = bestface->n; |
|---|
| 556 | depth = Max<F>(0,bestface->d); |
|---|
| 557 | for(U i=0;i<2;++i) |
|---|
| 558 | { |
|---|
| 559 | const F s(F(i?-1:1)); |
|---|
| 560 | for(U j=0;j<3;++j) |
|---|
| 561 | { |
|---|
| 562 | features[i][j]=gjk->LocalSupport(s*bestface->v[j]->r,i); |
|---|
| 563 | } |
|---|
| 564 | } |
|---|
| 565 | nearest[0] = features[0][0]*b.x()+features[0][1]*b.y()+features[0][2]*b.z(); |
|---|
| 566 | nearest[1] = features[1][0]*b.x()+features[1][1]*b.y()+features[1][2]*b.z(); |
|---|
| 567 | } else failed=true; |
|---|
| 568 | sa->endBlock(sablock); |
|---|
| 569 | return(depth); |
|---|
| 570 | } |
|---|
| 571 | }; |
|---|
| 572 | } |
|---|
| 573 | |
|---|
| 574 | // |
|---|
| 575 | // Api |
|---|
| 576 | // |
|---|
| 577 | |
|---|
| 578 | using namespace gjkepa_impl; |
|---|
| 579 | |
|---|
| 580 | |
|---|
| 581 | |
|---|
| 582 | // |
|---|
| 583 | bool btGjkEpaSolver::Collide(const btConvexShape *shape0,const btTransform &wtrs0, |
|---|
| 584 | const btConvexShape *shape1,const btTransform &wtrs1, |
|---|
| 585 | btScalar radialmargin, |
|---|
| 586 | btStackAlloc* stackAlloc, |
|---|
| 587 | sResults& results) |
|---|
| 588 | { |
|---|
| 589 | |
|---|
| 590 | |
|---|
| 591 | /* Initialize */ |
|---|
| 592 | results.witnesses[0] = |
|---|
| 593 | results.witnesses[1] = |
|---|
| 594 | results.normal = Vector3(0,0,0); |
|---|
| 595 | results.depth = 0; |
|---|
| 596 | results.status = sResults::Separated; |
|---|
| 597 | results.epa_iterations = 0; |
|---|
| 598 | results.gjk_iterations = 0; |
|---|
| 599 | /* Use GJK to locate origin */ |
|---|
| 600 | GJK gjk(stackAlloc, |
|---|
| 601 | wtrs0.getBasis(),wtrs0.getOrigin(),shape0, |
|---|
| 602 | wtrs1.getBasis(),wtrs1.getOrigin(),shape1, |
|---|
| 603 | radialmargin+EPA_accuracy); |
|---|
| 604 | const Z collide(gjk.SearchOrigin()); |
|---|
| 605 | results.gjk_iterations = static_cast<int>(gjk.iterations+1); |
|---|
| 606 | if(collide) |
|---|
| 607 | { |
|---|
| 608 | /* Then EPA for penetration depth */ |
|---|
| 609 | EPA epa(&gjk); |
|---|
| 610 | const F pd(epa.EvaluatePD()); |
|---|
| 611 | results.epa_iterations = static_cast<int>(epa.iterations+1); |
|---|
| 612 | if(pd>0) |
|---|
| 613 | { |
|---|
| 614 | results.status = sResults::Penetrating; |
|---|
| 615 | results.normal = epa.normal; |
|---|
| 616 | results.depth = pd; |
|---|
| 617 | results.witnesses[0] = epa.nearest[0]; |
|---|
| 618 | results.witnesses[1] = epa.nearest[1]; |
|---|
| 619 | return(true); |
|---|
| 620 | } else { if(epa.failed) results.status=sResults::EPA_Failed; } |
|---|
| 621 | } else { if(gjk.failed) results.status=sResults::GJK_Failed; } |
|---|
| 622 | return(false); |
|---|
| 623 | } |
|---|
| 624 | |
|---|
| 625 | |
|---|
| 626 | |
|---|
| 627 | |
|---|
| 628 | |
|---|
