btConvexHull.cpp

Go to the documentation of this file.
00001 /*
00002 Stan Melax Convex Hull Computation
00003 Copyright (c) 2003-2006 Stan Melax http://www.melax.com/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
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 //------- btPlane ----------
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 //--------- Utility Functions ------
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         // returns the point where the line p0-p1 intersects the plane n&d
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         // return the normal of the triangle
00105         // inscribed by v0, v1, and v2
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)  // Yuck - this is really ugly
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); // 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                 // release compile fix
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; // simplex failed
00542 
00543 
00544 
00545         btVector3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) / btScalar(4.0);  // a valid interior point
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                 //int3 ti=*te;
00567                 int v=te->vmax;
00568                 btAssert(v != -1);
00569                 btAssert(!isextreme[v]);  // wtf we've already done this vertex
00570                 isextreme[v]=1;
00571                 //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already
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                 // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle
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; // already done that vertex - algorithm needs to be able to terminate.
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 //********  HullLib header
00682 //*********************************************************************
00683 //*********************************************************************
00684 
00685 //*********************************************************************
00686 //*********************************************************************
00687 //********  HullLib implementation
00688 //*********************************************************************
00689 //*********************************************************************
00690 
00691 HullError HullLibrary::CreateConvexHull(const HullDesc       &desc,           // describes the input request
00692                                                                                                                                                                         HullResult           &result)         // contains the resulst
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 ); // normalize point cloud, remove duplicates!
00710 
00711         if ( ok )
00712         {
00713 
00714 
00715 //              if ( 1 ) // scale vertices back to their original size.
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                         // re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table.
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) ) // if he wants the results as triangle!
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 //                              if ( 1 )
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) // release memory allocated for this result, we are done with it.
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         // XXX, might be broken
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,       // output number of vertices
00859                                    btVector3 *vertices,                 // location to store the results.
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) /* close enough to consider two btScalaring point numbers to be 'the same'. */
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 //      if ( 1 )
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); // one centimeter
00924                 }
00925                 else
00926                 {
00927                         if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge.
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; // return cube
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]; // normalize
00990                         py = py*recip[1]; // normalize
00991                         pz = pz*recip[2]; // normalize
00992                 }
00993 
00994 //              if ( 1 )
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                                         // ok, it is close enough to the old one
01014                                         // now let us see if it is further from the center of the point cloud than the one we already recorded.
01015                                         // in which case we keep this one instead.
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         // ok..now make sure we didn't prune so many vertices it is now invalid.
01045 //      if ( 1 )
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); // one centimeter
01079                         }
01080                         else
01081                         {
01082                                 if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge.
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; // add box
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]; // original array index
01134 
01135                 btAssert( v >= 0 && v < vcount );
01136 
01137                 if ( usedIndices[static_cast<int>(v)] ) // if already remapped
01138                 {
01139                         indices[i] = usedIndices[static_cast<int>(v)]-1; // index to new array
01140                 }
01141                 else
01142                 {
01143 
01144                         indices[i] = ocount;      // new index mapping
01145 
01146                         overts[ocount][0] = verts[v][0]; // copy old vert to new vert array
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++; // increment output vert count
01157 
01158                         btAssert( ocount >=0 && ocount <= vcount );
01159 
01160                         usedIndices[static_cast<int>(v)] = ocount; // assign new index remapping
01161 
01162                 
01163                 }
01164         }
01165 
01166         
01167 }