bullet: Update to current svn, r2636
[blender.git] / 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 "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 }