diff options
Diffstat (limited to 'extern/bullet2/src/LinearMath/btConvexHull.cpp')
| -rw-r--r-- | extern/bullet2/src/LinearMath/btConvexHull.cpp | 953 |
1 files changed, 453 insertions, 500 deletions
diff --git a/extern/bullet2/src/LinearMath/btConvexHull.cpp b/extern/bullet2/src/LinearMath/btConvexHull.cpp index 2ae855dbc1f..e7de2a3694c 100644 --- a/extern/bullet2/src/LinearMath/btConvexHull.cpp +++ b/extern/bullet2/src/LinearMath/btConvexHull.cpp @@ -20,51 +20,52 @@ subject to the following restrictions: #include "btMinMax.h" #include "btVector3.h" - - - - //---------------------------------- -class int3 +class int3 { public: - int x,y,z; + 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];} + 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)); } - +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 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 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); + 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; @@ -74,105 +75,96 @@ btVector3 ThreePlaneIntersection(const btPlane &p0,const btPlane &p1, const btP potentialVertex += n1n2; potentialVertex *= quotient; - btVector3 result(potentialVertex.getX(),potentialVertex.getY(),potentialVertex.getZ()); + 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); - +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 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); + 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); + 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(); + 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) - { + 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); + plane.normal = btCross(vdir, cp).normalized(); + plane.dist = -btDot(plane.normal, vstart); + *upoint = PlaneLineIntersection(plane, ustart, ustart + udir); } - if(vpoint) - { + if (vpoint) + { btPlane plane; - plane.normal = btCross(udir,cp).normalized(); - plane.dist = -btDot(plane.normal,ustart); - *vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir); + 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 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) +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); +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]); +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; } @@ -185,7 +177,7 @@ public: unsigned char undermap; unsigned char overmap; }; -class EdgeFlag +class EdgeFlag { public: unsigned char planetest; @@ -199,146 +191,135 @@ public: unsigned char undermap; unsigned char overmap; }; -class Coplanar{ +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) +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]) + 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; + if (m == -1 || btDot(p[i], dir) > btDot(p[m], dir)) + m = i; } - btAssert(m!=-1); + 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)); + 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 { + } + else + { return b.normalized(); } } - -template<class T> -int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow) +template <class T> +int maxdirsterid(const T *p, int count, const T &dir, btAlignedObjectArray<int> &allow) { - int m=-1; - while(m==-1) + int m = -1; + while (m == -1) { - m = maxdirfiltered(p,count,dir,allow); - if(allow[m]==3) return m; + 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)) + 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) + 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; + allow[m] = 3; return m; } - if(ma!=-1 && ma!=mb) // Yuck - this is really ugly + 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)) + 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) + 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; + allow[m] = 3; return m; } - mc=md; + mc = md; } } - ma=mb; + ma = mb; } - allow[m]=0; - m=-1; + 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) +int operator==(const int3 &a, const int3 &b); +int operator==(const int3 &a, const int3 &b) { - for(int i=0;i<3;i++) + for (int i = 0; i < 3; i++) { - if(a[i]!=b[i]) return 0; + 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) +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??? + 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) +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++) + for (int i = 0; i < 3; i++) { - int i1= (i+1)%3; - if(t[i]==a && t[i1]==b) return 1; + 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) ; + 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 shareedge(const int3 &a, const int3 &b); +int shareedge(const int3 &a, const int3 &b) { int i; - for(i=0;i<3;i++) + for (i = 0; i < 3; i++) { - int i1= (i+1)%3; - if(hasedge(a,b[i1],b[i])) return 1; + int i1 = (i + 1) % 3; + if (hasedge(a, b[i1], b[i])) return 1; } return 0; } class btHullTriangle; - - class btHullTriangle : public int3 { public: @@ -346,51 +327,50 @@ public: int id; int vmax; btScalar rise; - btHullTriangle(int a,int b,int c):int3(a,b,c),n(-1,-1,-1) + btHullTriangle(int a, int b, int c) : int3(a, b, c), n(-1, -1, -1) { - vmax=-1; + vmax = -1; rise = btScalar(0.0); } ~btHullTriangle() { } - int &neib(int a,int b); + int &neib(int a, int b); }; - -int &btHullTriangle::neib(int a,int b) +int &btHullTriangle::neib(int a, int b) { - static int er=-1; + static int er = -1; int i; - for(i=0;i<3;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]; + 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) +void HullLibrary::b2bfix(btHullTriangle *s, btHullTriangle *t) { int i; - for(i=0;i<3;i++) + for (i = 0; i < 3; i++) { - int i1=(i+1)%3; - int i2=(i+2)%3; + 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); + 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) +void HullLibrary::removeb2b(btHullTriangle *s, btHullTriangle *t) { - b2bfix(s,t); + b2bfix(s, t); deAllocateTriangle(s); deAllocateTriangle(t); @@ -401,11 +381,11 @@ void HullLibrary::checkit(btHullTriangle *t) (void)t; int i; - btAssert(m_tris[t->id]==t); - for(i=0;i<3;i++) + btAssert(m_tris[t->id] == t); + for (i = 0; i < 3; i++) { - int i1=(i+1)%3; - int i2=(i+2)%3; + int i1 = (i + 1) % 3; + int i2 = (i + 2) % 3; int a = (*t)[i1]; int b = (*t)[i2]; @@ -415,226 +395,233 @@ void HullLibrary::checkit(btHullTriangle *t) (void)a; (void)b; - btAssert(a!=b); - btAssert( m_tris[t->n[i]]->neib(b,a) == t->id); + btAssert(a != b); + btAssert(m_tris[t->n[i]]->neib(b, a) == t->id); } } -btHullTriangle* HullLibrary::allocateTriangle(int a,int b,int c) +btHullTriangle *HullLibrary::allocateTriangle(int a, int b, int c) { - void* mem = btAlignedAlloc(sizeof(btHullTriangle),16); - btHullTriangle* tr = new (mem)btHullTriangle(a,b,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) +void HullLibrary::deAllocateTriangle(btHullTriangle *tri) { - btAssert(m_tris[tri->id]==tri); - m_tris[tri->id]=NULL; + btAssert(m_tris[tri->id] == tri); + m_tris[tri->id] = NULL; tri->~btHullTriangle(); btAlignedFree(tri); } - -void HullLibrary::extrude(btHullTriangle *t0,int v) +void HullLibrary::extrude(btHullTriangle *t0, int v) { - int3 t= *t0; + 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; + 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]]); + 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) +btHullTriangle *HullLibrary::extrudable(btScalar epsilon) { int i; - btHullTriangle *t=NULL; - for(i=0;i<m_tris.size();i++) + btHullTriangle *t = NULL; + for (i = 0; i < m_tris.size(); i++) { - if(!t || (m_tris[i] && t->rise<m_tris[i]->rise)) + if (!t || (m_tris[i] && t->rise < m_tris[i]->rise)) { t = m_tris[i]; } } - return (t->rise >epsilon)?t:NULL ; + return (t->rise > epsilon) ? t : NULL; } - - - -int4 HullLibrary::FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray<int> &allow) +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]); + 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 { + } + else + { basis[1] = basis[2]; - basis[1].normalize (); + basis[1].normalize(); } - int p2 = maxdirsterid(verts,verts_count,basis[1],allow); - if(p2 == p0 || p2 == p1) + int p2 = maxdirsterid(verts, verts_count, basis[1], allow); + if (p2 == p0 || p2 == p1) { - p2 = maxdirsterid(verts,verts_count,-basis[1],allow); + p2 = maxdirsterid(verts, verts_count, -basis[1], allow); } - if(p2 == p0 || p2 == p1) - return int4(-1,-1,-1,-1); + 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) {btSwap(p2,p3);} - return int4(p0,p1,p2,p3); + 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) + { + btSwap(p2, p3); + } + return int4(p0, p1, p2, p3); } -int HullLibrary::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; + if (verts_count < 4) return 0; + if (vlimit == 0) vlimit = 1000000000; int j; - btVector3 bmin(*verts),bmax(*verts); + 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++) + for (j = 0; j < verts_count; j++) { allow.push_back(1); isextreme.push_back(0); - bmin.setMin (verts[j]); - bmax.setMax (verts[j]); + 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++) + 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]; + 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]]); + 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)) + vlimit -= 4; + while (vlimit > 0 && ((te = extrudable(epsilon)) != 0)) { //int3 ti=*te; - int v=te->vmax; + int v = te->vmax; btAssert(v != -1); btAssert(!isextreme[v]); // wtf we've already done this vertex - isextreme[v]=1; + 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)) + 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); + 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--) + 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) ) + 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(); + btAssert(nb); + btAssert(!hasvert(*nb, v)); + btAssert(nb->id < j); + extrude(nb, v); + j = m_tris.size(); } - } - j=m_tris.size(); - while(j--) + } + 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]) + 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. + 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]]); + t->rise = btDot(n, verts[t->vmax] - verts[(*t)[0]]); } } - vlimit --; + vlimit--; } return 1; } -int HullLibrary::calchull(btVector3 *verts,int verts_count, TUIntArray& 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; + int rc = calchullgen(verts, verts_count, vlimit); + if (!rc) return 0; btAlignedObjectArray<int> ts; int i; - for(i=0;i<m_tris.size();i++) + for (i = 0; i < m_tris.size(); i++) { - if(m_tris[i]) + if (m_tris[i]) { - for(int j=0;j<3;j++) + for (int j = 0; j < 3; j++) ts.push_back((*m_tris[i])[j]); deAllocateTriangle(m_tris[i]); } } - tris_count = ts.size()/3; + tris_count = ts.size() / 3; tris_out.resize(ts.size()); - - for (i=0;i<ts.size();i++) + + for (i = 0; i < ts.size(); i++) { tris_out[i] = static_cast<unsigned int>(ts[i]); } @@ -643,29 +630,22 @@ int HullLibrary::calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out return 1; } - - - - -bool HullLibrary::ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit) +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; + 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() ) + if (result.m_Indices.size()) { result.m_Indices.clear(); } @@ -675,7 +655,6 @@ void ReleaseHull(PHullResult &result) result.mVertices = 0; } - //********************************************************************* //********************************************************************* //******** HullLib header @@ -688,16 +667,15 @@ void ReleaseHull(PHullResult &result) //********************************************************************* //********************************************************************* -HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // describes the input request - HullResult &result) // contains the resulst +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; + if (vcount < 8) vcount = 8; btAlignedObjectArray<btVector3> vertexSource; vertexSource.resize(static_cast<int>(vcount)); @@ -706,87 +684,82 @@ HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // unsigned int ovcount; - bool ok = CleanupVertices(desc.mVcount,desc.mVertices, desc.mVertexStride, ovcount, &vertexSource[0], 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 ) + if (ok) { - - -// if ( 1 ) // scale vertices back to their original size. + // if ( 1 ) // scale vertices back to their original size. { - for (unsigned int i=0; i<ovcount; i++) + 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]; + 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); + ok = ComputeHull(ovcount, &vertexSource[0], hr, desc.mMaxVertices); - if ( ok ) + if (ok) { - // re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table. - btAlignedObjectArray<btVector3> vertexScratch; + btAlignedObjectArray<btVector3> vertexScratch; vertexScratch.resize(static_cast<int>(hr.mVcount)); - BringOutYourDead(hr.mVertices,hr.mVcount, &vertexScratch[0], ovcount, &hr.m_Indices[0], hr.mIndexCount ); + 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! + if (desc.HasHullFlag(QF_TRIANGLES)) // if he wants the results as triangle! { - result.mPolygons = false; + result.mPolygons = false; result.mNumOutputVertices = ovcount; result.m_OutputVertices.resize(static_cast<int>(ovcount)); - result.mNumFaces = hr.mFaceCount; - result.mNumIndices = hr.mIndexCount; + 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 ); + memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3) * ovcount); - if ( desc.HasHullFlag(QF_REVERSE_ORDER) ) + if (desc.HasHullFlag(QF_REVERSE_ORDER)) { - const unsigned int *source = &hr.m_Indices[0]; - unsigned int *dest = &result.m_Indices[0]; + unsigned int *dest = &result.m_Indices[0]; - for (unsigned int i=0; i<hr.mFaceCount; i++) + 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; + dest += 3; + source += 3; } - } else { - memcpy(&result.m_Indices[0], &hr.m_Indices[0], sizeof(unsigned int)*hr.mIndexCount); + memcpy(&result.m_Indices[0], &hr.m_Indices[0], sizeof(unsigned int) * hr.mIndexCount); } } else { - result.mPolygons = true; + 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.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 ); + memcpy(&result.m_OutputVertices[0], &vertexScratch[0], sizeof(btVector3) * ovcount); -// if ( 1 ) + // 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++) + 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) ) + if (desc.HasHullFlag(QF_REVERSE_ORDER)) { dest[1] = source[2]; dest[2] = source[1]; @@ -799,8 +772,8 @@ HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // dest[3] = source[2]; } - dest+=4; - source+=3; + dest += 4; + source += 3; } } } @@ -811,90 +784,83 @@ HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // return ret; } - - -HullError HullLibrary::ReleaseResult(HullResult &result) // release memory allocated for this result, we are done with it. +HullError HullLibrary::ReleaseResult(HullResult &result) // release memory allocated for this result, we are done with it. { - if ( result.m_OutputVertices.size()) + if (result.m_OutputVertices.size()) { - result.mNumOutputVertices=0; + result.mNumOutputVertices = 0; result.m_OutputVertices.clear(); } - if ( result.m_Indices.size() ) + if (result.m_Indices.size()) { - result.mNumIndices=0; + result.mNumIndices = 0; result.m_Indices.clear(); } return QE_OK; } - -static void addPoint(unsigned int &vcount,btVector3 *p,btScalar x,btScalar y,btScalar z) +static void addPoint(unsigned int &vcount, btVector3 *p, btScalar x, btScalar y, btScalar z) { // XXX, might be broken - btVector3& dest = p[vcount]; + 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 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; + 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) +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; + 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}; + btScalar recip[3] = {0.f, 0.f, 0.f}; - if ( scale ) + 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 }; + btScalar bmin[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; + btScalar bmax[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; - const char *vtx = (const char *) svertices; + const char *vtx = (const char *)svertices; -// if ( 1 ) + // if ( 1 ) { - for (unsigned int i=0; i<svcount; i++) + for (unsigned int i = 0; i < svcount; i++) { - const btScalar *p = (const btScalar *) vtx; + const btScalar *p = (const btScalar *)vtx; - vtx+=stride; + vtx += stride; - for (int j=0; j<3; j++) + 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]; + if (p[j] < bmin[j]) bmin[j] = p[j]; + if (p[j] > bmax[j]) bmax[j] = p[j]; } } } @@ -905,28 +871,27 @@ bool HullLibrary::CleanupVertices(unsigned int svcount, 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]; + 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 ) + 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 (dx > EPSILON && dx < len) len = dx; + if (dy > EPSILON && dy < len) len = dy; + if (dz > EPSILON && dz < len) len = dz; - if ( len == FLT_MAX ) + if (len == FLT_MAX) { - dx = dy = dz = btScalar(0.01); // one centimeter + 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); + 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; @@ -938,22 +903,20 @@ bool HullLibrary::CleanupVertices(unsigned int svcount, 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 - + 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 ) + if (scale) { scale[0] = dx; scale[1] = dy; @@ -963,75 +926,70 @@ bool HullLibrary::CleanupVertices(unsigned int svcount, recip[1] = 1 / dy; recip[2] = 1 / dz; - center[0]*=recip[0]; - center[1]*=recip[1]; - center[2]*=recip[2]; - + center[0] *= recip[0]; + center[1] *= recip[1]; + center[2] *= recip[2]; } - } + vtx = (const char *)svertices; - - vtx = (const char *) svertices; - - for (unsigned int i=0; i<svcount; i++) + for (unsigned int i = 0; i < svcount; i++) { const btVector3 *p = (const btVector3 *)vtx; - vtx+=stride; + vtx += stride; btScalar px = p->getX(); btScalar py = p->getY(); btScalar pz = p->getZ(); - if ( scale ) + if (scale) { - px = px*recip[0]; // normalize - py = py*recip[1]; // normalize - pz = pz*recip[2]; // normalize + px = px * recip[0]; // normalize + py = py * recip[1]; // normalize + pz = pz * recip[2]; // normalize } -// if ( 1 ) + // if ( 1 ) { unsigned int j; - for (j=0; j<vcount; j++) + for (j = 0; j < vcount; j++) { /// XXX might be broken - btVector3& v = vertices[j]; + btVector3 &v = vertices[j]; btScalar x = v[0]; btScalar y = v[1]; btScalar z = v[2]; - btScalar dx = btFabs(x - px ); - btScalar dy = btFabs(y - py ); - btScalar dz = btFabs(z - pz ); + btScalar dx = btFabs(x - px); + btScalar dy = btFabs(y - py); + btScalar dz = btFabs(z - pz); - if ( dx < normalepsilon && dy < normalepsilon && dz < normalepsilon ) + 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); + btScalar dist1 = GetDist(px, py, pz, center); + btScalar dist2 = GetDist(v[0], v[1], v[2], center); - if ( dist1 > dist2 ) + if (dist1 > dist2) { v[0] = px; v[1] = py; v[2] = pz; - } break; } } - if ( j == vcount ) + if (j == vcount) { - btVector3& dest = vertices[vcount]; + btVector3 &dest = vertices[vcount]; dest[0] = px; dest[1] = py; dest[2] = pz; @@ -1042,18 +1000,18 @@ bool HullLibrary::CleanupVertices(unsigned int svcount, } // ok..now make sure we didn't prune so many vertices it is now invalid. -// if ( 1 ) + // if ( 1 ) { - btScalar bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX }; - btScalar bmax[3] = { -FLT_MAX, -FLT_MAX, -FLT_MAX }; + 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++) + for (unsigned int i = 0; i < vcount; i++) { - const btVector3& p = vertices[i]; - for (int j=0; j<3; j++) + 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]; + if (p[j] < bmin[j]) bmin[j] = p[j]; + if (p[j] > bmax[j]) bmax[j] = p[j]; } } @@ -1061,27 +1019,27 @@ bool HullLibrary::CleanupVertices(unsigned int svcount, btScalar dy = bmax[1] - bmin[1]; btScalar dz = bmax[2] - bmin[2]; - if ( dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount < 3) + 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 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 (dx >= EPSILON && dx < len) len = dx; + if (dy >= EPSILON && dy < len) len = dy; + if (dz >= EPSILON && dz < len) len = dz; - if ( len == FLT_MAX ) + if (len == FLT_MAX) { - dx = dy = dz = btScalar(0.01); // one centimeter + 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); + 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; @@ -1093,16 +1051,16 @@ bool HullLibrary::CleanupVertices(unsigned int svcount, btScalar z1 = cz - dz; btScalar z2 = cz + dz; - vcount = 0; // add box + 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); + 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; } @@ -1111,57 +1069,52 @@ bool HullLibrary::CleanupVertices(unsigned int svcount, return true; } -void HullLibrary::BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount) +void HullLibrary::BringOutYourDead(const btVector3 *verts, unsigned int vcount, btVector3 *overts, unsigned int &ocount, unsigned int *indices, unsigned indexcount) { - btAlignedObjectArray<int>tmpIndices; + btAlignedObjectArray<int> tmpIndices; tmpIndices.resize(m_vertexIndexMapping.size()); int i; - for (i=0;i<m_vertexIndexMapping.size();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); + memset(&usedIndices[0], 0, sizeof(unsigned int) * vcount); ocount = 0; - for (i=0; i<int (indexcount); i++) + for (i = 0; i < int(indexcount); i++) { - unsigned int v = indices[i]; // original array index + unsigned int v = indices[i]; // original array index - btAssert( v >= 0 && v < vcount ); + btAssert(v >= 0 && v < vcount); - if ( usedIndices[static_cast<int>(v)] ) // if already remapped + if (usedIndices[static_cast<int>(v)]) // if already remapped { - indices[i] = usedIndices[static_cast<int>(v)]-1; // index to new array + indices[i] = usedIndices[static_cast<int>(v)] - 1; // index to new array } else { + indices[i] = ocount; // new index mapping - indices[i] = ocount; // new index mapping - - overts[ocount][0] = verts[v][0]; // copy old vert to new vert array + 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++) + for (int k = 0; k < m_vertexIndexMapping.size(); k++) { - if (tmpIndices[k]==int(v)) - m_vertexIndexMapping[k]=ocount; + if (tmpIndices[k] == int(v)) + m_vertexIndexMapping[k] = ocount; } - ocount++; // increment output vert count - - btAssert( ocount >=0 && ocount <= vcount ); + ocount++; // increment output vert count - usedIndices[static_cast<int>(v)] = ocount; // assign new index remapping + btAssert(ocount >= 0 && ocount <= vcount); - + usedIndices[static_cast<int>(v)] = ocount; // assign new index remapping } } - - } |