uhh... screw it -- lots of fixes

Work in progress code from a couple weeks ago that I just picked up
again today, don't feel like listing everything out separarely.

src/com/blarg/gdx/math/SweptSphere.java

@@ -8,8 +8,18 @@ public class SweptSphere {
 	public boolean foundCollision;
 	public float nearestCollisionDistance;
 	public final Vector3 nearestCollisionPoint = new Vector3();
+
+	public boolean isInMotion;
+	public boolean wasInMotion;
+	public boolean isFalling;
+	public boolean wasFalling;
 	public boolean isOnGround;
+	public boolean wasOnGround;
 	public boolean isSliding;
+	public boolean wasSliding;
+	public float fallDistance;
+	public float currentFallDistance;
+	public float lastPositionY;
 	public final Vector3 slidingPlaneNormal = new Vector3();
 	public final Vector3 slidingPlaneOrigin = new Vector3();
 
@@ -33,13 +43,23 @@ public class SweptSphere {
 
 	public void reset() {
 		position.set(Vector3.Zero);
-		foundCollision = false;
-		nearestCollisionDistance = 0.0f;
 		nearestCollisionPoint.set(Vector3.Zero);
+		isInMotion = false;
+		wasInMotion = false;
+		isFalling = false;
+		wasFalling = false;
 		isOnGround = false;
+		wasOnGround = false;
 		isSliding = false;
+		wasSliding = false;
+		fallDistance = 0.0f;
+		currentFallDistance = 0.0f;
+		lastPositionY = 0.0f;
 		slidingPlaneNormal.set(Vector3.Zero);
 		slidingPlaneOrigin.set(Vector3.Zero);
+
+		foundCollision = false;
+		nearestCollisionDistance = 0.0f;
 		radius.set(Vector3.Zero);
 		esPosition.set(Vector3.Zero);
 		esVelocity.set(Vector3.Zero);

src/com/blarg/gdx/math/SweptSphereHandler.java

@@ -12,6 +12,9 @@ public class SweptSphereHandler {
 	public float collisionVeryCloseDistance = 0.005f;
 	public float onGroundTolerance = 0.1f;
 
+	public final Vector3 possibleCollisionAreaMinOffset = new Vector3();
+	public final Vector3 possibleCollisionAreaMaxOffset = new Vector3();
+
 	static final Vector3 tmp1 = new Vector3();
 	static final BoundingBox possibleCollisionArea = new BoundingBox();
 
@@ -25,32 +28,61 @@ public class SweptSphereHandler {
 		this.maxRecursionDepth = maxRecursionDepth;
 	}
 
-	public void handleMovement(SweptSphere sphere, Vector3 velocity, Vector3 outVelocity, boolean onlySlideIfTooSteep, float tooSteepAngleY) {
+	static final Vector3 postResponseVelocity = new Vector3();
+	static final Vector3 postGravityResponseVelocity = new Vector3();
+	public void handleMovement(SweptSphere sphere,
+	                           Vector3 sphereVelocity,
+	                           Vector3 gravityVelocity,
+	                           Vector3 outVelocity,
+	                           boolean canSlide,
+	                           boolean onlySlideIfTooSteep,
+	                           float tooSteepAngleY) {
+		updateIsInMotionState(sphere, sphereVelocity);
+		updateIsFallingState(sphere);
+
+		calculatePossibleCollisionArea(sphere, sphereVelocity);
+
+		sphere.wasSliding = sphere.isSliding;
+		sphere.isSliding = false;
+		postResponseVelocity.set(Vector3.Zero);
+		handleMovement(sphere, sphereVelocity, postResponseVelocity, canSlide, false, 0.0f);
+		if (gravityVelocity != null) {
+			postGravityResponseVelocity.set(Vector3.Zero);
+			handleMovement(sphere, gravityVelocity, postGravityResponseVelocity, canSlide, onlySlideIfTooSteep, tooSteepAngleY);
+			postResponseVelocity.add(postGravityResponseVelocity);
+		}
+
+		outVelocity.set(postResponseVelocity);
+
+		updateIsOnGroundState(sphere, outVelocity);
+	}
+
+	static final Vector3 esPosition = new Vector3();
+	static final Vector3 esVelocity = new Vector3();
+	static final Vector3 resultingVelocity = new Vector3();
+	public void handleMovement(SweptSphere sphere, Vector3 velocity, Vector3 outVelocity, boolean canSlide, boolean onlySlideIfTooSteep, float tooSteepAngleY) {
 		// don't attempt to process movement if the entity is not moving!
 		if (velocity.len2() > 0.0f) {
 			// calculate maximum possible collision area (world space)
-			calculatePossibleCollisionArea(sphere, velocity);
+			//calculatePossibleCollisionArea(sphere, velocity);
 
 			// convert position and velocity to ellipsoid space
-			Vector3 esPosition = new Vector3();
-			Vector3 esVelocity = new Vector3();
 			SweptSphere.toEllipsoidSpace(sphere.position, sphere.radius, esPosition);
 			SweptSphere.toEllipsoidSpace(velocity, sphere.radius, esVelocity);
 
 			// check for and respond to any collisions along this velocity vector
 			sphere.nearestCollisionDistance = 0.0f;
+			sphere.nearestCollisionPoint.set(Vector3.Zero);
 			sphere.foundCollision = false;
 			sphere.esIntersectionPoint.set(Vector3.Zero);
-			Vector3 resultingVelocity = new Vector3();
+			resultingVelocity.set(Vector3.Zero);
-			Vector3 newEsPosition = getNewPositionForMovement(0, sphere, esPosition, esVelocity, resultingVelocity, true, onlySlideIfTooSteep, tooSteepAngleY);
+			Vector3 newEsPosition = getNewPositionForMovement(0, sphere, esPosition, esVelocity, resultingVelocity, canSlide, onlySlideIfTooSteep, tooSteepAngleY);
 
 			// resulting velocity will have been calculated in ellipsoid space
 			SweptSphere.fromEllipsoidSpace(resultingVelocity, sphere.radius);
 
 			if (sphere.foundCollision)
 				SweptSphere.fromEllipsoidSpace(sphere.esIntersectionPoint, sphere.radius, sphere.nearestCollisionPoint);
-			else
-				sphere.nearestCollisionPoint.set(Vector3.Zero);
 
 			// sliding plane origin will be in ellipsoid space still...
 			SweptSphere.fromEllipsoidSpace(sphere.slidingPlaneOrigin, sphere.radius);
@@ -64,6 +96,12 @@ public class SweptSphereHandler {
 			outVelocity.set(Vector3.Zero);
 	}
 
+	static final Vector3 resultingPosition = new Vector3();
+	static final Vector3 tmpDestination = new Vector3();
+	static final Vector3 tmpNewPosition = new Vector3();
+	static final Vector3 tmpNewDestination = new Vector3();
+	static final Vector3 tmpNewVelocity = new Vector3();
+	static final Plane slidingPlane = new Plane(Vector3.Zero, 0.0f);
 	private Vector3 getNewPositionForMovement(int recursionDepth,
 	                                          SweptSphere sphere,
 	                                          Vector3 currentPosition,
@@ -74,13 +112,13 @@ public class SweptSphereHandler {
 	                                          float tooSteepAngleY) {
 		// don't recurse too much
 		if (recursionDepth > maxRecursionDepth)
-			return currentPosition;
+			return resultingPosition.set(currentPosition);
 
 		responseVelocity.set(velocity);
 
 		// set up the collision check information
 		sphere.esVelocity.set(velocity);
-		sphere.esNormalizedVelocity.set(velocity.nor());
+		sphere.esNormalizedVelocity.set(velocity).nor();
 		sphere.esPosition.set(currentPosition);
 		sphere.foundCollision = false;
 
@@ -89,12 +127,12 @@ public class SweptSphereHandler {
 
 		// if there was no collision, simply move along the velocity vector
 		if (!sphere.foundCollision)
-			return new Vector3(currentPosition).add(velocity);
+			return resultingPosition.set(currentPosition).add(velocity);
 
 		// a collision did occur
 
-		Vector3 destination = new Vector3(currentPosition).add(velocity);
+		tmpDestination.set(currentPosition).add(velocity);
-		Vector3 newPosition = new Vector3(currentPosition);
+		tmpNewPosition.set(currentPosition);
 
 		if (sphere.nearestCollisionDistance >= collisionVeryCloseDistance) {
 			// we haven't yet moved up too close to the nearest collision, so
@@ -113,9 +151,8 @@ public class SweptSphereHandler {
 			if (moveUpLength == 0.0f)
 				moveUpLength = sphere.nearestCollisionDistance - (collisionVeryCloseDistance * 0.5f);
 
-			tmp1.set(velocity);
+			MathHelpers.setLengthOf(tmp1.set(velocity), moveUpLength);
-			MathHelpers.setLengthOf(tmp1, moveUpLength);
+			tmpNewPosition.set(sphere.esPosition).add(tmp1);
-			newPosition.set(sphere.esPosition).add(tmp1);
 
 			// adjust the polygon intersection point, so the sliding plane will be
 			// unaffected by the fact that we move slightly less than the collision
@@ -127,39 +164,41 @@ public class SweptSphereHandler {
 
 		if (!canSlide) {
 			responseVelocity.set(Vector3.Zero);
-			return newPosition;
+			return resultingPosition.set(tmpNewPosition);
 		}
 
 		// we can slide, so determine the sliding plane
 		sphere.slidingPlaneOrigin.set(sphere.esIntersectionPoint);
-		sphere.slidingPlaneNormal.set(newPosition).sub(sphere.esIntersectionPoint).nor();
+		sphere.slidingPlaneNormal.set(tmpNewPosition).sub(sphere.esIntersectionPoint).nor();
-		Plane slidingPlane = new Plane(sphere.slidingPlaneNormal, sphere.slidingPlaneOrigin);
+		slidingPlane.set(sphere.slidingPlaneOrigin, sphere.slidingPlaneNormal);
 
 		// determine slide angle and then check if we need to bail out on sliding
 		// depending on how steep the slide plane is
-		float slidingYAngle = (float)Math.acos(sphere.slidingPlaneNormal.dot(Vector3.Y));
+		float slidingYAngle = (float)Math.acos(sphere.slidingPlaneNormal.dot(Vector3.Y)) * MathUtils.degreesToRadians;
 
-		if (onlySlideIfTooSteep && slidingYAngle < (tooSteepAngleY * MathUtils.degreesToRadians)) {
+		if (onlySlideIfTooSteep && slidingYAngle < tooSteepAngleY) {
 			responseVelocity.set(Vector3.Zero);
-			return newPosition;
+			return resultingPosition.set(tmpNewPosition);
 		}
 
-		tmp1.set(sphere.slidingPlaneNormal).scl(slidingPlane.distance(destination));
+		tmpNewDestination.set(tmpDestination)
-		Vector3 newDestination = new Vector3(destination).sub(tmp1);
+		                 .sub(tmp1.set(sphere.slidingPlaneNormal)
+		                          .scl(slidingPlane.distance(tmpDestination)));
 
 		// generate the slide vector, which will become our new velocity vector
 		// for the next iteration
 		sphere.isSliding = true;
-		Vector3 newVelocity = new Vector3(newDestination).sub(sphere.esIntersectionPoint);
+		tmpNewVelocity.set(tmpNewDestination)
-		responseVelocity.set(newVelocity);
+		              .sub(sphere.esIntersectionPoint);
+		responseVelocity.set(tmpNewVelocity);
 
 		// don't recurse if the velocity is very small
-		if (newVelocity.len() < collisionVeryCloseDistance)
+		if (tmpNewVelocity.len() < collisionVeryCloseDistance)
-			return newPosition;
+			return resultingPosition.set(tmpNewPosition);
 
 		// recurse
 		++recursionDepth;
-		return getNewPositionForMovement(recursionDepth, sphere, newPosition, newVelocity, responseVelocity, canSlide, onlySlideIfTooSteep, tooSteepAngleY);
+		return getNewPositionForMovement(recursionDepth, sphere, tmpNewPosition, tmpNewVelocity, responseVelocity, canSlide, onlySlideIfTooSteep, tooSteepAngleY);
 	}
 
 	private void calculatePossibleCollisionArea(SweptSphere sphere, Vector3 velocity) {
@@ -169,10 +208,48 @@ public class SweptSphereHandler {
 		possibleCollisionArea.min.x = Math.min(sphere.position.x, tmp1.x) - radius.x;
 		possibleCollisionArea.min.y = Math.min(sphere.position.y, tmp1.y) - radius.y;
 		possibleCollisionArea.min.z = Math.min(sphere.position.z, tmp1.z) - radius.z;
+		possibleCollisionArea.min.add(possibleCollisionAreaMinOffset);
 
 		possibleCollisionArea.max.x = Math.max(sphere.position.x, tmp1.x) + radius.x;
 		possibleCollisionArea.max.y = Math.max(sphere.position.y, tmp1.y) + radius.y;
 		possibleCollisionArea.max.z = Math.max(sphere.position.z, tmp1.z) + radius.z;
+		possibleCollisionArea.max.add(possibleCollisionAreaMaxOffset);
 	}
 
+	private void updateIsInMotionState(SweptSphere sphere, Vector3 velocity) {
+		sphere.wasInMotion = sphere.isInMotion;
+
+		if (MathHelpers.areAlmostEqual(velocity.len(), 0.0f))
+			sphere.isInMotion = false;
+		else
+			sphere.isInMotion = true;
+	}
+
+	private void updateIsFallingState(SweptSphere sphere) {
+		float currentYPosition = sphere.position.y;
+
+		sphere.wasFalling = sphere.isFalling;
+
+		if (sphere.isOnGround) {
+			// not falling anymore, total fall distance will be recorded for the
+			// rest of this tick, but will be cleared next tick when this runs again
+			sphere.isFalling = false;
+			sphere.fallDistance = sphere.currentFallDistance;
+			sphere.currentFallDistance = 0.0f;
+		} else if (currentYPosition < sphere.lastPositionY) {
+			// falling (current Y coord is lower then the one from last tick)
+			sphere.isFalling = true;
+			sphere.currentFallDistance += (sphere.lastPositionY - currentYPosition);
+		}
+
+		sphere.lastPositionY = currentYPosition;
+	}
+
+	private void updateIsOnGroundState(SweptSphere sphere, Vector3 velocity) {
+		sphere.wasOnGround = sphere.isOnGround;
+		if (sphere.foundCollision && MathHelpers.areAlmostEqual(velocity.y, 0.0f, onGroundTolerance))
+			sphere.isOnGround = true;
+		else
+			sphere.isOnGround = false;
+	}
 }