@Override
public void getEdge(int i, Vector3f pa, Vector3f pb) {
int index0 = i % points.size();
int index1 = (i + 1) % points.size();
VectorUtil.mul(pa, points.getQuick(index0), localScaling);
VectorUtil.mul(pb, points.getQuick(index1), localScaling);
}
@Override
public Vector3 localGetSupportingVertexWithoutMargin(Vector3 vec0, Vector3 out) {
Stack stack = Stack.enter();
Vector3 supVec = out;
supVec.set(0f, 0f, 0f);
float maxDot = -1e30f;
Vector3 vec = stack.alloc(vec0);
float lenSqr = vec.len2();
if (lenSqr < 0.0001f) {
vec.set(1f, 0f, 0f);
} else {
float rlen = 1f / (float) Math.sqrt(lenSqr);
vec.scl(rlen);
}
Vector3 vtx = stack.allocVector3();
float newDot;
float radius = getRadius();
Vector3 tmp1 = stack.allocVector3();
Vector3 tmp2 = stack.allocVector3();
Vector3 pos = stack.allocVector3();
{
pos.set(0f, 0f, 0f);
VectorUtil.setCoord(pos, getUpAxis(), getHalfHeight());
VectorUtil.mul(tmp1, vec, localScaling);
tmp1.scl(radius);
tmp2.set(vec).scl(getMargin());
vtx.set(pos).add(tmp1);
vtx.sub(tmp2);
newDot = vec.dot(vtx);
if (newDot > maxDot) {
maxDot = newDot;
supVec.set(vtx);
}
}
{
pos.set(0f, 0f, 0f);
VectorUtil.setCoord(pos, getUpAxis(), -getHalfHeight());
VectorUtil.mul(tmp1, vec, localScaling);
tmp1.scl(radius);
tmp2.set(vec).scl(getMargin());
vtx.set(pos).add(tmp1);
vtx.sub(tmp2);
newDot = vec.dot(vtx);
if (newDot > maxDot) {
maxDot = newDot;
supVec.set(vtx);
}
}
stack.leave();
return out;
}
@Override
public void getAabb(Transform t, Vector3 aabbMin, Vector3 aabbMax) {
Stack stack = Stack.enter();
Vector3 tmp = stack.allocVector3();
Vector3 halfExtents = stack.allocVector3();
halfExtents.set(getRadius(), getRadius(), getRadius());
VectorUtil.setCoord(halfExtents, upAxis, getRadius() + getHalfHeight());
halfExtents.x += getMargin();
halfExtents.y += getMargin();
halfExtents.z += getMargin();
Matrix3 abs_b = stack.allocMatrix3();
abs_b.set(t.basis);
MatrixUtil.absolute(abs_b);
Vector3 center = t.origin;
Vector3 extent = stack.allocVector3();
MatrixUtil.getRow(abs_b, 0, tmp);
extent.x = tmp.dot(halfExtents);
MatrixUtil.getRow(abs_b, 1, tmp);
extent.y = tmp.dot(halfExtents);
MatrixUtil.getRow(abs_b, 2, tmp);
extent.z = tmp.dot(halfExtents);
aabbMin.set(center).sub(extent);
aabbMax.set(center).add(extent);
stack.leave();
}
@Override
public void calculateLocalInertia(float mass, Vector3 inertia) {
// as an approximation, take the inertia of the box that bounds the spheres
Stack stack = Stack.enter();
Transform ident = stack.allocTransform();
ident.setIdentity();
float radius = getRadius();
Vector3 halfExtents = stack.allocVector3();
halfExtents.set(radius, radius, radius);
VectorUtil.setCoord(halfExtents, getUpAxis(), radius + getHalfHeight());
float margin = BulletGlobals.CONVEX_DISTANCE_MARGIN;
float lx = 2f * (halfExtents.x + margin);
float ly = 2f * (halfExtents.y + margin);
float lz = 2f * (halfExtents.z + margin);
float x2 = lx * lx;
float y2 = ly * ly;
float z2 = lz * lz;
float scaledmass = mass * 0.08333333f;
inertia.x = scaledmass * (y2 + z2);
inertia.y = scaledmass * (x2 + z2);
inertia.z = scaledmass * (x2 + y2);
stack.leave();
}
@Override
public Vector3f localGetSupportingVertexWithoutMargin(Vector3f vec0, Vector3f out) {
Vector3f supVec = out;
supVec.set(0f, 0f, 0f);
float newDot, maxDot = -1e30f;
Vector3f vec = Stack.alloc(vec0);
float lenSqr = vec.lengthSquared();
if (lenSqr < 0.0001f) {
vec.set(1f, 0f, 0f);
} else {
float rlen = 1f / (float) Math.sqrt(lenSqr);
vec.scale(rlen);
}
Vector3f vtx = Stack.alloc(Vector3f.class);
for (int i = 0; i < points.size(); i++) {
VectorUtil.mul(vtx, points.getQuick(i), localScaling);
newDot = vec.dot(vtx);
if (newDot > maxDot) {
maxDot = newDot;
supVec.set(vtx);
}
}
return out;
}
@Override
public void batchedUnitVectorGetSupportingVertexWithoutMargin(
Vector3f[] vectors, Vector3f[] supportVerticesOut, int numVectors) {
float newDot;
// JAVA NOTE: rewritten as code used W coord for temporary usage in Vector3
// TODO: optimize it
float[] wcoords = new float[numVectors];
// use 'w' component of supportVerticesOut?
{
for (int i = 0; i < numVectors; i++) {
// supportVerticesOut[i][3] = btScalar(-1e30);
wcoords[i] = -1e30f;
}
}
Vector3f vtx = Stack.alloc(Vector3f.class);
for (int i = 0; i < points.size(); i++) {
VectorUtil.mul(vtx, points.getQuick(i), localScaling);
for (int j = 0; j < numVectors; j++) {
Vector3f vec = vectors[j];
newDot = vec.dot(vtx);
// if (newDot > supportVerticesOut[j][3])
if (newDot > wcoords[j]) {
// WARNING: don't swap next lines, the w component would get overwritten!
supportVerticesOut[j].set(vtx);
// supportVerticesOut[j][3] = newDot;
wcoords[j] = newDot;
}
}
}
}
public void internalProcessTriangleIndex(Vector3f[] triangle, int partId, int triangleIndex) {
VectorUtil.setMin(aabbMin, triangle[0]);
VectorUtil.setMax(aabbMax, triangle[0]);
VectorUtil.setMin(aabbMin, triangle[1]);
VectorUtil.setMax(aabbMax, triangle[1]);
VectorUtil.setMin(aabbMin, triangle[2]);
VectorUtil.setMax(aabbMax, triangle[2]);
}
public float getHalfHeight() {
return VectorUtil.getCoord(implicitShapeDimensions, upAxis);
}
public float getRadius() {
int radiusAxis = (upAxis + 2) % 3;
return VectorUtil.getCoord(implicitShapeDimensions, radiusAxis);
}
public boolean calcTimeOfImpact(
Transform fromA, Transform toA, Transform fromB, Transform toB, CastResult result) {
simplexSolver.reset();
// compute linear velocity for this interval, to interpolate
// assume no rotation/angular velocity, assert here?
Vector3d linVelA = new Vector3d();
Vector3d linVelB = new Vector3d();
linVelA.sub(toA.origin, fromA.origin);
linVelB.sub(toB.origin, fromB.origin);
double radius = 0.001f;
double lambda = 0f;
Vector3d v = new Vector3d();
v.set(1f, 0f, 0f);
int maxIter = MAX_ITERATIONS;
Vector3d n = new Vector3d();
n.set(0f, 0f, 0f);
boolean hasResult = false;
Vector3d c = new Vector3d();
Vector3d r = new Vector3d();
r.sub(linVelA, linVelB);
double lastLambda = lambda;
// btScalar epsilon = btScalar(0.001);
int numIter = 0;
// first solution, using GJK
Transform identityTrans = new Transform();
identityTrans.setIdentity();
// result.drawCoordSystem(sphereTr);
PointCollector pointCollector = new PointCollector();
gjk.init(convexA, convexB, simplexSolver, null); // penetrationDepthSolver);
ClosestPointInput input = pointInputsPool.get();
input.init();
try {
// we don't use margins during CCD
// gjk.setIgnoreMargin(true);
input.transformA.set(fromA);
input.transformB.set(fromB);
gjk.getClosestPoints(input, pointCollector, null);
hasResult = pointCollector.hasResult;
c.set(pointCollector.pointInWorld);
if (hasResult) {
double dist;
dist = pointCollector.distance;
n.set(pointCollector.normalOnBInWorld);
// not close enough
while (dist > radius) {
numIter++;
if (numIter > maxIter) {
return false; // todo: report a failure
}
double dLambda = 0f;
double projectedLinearVelocity = r.dot(n);
dLambda = dist / (projectedLinearVelocity);
lambda = lambda - dLambda;
if (lambda > 1f) {
return false;
}
if (lambda < 0f) {
return false; // todo: next check with relative epsilon
}
if (lambda <= lastLambda) {
return false;
// n.setValue(0,0,0);
// break;
}
lastLambda = lambda;
// interpolate to next lambda
result.debugDraw(lambda);
VectorUtil.setInterpolate3(input.transformA.origin, fromA.origin, toA.origin, lambda);
VectorUtil.setInterpolate3(input.transformB.origin, fromB.origin, toB.origin, lambda);
gjk.getClosestPoints(input, pointCollector, null);
if (pointCollector.hasResult) {
if (pointCollector.distance < 0f) {
result.fraction = lastLambda;
n.set(pointCollector.normalOnBInWorld);
result.normal.set(n);
result.hitPoint.set(pointCollector.pointInWorld);
return true;
}
c.set(pointCollector.pointInWorld);
n.set(pointCollector.normalOnBInWorld);
dist = pointCollector.distance;
} else {
// ??
return false;
}
}
// is n normalized?
// don't report time of impact for motion away from the contact normal (or causes minor
// penetration)
if (n.dot(r) >= -result.allowedPenetration) {
return false;
}
result.fraction = lambda;
result.normal.set(n);
result.hitPoint.set(c);
return true;
}
return false;
} finally {
pointInputsPool.release(input);
}
}
@Override
public void getVertex(int i, Vector3f vtx) {
VectorUtil.mul(vtx, points.getQuick(i), localScaling);
}
public float EvaluatePD(float accuracy) {
pushStack();
try {
Vector3f tmp = Stack.alloc(Vector3f.class);
// btBlock* sablock = sa->beginBlock();
Face bestface = null;
int markid = 1;
depth = -cstInf;
normal.set(0f, 0f, 0f);
root = null;
nfaces = 0;
iterations = 0;
failed = false;
/* Prepare hull */
if (gjk.EncloseOrigin()) {
// const U* pfidx = 0;
int[][] pfidx_ptr = null;
int pfidx_index = 0;
int nfidx = 0;
// const U* peidx = 0;
int[][] peidx_ptr = null;
int peidx_index = 0;
int neidx = 0;
Mkv[] basemkv = new Mkv[5];
Face[] basefaces = new Face[6];
switch (gjk.order) {
// Tetrahedron
case 3:
{
// pfidx=(const U*)fidx;
pfidx_ptr = tetrahedron_fidx;
pfidx_index = 0;
nfidx = 4;
// peidx=(const U*)eidx;
peidx_ptr = tetrahedron_eidx;
peidx_index = 0;
neidx = 6;
}
break;
// Hexahedron
case 4:
{
// pfidx=(const U*)fidx;
pfidx_ptr = hexahedron_fidx;
pfidx_index = 0;
nfidx = 6;
// peidx=(const U*)eidx;
peidx_ptr = hexahedron_eidx;
peidx_index = 0;
neidx = 9;
}
break;
}
int i;
for (i = 0; i <= gjk.order; ++i) {
basemkv[i] = new Mkv();
basemkv[i].set(gjk.simplex[i]);
}
for (i = 0; i < nfidx; ++i, pfidx_index++) {
basefaces[i] =
NewFace(
basemkv[pfidx_ptr[pfidx_index][0]],
basemkv[pfidx_ptr[pfidx_index][1]],
basemkv[pfidx_ptr[pfidx_index][2]]);
}
for (i = 0; i < neidx; ++i, peidx_index++) {
Link(
basefaces[peidx_ptr[peidx_index][0]],
peidx_ptr[peidx_index][1],
basefaces[peidx_ptr[peidx_index][2]],
peidx_ptr[peidx_index][3]);
}
}
if (0 == nfaces) {
// sa->endBlock(sablock);
return (depth);
}
/* Expand hull */
for (; iterations < EPA_maxiterations; ++iterations) {
Face bf = FindBest();
if (bf != null) {
tmp.negate(bf.n);
Mkv w = Support(tmp);
float d = bf.n.dot(w.w) + bf.d;
bestface = bf;
if (d < -accuracy) {
Face[] cf = new Face[] {null};
Face[] ff = new Face[] {null};
int nf = 0;
Detach(bf);
bf.mark = ++markid;
for (int i = 0; i < 3; ++i) {
nf += BuildHorizon(markid, w, bf.f[i], bf.e[i], cf, ff);
}
if (nf <= 2) {
break;
}
Link(cf[0], 1, ff[0], 2);
} else {
break;
}
} else {
break;
}
}
/* Extract contact */
if (bestface != null) {
Vector3f b = GetCoordinates(bestface, Stack.alloc(Vector3f.class));
normal.set(bestface.n);
depth = Math.max(0, bestface.d);
for (int i = 0; i < 2; ++i) {
float s = i != 0 ? -1f : 1f;
for (int j = 0; j < 3; ++j) {
tmp.scale(s, bestface.v[j].r);
gjk.LocalSupport(tmp, i, features[i][j]);
}
}
Vector3f tmp1 = Stack.alloc(Vector3f.class);
Vector3f tmp2 = Stack.alloc(Vector3f.class);
Vector3f tmp3 = Stack.alloc(Vector3f.class);
tmp1.scale(b.x, features[0][0]);
tmp2.scale(b.y, features[0][1]);
tmp3.scale(b.z, features[0][2]);
VectorUtil.add(nearest[0], tmp1, tmp2, tmp3);
tmp1.scale(b.x, features[1][0]);
tmp2.scale(b.y, features[1][1]);
tmp3.scale(b.z, features[1][2]);
VectorUtil.add(nearest[1], tmp1, tmp2, tmp3);
} else {
failed = true;
}
// sa->endBlock(sablock);
return (depth);
} finally {
popStack();
}
}
