Patch #17675: Tooltips for fluid control panel - provided by Kai Kostack
[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 "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 }