private float moveUp(float riseAmount, CharacterCollider collider, Vector3f position) {
Vector3f to =
new Vector3f(position.x, position.y + riseAmount + VERTICAL_PENETRATION_LEEWAY, position.z);
if (collider != null) {
SweepCallback callback = collider.sweep(position, to, VERTICAL_PENETRATION_LEEWAY, -1f);
if (callback.hasHit()) {
float actualDist =
Math.max(
0,
((riseAmount + VERTICAL_PENETRATION_LEEWAY) * callback.getClosestHitFraction())
- VERTICAL_PENETRATION_LEEWAY);
position.y += actualDist;
return actualDist;
}
}
position.y += riseAmount;
return riseAmount;
}
private boolean moveHorizontal(
Vector3f horizMove,
CharacterCollider collider,
Vector3f position,
float slopeFactor,
float stepHeight) {
float remainingFraction = 1.0f;
float dist = horizMove.length();
if (dist < physics.getEpsilon()) {
return false;
}
boolean horizontalHit = false;
Vector3f normalizedDir = Vector3fUtil.safeNormalize(horizMove, new Vector3f());
if (collider == null) {
// ignore collision
normalizedDir.scale(dist);
position.add(normalizedDir);
return false;
}
Vector3f targetPos = new Vector3f(normalizedDir);
targetPos.scale(dist + HORIZONTAL_PENETRATION_LEEWAY);
targetPos.add(position);
int iteration = 0;
Vector3f lastHitNormal = new Vector3f(0, 1, 0);
while (remainingFraction >= 0.01f && iteration++ < 10) {
SweepCallback callback =
collider.sweep(position, targetPos, HORIZONTAL_PENETRATION, slopeFactor);
/* Note: this isn't quite correct (after the first iteration the closestHitFraction is only for part of the moment)
but probably close enough */
float actualDist =
Math.max(
0,
(dist + HORIZONTAL_PENETRATION_LEEWAY) * callback.getClosestHitFraction()
- HORIZONTAL_PENETRATION_LEEWAY);
if (actualDist != 0) {
remainingFraction -= actualDist / dist;
}
if (callback.hasHit()) {
if (actualDist > physics.getEpsilon()) {
Vector3f actualMove = new Vector3f(normalizedDir);
actualMove.scale(actualDist);
position.add(actualMove);
}
dist -= actualDist;
Vector3f newDir = new Vector3f(normalizedDir);
newDir.scale(dist);
float slope = callback.getHitNormalWorld().dot(new Vector3f(0, 1, 0));
// We step up if we're hitting a big slope, or if we're grazing
// the ground, otherwise we move up a shallow slope.
if (slope < slopeFactor || 1 - slope < physics.getEpsilon()) {
boolean stepping =
checkStep(collider, position, newDir, callback, slopeFactor, stepHeight);
if (!stepping) {
horizontalHit = true;
Vector3f newHorizDir = new Vector3f(newDir.x, 0, newDir.z);
Vector3f horizNormal =
new Vector3f(callback.getHitNormalWorld().x, 0, callback.getHitNormalWorld().z);
if (horizNormal.lengthSquared() > physics.getEpsilon()) {
horizNormal.normalize();
if (lastHitNormal.dot(horizNormal) > physics.getEpsilon()) {
break;
}
lastHitNormal.set(horizNormal);
extractResidualMovement(horizNormal, newHorizDir);
}
newDir.set(newHorizDir);
}
} else {
// Hitting a shallow slope, move up it
Vector3f newHorizDir = new Vector3f(newDir.x, 0, newDir.z);
extractResidualMovement(callback.getHitNormalWorld(), newDir);
Vector3f modHorizDir = new Vector3f(newDir);
modHorizDir.y = 0;
newDir.scale(newHorizDir.length() / modHorizDir.length());
}
float sqrDist = newDir.lengthSquared();
if (sqrDist > physics.getEpsilon()) {
newDir.normalize();
if (newDir.dot(normalizedDir) <= 0.0f) {
break;
}
} else {
break;
}
dist = (float) Math.sqrt(sqrDist);
normalizedDir.set(newDir);
targetPos.set(normalizedDir);
targetPos.scale(dist + HORIZONTAL_PENETRATION_LEEWAY);
targetPos.add(position);
} else {
normalizedDir.scale(dist);
position.add(normalizedDir);
break;
}
}
return horizontalHit;
}
/**
* Sets the location in the physics engine.
*
* @param entity The entity to set the location of.
* @param newPos The new position of the entity.
*/
private void setPhysicsLocation(EntityRef entity, Vector3f newPos) {
CharacterCollider collider = physics.getCharacterCollider(entity);
collider.setLocation(newPos);
}
private boolean moveDown(
float dist, float slopeFactor, CharacterCollider collider, Vector3f position) {
if (collider == null) {
position.y += dist;
return false;
}
float remainingDist = -dist;
Vector3f targetPos = new Vector3f(position);
targetPos.y -= remainingDist + VERTICAL_PENETRATION_LEEWAY;
Vector3f normalizedDir = new Vector3f(0, -1, 0);
boolean hit = false;
int iteration = 0;
while (remainingDist > physics.getEpsilon() && iteration++ < 10) {
SweepCallback callback = collider.sweep(position, targetPos, VERTICAL_PENETRATION, -1.0f);
float actualDist =
Math.max(
0,
(remainingDist + VERTICAL_PENETRATION_LEEWAY) * callback.getClosestHitFraction()
- VERTICAL_PENETRATION_LEEWAY);
Vector3f expectedMove = new Vector3f(targetPos);
expectedMove.sub(position);
if (expectedMove.lengthSquared() > physics.getEpsilon()) {
expectedMove.normalize();
expectedMove.scale(actualDist);
position.add(expectedMove);
}
remainingDist -= actualDist;
if (remainingDist < physics.getEpsilon()) {
break;
}
if (callback.hasHit()) {
float originalSlope = callback.getHitNormalWorld().dot(new Vector3f(0, 1, 0));
if (originalSlope < slopeFactor) {
float slope = callback.calculateAverageSlope(originalSlope, CHECK_FORWARD_DIST);
if (slope < slopeFactor) {
remainingDist -= actualDist;
expectedMove.set(targetPos);
expectedMove.sub(position);
extractResidualMovement(callback.getHitNormalWorld(), expectedMove);
float sqrDist = expectedMove.lengthSquared();
if (sqrDist > physics.getEpsilon()) {
expectedMove.normalize();
if (expectedMove.dot(normalizedDir) <= 0.0f) {
hit = true;
break;
}
} else {
hit = true;
break;
}
if (expectedMove.y > -physics.getEpsilon()) {
hit = true;
break;
}
normalizedDir.set(expectedMove);
expectedMove.scale(-remainingDist / expectedMove.y + HORIZONTAL_PENETRATION_LEEWAY);
targetPos.set(position);
targetPos.add(expectedMove);
} else {
hit = true;
break;
}
} else {
hit = true;
break;
}
} else {
break;
}
}
if (iteration >= 10) {
hit = true;
}
return hit;
}
/**
* Sets the location in the physics engine.
*
* @param entity The entity to set the location of.
* @param newPos The new position of the entity.
*/
private static void setPhysicsLocation(EntityRef entity, Vector3f newPos) {
PhysicsEngine physics = CoreRegistry.get(PhysicsEngine.class);
CharacterCollider collider = physics.getCharacterCollider(entity);
collider.setLocation(newPos);
}