extern/bullet2/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 "LinearMath/btAlignedObjectArray.h"
  20 #include "LinearMath/btMinMax.h"
  21 #include "LinearMath/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= dot(plane.normal,dif);
 100                                 btScalar t = -(plane.dist+dot(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 * (dot(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=cross(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 = cross(udir,vdir).normalized();
 124
 125         btScalar distu = -dot(cp,ustart);
 126         btScalar distv = -dot(cp,vstart);
 127         btScalar dist = (btScalar)fabs(distu-distv);
 128         if(upoint)
 129                 {
 130                 btPlane plane;
 131                 plane.normal = cross(vdir,cp).normalized();
 132                 plane.dist = -dot(plane.normal,vstart);
 133                 *upoint = PlaneLineIntersection(plane,ustart,ustart+udir);
 134         }
 135         if(vpoint)
 136                 {
 137                 btPlane plane;
 138                 plane.normal = cross(udir,cp).normalized();
 139                 plane.dist = -dot(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  = dot(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 || dot(p[i],dir)>dot(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=cross(v,btVector3(0,0,1));
 242         btVector3 b=cross(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 = cross(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 = sinf(SIMD_RADS_PER_DEG*(x));
 266                         btScalar c = cosf(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 = sinf(SIMD_RADS_PER_DEG*(xx));
 279                                         btScalar c = cosf(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 (dot(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 Tri;
 346
 347
 348
 349 class Tri : public int3
 350 {
 351 public:
 352         int3 n;
 353         int id;
 354         int vmax;
 355         btScalar rise;
 356         Tri(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         ~Tri()
 362         {
 363         }
 364         int &neib(int a,int b);
 365 };
 366
 367
 368 int &Tri::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(Tri* s,Tri*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(Tri* s,Tri*t)
 399 {
 400         b2bfix(s,t);
 401         deAllocateTriangle(s);
 402
 403         deAllocateTriangle(t);
 404 }
 405
 406 void HullLibrary::checkit(Tri *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 Tri*    HullLibrary::allocateTriangle(int a,int b,int c)
 431 {
 432         void* mem = btAlignedAlloc(sizeof(Tri),16);
 433         Tri* tr = new (mem)Tri(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(Tri* tri)
 441 {
 442         btAssert(m_tris[tri->id]==tri);
 443         m_tris[tri->id]=NULL;
 444         tri->~Tri();
 445         btAlignedFree(tri);
 446 }
 447
 448
 449 void HullLibrary::extrude(Tri *t0,int v)
 450 {
 451         int3 t= *t0;
 452         int n = m_tris.size();
 453         Tri* 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         Tri* 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         Tri* 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 Tri* HullLibrary::extrudable(btScalar epsilon)
 473 {
 474         int i;
 475         Tri *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] = cross(btVector3(     btScalar(1),btScalar(0.02), btScalar(0)),basis[0]);
 499         basis[2] = cross(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] = cross(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(dot(verts[p3]-verts[p0],cross(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         Tri *t0 = allocateTriangle(p[2],p[3],p[1]); t0->n=int3(2,3,1);
 554         Tri *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0);
 555         Tri *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3);
 556         Tri *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                 Tri *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 = dot(n,verts[t->vmax]-verts[(*t)[0]]);
 568         }
 569         Tri *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)  || cross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) )
 596                         {
 597                                 Tri *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                         Tri *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 = dot(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
 873 #define EPSILON btScalar(0.000001) /* close enough to consider two btScalaring point numbers to be 'the same'. */
 874
 875         vcount = 0;
 876
 877         btScalar recip[3];
 878
 879         if ( scale )
 880         {
 881                 scale[0] = 1;
 882                 scale[1] = 1;
 883                 scale[2] = 1;
 884         }
 885
 886         btScalar bmin[3] = {  FLT_MAX,  FLT_MAX,  FLT_MAX };
 887         btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
 888
 889         const char *vtx = (const char *) svertices;
 890
 891 //      if ( 1 )
 892         {
 893                 for (unsigned int i=0; i<svcount; i++)
 894                 {
 895                         const btScalar *p = (const btScalar *) vtx;
 896
 897                         vtx+=stride;
 898
 899                         for (int j=0; j<3; j++)
 900                         {
 901                                 if ( p[j] < bmin[j] ) bmin[j] = p[j];
 902                                 if ( p[j] > bmax[j] ) bmax[j] = p[j];
 903                         }
 904                 }
 905         }
 906
 907         btScalar dx = bmax[0] - bmin[0];
 908         btScalar dy = bmax[1] - bmin[1];
 909         btScalar dz = bmax[2] - bmin[2];
 910
 911         btVector3 center;
 912
 913         center[0] = dx*btScalar(0.5) + bmin[0];
 914         center[1] = dy*btScalar(0.5) + bmin[1];
 915         center[2] = dz*btScalar(0.5) + bmin[2];
 916
 917         if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3 )
 918         {
 919
 920                 btScalar len = FLT_MAX;
 921
 922                 if ( dx > EPSILON && dx < len ) len = dx;
 923                 if ( dy > EPSILON && dy < len ) len = dy;
 924                 if ( dz > EPSILON && dz < len ) len = dz;
 925
 926                 if ( len == FLT_MAX )
 927                 {
 928                         dx = dy = dz = btScalar(0.01); // one centimeter
 929                 }
 930                 else
 931                 {
 932                         if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge.
 933                         if ( dy < EPSILON ) dy = len * btScalar(0.05);
 934                         if ( dz < EPSILON ) dz = len * btScalar(0.05);
 935                 }
 936
 937                 btScalar x1 = center[0] - dx;
 938                 btScalar x2 = center[0] + dx;
 939
 940                 btScalar y1 = center[1] - dy;
 941                 btScalar y2 = center[1] + dy;
 942
 943                 btScalar z1 = center[2] - dz;
 944                 btScalar z2 = center[2] + dz;
 945
 946                 addPoint(vcount,vertices,x1,y1,z1);
 947                 addPoint(vcount,vertices,x2,y1,z1);
 948                 addPoint(vcount,vertices,x2,y2,z1);
 949                 addPoint(vcount,vertices,x1,y2,z1);
 950                 addPoint(vcount,vertices,x1,y1,z2);
 951                 addPoint(vcount,vertices,x2,y1,z2);
 952                 addPoint(vcount,vertices,x2,y2,z2);
 953                 addPoint(vcount,vertices,x1,y2,z2);
 954
 955                 return true; // return cube
 956
 957
 958         }
 959         else
 960         {
 961                 if ( scale )
 962                 {
 963                         scale[0] = dx;
 964                         scale[1] = dy;
 965                         scale[2] = dz;
 966
 967                         recip[0] = 1 / dx;
 968                         recip[1] = 1 / dy;
 969                         recip[2] = 1 / dz;
 970
 971                         center[0]*=recip[0];
 972                         center[1]*=recip[1];
 973                         center[2]*=recip[2];
 974
 975                 }
 976
 977         }
 978
 979
 980
 981         vtx = (const char *) svertices;
 982
 983         for (unsigned int i=0; i<svcount; i++)
 984         {
 985                 const btVector3 *p = (const btVector3 *)vtx;
 986                 vtx+=stride;
 987
 988                 btScalar px = p->getX();
 989                 btScalar py = p->getY();
 990                 btScalar pz = p->getZ();
 991
 992                 if ( scale )
 993                 {
 994                         px = px*recip[0]; // normalize
 995                         py = py*recip[1]; // normalize
 996                         pz = pz*recip[2]; // normalize
 997                 }
 998
 999 //              if ( 1 )
1000                 {
1001                         unsigned int j;
1002
1003                         for (j=0; j<vcount; j++)
1004                         {
1005                                 /// XXX might be broken
1006                                 btVector3& v = vertices[j];
1007
1008                                 btScalar x = v[0];
1009                                 btScalar y = v[1];
1010                                 btScalar z = v[2];
1011
1012                                 btScalar dx = fabsf(x - px );
1013                                 btScalar dy = fabsf(y - py );
1014                                 btScalar dz = fabsf(z - pz );
1015
1016                                 if ( dx < normalepsilon && dy < normalepsilon && dz < normalepsilon )
1017                                 {
1018                                         // ok, it is close enough to the old one
1019                                         // now let us see if it is further from the center of the point cloud than the one we already recorded.
1020                                         // in which case we keep this one instead.
1021
1022                                         btScalar dist1 = GetDist(px,py,pz,center);
1023                                         btScalar dist2 = GetDist(v[0],v[1],v[2],center);
1024
1025                                         if ( dist1 > dist2 )
1026                                         {
1027                                                 v[0] = px;
1028                                                 v[1] = py;
1029                                                 v[2] = pz;
1030                                         }
1031
1032                                         break;
1033                                 }
1034                         }
1035
1036                         if ( j == vcount )
1037                         {
1038                                 btVector3& dest = vertices[vcount];
1039                                 dest[0] = px;
1040                                 dest[1] = py;
1041                                 dest[2] = pz;
1042                                 vcount++;
1043                         }
1044                 }
1045         }
1046
1047         // ok..now make sure we didn't prune so many vertices it is now invalid.
1048 //      if ( 1 )
1049         {
1050                 btScalar bmin[3] = {  FLT_MAX,  FLT_MAX,  FLT_MAX };
1051                 btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
1052
1053                 for (unsigned int i=0; i<vcount; i++)
1054                 {
1055                         const btVector3& p = vertices[i];
1056                         for (int j=0; j<3; j++)
1057                         {
1058                                 if ( p[j] < bmin[j] ) bmin[j] = p[j];
1059                                 if ( p[j] > bmax[j] ) bmax[j] = p[j];
1060                         }
1061                 }
1062
1063                 btScalar dx = bmax[0] - bmin[0];
1064                 btScalar dy = bmax[1] - bmin[1];
1065                 btScalar dz = bmax[2] - bmin[2];
1066
1067                 if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3)
1068                 {
1069                         btScalar cx = dx*btScalar(0.5) + bmin[0];
1070                         btScalar cy = dy*btScalar(0.5) + bmin[1];
1071                         btScalar cz = dz*btScalar(0.5) + bmin[2];
1072
1073                         btScalar len = FLT_MAX;
1074
1075                         if ( dx >= EPSILON && dx < len ) len = dx;
1076                         if ( dy >= EPSILON && dy < len ) len = dy;
1077                         if ( dz >= EPSILON && dz < len ) len = dz;
1078
1079                         if ( len == FLT_MAX )
1080                         {
1081                                 dx = dy = dz = btScalar(0.01); // one centimeter
1082                         }
1083                         else
1084                         {
1085                                 if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge.
1086                                 if ( dy < EPSILON ) dy = len * btScalar(0.05);
1087                                 if ( dz < EPSILON ) dz = len * btScalar(0.05);
1088                         }
1089
1090                         btScalar x1 = cx - dx;
1091                         btScalar x2 = cx + dx;
1092
1093                         btScalar y1 = cy - dy;
1094                         btScalar y2 = cy + dy;
1095
1096                         btScalar z1 = cz - dz;
1097                         btScalar z2 = cz + dz;
1098
1099                         vcount = 0; // add box
1100
1101                         addPoint(vcount,vertices,x1,y1,z1);
1102                         addPoint(vcount,vertices,x2,y1,z1);
1103                         addPoint(vcount,vertices,x2,y2,z1);
1104                         addPoint(vcount,vertices,x1,y2,z1);
1105                         addPoint(vcount,vertices,x1,y1,z2);
1106                         addPoint(vcount,vertices,x2,y1,z2);
1107                         addPoint(vcount,vertices,x2,y2,z2);
1108                         addPoint(vcount,vertices,x1,y2,z2);
1109
1110                         return true;
1111                 }
1112         }
1113
1114         return true;
1115 }
1116
1117 void HullLibrary::BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount)
1118 {
1119         TUIntArray usedIndices;
1120         usedIndices.resize(static_cast<int>(vcount));
1121         memset(&usedIndices[0],0,sizeof(unsigned int)*vcount);
1122
1123         ocount = 0;
1124
1125         for (unsigned int i=0; i<indexcount; i++)
1126         {
1127                 unsigned int v = indices[i]; // original array index
1128
1129                 btAssert( v >= 0 && v < vcount );
1130
1131                 if ( usedIndices[static_cast<int>(v)] ) // if already remapped
1132                 {
1133                         indices[i] = usedIndices[static_cast<int>(v)]-1; // index to new array
1134                 }
1135                 else
1136                 {
1137
1138                         indices[i] = ocount;      // new index mapping
1139
1140                         overts[ocount][0] = verts[v][0]; // copy old vert to new vert array
1141                         overts[ocount][1] = verts[v][1];
1142                         overts[ocount][2] = verts[v][2];
1143
1144                         ocount++; // increment output vert count
1145
1146                         btAssert( ocount >=0 && ocount <= vcount );
1147
1148                         usedIndices[static_cast<int>(v)] = ocount; // assign new index remapping
1149                 }
1150         }
1151
1152
1153 }