Page MenuHome
Differential D8762 Diff 28333 extern/bullet2/src/LinearMath/btConvexHull.cpp

Changeset View

Standalone View

View Options

extern/bullet2/src/LinearMath/btConvexHull.cpp

Show All 14 Lines
#include <string.h> #include <string.h>
#include "btConvexHull.h" #include "btConvexHull.h"
#include "btAlignedObjectArray.h" #include "btAlignedObjectArray.h"
#include "btMinMax.h" #include "btMinMax.h"
#include "btVector3.h" #include "btVector3.h"
//---------------------------------- //----------------------------------
class int3 class int3
{ {
public: public:
int x,y,z; int x, y, z;
int3(){}; int3(){};
int3(int _x,int _y, int _z){x=_x;y=_y;z=_z;} int3(int _x, int _y, int _z)
{
x = _x;
y = _y;
z = _z;
}
const int& operator[](int i) const {return (&x)[i];} const int &operator[](int i) const { return (&x)[i]; }
int& operator[](int i) {return (&x)[i];} int &operator[](int i) { return (&x)[i]; }
}; };
//------- btPlane ---------- //------- btPlane ----------
inline btPlane PlaneFlip(const btPlane &plane){return btPlane(-plane.normal,-plane.dist);} 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 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 int coplanar(const btPlane &a, const btPlane &b) { return (a == b || a == PlaneFlip(b)); }
//--------- Utility Functions ------ //--------- Utility Functions ------
btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1); btVector3 PlaneLineIntersection(const btPlane &plane, const btVector3 &p0, const btVector3 &p1);
btVector3 PlaneProject(const btPlane &plane, const btVector3 &point); 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 N1 = p0.normal;
btVector3 N2 = p1.normal; btVector3 N2 = p1.normal;
btVector3 N3 = p2.normal; btVector3 N3 = p2.normal;
btVector3 n2n3; n2n3 = N2.cross(N3); btVector3 n2n3;
btVector3 n3n1; n3n1 = N3.cross(N1); n2n3 = N2.cross(N3);
btVector3 n1n2; n1n2 = N1.cross(N2); btVector3 n3n1;
n3n1 = N3.cross(N1);
btVector3 n1n2;
n1n2 = N1.cross(N2);
btScalar quotient = (N1.dot(n2n3)); btScalar quotient = (N1.dot(n2n3));
btAssert(btFabs(quotient) > btScalar(0.000001)); btAssert(btFabs(quotient) > btScalar(0.000001));
quotient = btScalar(-1.) / quotient; quotient = btScalar(-1.) / quotient;
n2n3 *= p0.dist; n2n3 *= p0.dist;
n3n1 *= p1.dist; n3n1 *= p1.dist;
n1n2 *= p2.dist; n1n2 *= p2.dist;
btVector3 potentialVertex = n2n3; btVector3 potentialVertex = n2n3;
potentialVertex += n3n1; potentialVertex += n3n1;
potentialVertex += n1n2; potentialVertex += n1n2;
potentialVertex *= quotient; potentialVertex *= quotient;
btVector3 result(potentialVertex.getX(),potentialVertex.getY(),potentialVertex.getZ()); btVector3 result(potentialVertex.getX(), potentialVertex.getY(), potentialVertex.getZ());
return result; return result;
} }
btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint=NULL, btVector3 *vpoint=NULL); 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 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2);
btVector3 NormalOf(const btVector3 *vert, const int n); btVector3 NormalOf(const btVector3 *vert, const int n);
btVector3 PlaneLineIntersection(const btPlane &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 // returns the point where the line p0-p1 intersects the plane n&d
static btVector3 dif; btVector3 dif;
dif = p1-p0; dif = p1 - p0;
btScalar dn= btDot(plane.normal,dif); btScalar dn = btDot(plane.normal, dif);
btScalar t = -(plane.dist+btDot(plane.normal,p0) )/dn; btScalar t = -(plane.dist + btDot(plane.normal, p0)) / dn;
return p0 + (dif*t); return p0 + (dif * t);
} }
btVector3 PlaneProject(const btPlane &plane, const btVector3 &point) 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) btVector3 TriNormal(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2)
{ {
// return the normal of the triangle // return the normal of the triangle
// inscribed by v0, v1, and v2 // inscribed by v0, v1, and v2
btVector3 cp=btCross(v1-v0,v2-v1); btVector3 cp = btCross(v1 - v0, v2 - v1);
btScalar m=cp.length(); btScalar m = cp.length();
if(m==0) return btVector3(1,0,0); if (m == 0) return btVector3(1, 0, 0);
return cp*(btScalar(1.0)/m); return cp * (btScalar(1.0) / m);
} }
btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint, btVector3 *vpoint) btScalar DistanceBetweenLines(const btVector3 &ustart, const btVector3 &udir, const btVector3 &vstart, const btVector3 &vdir, btVector3 *upoint, btVector3 *vpoint)
{ {
static btVector3 cp; btVector3 cp;
cp = btCross(udir,vdir).normalized(); cp = btCross(udir, vdir).normalized();
btScalar distu = -btDot(cp,ustart); btScalar distu = -btDot(cp, ustart);
btScalar distv = -btDot(cp,vstart); btScalar distv = -btDot(cp, vstart);
btScalar dist = (btScalar)fabs(distu-distv); btScalar dist = (btScalar)fabs(distu - distv);
if(upoint) if (upoint)
{ {
btPlane plane; btPlane plane;
plane.normal = btCross(vdir,cp).normalized(); plane.normal = btCross(vdir, cp).normalized();
plane.dist = -btDot(plane.normal,vstart); plane.dist = -btDot(plane.normal, vstart);
*upoint = PlaneLineIntersection(plane,ustart,ustart+udir); *upoint = PlaneLineIntersection(plane, ustart, ustart + udir);
} }
if(vpoint) if (vpoint)
{ {
btPlane plane; btPlane plane;
plane.normal = btCross(udir,cp).normalized(); plane.normal = btCross(udir, cp).normalized();
plane.dist = -btDot(plane.normal,ustart); plane.dist = -btDot(plane.normal, ustart);
*vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir); *vpoint = PlaneLineIntersection(plane, vstart, vstart + vdir);
} }
return dist; return dist;
} }
#define COPLANAR (0) #define COPLANAR (0)
#define UNDER (1) #define UNDER (1)
#define OVER (2) #define OVER (2)
#define SPLIT (OVER|UNDER) #define SPLIT (OVER | UNDER)
#define PAPERWIDTH (btScalar(0.001)) #define PAPERWIDTH (btScalar(0.001))
btScalar planetestepsilon = PAPERWIDTH; btScalar planetestepsilon = PAPERWIDTH;
typedef ConvexH::HalfEdge HalfEdge; 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); vertices.resize(vertices_size);
edges.resize(edges_size); edges.resize(edges_size);
facets.resize(facets_size); facets.resize(facets_size);
} }
int PlaneTest(const btPlane &p, const btVector3 &v); int PlaneTest(const btPlane &p, const btVector3 &v);
int PlaneTest(const btPlane &p, const btVector3 &v) { int PlaneTest(const btPlane &p, const btVector3 &v)
{
btScalar a = btDot(v,p.normal)+p.dist; btScalar a = btDot(v, p.normal) + p.dist;
int flag = (a>planetestepsilon)?OVER:((a<-planetestepsilon)?UNDER:COPLANAR); int flag = (a > planetestepsilon) ? OVER : ((a < -planetestepsilon) ? UNDER : COPLANAR);
return flag; return flag;
} }
int SplitTest(ConvexH &convex,const btPlane &plane); int SplitTest(ConvexH &convex, const btPlane &plane);
int SplitTest(ConvexH &convex,const btPlane &plane) { int SplitTest(ConvexH &convex, const btPlane &plane)
{
int flag=0; int flag = 0;
for(int i=0;i<convex.vertices.size();i++) { for (int i = 0; i < convex.vertices.size(); i++)
{
flag |= PlaneTest(plane,convex.vertices[i]); flag |= PlaneTest(plane, convex.vertices[i]);
} }
return flag; return flag;
} }
class VertFlag class VertFlag
{ {
public: public:
unsigned char planetest; unsigned char planetest;
unsigned char junk; unsigned char junk;
unsigned char undermap; unsigned char undermap;
unsigned char overmap; unsigned char overmap;
}; };
class EdgeFlag class EdgeFlag
{ {
public: public:
unsigned char planetest; unsigned char planetest;
unsigned char fixes; unsigned char fixes;
short undermap; short undermap;
short overmap; short overmap;
}; };
class PlaneFlag class PlaneFlag
{ {
public: public:
unsigned char undermap; unsigned char undermap;
unsigned char overmap; unsigned char overmap;
}; };
class Coplanar{ class Coplanar
{
public: public:
unsigned short ea; unsigned short ea;
unsigned char v0; unsigned char v0;
unsigned char v1; unsigned char v1;
}; };
template<class T> template <class T>
int maxdirfiltered(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow) int maxdirfiltered(const T *p, int count, const T &dir, btAlignedObjectArray<int> &allow)
{ {
btAssert(count); btAssert(count);
int m=-1; int m = -1;
for(int i=0;i<count;i++) for (int i = 0; i < count; i++)
if(allow[i]) if (allow[i])
{ {
if(m==-1 || btDot(p[i],dir)>btDot(p[m],dir)) if (m == -1 || btDot(p[i], dir) > btDot(p[m], dir))
m=i; m = i;
} }
btAssert(m!=-1); btAssert(m != -1);
return m; return m;
} }
btVector3 orth(const btVector3 &v); btVector3 orth(const btVector3 &v);
btVector3 orth(const btVector3 &v) btVector3 orth(const btVector3 &v)
{ {
btVector3 a=btCross(v,btVector3(0,0,1)); btVector3 a = btCross(v, btVector3(0, 0, 1));
btVector3 b=btCross(v,btVector3(0,1,0)); btVector3 b = btCross(v, btVector3(0, 1, 0));
if (a.length() > b.length()) if (a.length() > b.length())
{ {
return a.normalized(); return a.normalized();
} else { }
else
{
return b.normalized(); return b.normalized();
} }
} }
template<class T> template <class T>
int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray<int> &allow) int maxdirsterid(const T *p, int count, const T &dir, btAlignedObjectArray<int> &allow)
{ {
int m=-1; int m = -1;
while(m==-1) while (m == -1)
{ {
m = maxdirfiltered(p,count,dir,allow); m = maxdirfiltered(p, count, dir, allow);
if(allow[m]==3) return m; if (allow[m] == 3) return m;
T u = orth(dir); T u = orth(dir);
T v = btCross(u,dir); T v = btCross(u, dir);
int ma=-1; int ma = -1;
for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0)) for (btScalar x = btScalar(0.0); x <= btScalar(360.0); x += btScalar(45.0))
{ {
btScalar s = btSin(SIMD_RADS_PER_DEG*(x)); btScalar s = btSin(SIMD_RADS_PER_DEG * (x));
btScalar c = btCos(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); int mb = maxdirfiltered(p, count, dir + (u * s + v * c) * btScalar(0.025), allow);
if(ma==m && mb==m) if (ma == m && mb == m)
{ {
allow[m]=3; allow[m] = 3;
return m; 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; 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 s = btSin(SIMD_RADS_PER_DEG * (xx));
btScalar c = btCos(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); int md = maxdirfiltered(p, count, dir + (u * s + v * c) * btScalar(0.025), allow);
if(mc==m && md==m) if (mc == m && md == m)
{ {
allow[m]=3; allow[m] = 3;
return m; return m;
} }
mc=md; mc = md;
} }
} }
ma=mb; ma = mb;
} }
allow[m]=0; allow[m] = 0;
m=-1; m = -1;
} }
btAssert(0); btAssert(0);
return m; 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; 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]]); btVector3 n = TriNormal(vertices[t[0]], vertices[t[1]], vertices[t[2]]);
return (btDot(n,p-vertices[t[0]]) > epsilon); // EPSILON??? 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; int i1 = (i + 1) % 3;
if(t[i]==a && t[i1]==b) return 1; if (t[i] == a && t[i1] == b) return 1;
} }
return 0; return 0;
} }
int hasvert(const int3 &t, int v); int hasvert(const int3 &t, int v);
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; int i;
for(i=0;i<3;i++) for (i = 0; i < 3; i++)
{ {
int i1= (i+1)%3; int i1 = (i + 1) % 3;
if(hasedge(a,b[i1],b[i])) return 1; if (hasedge(a, b[i1], b[i])) return 1;
} }
return 0; return 0;
} }
class btHullTriangle; class btHullTriangle;
class btHullTriangle : public int3 class btHullTriangle : public int3
{ {
public: public:
int3 n; int3 n;
int id; int id;
int vmax; int vmax;
btScalar rise; 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); rise = btScalar(0.0);
} }
~btHullTriangle() ~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; int i;
for(i=0;i<3;i++) for (i = 0; i < 3; i++)
{ {
int i1=(i+1)%3; int i1 = (i + 1) % 3;
int i2=(i+2)%3; int i2 = (i + 2) % 3;
if((*this)[i]==a && (*this)[i1]==b) return n[i2]; if ((*this)[i] == a && (*this)[i1] == b) return n[i2];
if((*this)[i]==b && (*this)[i1]==a) return n[i2]; if ((*this)[i] == b && (*this)[i1] == a) return n[i2];
} }
btAssert(0); btAssert(0);
return er; return er;
} }
void HullLibrary::b2bfix(btHullTriangle* s,btHullTriangle*t) void HullLibrary::b2bfix(btHullTriangle *s, btHullTriangle *t)
{ {
int i; int i;
for(i=0;i<3;i++) for (i = 0; i < 3; i++)
{ {
int i1=(i+1)%3; int i1 = (i + 1) % 3;
int i2=(i+2)%3; int i2 = (i + 2) % 3;
int a = (*s)[i1]; int a = (*s)[i1];
int b = (*s)[i2]; int b = (*s)[i2];
btAssert(m_tris[s->neib(a,b)]->neib(b,a) == s->id); btAssert(m_tris[s->neib(a, b)]->neib(b, a) == s->id);
btAssert(m_tris[t->neib(a,b)]->neib(b,a) == t->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[s->neib(a, b)]->neib(b, a) = t->neib(b, a);
m_tris[t->neib(b,a)]->neib(a,b) = s->neib(a,b); 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(s);
deAllocateTriangle(t); deAllocateTriangle(t);
} }
void HullLibrary::checkit(btHullTriangle *t) void HullLibrary::checkit(btHullTriangle *t)
{ {
(void)t; (void)t;
int i; int i;
btAssert(m_tris[t->id]==t); btAssert(m_tris[t->id] == t);
for(i=0;i<3;i++) for (i = 0; i < 3; i++)
{ {
int i1=(i+1)%3; int i1 = (i + 1) % 3;
int i2=(i+2)%3; int i2 = (i + 2) % 3;
int a = (*t)[i1]; int a = (*t)[i1];
int b = (*t)[i2]; int b = (*t)[i2];
// release compile fix // release compile fix
(void)i1; (void)i1;
(void)i2; (void)i2;
(void)a; (void)a;
(void)b; (void)b;
btAssert(a!=b); btAssert(a != b);
btAssert( m_tris[t->n[i]]->neib(b,a) == t->id); 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); void *mem = btAlignedAlloc(sizeof(btHullTriangle), 16);
btHullTriangle* tr = new (mem)btHullTriangle(a,b,c); btHullTriangle *tr = new (mem) btHullTriangle(a, b, c);
tr->id = m_tris.size(); tr->id = m_tris.size();
m_tris.push_back(tr); m_tris.push_back(tr);
return tr; return tr;
} }
void HullLibrary::deAllocateTriangle(btHullTriangle* tri) void HullLibrary::deAllocateTriangle(btHullTriangle *tri)
{ {
btAssert(m_tris[tri->id]==tri); btAssert(m_tris[tri->id] == tri);
m_tris[tri->id]=NULL; m_tris[tri->id] = NULL;
tri->~btHullTriangle(); tri->~btHullTriangle();
btAlignedFree(tri); 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(); int n = m_tris.size();
btHullTriangle* ta = allocateTriangle(v,t[1],t[2]); btHullTriangle *ta = allocateTriangle(v, t[1], t[2]);
ta->n = int3(t0->n[0],n+1,n+2); ta->n = int3(t0->n[0], n + 1, n + 2);
m_tris[t0->n[0]]->neib(t[1],t[2]) = n+0; m_tris[t0->n[0]]->neib(t[1], t[2]) = n + 0;
btHullTriangle* tb = allocateTriangle(v,t[2],t[0]); btHullTriangle *tb = allocateTriangle(v, t[2], t[0]);
tb->n = int3(t0->n[1],n+2,n+0); tb->n = int3(t0->n[1], n + 2, n + 0);
m_tris[t0->n[1]]->neib(t[2],t[0]) = n+1; m_tris[t0->n[1]]->neib(t[2], t[0]) = n + 1;
btHullTriangle* tc = allocateTriangle(v,t[0],t[1]); btHullTriangle *tc = allocateTriangle(v, t[0], t[1]);
tc->n = int3(t0->n[2],n+0,n+1); tc->n = int3(t0->n[2], n + 0, n + 1);
m_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(ta);
checkit(tb); checkit(tb);
checkit(tc); checkit(tc);
if(hasvert(*m_tris[ta->n[0]],v)) removeb2b(ta,m_tris[ta->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[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[tc->n[0]], v)) removeb2b(tc, m_tris[tc->n[0]]);
deAllocateTriangle(t0); deAllocateTriangle(t0);
} }
btHullTriangle* HullLibrary::extrudable(btScalar epsilon) btHullTriangle *HullLibrary::extrudable(btScalar epsilon)
{ {
int i; int i;
btHullTriangle *t=NULL; btHullTriangle *t = NULL;
for(i=0;i<m_tris.size();i++) 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]; 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]; btVector3 basis[3];
basis[0] = btVector3( btScalar(0.01), btScalar(0.02), btScalar(1.0) ); basis[0] = btVector3(btScalar(0.01), btScalar(0.02), btScalar(1.0));
int p0 = maxdirsterid(verts,verts_count, basis[0],allow); int p0 = maxdirsterid(verts, verts_count, basis[0], allow);
int p1 = maxdirsterid(verts,verts_count,-basis[0],allow); int p1 = maxdirsterid(verts, verts_count, -basis[0], allow);
basis[0] = verts[p0]-verts[p1]; basis[0] = verts[p0] - verts[p1];
if(p0==p1 || basis[0]==btVector3(0,0,0)) if (p0 == p1 || basis[0] == btVector3(0, 0, 0))
return int4(-1,-1,-1,-1); return int4(-1, -1, -1, -1);
basis[1] = btCross(btVector3( btScalar(1),btScalar(0.02), btScalar(0)),basis[0]); 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[2] = btCross(btVector3(btScalar(-0.02), btScalar(1), btScalar(0)), basis[0]);
if (basis[1].length() > basis[2].length()) if (basis[1].length() > basis[2].length())
{ {
basis[1].normalize(); basis[1].normalize();
} else { }
else
{
basis[1] = basis[2]; basis[1] = basis[2];
basis[1].normalize (); basis[1].normalize();
} }
int p2 = maxdirsterid(verts,verts_count,basis[1],allow); int p2 = maxdirsterid(verts, verts_count, basis[1], allow);
if(p2 == p0 || p2 == p1) 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) if (p2 == p0 || p2 == p1)
return int4(-1,-1,-1,-1); return int4(-1, -1, -1, -1);
basis[1] = verts[p2] - verts[p0]; basis[1] = verts[p2] - verts[p0];
basis[2] = btCross(basis[1],basis[0]).normalized(); basis[2] = btCross(basis[1], basis[0]).normalized();
int p3 = maxdirsterid(verts,verts_count,basis[2],allow); 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) p3 = maxdirsterid(verts, verts_count, -basis[2], allow);
if(p3==p0||p3==p1||p3==p2) if (p3 == p0 || p3 == p1 || p3 == p2)
return int4(-1,-1,-1,-1); return int4(-1, -1, -1, -1);
btAssert(!(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(btDot(verts[p3]-verts[p0],btCross(verts[p1]-verts[p0],verts[p2]-verts[p0])) <0) {btSwap(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); 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 (verts_count < 4) return 0;
if(vlimit==0) vlimit=1000000000; if (vlimit == 0) vlimit = 1000000000;
int j; int j;
btVector3 bmin(*verts),bmax(*verts); btVector3 bmin(*verts), bmax(*verts);
btAlignedObjectArray<int> isextreme; btAlignedObjectArray<int> isextreme;
isextreme.reserve(verts_count); isextreme.reserve(verts_count);
btAlignedObjectArray<int> allow; btAlignedObjectArray<int> allow;
allow.reserve(verts_count); allow.reserve(verts_count);
for(j=0;j<verts_count;j++) for (j = 0; j < verts_count; j++)
{ {
allow.push_back(1); allow.push_back(1);
isextreme.push_back(0); isextreme.push_back(0);
bmin.setMin (verts[j]); bmin.setMin(verts[j]);
bmax.setMax (verts[j]); bmax.setMax(verts[j]);
} }
btScalar epsilon = (bmax-bmin).length() * btScalar(0.001); btScalar epsilon = (bmax - bmin).length() * btScalar(0.001);
btAssert (epsilon != 0.0); btAssert(epsilon != 0.0);
int4 p = FindSimplex(verts,verts_count,allow); int4 p = FindSimplex(verts, verts_count, allow);
if(p.x==-1) return 0; // simplex failed 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 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 *t0 = allocateTriangle(p[2], p[3], p[1]);
btHullTriangle *t1 = allocateTriangle(p[3],p[2],p[0]); t1->n=int3(3,2,0); t0->n = int3(2, 3, 1);
btHullTriangle *t2 = allocateTriangle(p[0],p[1],p[3]); t2->n=int3(0,1,3); btHullTriangle *t1 = allocateTriangle(p[3], p[2], p[0]);
btHullTriangle *t3 = allocateTriangle(p[1],p[0],p[2]); t3->n=int3(1,0,2); 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; isextreme[p[0]] = isextreme[p[1]] = isextreme[p[2]] = isextreme[p[3]] = 1;
checkit(t0);checkit(t1);checkit(t2);checkit(t3); checkit(t0);
checkit(t1);
checkit(t2);
checkit(t3);
for(j=0;j<m_tris.size();j++) for (j = 0; j < m_tris.size(); j++)
{ {
btHullTriangle *t=m_tris[j]; btHullTriangle *t = m_tris[j];
btAssert(t); btAssert(t);
btAssert(t->vmax<0); btAssert(t->vmax < 0);
btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); btVector3 n = TriNormal(verts[(*t)[0]], verts[(*t)[1]], verts[(*t)[2]]);
t->vmax = maxdirsterid(verts,verts_count,n,allow); t->vmax = maxdirsterid(verts, verts_count, n, allow);
t->rise = btDot(n,verts[t->vmax]-verts[(*t)[0]]); t->rise = btDot(n, verts[t->vmax] - verts[(*t)[0]]);
} }
btHullTriangle *te; btHullTriangle *te;
vlimit-=4; vlimit -= 4;
while(vlimit >0 && ((te=extrudable(epsilon)) != 0)) while (vlimit > 0 && ((te = extrudable(epsilon)) != 0))
{ {
//int3 ti=*te; //int3 ti=*te;
int v=te->vmax; int v = te->vmax;
btAssert(v != -1); btAssert(v != -1);
btAssert(!isextreme[v]); // wtf we've already done this vertex 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 //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already
j=m_tris.size(); j = m_tris.size();
while(j--) { while (j--)
{
if(!m_tris[j]) continue; if (!m_tris[j]) continue;
int3 t=*m_tris[j]; int3 t = *m_tris[j];
if(above(verts,t,verts[v],btScalar(0.01)*epsilon)) 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 // now check for those degenerate cases where we have a flipped triangle or a really skinny triangle
j=m_tris.size(); j = m_tris.size();
while(j--) while (j--)
{ {
if(!m_tris[j]) continue; if (!m_tris[j]) continue;
if(!hasvert(*m_tris[j],v)) break; if (!hasvert(*m_tris[j], v)) break;
int3 nt=*m_tris[j]; 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 (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]]; btHullTriangle *nb = m_tris[m_tris[j]->n[0]];
btAssert(nb);btAssert(!hasvert(*nb,v));btAssert(nb->id<j); btAssert(nb);
btAssert(!hasvert(*nb, v));
btAssert(nb->id < j);
extrude(nb,v); extrude(nb, v);
j=m_tris.size(); j = m_tris.size();
} }
} }
j=m_tris.size(); j = m_tris.size();
while(j--) while (j--)
{ {
btHullTriangle *t=m_tris[j]; btHullTriangle *t = m_tris[j];
if(!t) continue; if (!t) continue;
if(t->vmax>=0) break; if (t->vmax >= 0) break;
btVector3 n=TriNormal(verts[(*t)[0]],verts[(*t)[1]],verts[(*t)[2]]); btVector3 n = TriNormal(verts[(*t)[0]], verts[(*t)[1]], verts[(*t)[2]]);
t->vmax = maxdirsterid(verts,verts_count,n,allow); t->vmax = maxdirsterid(verts, verts_count, n, allow);
if(isextreme[t->vmax]) 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 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; 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) ; int rc = calchullgen(verts, verts_count, vlimit);
if(!rc) return 0; if (!rc) return 0;
btAlignedObjectArray<int> ts; btAlignedObjectArray<int> ts;
int i; 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]); ts.push_back((*m_tris[i])[j]);
deAllocateTriangle(m_tris[i]); deAllocateTriangle(m_tris[i]);
} }
} }
tris_count = ts.size()/3; tris_count = ts.size() / 3;
tris_out.resize(ts.size()); 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]); tris_out[i] = static_cast<unsigned int>(ts[i]);
} }
m_tris.resize(0); m_tris.resize(0);
return 1; 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 tris_count;
int ret = calchull( (btVector3 *) vertices, (int) vcount, result.m_Indices, tris_count, static_cast<int>(vlimit) ); int ret = calchull((btVector3 *)vertices, (int)vcount, result.m_Indices, tris_count, static_cast<int>(vlimit));
if(!ret) return false; if (!ret) return false;
result.mIndexCount = (unsigned int) (tris_count*3); result.mIndexCount = (unsigned int)(tris_count * 3);
result.mFaceCount = (unsigned int) tris_count; result.mFaceCount = (unsigned int)tris_count;
result.mVertices = (btVector3*) vertices; result.mVertices = (btVector3 *)vertices;
result.mVcount = (unsigned int) vcount; result.mVcount = (unsigned int)vcount;
return true; return true;
} }
void ReleaseHull(PHullResult &result); void ReleaseHull(PHullResult &result);
void ReleaseHull(PHullResult &result) void ReleaseHull(PHullResult &result)
{ {
if ( result.m_Indices.size() ) if (result.m_Indices.size())
{ {
result.m_Indices.clear(); result.m_Indices.clear();
} }
result.mVcount = 0; result.mVcount = 0;
result.mIndexCount = 0; result.mIndexCount = 0;
result.mVertices = 0; result.mVertices = 0;
} }
//********************************************************************* //*********************************************************************
//********************************************************************* //*********************************************************************
//******** HullLib header //******** HullLib header
//********************************************************************* //*********************************************************************
//********************************************************************* //*********************************************************************
//********************************************************************* //*********************************************************************
//********************************************************************* //*********************************************************************
//******** HullLib implementation //******** HullLib implementation
//********************************************************************* //*********************************************************************
//********************************************************************* //*********************************************************************
HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // describes the input request HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // describes the input request
HullResult &result) // contains the resulst HullResult &result) // contains the resulst
{ {
HullError ret = QE_FAIL; HullError ret = QE_FAIL;
PHullResult hr; PHullResult hr;
unsigned int vcount = desc.mVcount; unsigned int vcount = desc.mVcount;
if ( vcount < 8 ) vcount = 8; if (vcount < 8) vcount = 8;
btAlignedObjectArray<btVector3> vertexSource; btAlignedObjectArray<btVector3> vertexSource;
vertexSource.resize(static_cast<int>(vcount)); vertexSource.resize(static_cast<int>(vcount));
btVector3 scale; btVector3 scale;
unsigned int ovcount; 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)]; btVector3 &v = vertexSource[static_cast<int>(i)];
v[0]*=scale[0]; v[0] *= scale[0];
v[1]*=scale[1]; v[1] *= scale[1];
v[2]*=scale[2]; 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. // 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)); 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; 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.mNumOutputVertices = ovcount;
result.m_OutputVertices.resize(static_cast<int>(ovcount)); result.m_OutputVertices.resize(static_cast<int>(ovcount));
result.mNumFaces = hr.mFaceCount; result.mNumFaces = hr.mFaceCount;
result.mNumIndices = hr.mIndexCount; result.mNumIndices = hr.mIndexCount;
result.m_Indices.resize(static_cast<int>(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]; 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[0] = source[2];
dest[1] = source[1]; dest[1] = source[1];
dest[2] = source[0]; dest[2] = source[0];
dest+=3; dest += 3;
source+=3; source += 3;
} }
} }
else 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 else
{ {
result.mPolygons = true; result.mPolygons = true;
result.mNumOutputVertices = ovcount; result.mNumOutputVertices = ovcount;
result.m_OutputVertices.resize(static_cast<int>(ovcount)); result.m_OutputVertices.resize(static_cast<int>(ovcount));
result.mNumFaces = hr.mFaceCount; result.mNumFaces = hr.mFaceCount;
result.mNumIndices = hr.mIndexCount+hr.mFaceCount; result.mNumIndices = hr.mIndexCount + hr.mFaceCount;
result.m_Indices.resize(static_cast<int>(result.mNumIndices)); 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]; 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] = 3; dest[0] = 3;
if ( desc.HasHullFlag(QF_REVERSE_ORDER) ) if (desc.HasHullFlag(QF_REVERSE_ORDER))
{ {
dest[1] = source[2]; dest[1] = source[2];
dest[2] = source[1]; dest[2] = source[1];
dest[3] = source[0]; dest[3] = source[0];
} }
else else
{ {
dest[1] = source[0]; dest[1] = source[0];
dest[2] = source[1]; dest[2] = source[1];
dest[3] = source[2]; dest[3] = source[2];
} }
dest+=4; dest += 4;
source+=3; source += 3;
} }
} }
} }
ReleaseHull(hr); ReleaseHull(hr);
} }
} }
return ret; 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(); result.m_OutputVertices.clear();
} }
if ( result.m_Indices.size() ) if (result.m_Indices.size())
{ {
result.mNumIndices=0; result.mNumIndices = 0;
result.m_Indices.clear(); result.m_Indices.clear();
} }
return QE_OK; 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 // XXX, might be broken
btVector3& dest = p[vcount]; btVector3 &dest = p[vcount];
dest[0] = x; dest[0] = x;
dest[1] = y; dest[1] = y;
dest[2] = z; dest[2] = z;
vcount++; 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 dx = px - p2[0];
btScalar dy = py - p2[1]; btScalar dy = py - p2[1];
btScalar dz = pz - p2[2]; 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, bool HullLibrary::CleanupVertices(unsigned int svcount,
const btVector3 *svertices, const btVector3 *svertices,
unsigned int stride, unsigned int stride,
unsigned int &vcount, // output number of vertices unsigned int &vcount, // output number of vertices
btVector3 *vertices, // location to store the results. btVector3 *vertices, // location to store the results.
btScalar normalepsilon, btScalar normalepsilon,
btVector3& scale) btVector3 &scale)
{ {
if ( svcount == 0 ) return false; if (svcount == 0) return false;
m_vertexIndexMapping.resize(0); m_vertexIndexMapping.resize(0);
#define EPSILON btScalar(0.000001) /* close enough to consider two btScalaring point numbers to be 'the same'. */ #define EPSILON btScalar(0.000001) /* close enough to consider two btScalaring point numbers to be 'the same'. */
vcount = 0; 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[0] = 1;
scale[1] = 1; scale[1] = 1;
scale[2] = 1; scale[2] = 1;
} }
btScalar bmin[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 }; 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] < bmin[j]) bmin[j] = p[j];
if ( p[j] > bmax[j] ) bmax[j] = p[j]; if (p[j] > bmax[j]) bmax[j] = p[j];
} }
} }
} }
btScalar dx = bmax[0] - bmin[0]; btScalar dx = bmax[0] - bmin[0];
btScalar dy = bmax[1] - bmin[1]; btScalar dy = bmax[1] - bmin[1];
btScalar dz = bmax[2] - bmin[2]; btScalar dz = bmax[2] - bmin[2];
btVector3 center; btVector3 center;
center[0] = dx*btScalar(0.5) + bmin[0]; center[0] = dx * btScalar(0.5) + bmin[0];
center[1] = dy*btScalar(0.5) + bmin[1]; center[1] = dy * btScalar(0.5) + bmin[1];
center[2] = dz*btScalar(0.5) + bmin[2]; 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; btScalar len = FLT_MAX;
if ( dx > EPSILON && dx < len ) len = dx; if (dx > EPSILON && dx < len) len = dx;
if ( dy > EPSILON && dy < len ) len = dy; if (dy > EPSILON && dy < len) len = dy;
if ( dz > EPSILON && dz < len ) len = dz; 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 else
{ {
if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge. if (dx < EPSILON) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge.
if ( dy < EPSILON ) dy = len * btScalar(0.05); if (dy < EPSILON) dy = len * btScalar(0.05);
if ( dz < EPSILON ) dz = len * btScalar(0.05); if (dz < EPSILON) dz = len * btScalar(0.05);
} }
btScalar x1 = center[0] - dx; btScalar x1 = center[0] - dx;
btScalar x2 = center[0] + dx; btScalar x2 = center[0] + dx;
btScalar y1 = center[1] - dy; btScalar y1 = center[1] - dy;
btScalar y2 = center[1] + dy; btScalar y2 = center[1] + dy;
btScalar z1 = center[2] - dz; btScalar z1 = center[2] - dz;
btScalar z2 = center[2] + dz; btScalar z2 = center[2] + dz;
addPoint(vcount,vertices,x1,y1,z1); addPoint(vcount, vertices, x1, y1, z1);
addPoint(vcount,vertices,x2,y1,z1); addPoint(vcount, vertices, x2, y1, z1);
addPoint(vcount,vertices,x2,y2,z1); addPoint(vcount, vertices, x2, y2, z1);
addPoint(vcount,vertices,x1,y2,z1); addPoint(vcount, vertices, x1, y2, z1);
addPoint(vcount,vertices,x1,y1,z2); addPoint(vcount, vertices, x1, y1, z2);
addPoint(vcount,vertices,x2,y1,z2); addPoint(vcount, vertices, x2, y1, z2);
addPoint(vcount,vertices,x2,y2,z2); addPoint(vcount, vertices, x2, y2, z2);
addPoint(vcount,vertices,x1,y2,z2); addPoint(vcount, vertices, x1, y2, z2);
return true; // return cube return true; // return cube
} }
else else
{ {
if ( scale ) if (scale)
{ {
scale[0] = dx; scale[0] = dx;
scale[1] = dy; scale[1] = dy;
scale[2] = dz; scale[2] = dz;
recip[0] = 1 / dx; recip[0] = 1 / dx;
recip[1] = 1 / dy; recip[1] = 1 / dy;
recip[2] = 1 / dz; recip[2] = 1 / dz;
center[0]*=recip[0]; center[0] *= recip[0];
center[1]*=recip[1]; center[1] *= recip[1];
center[2]*=recip[2]; 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; const btVector3 *p = (const btVector3 *)vtx;
vtx+=stride; vtx += stride;
btScalar px = p->getX(); btScalar px = p->getX();
btScalar py = p->getY(); btScalar py = p->getY();
btScalar pz = p->getZ(); btScalar pz = p->getZ();
if ( scale ) if (scale)
{ {
px = px*recip[0]; // normalize px = px * recip[0]; // normalize
py = py*recip[1]; // normalize py = py * recip[1]; // normalize
pz = pz*recip[2]; // normalize pz = pz * recip[2]; // normalize
} }
// if ( 1 ) // if ( 1 )
{ {
unsigned int j; unsigned int j;
for (j=0; j<vcount; j++) for (j = 0; j < vcount; j++)
{ {
/// XXX might be broken /// XXX might be broken
btVector3& v = vertices[j]; btVector3 &v = vertices[j];
btScalar x = v[0]; btScalar x = v[0];
btScalar y = v[1]; btScalar y = v[1];
btScalar z = v[2]; btScalar z = v[2];
btScalar dx = btFabs(x - px ); btScalar dx = btFabs(x - px);
btScalar dy = btFabs(y - py ); btScalar dy = btFabs(y - py);
btScalar dz = btFabs(z - pz ); 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 // 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. // 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. // in which case we keep this one instead.
btScalar dist1 = GetDist(px,py,pz,center); btScalar dist1 = GetDist(px, py, pz, center);
btScalar dist2 = GetDist(v[0],v[1],v[2],center); btScalar dist2 = GetDist(v[0], v[1], v[2], center);
if ( dist1 > dist2 ) if (dist1 > dist2)
{ {
v[0] = px; v[0] = px;
v[1] = py; v[1] = py;
v[2] = pz; v[2] = pz;
} }
break; break;
} }
} }
if ( j == vcount ) if (j == vcount)
{ {
btVector3& dest = vertices[vcount]; btVector3 &dest = vertices[vcount];
dest[0] = px; dest[0] = px;
dest[1] = py; dest[1] = py;
dest[2] = pz; dest[2] = pz;
vcount++; vcount++;
} }
m_vertexIndexMapping.push_back(j); m_vertexIndexMapping.push_back(j);
} }
} }
// ok..now make sure we didn't prune so many vertices it is now invalid. // 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 bmin[3] = {FLT_MAX, FLT_MAX, FLT_MAX};
btScalar bmax[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]; const btVector3 &p = vertices[i];
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] < bmin[j]) bmin[j] = p[j];
if ( p[j] > bmax[j] ) bmax[j] = p[j]; if (p[j] > bmax[j]) bmax[j] = p[j];
} }
} }
btScalar dx = bmax[0] - bmin[0]; btScalar dx = bmax[0] - bmin[0];
btScalar dy = bmax[1] - bmin[1]; btScalar dy = bmax[1] - bmin[1];
btScalar dz = bmax[2] - bmin[2]; 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 cx = dx * btScalar(0.5) + bmin[0];
btScalar cy = dy*btScalar(0.5) + bmin[1]; btScalar cy = dy * btScalar(0.5) + bmin[1];
btScalar cz = dz*btScalar(0.5) + bmin[2]; btScalar cz = dz * btScalar(0.5) + bmin[2];
btScalar len = FLT_MAX; btScalar len = FLT_MAX;
if ( dx >= EPSILON && dx < len ) len = dx; if (dx >= EPSILON && dx < len) len = dx;
if ( dy >= EPSILON && dy < len ) len = dy; if (dy >= EPSILON && dy < len) len = dy;
if ( dz >= EPSILON && dz < len ) len = dz; 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 else
{ {
if ( dx < EPSILON ) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge. if (dx < EPSILON) dx = len * btScalar(0.05); // 1/5th the shortest non-zero edge.
if ( dy < EPSILON ) dy = len * btScalar(0.05); if (dy < EPSILON) dy = len * btScalar(0.05);
if ( dz < EPSILON ) dz = len * btScalar(0.05); if (dz < EPSILON) dz = len * btScalar(0.05);
} }
btScalar x1 = cx - dx; btScalar x1 = cx - dx;
btScalar x2 = cx + dx; btScalar x2 = cx + dx;
btScalar y1 = cy - dy; btScalar y1 = cy - dy;
btScalar y2 = cy + dy; btScalar y2 = cy + dy;
btScalar z1 = cz - dz; btScalar z1 = cz - dz;
btScalar z2 = cz + dz; btScalar z2 = cz + dz;
vcount = 0; // add box vcount = 0; // add box
addPoint(vcount,vertices,x1,y1,z1); addPoint(vcount, vertices, x1, y1, z1);
addPoint(vcount,vertices,x2,y1,z1); addPoint(vcount, vertices, x2, y1, z1);
addPoint(vcount,vertices,x2,y2,z1); addPoint(vcount, vertices, x2, y2, z1);
addPoint(vcount,vertices,x1,y2,z1); addPoint(vcount, vertices, x1, y2, z1);
addPoint(vcount,vertices,x1,y1,z2); addPoint(vcount, vertices, x1, y1, z2);
addPoint(vcount,vertices,x2,y1,z2); addPoint(vcount, vertices, x2, y1, z2);
addPoint(vcount,vertices,x2,y2,z2); addPoint(vcount, vertices, x2, y2, z2);
addPoint(vcount,vertices,x1,y2,z2); addPoint(vcount, vertices, x1, y2, z2);
return true; return true;
} }
} }
return true; 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()); tmpIndices.resize(m_vertexIndexMapping.size());
int i; int i;
for (i=0;i<m_vertexIndexMapping.size();i++) for (i = 0; i < m_vertexIndexMapping.size(); i++)
{ {
tmpIndices[i] = m_vertexIndexMapping[i]; tmpIndices[i] = m_vertexIndexMapping[i];
} }
TUIntArray usedIndices; TUIntArray usedIndices;
usedIndices.resize(static_cast<int>(vcount)); usedIndices.resize(static_cast<int>(vcount));
memset(&usedIndices[0],0,sizeof(unsigned int)*vcount); memset(&usedIndices[0], 0, sizeof(unsigned int) * vcount);
ocount = 0; 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 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][1] = verts[v][1];
overts[ocount][2] = verts[v][2]; 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)) if (tmpIndices[k] == int(v))
m_vertexIndexMapping[k]=ocount; m_vertexIndexMapping[k] = ocount;
} }
ocount++; // increment output vert count ocount++; // increment output vert count
btAssert( ocount >=0 && ocount <= vcount ); btAssert(ocount >= 0 && ocount <= vcount);
usedIndices[static_cast<int>(v)] = ocount; // assign new index remapping usedIndices[static_cast<int>(v)] = ocount; // assign new index remapping
} }
} }
} }