protected void integrateTransforms(float timeStep) {
Vector3f tmp = Stack.alloc(Vector3f.class);
Transform tmpTrans = Stack.alloc(Transform.class);
Transform predictedTrans = Stack.alloc(Transform.class);
for (int i = 0; i < collisionObjects.size(); i++) {
CollisionObject colObj = collisionObjects.getQuick(i);
RigidBody body = RigidBody.upcast(colObj);
if (body != null) {
body.setHitFraction(1f);
if (body.isActive() && (!body.isStaticOrKinematicObject())) {
body.predictIntegratedTransform(timeStep, predictedTrans);
tmp.sub(predictedTrans.origin, body.getWorldTransform(tmpTrans).origin);
float squareMotion = tmp.lengthSquared();
if (body.getCcdSquareMotionThreshold() != 0f
&& body.getCcdSquareMotionThreshold() < squareMotion) {
if (body.getCollisionShape().isConvex()) {
ClosestNotMeConvexResultCallback sweepResults =
new ClosestNotMeConvexResultCallback(
body,
body.getWorldTransform(tmpTrans).origin,
predictedTrans.origin,
getBroadphase().getOverlappingPairCache(),
getDispatcher());
// ConvexShape convexShape = (ConvexShape)body.getCollisionShape();
SphereShape tmpSphere =
new SphereShape(
body
.getCcdSweptSphereRadius()); // btConvexShape* convexShape =
// static_cast<btConvexShape*>(body->getCollisionShape());
sweepResults.collisionFilterGroup = body.getBroadphaseProxy().collisionFilterGroup;
sweepResults.collisionFilterMask = body.getBroadphaseProxy().collisionFilterMask;
convexSweepTest(
tmpSphere, body.getWorldTransform(tmpTrans), predictedTrans, sweepResults);
// JAVA NOTE: added closestHitFraction test to prevent objects being stuck
if (sweepResults.hasHit() && (sweepResults.closestHitFraction > 0.0001f)) {
body.setHitFraction(sweepResults.closestHitFraction);
body.predictIntegratedTransform(timeStep * body.getHitFraction(), predictedTrans);
body.setHitFraction(0f);
// System.out.printf("clamped integration to hit fraction = %f\n",
// sweepResults.closestHitFraction);
}
}
}
body.proceedToTransform(predictedTrans);
}
}
}
}
