Back to index

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