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