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();