/*
|
* Java port of Bullet (c) 2008 Martin Dvorak <jezek2@advel.cz>
|
*
|
* ShapeHull implemented by John McCutchan.
|
*
|
* Bullet Continuous Collision Detection and Physics Library
|
* Copyright (c) 2003-2008 Erwin Coumans http://www.bulletphysics.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.
|
*/
|
|
package com.bulletphysics.collision.shapes;
|
|
import com.bulletphysics.linearmath.MiscUtil;
|
import com.bulletphysics.linearmath.convexhull.*;
|
import com.bulletphysics.util.IntArrayList;
|
import com.bulletphysics.util.ObjectArrayList;
|
import cz.advel.stack.Stack;
|
import javax.vecmath.Vector3f;
|
import javax.vecmath.Vector3d;
|
|
/**
|
* ShapeHull takes a {@link ConvexShape}, builds the convex hull using {@link HullLibrary}
|
* and provides triangle indices and vertices.
|
*
|
* @author jezek2
|
*/
|
public class ShapeHull implements java.io.Serializable
|
{
|
static final long serialVersionUID = -1102208318940158328L;
|
|
protected ObjectArrayList<Vector3f> vertices = new ObjectArrayList<Vector3f>();
|
protected ObjectArrayList<Vector3f> normals = new ObjectArrayList<Vector3f>();
|
protected IntArrayList indices = new IntArrayList();
|
protected int numIndices;
|
protected ConvexShape shape;
|
|
protected ObjectArrayList<Vector3f> unitSpherePoints = new ObjectArrayList<Vector3f>();
|
|
public ShapeHull(ConvexShape shape) {
|
this.shape = shape;
|
this.vertices.clear();
|
this.normals.clear();
|
this.indices.clear();
|
this.numIndices = 0;
|
|
// MiscUtil.resize(unitSpherePoints, NUM_UNITSPHERE_POINTS+ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS*2, Vector3f.class);
|
//// for (int i=constUnitSpherePoints.size(); --i>=0;) {
|
//// unitSpherePoints.getQuick(i).set(constUnitSpherePoints.getQuick(i));
|
//// }
|
// int lines = (int)Math.sqrt(NUM_UNITSPHERE_POINTS);
|
// int rows = lines;
|
// Vector3f vec = new Vector3f();
|
// Superellipsoid supere = new Superellipsoid();
|
// for (int i=lines; --i>=0;)
|
// {
|
// for (int j=rows; --j>=0;)
|
// {
|
// double u = i; u /= lines-1;
|
// double v = j; v /= rows-1;
|
//
|
// supere.biparamFunction(u, v, vec);
|
//
|
// unitSpherePoints.getQuick(i*rows + j).set(vec);
|
// }
|
// }
|
}
|
|
public boolean buildHull(float margin)
|
{
|
int lines = (int)Math.sqrt(NUM_UNITSPHERE_POINTS);
|
int rows = lines;
|
|
MiscUtil.resize(vertices, NUM_UNITSPHERE_POINTS*2, Vector3f.class);
|
MiscUtil.resize(normals, NUM_UNITSPHERE_POINTS*2, Vector3f.class);
|
|
Vector3f vec = new Vector3f();
|
Vector3f norm = new Vector3f();
|
Vector3f normout = new Vector3f();
|
Vector3f vec2 = new Vector3f();
|
Superellipsoid supere = new Superellipsoid();
|
|
CapsuleShape capsule = (CapsuleShape)shape;
|
|
int loops = 1;
|
|
if (capsule.anchor)
|
loops++;
|
|
for (int loop=loops; --loop>=0;)
|
for (int j=rows; --j>=0;)
|
{
|
int k=0;
|
|
for (int i=lines; --i>=0;)
|
{
|
double u = i; u /= lines-1;
|
double v = j; v /= rows-1;
|
|
supere.biparamFunction(u, v, vec, norm);
|
|
capsule.localGetSupportingVertexWithNorm(vec, vec2, norm, normout);
|
|
if (loop == 1)
|
{
|
//vec2.scale(0.5f);
|
switch (capsule.getUpAxis())
|
{
|
case 0: vec2.x /= capsule.getHalfHeight(); vec2.x *= capsule.getRadius()/2; vec2.x -= capsule.getHalfHeight(); break;
|
case 1: vec2.y /= capsule.getHalfHeight(); vec2.y *= capsule.getRadius()/2; vec2.y -= capsule.getHalfHeight(); break;
|
case 2: vec2.z /= capsule.getHalfHeight(); vec2.z *= capsule.getRadius()/2; vec2.z -= capsule.getHalfHeight(); break;
|
}
|
}
|
vertices.getQuick(loop*NUM_UNITSPHERE_POINTS + i*rows + j).set(vec2);
|
normals.getQuick(loop*NUM_UNITSPHERE_POINTS + i*rows + j).set(normout);
|
}
|
}
|
|
numIndices = (lines*(rows-1) * 6) * loops;
|
|
int i = 0;
|
int j = 0;
|
|
MiscUtil.resize(indices, numIndices, 0);
|
for (int in=0; in<numIndices/loops;)
|
{
|
indices.set(in++, i*(rows-1) + j);
|
indices.set(in++, (i+1)*(rows-1) + j+1);
|
indices.set(in++, i*(rows-1) + j+1);
|
indices.set(in++, i*(rows-1) + j);
|
indices.set(in++, (i+1)*(rows-1) + j);
|
indices.set(in++, (i+1)*(rows-1) + j+1);
|
|
j++;
|
if (j >= rows-1)
|
{
|
j = 0;
|
i++;
|
}
|
}
|
|
i = 0;
|
j = 0;
|
|
for (int in=numIndices/loops; in<numIndices;)
|
{
|
indices.set(in++, NUM_UNITSPHERE_POINTS + i*(rows-1) + j);
|
indices.set(in++, NUM_UNITSPHERE_POINTS + (i+1)*(rows-1) + j+1);
|
indices.set(in++, NUM_UNITSPHERE_POINTS + i*(rows-1) + j+1);
|
indices.set(in++, NUM_UNITSPHERE_POINTS + i*(rows-1) + j);
|
indices.set(in++, NUM_UNITSPHERE_POINTS + (i+1)*(rows-1) + j);
|
indices.set(in++, NUM_UNITSPHERE_POINTS + (i+1)*(rows-1) + j+1);
|
|
j++;
|
if (j >= rows-1)
|
{
|
j = 0;
|
i++;
|
}
|
}
|
|
return true;
|
}
|
|
public boolean buildHull0(float margin)
|
{
|
Vector3f norm = Stack.alloc(Vector3f.class);
|
|
int numSampleDirections = NUM_UNITSPHERE_POINTS;
|
{
|
int numPDA = shape.getNumPreferredPenetrationDirections();
|
if (numPDA != 0) {
|
for (int i=0; i<numPDA; i++) {
|
shape.getPreferredPenetrationDirection(i, norm);
|
unitSpherePoints.getQuick(numSampleDirections).set(norm);
|
numSampleDirections++;
|
}
|
}
|
}
|
|
ObjectArrayList<Vector3f> supportPoints = new ObjectArrayList<Vector3f>();
|
MiscUtil.resize(supportPoints, NUM_UNITSPHERE_POINTS + ConvexShape.MAX_PREFERRED_PENETRATION_DIRECTIONS * 2, Vector3f.class);
|
|
for (int i=0; i<numSampleDirections; i++) {
|
shape.localGetSupportingVertex(unitSpherePoints.getQuick(i), supportPoints.getQuick(i));
|
}
|
|
HullDesc hd = new HullDesc();
|
hd.flags = HullFlags.TRIANGLES;
|
hd.vcount = numSampleDirections;
|
|
//#ifdef BT_USE_DOUBLE_PRECISION
|
//hd.mVertices = &supportPoints[0];
|
//hd.mVertexStride = sizeof(btVector3);
|
//#else
|
hd.vertices = supportPoints;
|
//hd.vertexStride = 3 * 4;
|
//#endif
|
|
HullLibrary hl = new HullLibrary();
|
HullResult hr = new HullResult();
|
if (!hl.createConvexHull(hd, hr)) {
|
return false;
|
}
|
|
MiscUtil.resize(vertices, hr.numOutputVertices, Vector3f.class);
|
MiscUtil.resize(normals, hr.numOutputVertices, Vector3f.class);
|
|
for (int i=0; i<hr.numOutputVertices; i++) {
|
vertices.getQuick(i).set(hr.outputVertices.getQuick(i));
|
}
|
numIndices = hr.numIndices;
|
MiscUtil.resize(indices, numIndices, 0);
|
for (int i=0; i<numIndices; i++) {
|
indices.set(i, hr.indices.get(i));
|
}
|
|
// free temporary hull result that we just copied
|
hl.releaseResult(hr);
|
|
return true;
|
}
|
|
public int numTriangles() {
|
return numIndices / 3;
|
}
|
|
public int numVertices() {
|
return vertices.size();
|
}
|
|
public int numIndices() {
|
return numIndices;
|
}
|
|
public ObjectArrayList<Vector3f> getVertexPointer() {
|
return vertices;
|
}
|
|
public ObjectArrayList<Vector3f> getNormalPointer() {
|
return normals;
|
}
|
|
public IntArrayList getIndexPointer() {
|
return indices;
|
}
|
|
////////////////////////////////////////////////////////////////////////////
|
|
private static int NUM_UNITSPHERE_POINTS = 15*15; // 420;
|
|
private static ObjectArrayList<Vector3f> constUnitSpherePoints = new ObjectArrayList<Vector3f>();
|
|
static {
|
constUnitSpherePoints.add(new Vector3f(0.000000f, -0.000000f, -1.000000f));
|
constUnitSpherePoints.add(new Vector3f(0.723608f, -0.525725f, -0.447219f));
|
constUnitSpherePoints.add(new Vector3f(-0.276388f, -0.850649f, -0.447219f));
|
constUnitSpherePoints.add(new Vector3f(-0.894426f, -0.000000f, -0.447216f));
|
constUnitSpherePoints.add(new Vector3f(-0.276388f, 0.850649f, -0.447220f));
|
constUnitSpherePoints.add(new Vector3f(0.723608f, 0.525725f, -0.447219f));
|
constUnitSpherePoints.add(new Vector3f(0.276388f, -0.850649f, 0.447220f));
|
constUnitSpherePoints.add(new Vector3f(-0.723608f, -0.525725f, 0.447219f));
|
constUnitSpherePoints.add(new Vector3f(-0.723608f, 0.525725f, 0.447219f));
|
constUnitSpherePoints.add(new Vector3f(0.276388f, 0.850649f, 0.447219f));
|
constUnitSpherePoints.add(new Vector3f(0.894426f, 0.000000f, 0.447216f));
|
constUnitSpherePoints.add(new Vector3f(-0.000000f, 0.000000f, 1.000000f));
|
constUnitSpherePoints.add(new Vector3f(0.425323f, -0.309011f, -0.850654f));
|
constUnitSpherePoints.add(new Vector3f(-0.162456f, -0.499995f, -0.850654f));
|
constUnitSpherePoints.add(new Vector3f(0.262869f, -0.809012f, -0.525738f));
|
constUnitSpherePoints.add(new Vector3f(0.425323f, 0.309011f, -0.850654f));
|
constUnitSpherePoints.add(new Vector3f(0.850648f, -0.000000f, -0.525736f));
|
constUnitSpherePoints.add(new Vector3f(-0.525730f, -0.000000f, -0.850652f));
|
constUnitSpherePoints.add(new Vector3f(-0.688190f, -0.499997f, -0.525736f));
|
constUnitSpherePoints.add(new Vector3f(-0.162456f, 0.499995f, -0.850654f));
|
constUnitSpherePoints.add(new Vector3f(-0.688190f, 0.499997f, -0.525736f));
|
constUnitSpherePoints.add(new Vector3f(0.262869f, 0.809012f, -0.525738f));
|
constUnitSpherePoints.add(new Vector3f(0.951058f, 0.309013f, 0.000000f));
|
constUnitSpherePoints.add(new Vector3f(0.951058f, -0.309013f, 0.000000f));
|
constUnitSpherePoints.add(new Vector3f(0.587786f, -0.809017f, 0.000000f));
|
constUnitSpherePoints.add(new Vector3f(0.000000f, -1.000000f, 0.000000f));
|
constUnitSpherePoints.add(new Vector3f(-0.587786f, -0.809017f, 0.000000f));
|
constUnitSpherePoints.add(new Vector3f(-0.951058f, -0.309013f, -0.000000f));
|
constUnitSpherePoints.add(new Vector3f(-0.951058f, 0.309013f, -0.000000f));
|
constUnitSpherePoints.add(new Vector3f(-0.587786f, 0.809017f, -0.000000f));
|
constUnitSpherePoints.add(new Vector3f(-0.000000f, 1.000000f, -0.000000f));
|
constUnitSpherePoints.add(new Vector3f(0.587786f, 0.809017f, -0.000000f));
|
constUnitSpherePoints.add(new Vector3f(0.688190f, -0.499997f, 0.525736f));
|
constUnitSpherePoints.add(new Vector3f(-0.262869f, -0.809012f, 0.525738f));
|
constUnitSpherePoints.add(new Vector3f(-0.850648f, 0.000000f, 0.525736f));
|
constUnitSpherePoints.add(new Vector3f(-0.262869f, 0.809012f, 0.525738f));
|
constUnitSpherePoints.add(new Vector3f(0.688190f, 0.499997f, 0.525736f));
|
constUnitSpherePoints.add(new Vector3f(0.525730f, 0.000000f, 0.850652f));
|
constUnitSpherePoints.add(new Vector3f(0.162456f, -0.499995f, 0.850654f));
|
constUnitSpherePoints.add(new Vector3f(-0.425323f, -0.309011f, 0.850654f));
|
constUnitSpherePoints.add(new Vector3f(-0.425323f, 0.309011f, 0.850654f));
|
constUnitSpherePoints.add(new Vector3f(0.162456f, 0.499995f, 0.850654f));
|
}
|
|
static int tablelength = 257;
|
|
static double power1 = -1;
|
static double[] powtable1 = new double[tablelength];
|
static double power2 = -1;
|
static double[] powtable2 = new double[tablelength];
|
|
class Superellipsoid //extends Biparam /*Sphere*/ implements java.io.Serializable
|
{
|
|
Superellipsoid()
|
{
|
// inPnt = new cVector();
|
du = new Vector3f();
|
dv = new Vector3f();
|
//
|
// name = "Superellipsoid";
|
east = 0.75;
|
north = 1;
|
//uDivs = vDivs = 32;
|
// uDivs = 35;
|
// vDivs = 34;
|
// minUDivs = 3;
|
// minVDivs = 2;
|
}
|
|
double signPower(double base, double pow)
|
{
|
//if (base == 0)
|
// base = 0.0000001;
|
|
if (base == 0 && pow == 0 || base == 1)
|
return 1;
|
|
if (base == 0)
|
return 0;
|
|
if (base == -1)
|
return -1;
|
|
if (base >= 0)
|
return (double)Mathpow(base, pow);
|
else
|
return -(double)Mathpow(-base, pow);
|
}
|
|
double Mathpow(double base, double pow)
|
{
|
int basei = (int) (base * (tablelength - 1));
|
|
double[] table = null;
|
|
if (pow == power1)
|
{
|
table = powtable1;
|
}
|
else if (pow == power2)
|
{
|
table = powtable2;
|
}
|
else
|
{
|
if (power2 == -1)
|
{
|
power2 = pow;
|
table = powtable2 = new double[tablelength];
|
}
|
else
|
{
|
power1 = pow;
|
table = powtable1 = new double[tablelength];
|
power2 = -1;
|
}
|
}
|
|
if (basei != 0 && table[basei] == 0)
|
{
|
table[basei] = Math.pow(basei/(tablelength - 1.0), pow);
|
//if (table[basei] == 0)
|
// System.out.println("POW == 0 : " + basei);
|
}
|
|
if (table[basei+1] == 0)
|
{
|
table[basei+1] = Math.pow((basei+1.0)/(tablelength - 1.0), pow);
|
//if (table[basei+1] == 0)
|
// System.out.println("POW == 0");
|
}
|
|
double offset = base * (tablelength - 1) - basei;
|
//System.out.println("offset = " + offset);
|
|
return table[basei] * (1 - offset) + table[basei+1] * offset;
|
}
|
|
double DsignPower(double base, double pow)
|
{
|
if (base == 0)
|
base = 0.0000000001;
|
|
if (base == 1 || base == -1)
|
return pow;
|
|
if (base > 0)
|
return pow * (double)(Mathpow(base, pow) / base); // - 1);
|
else // if (base < 0)
|
return pow * (double)(Mathpow(-base, pow) / -base); // - 1);
|
//else
|
//{
|
// if (pow == 1)
|
// return 1;
|
// else
|
// return 0;
|
//}
|
}
|
|
Vector3f du;
|
Vector3f dv;
|
|
//Vertex
|
void
|
biparamFunction(double u, double v, Vector3f out, Vector3f norm)
|
{
|
//System.out.println("U = " + u + "; V = " + v);
|
double uAng = u * 2 * Math.PI; // LA.toRadians(u * 360);
|
double vAng = v * Math.PI - Math.PI/2; // LA.toRadians(v * 180 - 90);
|
double mcu = Math.cos(uAng);
|
double msu = Math.sin(uAng);
|
double mcv = Math.cos(vAng);
|
double msv = Math.sin(vAng);
|
if (u == 1)
|
{
|
mcu = 1;
|
msu = 0;
|
}
|
if (v == 1)
|
{
|
mcv = 0;
|
msv = 1;
|
}
|
if (v == 0)
|
{
|
mcv = 0;
|
msv = -1;
|
}
|
double cu = signPower(mcu, north);
|
double su = signPower(msu, north);
|
double cv = signPower(mcv, east);
|
double sv = signPower(msv, east);
|
double z = radius * cv * cu;
|
double x = radius * cv * su;
|
double y = radius * sv;
|
|
double dcu = DsignPower(mcu, north) * -msu;
|
double dsu = DsignPower(msu, north) * mcu;
|
double dcv = DsignPower(mcv, east) * -msv;
|
double dsv = DsignPower(msv, east) * mcv;
|
|
du.z = (float)(cv * dcu);
|
du.x = (float)(cv * dsu);
|
du.y = (float)(0);
|
dv.z = (float)(dcv * cu);
|
dv.x = (float)(dcv * su);
|
dv.y = (float)(dsv);
|
|
//if (du.length() == 0)
|
// System.out.println("DU is null");
|
//if (dv.length() == 0)
|
// System.out.println("DV is null");
|
out./*pos.*/x = (float)x; // = LA.newVector(x, y, z);
|
out./*pos.*/y = (float)y;
|
out./*pos.*/z = (float)z;
|
|
if (du.dot(du) == 0)
|
{
|
norm.set(out);
|
}
|
else
|
{
|
du.normalize();
|
dv.normalize();
|
|
norm.cross(du, dv);
|
// if (norm.dot(norm) == 0)
|
// norm.z = 1;
|
}
|
|
norm.normalize();
|
// Vertex temp = new Vertex();
|
//
|
// temp.norm = LA.newVector(x, y, z);
|
// LA.vecCross(du,dv, temp.norm);
|
// if (temp.norm.length() == 0)
|
// temp.norm.y = 2*v - 1;
|
// else
|
// LA.vecNormalize(temp.norm);
|
|
//if (temp.norm.dot(temp/*.pos*/) < 0)
|
//temp.norm.mul(-1);
|
|
//LA.vecAdd(temp/*.pos*/, center, temp/*.pos*/);
|
|
// return temp;
|
}
|
|
|
double east;
|
double north;
|
double radius = 1;
|
// private cVector inPnt;
|
}
|
}
|
