diff --git a/src/BulletCollision/CollisionShapes/btShapeHull.cpp b/src/BulletCollision/CollisionShapes/btShapeHull.cpp index b760431b2..837eb8949 100644 --- a/src/BulletCollision/CollisionShapes/btShapeHull.cpp +++ b/src/BulletCollision/CollisionShapes/btShapeHull.cpp @@ -111,19 +111,10 @@ btShapeHull::buildHull (btScalar margin) hd.mVcount = numSampleDirections; #ifdef BT_USE_DOUBLE_PRECISION - float* tmpVerts = new float[numSampleDirections*3]; - - for (i=0;i -#include #include -#include -#include -#include #include "btConvexHull.h" #include "LinearMath/btAlignedObjectArray.h" - +#include "LinearMath/btMinMax.h" #include "LinearMath/btVector3.h" -#define PI 3.14159264f - - - -#define DEG2RAD (PI / 180.0f) -#define RAD2DEG (180.0f / PI) -#define SQRT_OF_2 (1.4142135f) -#define OFFSET(Class,Member) (((char*) (&(((Class*)NULL)-> Member )))- ((char*)NULL)) - - - -int argmin(float a[],int n); -float sqr(float a); -float clampf(float a) ; -float Round(float a,float precision); -float Interpolate(const float &f0,const float &f1,float alpha) ; - template void Swap(T &a,T &b) { @@ -51,18 +29,6 @@ void Swap(T &a,T &b) } -template -T Max(const T &a,const T &b) -{ - return (a>b)?a:b; -} - -template -T Min(const T &a,const T &b) -{ - return (a=0&&i<3);return ((float*)this)[i];} - const float& operator[](int i) const {assert(i>=0&&i<3);return ((float*)this)[i];} -# ifdef PLUGIN_3DSMAX - float3(const Point3 &p):x(p.x),y(p.y),z(p.z){} - operator Point3(){return *((Point3*)this);} -# endif -}; - - -float3& operator+=( float3 &a, const float3& b ); -float3& operator-=( float3 &a ,const float3& b ); -float3& operator*=( float3 &v ,const float s ); -float3& operator/=( float3 &v, const float s ); - -float magnitude( const float3& v ); -float3 normalize( const float3& v ); -float3 safenormalize(const float3 &v); -float3 vabs(const float3 &v); -float3 operator+( const float3& a, const float3& b ); -float3 operator-( const float3& a, const float3& b ); -float3 operator-( const float3& v ); -float3 operator*( const float3& v, const float s ); -float3 operator*( const float s, const float3& v ); -float3 operator/( const float3& v, const float s ); -inline int operator==( const float3 &a, const float3 &b ) { return (a.x==b.x && a.y==b.y && a.z==b.z); } -inline int operator!=( const float3 &a, const float3 &b ) { return (a.x!=b.x || a.y!=b.y || a.z!=b.z); } -// due to ambiguity and inconsistent standards ther are no overloaded operators for mult such as va*vb. -float dot( const float3& a, const float3& b ); -float3 cmul( const float3 &a, const float3 &b); -float3 cross( const float3& a, const float3& b ); -float3 Interpolate(const float3 &v0,const float3 &v1,float alpha); -float3 Round(const float3& a,float precision); -float3 VectorMax(const float3 &a, const float3 &b); -float3 VectorMin(const float3 &a, const float3 &b); - - //------- Plane ---------- class Plane { public: - float3 normal; - float dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0 - Plane(const float3 &n,float d):normal(n),dist(d){} + btVector3 normal; + btScalar dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0 + Plane(const btVector3 &n,btScalar d):normal(n),dist(d){} Plane():normal(),dist(0){} }; @@ -143,15 +61,14 @@ inline int coplanar( const Plane &a, const Plane &b ) { return (a==b || a==Plane //--------- Utility Functions ------ -float3 PlaneLineIntersection(const Plane &plane, const float3 &p0, const float3 &p1); -float3 PlaneProject(const Plane &plane, const float3 &point); -float3 LineProject(const float3 &p0, const float3 &p1, const float3 &a); // projects a onto infinite line p0p1 -float LineProjectTime(const float3 &p0, const float3 &p1, const float3 &a); -float3 ThreePlaneIntersection(const Plane &p0,const Plane &p1, const Plane &p2) +btVector3 PlaneLineIntersection(const Plane &plane, const btVector3 &p0, const btVector3 &p1); +btVector3 PlaneProject(const Plane &plane, const btVector3 &point); + +btVector3 ThreePlaneIntersection(const Plane &p0,const Plane &p1, const Plane &p2) { - btVector3 N1(p0.normal.x,p0.normal.y,p0.normal.z); - btVector3 N2(p1.normal.x,p1.normal.y,p1.normal.z); - btVector3 N3(p2.normal.x,p2.normal.y,p2.normal.z); + btVector3 N1 = p0.normal; + btVector3 N2 = p1.normal; + btVector3 N3 = p2.normal; btVector3 n2n3; n2n3 = N2.cross(N3); btVector3 n3n1; n3n1 = N3.cross(N1); @@ -170,395 +87,61 @@ float3 ThreePlaneIntersection(const Plane &p0,const Plane &p1, const Plane &p2) potentialVertex += n1n2; potentialVertex *= quotient; - float3 result(potentialVertex.getX(),potentialVertex.getY(),potentialVertex.getZ()); + btVector3 result(potentialVertex.getX(),potentialVertex.getY(),potentialVertex.getZ()); return result; } -int BoxInside(const float3 &p,const float3 &bmin, const float3 &bmax) ; -int BoxIntersect(const float3 &v0, const float3 &v1, const float3 &bmin, const float3 &bmax, float3 *impact); -float DistanceBetweenLines(const float3 &ustart, const float3 &udir, const float3 &vstart, const float3 &vdir, float3 *upoint=NULL, float3 *vpoint=NULL); -float3 TriNormal(const float3 &v0, const float3 &v1, const float3 &v2); -float3 NormalOf(const float3 *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); -float sqr(float a) {return a*a;} -float clampf(float a) {return Min(1.0f,Max(0.0f,a));} - - -float Round(float a,float precision) -{ - return floorf(0.5f+a/precision)*precision; -} - - -float Interpolate(const float &f0,const float &f1,float alpha) -{ - return f0*(1-alpha) + f1*alpha; -} - - -int argmin(float a[],int n) -{ - int r=0; - for(int i=1;i= bmin.x && p.x <=bmax.x && - p.y >= bmin.y && p.y <=bmax.y && - p.z >= bmin.z && p.z <=bmax.z ); -} + static btVector3 cp; + cp = cross(udir,vdir).normalized(); - -int BoxIntersect(const float3 &v0, const float3 &v1, const float3 &bmin, const float3 &bmax,float3 *impact) -{ - if(BoxInside(v0,bmin,bmax)) - { - *impact=v0; - return 1; - } - if(v0.x<=bmin.x && v1.x>=bmin.x) - { - float a = (bmin.x-v0.x)/(v1.x-v0.x); - //v.x = bmin.x; - float vy = (1-a) *v0.y + a*v1.y; - float vz = (1-a) *v0.z + a*v1.z; - if(vy>=bmin.y && vy<=bmax.y && vz>=bmin.z && vz<=bmax.z) - { - impact->x = bmin.x; - impact->y = vy; - impact->z = vz; - return 1; - } - } - else if(v0.x >= bmax.x && v1.x <= bmax.x) - { - float a = (bmax.x-v0.x)/(v1.x-v0.x); - //v.x = bmax.x; - float vy = (1-a) *v0.y + a*v1.y; - float vz = (1-a) *v0.z + a*v1.z; - if(vy>=bmin.y && vy<=bmax.y && vz>=bmin.z && vz<=bmax.z) - { - impact->x = bmax.x; - impact->y = vy; - impact->z = vz; - return 1; - } - } - if(v0.y<=bmin.y && v1.y>=bmin.y) - { - float a = (bmin.y-v0.y)/(v1.y-v0.y); - float vx = (1-a) *v0.x + a*v1.x; - //v.y = bmin.y; - float vz = (1-a) *v0.z + a*v1.z; - if(vx>=bmin.x && vx<=bmax.x && vz>=bmin.z && vz<=bmax.z) - { - impact->x = vx; - impact->y = bmin.y; - impact->z = vz; - return 1; - } - } - else if(v0.y >= bmax.y && v1.y <= bmax.y) - { - float a = (bmax.y-v0.y)/(v1.y-v0.y); - float vx = (1-a) *v0.x + a*v1.x; - // vy = bmax.y; - float vz = (1-a) *v0.z + a*v1.z; - if(vx>=bmin.x && vx<=bmax.x && vz>=bmin.z && vz<=bmax.z) - { - impact->x = vx; - impact->y = bmax.y; - impact->z = vz; - return 1; - } - } - if(v0.z<=bmin.z && v1.z>=bmin.z) - { - float a = (bmin.z-v0.z)/(v1.z-v0.z); - float vx = (1-a) *v0.x + a*v1.x; - float vy = (1-a) *v0.y + a*v1.y; - // v.z = bmin.z; - if(vy>=bmin.y && vy<=bmax.y && vx>=bmin.x && vx<=bmax.x) - { - impact->x = vx; - impact->y = vy; - impact->z = bmin.z; - return 1; - } - } - else if(v0.z >= bmax.z && v1.z <= bmax.z) - { - float a = (bmax.z-v0.z)/(v1.z-v0.z); - float vx = (1-a) *v0.x + a*v1.x; - float vy = (1-a) *v0.y + a*v1.y; - // v.z = bmax.z; - if(vy>=bmin.y && vy<=bmax.y && vx>=bmin.x && vx<=bmax.x) - { - impact->x = vx; - impact->y = vy; - impact->z = bmax.z; - return 1; - } - } - return 0; -} - - -float DistanceBetweenLines(const float3 &ustart, const float3 &udir, const float3 &vstart, const float3 &vdir, float3 *upoint, float3 *vpoint) -{ - static float3 cp; - cp = normalize(cross(udir,vdir)); - - float distu = -dot(cp,ustart); - float distv = -dot(cp,vstart); - float dist = (float)fabs(distu-distv); + btScalar distu = -dot(cp,ustart); + btScalar distv = -dot(cp,vstart); + btScalar dist = (btScalar)fabs(distu-distv); if(upoint) { Plane plane; - plane.normal = normalize(cross(vdir,cp)); + plane.normal = cross(vdir,cp).normalized(); plane.dist = -dot(plane.normal,vstart); *upoint = PlaneLineIntersection(plane,ustart,ustart+udir); } if(vpoint) { Plane plane; - plane.normal = normalize(cross(udir,cp)); + plane.normal = cross(udir,cp).normalized(); plane.dist = -dot(plane.normal,ustart); *vpoint = PlaneLineIntersection(plane,vstart,vstart+vdir); } @@ -584,21 +167,21 @@ public: unsigned int mVcount; unsigned int mIndexCount; unsigned int mFaceCount; - float *mVertices; + btVector3* mVertices; unsigned int *mIndices; }; -#define REAL3 float3 -#define REAL float +#define REAL3 btVector3 +#define REAL btScalar #define COPLANAR (0) #define UNDER (1) #define OVER (2) #define SPLIT (OVER|UNDER) -#define PAPERWIDTH (0.001f) +#define PAPERWIDTH (btScalar(0.001)) -float planetestepsilon = PAPERWIDTH; +btScalar planetestepsilon = PAPERWIDTH; class ConvexH @@ -810,21 +393,21 @@ ConvexH *test_cube() { } ConvexH *ConvexHMakeCube(const REAL3 &bmin, const REAL3 &bmax) { ConvexH *convex = test_cube(); - convex->vertices[0] = REAL3(bmin.x,bmin.y,bmin.z); - convex->vertices[1] = REAL3(bmin.x,bmin.y,bmax.z); - convex->vertices[2] = REAL3(bmin.x,bmax.y,bmin.z); - convex->vertices[3] = REAL3(bmin.x,bmax.y,bmax.z); - convex->vertices[4] = REAL3(bmax.x,bmin.y,bmin.z); - convex->vertices[5] = REAL3(bmax.x,bmin.y,bmax.z); - convex->vertices[6] = REAL3(bmax.x,bmax.y,bmin.z); - convex->vertices[7] = REAL3(bmax.x,bmax.y,bmax.z); + convex->vertices[0] = REAL3(bmin.getX(),bmin.getY(),bmin.getZ()); + convex->vertices[1] = REAL3(bmin.getX(),bmin.getY(),bmax.getZ()); + convex->vertices[2] = REAL3(bmin.getX(),bmax.getY(),bmin.getZ()); + convex->vertices[3] = REAL3(bmin.getX(),bmax.getY(),bmax.getZ()); + convex->vertices[4] = REAL3(bmax.getX(),bmin.getY(),bmin.getZ()); + convex->vertices[5] = REAL3(bmax.getX(),bmin.getY(),bmax.getZ()); + convex->vertices[6] = REAL3(bmax.getX(),bmax.getY(),bmin.getZ()); + convex->vertices[7] = REAL3(bmax.getX(),bmax.getY(),bmax.getZ()); - convex->facets[0] = Plane(REAL3(-1,0,0), bmin.x); - convex->facets[1] = Plane(REAL3(1,0,0), -bmax.x); - convex->facets[2] = Plane(REAL3(0,-1,0), bmin.y); - convex->facets[3] = Plane(REAL3(0,1,0), -bmax.y); - convex->facets[4] = Plane(REAL3(0,0,-1), bmin.z); - convex->facets[5] = Plane(REAL3(0,0,1), -bmax.z); + convex->facets[0] = Plane(REAL3(-1,0,0), bmin.getX()); + convex->facets[1] = Plane(REAL3(1,0,0), -bmax.getX()); + convex->facets[2] = Plane(REAL3(0,-1,0), bmin.getY()); + convex->facets[3] = Plane(REAL3(0,1,0), -bmax.getY()); + convex->facets[4] = Plane(REAL3(0,0,-1), bmin.getZ()); + convex->facets[5] = Plane(REAL3(0,0,1), -bmax.getZ()); return convex; } ConvexH *ConvexHCrop(ConvexH &convex,const Plane &slice) @@ -1136,7 +719,7 @@ ConvexH *ConvexHCrop(ConvexH &convex,const Plane &slice) -static int candidateplane(Plane *planes,int planes_count,ConvexH *convex,float epsilon) +static int candidateplane(Plane *planes,int planes_count,ConvexH *convex,btScalar epsilon) { int p = 0 ; REAL md= 0 ; @@ -1146,7 +729,7 @@ static int candidateplane(Plane *planes,int planes_count,ConvexH *convex,float e REAL d=0; for(int j=0;jvertices.size();j++) { - d = Max(d,dot(convex->vertices[j],planes[i].normal)+planes[i].dist); + d = btMax(d,dot(convex->vertices[j],planes[i].normal)+planes[i].dist); } if(i==0 || d>md) { @@ -1175,19 +758,26 @@ int maxdirfiltered(const T *p,int count,const T &dir,btAlignedObjectArray & { assert(count); int m=-1; - for(int i=0;idot(p[m],dir)) m=i; - } + for(int i=0;idot(p[m],dir)) + m=i; + } assert(m!=-1); return m; } -float3 orth(const float3 &v) +btVector3 orth(const btVector3 &v) { - float3 a=cross(v,float3(0,0,1)); - float3 b=cross(v,float3(0,1,0)); - return normalize((magnitude(a)>magnitude(b))?a:b); + btVector3 a=cross(v,btVector3(0,0,1)); + btVector3 b=cross(v,btVector3(0,1,0)); + if (a.length() > b.length()) + { + return a.normalized(); + } else { + return b.normalized(); + } } @@ -1202,11 +792,11 @@ int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray &al T u = orth(dir); T v = cross(u,dir); int ma=-1; - for(float x = 0.0f ; x<= 360.0f ; x+= 45.0f) + for(btScalar x = btScalar(0.0) ; x<= btScalar(360.0) ; x+= btScalar(45.0)) { - float s = sinf(DEG2RAD*(x)); - float c = cosf(DEG2RAD*(x)); - int mb = maxdirfiltered(p,count,dir+(u*s+v*c)*0.025f,allow); + btScalar s = sinf(SIMD_RADS_PER_DEG*(x)); + btScalar c = cosf(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; @@ -1215,11 +805,11 @@ int maxdirsterid(const T *p,int count,const T &dir,btAlignedObjectArray &al if(ma!=-1 && ma!=mb) // Yuck - this is really ugly { int mc = ma; - for(float xx = x-40.0f ; xx <= x ; xx+= 5.0f) + for(btScalar xx = x-btScalar(40.0) ; xx <= x ; xx+= btScalar(5.0)) { - float s = sinf(DEG2RAD*(xx)); - float c = cosf(DEG2RAD*(xx)); - int md = maxdirfiltered(p,count,dir+(u*s+v*c)*0.025f,allow); + btScalar s = sinf(SIMD_RADS_PER_DEG*(xx)); + btScalar c = cosf(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; @@ -1265,9 +855,9 @@ int b2b(const int3 &a,const int3 &b) { return isa(a,int3(b[2],b[1],b[0])); } -int above(float3* vertices,const int3& t, const float3 &p, float epsilon) +int above(btVector3* vertices,const int3& t, const btVector3 &p, btScalar epsilon) { - float3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]); + btVector3 n=TriNormal(vertices[t[0]],vertices[t[1]],vertices[t[2]]); return (dot(n,p-vertices[t[0]]) > epsilon); // EPSILON??? } int hasedge(const int3 &t, int a,int b) @@ -1304,13 +894,13 @@ public: int3 n; int id; int vmax; - float rise; + btScalar rise; Tri(int a,int b,int c):int3(a,b,c),n(-1,-1,-1) { id = tris.size(); tris.push_back(this); vmax=-1; - rise = 0.0f; + rise = btScalar(0.0); } ~Tri() { @@ -1395,7 +985,7 @@ void extrude(Tri *t0,int v) } -Tri *extrudable(float epsilon) +Tri *extrudable(btScalar epsilon) { int i; Tri *t=NULL; @@ -1421,18 +1011,24 @@ public: -int4 FindSimplex(float3 *verts,int verts_count,btAlignedObjectArray &allow) +int4 FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray &allow) { - float3 basis[3]; - basis[0] = float3( 0.01f, 0.02f, 1.0f ); + 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]==float3(0,0,0)) + if(p0==p1 || basis[0]==btVector3(0,0,0)) return int4(-1,-1,-1,-1); - basis[1] = cross(float3( 1, 0.02f, 0),basis[0]); - basis[2] = cross(float3(-0.02f, 1, 0),basis[0]); - basis[1] = normalize( (magnitude(basis[1])>magnitude(basis[2])) ? basis[1]:basis[2]); + basis[1] = cross(btVector3( btScalar(1),btScalar(0.02), btScalar(0)),basis[0]); + basis[2] = cross(btVector3(btScalar(-0.02), btScalar(1), btScalar(0)),basis[0]); + if (basis[1].length() > basis[2].length()) + { + basis[1].normalize(); + } else { + basis[1] = basis[2]; + basis[1].normalize (); + } int p2 = maxdirsterid(verts,verts_count,basis[1],allow); if(p2 == p0 || p2 == p1) { @@ -1441,7 +1037,7 @@ int4 FindSimplex(float3 *verts,int verts_count,btAlignedObjectArray &allow) if(p2 == p0 || p2 == p1) return int4(-1,-1,-1,-1); basis[1] = verts[p2] - verts[p0]; - basis[2] = normalize(cross(basis[1],basis[0])); + basis[2] = cross(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) @@ -1451,12 +1047,12 @@ int4 FindSimplex(float3 *verts,int verts_count,btAlignedObjectArray &allow) return int4(p0,p1,p2,p3); } -int calchullgen(float3 *verts,int verts_count, int vlimit) +int calchullgen(btVector3 *verts,int verts_count, int vlimit) { if(verts_count <4) return 0; if(vlimit==0) vlimit=1000000000; int j; - float3 bmin(*verts),bmax(*verts); + btVector3 bmin(*verts),bmax(*verts); btAlignedObjectArray isextreme; isextreme.reserve(verts_count); btAlignedObjectArray allow; @@ -1466,10 +1062,11 @@ int calchullgen(float3 *verts,int verts_count, int vlimit) { allow.push_back(1); isextreme.push_back(0); - bmin = VectorMin(bmin,verts[j]); - bmax = VectorMax(bmax,verts[j]); + bmin.setMin (verts[j]); + bmax.setMax (verts[j]); } - float epsilon = magnitude(bmax-bmin) * 0.001f; + btScalar epsilon = (bmax-bmin).length() * btScalar(0.001); + btAssert (epsilon != 0.0); int4 p = FindSimplex(verts,verts_count,allow); @@ -1477,7 +1074,7 @@ int calchullgen(float3 *verts,int verts_count, int vlimit) - float3 center = (verts[p[0]]+verts[p[1]]+verts[p[2]]+verts[p[3]]) /4.0f; // 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 Tri *t0 = new Tri(p[2],p[3],p[1]); t0->n=int3(2,3,1); Tri *t1 = new Tri(p[3],p[2],p[0]); t1->n=int3(3,2,0); Tri *t2 = new Tri(p[0],p[1],p[3]); t2->n=int3(0,1,3); @@ -1490,7 +1087,7 @@ int calchullgen(float3 *verts,int verts_count, int vlimit) Tri *t=tris[j]; assert(t); assert(t->vmax<0); - float3 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->rise = dot(n,verts[t->vmax]-verts[(*t)[0]]); } @@ -1500,6 +1097,7 @@ int calchullgen(float3 *verts,int verts_count, int vlimit) { int3 ti=*te; int v=te->vmax; + assert(v != -1); assert(!isextreme[v]); // wtf we've already done this vertex isextreme[v]=1; //if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already @@ -1507,7 +1105,7 @@ int calchullgen(float3 *verts,int verts_count, int vlimit) while(j--) { if(!tris[j]) continue; int3 t=*tris[j]; - if(above(verts,t,verts[v],0.01f*epsilon)) + if(above(verts,t,verts[v],btScalar(0.01)*epsilon)) { extrude(tris[j],v); } @@ -1519,7 +1117,7 @@ int calchullgen(float3 *verts,int verts_count, int vlimit) if(!tris[j]) continue; if(!hasvert(*tris[j],v)) break; int3 nt=*tris[j]; - if(above(verts,nt,center,0.01f*epsilon) || magnitude(cross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]))< epsilon*epsilon*0.1f ) + if(above(verts,nt,center,btScalar(0.01)*epsilon) || cross(verts[nt[1]]-verts[nt[0]],verts[nt[2]]-verts[nt[1]]).length()< epsilon*epsilon*btScalar(0.1) ) { Tri *nb = tris[tris[j]->n[0]]; assert(nb);assert(!hasvert(*nb,v));assert(nb->idvmax>=0) break; - float3 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); if(isextreme[t->vmax]) { @@ -1549,7 +1147,7 @@ int calchullgen(float3 *verts,int verts_count, int vlimit) return 1; } -int calchull(float3 *verts,int verts_count, int *&tris_out, int &tris_count,int vlimit) +int calchull(btVector3 *verts,int verts_count, int *&tris_out, int &tris_count,int vlimit) { int rc=calchullgen(verts,verts_count, vlimit) ; if(!rc) return 0; @@ -1579,32 +1177,32 @@ int calchull(float3 *verts,int verts_count, int *&tris_out, int &tris_count,int -int overhull(Plane *planes,int planes_count,float3 *verts, int verts_count,int maxplanes, - float3 *&verts_out, int &verts_count_out, int *&faces_out, int &faces_count_out ,float inflate) +int overhull(Plane *planes,int planes_count,btVector3 *verts, int verts_count,int maxplanes, + btVector3 *&verts_out, int &verts_count_out, int *&faces_out, int &faces_count_out ,btScalar inflate) { int i,j; if(verts_count <4) return 0; - maxplanes = Min(maxplanes,planes_count); - float3 bmin(verts[0]),bmax(verts[0]); + maxplanes = btMin(maxplanes,planes_count); + btVector3 bmin(verts[0]),bmax(verts[0]); for(i=0;i=0) @@ -1638,11 +1236,11 @@ int overhull(Plane *planes,int planes_count,float3 *verts, int verts_count,int m faces_out[0]=k; // number of faces. assert(k==c->facets.size()); assert(faces_count_out == 1+c->facets.size()+c->edges.size()); - verts_out = new float3[c->vertices.size()]; + verts_out = new btVector3[c->vertices.size()]; verts_count_out = c->vertices.size(); for(i=0;ivertices.size();i++) { - verts_out[i] = float3(c->vertices[i]); + verts_out[i] = btVector3(c->vertices[i]); } c->vertices.resize(0); delete c; @@ -1651,7 +1249,7 @@ int overhull(Plane *planes,int planes_count,float3 *verts, int verts_count,int m -bool ComputeHull(unsigned int vcount,const float *vertices,PHullResult &result,unsigned int vlimit,float inflate) +bool ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit) { @@ -1659,11 +1257,11 @@ bool ComputeHull(unsigned int vcount,const float *vertices,PHullResult &result,u int *tris_out; int tris_count; - int ret = calchull( (float3 *) vertices, (int) vcount, tris_out, tris_count, vlimit ); + int ret = calchull( (btVector3 *) vertices, (int) vcount, tris_out, tris_count, vlimit ); if(!ret) return false; result.mIndexCount = (unsigned int) (tris_count*3); result.mFaceCount = (unsigned int) tris_count; - result.mVertices = (float*) vertices; + result.mVertices = (btVector3*) vertices; result.mVcount = (unsigned int) vcount; result.mIndices = (unsigned int *) tris_out; return true; @@ -1709,10 +1307,9 @@ HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // unsigned int vcount = desc.mVcount; if ( vcount < 8 ) vcount = 8; - float *vsource = (float *) malloc( sizeof(float)*vcount*3); + btVector3* vsource = (btVector3*) malloc (sizeof(btVector3)*vcount); - - float scale[3]; + btVector3 scale; unsigned int ovcount; @@ -1726,24 +1323,20 @@ HullError HullLibrary::CreateConvexHull(const HullDesc &desc, // { for (unsigned int i=0; i EPSILON && dx < len ) len = dx; if ( dy > EPSILON && dy < len ) len = dy; @@ -1939,23 +1533,23 @@ bool HullLibrary::CleanupVertices(unsigned int svcount, if ( len == FLT_MAX ) { - dx = dy = dz = 0.01f; // one centimeter + dx = dy = dz = btScalar(0.01); // one centimeter } else { - if ( dx < EPSILON ) dx = len * 0.05f; // 1/5th the shortest non-zero edge. - if ( dy < EPSILON ) dy = len * 0.05f; - if ( dz < EPSILON ) dz = len * 0.05f; + 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); } - float x1 = center[0] - dx; - float x2 = center[0] + dx; + btScalar x1 = center[0] - dx; + btScalar x2 = center[0] + dx; - float y1 = center[1] - dy; - float y2 = center[1] + dy; + btScalar y1 = center[1] - dy; + btScalar y2 = center[1] + dy; - float z1 = center[2] - dz; - float z2 = center[2] + dz; + btScalar z1 = center[2] - dz; + btScalar z2 = center[2] + dz; addPoint(vcount,vertices,x1,y1,z1); addPoint(vcount,vertices,x2,y1,z1); @@ -1996,13 +1590,12 @@ bool HullLibrary::CleanupVertices(unsigned int svcount, for (unsigned int i=0; igetX(); + btScalar py = p->getY(); + btScalar pz = p->getZ(); if ( scale ) { @@ -2017,15 +1610,16 @@ bool HullLibrary::CleanupVertices(unsigned int svcount, for (j=0; j dist2 ) { @@ -2049,7 +1643,7 @@ bool HullLibrary::CleanupVertices(unsigned int svcount, if ( j == vcount ) { - float *dest = &vertices[vcount*3]; + btVector3& dest = vertices[vcount]; dest[0] = px; dest[1] = py; dest[2] = pz; @@ -2061,12 +1655,12 @@ bool HullLibrary::CleanupVertices(unsigned int svcount, // ok..now make sure we didn't prune so many vertices it is now invalid. if ( 1 ) { - float bmin[3] = { FLT_MAX, FLT_MAX, FLT_MAX }; - float 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= EPSILON && dx < len ) len = dx; if ( dy >= EPSILON && dy < len ) len = dy; @@ -2092,23 +1686,23 @@ bool HullLibrary::CleanupVertices(unsigned int svcount, if ( len == FLT_MAX ) { - dx = dy = dz = 0.01f; // one centimeter + dx = dy = dz = btScalar(0.01); // one centimeter } else { - if ( dx < EPSILON ) dx = len * 0.05f; // 1/5th the shortest non-zero edge. - if ( dy < EPSILON ) dy = len * 0.05f; - if ( dz < EPSILON ) dz = len * 0.05f; + 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); } - float x1 = cx - dx; - float x2 = cx + dx; + btScalar x1 = cx - dx; + btScalar x2 = cx + dx; - float y1 = cy - dy; - float y2 = cy + dy; + btScalar y1 = cy - dy; + btScalar y2 = cy + dy; - float z1 = cz - dz; - float z2 = cz + dz; + btScalar z1 = cz - dz; + btScalar z2 = cz + dz; vcount = 0; // add box @@ -2128,7 +1722,7 @@ bool HullLibrary::CleanupVertices(unsigned int svcount, return true; } -void HullLibrary::BringOutYourDead(const float *verts,unsigned int vcount, float *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) { unsigned int *used = (unsigned int *)malloc(sizeof(unsigned int)*vcount); memset(used,0,sizeof(unsigned int)*vcount); @@ -2150,9 +1744,9 @@ void HullLibrary::BringOutYourDead(const float *verts,unsigned int vcount, float indices[i] = ocount; // new index mapping - overts[ocount*3+0] = verts[v*3+0]; // copy old vert to new vert array - overts[ocount*3+1] = verts[v*3+1]; - overts[ocount*3+2] = verts[v*3+2]; + 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]; ocount++; // increment output vert count diff --git a/src/LinearMath/btConvexHull.h b/src/LinearMath/btConvexHull.h index 7c7d173fd..6ea635efb 100644 --- a/src/LinearMath/btConvexHull.h +++ b/src/LinearMath/btConvexHull.h @@ -19,6 +19,8 @@ subject to the following restrictions: #ifndef CD_HULL_H #define CD_HULL_H +#include "LinearMath/btVector3.h" + class HullResult { public: @@ -33,7 +35,7 @@ public: } bool mPolygons; // true if indices represents polygons, false indices are triangles unsigned int mNumOutputVertices; // number of vertices in the output hull - float *mOutputVertices; // array of vertices, 3 floats each x,y,z + btVector3 *mOutputVertices; // array of vertices unsigned int mNumFaces; // the number of faces produced unsigned int mNumIndices; // the total number of indices unsigned int *mIndices; // pointer to indices. @@ -46,8 +48,7 @@ enum HullFlag { QF_TRIANGLES = (1<<0), // report results as triangles, not polygons. QF_REVERSE_ORDER = (1<<1), // reverse order of the triangle indices. - QF_SKIN_WIDTH = (1<<2), // extrude hull based on this skin width - QF_DEFAULT = 0 + QF_DEFAULT = QF_TRIANGLES }; @@ -59,25 +60,23 @@ public: mFlags = QF_DEFAULT; mVcount = 0; mVertices = 0; - mVertexStride = sizeof(float)*3; + mVertexStride = sizeof(btVector3); mNormalEpsilon = 0.001f; - mMaxVertices = 4096; // maximum number of points to be considered for a convex hull. - mMaxFaces = 4096; - mSkinWidth = 0.01f; // default is one centimeter + mMaxVertices = 4096; // maximum number of points to be considered for a convex hull. + mMaxFaces = 4096; }; HullDesc(HullFlag flag, - unsigned int vcount, - const float *vertices, - unsigned int stride) + unsigned int vcount, + const btVector3 *vertices, + unsigned int stride = sizeof(btVector3)) { mFlags = flag; mVcount = vcount; mVertices = vertices; mVertexStride = stride; - mNormalEpsilon = 0.001f; + mNormalEpsilon = btScalar(0.001); mMaxVertices = 4096; - mSkinWidth = 0.01f; // default is one centimeter } bool HasHullFlag(HullFlag flag) const @@ -98,11 +97,10 @@ public: unsigned int mFlags; // flags to use when generating the convex hull. unsigned int mVcount; // number of vertices in the input point cloud - const float *mVertices; // the array of vertices. + const btVector3 *mVertices; // the array of vertices. unsigned int mVertexStride; // the stride of each vertex, in bytes. - float mNormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on. - float mSkinWidth; - unsigned int mMaxVertices; // maximum number of vertices to be considered for the hull! + btScalar mNormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on. + unsigned int mMaxVertices; // maximum number of vertices to be considered for the hull! unsigned int mMaxFaces; }; @@ -116,26 +114,23 @@ class HullLibrary { public: - HullError CreateConvexHull(const HullDesc &desc, // describes the input request - HullResult &result); // contains the resulst - + HullError CreateConvexHull(const HullDesc& desc, // describes the input request + HullResult& result); // contains the resulst HullError ReleaseResult(HullResult &result); // release memory allocated for this result, we are done with it. - private: - //BringOutYourDead (John Ratcliff): When you create a convex hull you hand it a large input set of vertices forming a 'point cloud'. //After the hull is generated it give you back a set of polygon faces which index the *original* point cloud. //The thing is, often times, there are many 'dead vertices' in the point cloud that are on longer referenced by the hull. //The routine 'BringOutYourDead' find only the referenced vertices, copies them to an new buffer, and re-indexes the hull so that it is a minimal representation. - void BringOutYourDead(const float *verts,unsigned int vcount, float *overts,unsigned int &ocount,unsigned int *indices,unsigned indexcount); + void BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int* indices,unsigned indexcount); - bool CleanupVertices(unsigned int svcount, - const float *svertices, - unsigned int stride, - unsigned int &vcount, // output number of vertices - float *vertices, // location to store the results. - float normalepsilon, - float *scale); + bool 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); };