diff --git a/src/com/blarg/gdx/math/SweptSphere.java b/src/com/blarg/gdx/math/SweptSphere.java index a6dde19..e6c7aae 100644 --- a/src/com/blarg/gdx/math/SweptSphere.java +++ b/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); diff --git a/src/com/blarg/gdx/math/SweptSphereHandler.java b/src/com/blarg/gdx/math/SweptSphereHandler.java index 521a402..e9b2d4e 100644 --- a/src/com/blarg/gdx/math/SweptSphereHandler.java +++ b/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(); - Vector3 newEsPosition = getNewPositionForMovement(0, sphere, esPosition, esVelocity, resultingVelocity, true, onlySlideIfTooSteep, tooSteepAngleY); + resultingVelocity.set(Vector3.Zero); + 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); - Vector3 newPosition = new Vector3(currentPosition); + tmpDestination.set(currentPosition).add(velocity); + 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, moveUpLength); - newPosition.set(sphere.esPosition).add(tmp1); + MathHelpers.setLengthOf(tmp1.set(velocity), moveUpLength); + tmpNewPosition.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(); - Plane slidingPlane = new Plane(sphere.slidingPlaneNormal, sphere.slidingPlaneOrigin); + sphere.slidingPlaneNormal.set(tmpNewPosition).sub(sphere.esIntersectionPoint).nor(); + 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)); - Vector3 newDestination = new Vector3(destination).sub(tmp1); + tmpNewDestination.set(tmpDestination) + .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); - responseVelocity.set(newVelocity); + tmpNewVelocity.set(tmpNewDestination) + .sub(sphere.esIntersectionPoint); + responseVelocity.set(tmpNewVelocity); // don't recurse if the velocity is very small - if (newVelocity.len() < collisionVeryCloseDistance) - return newPosition; + if (tmpNewVelocity.len() < collisionVeryCloseDistance) + 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; + } }