com.bulletphysics.linearmath.convexhull.HullLibrary Maven / Gradle / Ivy
Go to download
Show more of this group Show more artifacts with this name
Show all versions of jbullet Show documentation
Show all versions of jbullet Show documentation
JBullet - Java port of Bullet Physics Library
The newest version!
/*
* Java port of Bullet (c) 2008 Martin Dvorak
*
* Stan Melax Convex Hull Computation
* Copyright (c) 2008 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.
*/
// includes modifications/improvements by John Ratcliff, see BringOutYourDead below.
package com.bulletphysics.linearmath.convexhull;
import com.bulletphysics.BulletGlobals;
import com.bulletphysics.collision.shapes.ShapeHull;
import com.bulletphysics.linearmath.MiscUtil;
import com.bulletphysics.linearmath.VectorUtil;
import com.bulletphysics.util.IntArrayList;
import com.bulletphysics.util.ObjectArrayList;
import cz.advel.stack.Stack;
import javax.vecmath.Vector3f;
/**
* HullLibrary class can create a convex hull from a collection of vertices, using
* the ComputeHull method. The {@link ShapeHull} class uses this HullLibrary to create
* a approximate convex mesh given a general (non-polyhedral) convex shape.
*
* @author jezek2
*/
public class HullLibrary {
public final IntArrayList vertexIndexMapping = new IntArrayList();
private ObjectArrayList tris = new ObjectArrayList();
/**
* Converts point cloud to polygonal representation.
*
* @param desc describes the input request
* @param result contains the result
* @return whether conversion was successful
*/
public boolean createConvexHull(HullDesc desc, HullResult result) {
boolean ret = false;
PHullResult hr = new PHullResult();
int vcount = desc.vcount;
if (vcount < 8) vcount = 8;
ObjectArrayList vertexSource = new ObjectArrayList();
MiscUtil.resize(vertexSource, vcount, Vector3f.class);
Vector3f scale = Stack.alloc(Vector3f.class);
int[] ovcount = new int[1];
boolean ok = cleanupVertices(desc.vcount, desc.vertices, desc.vertexStride, ovcount, vertexSource, desc.normalEpsilon, scale); // normalize point cloud, remove duplicates!
if (ok) {
// if ( 1 ) // scale vertices back to their original size.
{
for (int i=0; i vertexScratch = new ObjectArrayList();
MiscUtil.resize(vertexScratch, hr.vcount, Vector3f.class);
bringOutYourDead(hr.vertices, hr.vcount, vertexScratch, ovcount, hr.indices, hr.indexCount);
ret = true;
if (desc.hasHullFlag(HullFlags.TRIANGLES)) { // if he wants the results as triangle!
result.polygons = false;
result.numOutputVertices = ovcount[0];
MiscUtil.resize(result.outputVertices, ovcount[0], Vector3f.class);
result.numFaces = hr.faceCount;
result.numIndices = hr.indexCount;
MiscUtil.resize(result.indices, hr.indexCount, 0);
for (int i=0; i vertices, PHullResult result, int vlimit) {
int[] tris_count = new int[1];
int ret = calchull(vertices, vcount, result.indices, tris_count, vlimit);
if (ret == 0) return false;
result.indexCount = tris_count[0] * 3;
result.faceCount = tris_count[0];
result.vertices = vertices;
result.vcount = vcount;
return true;
}
private Tri allocateTriangle(int a, int b, int c) {
Tri tr = new Tri(a, b, c);
tr.id = tris.size();
tris.add(tr);
return tr;
}
private void deAllocateTriangle(Tri tri) {
assert (tris.getQuick(tri.id) == tri);
tris.setQuick(tri.id, null);
}
private void b2bfix(Tri s, Tri t) {
for (int i=0; i<3; i++) {
int i1 = (i + 1) % 3;
int i2 = (i + 2) % 3;
int a = s.getCoord(i1);
int b = s.getCoord(i2);
assert (tris.getQuick(s.neib(a, b).get()).neib(b, a).get() == s.id);
assert (tris.getQuick(t.neib(a, b).get()).neib(b, a).get() == t.id);
tris.getQuick(s.neib(a, b).get()).neib(b, a).set(t.neib(b, a).get());
tris.getQuick(t.neib(b, a).get()).neib(a, b).set(s.neib(a, b).get());
}
}
private void removeb2b(Tri s, Tri t) {
b2bfix(s, t);
deAllocateTriangle(s);
deAllocateTriangle(t);
}
private void checkit(Tri t) {
assert (tris.getQuick(t.id) == t);
for (int i=0; i<3; i++) {
int i1 = (i + 1) % 3;
int i2 = (i + 2) % 3;
int a = t.getCoord(i1);
int b = t.getCoord(i2);
assert (a != b);
assert (tris.getQuick(t.n.getCoord(i)).neib(b, a).get() == t.id);
}
}
private Tri extrudable(float epsilon) {
Tri t = null;
for (int i=0; i epsilon) ? t : null;
}
private int calchull(ObjectArrayList verts, int verts_count, IntArrayList tris_out, int[] tris_count, int vlimit) {
int rc = calchullgen(verts, verts_count, vlimit);
if (rc == 0) return 0;
IntArrayList ts = new IntArrayList();
for (int i=0; i verts, int verts_count, int vlimit) {
if (verts_count < 4) return 0;
Vector3f tmp = Stack.alloc(Vector3f.class);
Vector3f tmp1 = Stack.alloc(Vector3f.class);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
if (vlimit == 0) {
vlimit = 1000000000;
}
//int j;
Vector3f bmin = Stack.alloc((Vector3f) verts.getQuick(0));
Vector3f bmax = Stack.alloc((Vector3f) verts.getQuick(0));
IntArrayList isextreme = new IntArrayList();
//isextreme.reserve(verts_count);
IntArrayList allow = new IntArrayList();
//allow.reserve(verts_count);
for (int j=0; j 0 && ((te = extrudable(epsilon)) != null)) {
Int3 ti = te;
int v = te.vmax;
assert (v != -1);
assert (isextreme.get(v) == 0); // wtf we've already done this vertex
isextreme.set(v, 1);
//if(v==p0 || v==p1 || v==p2 || v==p3) continue; // done these already
int j = tris.size();
while ((j--) != 0) {
if (tris.getQuick(j) == null) {
continue;
}
Int3 t = tris.getQuick(j);
if (above(verts, t, verts.getQuick(v), 0.01f * epsilon)) {
extrude(tris.getQuick(j), v);
}
}
// now check for those degenerate cases where we have a flipped triangle or a really skinny triangle
j = tris.size();
while ((j--) != 0) {
if (tris.getQuick(j) == null) {
continue;
}
if (!hasvert(tris.getQuick(j), v)) {
break;
}
Int3 nt = tris.getQuick(j);
tmp1.sub(verts.getQuick(nt.getCoord(1)), verts.getQuick(nt.getCoord(0)));
tmp2.sub(verts.getQuick(nt.getCoord(2)), verts.getQuick(nt.getCoord(1)));
tmp.cross(tmp1, tmp2);
if (above(verts, nt, center, 0.01f * epsilon) || tmp.length() < epsilon * epsilon * 0.1f) {
Tri nb = tris.getQuick(tris.getQuick(j).n.getCoord(0));
assert (nb != null);
assert (!hasvert(nb, v));
assert (nb.id < j);
extrude(nb, v);
j = tris.size();
}
}
j = tris.size();
while ((j--) != 0) {
Tri t = tris.getQuick(j);
if (t == null) {
continue;
}
if (t.vmax >= 0) {
break;
}
triNormal(verts.getQuick(t.getCoord(0)), verts.getQuick(t.getCoord(1)), verts.getQuick(t.getCoord(2)), n);
t.vmax = maxdirsterid(verts, verts_count, n, allow);
if (isextreme.get(t.vmax) != 0) {
t.vmax = -1; // already done that vertex - algorithm needs to be able to terminate.
}
else {
tmp.sub(verts.getQuick(t.vmax), verts.getQuick(t.getCoord(0)));
t.rise = n.dot(tmp);
}
}
vlimit--;
}
return 1;
}
private Int4 findSimplex(ObjectArrayList verts, int verts_count, IntArrayList allow, Int4 out) {
Vector3f tmp = Stack.alloc(Vector3f.class);
Vector3f tmp1 = Stack.alloc(Vector3f.class);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
Vector3f[] basis = new Vector3f[/*3*/] { Stack.alloc(Vector3f.class), Stack.alloc(Vector3f.class), Stack.alloc(Vector3f.class) };
basis[0].set(0.01f, 0.02f, 1.0f);
int p0 = maxdirsterid(verts, verts_count, basis[0], allow);
tmp.negate(basis[0]);
int p1 = maxdirsterid(verts, verts_count, tmp, allow);
basis[0].sub(verts.getQuick(p0), verts.getQuick(p1));
if (p0 == p1 || (basis[0].x == 0f && basis[0].y == 0f && basis[0].z == 0f)) {
out.set(-1, -1, -1, -1);
return out;
}
tmp.set(1f, 0.02f, 0f);
basis[1].cross(tmp, basis[0]);
tmp.set(-0.02f, 1f, 0f);
basis[2].cross(tmp, basis[0]);
if (basis[1].length() > basis[2].length()) {
basis[1].normalize();
}
else {
basis[1].set(basis[2]);
basis[1].normalize();
}
int p2 = maxdirsterid(verts, verts_count, basis[1], allow);
if (p2 == p0 || p2 == p1) {
tmp.negate(basis[1]);
p2 = maxdirsterid(verts, verts_count, tmp, allow);
}
if (p2 == p0 || p2 == p1) {
out.set(-1, -1, -1, -1);
return out;
}
basis[1].sub(verts.getQuick(p2), verts.getQuick(p0));
basis[2].cross(basis[1], basis[0]);
basis[2].normalize();
int p3 = maxdirsterid(verts, verts_count, basis[2], allow);
if (p3 == p0 || p3 == p1 || p3 == p2) {
tmp.negate(basis[2]);
p3 = maxdirsterid(verts, verts_count, tmp, allow);
}
if (p3 == p0 || p3 == p1 || p3 == p2) {
out.set(-1, -1, -1, -1);
return out;
}
assert (!(p0 == p1 || p0 == p2 || p0 == p3 || p1 == p2 || p1 == p3 || p2 == p3));
tmp1.sub(verts.getQuick(p1), verts.getQuick(p0));
tmp2.sub(verts.getQuick(p2), verts.getQuick(p0));
tmp2.cross(tmp1, tmp2);
tmp1.sub(verts.getQuick(p3), verts.getQuick(p0));
if (tmp1.dot(tmp2) < 0) {
int swap_tmp = p2;
p2 = p3;
p3 = swap_tmp;
}
out.set(p0, p1, p2, p3);
return out;
}
//private ConvexH convexHCrop(ConvexH convex,Plane slice);
private void extrude(Tri t0, int v) {
Int3 t = new Int3(t0);
int n = tris.size();
Tri ta = allocateTriangle(v, t.getCoord(1), t.getCoord(2));
ta.n.set(t0.n.getCoord(0), n + 1, n + 2);
tris.getQuick(t0.n.getCoord(0)).neib(t.getCoord(1), t.getCoord(2)).set(n + 0);
Tri tb = allocateTriangle(v, t.getCoord(2), t.getCoord(0));
tb.n.set(t0.n.getCoord(1), n + 2, n + 0);
tris.getQuick(t0.n.getCoord(1)).neib(t.getCoord(2), t.getCoord(0)).set(n + 1);
Tri tc = allocateTriangle(v, t.getCoord(0), t.getCoord(1));
tc.n.set(t0.n.getCoord(2), n + 0, n + 1);
tris.getQuick(t0.n.getCoord(2)).neib(t.getCoord(0), t.getCoord(1)).set(n + 2);
checkit(ta);
checkit(tb);
checkit(tc);
if (hasvert(tris.getQuick(ta.n.getCoord(0)), v)) {
removeb2b(ta, tris.getQuick(ta.n.getCoord(0)));
}
if (hasvert(tris.getQuick(tb.n.getCoord(0)), v)) {
removeb2b(tb, tris.getQuick(tb.n.getCoord(0)));
}
if (hasvert(tris.getQuick(tc.n.getCoord(0)), v)) {
removeb2b(tc, tris.getQuick(tc.n.getCoord(0)));
}
deAllocateTriangle(t0);
}
//private ConvexH test_cube();
//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.
private void bringOutYourDead(ObjectArrayList verts, int vcount, ObjectArrayList overts, int[] ocount, IntArrayList indices, int indexcount) {
IntArrayList tmpIndices = new IntArrayList();
for (int i=0; i= 0 && v < vcount);
if (usedIndices.get(v) != 0) { // if already remapped
indices.set(i, usedIndices.get(v) - 1); // index to new array
}
else {
indices.set(i, ocount[0]); // new index mapping
overts.getQuick(ocount[0]).set(verts.getQuick(v)); // copy old vert to new vert array
for (int k = 0; k < vertexIndexMapping.size(); k++) {
if (tmpIndices.get(k) == v) {
vertexIndexMapping.set(k, ocount[0]);
}
}
ocount[0]++; // increment output vert count
assert (ocount[0] >= 0 && ocount[0] <= vcount);
usedIndices.set(v, ocount[0]); // assign new index remapping
}
}
}
private static final float EPSILON = 0.000001f; /* close enough to consider two btScalaring point numbers to be 'the same'. */
private boolean cleanupVertices(int svcount,
ObjectArrayList svertices,
int stride,
int[] vcount, // output number of vertices
ObjectArrayList vertices, // location to store the results.
float normalepsilon,
Vector3f scale) {
if (svcount == 0) {
return false;
}
vertexIndexMapping.clear();
vcount[0] = 0;
float[] recip = new float[3];
if (scale != null) {
scale.set(1, 1, 1);
}
float[] bmin = new float[] { Float.MAX_VALUE, Float.MAX_VALUE, Float.MAX_VALUE };
float[] bmax = new float[] { -Float.MAX_VALUE, -Float.MAX_VALUE, -Float.MAX_VALUE };
ObjectArrayList vtx_ptr = svertices;
int vtx_idx = 0;
// if ( 1 )
{
for (int i=0; i bmax[j]) {
bmax[j] = VectorUtil.getCoord(p, j);
}
}
}
}
float dx = bmax[0] - bmin[0];
float dy = bmax[1] - bmin[1];
float dz = bmax[2] - bmin[2];
Vector3f center = Stack.alloc(Vector3f.class);
center.x = dx * 0.5f + bmin[0];
center.y = dy * 0.5f + bmin[1];
center.z = dz * 0.5f + bmin[2];
if (dx < EPSILON || dy < EPSILON || dz < EPSILON || svcount < 3) {
float len = Float.MAX_VALUE;
if (dx > EPSILON && dx < len) len = dx;
if (dy > EPSILON && dy < len) len = dy;
if (dz > EPSILON && dz < len) len = dz;
if (len == Float.MAX_VALUE) {
dx = dy = dz = 0.01f; // 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;
}
float x1 = center.x - dx;
float x2 = center.x + dx;
float y1 = center.y - dy;
float y2 = center.y + dy;
float z1 = center.z - dz;
float z2 = center.z + 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 != null) {
scale.x = dx;
scale.y = dy;
scale.z = dz;
recip[0] = 1f / dx;
recip[1] = 1f / dy;
recip[2] = 1f / dz;
center.x *= recip[0];
center.y *= recip[1];
center.z *= recip[2];
}
}
vtx_ptr = svertices;
vtx_idx = 0;
for (int i=0; i dist2) {
v.x = px;
v.y = py;
v.z = pz;
}
break;
}
}
if (j == vcount[0]) {
Vector3f dest = vertices.getQuick(vcount[0]);
dest.x = px;
dest.y = py;
dest.z = pz;
vcount[0]++;
}
vertexIndexMapping.add(j);
}
}
// ok..now make sure we didn't prune so many vertices it is now invalid.
// if ( 1 )
{
bmin = new float[] { Float.MAX_VALUE, Float.MAX_VALUE, Float.MAX_VALUE };
bmax = new float[] { -Float.MAX_VALUE, -Float.MAX_VALUE, -Float.MAX_VALUE };
for (int i=0; i bmax[j]) {
bmax[j] = VectorUtil.getCoord(p, j);
}
}
}
dx = bmax[0] - bmin[0];
dy = bmax[1] - bmin[1];
dz = bmax[2] - bmin[2];
if (dx < EPSILON || dy < EPSILON || dz < EPSILON || vcount[0] < 3) {
float cx = dx * 0.5f + bmin[0];
float cy = dy * 0.5f + bmin[1];
float cz = dz * 0.5f + bmin[2];
float len = Float.MAX_VALUE;
if (dx >= EPSILON && dx < len) len = dx;
if (dy >= EPSILON && dy < len) len = dy;
if (dz >= EPSILON && dz < len) len = dz;
if (len == Float.MAX_VALUE) {
dx = dy = dz = 0.01f; // 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;
}
float x1 = cx - dx;
float x2 = cx + dx;
float y1 = cy - dy;
float y2 = cy + dy;
float z1 = cz - dz;
float z2 = cz + dz;
vcount[0] = 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;
}
////////////////////////////////////////////////////////////////////////////
private static boolean hasvert(Int3 t, int v) {
return (t.getCoord(0) == v || t.getCoord(1) == v || t.getCoord(2) == v);
}
private static Vector3f orth(Vector3f v, Vector3f out) {
Vector3f a = Stack.alloc(Vector3f.class);
a.set(0f, 0f, 1f);
a.cross(v, a);
Vector3f b = Stack.alloc(Vector3f.class);
b.set(0f, 1f, 0f);
b.cross(v, b);
if (a.length() > b.length()) {
out.normalize(a);
return out;
}
else {
out.normalize(b);
return out;
}
}
private static int maxdirfiltered(ObjectArrayList p, int count, Vector3f dir, IntArrayList allow) {
assert (count != 0);
int m = -1;
for (int i=0; i p.getQuick(m).dot(dir)) {
m = i;
}
}
}
assert (m != -1);
return m;
}
private static int maxdirsterid(ObjectArrayList p, int count, Vector3f dir, IntArrayList allow) {
Vector3f tmp = Stack.alloc(Vector3f.class);
Vector3f tmp1 = Stack.alloc(Vector3f.class);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
Vector3f u = Stack.alloc(Vector3f.class);
Vector3f v = Stack.alloc(Vector3f.class);
int m = -1;
while (m == -1) {
m = maxdirfiltered(p, count, dir, allow);
if (allow.get(m) == 3) {
return m;
}
orth(dir, u);
v.cross(u, dir);
int ma = -1;
for (float x = 0f; x <= 360f; x += 45f) {
float s = (float) Math.sin(BulletGlobals.SIMD_RADS_PER_DEG * (x));
float c = (float) Math.cos(BulletGlobals.SIMD_RADS_PER_DEG * (x));
tmp1.scale(s, u);
tmp2.scale(c, v);
tmp.add(tmp1, tmp2);
tmp.scale(0.025f);
tmp.add(dir);
int mb = maxdirfiltered(p, count, tmp, allow);
if (ma == m && mb == m) {
allow.set(m, 3);
return m;
}
if (ma != -1 && ma != mb) { // Yuck - this is really ugly
int mc = ma;
for (float xx = x - 40f; xx <= x; xx += 5f) {
s = (float)Math.sin(BulletGlobals.SIMD_RADS_PER_DEG * (xx));
c = (float)Math.cos(BulletGlobals.SIMD_RADS_PER_DEG * (xx));
tmp1.scale(s, u);
tmp2.scale(c, v);
tmp.add(tmp1, tmp2);
tmp.scale(0.025f);
tmp.add(dir);
int md = maxdirfiltered(p, count, tmp, allow);
if (mc == m && md == m) {
allow.set(m, 3);
return m;
}
mc = md;
}
}
ma = mb;
}
allow.set(m, 0);
m = -1;
}
assert (false);
return m;
}
private static Vector3f triNormal(Vector3f v0, Vector3f v1, Vector3f v2, Vector3f out) {
Vector3f tmp1 = Stack.alloc(Vector3f.class);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
// return the normal of the triangle
// inscribed by v0, v1, and v2
tmp1.sub(v1, v0);
tmp2.sub(v2, v1);
Vector3f cp = Stack.alloc(Vector3f.class);
cp.cross(tmp1, tmp2);
float m = cp.length();
if (m == 0) {
out.set(1f, 0f, 0f);
return out;
}
out.scale(1f / m, cp);
return out;
}
private static boolean above(ObjectArrayList vertices, Int3 t, Vector3f p, float epsilon) {
Vector3f n = triNormal(vertices.getQuick(t.getCoord(0)), vertices.getQuick(t.getCoord(1)), vertices.getQuick(t.getCoord(2)), Stack.alloc(Vector3f.class));
Vector3f tmp = Stack.alloc(Vector3f.class);
tmp.sub(p, vertices.getQuick(t.getCoord(0)));
return (n.dot(tmp) > epsilon); // EPSILON???
}
private static void releaseHull(PHullResult result) {
if (result.indices.size() != 0) {
result.indices.clear();
}
result.vcount = 0;
result.indexCount = 0;
result.vertices = null;
}
private static void addPoint(int[] vcount, ObjectArrayList p, float x, float y, float z) {
// XXX, might be broken
Vector3f dest = p.getQuick(vcount[0]);
dest.x = x;
dest.y = y;
dest.z = z;
vcount[0]++;
}
private static float getDist(float px, float py, float pz, Vector3f p2) {
float dx = px - p2.x;
float dy = py - p2.y;
float dz = pz - p2.z;
return dx*dx + dy*dy + dz*dz;
}
}