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);
}
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);
}
}
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]);
}
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;
}
}
public void warp(Vector3d origin) {
Transform xform = new Transform();
xform.setIdentity();
xform.origin.set(origin);
ghostObject.setWorldTransform(xform);
}
