src/LinearMath/btConvexHull.cpp

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