1 /* 2 Stan Melax Convex Hull Computation 3 Copyright (c) 2003-2006 Stan Melax http://www.melax.com/ 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 use of this software. 7 Permission is granted to anyone to use this software for any purpose, 8 including commercial applications, and to alter it and redistribute it freely, 9 subject to the following restrictions: 10 11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 13 3. This notice may not be removed or altered from any source distribution. 14 */ 15 16 #include <string.h> 17 18 #include "btConvexHull.h" 19 #include "btAlignedObjectArray.h" 20 #include "btMinMax.h" 21 #include "btVector3.h" 22 23 24 25 26 27 //---------------------------------- 28 29 class int3 30 { 31 public: 32 int x,y,z; 33 int3(){}; 34 int3(int _x,int _y, int _z){x=_x;y=_y;z=_z;} 35 const int& operator[](int i) const {return (&x)[i];} 36 int& operator[](int i) {return (&x)[i];} 37 }; 38 39 40 //------- btPlane ---------- 41 42 43 inline btPlane PlaneFlip(const btPlane &plane){return btPlane(-plane.normal,-plane.dist);} 44 inline int operator==( const btPlane &a, const btPlane &b ) { return (a.normal==b.normal && a.dist==b.dist); } 45 inline int coplanar( const btPlane &a, const btPlane &b ) { return (a==b || a==PlaneFlip(b)); } 46 47 48 //--------- Utility Functions ------ 49 50 btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1); 51 btVector3 PlaneProject(const btPlane &plane, const btVector3 &point); 52 53 btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2); 54 btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2) 55 { 56 btVector3 N1 = p0.normal; 57 btVector3 N2 = p1.normal; 58 btVector3 N3 = p2.normal; 59 60 btVector3 n2n3; n2n3 = N2.cross(N3); 61 btVector3 n3n1; n3n1 = N3.cross(N1); 62 btVector3 n1n2; n1n2 = N1.cross(N2); 63 64 btScalar quotient = (N1.dot(n2n3)); 65 66 btAssert(btFabs(quotient) > btScalar(0.000001)); 67 68 quotient = btScalar(-1.) / quotient; 69 n2n3 *= p0.dist; 70 n3n1 *= p1.dist; 71 n1n2 *= p2.dist; 72 btVector3 potentialVertex = n2n3; 73 potentialVertex += n3n1; 74 potentialVertex += n1n2; 75 potentialVertex *= quotient; 76 77 btVector3 result(potentialVertex.getX(),potentialVertex.getY(),potentialVertex.getZ()); 78 return result; 79 80 } 81 82 btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint=NULL, btVector3 *vpoint=NULL); 83 btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2); 84 btVector3 NormalOf(const btVector3 *vert, const int n); 85 86 87 btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1) 88 { 89 // returns the point where the line p0-p1 intersects the plane n&d 90 static btVector3 dif; 91 dif = p1-p0; 92 btScalar dn= btDot(plane.normal,dif); 93 btScalar t = -(plane.dist+btDot(plane.normal,p0) )/dn; 94 return p0 + (dif*t); 95 } 96 97 btVector3 PlaneProject(const btPlane &plane, const btVector3 &point) 98 { 99 return point - plane.normal * (btDot(point,plane.normal)+plane.dist); 100 } 101 102 btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2) 103 { 104 // return the normal of the triangle 105 // inscribed by v0, v1, and v2 106 btVector3 cp=btCross(v1-v0,v2-v1); 107 btScalar m=cp.length(); 108 if(m==0) return btVector3(1,0,0); 109 return cp*(btScalar(1.0)/m); 110 } 111 112 113 btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint, btVector3 *vpoint) 114 { 115 static btVector3 cp; 116 cp = btCross(udir,vdir).normalized(); 117 118 btScalar distu = -btDot(cp,ustart); 119 btScalar distv = -btDot(cp,vstart); 120 btScalar dist = (btScalar)fabs(distu-distv); 121 if(upoint) 122 { 123 btPlane plane; 124 plane.normal = btCross(vdir,cp).normalized(); 125 plane.dist = -btDot(plane.normal,vstart); 126 *upoint = PlaneLineIntersection(plane,ustart,ustart+udir); 127 } 128 if(vpoint) 129 { 130 btPlane plane; 131 plane.normal = btCross(udir,cp).normalized(); 132 plane.dist = -btDot(plane.normal,ustart); 133 *vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir); 134 } 135 return dist; 136 } 137 138 139 140 141 142 143 144 #define COPLANAR (0) 145 #define UNDER (1) 146 #define OVER (2) 147 #define SPLIT (OVER|UNDER) 148 #define PAPERWIDTH (btScalar(0.001)) 149 150 btScalar planetestepsilon = PAPERWIDTH; 151 152 153 154 typedef ConvexH::HalfEdge HalfEdge; 155 156 ConvexH::ConvexH(int vertices_size,int edges_size,int facets_size) 157 { 158 vertices.resize(vertices_size); 159 edges.resize(edges_size); 160 facets.resize(facets_size); 161 } 162 163 164 int PlaneTest(const btPlane &p, const btVector3 &v); 165 int PlaneTest(const btPlane &p, const btVector3 &v) { 166 btScalar a = btDot(v,p.normal)+p.dist; 167 int flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR); 168 return flag; 169 } 170 171 int SplitTest(ConvexH &convex,const btPlane &plane); 172 int SplitTest(ConvexH &convex,const btPlane &plane) { 173 int flag=0; 174 for(int i=0;i<convex.vertices.size();i++) { 175 flag |= PlaneTest(plane,convex.vertices[i]); 176 } 177 return flag; 178 } 179 180 class VertFlag 181 { 182 public: 183 unsigned char planetest; 184 unsigned char junk; 185 unsigned char undermap; 186 unsigned char overmap; 187 }; 188 class EdgeFlag 189 { 190 public: 191 unsigned char planetest; 192 unsigned char fixes; 193 short undermap; 194 short overmap; 195 }; 196 class PlaneFlag 197 { 198 public: 199 unsigned char undermap; 200 unsigned char overmap; 201 }; 202 class Coplanar{ 203 public: 204 unsigned short ea; 205 unsigned char v0; 206 unsigned char v1; 207 }; 208 209 210 211 212 213 214 215 216 template<class T> 217 int maxdirfiltered(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow) 218 { 219 btAssert(count); 220 int m=-1; 221 for(int i=0;i<count;i++) 222 if(allow[i]) 223 { 224 if(m==-1 || btDot(p[i],dir)>btDot(p[m],dir)) 225 m=i; 226 } 227 btAssert(m!=-1); 228 return m; 229 } 230 231 btVector3 orth(const btVector3 &v); 232 btVector3 orth(const btVector3 &v) 233 { 234 btVector3 a=btCross(v,btVector3(0,0,1)); 235 btVector3 b=btCross(v,btVector3(0,1,0)); 236 if (a.length() > b.length()) 237 { 238 return a.normalized(); 239 } else { 240 return b.normalized(); 241 } 242 } 243 244 245 template<class T> 246 int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow) 247 { 248 int m=-1; 249 while(m==-1) 250 { 251 m = maxdirfiltered(p,count,dir,allow); 252 if(allow[m]==3) return m; 253 T u = orth(dir); 254 T v = btCross(u,dir); 255 int ma=-1; 256 for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0)) 257 { 258 btScalar s = btSin(SIMD_RADS_PER_DEG*(x)); 259 btScalar c = btCos(SIMD_RADS_PER_DEG*(x)); 260 int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow); 261 if(ma==m && mb==m) 262 { 263 allow[m]=3; 264 return m; 265 } 266 if(ma!=-1 && ma!=mb) // Yuck - this is really ugly 267 { 268 int mc = ma; 269 for(btScalar xx = x-btScalar(40.0) ; xx <= x ; xx+= btScalar(5.0)) 270 { 271 btScalar s = btSin(SIMD_RADS_PER_DEG*(xx)); 272 btScalar c = btCos(SIMD_RADS_PER_DEG*(xx)); 273 int md = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow); 274 if(mc==m && md==m) 275 { 276 allow[m]=3; 277 return m; 278 } 279 mc=md; 280 } 281 } 282 ma=mb; 283 } 284 allow[m]=0; 285 m=-1; 286 } 287 btAssert(0); 288 return m; 289 } 290 291 292 293 294 int operator ==(const int3 &a,const int3 &b); 295 int operator ==(const int3 &a,const int3 &b) 296 { 297 for(int i=0;i<3;i++) 298 { 299 if(a[i]!=b[i]) return 0; 300 } 301 return 1; 302 } 303 304 305 int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon); 306 int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon) 307 { 308 btVector3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]); 309 return (btDot(n,p-vertices[t[0]]) > epsilon); // EPSILON??? 310 } 311 int hasedge(const int3 &t, int a,int b); 312 int hasedge(const int3 &t, int a,int b) 313 { 314 for(int i=0;i<3;i++) 315 { 316 int i1= (i+1)%3; 317 if(t[i]==a && t[i1]==b) return 1; 318 } 319 return 0; 320 } 321 int hasvert(const int3 &t, int v); 322 int hasvert(const int3 &t, int v) 323 { 324 return (t[0]==v || t[1]==v || t[2]==v) ; 325 } 326 int shareedge(const int3 &a,const int3 &b); 327 int shareedge(const int3 &a,const int3 &b) 328 { 329 int i; 330 for(i=0;i<3;i++) 331 { 332 int i1= (i+1)%3; 333 if(hasedge(a,b[i1],b[i])) return 1; 334 } 335 return 0; 336 } 337 338 class btHullTriangle; 339 340 341 342 class btHullTriangle : public int3 343 { 344 public: 345 int3 n; 346 int id; 347 int vmax; 348 btScalar rise; 349 btHullTriangle(int a,int b,int c):int3(a,b,c),n(-1,-1,-1) 350 { 351 vmax=-1; 352 rise = btScalar(0.0); 353 } 354 ~btHullTriangle() 355 { 356 } 357 int &neib(int a,int b); 358 }; 359 360 361 int &btHullTriangle::neib(int a,int b) 362 { 363 static int er=-1; 364 int i; 365 for(i=0;i<3;i++) 366 { 367 int i1=(i+1)%3; 368 int i2=(i+2)%3; 369 if((*this)[i]==a && (*this)[i1]==b) return n[i2]; 370 if((*this)[i]==b && (*this)[i1]==a) return n[i2]; 371 } 372 btAssert(0); 373 return er; 374 } 375 void HullLibrary::b2bfix(btHullTriangle* s,btHullTriangle*t) 376 { 377 int i; 378 for(i=0;i<3;i++) 379 { 380 int i1=(i+1)%3; 381 int i2=(i+2)%3; 382 int a = (*s)[i1]; 383 int b = (*s)[i2]; 384 btAssert(m_tris[s->neib(a,b)]->neib(b,a) == s->id); 385 btAssert(m_tris[t->neib(a,b)]->neib(b,a) == t->id); 386 m_tris[s->neib(a,b)]->neib(b,a) = t->neib(b,a); 387 m_tris[t->neib(b,a)]->neib(a,b) = s->neib(a,b); 388 } 389 } 390 391 void HullLibrary::removeb2b(btHullTriangle* s,btHullTriangle*t) 392 { 393 b2bfix(s,t); 394 deAllocateTriangle(s); 395 396 deAllocateTriangle(t); 397 } 398 399 void HullLibrary::checkit(btHullTriangle *t) 400 { 401 (void)t; 402 403 int i; 404 btAssert(m_tris[t->id]==t); 405 for(i=0;i<3;i++) 406 { 407 int i1=(i+1)%3; 408 int i2=(i+2)%3; 409 int a = (*t)[i1]; 410 int b = (*t)[i2]; 411 412 // release compile fix 413 (void)i1; 414 (void)i2; 415 (void)a; 416 (void)b; 417 418 btAssert(a!=b); 419 btAssert( m_tris[t->n[i]]->neib(b,a) == t->id); 420 } 421 } 422 423 btHullTriangle* HullLibrary::allocateTriangle(int a,int b,int c) 424 { 425 void* mem = btAlignedAlloc(sizeof(btHullTriangle),16); 426 btHullTriangle* tr = new (mem)btHullTriangle(a,b,c); 427 tr->id = m_tris.size(); 428 m_tris.push_back(tr); 429 430 return tr; 431 } 432 433 void HullLibrary::deAllocateTriangle(btHullTriangle* tri) 434 { 435 btAssert(m_tris[tri->id]==tri); 436 m_tris[tri->id]=NULL; 437 tri->~btHullTriangle(); 438 btAlignedFree(tri); 439 } 440 441 442 void HullLibrary::extrude(btHullTriangle *t0,int v) 443 { 444 int3 t= *t0; 445 int n = m_tris.size(); 446 btHullTriangle* ta = allocateTriangle(v,t[1],t[2]); 447 ta->n = int3(t0->n[0],n+1,n+2); 448 m_tris[t0->n[0]]->neib(t[1],t[2]) = n+0; 449 btHullTriangle* tb = allocateTriangle(v,t[2],t[0]); 450 tb->n = int3(t0->n[1],n+2,n+0); 451 m_tris[t0->n[1]]->neib(t[2],t[0]) = n+1; 452 btHullTriangle* tc = allocateTriangle(v,t[0],t[1]); 453 tc->n = int3(t0->n[2],n+0,n+1); 454 m_tris[t0->n[2]]->neib(t[0],t[1]) = n+2; 455 checkit(ta); 456 checkit(tb); 457 checkit(tc); 458 if(hasvert(*m_tris[ta->n[0]],v)) removeb2b(ta,m_tris[ta->n[0]]); 459 if(hasvert(*m_tris[tb->n[0]],v)) removeb2b(tb,m_tris[tb->n[0]]); 460 if(hasvert(*m_tris[tc->n[0]],v)) removeb2b(tc,m_tris[tc->n[0]]); 461 deAllocateTriangle(t0); 462 463 } 464 465 btHullTriangle* HullLibrary::extrudable(btScalar epsilon) 466 { 467 int i; 468 btHullTriangle *t=NULL; 469 for(i=0;i<m_tris.size();i++) 470 { 471 if(!t || (m_tris[i] && t->rise<m_tris[i]->rise)) 472 { 473 t = m_tris[i]; 474 } 475 } 476 return (t->rise >epsilon)?t:NULL ; 477 } 478 479 480 481 482 int4 HullLibrary::FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray<int> &allow) 483 { 484 btVector3 basis[3]; 485 basis[0] = btVector3( btScalar(0.01), btScalar(0.02), btScalar(1.0) ); 486 int p0 = maxdirsterid(verts,verts_count, basis[0],allow); 487 int p1 = maxdirsterid(verts,verts_count,-basis[0],allow); 488 basis[0] = verts[p0]-verts[p1]; 489 if(p0==p1 || basis[0]==btVector3(0,0,0)) 490 return int4(-1,-1,-1,-1); 491 basis[1] = btCross(btVector3( btScalar(1),btScalar(0.02), btScalar(0)),basis[0]); 492 basis[2] = btCross(btVector3(btScalar(-0.02), btScalar(1), btScalar(0)),basis[0]); 493 if (basis[1].length() > basis[2].length()) 494 { 495 basis[1].normalize(); 496 } else { 497 basis[1] = basis[2]; 498 basis[1].normalize (); 499 } 500 int p2 = maxdirsterid(verts,verts_count,basis[1],allow); 501 if(p2 == p0 || p2 == p1) 502 { 503 p2 = maxdirsterid(verts,verts_count,-basis[1],allow); 504 } 505 if(p2 == p0 || p2 == p1) 506 return int4(-1,-1,-1,-1); 507 basis[1] = verts[p2] - verts[p0]; 508 basis[2] = btCross(basis[1],basis[0]).normalized(); 509 int p3 = maxdirsterid(verts,verts_count,basis[2],allow); 510 if(p3==p0||p3==p1||p3==p2) p3 = maxdirsterid(verts,verts_count,-basis[2],allow); 511 if(p3==p0||p3==p1||p3==p2) 512 return int4(-1,-1,-1,-1); 513 btAssert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3)); 514 if(btDot(verts[p3]-verts[p0],btCross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {btSwap(p2,p3);} 515 return int4(p0,p1,p2,p3); 516 } 517 518 int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit) 519 { 520 if(verts_count <4) return 0; 521 if(vlimit==0) vlimit=1000000000; 522 int j; 523 btVector3 bmin(*verts),bmax(*verts); 524 btAlignedObjectArray<int> isextreme; 525 isextreme.reserve(verts_count); 526 btAlignedObjectArray<int> allow; 527 allow.reserve(verts_count); 528 529 for(j=0;j<verts_count;j++) 530 { 531 allow.push_back(1); 532 isextreme.push_back(0); 533 bmin.setMin (verts[j]); 534 bmax.setMax (verts[j]); 535 } 536 btScalar epsilon = (bmax-bmin).length() * btScalar(0.001); 537 btAssert (epsilon != 0.0); 538 539 540 int4 p = FindSimplex(verts,verts_count,allow); 541 if(p.x==-1) return 0; // simplex failed 542 543 544 545 btVector3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) / btScalar(4.0); // a valid interior point 546 btHullTriangle *t0 = allocateTriangle(p[2],p[3],p[1]); t0->n=int3(2,3,1); 547 btHullTriangle *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0); 548 btHullTriangle *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3); 549 btHullTriangle *t3 = allocateTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2); 550 isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1; 551 checkit(t0);checkit(t1);checkit(t2);checkit(t3); 552 553 for(j=0;j<m_tris.size();j++) 554 { 555 btHullTriangle *t=m_tris[j]; 556 btAssert(t); 557 btAssert(t->vmax<0); 558 btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); 559 t->vmax = maxdirsterid(verts,verts_count,n,allow); 560 t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]); 561 } 562 btHullTriangle *te; 563 vlimit-=4; 564 while(vlimit >0 && ((te=extrudable(epsilon)) != 0)) 565 { 566 //int3 ti=*te; 567 int v=te->vmax; 568 btAssert(v != -1); 569 btAssert(!isextreme[v]); // wtf we've already done this vertex 570 isextreme[v]=1; 571 //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already 572 j=m_tris.size(); 573 while(j--) { 574 if(!m_tris[j]) continue; 575 int3 t=*m_tris[j]; 576 if(above(verts,t,verts[v],btScalar(0.01)*epsilon)) 577 { 578 extrude(m_tris[j],v); 579 } 580 } 581 // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle 582 j=m_tris.size(); 583 while(j--) 584 { 585 if(!m_tris[j]) continue; 586 if(!hasvert(*m_tris[j],v)) break; 587 int3 nt=*m_tris[j]; 588 if(above(verts,nt,center,btScalar(0.01)*epsilon) || btCross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) ) 589 { 590 btHullTriangle *nb = m_tris[m_tris[j]->n[0]]; 591 btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->id<j); 592 extrude(nb,v); 593 j=m_tris.size(); 594 } 595 } 596 j=m_tris.size(); 597 while(j--) 598 { 599 btHullTriangle *t=m_tris[j]; 600 if(!t) continue; 601 if(t->vmax>=0) break; 602 btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); 603 t->vmax = maxdirsterid(verts,verts_count,n,allow); 604 if(isextreme[t->vmax]) 605 { 606 t->vmax=-1; // already done that vertex - algorithm needs to be able to terminate. 607 } 608 else 609 { 610 t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]); 611 } 612 } 613 vlimit --; 614 } 615 return 1; 616 } 617 618 int HullLibrary::calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit) 619 { 620 int rc=calchullgen(verts,verts_count, vlimit) ; 621 if(!rc) return 0; 622 btAlignedObjectArray<int> ts; 623 int i; 624 625 for(i=0;i<m_tris.size();i++) 626 { 627 if(m_tris[i]) 628 { 629 for(int j=0;j<3;j++) 630 ts.push_back((*m_tris[i])[j]); 631 deAllocateTriangle(m_tris[i]); 632 } 633 } 634 tris_count = ts.size()/3; 635 tris_out.resize(ts.size()); 636 637 for (i=0;i<ts.size();i++) 638 { 639 tris_out[i] = static_cast<unsigned int>(ts[i]); 640 } 641 m_tris.resize(0); 642 643 return 1; 644 } 645 646 647 648 649 650 bool HullLibrary::ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit) 651 { 652 653 int tris_count; 654 int ret = calchull( (btVector3 *) vertices, (int) vcount, result.m_Indices, tris_count, static_cast<int>(vlimit) ); 655 if(!ret) return false; 656 result.mIndexCount = (unsigned int) (tris_count*3); 657 result.mFaceCount = (unsigned int) tris_count; 658 result.mVertices = (btVector3*) vertices; 659 result.mVcount = (unsigned int) vcount; 660 return true; 661 662 } 663 664 665 void ReleaseHull(PHullResult &result); 666 void ReleaseHull(PHullResult &result) 667 { 668 if ( result.m_Indices.size() ) 669 { 670 result.m_Indices.clear(); 671 } 672 673 result.mVcount = 0; 674 result.mIndexCount = 0; 675 result.mVertices = 0; 676 } 677 678 679 //********************************************************************* 680 //********************************************************************* 681 //******** HullLib header 682 //********************************************************************* 683 //********************************************************************* 684 685 //********************************************************************* 686 //********************************************************************* 687 //******** HullLib implementation 688 //********************************************************************* 689 //********************************************************************* 690 691 HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // describes the input request 692 HullResult &result) // contains the resulst 693 { 694 HullError ret = QE_FAIL; 695 696 697 PHullResult hr; 698 699 unsigned int vcount = desc.mVcount; 700 if ( vcount < 8 ) vcount = 8; 701 702 btAlignedObjectArray<btVector3> vertexSource; 703 vertexSource.resize(static_cast<int>(vcount)); 704 705 btVector3 scale; 706 707 unsigned int ovcount; 708 709 bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, &vertexSource[0], desc.mNormalEpsilon, scale ); // normalize point cloud, remove duplicates! 710 711 if ( ok ) 712 { 713 714 715 // if ( 1 ) // scale vertices back to their original size. 716 { 717 for (unsigned int i=0; i<ovcount; i++) 718 { 719 btVector3& v = vertexSource[static_cast<int>(i)]; 720 v[0]*=scale[0]; 721 v[1]*=scale[1]; 722 v[2]*=scale[2]; 723 } 724 } 725 726 ok = ComputeHull(ovcount,&vertexSource[0],hr,desc.mMaxVertices); 727 728 if ( ok ) 729 { 730 731 // re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table. 732 btAlignedObjectArray<btVector3> vertexScratch; 733 vertexScratch.resize(static_cast<int>(hr.mVcount)); 734 735 BringOutYourDead(hr.mVertices,hr.mVcount, &vertexScratch[0], ovcount, &hr.m_Indices[0], hr.mIndexCount ); 736 737 ret = QE_OK; 738 739 if ( desc.HasHullFlag(QF_TRIANGLES) ) // if he wants the results as triangle! 740 { 741 result.mPolygons = false; 742 result.mNumOutputVertices = ovcount; 743 result.m_OutputVertices.resize(static_cast<int>(ovcount)); 744 result.mNumFaces = hr.mFaceCount; 745 result.mNumIndices = hr.mIndexCount; 746 747 result.m_Indices.resize(static_cast<int>(hr.mIndexCount)); 748 749 memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount ); 750 751 if ( desc.HasHullFlag(QF_REVERSE_ORDER) ) 752 { 753 754 const unsigned int *source = &hr.m_Indices[0]; 755 unsigned int *dest = &result.m_Indices[0]; 756 757 for (unsigned int i=0; i<hr.mFaceCount; i++) 758 { 759 dest[0] = source[2]; 760 dest[1] = source[1]; 761 dest[2] = source[0]; 762 dest+=3; 763 source+=3; 764 } 765 766 } 767 else 768 { 769 memcpy(&result.m_Indices[0], &hr.m_Indices[0], sizeof(unsigned int)*hr.mIndexCount); 770 } 771 } 772 else 773 { 774 result.mPolygons = true; 775 result.mNumOutputVertices = ovcount; 776 result.m_OutputVertices.resize(static_cast<int>(ovcount)); 777 result.mNumFaces = hr.mFaceCount; 778 result.mNumIndices = hr.mIndexCount+hr.mFaceCount; 779 result.m_Indices.resize(static_cast<int>(result.mNumIndices)); 780 memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount ); 781 782 // if ( 1 ) 783 { 784 const unsigned int *source = &hr.m_Indices[0]; 785 unsigned int *dest = &result.m_Indices[0]; 786 for (unsigned int i=0; i<hr.mFaceCount; i++) 787 { 788 dest[0] = 3; 789 if ( desc.HasHullFlag(QF_REVERSE_ORDER) ) 790 { 791 dest[1] = source[2]; 792 dest[2] = source[1]; 793 dest[3] = source[0]; 794 } 795 else 796 { 797 dest[1] = source[0]; 798 dest[2] = source[1]; 799 dest[3] = source[2]; 800 } 801 802 dest+=4; 803 source+=3; 804 } 805 } 806 } 807 ReleaseHull(hr); 808 } 809 } 810 811 return ret; 812 } 813 814 815 816 HullError HullLibrary::ReleaseResult(HullResult &result) // release memory allocated for this result, we are done with it. 817 { 818 if ( result.m_OutputVertices.size()) 819 { 820 result.mNumOutputVertices=0; 821 result.m_OutputVertices.clear(); 822 } 823 if ( result.m_Indices.size() ) 824 { 825 result.mNumIndices=0; 826 result.m_Indices.clear(); 827 } 828 return QE_OK; 829 } 830 831 832 static void addPoint(unsigned int &vcount,btVector3 *p,btScalar x,btScalar y,btScalar z) 833 { 834 // XXX, might be broken 835 btVector3& dest = p[vcount]; 836 dest[0] = x; 837 dest[1] = y; 838 dest[2] = z; 839 vcount++; 840 } 841 842 btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2); 843 btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2) 844 { 845 846 btScalar dx = px - p2[0]; 847 btScalar dy = py - p2[1]; 848 btScalar dz = pz - p2[2]; 849 850 return dx*dx+dy*dy+dz*dz; 851 } 852 853 854 855 bool HullLibrary::CleanupVertices(unsigned int svcount, 856 const btVector3 *svertices, 857 unsigned int stride, 858 unsigned int &vcount, // output number of vertices 859 btVector3 *vertices, // location to store the results. 860 btScalar normalepsilon, 861 btVector3& scale) 862 { 863 if ( svcount == 0 ) return false; 864 865 m_vertexIndexMapping.resize(0); 866 867 868 #define EPSILON btScalar(0.000001) /* close enough to consider two btScalaring point numbers to be 'the same'. */ 869 870 vcount = 0; 871 872 btScalar recip[3]={0.f,0.f,0.f}; 873 874 if ( scale ) 875 { 876 scale[0] = 1; 877 scale[1] = 1; 878 scale[2] = 1; 879 } 880 881 btScalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX }; 882 btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX }; 883 884 const char *vtx = (const char *) svertices; 885 886 // if ( 1 ) 887 { 888 for (unsigned int i=0; i<svcount; i++) 889 { 890 const btScalar *p = (const btScalar *) vtx; 891 892 vtx+=stride; 893 894 for (int j=0; j<3; j++) 895 { 896 if ( p[j] < bmin[j] ) bmin[j] = p[j]; 897 if ( p[j] > bmax[j] ) bmax[j] = p[j]; 898 } 899 } 900 } 901 902 btScalar dx = bmax[0] - bmin[0]; 903 btScalar dy = bmax[1] - bmin[1]; 904 btScalar dz = bmax[2] - bmin[2]; 905 906 btVector3 center; 907 908 center[0] = dx*btScalar(0.5) + bmin[0]; 909 center[1] = dy*btScalar(0.5) + bmin[1]; 910 center[2] = dz*btScalar(0.5) + bmin[2]; 911 912 if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3 ) 913 { 914 915 btScalar len = FLT_MAX; 916 917 if ( dx > EPSILON && dx < len ) len = dx; 918 if ( dy > EPSILON && dy < len ) len = dy; 919 if ( dz > EPSILON && dz < len ) len = dz; 920 921 if ( len == FLT_MAX ) 922 { 923 dx = dy = dz = btScalar(0.01); // one centimeter 924 } 925 else 926 { 927 if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge. 928 if ( dy < EPSILON ) dy = len * btScalar(0.05); 929 if ( dz < EPSILON ) dz = len * btScalar(0.05); 930 } 931 932 btScalar x1 = center[0] - dx; 933 btScalar x2 = center[0] + dx; 934 935 btScalar y1 = center[1] - dy; 936 btScalar y2 = center[1] + dy; 937 938 btScalar z1 = center[2] - dz; 939 btScalar z2 = center[2] + dz; 940 941 addPoint(vcount,vertices,x1,y1,z1); 942 addPoint(vcount,vertices,x2,y1,z1); 943 addPoint(vcount,vertices,x2,y2,z1); 944 addPoint(vcount,vertices,x1,y2,z1); 945 addPoint(vcount,vertices,x1,y1,z2); 946 addPoint(vcount,vertices,x2,y1,z2); 947 addPoint(vcount,vertices,x2,y2,z2); 948 addPoint(vcount,vertices,x1,y2,z2); 949 950 return true; // return cube 951 952 953 } 954 else 955 { 956 if ( scale ) 957 { 958 scale[0] = dx; 959 scale[1] = dy; 960 scale[2] = dz; 961 962 recip[0] = 1 / dx; 963 recip[1] = 1 / dy; 964 recip[2] = 1 / dz; 965 966 center[0]*=recip[0]; 967 center[1]*=recip[1]; 968 center[2]*=recip[2]; 969 970 } 971 972 } 973 974 975 976 vtx = (const char *) svertices; 977 978 for (unsigned int i=0; i<svcount; i++) 979 { 980 const btVector3 *p = (const btVector3 *)vtx; 981 vtx+=stride; 982 983 btScalar px = p->getX(); 984 btScalar py = p->getY(); 985 btScalar pz = p->getZ(); 986 987 if ( scale ) 988 { 989 px = px*recip[0]; // normalize 990 py = py*recip[1]; // normalize 991 pz = pz*recip[2]; // normalize 992 } 993 994 // if ( 1 ) 995 { 996 unsigned int j; 997 998 for (j=0; j<vcount; j++) 999 { 1000 /// XXX might be broken 1001 btVector3& v = vertices[j]; 1002 1003 btScalar x = v[0]; 1004 btScalar y = v[1]; 1005 btScalar z = v[2]; 1006 1007 btScalar dx = btFabs(x - px ); 1008 btScalar dy = btFabs(y - py ); 1009 btScalar dz = btFabs(z - pz ); 1010 1011 if ( dx < normalepsilon && dy < normalepsilon && dz < normalepsilon ) 1012 { 1013 // ok, it is close enough to the old one 1014 // now let us see if it is further from the center of the point cloud than the one we already recorded. 1015 // in which case we keep this one instead. 1016 1017 btScalar dist1 = GetDist(px,py,pz,center); 1018 btScalar dist2 = GetDist(v[0],v[1],v[2],center); 1019 1020 if ( dist1 > dist2 ) 1021 { 1022 v[0] = px; 1023 v[1] = py; 1024 v[2] = pz; 1025 1026 } 1027 1028 break; 1029 } 1030 } 1031 1032 if ( j == vcount ) 1033 { 1034 btVector3& dest = vertices[vcount]; 1035 dest[0] = px; 1036 dest[1] = py; 1037 dest[2] = pz; 1038 vcount++; 1039 } 1040 m_vertexIndexMapping.push_back(j); 1041 } 1042 } 1043 1044 // ok..now make sure we didn't prune so many vertices it is now invalid. 1045 // if ( 1 ) 1046 { 1047 btScalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX }; 1048 btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX }; 1049 1050 for (unsigned int i=0; i<vcount; i++) 1051 { 1052 const btVector3& p = vertices[i]; 1053 for (int j=0; j<3; j++) 1054 { 1055 if ( p[j] < bmin[j] ) bmin[j] = p[j]; 1056 if ( p[j] > bmax[j] ) bmax[j] = p[j]; 1057 } 1058 } 1059 1060 btScalar dx = bmax[0] - bmin[0]; 1061 btScalar dy = bmax[1] - bmin[1]; 1062 btScalar dz = bmax[2] - bmin[2]; 1063 1064 if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3) 1065 { 1066 btScalar cx = dx*btScalar(0.5) + bmin[0]; 1067 btScalar cy = dy*btScalar(0.5) + bmin[1]; 1068 btScalar cz = dz*btScalar(0.5) + bmin[2]; 1069 1070 btScalar len = FLT_MAX; 1071 1072 if ( dx >= EPSILON && dx < len ) len = dx; 1073 if ( dy >= EPSILON && dy < len ) len = dy; 1074 if ( dz >= EPSILON && dz < len ) len = dz; 1075 1076 if ( len == FLT_MAX ) 1077 { 1078 dx = dy = dz = btScalar(0.01); // one centimeter 1079 } 1080 else 1081 { 1082 if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge. 1083 if ( dy < EPSILON ) dy = len * btScalar(0.05); 1084 if ( dz < EPSILON ) dz = len * btScalar(0.05); 1085 } 1086 1087 btScalar x1 = cx - dx; 1088 btScalar x2 = cx + dx; 1089 1090 btScalar y1 = cy - dy; 1091 btScalar y2 = cy + dy; 1092 1093 btScalar z1 = cz - dz; 1094 btScalar z2 = cz + dz; 1095 1096 vcount = 0; // add box 1097 1098 addPoint(vcount,vertices,x1,y1,z1); 1099 addPoint(vcount,vertices,x2,y1,z1); 1100 addPoint(vcount,vertices,x2,y2,z1); 1101 addPoint(vcount,vertices,x1,y2,z1); 1102 addPoint(vcount,vertices,x1,y1,z2); 1103 addPoint(vcount,vertices,x2,y1,z2); 1104 addPoint(vcount,vertices,x2,y2,z2); 1105 addPoint(vcount,vertices,x1,y2,z2); 1106 1107 return true; 1108 } 1109 } 1110 1111 return true; 1112 } 1113 1114 void HullLibrary::BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount) 1115 { 1116 btAlignedObjectArray<int>tmpIndices; 1117 tmpIndices.resize(m_vertexIndexMapping.size()); 1118 int i; 1119 1120 for (i=0;i<m_vertexIndexMapping.size();i++) 1121 { 1122 tmpIndices[i] = m_vertexIndexMapping[i]; 1123 } 1124 1125 TUIntArray usedIndices; 1126 usedIndices.resize(static_cast<int>(vcount)); 1127 memset(&usedIndices[0],0,sizeof(unsigned int)*vcount); 1128 1129 ocount = 0; 1130 1131 for (i=0; i<int (indexcount); i++) 1132 { 1133 unsigned int v = indices[i]; // original array index 1134 1135 btAssert( v >= 0 && v < vcount ); 1136 1137 if ( usedIndices[static_cast<int>(v)] ) // if already remapped 1138 { 1139 indices[i] = usedIndices[static_cast<int>(v)]-1; // index to new array 1140 } 1141 else 1142 { 1143 1144 indices[i] = ocount; // new index mapping 1145 1146 overts[ocount][0] = verts[v][0]; // copy old vert to new vert array 1147 overts[ocount][1] = verts[v][1]; 1148 overts[ocount][2] = verts[v][2]; 1149 1150 for (int k=0;k<m_vertexIndexMapping.size();k++) 1151 { 1152 if (tmpIndices[k]==int(v)) 1153 m_vertexIndexMapping[k]=ocount; 1154 } 1155 1156 ocount++; // increment output vert count 1157 1158 btAssert( ocount >=0 && ocount <= vcount ); 1159 1160 usedIndices[static_cast<int>(v)] = ocount; // assign new index remapping 1161 1162 1163 } 1164 } 1165 1166 1167 } 1168
