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;
}
public void playerStep(CollisionWorld collisionWorld, double dt) {
// printf("playerStep(): ");
// printf(" dt = %f", dt);
// quick check...
if (!useWalkDirection && velocityTimeInterval <= 0.0f) {
// printf("\n");
return; // no motion
}
wasOnGround = onGround();
// Update fall velocity.
verticalVelocity -= gravity * dt;
if (verticalVelocity > 0.0 && verticalVelocity > jumpSpeed) {
verticalVelocity = jumpSpeed;
}
if (verticalVelocity < 0.0 && Math.abs(verticalVelocity) > Math.abs(fallSpeed)) {
verticalVelocity = -Math.abs(fallSpeed);
}
verticalOffset = verticalVelocity * dt;
Transform xform = ghostObject.getWorldTransform(new Transform());
// printf("walkDirection(%f,%f,%f)\n",walkDirection[0],walkDirection[1],walkDirection[2]);
// printf("walkSpeed=%f\n",walkSpeed);
stepUp(collisionWorld);
if (useWalkDirection) {
// System.out.println("playerStep 3");
stepForwardAndStrafe(collisionWorld, walkDirection);
} else {
System.out.println("playerStep 4");
// printf(" time: %f", m_velocityTimeInterval);
// still have some time left for moving!
double dtMoving = (dt < velocityTimeInterval) ? dt : velocityTimeInterval;
velocityTimeInterval -= dt;
// how far will we move while we are moving?
Vector3d move = new Vector3d();
move.scale(dtMoving, walkDirection);
// printf(" dtMoving: %f", dtMoving);
// okay, step
stepForwardAndStrafe(collisionWorld, move);
}
stepDown(collisionWorld, dt);
// printf("\n");
xform.origin.set(currentPosition);
ghostObject.setWorldTransform(xform);
}
public void preStep(CollisionWorld collisionWorld) {
int numPenetrationLoops = 0;
touchingContact = false;
while (recoverFromPenetration(collisionWorld)) {
numPenetrationLoops++;
touchingContact = true;
if (numPenetrationLoops > 4) {
// printf("character could not recover from penetration = %d\n", numPenetrationLoops);
break;
}
}
currentPosition.set(ghostObject.getWorldTransform(new Transform()).origin);
targetPosition.set(currentPosition);
// printf("m_targetPosition=%f,%f,%f\n",m_targetPosition[0],m_targetPosition[1],m_targetPosition[2]);
}