Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/netp3/src/bullet/LinearMath/btConvexHull.cpp @ 2989

Last change on this file since 2989 was 2882, checked in by rgrieder, 16 years ago

Update from Bullet 2.73 to 2.74.
  • Property svn:eol-style set to native
File size: 27.6 KB
Line 
1/*
2Stan Melax Convex Hull Computation
3Copyright (c) 2003-2006 Stan Melax http://www.melax.com/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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
25template <class T>
26void Swap(T &a,T &b)
27{
28        T tmp = a;
29        a=b;
30        b=tmp;
31}
32
33
34//----------------------------------
35
36class int3 
37{
38public:
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
50inline btPlane PlaneFlip(const btPlane &plane){return btPlane(-plane.normal,-plane.dist);}
51inline int operator==( const btPlane &a, const btPlane &b ) { return (a.normal==b.normal && a.dist==b.dist); }
52inline int coplanar( const btPlane &a, const btPlane &b ) { return (a==b || a==PlaneFlip(b)); }
53
54
55//--------- Utility Functions ------
56
57btVector3  PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1);
58btVector3  PlaneProject(const btPlane &plane, const btVector3 &point);
59
60btVector3  ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2);
61btVector3  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
89btScalar   DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint=NULL, btVector3 *vpoint=NULL);
90btVector3  TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2);
91btVector3  NormalOf(const btVector3 *vert, const int n);
92
93
94btVector3 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
104btVector3 PlaneProject(const btPlane &plane, const btVector3 &point)
105{
106        return point - plane.normal * (dot(point,plane.normal)+plane.dist);
107}
108
109btVector3 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
120btScalar 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
157btScalar planetestepsilon = PAPERWIDTH;
158
159
160
161typedef ConvexH::HalfEdge HalfEdge;
162
163ConvexH::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
171int PlaneTest(const btPlane &p, const btVector3 &v);
172int 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
178int SplitTest(ConvexH &convex,const btPlane &plane);
179int 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
187class VertFlag
188{
189public:
190        unsigned char planetest;
191        unsigned char junk;
192        unsigned char undermap;
193        unsigned char overmap;
194};
195class EdgeFlag 
196{
197public:
198        unsigned char planetest;
199        unsigned char fixes;
200        short undermap;
201        short overmap;
202};
203class PlaneFlag
204{
205public:
206        unsigned char undermap;
207        unsigned char overmap;
208};
209class Coplanar{
210public:
211        unsigned short ea;
212        unsigned char v0;
213        unsigned char v1;
214};
215
216
217
218
219
220
221
222
223template<class T>
224int 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
238btVector3 orth(const btVector3 &v);
239btVector3 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
252template<class T>
253int 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 = 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
301int operator ==(const int3 &a,const int3 &b);
302int 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
312int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon);
313int 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}
318int hasedge(const int3 &t, int a,int b);
319int 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}
328int hasvert(const int3 &t, int v);
329int hasvert(const int3 &t, int v)
330{
331        return (t[0]==v || t[1]==v || t[2]==v) ;
332}
333int shareedge(const int3 &a,const int3 &b);
334int 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
345class btHullTriangle;
346
347
348
349class btHullTriangle : public int3
350{
351public:
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
368int &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}
382void 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
398void HullLibrary::removeb2b(btHullTriangle* s,btHullTriangle*t)
399{
400        b2bfix(s,t);
401        deAllocateTriangle(s);
402
403        deAllocateTriangle(t);
404}
405
406void 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
430btHullTriangle* 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
440void    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
449void 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
472btHullTriangle* 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
489int4 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
525int 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 = dot(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)  || cross(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 = dot(n,verts[t->vmax]-verts[(*t)[0]]);
618                        }
619                }
620                vlimit --;
621        }
622        return 1;
623}
624
625int 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
657bool 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
672void ReleaseHull(PHullResult &result);
673void 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
698HullError 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
823HullError 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
839static 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
849btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2);
850btScalar 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
862bool  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];
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 = fabsf(x - px );
1015                                btScalar dy = fabsf(y - py );
1016                                btScalar dz = fabsf(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
1121void 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<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]==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}
Note: See TracBrowser for help on using the repository browser.