add swept-sphere collision check implementation

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;
+	}
 }
 

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) {

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);
+	}
+}