00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016 #include <string.h>
00017
00018 #include "btConvexHull.h"
00019 #include "btAlignedObjectArray.h"
00020 #include "btMinMax.h"
00021 #include "btVector3.h"
00022
00023
00024
00025
00026
00027
00028
00029 class int3
00030 {
00031 public:
00032 int x,y,z;
00033 int3(){};
00034 int3(int _x,int _y, int _z){x=_x;y=_y;z=_z;}
00035 const int& operator[](int i) const {return (&x)[i];}
00036 int& operator[](int i) {return (&x)[i];}
00037 };
00038
00039
00040
00041
00042
00043 inline btPlane PlaneFlip(const btPlane &plane){return btPlane(-plane.normal,-plane.dist);}
00044 inline int operator==( const btPlane &a, const btPlane &b ) { return (a.normal==b.normal && a.dist==b.dist); }
00045 inline int coplanar( const btPlane &a, const btPlane &b ) { return (a==b || a==PlaneFlip(b)); }
00046
00047
00048
00049
00050 btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1);
00051 btVector3 PlaneProject(const btPlane &plane, const btVector3 &point);
00052
00053 btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2);
00054 btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2)
00055 {
00056 btVector3 N1 = p0.normal;
00057 btVector3 N2 = p1.normal;
00058 btVector3 N3 = p2.normal;
00059
00060 btVector3 n2n3; n2n3 = N2.cross(N3);
00061 btVector3 n3n1; n3n1 = N3.cross(N1);
00062 btVector3 n1n2; n1n2 = N1.cross(N2);
00063
00064 btScalar quotient = (N1.dot(n2n3));
00065
00066 btAssert(btFabs(quotient) > btScalar(0.000001));
00067
00068 quotient = btScalar(-1.) / quotient;
00069 n2n3 *= p0.dist;
00070 n3n1 *= p1.dist;
00071 n1n2 *= p2.dist;
00072 btVector3 potentialVertex = n2n3;
00073 potentialVertex += n3n1;
00074 potentialVertex += n1n2;
00075 potentialVertex *= quotient;
00076
00077 btVector3 result(potentialVertex.getX(),potentialVertex.getY(),potentialVertex.getZ());
00078 return result;
00079
00080 }
00081
00082 btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint=NULL, btVector3 *vpoint=NULL);
00083 btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2);
00084 btVector3 NormalOf(const btVector3 *vert, const int n);
00085
00086
00087 btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1)
00088 {
00089
00090 static btVector3 dif;
00091 dif = p1-p0;
00092 btScalar dn= btDot(plane.normal,dif);
00093 btScalar t = -(plane.dist+btDot(plane.normal,p0) )/dn;
00094 return p0 + (dif*t);
00095 }
00096
00097 btVector3 PlaneProject(const btPlane &plane, const btVector3 &point)
00098 {
00099 return point - plane.normal * (btDot(point,plane.normal)+plane.dist);
00100 }
00101
00102 btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2)
00103 {
00104
00105
00106 btVector3 cp=btCross(v1-v0,v2-v1);
00107 btScalar m=cp.length();
00108 if(m==0) return btVector3(1,0,0);
00109 return cp*(btScalar(1.0)/m);
00110 }
00111
00112
00113 btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint, btVector3 *vpoint)
00114 {
00115 static btVector3 cp;
00116 cp = btCross(udir,vdir).normalized();
00117
00118 btScalar distu = -btDot(cp,ustart);
00119 btScalar distv = -btDot(cp,vstart);
00120 btScalar dist = (btScalar)fabs(distu-distv);
00121 if(upoint)
00122 {
00123 btPlane plane;
00124 plane.normal = btCross(vdir,cp).normalized();
00125 plane.dist = -btDot(plane.normal,vstart);
00126 *upoint = PlaneLineIntersection(plane,ustart,ustart+udir);
00127 }
00128 if(vpoint)
00129 {
00130 btPlane plane;
00131 plane.normal = btCross(udir,cp).normalized();
00132 plane.dist = -btDot(plane.normal,ustart);
00133 *vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir);
00134 }
00135 return dist;
00136 }
00137
00138
00139
00140
00141
00142
00143
00144 #define COPLANAR (0)
00145 #define UNDER (1)
00146 #define OVER (2)
00147 #define SPLIT (OVER|UNDER)
00148 #define PAPERWIDTH (btScalar(0.001))
00149
00150 btScalar planetestepsilon = PAPERWIDTH;
00151
00152
00153
00154 typedef ConvexH::HalfEdge HalfEdge;
00155
00156 ConvexH::ConvexH(int vertices_size,int edges_size,int facets_size)
00157 {
00158 vertices.resize(vertices_size);
00159 edges.resize(edges_size);
00160 facets.resize(facets_size);
00161 }
00162
00163
00164 int PlaneTest(const btPlane &p, const btVector3 &v);
00165 int PlaneTest(const btPlane &p, const btVector3 &v) {
00166 btScalar a = btDot(v,p.normal)+p.dist;
00167 int flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR);
00168 return flag;
00169 }
00170
00171 int SplitTest(ConvexH &convex,const btPlane &plane);
00172 int SplitTest(ConvexH &convex,const btPlane &plane) {
00173 int flag=0;
00174 for(int i=0;i<convex.vertices.size();i++) {
00175 flag |= PlaneTest(plane,convex.vertices[i]);
00176 }
00177 return flag;
00178 }
00179
00180 class VertFlag
00181 {
00182 public:
00183 unsigned char planetest;
00184 unsigned char junk;
00185 unsigned char undermap;
00186 unsigned char overmap;
00187 };
00188 class EdgeFlag
00189 {
00190 public:
00191 unsigned char planetest;
00192 unsigned char fixes;
00193 short undermap;
00194 short overmap;
00195 };
00196 class PlaneFlag
00197 {
00198 public:
00199 unsigned char undermap;
00200 unsigned char overmap;
00201 };
00202 class Coplanar{
00203 public:
00204 unsigned short ea;
00205 unsigned char v0;
00206 unsigned char v1;
00207 };
00208
00209
00210
00211
00212
00213
00214
00215
00216 template<class T>
00217 int maxdirfiltered(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow)
00218 {
00219 btAssert(count);
00220 int m=-1;
00221 for(int i=0;i<count;i++)
00222 if(allow[i])
00223 {
00224 if(m==-1 || btDot(p[i],dir)>btDot(p[m],dir))
00225 m=i;
00226 }
00227 btAssert(m!=-1);
00228 return m;
00229 }
00230
00231 btVector3 orth(const btVector3 &v);
00232 btVector3 orth(const btVector3 &v)
00233 {
00234 btVector3 a=btCross(v,btVector3(0,0,1));
00235 btVector3 b=btCross(v,btVector3(0,1,0));
00236 if (a.length() > b.length())
00237 {
00238 return a.normalized();
00239 } else {
00240 return b.normalized();
00241 }
00242 }
00243
00244
00245 template<class T>
00246 int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow)
00247 {
00248 int m=-1;
00249 while(m==-1)
00250 {
00251 m = maxdirfiltered(p,count,dir,allow);
00252 if(allow[m]==3) return m;
00253 T u = orth(dir);
00254 T v = btCross(u,dir);
00255 int ma=-1;
00256 for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0))
00257 {
00258 btScalar s = btSin(SIMD_RADS_PER_DEG*(x));
00259 btScalar c = btCos(SIMD_RADS_PER_DEG*(x));
00260 int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
00261 if(ma==m && mb==m)
00262 {
00263 allow[m]=3;
00264 return m;
00265 }
00266 if(ma!=-1 && ma!=mb)
00267 {
00268 int mc = ma;
00269 for(btScalar xx = x-btScalar(40.0) ; xx <= x ; xx+= btScalar(5.0))
00270 {
00271 btScalar s = btSin(SIMD_RADS_PER_DEG*(xx));
00272 btScalar c = btCos(SIMD_RADS_PER_DEG*(xx));
00273 int md = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow);
00274 if(mc==m && md==m)
00275 {
00276 allow[m]=3;
00277 return m;
00278 }
00279 mc=md;
00280 }
00281 }
00282 ma=mb;
00283 }
00284 allow[m]=0;
00285 m=-1;
00286 }
00287 btAssert(0);
00288 return m;
00289 }
00290
00291
00292
00293
00294 int operator ==(const int3 &a,const int3 &b);
00295 int operator ==(const int3 &a,const int3 &b)
00296 {
00297 for(int i=0;i<3;i++)
00298 {
00299 if(a[i]!=b[i]) return 0;
00300 }
00301 return 1;
00302 }
00303
00304
00305 int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon);
00306 int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon)
00307 {
00308 btVector3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]);
00309 return (btDot(n,p-vertices[t[0]]) > epsilon);
00310 }
00311 int hasedge(const int3 &t, int a,int b);
00312 int hasedge(const int3 &t, int a,int b)
00313 {
00314 for(int i=0;i<3;i++)
00315 {
00316 int i1= (i+1)%3;
00317 if(t[i]==a && t[i1]==b) return 1;
00318 }
00319 return 0;
00320 }
00321 int hasvert(const int3 &t, int v);
00322 int hasvert(const int3 &t, int v)
00323 {
00324 return (t[0]==v || t[1]==v || t[2]==v) ;
00325 }
00326 int shareedge(const int3 &a,const int3 &b);
00327 int shareedge(const int3 &a,const int3 &b)
00328 {
00329 int i;
00330 for(i=0;i<3;i++)
00331 {
00332 int i1= (i+1)%3;
00333 if(hasedge(a,b[i1],b[i])) return 1;
00334 }
00335 return 0;
00336 }
00337
00338 class btHullTriangle;
00339
00340
00341
00342 class btHullTriangle : public int3
00343 {
00344 public:
00345 int3 n;
00346 int id;
00347 int vmax;
00348 btScalar rise;
00349 btHullTriangle(int a,int b,int c):int3(a,b,c),n(-1,-1,-1)
00350 {
00351 vmax=-1;
00352 rise = btScalar(0.0);
00353 }
00354 ~btHullTriangle()
00355 {
00356 }
00357 int &neib(int a,int b);
00358 };
00359
00360
00361 int &btHullTriangle::neib(int a,int b)
00362 {
00363 static int er=-1;
00364 int i;
00365 for(i=0;i<3;i++)
00366 {
00367 int i1=(i+1)%3;
00368 int i2=(i+2)%3;
00369 if((*this)[i]==a && (*this)[i1]==b) return n[i2];
00370 if((*this)[i]==b && (*this)[i1]==a) return n[i2];
00371 }
00372 btAssert(0);
00373 return er;
00374 }
00375 void HullLibrary::b2bfix(btHullTriangle* s,btHullTriangle*t)
00376 {
00377 int i;
00378 for(i=0;i<3;i++)
00379 {
00380 int i1=(i+1)%3;
00381 int i2=(i+2)%3;
00382 int a = (*s)[i1];
00383 int b = (*s)[i2];
00384 btAssert(m_tris[s->neib(a,b)]->neib(b,a) == s->id);
00385 btAssert(m_tris[t->neib(a,b)]->neib(b,a) == t->id);
00386 m_tris[s->neib(a,b)]->neib(b,a) = t->neib(b,a);
00387 m_tris[t->neib(b,a)]->neib(a,b) = s->neib(a,b);
00388 }
00389 }
00390
00391 void HullLibrary::removeb2b(btHullTriangle* s,btHullTriangle*t)
00392 {
00393 b2bfix(s,t);
00394 deAllocateTriangle(s);
00395
00396 deAllocateTriangle(t);
00397 }
00398
00399 void HullLibrary::checkit(btHullTriangle *t)
00400 {
00401 (void)t;
00402
00403 int i;
00404 btAssert(m_tris[t->id]==t);
00405 for(i=0;i<3;i++)
00406 {
00407 int i1=(i+1)%3;
00408 int i2=(i+2)%3;
00409 int a = (*t)[i1];
00410 int b = (*t)[i2];
00411
00412
00413 (void)i1;
00414 (void)i2;
00415 (void)a;
00416 (void)b;
00417
00418 btAssert(a!=b);
00419 btAssert( m_tris[t->n[i]]->neib(b,a) == t->id);
00420 }
00421 }
00422
00423 btHullTriangle* HullLibrary::allocateTriangle(int a,int b,int c)
00424 {
00425 void* mem = btAlignedAlloc(sizeof(btHullTriangle),16);
00426 btHullTriangle* tr = new (mem)btHullTriangle(a,b,c);
00427 tr->id = m_tris.size();
00428 m_tris.push_back(tr);
00429
00430 return tr;
00431 }
00432
00433 void HullLibrary::deAllocateTriangle(btHullTriangle* tri)
00434 {
00435 btAssert(m_tris[tri->id]==tri);
00436 m_tris[tri->id]=NULL;
00437 tri->~btHullTriangle();
00438 btAlignedFree(tri);
00439 }
00440
00441
00442 void HullLibrary::extrude(btHullTriangle *t0,int v)
00443 {
00444 int3 t= *t0;
00445 int n = m_tris.size();
00446 btHullTriangle* ta = allocateTriangle(v,t[1],t[2]);
00447 ta->n = int3(t0->n[0],n+1,n+2);
00448 m_tris[t0->n[0]]->neib(t[1],t[2]) = n+0;
00449 btHullTriangle* tb = allocateTriangle(v,t[2],t[0]);
00450 tb->n = int3(t0->n[1],n+2,n+0);
00451 m_tris[t0->n[1]]->neib(t[2],t[0]) = n+1;
00452 btHullTriangle* tc = allocateTriangle(v,t[0],t[1]);
00453 tc->n = int3(t0->n[2],n+0,n+1);
00454 m_tris[t0->n[2]]->neib(t[0],t[1]) = n+2;
00455 checkit(ta);
00456 checkit(tb);
00457 checkit(tc);
00458 if(hasvert(*m_tris[ta->n[0]],v)) removeb2b(ta,m_tris[ta->n[0]]);
00459 if(hasvert(*m_tris[tb->n[0]],v)) removeb2b(tb,m_tris[tb->n[0]]);
00460 if(hasvert(*m_tris[tc->n[0]],v)) removeb2b(tc,m_tris[tc->n[0]]);
00461 deAllocateTriangle(t0);
00462
00463 }
00464
00465 btHullTriangle* HullLibrary::extrudable(btScalar epsilon)
00466 {
00467 int i;
00468 btHullTriangle *t=NULL;
00469 for(i=0;i<m_tris.size();i++)
00470 {
00471 if(!t || (m_tris[i] && t->rise<m_tris[i]->rise))
00472 {
00473 t = m_tris[i];
00474 }
00475 }
00476 return (t->rise >epsilon)?t:NULL ;
00477 }
00478
00479
00480
00481
00482 int4 HullLibrary::FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray<int> &allow)
00483 {
00484 btVector3 basis[3];
00485 basis[0] = btVector3( btScalar(0.01), btScalar(0.02), btScalar(1.0) );
00486 int p0 = maxdirsterid(verts,verts_count, basis[0],allow);
00487 int p1 = maxdirsterid(verts,verts_count,-basis[0],allow);
00488 basis[0] = verts[p0]-verts[p1];
00489 if(p0==p1 || basis[0]==btVector3(0,0,0))
00490 return int4(-1,-1,-1,-1);
00491 basis[1] = btCross(btVector3( btScalar(1),btScalar(0.02), btScalar(0)),basis[0]);
00492 basis[2] = btCross(btVector3(btScalar(-0.02), btScalar(1), btScalar(0)),basis[0]);
00493 if (basis[1].length() > basis[2].length())
00494 {
00495 basis[1].normalize();
00496 } else {
00497 basis[1] = basis[2];
00498 basis[1].normalize ();
00499 }
00500 int p2 = maxdirsterid(verts,verts_count,basis[1],allow);
00501 if(p2 == p0 || p2 == p1)
00502 {
00503 p2 = maxdirsterid(verts,verts_count,-basis[1],allow);
00504 }
00505 if(p2 == p0 || p2 == p1)
00506 return int4(-1,-1,-1,-1);
00507 basis[1] = verts[p2] - verts[p0];
00508 basis[2] = btCross(basis[1],basis[0]).normalized();
00509 int p3 = maxdirsterid(verts,verts_count,basis[2],allow);
00510 if(p3==p0||p3==p1||p3==p2) p3 = maxdirsterid(verts,verts_count,-basis[2],allow);
00511 if(p3==p0||p3==p1||p3==p2)
00512 return int4(-1,-1,-1,-1);
00513 btAssert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3));
00514 if(btDot(verts[p3]-verts[p0],btCross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {btSwap(p2,p3);}
00515 return int4(p0,p1,p2,p3);
00516 }
00517
00518 int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit)
00519 {
00520 if(verts_count <4) return 0;
00521 if(vlimit==0) vlimit=1000000000;
00522 int j;
00523 btVector3 bmin(*verts),bmax(*verts);
00524 btAlignedObjectArray<int> isextreme;
00525 isextreme.reserve(verts_count);
00526 btAlignedObjectArray<int> allow;
00527 allow.reserve(verts_count);
00528
00529 for(j=0;j<verts_count;j++)
00530 {
00531 allow.push_back(1);
00532 isextreme.push_back(0);
00533 bmin.setMin (verts[j]);
00534 bmax.setMax (verts[j]);
00535 }
00536 btScalar epsilon = (bmax-bmin).length() * btScalar(0.001);
00537 btAssert (epsilon != 0.0);
00538
00539
00540 int4 p = FindSimplex(verts,verts_count,allow);
00541 if(p.x==-1) return 0;
00542
00543
00544
00545 btVector3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) / btScalar(4.0);
00546 btHullTriangle *t0 = allocateTriangle(p[2],p[3],p[1]); t0->n=int3(2,3,1);
00547 btHullTriangle *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0);
00548 btHullTriangle *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3);
00549 btHullTriangle *t3 = allocateTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2);
00550 isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1;
00551 checkit(t0);checkit(t1);checkit(t2);checkit(t3);
00552
00553 for(j=0;j<m_tris.size();j++)
00554 {
00555 btHullTriangle *t=m_tris[j];
00556 btAssert(t);
00557 btAssert(t->vmax<0);
00558 btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
00559 t->vmax = maxdirsterid(verts,verts_count,n,allow);
00560 t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]);
00561 }
00562 btHullTriangle *te;
00563 vlimit-=4;
00564 while(vlimit >0 && ((te=extrudable(epsilon)) != 0))
00565 {
00566
00567 int v=te->vmax;
00568 btAssert(v != -1);
00569 btAssert(!isextreme[v]);
00570 isextreme[v]=1;
00571
00572 j=m_tris.size();
00573 while(j--) {
00574 if(!m_tris[j]) continue;
00575 int3 t=*m_tris[j];
00576 if(above(verts,t,verts[v],btScalar(0.01)*epsilon))
00577 {
00578 extrude(m_tris[j],v);
00579 }
00580 }
00581
00582 j=m_tris.size();
00583 while(j--)
00584 {
00585 if(!m_tris[j]) continue;
00586 if(!hasvert(*m_tris[j],v)) break;
00587 int3 nt=*m_tris[j];
00588 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) )
00589 {
00590 btHullTriangle *nb = m_tris[m_tris[j]->n[0]];
00591 btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->id<j);
00592 extrude(nb,v);
00593 j=m_tris.size();
00594 }
00595 }
00596 j=m_tris.size();
00597 while(j--)
00598 {
00599 btHullTriangle *t=m_tris[j];
00600 if(!t) continue;
00601 if(t->vmax>=0) break;
00602 btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
00603 t->vmax = maxdirsterid(verts,verts_count,n,allow);
00604 if(isextreme[t->vmax])
00605 {
00606 t->vmax=-1;
00607 }
00608 else
00609 {
00610 t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]);
00611 }
00612 }
00613 vlimit --;
00614 }
00615 return 1;
00616 }
00617
00618 int HullLibrary::calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit)
00619 {
00620 int rc=calchullgen(verts,verts_count, vlimit) ;
00621 if(!rc) return 0;
00622 btAlignedObjectArray<int> ts;
00623 int i;
00624
00625 for(i=0;i<m_tris.size();i++)
00626 {
00627 if(m_tris[i])
00628 {
00629 for(int j=0;j<3;j++)
00630 ts.push_back((*m_tris[i])[j]);
00631 deAllocateTriangle(m_tris[i]);
00632 }
00633 }
00634 tris_count = ts.size()/3;
00635 tris_out.resize(ts.size());
00636
00637 for (i=0;i<ts.size();i++)
00638 {
00639 tris_out[i] = static_cast<unsigned int>(ts[i]);
00640 }
00641 m_tris.resize(0);
00642
00643 return 1;
00644 }
00645
00646
00647
00648
00649
00650 bool HullLibrary::ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit)
00651 {
00652
00653 int tris_count;
00654 int ret = calchull( (btVector3 *) vertices, (int) vcount, result.m_Indices, tris_count, static_cast<int>(vlimit) );
00655 if(!ret) return false;
00656 result.mIndexCount = (unsigned int) (tris_count*3);
00657 result.mFaceCount = (unsigned int) tris_count;
00658 result.mVertices = (btVector3*) vertices;
00659 result.mVcount = (unsigned int) vcount;
00660 return true;
00661
00662 }
00663
00664
00665 void ReleaseHull(PHullResult &result);
00666 void ReleaseHull(PHullResult &result)
00667 {
00668 if ( result.m_Indices.size() )
00669 {
00670 result.m_Indices.clear();
00671 }
00672
00673 result.mVcount = 0;
00674 result.mIndexCount = 0;
00675 result.mVertices = 0;
00676 }
00677
00678
00679
00680
00681
00682
00683
00684
00685
00686
00687
00688
00689
00690
00691 HullError HullLibrary::CreateConvexHull(const HullDesc &desc,
00692 HullResult &result)
00693 {
00694 HullError ret = QE_FAIL;
00695
00696
00697 PHullResult hr;
00698
00699 unsigned int vcount = desc.mVcount;
00700 if ( vcount < 8 ) vcount = 8;
00701
00702 btAlignedObjectArray<btVector3> vertexSource;
00703 vertexSource.resize(static_cast<int>(vcount));
00704
00705 btVector3 scale;
00706
00707 unsigned int ovcount;
00708
00709 bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, &vertexSource[0], desc.mNormalEpsilon, scale );
00710
00711 if ( ok )
00712 {
00713
00714
00715
00716 {
00717 for (unsigned int i=0; i<ovcount; i++)
00718 {
00719 btVector3& v = vertexSource[static_cast<int>(i)];
00720 v[0]*=scale[0];
00721 v[1]*=scale[1];
00722 v[2]*=scale[2];
00723 }
00724 }
00725
00726 ok = ComputeHull(ovcount,&vertexSource[0],hr,desc.mMaxVertices);
00727
00728 if ( ok )
00729 {
00730
00731
00732 btAlignedObjectArray<btVector3> vertexScratch;
00733 vertexScratch.resize(static_cast<int>(hr.mVcount));
00734
00735 BringOutYourDead(hr.mVertices,hr.mVcount, &vertexScratch[0], ovcount, &hr.m_Indices[0], hr.mIndexCount );
00736
00737 ret = QE_OK;
00738
00739 if ( desc.HasHullFlag(QF_TRIANGLES) )
00740 {
00741 result.mPolygons = false;
00742 result.mNumOutputVertices = ovcount;
00743 result.m_OutputVertices.resize(static_cast<int>(ovcount));
00744 result.mNumFaces = hr.mFaceCount;
00745 result.mNumIndices = hr.mIndexCount;
00746
00747 result.m_Indices.resize(static_cast<int>(hr.mIndexCount));
00748
00749 memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount );
00750
00751 if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
00752 {
00753
00754 const unsigned int *source = &hr.m_Indices[0];
00755 unsigned int *dest = &result.m_Indices[0];
00756
00757 for (unsigned int i=0; i<hr.mFaceCount; i++)
00758 {
00759 dest[0] = source[2];
00760 dest[1] = source[1];
00761 dest[2] = source[0];
00762 dest+=3;
00763 source+=3;
00764 }
00765
00766 }
00767 else
00768 {
00769 memcpy(&result.m_Indices[0], &hr.m_Indices[0], sizeof(unsigned int)*hr.mIndexCount);
00770 }
00771 }
00772 else
00773 {
00774 result.mPolygons = true;
00775 result.mNumOutputVertices = ovcount;
00776 result.m_OutputVertices.resize(static_cast<int>(ovcount));
00777 result.mNumFaces = hr.mFaceCount;
00778 result.mNumIndices = hr.mIndexCount+hr.mFaceCount;
00779 result.m_Indices.resize(static_cast<int>(result.mNumIndices));
00780 memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount );
00781
00782
00783 {
00784 const unsigned int *source = &hr.m_Indices[0];
00785 unsigned int *dest = &result.m_Indices[0];
00786 for (unsigned int i=0; i<hr.mFaceCount; i++)
00787 {
00788 dest[0] = 3;
00789 if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
00790 {
00791 dest[1] = source[2];
00792 dest[2] = source[1];
00793 dest[3] = source[0];
00794 }
00795 else
00796 {
00797 dest[1] = source[0];
00798 dest[2] = source[1];
00799 dest[3] = source[2];
00800 }
00801
00802 dest+=4;
00803 source+=3;
00804 }
00805 }
00806 }
00807 ReleaseHull(hr);
00808 }
00809 }
00810
00811 return ret;
00812 }
00813
00814
00815
00816 HullError HullLibrary::ReleaseResult(HullResult &result)
00817 {
00818 if ( result.m_OutputVertices.size())
00819 {
00820 result.mNumOutputVertices=0;
00821 result.m_OutputVertices.clear();
00822 }
00823 if ( result.m_Indices.size() )
00824 {
00825 result.mNumIndices=0;
00826 result.m_Indices.clear();
00827 }
00828 return QE_OK;
00829 }
00830
00831
00832 static void addPoint(unsigned int &vcount,btVector3 *p,btScalar x,btScalar y,btScalar z)
00833 {
00834
00835 btVector3& dest = p[vcount];
00836 dest[0] = x;
00837 dest[1] = y;
00838 dest[2] = z;
00839 vcount++;
00840 }
00841
00842 btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2);
00843 btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2)
00844 {
00845
00846 btScalar dx = px - p2[0];
00847 btScalar dy = py - p2[1];
00848 btScalar dz = pz - p2[2];
00849
00850 return dx*dx+dy*dy+dz*dz;
00851 }
00852
00853
00854
00855 bool HullLibrary::CleanupVertices(unsigned int svcount,
00856 const btVector3 *svertices,
00857 unsigned int stride,
00858 unsigned int &vcount,
00859 btVector3 *vertices,
00860 btScalar normalepsilon,
00861 btVector3& scale)
00862 {
00863 if ( svcount == 0 ) return false;
00864
00865 m_vertexIndexMapping.resize(0);
00866
00867
00868 #define EPSILON btScalar(0.000001)
00869
00870 vcount = 0;
00871
00872 btScalar recip[3]={0.f,0.f,0.f};
00873
00874 if ( scale )
00875 {
00876 scale[0] = 1;
00877 scale[1] = 1;
00878 scale[2] = 1;
00879 }
00880
00881 btScalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX };
00882 btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
00883
00884 const char *vtx = (const char *) svertices;
00885
00886
00887 {
00888 for (unsigned int i=0; i<svcount; i++)
00889 {
00890 const btScalar *p = (const btScalar *) vtx;
00891
00892 vtx+=stride;
00893
00894 for (int j=0; j<3; j++)
00895 {
00896 if ( p[j] < bmin[j] ) bmin[j] = p[j];
00897 if ( p[j] > bmax[j] ) bmax[j] = p[j];
00898 }
00899 }
00900 }
00901
00902 btScalar dx = bmax[0] - bmin[0];
00903 btScalar dy = bmax[1] - bmin[1];
00904 btScalar dz = bmax[2] - bmin[2];
00905
00906 btVector3 center;
00907
00908 center[0] = dx*btScalar(0.5) + bmin[0];
00909 center[1] = dy*btScalar(0.5) + bmin[1];
00910 center[2] = dz*btScalar(0.5) + bmin[2];
00911
00912 if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3 )
00913 {
00914
00915 btScalar len = FLT_MAX;
00916
00917 if ( dx > EPSILON && dx < len ) len = dx;
00918 if ( dy > EPSILON && dy < len ) len = dy;
00919 if ( dz > EPSILON && dz < len ) len = dz;
00920
00921 if ( len == FLT_MAX )
00922 {
00923 dx = dy = dz = btScalar(0.01);
00924 }
00925 else
00926 {
00927 if ( dx < EPSILON ) dx = len * btScalar(0.05);
00928 if ( dy < EPSILON ) dy = len * btScalar(0.05);
00929 if ( dz < EPSILON ) dz = len * btScalar(0.05);
00930 }
00931
00932 btScalar x1 = center[0] - dx;
00933 btScalar x2 = center[0] + dx;
00934
00935 btScalar y1 = center[1] - dy;
00936 btScalar y2 = center[1] + dy;
00937
00938 btScalar z1 = center[2] - dz;
00939 btScalar z2 = center[2] + dz;
00940
00941 addPoint(vcount,vertices,x1,y1,z1);
00942 addPoint(vcount,vertices,x2,y1,z1);
00943 addPoint(vcount,vertices,x2,y2,z1);
00944 addPoint(vcount,vertices,x1,y2,z1);
00945 addPoint(vcount,vertices,x1,y1,z2);
00946 addPoint(vcount,vertices,x2,y1,z2);
00947 addPoint(vcount,vertices,x2,y2,z2);
00948 addPoint(vcount,vertices,x1,y2,z2);
00949
00950 return true;
00951
00952
00953 }
00954 else
00955 {
00956 if ( scale )
00957 {
00958 scale[0] = dx;
00959 scale[1] = dy;
00960 scale[2] = dz;
00961
00962 recip[0] = 1 / dx;
00963 recip[1] = 1 / dy;
00964 recip[2] = 1 / dz;
00965
00966 center[0]*=recip[0];
00967 center[1]*=recip[1];
00968 center[2]*=recip[2];
00969
00970 }
00971
00972 }
00973
00974
00975
00976 vtx = (const char *) svertices;
00977
00978 for (unsigned int i=0; i<svcount; i++)
00979 {
00980 const btVector3 *p = (const btVector3 *)vtx;
00981 vtx+=stride;
00982
00983 btScalar px = p->getX();
00984 btScalar py = p->getY();
00985 btScalar pz = p->getZ();
00986
00987 if ( scale )
00988 {
00989 px = px*recip[0];
00990 py = py*recip[1];
00991 pz = pz*recip[2];
00992 }
00993
00994
00995 {
00996 unsigned int j;
00997
00998 for (j=0; j<vcount; j++)
00999 {
01001 btVector3& v = vertices[j];
01002
01003 btScalar x = v[0];
01004 btScalar y = v[1];
01005 btScalar z = v[2];
01006
01007 btScalar dx = btFabs(x - px );
01008 btScalar dy = btFabs(y - py );
01009 btScalar dz = btFabs(z - pz );
01010
01011 if ( dx < normalepsilon && dy < normalepsilon && dz < normalepsilon )
01012 {
01013
01014
01015
01016
01017 btScalar dist1 = GetDist(px,py,pz,center);
01018 btScalar dist2 = GetDist(v[0],v[1],v[2],center);
01019
01020 if ( dist1 > dist2 )
01021 {
01022 v[0] = px;
01023 v[1] = py;
01024 v[2] = pz;
01025
01026 }
01027
01028 break;
01029 }
01030 }
01031
01032 if ( j == vcount )
01033 {
01034 btVector3& dest = vertices[vcount];
01035 dest[0] = px;
01036 dest[1] = py;
01037 dest[2] = pz;
01038 vcount++;
01039 }
01040 m_vertexIndexMapping.push_back(j);
01041 }
01042 }
01043
01044
01045
01046 {
01047 btScalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX };
01048 btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
01049
01050 for (unsigned int i=0; i<vcount; i++)
01051 {
01052 const btVector3& p = vertices[i];
01053 for (int j=0; j<3; j++)
01054 {
01055 if ( p[j] < bmin[j] ) bmin[j] = p[j];
01056 if ( p[j] > bmax[j] ) bmax[j] = p[j];
01057 }
01058 }
01059
01060 btScalar dx = bmax[0] - bmin[0];
01061 btScalar dy = bmax[1] - bmin[1];
01062 btScalar dz = bmax[2] - bmin[2];
01063
01064 if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3)
01065 {
01066 btScalar cx = dx*btScalar(0.5) + bmin[0];
01067 btScalar cy = dy*btScalar(0.5) + bmin[1];
01068 btScalar cz = dz*btScalar(0.5) + bmin[2];
01069
01070 btScalar len = FLT_MAX;
01071
01072 if ( dx >= EPSILON && dx < len ) len = dx;
01073 if ( dy >= EPSILON && dy < len ) len = dy;
01074 if ( dz >= EPSILON && dz < len ) len = dz;
01075
01076 if ( len == FLT_MAX )
01077 {
01078 dx = dy = dz = btScalar(0.01);
01079 }
01080 else
01081 {
01082 if ( dx < EPSILON ) dx = len * btScalar(0.05);
01083 if ( dy < EPSILON ) dy = len * btScalar(0.05);
01084 if ( dz < EPSILON ) dz = len * btScalar(0.05);
01085 }
01086
01087 btScalar x1 = cx - dx;
01088 btScalar x2 = cx + dx;
01089
01090 btScalar y1 = cy - dy;
01091 btScalar y2 = cy + dy;
01092
01093 btScalar z1 = cz - dz;
01094 btScalar z2 = cz + dz;
01095
01096 vcount = 0;
01097
01098 addPoint(vcount,vertices,x1,y1,z1);
01099 addPoint(vcount,vertices,x2,y1,z1);
01100 addPoint(vcount,vertices,x2,y2,z1);
01101 addPoint(vcount,vertices,x1,y2,z1);
01102 addPoint(vcount,vertices,x1,y1,z2);
01103 addPoint(vcount,vertices,x2,y1,z2);
01104 addPoint(vcount,vertices,x2,y2,z2);
01105 addPoint(vcount,vertices,x1,y2,z2);
01106
01107 return true;
01108 }
01109 }
01110
01111 return true;
01112 }
01113
01114 void HullLibrary::BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount)
01115 {
01116 btAlignedObjectArray<int>tmpIndices;
01117 tmpIndices.resize(m_vertexIndexMapping.size());
01118 int i;
01119
01120 for (i=0;i<m_vertexIndexMapping.size();i++)
01121 {
01122 tmpIndices[i] = m_vertexIndexMapping[i];
01123 }
01124
01125 TUIntArray usedIndices;
01126 usedIndices.resize(static_cast<int>(vcount));
01127 memset(&usedIndices[0],0,sizeof(unsigned int)*vcount);
01128
01129 ocount = 0;
01130
01131 for (i=0; i<int (indexcount); i++)
01132 {
01133 unsigned int v = indices[i];
01134
01135 btAssert( v >= 0 && v < vcount );
01136
01137 if ( usedIndices[static_cast<int>(v)] )
01138 {
01139 indices[i] = usedIndices[static_cast<int>(v)]-1;
01140 }
01141 else
01142 {
01143
01144 indices[i] = ocount;
01145
01146 overts[ocount][0] = verts[v][0];
01147 overts[ocount][1] = verts[v][1];
01148 overts[ocount][2] = verts[v][2];
01149
01150 for (int k=0;k<m_vertexIndexMapping.size();k++)
01151 {
01152 if (tmpIndices[k]==int(v))
01153 m_vertexIndexMapping[k]=ocount;
01154 }
01155
01156 ocount++;
01157
01158 btAssert( ocount >=0 && ocount <= vcount );
01159
01160 usedIndices[static_cast<int>(v)] = ocount;
01161
01162
01163 }
01164 }
01165
01166
01167 }