/* */ public class CompoundCollisionAlgorithmExt$SwappedCreateFunc
extends CollisionAlgorithmCreateFunc
/* */ {
/* 58 */ private final ObjectPool pool = ObjectPool.get(CompoundCollisionAlgorithmExt.class);
/* */
/* */ public CollisionAlgorithm createCollisionAlgorithm(
CollisionAlgorithmConstructionInfo paramCollisionAlgorithmConstructionInfo,
CollisionObject paramCollisionObject1,
CollisionObject paramCollisionObject2)
/* */ {
/* */ CompoundCollisionAlgorithmExt localCompoundCollisionAlgorithmExt;
/* 67 */ CompoundCollisionAlgorithmExt.access$002(
localCompoundCollisionAlgorithmExt = new CompoundCollisionAlgorithmExt(),
/* 67 */ paramCollisionObject2);
/* 68 */ CompoundCollisionAlgorithmExt.access$102(
localCompoundCollisionAlgorithmExt, paramCollisionObject1);
/* */
/* 70 */ localCompoundCollisionAlgorithmExt.init(paramCollisionAlgorithmConstructionInfo);
/* 71 */ localCompoundCollisionAlgorithmExt.swapped = true;
/* 72 */ return localCompoundCollisionAlgorithmExt;
/* */ }
/* */
/* */ public void releaseCollisionAlgorithm(CollisionAlgorithm paramCollisionAlgorithm)
/* */ {
/* 79 */ CompoundCollisionAlgorithmExt.access$200(
paramCollisionAlgorithm = (CompoundCollisionAlgorithmExt) paramCollisionAlgorithm)
.pool
/* 79 */ .release(paramCollisionAlgorithm);
/* */ }
/* */ }
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);
}
}