== SoC Bullet - Bullet Upgrade to 2.76 ==
[blender.git] / extern / bullet2 / 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 }