diff --git a/Extras/ConvexHull/btConvexHull.cpp b/Extras/ConvexHull/btConvexHull.cpp index 56773a5ec..511371ee0 100644 --- a/Extras/ConvexHull/btConvexHull.cpp +++ b/Extras/ConvexHull/btConvexHull.cpp @@ -20,6 +20,8 @@ subject to the following restrictions: #include "LinearMath/btMinMax.h" #include "LinearMath/btVector3.h" + + template void Swap(T &a,T &b) { @@ -42,29 +44,20 @@ public: }; -//------- Plane ---------- +//------- btPlane ---------- -class Plane -{ - public: - btVector3 normal; - btScalar dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0 - Plane(const btVector3 &n,btScalar d):normal(n),dist(d){} - Plane():normal(),dist(0){} - -}; -inline Plane PlaneFlip(const Plane &plane){return Plane(-plane.normal,-plane.dist);} -inline int operator==( const Plane &a, const Plane &b ) { return (a.normal==b.normal && a.dist==b.dist); } -inline int coplanar( const Plane &a, const Plane &b ) { return (a==b || a==PlaneFlip(b)); } +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 Plane &plane, const btVector3 &p0, const btVector3 &p1); -btVector3 PlaneProject(const Plane &plane, const btVector3 &point); +btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1); +btVector3 PlaneProject(const btPlane &plane, const btVector3 &point); -btVector3 ThreePlaneIntersection(const Plane &p0,const Plane &p1, const Plane &p2) +btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btPlane &p2) { btVector3 N1 = p0.normal; btVector3 N2 = p1.normal; @@ -97,7 +90,7 @@ btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 & btVector3 NormalOf(const btVector3 *vert, const int n); -btVector3 PlaneLineIntersection(const Plane &plane, const btVector3 &p0, const btVector3 &p1) +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; @@ -107,7 +100,7 @@ btVector3 PlaneLineIntersection(const Plane &plane, const btVector3 &p0, const b return p0 + (dif*t); } -btVector3 PlaneProject(const Plane &plane, const btVector3 &point) +btVector3 PlaneProject(const btPlane &plane, const btVector3 &point) { return point - plane.normal * (dot(point,plane.normal)+plane.dist); } @@ -133,14 +126,14 @@ btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, co btScalar dist = (btScalar)fabs(distu-distv); if(upoint) { - Plane plane; + btPlane plane; plane.normal = cross(vdir,cp).normalized(); plane.dist = -dot(plane.normal,vstart); *upoint = PlaneLineIntersection(plane,ustart,ustart+udir); } if(vpoint) { - Plane plane; + btPlane plane; plane.normal = cross(udir,cp).normalized(); plane.dist = -dot(plane.normal,ustart); *vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir); @@ -151,29 +144,8 @@ btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, co -class PHullResult -{ -public: - - PHullResult(void) - { - mVcount = 0; - mIndexCount = 0; - mFaceCount = 0; - mVertices = 0; - mIndices = 0; - } - - unsigned int mVcount; - unsigned int mIndexCount; - unsigned int mFaceCount; - btVector3* mVertices; - unsigned int *mIndices; -}; -#define REAL3 btVector3 -#define REAL btScalar #define COPLANAR (0) #define UNDER (1) @@ -184,33 +156,6 @@ public: btScalar planetestepsilon = PAPERWIDTH; -class ConvexH -{ - public: - class HalfEdge - { - public: - short ea; // the other half of the edge (index into edges list) - unsigned char v; // the vertex at the start of this edge (index into vertices list) - unsigned char p; // the facet on which this edge lies (index into facets list) - HalfEdge(){} - HalfEdge(short _ea,unsigned char _v, unsigned char _p):ea(_ea),v(_v),p(_p){} - }; - ConvexH() - { - int i; - i=0; - } - ~ConvexH() - { - int i; - i=0; - } - btAlignedObjectArray vertices; - btAlignedObjectArray edges; - btAlignedObjectArray facets; - ConvexH(int vertices_size,int edges_size,int facets_size); -}; typedef ConvexH::HalfEdge HalfEdge; @@ -221,36 +166,15 @@ ConvexH::ConvexH(int vertices_size,int edges_size,int facets_size) facets.resize(facets_size); } -ConvexH *ConvexHDup(ConvexH *src) { - ConvexH *dst = new ConvexH(src->vertices.size(),src->edges.size(),src->facets.size()); - - int i; - for (i=0;ivertices.size();i++) - { - dst->vertices[i] = src->vertices[i]; - } - - for (i=0;iedges.size();i++) - { - dst->edges[i] = src->edges[i]; - } - - for (i=0;ifacets.size();i++) - { - dst->facets[i] = src->facets[i]; - } - - return dst; -} -int PlaneTest(const Plane &p, const REAL3 &v) { - REAL a = dot(v,p.normal)+p.dist; +int PlaneTest(const btPlane &p, const btVector3 &v) { + btScalar a = dot(v,p.normal)+p.dist; int flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR); return flag; } -int SplitTest(ConvexH &convex,const Plane &plane) { +int SplitTest(ConvexH &convex,const btPlane &plane) { int flag=0; for(int i=0;i= convex.edges.size() || convex.edges[inext].p != convex.edges[i].p) { - inext = estart; - } - assert(convex.edges[inext].p == convex.edges[i].p); - int nb = convex.edges[i].ea; - assert(nb!=255); - if(nb==255 || nb==-1) return 0; - assert(nb!=-1); - assert(i== convex.edges[nb].ea); - } - for(i=0;i= convex.edges.size() || convex.edges[i1].p != convex.edges[i].p) { - i1 = estart; - } - int i2 = i1+1; - if(i2>= convex.edges.size() || convex.edges[i2].p != convex.edges[i].p) { - i2 = estart; - } - if(i==i2) continue; // i sliced tangent to an edge and created 2 meaningless edges - REAL3 localnormal = TriNormal(convex.vertices[convex.edges[i ].v], - convex.vertices[convex.edges[i1].v], - convex.vertices[convex.edges[i2].v]); - assert(dot(localnormal,convex.facets[convex.edges[i].p].normal)>0); - if(dot(localnormal,convex.facets[convex.edges[i].p].normal)<=0)return 0; - } - return 1; -} - -// back to back quads -ConvexH *test_btbq() { - ConvexH *convex = new ConvexH(4,8,2); - convex->vertices[0] = REAL3(0,0,0); - convex->vertices[1] = REAL3(1,0,0); - convex->vertices[2] = REAL3(1,1,0); - convex->vertices[3] = REAL3(0,1,0); - convex->facets[0] = Plane(REAL3(0,0,1),0); - convex->facets[1] = Plane(REAL3(0,0,-1),0); - convex->edges[0] = HalfEdge(7,0,0); - convex->edges[1] = HalfEdge(6,1,0); - convex->edges[2] = HalfEdge(5,2,0); - convex->edges[3] = HalfEdge(4,3,0); - - convex->edges[4] = HalfEdge(3,0,1); - convex->edges[5] = HalfEdge(2,3,1); - convex->edges[6] = HalfEdge(1,2,1); - convex->edges[7] = HalfEdge(0,1,1); - AssertIntact(*convex); - return convex; -} -ConvexH *test_cube() { - ConvexH *convex = new ConvexH(8,24,6); - convex->vertices[0] = REAL3(0,0,0); - convex->vertices[1] = REAL3(0,0,1); - convex->vertices[2] = REAL3(0,1,0); - convex->vertices[3] = REAL3(0,1,1); - convex->vertices[4] = REAL3(1,0,0); - convex->vertices[5] = REAL3(1,0,1); - convex->vertices[6] = REAL3(1,1,0); - convex->vertices[7] = REAL3(1,1,1); - - convex->facets[0] = Plane(REAL3(-1,0,0),0); - convex->facets[1] = Plane(REAL3(1,0,0),-1); - convex->facets[2] = Plane(REAL3(0,-1,0),0); - convex->facets[3] = Plane(REAL3(0,1,0),-1); - convex->facets[4] = Plane(REAL3(0,0,-1),0); - convex->facets[5] = Plane(REAL3(0,0,1),-1); - - convex->edges[0 ] = HalfEdge(11,0,0); - convex->edges[1 ] = HalfEdge(23,1,0); - convex->edges[2 ] = HalfEdge(15,3,0); - convex->edges[3 ] = HalfEdge(16,2,0); - - convex->edges[4 ] = HalfEdge(13,6,1); - convex->edges[5 ] = HalfEdge(21,7,1); - convex->edges[6 ] = HalfEdge( 9,5,1); - convex->edges[7 ] = HalfEdge(18,4,1); - - convex->edges[8 ] = HalfEdge(19,0,2); - convex->edges[9 ] = HalfEdge( 6,4,2); - convex->edges[10] = HalfEdge(20,5,2); - convex->edges[11] = HalfEdge( 0,1,2); - - convex->edges[12] = HalfEdge(22,3,3); - convex->edges[13] = HalfEdge( 4,7,3); - convex->edges[14] = HalfEdge(17,6,3); - convex->edges[15] = HalfEdge( 2,2,3); - - convex->edges[16] = HalfEdge( 3,0,4); - convex->edges[17] = HalfEdge(14,2,4); - convex->edges[18] = HalfEdge( 7,6,4); - convex->edges[19] = HalfEdge( 8,4,4); - - convex->edges[20] = HalfEdge(10,1,5); - convex->edges[21] = HalfEdge( 5,5,5); - convex->edges[22] = HalfEdge(12,7,5); - convex->edges[23] = HalfEdge( 1,3,5); - - - return convex; -} -ConvexH *ConvexHMakeCube(const REAL3 &bmin, const REAL3 &bmax) { - ConvexH *convex = test_cube(); - convex->vertices[0] = REAL3(bmin.getX(),bmin.getY(),bmin.getZ()); - convex->vertices[1] = REAL3(bmin.getX(),bmin.getY(),bmax.getZ()); - convex->vertices[2] = REAL3(bmin.getX(),bmax.getY(),bmin.getZ()); - convex->vertices[3] = REAL3(bmin.getX(),bmax.getY(),bmax.getZ()); - convex->vertices[4] = REAL3(bmax.getX(),bmin.getY(),bmin.getZ()); - convex->vertices[5] = REAL3(bmax.getX(),bmin.getY(),bmax.getZ()); - convex->vertices[6] = REAL3(bmax.getX(),bmax.getY(),bmin.getZ()); - convex->vertices[7] = REAL3(bmax.getX(),bmax.getY(),bmax.getZ()); - - convex->facets[0] = Plane(REAL3(-1,0,0), bmin.getX()); - convex->facets[1] = Plane(REAL3(1,0,0), -bmax.getX()); - convex->facets[2] = Plane(REAL3(0,-1,0), bmin.getY()); - convex->facets[3] = Plane(REAL3(0,1,0), -bmax.getY()); - convex->facets[4] = Plane(REAL3(0,0,-1), bmin.getZ()); - convex->facets[5] = Plane(REAL3(0,0,1), -bmax.getZ()); - return convex; -} -ConvexH *ConvexHCrop(ConvexH &convex,const Plane &slice) -{ - int i; - int vertcountunder=0; - int vertcountover =0; - btAlignedObjectArray vertscoplanar; // existing vertex members of convex that are coplanar - vertscoplanar.resize(0); - btAlignedObjectArray edgesplit; // existing edges that members of convex that cross the splitplane - edgesplit.resize(0); - - assert(convex.edges.size()<480); - - EdgeFlag edgeflag[512]; - VertFlag vertflag[256]; - PlaneFlag planeflag[128]; - HalfEdge tmpunderedges[512]; - Plane tmpunderplanes[128]; - Coplanar coplanaredges[512]; - int coplanaredges_num=0; - - btAlignedObjectArray createdverts; - // do the side-of-plane tests - for(i=0;i= convex.edges.size() || convex.edges[e1].p!=currentplane) { - enextface = e1; - e1=estart; - } - HalfEdge &edge0 = convex.edges[e0]; - HalfEdge &edge1 = convex.edges[e1]; - HalfEdge &edgea = convex.edges[edge0.ea]; - - - planeside |= vertflag[edge0.v].planetest; - //if((vertflag[edge0.v].planetest & vertflag[edge1.v].planetest) == COPLANAR) { - // assert(ecop==-1); - // ecop=e; - //} - - - if(vertflag[edge0.v].planetest == OVER && vertflag[edge1.v].planetest == OVER){ - // both endpoints over plane - edgeflag[e0].undermap = -1; - } - else if((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest) == UNDER) { - // at least one endpoint under, the other coplanar or under - - edgeflag[e0].undermap = under_edge_count; - tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap; - tmpunderedges[under_edge_count].p = underplanescount; - if(edge0.ea < e0) { - // connect the neighbors - assert(edgeflag[edge0.ea].undermap !=-1); - tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap; - tmpunderedges[edgeflag[edge0.ea].undermap].ea = under_edge_count; - } - under_edge_count++; - } - else if((vertflag[edge0.v].planetest | vertflag[edge1.v].planetest) == COPLANAR) { - // both endpoints coplanar - // must check a 3rd point to see if UNDER - int e2 = e1+1; - if(e2>=convex.edges.size() || convex.edges[e2].p!=currentplane) { - e2 = estart; - } - assert(convex.edges[e2].p==currentplane); - HalfEdge &edge2 = convex.edges[e2]; - if(vertflag[edge2.v].planetest==UNDER) { - - edgeflag[e0].undermap = under_edge_count; - tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap; - tmpunderedges[under_edge_count].p = underplanescount; - tmpunderedges[under_edge_count].ea = -1; - // make sure this edge is added to the "coplanar" list - coplanaredge = under_edge_count; - vout = vertflag[edge0.v].undermap; - vin = vertflag[edge1.v].undermap; - under_edge_count++; - } - else { - edgeflag[e0].undermap = -1; - } - } - else if(vertflag[edge0.v].planetest == UNDER && vertflag[edge1.v].planetest == OVER) { - // first is under 2nd is over - - edgeflag[e0].undermap = under_edge_count; - tmpunderedges[under_edge_count].v = vertflag[edge0.v].undermap; - tmpunderedges[under_edge_count].p = underplanescount; - if(edge0.ea < e0) { - assert(edgeflag[edge0.ea].undermap !=-1); - // connect the neighbors - tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap; - tmpunderedges[edgeflag[edge0.ea].undermap].ea = under_edge_count; - vout = tmpunderedges[edgeflag[edge0.ea].undermap].v; - } - else { - Plane &p0 = convex.facets[edge0.p]; - Plane &pa = convex.facets[edgea.p]; - createdverts.push_back(ThreePlaneIntersection(p0,pa,slice)); - //createdverts.push_back(PlaneProject(slice,PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v]))); - //createdverts.push_back(PlaneLineIntersection(slice,convex.vertices[edge0.v],convex.vertices[edgea.v])); - vout = vertcountunder++; - } - under_edge_count++; - /// hmmm something to think about: i might be able to output this edge regarless of - // wheter or not we know v-in yet. ok i;ll try this now: - tmpunderedges[under_edge_count].v = vout; - tmpunderedges[under_edge_count].p = underplanescount; - tmpunderedges[under_edge_count].ea = -1; - coplanaredge = under_edge_count; - under_edge_count++; - - if(vin!=-1) { - // we previously processed an edge where we came under - // now we know about vout as well - - // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!! - } - - } - else if(vertflag[edge0.v].planetest == COPLANAR && vertflag[edge1.v].planetest == OVER) { - // first is coplanar 2nd is over - - edgeflag[e0].undermap = -1; - vout = vertflag[edge0.v].undermap; - // I hate this but i have to make sure part of this face is UNDER before ouputting this vert - int k=estart; - assert(edge0.p == currentplane); - while(!(planeside&UNDER) && k= vertcountunderold); // for debugging only - } - if(vout!=-1) { - // we previously processed an edge where we went over - // now we know vin too - // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!! - } - // output edge - tmpunderedges[under_edge_count].v = vin; - tmpunderedges[under_edge_count].p = underplanescount; - edgeflag[e0].undermap = under_edge_count; - if(e0>edge0.ea) { - assert(edgeflag[edge0.ea].undermap !=-1); - // connect the neighbors - tmpunderedges[under_edge_count].ea = edgeflag[edge0.ea].undermap; - tmpunderedges[edgeflag[edge0.ea].undermap].ea = under_edge_count; - } - assert(edgeflag[e0].undermap == under_edge_count); - under_edge_count++; - } - else if(vertflag[edge0.v].planetest == OVER && vertflag[edge1.v].planetest == COPLANAR) { - // first is over next is coplanar - - edgeflag[e0].undermap = -1; - vin = vertflag[edge1.v].undermap; - assert(vin!=-1); - if(vout!=-1) { - // we previously processed an edge where we came under - // now we know both endpoints - // ADD THIS EDGE TO THE LIST OF EDGES THAT NEED NEIGHBOR ON PARTITION PLANE!! - } - - } - else { - assert(0); - } - - - e0=e1; - e1++; // do the modulo at the beginning of the loop - - } while(e0!=estart) ; - e0 = enextface; - if(planeside&UNDER) { - planeflag[currentplane].undermap = underplanescount; - tmpunderplanes[underplanescount] = convex.facets[currentplane]; - underplanescount++; - } - else { - planeflag[currentplane].undermap = 0; - } - if(vout>=0 && (planeside&UNDER)) { - assert(vin>=0); - assert(coplanaredge>=0); - assert(coplanaredge!=511); - coplanaredges[coplanaredges_num].ea = coplanaredge; - coplanaredges[coplanaredges_num].v0 = vin; - coplanaredges[coplanaredges_num].v1 = vout; - coplanaredges_num++; - } - } - - // add the new plane to the mix: - if(coplanaredges_num>0) { - tmpunderplanes[underplanescount++]=slice; - } - for(i=0;i=coplanaredges_num) - { - assert(jvertices.size();j++) - { - d = btMax(d,dot(convex->vertices[j],planes[i].normal)+planes[i].dist); - } - if(i==0 || d>md) - { - p=i; - md=d; - } - } - return (md>epsilon)?p:-1; -} -template -inline int maxdir(const T *p,int count,const T &dir) -{ - assert(count); - int m=0; - for(int i=1;idot(p[m],dir)) m=i; - } - return m; -} + template int maxdirfiltered(const T *p,int count,const T &dir,btAlignedObjectArray &allow) { - assert(count); + btAssert(count); int m=-1; for(int i=0;i & if(m==-1 || dot(p[i],dir)>dot(p[m],dir)) m=i; } - assert(m!=-1); + btAssert(m!=-1); return m; } @@ -833,7 +288,7 @@ int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray &al allow[m]=0; m=-1; } - assert(0); + btAssert(0); return m; } @@ -849,22 +304,8 @@ int operator ==(const int3 &a,const int3 &b) return 1; } -int3 roll3(int3 a) -{ - int tmp=a[0]; - a[0]=a[1]; - a[1]=a[2]; - a[2]=tmp; - return a; -} -int isa(const int3 &a,const int3 &b) -{ - return ( a==b || roll3(a)==b || a==roll3(b) ); -} -int b2b(const int3 &a,const int3 &b) -{ - return isa(a,int3(b[2],b[1],b[0])); -} + + int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon) { btVector3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]); @@ -896,7 +337,7 @@ int shareedge(const int3 &a,const int3 &b) class Tri; -btAlignedObjectArray tris; + class Tri : public int3 { @@ -907,15 +348,11 @@ public: btScalar rise; Tri(int a,int b,int c):int3(a,b,c),n(-1,-1,-1) { - id = tris.size(); - tris.push_back(this); vmax=-1; rise = btScalar(0.0); } ~Tri() { - assert(tris[id]==this); - tris[id]=NULL; } int &neib(int a,int b); }; @@ -932,10 +369,10 @@ int &Tri::neib(int a,int b) if((*this)[i]==a && (*this)[i1]==b) return n[i2]; if((*this)[i]==b && (*this)[i1]==a) return n[i2]; } - assert(0); + btAssert(0); return er; } -void b2bfix(Tri* s,Tri*t) +void HullLibrary::b2bfix(Tri* s,Tri*t) { int i; for(i=0;i<3;i++) @@ -944,84 +381,96 @@ void b2bfix(Tri* s,Tri*t) int i2=(i+2)%3; int a = (*s)[i1]; int b = (*s)[i2]; - assert(tris[s->neib(a,b)]->neib(b,a) == s->id); - assert(tris[t->neib(a,b)]->neib(b,a) == t->id); - tris[s->neib(a,b)]->neib(b,a) = t->neib(b,a); - tris[t->neib(b,a)]->neib(a,b) = s->neib(a,b); + 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 removeb2b(Tri* s,Tri*t) +void HullLibrary::removeb2b(Tri* s,Tri*t) { b2bfix(s,t); - delete s; - delete t; + deAllocateTriangle(s); + + deAllocateTriangle(t); } -void checkit(Tri *t) +void HullLibrary::checkit(Tri *t) { int i; - assert(tris[t->id]==t); + 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]; - assert(a!=b); - assert( tris[t->n[i]]->neib(b,a) == t->id); + btAssert(a!=b); + btAssert( m_tris[t->n[i]]->neib(b,a) == t->id); } } -void extrude(Tri *t0,int v) + +Tri* HullLibrary::allocateTriangle(int a,int b,int c) +{ + void* mem = btAlignedAlloc(sizeof(Tri),16); + Tri* tr = new (mem)Tri(a,b,c); + tr->id = m_tris.size(); + m_tris.push_back(tr); + + return tr; +} + +void HullLibrary::deAllocateTriangle(Tri* tri) +{ + btAssert(m_tris[tri->id]==tri); + m_tris[tri->id]=NULL; + tri->~Tri(); + btAlignedFree(tri); +} + + +void HullLibrary::extrude(Tri *t0,int v) { int3 t= *t0; - int n = tris.size(); - Tri* ta = new Tri(v,t[1],t[2]); + int n = m_tris.size(); + Tri* ta = allocateTriangle(v,t[1],t[2]); ta->n = int3(t0->n[0],n+1,n+2); - tris[t0->n[0]]->neib(t[1],t[2]) = n+0; - Tri* tb = new Tri(v,t[2],t[0]); + m_tris[t0->n[0]]->neib(t[1],t[2]) = n+0; + Tri* tb = allocateTriangle(v,t[2],t[0]); tb->n = int3(t0->n[1],n+2,n+0); - tris[t0->n[1]]->neib(t[2],t[0]) = n+1; - Tri* tc = new Tri(v,t[0],t[1]); + m_tris[t0->n[1]]->neib(t[2],t[0]) = n+1; + Tri* tc = allocateTriangle(v,t[0],t[1]); tc->n = int3(t0->n[2],n+0,n+1); - tris[t0->n[2]]->neib(t[0],t[1]) = n+2; + m_tris[t0->n[2]]->neib(t[0],t[1]) = n+2; checkit(ta); checkit(tb); checkit(tc); - if(hasvert(*tris[ta->n[0]],v)) removeb2b(ta,tris[ta->n[0]]); - if(hasvert(*tris[tb->n[0]],v)) removeb2b(tb,tris[tb->n[0]]); - if(hasvert(*tris[tc->n[0]],v)) removeb2b(tc,tris[tc->n[0]]); - delete t0; + 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); } -Tri *extrudable(btScalar epsilon) +Tri* HullLibrary::extrudable(btScalar epsilon) { int i; Tri *t=NULL; - for(i=0;iriserise)) + if(!t || (m_tris[i] && t->riserise)) { - t = tris[i]; + t = m_tris[i]; } } return (t->rise >epsilon)?t:NULL ; } -class int4 -{ -public: - int x,y,z,w; - int4(){}; - int4(int _x,int _y, int _z,int _w){x=_x;y=_y;z=_z;w=_w;} - const int& operator[](int i) const {return (&x)[i];} - int& operator[](int i) {return (&x)[i];} -}; -int4 FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray &allow) +int4 HullLibrary::FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray &allow) { btVector3 basis[3]; basis[0] = btVector3( btScalar(0.01), btScalar(0.02), btScalar(1.0) ); @@ -1052,12 +501,12 @@ int4 FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray &all 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); - assert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3)); + btAssert(!(p0==p1||p0==p2||p0==p3||p1==p2||p1==p3||p2==p3)); if(dot(verts[p3]-verts[p0],cross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {Swap(p2,p3);} return int4(p0,p1,p2,p3); } -int calchullgen(btVector3 *verts,int verts_count, int vlimit) +int HullLibrary::calchullgen(btVector3 *verts,int verts_count, int vlimit) { if(verts_count <4) return 0; if(vlimit==0) vlimit=1000000000; @@ -1085,18 +534,18 @@ int calchullgen(btVector3 *verts,int verts_count, int vlimit) btVector3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) / btScalar(4.0); // a valid interior point - Tri *t0 = new Tri(p[2],p[3],p[1]); t0->n=int3(2,3,1); - Tri *t1 = new Tri(p[3],p[2],p[0]); t1->n=int3(3,2,0); - Tri *t2 = new Tri(p[0],p[1],p[3]); t2->n=int3(0,1,3); - Tri *t3 = new Tri(p[1],p[0],p[2]); t3->n=int3(1,0,2); + Tri *t0 = allocateTriangle(p[2],p[3],p[1]); t0->n=int3(2,3,1); + Tri *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0); + Tri *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3); + Tri *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;jvmax<0); + Tri *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 = dot(n,verts[t->vmax]-verts[(*t)[0]]); @@ -1107,38 +556,38 @@ int calchullgen(btVector3 *verts,int verts_count, int vlimit) { int3 ti=*te; int v=te->vmax; - assert(v != -1); - assert(!isextreme[v]); // wtf we've already done this vertex + 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=tris.size(); + j=m_tris.size(); while(j--) { - if(!tris[j]) continue; - int3 t=*tris[j]; + if(!m_tris[j]) continue; + int3 t=*m_tris[j]; if(above(verts,t,verts[v],btScalar(0.01)*epsilon)) { - extrude(tris[j],v); + extrude(m_tris[j],v); } } // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle - j=tris.size(); + j=m_tris.size(); while(j--) { - if(!tris[j]) continue; - if(!hasvert(*tris[j],v)) break; - int3 nt=*tris[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) || cross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) ) { - Tri *nb = tris[tris[j]->n[0]]; - assert(nb);assert(!hasvert(*nb,v));assert(nb->idn[0]]; + btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->idvmax>=0) break; btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); @@ -1157,123 +606,48 @@ int calchullgen(btVector3 *verts,int verts_count, int vlimit) return 1; } -int calchull(btVector3 *verts,int verts_count, int *&tris_out, int &tris_count,int vlimit) +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 ts; int i; - for(i=0;i=0) - { - ConvexH *tmp = c; - c = ConvexHCrop(*tmp,planes[k]); - if(c==NULL) {c=tmp; break;} // might want to debug this case better!!! - if(!AssertIntact(*c)) {c=tmp; break;} // might want to debug this case better too!!! - delete tmp; - } - - assert(AssertIntact(*c)); - //return c; - faces_out = (int*)new int[(1+c->facets.size()+c->edges.size())]; // new int[1+c->facets.size()+c->edges.size()]; - faces_count_out=0; - i=0; - faces_out[faces_count_out++]=-1; - k=0; - while(iedges.size()) - { - j=1; - while(j+iedges.size() && c->edges[i].p==c->edges[i+j].p) { j++; } - faces_out[faces_count_out++]=j; - while(j--) - { - faces_out[faces_count_out++] = c->edges[i].v; - i++; - } - k++; - } - faces_out[0]=k; // number of faces. - assert(k==c->facets.size()); - assert(faces_count_out == 1+c->facets.size()+c->edges.size()); - verts_out = new btVector3[c->vertices.size()]; - verts_count_out = c->vertices.size(); - for(i=0;ivertices.size();i++) - { - verts_out[i] = btVector3(c->vertices[i]); - } - c->vertices.resize(0); - delete c; - return 1; -} - - - -bool ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit) -{ - - - - int *tris_out; int tris_count; - int ret = calchull( (btVector3 *) vertices, (int) vcount, tris_out, tris_count, vlimit ); + int ret = calchull( (btVector3 *) vertices, (int) vcount, result.m_Indices, tris_count, 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; - result.mIndices = (unsigned int *) tris_out; return true; } @@ -1281,16 +655,14 @@ bool ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &resu void ReleaseHull(PHullResult &result) { - if ( result.mIndices ) + if ( result.m_Indices.size() ) { - delete[]result.mIndices; + result.m_Indices.clear(); } result.mVcount = 0; result.mIndexCount = 0; - result.mIndices = 0; result.mVertices = 0; - result.mIndices = 0; } @@ -1317,13 +689,14 @@ HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // unsigned int vcount = desc.mVcount; if ( vcount < 8 ) vcount = 8; - btVector3* vsource = new btVector3[vcount]; + btAlignedObjectArray vertexSource; + vertexSource.resize(vcount); btVector3 scale; unsigned int ovcount; - bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, vsource, desc.mNormalEpsilon, scale ); // normalize point cloud, remove duplicates! + bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, &vertexSource[0], desc.mNormalEpsilon, scale ); // normalize point cloud, remove duplicates! if ( ok ) { @@ -1333,21 +706,23 @@ HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // { for (unsigned int i=0; i vertexScratch; + vertexScratch.resize(hr.mVcount); + + BringOutYourDead(hr.mVertices,hr.mVcount, &vertexScratch[0], ovcount, &hr.m_Indices[0], hr.mIndexCount ); ret = QE_OK; @@ -1355,19 +730,19 @@ HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // { result.mPolygons = false; result.mNumOutputVertices = ovcount; - result.mOutputVertices = new btVector3[ovcount];; + result.m_OutputVertices.resize(ovcount); result.mNumFaces = hr.mFaceCount; result.mNumIndices = hr.mIndexCount; - result.mIndices = new unsigned int[hr.mIndexCount]; + result.m_Indices.resize(hr.mIndexCount); - memcpy(result.mOutputVertices, vscratch, sizeof(btVector3)*ovcount ); + memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3)*ovcount ); if ( desc.HasHullFlag(QF_REVERSE_ORDER) ) { - const unsigned int *source = hr.mIndices; - unsigned int *dest = result.mIndices; + const unsigned int *source = &hr.m_Indices[0]; + unsigned int *dest = &result.m_Indices[0]; for (unsigned int i=0; i= 0 && v < vcount ); + btAssert( v >= 0 && v < vcount ); - if ( used[v] ) // if already remapped + if ( usedIndices[v] ) // if already remapped { - indices[i] = used[v]-1; // index to new array + indices[i] = usedIndices[v]-1; // index to new array } else { @@ -1760,11 +1126,11 @@ void HullLibrary::BringOutYourDead(const btVector3* verts,unsigned int vcount, b ocount++; // increment output vert count - assert( ocount >=0 && ocount <= vcount ); + btAssert( ocount >=0 && ocount <= vcount ); - used[v] = ocount; // assign new index remapping + usedIndices[v] = ocount; // assign new index remapping } } - delete[] used; + } diff --git a/Extras/ConvexHull/btConvexHull.h b/Extras/ConvexHull/btConvexHull.h index 6ea635efb..1e3e1a74d 100644 --- a/Extras/ConvexHull/btConvexHull.h +++ b/Extras/ConvexHull/btConvexHull.h @@ -20,6 +20,9 @@ subject to the following restrictions: #define CD_HULL_H #include "LinearMath/btVector3.h" +#include "LinearMath/btAlignedObjectArray.h" + +typedef btAlignedObjectArray TUIntArray; class HullResult { @@ -28,17 +31,15 @@ public: { mPolygons = true; mNumOutputVertices = 0; - mOutputVertices = 0; mNumFaces = 0; mNumIndices = 0; - mIndices = 0; } bool mPolygons; // true if indices represents polygons, false indices are triangles unsigned int mNumOutputVertices; // number of vertices in the output hull - btVector3 *mOutputVertices; // array of vertices + btAlignedObjectArray m_OutputVertices; // array of vertices unsigned int mNumFaces; // the number of faces produced unsigned int mNumIndices; // the total number of indices - unsigned int *mIndices; // pointer to indices. + btAlignedObjectArray m_Indices; // pointer to indices. // If triangles, then indices are array indexes into the vertex list. // If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc.. @@ -110,14 +111,115 @@ enum HullError QE_FAIL // failed. }; +class btPlane +{ + public: + btVector3 normal; + btScalar dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0 + btPlane(const btVector3 &n,btScalar d):normal(n),dist(d){} + btPlane():normal(),dist(0){} + +}; + + + +class ConvexH +{ + public: + class HalfEdge + { + public: + short ea; // the other half of the edge (index into edges list) + unsigned char v; // the vertex at the start of this edge (index into vertices list) + unsigned char p; // the facet on which this edge lies (index into facets list) + HalfEdge(){} + HalfEdge(short _ea,unsigned char _v, unsigned char _p):ea(_ea),v(_v),p(_p){} + }; + ConvexH() + { + int i; + i=0; + } + ~ConvexH() + { + int i; + i=0; + } + btAlignedObjectArray vertices; + btAlignedObjectArray edges; + btAlignedObjectArray facets; + ConvexH(int vertices_size,int edges_size,int facets_size); +}; + + +class int4 +{ +public: + int x,y,z,w; + int4(){}; + int4(int _x,int _y, int _z,int _w){x=_x;y=_y;z=_z;w=_w;} + const int& operator[](int i) const {return (&x)[i];} + int& operator[](int i) {return (&x)[i];} +}; + +class PHullResult +{ +public: + + PHullResult(void) + { + mVcount = 0; + mIndexCount = 0; + mFaceCount = 0; + mVertices = 0; + } + + unsigned int mVcount; + unsigned int mIndexCount; + unsigned int mFaceCount; + btVector3* mVertices; + TUIntArray m_Indices; +}; + + + class HullLibrary { + + btAlignedObjectArray m_tris; + public: HullError CreateConvexHull(const HullDesc& desc, // describes the input request HullResult& result); // contains the resulst HullError ReleaseResult(HullResult &result); // release memory allocated for this result, we are done with it. + private: + + bool ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit); + + class Tri* allocateTriangle(int a,int b,int c); + void deAllocateTriangle(Tri*); + void b2bfix(Tri* s,Tri*t); + + void removeb2b(Tri* s,Tri*t); + + void checkit(Tri *t); + + Tri* extrudable(btScalar epsilon); + + int calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit); + + int calchullgen(btVector3 *verts,int verts_count, int vlimit); + + int4 FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray &allow); + + class ConvexH* ConvexHCrop(ConvexH& convex,const btPlane& slice); + + void extrude(class Tri* t0,int v); + + ConvexH* test_cube(); + //BringOutYourDead (John Ratcliff): When you create a convex hull you hand it a large input set of vertices forming a 'point cloud'. //After the hull is generated it give you back a set of polygon faces which index the *original* point cloud. //The thing is, often times, there are many 'dead vertices' in the point cloud that are on longer referenced by the hull. diff --git a/Extras/ConvexHull/btShapeHull.cpp b/Extras/ConvexHull/btShapeHull.cpp index b431050b2..4e33d242b 100644 --- a/Extras/ConvexHull/btShapeHull.cpp +++ b/Extras/ConvexHull/btShapeHull.cpp @@ -70,14 +70,13 @@ btShapeHull::btShapeHull (const btConvexShape* shape) { m_shape = shape; m_vertices.clear (); - m_indices = NULL; + m_indices.clear(); m_numIndices = 0; } btShapeHull::~btShapeHull () { - if (m_indices) - delete [] m_indices; + m_indices.clear(); m_vertices.clear (); } @@ -130,13 +129,13 @@ btShapeHull::buildHull (btScalar margin) for (i = 0; i < hr.mNumOutputVertices; i++) { - m_vertices[i] = hr.mOutputVertices[i]; + m_vertices[i] = hr.m_OutputVertices[i]; } m_numIndices = hr.mNumIndices; - m_indices = new unsigned int [m_numIndices]; + m_indices.resize(m_numIndices); for (i = 0; i < m_numIndices; i++) { - m_indices[i] = hr.mIndices[i]; + m_indices[i] = hr.m_Indices[i]; } // free temporary hull result that we just copied diff --git a/Extras/ConvexHull/btShapeHull.h b/Extras/ConvexHull/btShapeHull.h index 4084a332a..870bbf81f 100644 --- a/Extras/ConvexHull/btShapeHull.h +++ b/Extras/ConvexHull/btShapeHull.h @@ -41,12 +41,12 @@ public: } const unsigned int* getIndexPointer() const { - return m_indices; + return &m_indices[0]; } protected: btAlignedObjectArray m_vertices; - unsigned int* m_indices; + btAlignedObjectArray m_indices; unsigned int m_numIndices; const btConvexShape* m_shape; };