mirror of
https://github.com/godotengine/godot.git
synced 2025-11-04 07:31:16 +00:00
1120 lines
28 KiB
C++
1120 lines
28 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"
|
|
|
|
//----------------------------------
|
|
|
|
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
|
|
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)
|
|
{
|
|
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)
|
|
{
|
|
btSwap(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
|
|
}
|
|
}
|
|
}
|