source: trunk/src/bullet/LinearMath/btConvexHull.cpp @ 1808

Last change on this file since 1808 was 1808, checked in by sam, 8 years ago

build: fix shitloads of warnings.

  • Property svn:keywords set to Id
File size: 27.8 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 "btAlignedObjectArray.h"
20#include "btMinMax.h"
21#include "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= btDot(plane.normal,dif);
100                                btScalar t = -(plane.dist+btDot(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 * (btDot(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=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
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 = 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
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  = btDot(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 || btDot(p[i],dir)>btDot(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=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
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 = 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// LOL BEGIN
279                                        btScalar ss = btSin(SIMD_RADS_PER_DEG*(xx));
280                                        btScalar cc = btCos(SIMD_RADS_PER_DEG*(xx));
281                                        int md = maxdirfiltered(p,count,dir+(u*ss+v*cc)*btScalar(0.025),allow);
282// LOL END
283                                        if(mc==m && md==m)
284                                        {
285                                                allow[m]=3;
286                                                return m;
287                                        }
288                                        mc=md;
289                                }
290                        }
291                        ma=mb;
292                }
293                allow[m]=0;
294                m=-1;
295        }
296        btAssert(0);
297        return m;
298}
299
300
301
302
303int operator ==(const int3 &a,const int3 &b);
304int operator ==(const int3 &a,const int3 &b)
305{
306        for(int i=0;i<3;i++)
307        {
308                if(a[i]!=b[i]) return 0;
309        }
310        return 1;
311}
312
313
314// LOL BEGIN
315int above(btVector3 const* vertices,const int3& t, const btVector3 &p, btScalar epsilon);
316int above(btVector3 const* vertices,const int3& t, const btVector3 &p, btScalar epsilon)
317// LOL END
318{
319        btVector3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]);
320        return (btDot(n,p-vertices[t[0]]) > epsilon); // EPSILON???
321}
322int hasedge(const int3 &t, int a,int b);
323int hasedge(const int3 &t, int a,int b)
324{
325        for(int i=0;i<3;i++)
326        {
327                int i1= (i+1)%3;
328                if(t[i]==a && t[i1]==b) return 1;
329        }
330        return 0;
331}
332int hasvert(const int3 &t, int v);
333int hasvert(const int3 &t, int v)
334{
335        return (t[0]==v || t[1]==v || t[2]==v) ;
336}
337int shareedge(const int3 &a,const int3 &b);
338int shareedge(const int3 &a,const int3 &b)
339{
340        int i;
341        for(i=0;i<3;i++)
342        {
343                int i1= (i+1)%3;
344                if(hasedge(a,b[i1],b[i])) return 1;
345        }
346        return 0;
347}
348
349class btHullTriangle;
350
351
352
353class btHullTriangle : public int3
354{
355public:
356        int3 n;
357        int id;
358        int vmax;
359        btScalar rise;
360        btHullTriangle(int a,int b,int c):int3(a,b,c),n(-1,-1,-1)
361        {
362                vmax=-1;
363                rise = btScalar(0.0);
364        }
365        ~btHullTriangle()
366        {
367        }
368        int &neib(int a,int b);
369};
370
371
372int &btHullTriangle::neib(int a,int b)
373{
374        static int er=-1;
375        int i;
376        for(i=0;i<3;i++)
377        {
378                int i1=(i+1)%3;
379                int i2=(i+2)%3;
380                if((*this)[i]==a && (*this)[i1]==b) return n[i2];
381                if((*this)[i]==b && (*this)[i1]==a) return n[i2];
382        }
383        btAssert(0);
384        return er;
385}
386void HullLibrary::b2bfix(btHullTriangle* s,btHullTriangle*t)
387{
388        int i;
389        for(i=0;i<3;i++)
390        {
391                int i1=(i+1)%3;
392                int i2=(i+2)%3;
393                int a = (*s)[i1];
394                int b = (*s)[i2];
395                btAssert(m_tris[s->neib(a,b)]->neib(b,a) == s->id);
396                btAssert(m_tris[t->neib(a,b)]->neib(b,a) == t->id);
397                m_tris[s->neib(a,b)]->neib(b,a) = t->neib(b,a);
398                m_tris[t->neib(b,a)]->neib(a,b) = s->neib(a,b);
399        }
400}
401
402void HullLibrary::removeb2b(btHullTriangle* s,btHullTriangle*t)
403{
404        b2bfix(s,t);
405        deAllocateTriangle(s);
406
407        deAllocateTriangle(t);
408}
409
410void HullLibrary::checkit(btHullTriangle *t)
411{
412        (void)t;
413
414        int i;
415        btAssert(m_tris[t->id]==t);
416        for(i=0;i<3;i++)
417        {
418                int i1=(i+1)%3;
419                int i2=(i+2)%3;
420                int a = (*t)[i1];
421                int b = (*t)[i2];
422
423                // release compile fix
424                (void)i1;
425                (void)i2;
426                (void)a;
427                (void)b;
428
429                btAssert(a!=b);
430                btAssert( m_tris[t->n[i]]->neib(b,a) == t->id);
431        }
432}
433
434btHullTriangle* HullLibrary::allocateTriangle(int a,int b,int c)
435{
436        void* mem = btAlignedAlloc(sizeof(btHullTriangle),16);
437        btHullTriangle* tr = new (mem)btHullTriangle(a,b,c);
438        tr->id = m_tris.size();
439        m_tris.push_back(tr);
440
441        return tr;
442}
443
444void    HullLibrary::deAllocateTriangle(btHullTriangle* tri)
445{
446        btAssert(m_tris[tri->id]==tri);
447        m_tris[tri->id]=NULL;
448        tri->~btHullTriangle();
449        btAlignedFree(tri);
450}
451
452
453void HullLibrary::extrude(btHullTriangle *t0,int v)
454{
455        int3 t= *t0;
456        int n = m_tris.size();
457        btHullTriangle* ta = allocateTriangle(v,t[1],t[2]);
458        ta->n = int3(t0->n[0],n+1,n+2);
459        m_tris[t0->n[0]]->neib(t[1],t[2]) = n+0;
460        btHullTriangle* tb = allocateTriangle(v,t[2],t[0]);
461        tb->n = int3(t0->n[1],n+2,n+0);
462        m_tris[t0->n[1]]->neib(t[2],t[0]) = n+1;
463        btHullTriangle* tc = allocateTriangle(v,t[0],t[1]);
464        tc->n = int3(t0->n[2],n+0,n+1);
465        m_tris[t0->n[2]]->neib(t[0],t[1]) = n+2;
466        checkit(ta);
467        checkit(tb);
468        checkit(tc);
469        if(hasvert(*m_tris[ta->n[0]],v)) removeb2b(ta,m_tris[ta->n[0]]);
470        if(hasvert(*m_tris[tb->n[0]],v)) removeb2b(tb,m_tris[tb->n[0]]);
471        if(hasvert(*m_tris[tc->n[0]],v)) removeb2b(tc,m_tris[tc->n[0]]);
472        deAllocateTriangle(t0);
473
474}
475
476btHullTriangle* HullLibrary::extrudable(btScalar epsilon)
477{
478        int i;
479        btHullTriangle *t=NULL;
480        for(i=0;i<m_tris.size();i++)
481        {
482                if(!t || (m_tris[i] && t->rise<m_tris[i]->rise))
483                {
484                        t = m_tris[i];
485                }
486        }
487        return (t->rise >epsilon)?t:NULL ;
488}
489
490
491
492
493// LOL BEGIN
494int4 HullLibrary::FindSimplex(btVector3 const *verts,int verts_count,btAlignedObjectArray<int> &allow)
495// LOL END
496{
497        btVector3 basis[3];
498        basis[0] = btVector3( btScalar(0.01), btScalar(0.02), btScalar(1.0) );     
499        int p0 = maxdirsterid(verts,verts_count, basis[0],allow);   
500        int     p1 = maxdirsterid(verts,verts_count,-basis[0],allow);
501        basis[0] = verts[p0]-verts[p1];
502        if(p0==p1 || basis[0]==btVector3(0,0,0))
503                return int4(-1,-1,-1,-1);
504        basis[1] = btCross(btVector3(     btScalar(1),btScalar(0.02), btScalar(0)),basis[0]);
505        basis[2] = btCross(btVector3(btScalar(-0.02),     btScalar(1), btScalar(0)),basis[0]);
506        if (basis[1].length() > basis[2].length())
507        {
508                basis[1].normalize();
509        } else {
510                basis[1] = basis[2];
511                basis[1].normalize ();
512        }
513        int p2 = maxdirsterid(verts,verts_count,basis[1],allow);
514        if(p2 == p0 || p2 == p1)
515        {
516                p2 = maxdirsterid(verts,verts_count,-basis[1],allow);
517        }
518        if(p2 == p0 || p2 == p1)
519                return int4(-1,-1,-1,-1);
520        basis[1] = verts[p2] - verts[p0];
521        basis[2] = btCross(basis[1],basis[0]).normalized();
522        int p3 = maxdirsterid(verts,verts_count,basis[2],allow);
523        if(p3==p0||p3==p1||p3==p2) p3 = maxdirsterid(verts,verts_count,-basis[2],allow);
524        if(p3==p0||p3==p1||p3==p2)
525                return int4(-1,-1,-1,-1);
526        btAssert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3));
527        if(btDot(verts[p3]-verts[p0],btCross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {Swap(p2,p3);}
528        return int4(p0,p1,p2,p3);
529}
530
531// LOL BEGIN
532int HullLibrary::calchullgen(btVector3 const *verts,int verts_count, int vlimit)
533// LOL END
534{
535        if(verts_count <4) return 0;
536        if(vlimit==0) vlimit=1000000000;
537        int j;
538        btVector3 bmin(*verts),bmax(*verts);
539        btAlignedObjectArray<int> isextreme;
540        isextreme.reserve(verts_count);
541        btAlignedObjectArray<int> allow;
542        allow.reserve(verts_count);
543
544        for(j=0;j<verts_count;j++)
545        {
546                allow.push_back(1);
547                isextreme.push_back(0);
548                bmin.setMin (verts[j]);
549                bmax.setMax (verts[j]);
550        }
551        btScalar epsilon = (bmax-bmin).length() * btScalar(0.001);
552        btAssert (epsilon != 0.0);
553
554
555        int4 p = FindSimplex(verts,verts_count,allow);
556        if(p.x==-1) return 0; // simplex failed
557
558
559
560        btVector3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) / btScalar(4.0);  // a valid interior point
561        btHullTriangle *t0 = allocateTriangle(p[2],p[3],p[1]); t0->n=int3(2,3,1);
562        btHullTriangle *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0);
563        btHullTriangle *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3);
564        btHullTriangle *t3 = allocateTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2);
565        isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1;
566        checkit(t0);checkit(t1);checkit(t2);checkit(t3);
567
568        for(j=0;j<m_tris.size();j++)
569        {
570                btHullTriangle *t=m_tris[j];
571                btAssert(t);
572                btAssert(t->vmax<0);
573                btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
574                t->vmax = maxdirsterid(verts,verts_count,n,allow);
575                t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]);
576        }
577        btHullTriangle *te;
578        vlimit-=4;
579        while(vlimit >0 && ((te=extrudable(epsilon)) != 0))
580        {
581// LOL BEGIN
582                //int3 ti=*te;
583// LOL END
584                int v=te->vmax;
585                btAssert(v != -1);
586                btAssert(!isextreme[v]);  // wtf we've already done this vertex
587                isextreme[v]=1;
588                //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already
589                j=m_tris.size();
590                while(j--) {
591                        if(!m_tris[j]) continue;
592                        int3 t=*m_tris[j];
593                        if(above(verts,t,verts[v],btScalar(0.01)*epsilon))
594                        {
595                                extrude(m_tris[j],v);
596                        }
597                }
598                // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle
599                j=m_tris.size();
600                while(j--)
601                {
602                        if(!m_tris[j]) continue;
603                        if(!hasvert(*m_tris[j],v)) break;
604                        int3 nt=*m_tris[j];
605                        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) )
606                        {
607                                btHullTriangle *nb = m_tris[m_tris[j]->n[0]];
608                                btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->id<j);
609                                extrude(nb,v);
610                                j=m_tris.size();
611                        }
612                }
613                j=m_tris.size();
614                while(j--)
615                {
616                        btHullTriangle *t=m_tris[j];
617                        if(!t) continue;
618                        if(t->vmax>=0) break;
619                        btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]);
620                        t->vmax = maxdirsterid(verts,verts_count,n,allow);
621                        if(isextreme[t->vmax])
622                        {
623                                t->vmax=-1; // already done that vertex - algorithm needs to be able to terminate.
624                        }
625                        else
626                        {
627                                t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]);
628                        }
629                }
630                vlimit --;
631        }
632        return 1;
633}
634
635// LOL BEGIN
636int HullLibrary::calchull(btVector3 const *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit)
637// LOL END
638{
639        int rc=calchullgen(verts,verts_count,  vlimit) ;
640        if(!rc) return 0;
641        btAlignedObjectArray<int> ts;
642        int i;
643
644        for(i=0;i<m_tris.size();i++)
645        {
646                if(m_tris[i])
647                {
648                        for(int j=0;j<3;j++)
649                                ts.push_back((*m_tris[i])[j]);
650                        deAllocateTriangle(m_tris[i]);
651                }
652        }
653        tris_count = ts.size()/3;
654        tris_out.resize(ts.size());
655       
656        for (i=0;i<ts.size();i++)
657        {
658                tris_out[i] = static_cast<unsigned int>(ts[i]);
659        }
660        m_tris.resize(0);
661
662        return 1;
663}
664
665
666
667
668
669bool HullLibrary::ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit)
670{
671       
672        int    tris_count;
673// LOL BEGIN
674        int ret = calchull( vertices, (int) vcount, result.m_Indices, tris_count, static_cast<int>(vlimit) );
675// LOL END
676        if(!ret) return false;
677        result.mIndexCount = (unsigned int) (tris_count*3);
678        result.mFaceCount  = (unsigned int) tris_count;
679        result.mVertices   = (btVector3*) vertices;
680        result.mVcount     = (unsigned int) vcount;
681        return true;
682
683}
684
685
686void ReleaseHull(PHullResult &result);
687void ReleaseHull(PHullResult &result)
688{
689        if ( result.m_Indices.size() )
690        {
691                result.m_Indices.clear();
692        }
693
694        result.mVcount = 0;
695        result.mIndexCount = 0;
696        result.mVertices = 0;
697}
698
699
700//*********************************************************************
701//*********************************************************************
702//********  HullLib header
703//*********************************************************************
704//*********************************************************************
705
706//*********************************************************************
707//*********************************************************************
708//********  HullLib implementation
709//*********************************************************************
710//*********************************************************************
711
712HullError HullLibrary::CreateConvexHull(const HullDesc       &desc,           // describes the input request
713                                                                                                                                                                        HullResult           &result)         // contains the resulst
714{
715        HullError ret = QE_FAIL;
716
717
718        PHullResult hr;
719
720        unsigned int vcount = desc.mVcount;
721        if ( vcount < 8 ) vcount = 8;
722
723        btAlignedObjectArray<btVector3> vertexSource;
724        vertexSource.resize(static_cast<int>(vcount));
725
726        btVector3 scale;
727
728        unsigned int ovcount;
729
730        bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, &vertexSource[0], desc.mNormalEpsilon, scale ); // normalize point cloud, remove duplicates!
731
732        if ( ok )
733        {
734
735
736//              if ( 1 ) // scale vertices back to their original size.
737                {
738                        for (unsigned int i=0; i<ovcount; i++)
739                        {
740                                btVector3& v = vertexSource[static_cast<int>(i)];
741                                v[0]*=scale[0];
742                                v[1]*=scale[1];
743                                v[2]*=scale[2];
744                        }
745                }
746
747                ok = ComputeHull(ovcount,&vertexSource[0],hr,desc.mMaxVertices);
748
749                if ( ok )
750                {
751
752                        // re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table.
753                        btAlignedObjectArray<btVector3> vertexScratch;
754                        vertexScratch.resize(static_cast<int>(hr.mVcount));
755
756                        BringOutYourDead(hr.mVertices,hr.mVcount, &vertexScratch[0], ovcount, &hr.m_Indices[0], hr.mIndexCount );
757
758                        ret = QE_OK;
759
760                        if ( desc.HasHullFlag(QF_TRIANGLES) ) // if he wants the results as triangle!
761                        {
762                                result.mPolygons          = false;
763                                result.mNumOutputVertices = ovcount;
764                                result.m_OutputVertices.resize(static_cast<int>(ovcount));
765                                result.mNumFaces          = hr.mFaceCount;
766                                result.mNumIndices        = hr.mIndexCount;
767
768                                result.m_Indices.resize(static_cast<int>(hr.mIndexCount));
769
770                                memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount );
771
772                        if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
773                                {
774
775                                        const unsigned int *source = &hr.m_Indices[0];
776                                        unsigned int *dest   = &result.m_Indices[0];
777
778                                        for (unsigned int i=0; i<hr.mFaceCount; i++)
779                                        {
780                                                dest[0] = source[2];
781                                                dest[1] = source[1];
782                                                dest[2] = source[0];
783                                                dest+=3;
784                                                source+=3;
785                                        }
786
787                                }
788                                else
789                                {
790                                        memcpy(&result.m_Indices[0], &hr.m_Indices[0], sizeof(unsigned int)*hr.mIndexCount);
791                                }
792                        }
793                        else
794                        {
795                                result.mPolygons          = true;
796                                result.mNumOutputVertices = ovcount;
797                                result.m_OutputVertices.resize(static_cast<int>(ovcount));
798                                result.mNumFaces          = hr.mFaceCount;
799                                result.mNumIndices        = hr.mIndexCount+hr.mFaceCount;
800                                result.m_Indices.resize(static_cast<int>(result.mNumIndices));
801                                memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount );
802
803//                              if ( 1 )
804                                {
805                                        const unsigned int *source = &hr.m_Indices[0];
806                                        unsigned int *dest   = &result.m_Indices[0];
807                                        for (unsigned int i=0; i<hr.mFaceCount; i++)
808                                        {
809                                                dest[0] = 3;
810                                                if ( desc.HasHullFlag(QF_REVERSE_ORDER) )
811                                                {
812                                                        dest[1] = source[2];
813                                                        dest[2] = source[1];
814                                                        dest[3] = source[0];
815                                                }
816                                                else
817                                                {
818                                                        dest[1] = source[0];
819                                                        dest[2] = source[1];
820                                                        dest[3] = source[2];
821                                                }
822
823                                                dest+=4;
824                                                source+=3;
825                                        }
826                                }
827                        }
828                        ReleaseHull(hr);
829                }
830        }
831
832        return ret;
833}
834
835
836
837HullError HullLibrary::ReleaseResult(HullResult &result) // release memory allocated for this result, we are done with it.
838{
839        if ( result.m_OutputVertices.size())
840        {
841                result.mNumOutputVertices=0;
842                result.m_OutputVertices.clear();
843        }
844        if ( result.m_Indices.size() )
845        {
846                result.mNumIndices=0;
847                result.m_Indices.clear();
848        }
849        return QE_OK;
850}
851
852
853static void addPoint(unsigned int &vcount,btVector3 *p,btScalar x,btScalar y,btScalar z)
854{
855        // XXX, might be broken
856        btVector3& dest = p[vcount];
857        dest[0] = x;
858        dest[1] = y;
859        dest[2] = z;
860        vcount++;
861}
862
863btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2);
864btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2)
865{
866
867        btScalar dx = px - p2[0];
868        btScalar dy = py - p2[1];
869        btScalar dz = pz - p2[2];
870
871        return dx*dx+dy*dy+dz*dz;
872}
873
874
875
876bool  HullLibrary::CleanupVertices(unsigned int svcount,
877                                   const btVector3 *svertices,
878                                   unsigned int stride,
879                                   unsigned int &vcount,       // output number of vertices
880                                   btVector3 *vertices,                 // location to store the results.
881                                   btScalar  normalepsilon,
882                                   btVector3& scale)
883{
884        if ( svcount == 0 ) return false;
885
886        m_vertexIndexMapping.resize(0);
887
888
889#define EPSILON btScalar(0.000001) /* close enough to consider two btScalaring point numbers to be 'the same'. */
890
891        vcount = 0;
892
893        btScalar recip[3]={0.f,0.f,0.f};
894
895        if ( scale )
896        {
897                scale[0] = 1;
898                scale[1] = 1;
899                scale[2] = 1;
900        }
901
902        btScalar bmin[3] = {  FLT_MAX,  FLT_MAX,  FLT_MAX };
903        btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
904
905        const char *vtx = (const char *) svertices;
906
907//      if ( 1 )
908        {
909                for (unsigned int i=0; i<svcount; i++)
910                {
911                        const btScalar *p = (const btScalar *) vtx;
912
913                        vtx+=stride;
914
915                        for (int j=0; j<3; j++)
916                        {
917                                if ( p[j] < bmin[j] ) bmin[j] = p[j];
918                                if ( p[j] > bmax[j] ) bmax[j] = p[j];
919                        }
920                }
921        }
922
923        btScalar dx = bmax[0] - bmin[0];
924        btScalar dy = bmax[1] - bmin[1];
925        btScalar dz = bmax[2] - bmin[2];
926
927        btVector3 center;
928
929        center[0] = dx*btScalar(0.5) + bmin[0];
930        center[1] = dy*btScalar(0.5) + bmin[1];
931        center[2] = dz*btScalar(0.5) + bmin[2];
932
933        if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3 )
934        {
935
936                btScalar len = FLT_MAX;
937
938                if ( dx > EPSILON && dx < len ) len = dx;
939                if ( dy > EPSILON && dy < len ) len = dy;
940                if ( dz > EPSILON && dz < len ) len = dz;
941
942                if ( len == FLT_MAX )
943                {
944                        dx = dy = dz = btScalar(0.01); // one centimeter
945                }
946                else
947                {
948                        if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge.
949                        if ( dy < EPSILON ) dy = len * btScalar(0.05);
950                        if ( dz < EPSILON ) dz = len * btScalar(0.05);
951                }
952
953                btScalar x1 = center[0] - dx;
954                btScalar x2 = center[0] + dx;
955
956                btScalar y1 = center[1] - dy;
957                btScalar y2 = center[1] + dy;
958
959                btScalar z1 = center[2] - dz;
960                btScalar z2 = center[2] + dz;
961
962                addPoint(vcount,vertices,x1,y1,z1);
963                addPoint(vcount,vertices,x2,y1,z1);
964                addPoint(vcount,vertices,x2,y2,z1);
965                addPoint(vcount,vertices,x1,y2,z1);
966                addPoint(vcount,vertices,x1,y1,z2);
967                addPoint(vcount,vertices,x2,y1,z2);
968                addPoint(vcount,vertices,x2,y2,z2);
969                addPoint(vcount,vertices,x1,y2,z2);
970
971                return true; // return cube
972
973
974        }
975        else
976        {
977                if ( scale )
978                {
979                        scale[0] = dx;
980                        scale[1] = dy;
981                        scale[2] = dz;
982
983                        recip[0] = 1 / dx;
984                        recip[1] = 1 / dy;
985                        recip[2] = 1 / dz;
986
987                        center[0]*=recip[0];
988                        center[1]*=recip[1];
989                        center[2]*=recip[2];
990
991                }
992
993        }
994
995
996
997        vtx = (const char *) svertices;
998
999        for (unsigned int i=0; i<svcount; i++)
1000        {
1001                const btVector3 *p = (const btVector3 *)vtx;
1002                vtx+=stride;
1003
1004                btScalar px = p->getX();
1005                btScalar py = p->getY();
1006                btScalar pz = p->getZ();
1007
1008                if ( scale )
1009                {
1010                        px = px*recip[0]; // normalize
1011                        py = py*recip[1]; // normalize
1012                        pz = pz*recip[2]; // normalize
1013                }
1014
1015//              if ( 1 )
1016                {
1017                        unsigned int j;
1018
1019                        for (j=0; j<vcount; j++)
1020                        {
1021                                /// XXX might be broken
1022                                btVector3& v = vertices[j];
1023
1024                                btScalar x = v[0];
1025                                btScalar y = v[1];
1026                                btScalar z = v[2];
1027
1028                                btScalar dx = btFabs(x - px );
1029                                btScalar dy = btFabs(y - py );
1030                                btScalar dz = btFabs(z - pz );
1031
1032                                if ( dx < normalepsilon && dy < normalepsilon && dz < normalepsilon )
1033                                {
1034                                        // ok, it is close enough to the old one
1035                                        // now let us see if it is further from the center of the point cloud than the one we already recorded.
1036                                        // in which case we keep this one instead.
1037
1038                                        btScalar dist1 = GetDist(px,py,pz,center);
1039                                        btScalar dist2 = GetDist(v[0],v[1],v[2],center);
1040
1041                                        if ( dist1 > dist2 )
1042                                        {
1043                                                v[0] = px;
1044                                                v[1] = py;
1045                                                v[2] = pz;
1046                                               
1047                                        }
1048
1049                                        break;
1050                                }
1051                        }
1052
1053                        if ( j == vcount )
1054                        {
1055                                btVector3& dest = vertices[vcount];
1056                                dest[0] = px;
1057                                dest[1] = py;
1058                                dest[2] = pz;
1059                                vcount++;
1060                        }
1061                        m_vertexIndexMapping.push_back(j);
1062                }
1063        }
1064
1065        // ok..now make sure we didn't prune so many vertices it is now invalid.
1066//      if ( 1 )
1067        {
1068                btScalar bmin[3] = {  FLT_MAX,  FLT_MAX,  FLT_MAX };
1069                btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX };
1070
1071                for (unsigned int i=0; i<vcount; i++)
1072                {
1073                        const btVector3& p = vertices[i];
1074                        for (int j=0; j<3; j++)
1075                        {
1076                                if ( p[j] < bmin[j] ) bmin[j] = p[j];
1077                                if ( p[j] > bmax[j] ) bmax[j] = p[j];
1078                        }
1079                }
1080
1081                btScalar dx = bmax[0] - bmin[0];
1082                btScalar dy = bmax[1] - bmin[1];
1083                btScalar dz = bmax[2] - bmin[2];
1084
1085                if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3)
1086                {
1087                        btScalar cx = dx*btScalar(0.5) + bmin[0];
1088                        btScalar cy = dy*btScalar(0.5) + bmin[1];
1089                        btScalar cz = dz*btScalar(0.5) + bmin[2];
1090
1091                        btScalar len = FLT_MAX;
1092
1093                        if ( dx >= EPSILON && dx < len ) len = dx;
1094                        if ( dy >= EPSILON && dy < len ) len = dy;
1095                        if ( dz >= EPSILON && dz < len ) len = dz;
1096
1097                        if ( len == FLT_MAX )
1098                        {
1099                                dx = dy = dz = btScalar(0.01); // one centimeter
1100                        }
1101                        else
1102                        {
1103                                if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge.
1104                                if ( dy < EPSILON ) dy = len * btScalar(0.05);
1105                                if ( dz < EPSILON ) dz = len * btScalar(0.05);
1106                        }
1107
1108                        btScalar x1 = cx - dx;
1109                        btScalar x2 = cx + dx;
1110
1111                        btScalar y1 = cy - dy;
1112                        btScalar y2 = cy + dy;
1113
1114                        btScalar z1 = cz - dz;
1115                        btScalar z2 = cz + dz;
1116
1117                        vcount = 0; // add box
1118
1119                        addPoint(vcount,vertices,x1,y1,z1);
1120                        addPoint(vcount,vertices,x2,y1,z1);
1121                        addPoint(vcount,vertices,x2,y2,z1);
1122                        addPoint(vcount,vertices,x1,y2,z1);
1123                        addPoint(vcount,vertices,x1,y1,z2);
1124                        addPoint(vcount,vertices,x2,y1,z2);
1125                        addPoint(vcount,vertices,x2,y2,z2);
1126                        addPoint(vcount,vertices,x1,y2,z2);
1127
1128                        return true;
1129                }
1130        }
1131
1132        return true;
1133}
1134
1135void HullLibrary::BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount)
1136{
1137        btAlignedObjectArray<int>tmpIndices;
1138        tmpIndices.resize(m_vertexIndexMapping.size());
1139        int i;
1140
1141        for (i=0;i<m_vertexIndexMapping.size();i++)
1142        {
1143                tmpIndices[i] = m_vertexIndexMapping[i];
1144        }
1145
1146        TUIntArray usedIndices;
1147        usedIndices.resize(static_cast<int>(vcount));
1148        memset(&usedIndices[0],0,sizeof(unsigned int)*vcount);
1149
1150        ocount = 0;
1151
1152        for (i=0; i<int (indexcount); i++)
1153        {
1154                unsigned int v = indices[i]; // original array index
1155
1156                btAssert( v >= 0 && v < vcount );
1157
1158                if ( usedIndices[static_cast<int>(v)] ) // if already remapped
1159                {
1160                        indices[i] = usedIndices[static_cast<int>(v)]-1; // index to new array
1161                }
1162                else
1163                {
1164
1165                        indices[i] = ocount;      // new index mapping
1166
1167                        overts[ocount][0] = verts[v][0]; // copy old vert to new vert array
1168                        overts[ocount][1] = verts[v][1];
1169                        overts[ocount][2] = verts[v][2];
1170
1171                        for (int k=0;k<m_vertexIndexMapping.size();k++)
1172                        {
1173                                if (tmpIndices[k]==int(v))
1174                                        m_vertexIndexMapping[k]=ocount;
1175                        }
1176
1177                        ocount++; // increment output vert count
1178
1179                        btAssert( ocount >=0 && ocount <= vcount );
1180
1181                        usedIndices[static_cast<int>(v)] = ocount; // assign new index remapping
1182
1183               
1184                }
1185        }
1186
1187       
1188}
Note: See TracBrowser for help on using the repository browser.