/* * 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 = (Vector3f) Stack.alloc((Vector3f) verts.getQuick(0)); Vector3f bmax = (Vector3f) 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; } }