protected boolean recoverFromPenetration(CollisionWorld collisionWorld) {
boolean penetration = false;
collisionWorld
.getDispatcher()
.dispatchAllCollisionPairs(
ghostObject.getOverlappingPairCache(),
collisionWorld.getDispatchInfo(),
collisionWorld.getDispatcher());
currentPosition.set(ghostObject.getWorldTransform(new Transform()).origin);
double maxPen = 0.0f;
for (int i = 0; i < ghostObject.getOverlappingPairCache().getNumOverlappingPairs(); i++) {
manifoldArray.clear();
BroadphasePair collisionPair =
ghostObject.getOverlappingPairCache().getOverlappingPairArray().getQuick(i);
if (collisionPair.algorithm != null) {
collisionPair.algorithm.getAllContactManifolds(manifoldArray);
}
for (int j = 0; j < manifoldArray.size(); j++) {
PersistentManifold manifold = manifoldArray.getQuick(j);
double directionSign = manifold.getBody0() == ghostObject ? -1.0f : 1.0f;
for (int p = 0; p < manifold.getNumContacts(); p++) {
ManifoldPoint pt = manifold.getContactPoint(p);
double dist = pt.getDistance();
if (dist < 0.0f) {
if (dist < maxPen) {
maxPen = dist;
touchingNormal.set(pt.normalWorldOnB); // ??
touchingNormal.scale(directionSign);
}
currentPosition.scaleAdd(
directionSign * dist * 0.2f, pt.normalWorldOnB, currentPosition);
penetration = true;
} else {
// printf("touching %f\n", dist);
}
}
// manifold->clearManifold();
}
}
Transform newTrans = ghostObject.getWorldTransform(new Transform());
newTrans.origin.set(currentPosition);
ghostObject.setWorldTransform(newTrans);
// printf("m_touchingNormal =
// %f,%f,%f\n",m_touchingNormal[0],m_touchingNormal[1],m_touchingNormal[2]);
// System.out.println("recoverFromPenetration "+penetration+" "+touchingNormal);
return penetration;
}
protected void stepDown(CollisionWorld collisionWorld, double dt) {
Transform start = new Transform();
Transform end = new Transform();
// phase 3: down
double additionalDownStep = (wasOnGround /*&& !onGround()*/) ? stepHeight : 0.0f;
Vector3d step_drop = new Vector3d();
step_drop.scale(currentStepOffset + additionalDownStep, upAxisDirection[upAxis]);
double downVelocity =
(additionalDownStep == 0.0f && verticalVelocity < 0.0f ? -verticalVelocity : 0.0f) * dt;
Vector3d gravity_drop = new Vector3d();
gravity_drop.scale(downVelocity, upAxisDirection[upAxis]);
targetPosition.sub(step_drop);
targetPosition.sub(gravity_drop);
start.setIdentity();
end.setIdentity();
start.origin.set(currentPosition);
end.origin.set(targetPosition);
KinematicClosestNotMeConvexResultCallback callback =
new KinematicClosestNotMeConvexResultCallback(
ghostObject, upAxisDirection[upAxis], maxSlopeCosine);
callback.collisionFilterGroup = getGhostObject().getBroadphaseHandle().collisionFilterGroup;
callback.collisionFilterMask = getGhostObject().getBroadphaseHandle().collisionFilterMask;
if (useGhostObjectSweepTest) {
ghostObject.convexSweepTest(
convexShape,
start,
end,
callback,
collisionWorld.getDispatchInfo().allowedCcdPenetration);
} else {
collisionWorld.convexSweepTest(convexShape, start, end, callback);
}
if (callback.hasHit()) {
// we dropped a fraction of the height -> hit floor
currentPosition.interpolate(currentPosition, targetPosition, callback.closestHitFraction);
verticalVelocity = 0.0f;
verticalOffset = 0.0f;
} else {
// we dropped the full height
currentPosition.set(targetPosition);
}
}
protected void stepUp(CollisionWorld world) {
// phase 1: up
Transform start = new Transform();
Transform end = new Transform();
targetPosition.scaleAdd(
stepHeight + (verticalOffset > 0.0 ? verticalOffset : 0.0f),
upAxisDirection[upAxis],
currentPosition);
start.setIdentity();
end.setIdentity();
/* FIXME: Handle penetration properly */
start.origin.scaleAdd(
convexShape.getMargin() + addedMargin, upAxisDirection[upAxis], currentPosition);
end.origin.set(targetPosition);
// Find only sloped/flat surface hits, avoid wall and ceiling hits...
Vector3d up = new Vector3d();
up.scale(-1f, upAxisDirection[upAxis]);
KinematicClosestNotMeConvexResultCallback callback =
new KinematicClosestNotMeConvexResultCallback(ghostObject, up, 0.0f);
callback.collisionFilterGroup = getGhostObject().getBroadphaseHandle().collisionFilterGroup;
callback.collisionFilterMask = getGhostObject().getBroadphaseHandle().collisionFilterMask;
if (useGhostObjectSweepTest) {
ghostObject.convexSweepTest(
convexShape, start, end, callback, world.getDispatchInfo().allowedCcdPenetration);
} else {
world.convexSweepTest(convexShape, start, end, callback);
}
if (callback.hasHit()) {
// we moved up only a fraction of the step height
currentStepOffset = stepHeight * callback.closestHitFraction;
currentPosition.interpolate(currentPosition, targetPosition, callback.closestHitFraction);
verticalVelocity = 0.0f;
verticalOffset = 0.0f;
} else {
currentStepOffset = stepHeight;
currentPosition.set(targetPosition);
}
}
protected void stepForwardAndStrafe(CollisionWorld collisionWorld, Vector3d walkMove) {
// printf("m_normalizedDirection=%f,%f,%f\n",
// m_normalizedDirection[0],m_normalizedDirection[1],m_normalizedDirection[2]);
// phase 2: forward and strafe
Transform start = new Transform();
Transform end = new Transform();
targetPosition.add(currentPosition, walkMove);
start.setIdentity();
end.setIdentity();
double fraction = 1.0f;
Vector3d distance2Vec = new Vector3d();
distance2Vec.sub(currentPosition, targetPosition);
double distance2 = distance2Vec.lengthSquared();
// printf("distance2=%f\n",distance2);
/*if (touchingContact) {
if (normalizedDirection.dot(touchingNormal) > 0.0f) {
updateTargetPositionBasedOnCollision(touchingNormal);
}
}*/
int maxIter = 10;
while (fraction > 0.01f && maxIter-- > 0) {
start.origin.set(currentPosition);
end.origin.set(targetPosition);
KinematicClosestNotMeConvexResultCallback callback =
new KinematicClosestNotMeConvexResultCallback(
ghostObject, upAxisDirection[upAxis], -1.0f);
callback.collisionFilterGroup = getGhostObject().getBroadphaseHandle().collisionFilterGroup;
callback.collisionFilterMask = getGhostObject().getBroadphaseHandle().collisionFilterMask;
double margin = convexShape.getMargin();
convexShape.setMargin(margin + addedMargin);
if (useGhostObjectSweepTest) {
ghostObject.convexSweepTest(
convexShape,
start,
end,
callback,
collisionWorld.getDispatchInfo().allowedCcdPenetration);
} else {
collisionWorld.convexSweepTest(convexShape, start, end, callback);
}
convexShape.setMargin(margin);
fraction -= callback.closestHitFraction;
if (callback.hasHit()) {
// we moved only a fraction
Vector3d hitDistanceVec = new Vector3d();
hitDistanceVec.sub(callback.hitPointWorld, currentPosition);
// double hitDistance = hitDistanceVec.length();
// if the distance is farther than the collision margin, move
// if (hitDistance > addedMargin) {
// //printf("callback.m_closestHitFraction=%f\n",callback.m_closestHitFraction);
// currentPosition.interpolate(currentPosition, targetPosition,
// callback.closestHitFraction);
// }
updateTargetPositionBasedOnCollision(callback.hitNormalWorld);
Vector3d currentDir = new Vector3d();
currentDir.sub(targetPosition, currentPosition);
distance2 = currentDir.lengthSquared();
if (distance2 > BulletGlobals.SIMD_EPSILON) {
currentDir.normalize();
// see Quake2: "If velocity is against original velocity, stop ead to avoid tiny
// oscilations in sloping corners."
if (currentDir.dot(normalizedDirection) <= 0.0f) {
break;
}
} else {
// printf("currentDir: don't normalize a zero vector\n");
break;
}
} else {
// we moved whole way
currentPosition.set(targetPosition);
}
// if (callback.m_closestHitFraction == 0.f)
// break;
}
}