From 79bae46cb1f284f0ef2ab667e4b65e173965f795 Mon Sep 17 00:00:00 2001 From: gered Date: Sat, 14 Sep 2013 17:33:54 -0400 Subject: [PATCH] add swept-sphere collision check implementation --- .../blarg/gdx/math/IntersectionTester.java | 215 ++++++++++++++++++ src/com/blarg/gdx/math/MathHelpers.java | 32 +++ .../gdx/math/SweptSphereCollisionPacket.java | 41 ++++ 3 files changed, 288 insertions(+) create mode 100644 src/com/blarg/gdx/math/SweptSphereCollisionPacket.java diff --git a/src/com/blarg/gdx/math/IntersectionTester.java b/src/com/blarg/gdx/math/IntersectionTester.java index f9ea4e8..7180169 100644 --- a/src/com/blarg/gdx/math/IntersectionTester.java +++ b/src/com/blarg/gdx/math/IntersectionTester.java @@ -1,5 +1,6 @@ package com.blarg.gdx.math; +import com.badlogic.gdx.math.MathUtils; import com.badlogic.gdx.math.Plane; import com.badlogic.gdx.math.Vector3; import com.badlogic.gdx.math.collision.BoundingBox; @@ -310,5 +311,219 @@ public class IntersectionTester { else return false; } + + static final Vector3 p1 = new Vector3(); + static final Vector3 p2 = new Vector3(); + static final Vector3 p3 = new Vector3(); + static final Plane trianglePlane = new Plane(Vector3.Zero, 0.0f); + static final Vector3 planeIntersectionPoint = new Vector3(); + static final Vector3 collisionPoint = new Vector3(); + static final Vector3 edge = new Vector3(); + static final Vector3 baseToVertex = new Vector3(); + + public static boolean sweptSphereTest(SweptSphereCollisionPacket packet, final Vector3 v1, final Vector3 v2, final Vector3 v3) { + boolean foundCollision = false; + + tmp1.set(1.0f / packet.ellipsoidRadius.x, 1.0f / packet.ellipsoidRadius.y, 1.0f / packet.ellipsoidRadius.z); + p1.set(v1).scl(tmp1); + p2.set(v2).scl(tmp1); + p3.set(v3).scl(tmp1); + + trianglePlane.set(p1, p2, p3); + + // Is the triangle front-facing to the entity's velocity? + if (trianglePlane.isFrontFacing(packet.esNormalizedVelocity)) { + float t0; + float t1; + boolean embeddedInPlane = false; + float distToTrianglePlane = trianglePlane.distance(packet.esPosition); + float normalDotVelocity = trianglePlane.normal.dot(packet.esVelocity); + + // Is the sphere travelling parallel to the plane? + if (normalDotVelocity == 0.0f) { + if (Math.abs(distToTrianglePlane) >= 1.0f) { + // Sphere is not embedded in the plane, no collision possible + return false; + } else { + // Sphere is embedded in the plane, it intersects throughout the whole time period + embeddedInPlane = true; + t0 = 0.0f; + t1 = 1.0f; + } + } else { + // Not travelling parallel to the plane + t0 = (-1.0f - distToTrianglePlane) / normalDotVelocity; + t1 = (1.0f - distToTrianglePlane) / normalDotVelocity; + + // Swap so t0 < t1 + if (t0 > t1) { + float temp = t1; + t1 = t0; + t0 = temp; + } + + // Check that at least one result is within range + if (t0 > 1.0f || t1 < 0.0f) { + // Both values outside the range [0,1], no collision possible + return false; + } + + t0 = MathUtils.clamp(t0, 0.0f, 1.0f); + t1 = MathUtils.clamp(t1, 0.0f, 1.0f); + } + + // At this point, we have two time values (t0, t1) between which the + // swept sphere intersects with the triangle plane + float t = 1.0f; + + // First, check for a collision inside the triangle. This will happen + // at time t0 if at all as this is when the sphere rests on the front + // side of the triangle plane. + if (!embeddedInPlane) { + // planeIntersectionPoint = (packet.esPosition - trianglePlane.normal) + packet.esVelocity * t0 + tmp1.set(packet.esVelocity).scl(t0); + planeIntersectionPoint + .set(packet.esPosition) + .sub(trianglePlane.normal) + .add(tmp1); + + if (test(planeIntersectionPoint, p1, p2, p3)) { + foundCollision = true; + t = t0; + collisionPoint.set(planeIntersectionPoint); + } + } + + // If we haven't found a collision at this point, we need to check the + // points and edges of the triangle + if (!foundCollision) { + Vector3 velocity = packet.esVelocity; + Vector3 base = packet.esPosition; + float velocitySquaredLength = velocity.len2(); + float a, b, c; + float newT; + + // For each vertex or edge, we have a quadratic equation to be solved + // Check against the points first + + a = velocitySquaredLength; + + // P1 + b = 2.0f * velocity.dot(tmp1.set(base).sub(p1)); + c = tmp1.set(p1).sub(base).len2() - 1.0f; + newT = MathHelpers.getLowestQuadraticRoot(a, b, c, t); + if (!Float.isNaN(newT)) { + t = newT; + foundCollision = true; + collisionPoint.set(p1); + } + + // P2 + b = 2.0f * velocity.dot(tmp1.set(base).sub(p2)); + c = tmp1.set(p2).sub(base).len2() - 1.0f; + newT = MathHelpers.getLowestQuadraticRoot(a, b, c, t); + if (!Float.isNaN(newT)) { + t = newT; + foundCollision = true; + collisionPoint.set(p2); + } + + // P3 + b = 2.0f * velocity.dot(tmp1.set(base).sub(p3)); + c = tmp1.set(p3).sub(base).len2() - 1.0f; + newT = MathHelpers.getLowestQuadraticRoot(a, b, c, t); + if (!Float.isNaN(newT)) { + t = newT; + foundCollision = true; + collisionPoint.set(p3); + } + + // Now check against the edges + + // P1 -> P2 + edge.set(p2).sub(p1); + baseToVertex.set(p1).sub(base); + float edgeSquaredLength = edge.len2(); + float edgeDotVelocity = edge.dot(velocity); + float edgeDotBaseToVertex = edge.dot(baseToVertex); + + a = edgeSquaredLength * -velocitySquaredLength + edgeDotVelocity * edgeDotVelocity; + b = edgeSquaredLength * (2.0f * velocity.dot(baseToVertex)) - 2.0f * edgeDotVelocity * edgeDotBaseToVertex; + c = edgeSquaredLength * (1.0f - baseToVertex.len2()) + edgeDotBaseToVertex * edgeDotBaseToVertex; + + newT = MathHelpers.getLowestQuadraticRoot(a, b, c, t); + if (!Float.isNaN(newT)) { + // Check if intersection is within line segment + float f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeSquaredLength; + if (f >= 0.0f && f <= 1.0f) { + // Intersection took place within the segment + t = newT; + foundCollision = true; + collisionPoint.set(edge).scl(f).add(p1); + } + } + + // P2 -> P3 + edge.set(p3).sub(p2); + baseToVertex.set(p2).sub(base); + edgeSquaredLength = edge.len2(); + edgeDotVelocity = edge.dot(velocity); + edgeDotBaseToVertex = edge.dot(baseToVertex); + + a = edgeSquaredLength * -velocitySquaredLength + edgeDotVelocity * edgeDotVelocity; + b = edgeSquaredLength * (2.0f * velocity.dot(baseToVertex)) - 2.0f * edgeDotVelocity * edgeDotBaseToVertex; + c = edgeSquaredLength * (1.0f - baseToVertex.len2()) + edgeDotBaseToVertex * edgeDotBaseToVertex; + + newT = MathHelpers.getLowestQuadraticRoot(a, b, c, t); + if (!Float.isNaN(newT)) { + // Check if intersection is within line segment + float f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeSquaredLength; + if (f >= 0.0f && f <= 1.0f) { + // Intersection took place within the segment + t = newT; + foundCollision = true; + collisionPoint.set(edge).scl(f).add(p2); + } + } + + // P3 -> P1 + edge.set(p1).sub(p3); + baseToVertex.set(p3).sub(base); + edgeSquaredLength = edge.len2(); + edgeDotVelocity = edge.dot(velocity); + edgeDotBaseToVertex = edge.dot(baseToVertex); + + a = edgeSquaredLength * -velocitySquaredLength + edgeDotVelocity * edgeDotVelocity; + b = edgeSquaredLength * (2.0f * velocity.dot(baseToVertex)) - 2.0f * edgeDotVelocity * edgeDotBaseToVertex; + c = edgeSquaredLength * (1.0f - baseToVertex.len2()) + edgeDotBaseToVertex * edgeDotBaseToVertex; + + newT = MathHelpers.getLowestQuadraticRoot(a, b, c, t); + if (!Float.isNaN(newT)) { + // Check if intersection is within line segment + float f = (edgeDotVelocity * newT - edgeDotBaseToVertex) / edgeSquaredLength; + if (f >= 0.0f && f <= 1.0f) { + // Intersection took place within the segment + t = newT; + foundCollision = true; + collisionPoint.set(edge).scl(f).add(p3); + } + } + } + + // Set result of test + if (foundCollision) { + float distanceToCollision = t * packet.esVelocity.len(); + + // Does this triangle qualify for the closest collision? + if (!packet.foundCollision || distanceToCollision < packet.nearestDistance) { + packet.nearestDistance = distanceToCollision; + packet.esIntersectionPoint.set(collisionPoint); + packet.foundCollision = true; + } + } + } + + return foundCollision; + } } diff --git a/src/com/blarg/gdx/math/MathHelpers.java b/src/com/blarg/gdx/math/MathHelpers.java index 660d21a..b22cf44 100644 --- a/src/com/blarg/gdx/math/MathHelpers.java +++ b/src/com/blarg/gdx/math/MathHelpers.java @@ -206,6 +206,38 @@ public final class MathHelpers { result.z = desiredSize.z / originalSize.z; } + /** + * Basically the same as {@link com.badlogic.gdx.math.Intersector#getLowestPositiveRoot} except for the addition + * of a parameter maxR which limits the maximum root value we accept. Anything over this will also result + * in a Float.NaN return value. A Float.NaN return means "no valid solution." + */ + public static float getLowestQuadraticRoot (float a, float b, float c, float maxR) { + float determinant = (b * b) - (4.0f * a * c); + // if the determinant is negative, there is no solution (can't square root a negative) + if (determinant < 0.0f) + return Float.NaN; + + float sqrtDeterminant = (float)Math.sqrt(determinant); + float root1 = (-b - sqrtDeterminant) / (2.0f * a); + float root2 = (-b + sqrtDeterminant) / (2.0f * a); + + // sort so root1 <= root2 + if (root1 > root2) { + float tmp = root2; + root2 = root1; + root1 = tmp; + } + + // get the lowest root + if (root1 > 0.0f && root1 < maxR) + return root1; + if (root2 > 0.0f && root2 < maxR) + return root2; + + // no valid solutions found + return Float.NaN; + } + // convenience overloads that should not really be used except in non-performance-critical situations public static Vector2 getDirectionVector2(float degrees) { diff --git a/src/com/blarg/gdx/math/SweptSphereCollisionPacket.java b/src/com/blarg/gdx/math/SweptSphereCollisionPacket.java new file mode 100644 index 0000000..f270c68 --- /dev/null +++ b/src/com/blarg/gdx/math/SweptSphereCollisionPacket.java @@ -0,0 +1,41 @@ +package com.blarg.gdx.math; + +import com.badlogic.gdx.math.Vector3; + +public final class SweptSphereCollisionPacket { + // defines the x/y/z radius of the entity being checked + public final Vector3 ellipsoidRadius = new Vector3(); + + public boolean foundCollision; + public float nearestDistance; + + // the below fields are all in "ellipsoid space" + + public final Vector3 esVelocity = new Vector3(); // velocity of the entity + public final Vector3 esNormalizedVelocity = new Vector3(); + public final Vector3 esPosition = new Vector3(); // current position of the entity + + public final Vector3 esIntersectionPoint = new Vector3(); // if an intersection is found + + public void toEllipsoidSpace(Vector3 v, Vector3 out) { + out.x = v.x / ellipsoidRadius.x; + out.y = v.y / ellipsoidRadius.y; + out.z = v.z / ellipsoidRadius.z; + } + + public void fromEllipsoidSpace(Vector3 v, Vector3 out) { + out.x = v.x * ellipsoidRadius.x; + out.y = v.y * ellipsoidRadius.y; + out.z = v.z * ellipsoidRadius.z; + } + + public void reset() { + ellipsoidRadius.set(Vector3.Zero); + foundCollision = false; + nearestDistance = 0.0f; + esVelocity.set(Vector3.Zero); + esNormalizedVelocity.set(Vector3.Zero); + esPosition.set(Vector3.Zero); + esIntersectionPoint.set(Vector3.Zero); + } +}