| /* |
| Stan Melax Convex Hull Computation |
| Copyright (c) 2003-2006 Stan Melax http://www.melax.com/ |
| |
| This software is provided 'as-is', without any express or implied warranty. |
| In no event will the authors be held liable for any damages arising from the use of this software. |
| Permission is granted to anyone to use this software for any purpose, |
| including commercial applications, and to alter it and redistribute it freely, |
| subject to the following restrictions: |
| |
| 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. |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. |
| 3. This notice may not be removed or altered from any source distribution. |
| */ |
| |
| #include <string.h> |
| |
| #include "LinearMath/btConvexHull.h" |
| #include "LinearMath/btAlignedObjectArray.h" |
| #include "LinearMath/btMinMax.h" |
| #include "LinearMath/btVector3.h" |
| |
| |
| |
| template <class T> |
| void Swap(T &a,T &b) |
| { |
| T tmp = a; |
| a=b; |
| b=tmp; |
| } |
| |
| |
| //---------------------------------- |
| |
| class int3 |
| { |
| public: |
| int x,y,z; |
| int3(){}; |
| int3(int _x,int _y, int _z){x=_x;y=_y;z=_z;} |
| const int& operator[](int i) const {return (&x)[i];} |
| int& operator[](int i) {return (&x)[i];} |
| }; |
| |
| |
| //------- btPlane ---------- |
| |
| |
| inline btPlane PlaneFlip(const btPlane &plane){return btPlane(-plane.normal,-plane.dist);} |
| inline int operator==( const btPlane &a, const btPlane &b ) { return (a.normal==b.normal && a.dist==b.dist); } |
| inline int coplanar( const btPlane &a, const btPlane &b ) { return (a==b || a==PlaneFlip(b)); } |
| |
| |
| //--------- Utility Functions ------ |
| |
| btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1); |
| btVector3 PlaneProject(const btPlane &plane, const btVector3 &point); |
| |
| btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2); |
| btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2) |
| { |
| btVector3 N1 = p0.normal; |
| btVector3 N2 = p1.normal; |
| btVector3 N3 = p2.normal; |
| |
| btVector3 n2n3; n2n3 = N2.cross(N3); |
| btVector3 n3n1; n3n1 = N3.cross(N1); |
| btVector3 n1n2; n1n2 = N1.cross(N2); |
| |
| btScalar quotient = (N1.dot(n2n3)); |
| |
| btAssert(btFabs(quotient) > btScalar(0.000001)); |
| |
| quotient = btScalar(-1.) / quotient; |
| n2n3 *= p0.dist; |
| n3n1 *= p1.dist; |
| n1n2 *= p2.dist; |
| btVector3 potentialVertex = n2n3; |
| potentialVertex += n3n1; |
| potentialVertex += n1n2; |
| potentialVertex *= quotient; |
| |
| btVector3 result(potentialVertex.getX(),potentialVertex.getY(),potentialVertex.getZ()); |
| return result; |
| |
| } |
| |
| btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint=NULL, btVector3 *vpoint=NULL); |
| btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2); |
| btVector3 NormalOf(const btVector3 *vert, const int n); |
| |
| |
| btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1) |
| { |
| // returns the point where the line p0-p1 intersects the plane n&d |
| static btVector3 dif; |
| dif = p1-p0; |
| btScalar dn= btDot(plane.normal,dif); |
| btScalar t = -(plane.dist+btDot(plane.normal,p0) )/dn; |
| return p0 + (dif*t); |
| } |
| |
| btVector3 PlaneProject(const btPlane &plane, const btVector3 &point) |
| { |
| return point - plane.normal * (btDot(point,plane.normal)+plane.dist); |
| } |
| |
| btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2) |
| { |
| // return the normal of the triangle |
| // inscribed by v0, v1, and v2 |
| btVector3 cp=btCross(v1-v0,v2-v1); |
| btScalar m=cp.length(); |
| if(m==0) return btVector3(1,0,0); |
| return cp*(btScalar(1.0)/m); |
| } |
| |
| |
| btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint, btVector3 *vpoint) |
| { |
| static btVector3 cp; |
| cp = btCross(udir,vdir).normalized(); |
| |
| btScalar distu = -btDot(cp,ustart); |
| btScalar distv = -btDot(cp,vstart); |
| btScalar dist = (btScalar)fabs(distu-distv); |
| if(upoint) |
| { |
| btPlane plane; |
| plane.normal = btCross(vdir,cp).normalized(); |
| plane.dist = -btDot(plane.normal,vstart); |
| *upoint = PlaneLineIntersection(plane,ustart,ustart+udir); |
| } |
| if(vpoint) |
| { |
| btPlane plane; |
| plane.normal = btCross(udir,cp).normalized(); |
| plane.dist = -btDot(plane.normal,ustart); |
| *vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir); |
| } |
| return dist; |
| } |
| |
| |
| |
| |
| |
| |
| |
| #define COPLANAR (0) |
| #define UNDER (1) |
| #define OVER (2) |
| #define SPLIT (OVER|UNDER) |
| #define PAPERWIDTH (btScalar(0.001)) |
| |
| btScalar planetestepsilon = PAPERWIDTH; |
| |
| |
| |
| typedef ConvexH::HalfEdge HalfEdge; |
| |
| ConvexH::ConvexH(int vertices_size,int edges_size,int facets_size) |
| { |
| vertices.resize(vertices_size); |
| edges.resize(edges_size); |
| facets.resize(facets_size); |
| } |
| |
| |
| int PlaneTest(const btPlane &p, const btVector3 &v); |
| int PlaneTest(const btPlane &p, const btVector3 &v) { |
| btScalar a = btDot(v,p.normal)+p.dist; |
| int flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR); |
| return flag; |
| } |
| |
| int SplitTest(ConvexH &convex,const btPlane &plane); |
| int SplitTest(ConvexH &convex,const btPlane &plane) { |
| int flag=0; |
| for(int i=0;i<convex.vertices.size();i++) { |
| flag |= PlaneTest(plane,convex.vertices[i]); |
| } |
| return flag; |
| } |
| |
| class VertFlag |
| { |
| public: |
| unsigned char planetest; |
| unsigned char junk; |
| unsigned char undermap; |
| unsigned char overmap; |
| }; |
| class EdgeFlag |
| { |
| public: |
| unsigned char planetest; |
| unsigned char fixes; |
| short undermap; |
| short overmap; |
| }; |
| class PlaneFlag |
| { |
| public: |
| unsigned char undermap; |
| unsigned char overmap; |
| }; |
| class Coplanar{ |
| public: |
| unsigned short ea; |
| unsigned char v0; |
| unsigned char v1; |
| }; |
| |
| |
| |
| |
| |
| |
| |
| |
| template<class T> |
| int maxdirfiltered(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow) |
| { |
| btAssert(count); |
| int m=-1; |
| for(int i=0;i<count;i++) |
| if(allow[i]) |
| { |
| if(m==-1 || btDot(p[i],dir)>btDot(p[m],dir)) |
| m=i; |
| } |
| btAssert(m!=-1); |
| return m; |
| } |
| |
| btVector3 orth(const btVector3 &v); |
| btVector3 orth(const btVector3 &v) |
| { |
| btVector3 a=btCross(v,btVector3(0,0,1)); |
| btVector3 b=btCross(v,btVector3(0,1,0)); |
| if (a.length() > b.length()) |
| { |
| return a.normalized(); |
| } else { |
| return b.normalized(); |
| } |
| } |
| |
| |
| template<class T> |
| int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow) |
| { |
| int m=-1; |
| while(m==-1) |
| { |
| m = maxdirfiltered(p,count,dir,allow); |
| if(allow[m]==3) return m; |
| T u = orth(dir); |
| T v = btCross(u,dir); |
| int ma=-1; |
| for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0)) |
| { |
| btScalar s = btSin(SIMD_RADS_PER_DEG*(x)); |
| btScalar c = btCos(SIMD_RADS_PER_DEG*(x)); |
| int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow); |
| if(ma==m && mb==m) |
| { |
| allow[m]=3; |
| return m; |
| } |
| if(ma!=-1 && ma!=mb) // Yuck - this is really ugly |
| { |
| int mc = ma; |
| for(btScalar xx = x-btScalar(40.0) ; xx <= x ; xx+= btScalar(5.0)) |
| { |
| btScalar s = btSin(SIMD_RADS_PER_DEG*(xx)); |
| btScalar c = btCos(SIMD_RADS_PER_DEG*(xx)); |
| int md = maxdirfiltered(p,count,dir+(u*s+v*c)*btScalar(0.025),allow); |
| if(mc==m && md==m) |
| { |
| allow[m]=3; |
| return m; |
| } |
| mc=md; |
| } |
| } |
| ma=mb; |
| } |
| allow[m]=0; |
| m=-1; |
| } |
| btAssert(0); |
| return m; |
| } |
| |
| |
| |
| |
| int operator ==(const int3 &a,const int3 &b); |
| int operator ==(const int3 &a,const int3 &b) |
| { |
| for(int i=0;i<3;i++) |
| { |
| if(a[i]!=b[i]) return 0; |
| } |
| return 1; |
| } |
| |
| |
| int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon); |
| int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon) |
| { |
| btVector3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]); |
| return (btDot(n,p-vertices[t[0]]) > epsilon); // EPSILON??? |
| } |
| int hasedge(const int3 &t, int a,int b); |
| int hasedge(const int3 &t, int a,int b) |
| { |
| for(int i=0;i<3;i++) |
| { |
| int i1= (i+1)%3; |
| if(t[i]==a && t[i1]==b) return 1; |
| } |
| return 0; |
| } |
| int hasvert(const int3 &t, int v); |
| int hasvert(const int3 &t, int v) |
| { |
| return (t[0]==v || t[1]==v || t[2]==v) ; |
| } |
| int shareedge(const int3 &a,const int3 &b); |
| int shareedge(const int3 &a,const int3 &b) |
| { |
| int i; |
| for(i=0;i<3;i++) |
| { |
| int i1= (i+1)%3; |
| if(hasedge(a,b[i1],b[i])) return 1; |
| } |
| return 0; |
| } |
| |
| class btHullTriangle; |
| |
| |
| |
| class btHullTriangle : public int3 |
| { |
| public: |
| int3 n; |
| int id; |
| int vmax; |
| btScalar rise; |
| btHullTriangle(int a,int b,int c):int3(a,b,c),n(-1,-1,-1) |
| { |
| vmax=-1; |
| rise = btScalar(0.0); |
| } |
| ~btHullTriangle() |
| { |
| } |
| int &neib(int a,int b); |
| }; |
| |
| |
| int &btHullTriangle::neib(int a,int b) |
| { |
| static int er=-1; |
| int i; |
| for(i=0;i<3;i++) |
| { |
| int i1=(i+1)%3; |
| int i2=(i+2)%3; |
| if((*this)[i]==a && (*this)[i1]==b) return n[i2]; |
| if((*this)[i]==b && (*this)[i1]==a) return n[i2]; |
| } |
| btAssert(0); |
| return er; |
| } |
| void HullLibrary::b2bfix(btHullTriangle* s,btHullTriangle*t) |
| { |
| int i; |
| for(i=0;i<3;i++) |
| { |
| int i1=(i+1)%3; |
| int i2=(i+2)%3; |
| int a = (*s)[i1]; |
| int b = (*s)[i2]; |
| btAssert(m_tris[s->neib(a,b)]->neib(b,a) == s->id); |
| btAssert(m_tris[t->neib(a,b)]->neib(b,a) == t->id); |
| m_tris[s->neib(a,b)]->neib(b,a) = t->neib(b,a); |
| m_tris[t->neib(b,a)]->neib(a,b) = s->neib(a,b); |
| } |
| } |
| |
| void HullLibrary::removeb2b(btHullTriangle* s,btHullTriangle*t) |
| { |
| b2bfix(s,t); |
| deAllocateTriangle(s); |
| |
| deAllocateTriangle(t); |
| } |
| |
| void HullLibrary::checkit(btHullTriangle *t) |
| { |
| (void)t; |
| |
| int i; |
| btAssert(m_tris[t->id]==t); |
| for(i=0;i<3;i++) |
| { |
| int i1=(i+1)%3; |
| int i2=(i+2)%3; |
| int a = (*t)[i1]; |
| int b = (*t)[i2]; |
| |
| // release compile fix |
| (void)i1; |
| (void)i2; |
| (void)a; |
| (void)b; |
| |
| btAssert(a!=b); |
| btAssert( m_tris[t->n[i]]->neib(b,a) == t->id); |
| } |
| } |
| |
| btHullTriangle* HullLibrary::allocateTriangle(int a,int b,int c) |
| { |
| void* mem = btAlignedAlloc(sizeof(btHullTriangle),16); |
| btHullTriangle* tr = new (mem)btHullTriangle(a,b,c); |
| tr->id = m_tris.size(); |
| m_tris.push_back(tr); |
| |
| return tr; |
| } |
| |
| void HullLibrary::deAllocateTriangle(btHullTriangle* tri) |
| { |
| btAssert(m_tris[tri->id]==tri); |
| m_tris[tri->id]=NULL; |
| tri->~btHullTriangle(); |
| btAlignedFree(tri); |
| } |
| |
| |
| void HullLibrary::extrude(btHullTriangle *t0,int v) |
| { |
| int3 t= *t0; |
| int n = m_tris.size(); |
| btHullTriangle* ta = allocateTriangle(v,t[1],t[2]); |
| ta->n = int3(t0->n[0],n+1,n+2); |
| m_tris[t0->n[0]]->neib(t[1],t[2]) = n+0; |
| btHullTriangle* tb = allocateTriangle(v,t[2],t[0]); |
| tb->n = int3(t0->n[1],n+2,n+0); |
| m_tris[t0->n[1]]->neib(t[2],t[0]) = n+1; |
| btHullTriangle* tc = allocateTriangle(v,t[0],t[1]); |
| tc->n = int3(t0->n[2],n+0,n+1); |
| m_tris[t0->n[2]]->neib(t[0],t[1]) = n+2; |
| checkit(ta); |
| checkit(tb); |
| checkit(tc); |
| if(hasvert(*m_tris[ta->n[0]],v)) removeb2b(ta,m_tris[ta->n[0]]); |
| if(hasvert(*m_tris[tb->n[0]],v)) removeb2b(tb,m_tris[tb->n[0]]); |
| if(hasvert(*m_tris[tc->n[0]],v)) removeb2b(tc,m_tris[tc->n[0]]); |
| deAllocateTriangle(t0); |
| |
| } |
| |
| btHullTriangle* HullLibrary::extrudable(btScalar epsilon) |
| { |
| int i; |
| btHullTriangle *t=NULL; |
| for(i=0;i<m_tris.size();i++) |
| { |
| if(!t || (m_tris[i] && t->rise<m_tris[i]->rise)) |
| { |
| t = m_tris[i]; |
| } |
| } |
| return (t->rise >epsilon)?t:NULL ; |
| } |
| |
| |
| |
| |
| int4 HullLibrary::FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray<int> &allow) |
| { |
| btVector3 basis[3]; |
| basis[0] = btVector3( btScalar(0.01), btScalar(0.02), btScalar(1.0) ); |
| int p0 = maxdirsterid(verts,verts_count, basis[0],allow); |
| int p1 = maxdirsterid(verts,verts_count,-basis[0],allow); |
| basis[0] = verts[p0]-verts[p1]; |
| if(p0==p1 || basis[0]==btVector3(0,0,0)) |
| return int4(-1,-1,-1,-1); |
| basis[1] = btCross(btVector3( btScalar(1),btScalar(0.02), btScalar(0)),basis[0]); |
| basis[2] = btCross(btVector3(btScalar(-0.02), btScalar(1), btScalar(0)),basis[0]); |
| if (basis[1].length() > basis[2].length()) |
| { |
| basis[1].normalize(); |
| } else { |
| basis[1] = basis[2]; |
| basis[1].normalize (); |
| } |
| int p2 = maxdirsterid(verts,verts_count,basis[1],allow); |
| if(p2 == p0 || p2 == p1) |
| { |
| p2 = maxdirsterid(verts,verts_count,-basis[1],allow); |
| } |
| if(p2 == p0 || p2 == p1) |
| return int4(-1,-1,-1,-1); |
| basis[1] = verts[p2] - verts[p0]; |
| basis[2] = btCross(basis[1],basis[0]).normalized(); |
| int p3 = maxdirsterid(verts,verts_count,basis[2],allow); |
| if(p3==p0||p3==p1||p3==p2) p3 = maxdirsterid(verts,verts_count,-basis[2],allow); |
| if(p3==p0||p3==p1||p3==p2) |
| return int4(-1,-1,-1,-1); |
| btAssert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3)); |
| if(btDot(verts[p3]-verts[p0],btCross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {Swap(p2,p3);} |
| return int4(p0,p1,p2,p3); |
| } |
| |
| int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit) |
| { |
| if(verts_count <4) return 0; |
| if(vlimit==0) vlimit=1000000000; |
| int j; |
| btVector3 bmin(*verts),bmax(*verts); |
| btAlignedObjectArray<int> isextreme; |
| isextreme.reserve(verts_count); |
| btAlignedObjectArray<int> allow; |
| allow.reserve(verts_count); |
| |
| for(j=0;j<verts_count;j++) |
| { |
| allow.push_back(1); |
| isextreme.push_back(0); |
| bmin.setMin (verts[j]); |
| bmax.setMax (verts[j]); |
| } |
| btScalar epsilon = (bmax-bmin).length() * btScalar(0.001); |
| btAssert (epsilon != 0.0); |
| |
| |
| int4 p = FindSimplex(verts,verts_count,allow); |
| if(p.x==-1) return 0; // simplex failed |
| |
| |
| |
| btVector3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) / btScalar(4.0); // a valid interior point |
| btHullTriangle *t0 = allocateTriangle(p[2],p[3],p[1]); t0->n=int3(2,3,1); |
| btHullTriangle *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0); |
| btHullTriangle *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3); |
| btHullTriangle *t3 = allocateTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2); |
| isextreme[p[0]]=isextreme[p[1]]=isextreme[p[2]]=isextreme[p[3]]=1; |
| checkit(t0);checkit(t1);checkit(t2);checkit(t3); |
| |
| for(j=0;j<m_tris.size();j++) |
| { |
| btHullTriangle *t=m_tris[j]; |
| btAssert(t); |
| btAssert(t->vmax<0); |
| btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); |
| t->vmax = maxdirsterid(verts,verts_count,n,allow); |
| t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]); |
| } |
| btHullTriangle *te; |
| vlimit-=4; |
| while(vlimit >0 && ((te=extrudable(epsilon)) != 0)) |
| { |
| int3 ti=*te; |
| int v=te->vmax; |
| btAssert(v != -1); |
| btAssert(!isextreme[v]); // wtf we've already done this vertex |
| isextreme[v]=1; |
| //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already |
| j=m_tris.size(); |
| while(j--) { |
| if(!m_tris[j]) continue; |
| int3 t=*m_tris[j]; |
| if(above(verts,t,verts[v],btScalar(0.01)*epsilon)) |
| { |
| extrude(m_tris[j],v); |
| } |
| } |
| // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle |
| j=m_tris.size(); |
| while(j--) |
| { |
| if(!m_tris[j]) continue; |
| if(!hasvert(*m_tris[j],v)) break; |
| int3 nt=*m_tris[j]; |
| 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) ) |
| { |
| btHullTriangle *nb = m_tris[m_tris[j]->n[0]]; |
| btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->id<j); |
| extrude(nb,v); |
| j=m_tris.size(); |
| } |
| } |
| j=m_tris.size(); |
| while(j--) |
| { |
| btHullTriangle *t=m_tris[j]; |
| if(!t) continue; |
| if(t->vmax>=0) break; |
| btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); |
| t->vmax = maxdirsterid(verts,verts_count,n,allow); |
| if(isextreme[t->vmax]) |
| { |
| t->vmax=-1; // already done that vertex - algorithm needs to be able to terminate. |
| } |
| else |
| { |
| t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]); |
| } |
| } |
| vlimit --; |
| } |
| return 1; |
| } |
| |
| int HullLibrary::calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit) |
| { |
| int rc=calchullgen(verts,verts_count, vlimit) ; |
| if(!rc) return 0; |
| btAlignedObjectArray<int> ts; |
| int i; |
| |
| for(i=0;i<m_tris.size();i++) |
| { |
| if(m_tris[i]) |
| { |
| for(int j=0;j<3;j++) |
| ts.push_back((*m_tris[i])[j]); |
| deAllocateTriangle(m_tris[i]); |
| } |
| } |
| tris_count = ts.size()/3; |
| tris_out.resize(ts.size()); |
| |
| for (i=0;i<ts.size();i++) |
| { |
| tris_out[i] = static_cast<unsigned int>(ts[i]); |
| } |
| m_tris.resize(0); |
| |
| return 1; |
| } |
| |
| |
| |
| |
| |
| bool HullLibrary::ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit) |
| { |
| |
| int tris_count; |
| int ret = calchull( (btVector3 *) vertices, (int) vcount, result.m_Indices, tris_count, static_cast<int>(vlimit) ); |
| if(!ret) return false; |
| result.mIndexCount = (unsigned int) (tris_count*3); |
| result.mFaceCount = (unsigned int) tris_count; |
| result.mVertices = (btVector3*) vertices; |
| result.mVcount = (unsigned int) vcount; |
| return true; |
| |
| } |
| |
| |
| void ReleaseHull(PHullResult &result); |
| void ReleaseHull(PHullResult &result) |
| { |
| if ( result.m_Indices.size() ) |
| { |
| result.m_Indices.clear(); |
| } |
| |
| result.mVcount = 0; |
| result.mIndexCount = 0; |
| result.mVertices = 0; |
| } |
| |
| |
| //********************************************************************* |
| //********************************************************************* |
| //******** HullLib header |
| //********************************************************************* |
| //********************************************************************* |
| |
| //********************************************************************* |
| //********************************************************************* |
| //******** HullLib implementation |
| //********************************************************************* |
| //********************************************************************* |
| |
| HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // describes the input request |
| HullResult &result) // contains the resulst |
| { |
| HullError ret = QE_FAIL; |
| |
| |
| PHullResult hr; |
| |
| unsigned int vcount = desc.mVcount; |
| if ( vcount < 8 ) vcount = 8; |
| |
| btAlignedObjectArray<btVector3> vertexSource; |
| vertexSource.resize(static_cast<int>(vcount)); |
| |
| btVector3 scale; |
| |
| unsigned int ovcount; |
| |
| bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, &vertexSource[0], desc.mNormalEpsilon, scale ); // normalize point cloud, remove duplicates! |
| |
| if ( ok ) |
| { |
| |
| |
| // if ( 1 ) // scale vertices back to their original size. |
| { |
| for (unsigned int i=0; i<ovcount; i++) |
| { |
| btVector3& v = vertexSource[static_cast<int>(i)]; |
| v[0]*=scale[0]; |
| v[1]*=scale[1]; |
| v[2]*=scale[2]; |
| } |
| } |
| |
| ok = ComputeHull(ovcount,&vertexSource[0],hr,desc.mMaxVertices); |
| |
| if ( ok ) |
| { |
| |
| // re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table. |
| btAlignedObjectArray<btVector3> vertexScratch; |
| vertexScratch.resize(static_cast<int>(hr.mVcount)); |
| |
| BringOutYourDead(hr.mVertices,hr.mVcount, &vertexScratch[0], ovcount, &hr.m_Indices[0], hr.mIndexCount ); |
| |
| ret = QE_OK; |
| |
| if ( desc.HasHullFlag(QF_TRIANGLES) ) // if he wants the results as triangle! |
| { |
| result.mPolygons = false; |
| result.mNumOutputVertices = ovcount; |
| result.m_OutputVertices.resize(static_cast<int>(ovcount)); |
| result.mNumFaces = hr.mFaceCount; |
| result.mNumIndices = hr.mIndexCount; |
| |
| result.m_Indices.resize(static_cast<int>(hr.mIndexCount)); |
| |
| memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount ); |
| |
| if ( desc.HasHullFlag(QF_REVERSE_ORDER) ) |
| { |
| |
| const unsigned int *source = &hr.m_Indices[0]; |
| unsigned int *dest = &result.m_Indices[0]; |
| |
| for (unsigned int i=0; i<hr.mFaceCount; i++) |
| { |
| dest[0] = source[2]; |
| dest[1] = source[1]; |
| dest[2] = source[0]; |
| dest+=3; |
| source+=3; |
| } |
| |
| } |
| else |
| { |
| memcpy(&result.m_Indices[0], &hr.m_Indices[0], sizeof(unsigned int)*hr.mIndexCount); |
| } |
| } |
| else |
| { |
| result.mPolygons = true; |
| result.mNumOutputVertices = ovcount; |
| result.m_OutputVertices.resize(static_cast<int>(ovcount)); |
| result.mNumFaces = hr.mFaceCount; |
| result.mNumIndices = hr.mIndexCount+hr.mFaceCount; |
| result.m_Indices.resize(static_cast<int>(result.mNumIndices)); |
| memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount ); |
| |
| // if ( 1 ) |
| { |
| const unsigned int *source = &hr.m_Indices[0]; |
| unsigned int *dest = &result.m_Indices[0]; |
| for (unsigned int i=0; i<hr.mFaceCount; i++) |
| { |
| dest[0] = 3; |
| if ( desc.HasHullFlag(QF_REVERSE_ORDER) ) |
| { |
| dest[1] = source[2]; |
| dest[2] = source[1]; |
| dest[3] = source[0]; |
| } |
| else |
| { |
| dest[1] = source[0]; |
| dest[2] = source[1]; |
| dest[3] = source[2]; |
| } |
| |
| dest+=4; |
| source+=3; |
| } |
| } |
| } |
| ReleaseHull(hr); |
| } |
| } |
| |
| return ret; |
| } |
| |
| |
| |
| HullError HullLibrary::ReleaseResult(HullResult &result) // release memory allocated for this result, we are done with it. |
| { |
| if ( result.m_OutputVertices.size()) |
| { |
| result.mNumOutputVertices=0; |
| result.m_OutputVertices.clear(); |
| } |
| if ( result.m_Indices.size() ) |
| { |
| result.mNumIndices=0; |
| result.m_Indices.clear(); |
| } |
| return QE_OK; |
| } |
| |
| |
| static void addPoint(unsigned int &vcount,btVector3 *p,btScalar x,btScalar y,btScalar z) |
| { |
| // XXX, might be broken |
| btVector3& dest = p[vcount]; |
| dest[0] = x; |
| dest[1] = y; |
| dest[2] = z; |
| vcount++; |
| } |
| |
| btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2); |
| btScalar GetDist(btScalar px,btScalar py,btScalar pz,const btScalar *p2) |
| { |
| |
| btScalar dx = px - p2[0]; |
| btScalar dy = py - p2[1]; |
| btScalar dz = pz - p2[2]; |
| |
| return dx*dx+dy*dy+dz*dz; |
| } |
| |
| |
| |
| bool HullLibrary::CleanupVertices(unsigned int svcount, |
| const btVector3 *svertices, |
| unsigned int stride, |
| unsigned int &vcount, // output number of vertices |
| btVector3 *vertices, // location to store the results. |
| btScalar normalepsilon, |
| btVector3& scale) |
| { |
| if ( svcount == 0 ) return false; |
| |
| m_vertexIndexMapping.resize(0); |
| |
| |
| #define EPSILON btScalar(0.000001) /* close enough to consider two btScalaring point numbers to be 'the same'. */ |
| |
| vcount = 0; |
| |
| btScalar recip[3]={0.f,0.f,0.f}; |
| |
| if ( scale ) |
| { |
| scale[0] = 1; |
| scale[1] = 1; |
| scale[2] = 1; |
| } |
| |
| btScalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX }; |
| btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX }; |
| |
| const char *vtx = (const char *) svertices; |
| |
| // if ( 1 ) |
| { |
| for (unsigned int i=0; i<svcount; i++) |
| { |
| const btScalar *p = (const btScalar *) vtx; |
| |
| vtx+=stride; |
| |
| for (int j=0; j<3; j++) |
| { |
| if ( p[j] < bmin[j] ) bmin[j] = p[j]; |
| if ( p[j] > bmax[j] ) bmax[j] = p[j]; |
| } |
| } |
| } |
| |
| btScalar dx = bmax[0] - bmin[0]; |
| btScalar dy = bmax[1] - bmin[1]; |
| btScalar dz = bmax[2] - bmin[2]; |
| |
| btVector3 center; |
| |
| center[0] = dx*btScalar(0.5) + bmin[0]; |
| center[1] = dy*btScalar(0.5) + bmin[1]; |
| center[2] = dz*btScalar(0.5) + bmin[2]; |
| |
| if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3 ) |
| { |
| |
| btScalar len = FLT_MAX; |
| |
| if ( dx > EPSILON && dx < len ) len = dx; |
| if ( dy > EPSILON && dy < len ) len = dy; |
| if ( dz > EPSILON && dz < len ) len = dz; |
| |
| if ( len == FLT_MAX ) |
| { |
| dx = dy = dz = btScalar(0.01); // one centimeter |
| } |
| else |
| { |
| if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge. |
| if ( dy < EPSILON ) dy = len * btScalar(0.05); |
| if ( dz < EPSILON ) dz = len * btScalar(0.05); |
| } |
| |
| btScalar x1 = center[0] - dx; |
| btScalar x2 = center[0] + dx; |
| |
| btScalar y1 = center[1] - dy; |
| btScalar y2 = center[1] + dy; |
| |
| btScalar z1 = center[2] - dz; |
| btScalar z2 = center[2] + dz; |
| |
| addPoint(vcount,vertices,x1,y1,z1); |
| addPoint(vcount,vertices,x2,y1,z1); |
| addPoint(vcount,vertices,x2,y2,z1); |
| addPoint(vcount,vertices,x1,y2,z1); |
| addPoint(vcount,vertices,x1,y1,z2); |
| addPoint(vcount,vertices,x2,y1,z2); |
| addPoint(vcount,vertices,x2,y2,z2); |
| addPoint(vcount,vertices,x1,y2,z2); |
| |
| return true; // return cube |
| |
| |
| } |
| else |
| { |
| if ( scale ) |
| { |
| scale[0] = dx; |
| scale[1] = dy; |
| scale[2] = dz; |
| |
| recip[0] = 1 / dx; |
| recip[1] = 1 / dy; |
| recip[2] = 1 / dz; |
| |
| center[0]*=recip[0]; |
| center[1]*=recip[1]; |
| center[2]*=recip[2]; |
| |
| } |
| |
| } |
| |
| |
| |
| vtx = (const char *) svertices; |
| |
| for (unsigned int i=0; i<svcount; i++) |
| { |
| const btVector3 *p = (const btVector3 *)vtx; |
| vtx+=stride; |
| |
| btScalar px = p->getX(); |
| btScalar py = p->getY(); |
| btScalar pz = p->getZ(); |
| |
| if ( scale ) |
| { |
| px = px*recip[0]; // normalize |
| py = py*recip[1]; // normalize |
| pz = pz*recip[2]; // normalize |
| } |
| |
| // if ( 1 ) |
| { |
| unsigned int j; |
| |
| for (j=0; j<vcount; j++) |
| { |
| /// XXX might be broken |
| btVector3& v = vertices[j]; |
| |
| btScalar x = v[0]; |
| btScalar y = v[1]; |
| btScalar z = v[2]; |
| |
| btScalar dx = fabsf(x - px ); |
| btScalar dy = fabsf(y - py ); |
| btScalar dz = fabsf(z - pz ); |
| |
| if ( dx < normalepsilon && dy < normalepsilon && dz < normalepsilon ) |
| { |
| // ok, it is close enough to the old one |
| // now let us see if it is further from the center of the point cloud than the one we already recorded. |
| // in which case we keep this one instead. |
| |
| btScalar dist1 = GetDist(px,py,pz,center); |
| btScalar dist2 = GetDist(v[0],v[1],v[2],center); |
| |
| if ( dist1 > dist2 ) |
| { |
| v[0] = px; |
| v[1] = py; |
| v[2] = pz; |
| |
| } |
| |
| break; |
| } |
| } |
| |
| if ( j == vcount ) |
| { |
| btVector3& dest = vertices[vcount]; |
| dest[0] = px; |
| dest[1] = py; |
| dest[2] = pz; |
| vcount++; |
| } |
| m_vertexIndexMapping.push_back(j); |
| } |
| } |
| |
| // ok..now make sure we didn't prune so many vertices it is now invalid. |
| // if ( 1 ) |
| { |
| btScalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX }; |
| btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX }; |
| |
| for (unsigned int i=0; i<vcount; i++) |
| { |
| const btVector3& p = vertices[i]; |
| for (int j=0; j<3; j++) |
| { |
| if ( p[j] < bmin[j] ) bmin[j] = p[j]; |
| if ( p[j] > bmax[j] ) bmax[j] = p[j]; |
| } |
| } |
| |
| btScalar dx = bmax[0] - bmin[0]; |
| btScalar dy = bmax[1] - bmin[1]; |
| btScalar dz = bmax[2] - bmin[2]; |
| |
| if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3) |
| { |
| btScalar cx = dx*btScalar(0.5) + bmin[0]; |
| btScalar cy = dy*btScalar(0.5) + bmin[1]; |
| btScalar cz = dz*btScalar(0.5) + bmin[2]; |
| |
| btScalar len = FLT_MAX; |
| |
| if ( dx >= EPSILON && dx < len ) len = dx; |
| if ( dy >= EPSILON && dy < len ) len = dy; |
| if ( dz >= EPSILON && dz < len ) len = dz; |
| |
| if ( len == FLT_MAX ) |
| { |
| dx = dy = dz = btScalar(0.01); // one centimeter |
| } |
| else |
| { |
| if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge. |
| if ( dy < EPSILON ) dy = len * btScalar(0.05); |
| if ( dz < EPSILON ) dz = len * btScalar(0.05); |
| } |
| |
| btScalar x1 = cx - dx; |
| btScalar x2 = cx + dx; |
| |
| btScalar y1 = cy - dy; |
| btScalar y2 = cy + dy; |
| |
| btScalar z1 = cz - dz; |
| btScalar z2 = cz + dz; |
| |
| vcount = 0; // add box |
| |
| addPoint(vcount,vertices,x1,y1,z1); |
| addPoint(vcount,vertices,x2,y1,z1); |
| addPoint(vcount,vertices,x2,y2,z1); |
| addPoint(vcount,vertices,x1,y2,z1); |
| addPoint(vcount,vertices,x1,y1,z2); |
| addPoint(vcount,vertices,x2,y1,z2); |
| addPoint(vcount,vertices,x2,y2,z2); |
| addPoint(vcount,vertices,x1,y2,z2); |
| |
| return true; |
| } |
| } |
| |
| return true; |
| } |
| |
| void HullLibrary::BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount) |
| { |
| btAlignedObjectArray<int>tmpIndices; |
| tmpIndices.resize(m_vertexIndexMapping.size()); |
| int i; |
| |
| for (i=0;i<m_vertexIndexMapping.size();i++) |
| { |
| tmpIndices[i] = m_vertexIndexMapping[i]; |
| } |
| |
| TUIntArray usedIndices; |
| usedIndices.resize(static_cast<int>(vcount)); |
| memset(&usedIndices[0],0,sizeof(unsigned int)*vcount); |
| |
| ocount = 0; |
| |
| for (i=0; i<int (indexcount); i++) |
| { |
| unsigned int v = indices[i]; // original array index |
| |
| btAssert( v >= 0 && v < vcount ); |
| |
| if ( usedIndices[static_cast<int>(v)] ) // if already remapped |
| { |
| indices[i] = usedIndices[static_cast<int>(v)]-1; // index to new array |
| } |
| else |
| { |
| |
| indices[i] = ocount; // new index mapping |
| |
| overts[ocount][0] = verts[v][0]; // copy old vert to new vert array |
| overts[ocount][1] = verts[v][1]; |
| overts[ocount][2] = verts[v][2]; |
| |
| for (int k=0;k<m_vertexIndexMapping.size();k++) |
| { |
| if (tmpIndices[k]==v) |
| m_vertexIndexMapping[k]=ocount; |
| } |
| |
| ocount++; // increment output vert count |
| |
| btAssert( ocount >=0 && ocount <= vcount ); |
| |
| usedIndices[static_cast<int>(v)] = ocount; // assign new index remapping |
| |
| |
| } |
| } |
| |
| |
| } |