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