/*
|
* Java port of Bullet (c) 2008 Martin Dvorak <jezek2@advel.cz>
|
*
|
* 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<Tri> tris = new ObjectArrayList<Tri>();
|
|
/**
|
* 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<Vector3f> vertexSource = new ObjectArrayList<Vector3f>();
|
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<ovcount[0]; i++) {
|
Vector3f v = vertexSource.getQuick(i);
|
VectorUtil.mul(v, v, scale);
|
}
|
}
|
|
ok = computeHull(ovcount[0], vertexSource, hr, desc.maxVertices);
|
|
if (ok) {
|
// re-index triangle mesh so it refers to only used vertices, rebuild a new vertex table.
|
ObjectArrayList<Vector3f> vertexScratch = new ObjectArrayList<Vector3f>();
|
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<ovcount[0]; i++) {
|
result.outputVertices.getQuick(i).set(vertexScratch.getQuick(i));
|
}
|
|
if (desc.hasHullFlag(HullFlags.REVERSE_ORDER)) {
|
IntArrayList source_ptr = hr.indices;
|
int source_idx = 0;
|
|
IntArrayList dest_ptr = result.indices;
|
int dest_idx = 0;
|
|
for (int i=0; i<hr.faceCount; i++) {
|
dest_ptr.set(dest_idx + 0, source_ptr.get(source_idx + 2));
|
dest_ptr.set(dest_idx + 1, source_ptr.get(source_idx + 1));
|
dest_ptr.set(dest_idx + 2, source_ptr.get(source_idx + 0));
|
dest_idx += 3;
|
source_idx += 3;
|
}
|
}
|
else {
|
for (int i=0; i<hr.indexCount; i++) {
|
result.indices.set(i, hr.indices.get(i));
|
}
|
}
|
}
|
else {
|
result.polygons = true;
|
result.numOutputVertices = ovcount[0];
|
MiscUtil.resize(result.outputVertices, ovcount[0], Vector3f.class);
|
result.numFaces = hr.faceCount;
|
result.numIndices = hr.indexCount + hr.faceCount;
|
MiscUtil.resize(result.indices, result.numIndices, 0);
|
for (int i=0; i<ovcount[0]; i++) {
|
result.outputVertices.getQuick(i).set(vertexScratch.getQuick(i));
|
}
|
|
// if ( 1 )
|
{
|
IntArrayList source_ptr = hr.indices;
|
int source_idx = 0;
|
|
IntArrayList dest_ptr = result.indices;
|
int dest_idx = 0;
|
|
for (int i=0; i<hr.faceCount; i++) {
|
dest_ptr.set(dest_idx + 0, 3);
|
if (desc.hasHullFlag(HullFlags.REVERSE_ORDER)) {
|
dest_ptr.set(dest_idx + 1, source_ptr.get(source_idx + 2));
|
dest_ptr.set(dest_idx + 2, source_ptr.get(source_idx + 1));
|
dest_ptr.set(dest_idx + 3, source_ptr.get(source_idx + 0));
|
}
|
else {
|
dest_ptr.set(dest_idx + 1, source_ptr.get(source_idx + 0));
|
dest_ptr.set(dest_idx + 2, source_ptr.get(source_idx + 1));
|
dest_ptr.set(dest_idx + 3, source_ptr.get(source_idx + 2));
|
}
|
|
dest_idx += 4;
|
source_idx += 3;
|
}
|
}
|
}
|
releaseHull(hr);
|
}
|
}
|
|
return ret;
|
}
|
|
/**
|
* Release memory allocated for this result, we are done with it.
|
*/
|
public boolean releaseResult(HullResult result) {
|
if (result.outputVertices.size() != 0) {
|
result.numOutputVertices = 0;
|
result.outputVertices.clear();
|
}
|
if (result.indices.size() != 0) {
|
result.numIndices = 0;
|
result.indices.clear();
|
}
|
return true;
|
}
|
|
private boolean computeHull(int vcount, ObjectArrayList<Vector3f> 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<tris.size(); i++) {
|
if (t == null || (tris.getQuick(i) != null && t.rise < tris.getQuick(i).rise)) {
|
t = tris.getQuick(i);
|
}
|
}
|
return (t.rise > epsilon) ? t : null;
|
}
|
|
private int calchull(ObjectArrayList<Vector3f> 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<tris.size(); i++) {
|
if (tris.getQuick(i) != null) {
|
for (int j = 0; j < 3; j++) {
|
ts.add((tris.getQuick(i)).getCoord(j));
|
}
|
deAllocateTriangle(tris.getQuick(i));
|
}
|
}
|
tris_count[0] = ts.size() / 3;
|
MiscUtil.resize(tris_out, ts.size(), 0);
|
|
for (int i=0; i<ts.size(); i++) {
|
tris_out.set(i, ts.get(i));
|
}
|
MiscUtil.resize(tris, 0, Tri.class);
|
|
return 1;
|
}
|
|
private int calchullgen(ObjectArrayList<Vector3f> 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<verts_count; j++) {
|
allow.add(1);
|
isextreme.add(0);
|
VectorUtil.setMin(bmin, verts.getQuick(j));
|
VectorUtil.setMax(bmax, verts.getQuick(j));
|
}
|
tmp.sub(bmax, bmin);
|
float epsilon = tmp.length() * 0.001f;
|
assert (epsilon != 0f);
|
|
Int4 p = findSimplex(verts, verts_count, allow, new Int4());
|
if (p.x == -1) {
|
return 0; // simplex failed
|
|
// a valid interior point
|
}
|
Vector3f center = Stack.alloc(Vector3f.class);
|
VectorUtil.add(center, verts.getQuick(p.getCoord(0)), verts.getQuick(p.getCoord(1)), verts.getQuick(p.getCoord(2)), verts.getQuick(p.getCoord(3)));
|
center.scale(1f / 4f);
|
|
Tri t0 = allocateTriangle(p.getCoord(2), p.getCoord(3), p.getCoord(1));
|
t0.n.set(2, 3, 1);
|
Tri t1 = allocateTriangle(p.getCoord(3), p.getCoord(2), p.getCoord(0));
|
t1.n.set(3, 2, 0);
|
Tri t2 = allocateTriangle(p.getCoord(0), p.getCoord(1), p.getCoord(3));
|
t2.n.set(0, 1, 3);
|
Tri t3 = allocateTriangle(p.getCoord(1), p.getCoord(0), p.getCoord(2));
|
t3.n.set(1, 0, 2);
|
isextreme.set(p.getCoord(0), 1);
|
isextreme.set(p.getCoord(1), 1);
|
isextreme.set(p.getCoord(2), 1);
|
isextreme.set(p.getCoord(3), 1);
|
checkit(t0);
|
checkit(t1);
|
checkit(t2);
|
checkit(t3);
|
|
Vector3f n = Stack.alloc(Vector3f.class);
|
|
for (int j=0; j<tris.size(); j++) {
|
Tri t = tris.getQuick(j);
|
assert (t != null);
|
assert (t.vmax < 0);
|
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);
|
tmp.sub(verts.getQuick(t.vmax), verts.getQuick(t.getCoord(0)));
|
t.rise = n.dot(tmp);
|
}
|
Tri te;
|
vlimit -= 4;
|
while (vlimit > 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<Vector3f> 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<Vector3f> verts, int vcount, ObjectArrayList<Vector3f> overts, int[] ocount, IntArrayList indices, int indexcount) {
|
IntArrayList tmpIndices = new IntArrayList();
|
for (int i=0; i<vertexIndexMapping.size(); i++) {
|
tmpIndices.add(vertexIndexMapping.size());
|
}
|
|
IntArrayList usedIndices = new IntArrayList();
|
MiscUtil.resize(usedIndices, vcount, 0);
|
/*
|
JAVA NOTE: redudant
|
for (int i=0; i<vcount; i++) {
|
usedIndices.set(i, 0);
|
}
|
*/
|
|
ocount[0] = 0;
|
|
for (int i=0; i<indexcount; i++) {
|
int v = indices.get(i); // original array index
|
|
assert (v >= 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<Vector3f> svertices,
|
int stride,
|
int[] vcount, // output number of vertices
|
ObjectArrayList<Vector3f> 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<Vector3f> vtx_ptr = svertices;
|
int vtx_idx = 0;
|
|
// if ( 1 )
|
{
|
for (int i=0; i<svcount; i++) {
|
Vector3f p = vtx_ptr.getQuick(vtx_idx);
|
|
vtx_idx +=/*stride*/ 1;
|
|
for (int j=0; j<3; j++) {
|
if (VectorUtil.getCoord(p, j) < bmin[j]) {
|
bmin[j] = VectorUtil.getCoord(p, j);
|
}
|
if (VectorUtil.getCoord(p, j) > 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<svcount; i++) {
|
Vector3f p = vtx_ptr.getQuick(vtx_idx);
|
vtx_idx +=/*stride*/ 1;
|
|
float px = p.x;
|
float py = p.y;
|
float pz = p.z;
|
|
if (scale != null) {
|
px = px * recip[0]; // normalize
|
py = py * recip[1]; // normalize
|
pz = pz * recip[2]; // normalize
|
}
|
|
// if ( 1 )
|
{
|
int j;
|
|
for (j=0; j<vcount[0]; j++) {
|
/// XXX might be broken
|
Vector3f v = vertices.getQuick(j);
|
|
float x = v.x;
|
float y = v.y;
|
float z = v.z;
|
|
dx = Math.abs(x - px);
|
dy = Math.abs(y - py);
|
dz = Math.abs(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.
|
|
float dist1 = getDist(px, py, pz, center);
|
float dist2 = getDist(v.x, v.y, v.z, center);
|
|
if (dist1 > 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<vcount[0]; i++) {
|
Vector3f p = vertices.getQuick(i);
|
for (int j = 0; j < 3; j++) {
|
if (VectorUtil.getCoord(p, j) < bmin[j]) {
|
bmin[j] = VectorUtil.getCoord(p, j);
|
}
|
if (VectorUtil.getCoord(p, j) > 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<Vector3f> p, int count, Vector3f dir, IntArrayList allow) {
|
assert (count != 0);
|
int m = -1;
|
for (int i=0; i<count; i++) {
|
if (allow.get(i) != 0) {
|
if (m == -1 || p.getQuick(i).dot(dir) > p.getQuick(m).dot(dir)) {
|
m = i;
|
}
|
}
|
}
|
assert (m != -1);
|
return m;
|
}
|
|
private static int maxdirsterid(ObjectArrayList<Vector3f> 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<Vector3f> 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<Vector3f> 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;
|
}
|
|
}
|
