diff --git a/src/com/blarg/gdx/math/IntersectionTester.java b/src/com/blarg/gdx/math/IntersectionTester.java index 2284381..6d9bef4 100644 --- a/src/com/blarg/gdx/math/IntersectionTester.java +++ b/src/com/blarg/gdx/math/IntersectionTester.java @@ -1,6 +1,5 @@ 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; @@ -13,7 +12,7 @@ import com.badlogic.gdx.math.collision.Sphere; * tests already, but not all of them. I've just included all of these here for consistency's sake * and to make porting some of my other projects easier. */ -public class IntersectionTester { +public final class IntersectionTester { static final Vector3 tmp1 = new Vector3(); static final Vector3 tmp2 = new Vector3(); static final Vector3 tmp3 = new Vector3(); @@ -311,219 +310,5 @@ 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, Vector3 v1, Vector3 v2, 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/SweptSphereCollisionTester.java b/src/com/blarg/gdx/math/SweptSphereCollisionTester.java new file mode 100644 index 0000000..8fdec90 --- /dev/null +++ b/src/com/blarg/gdx/math/SweptSphereCollisionTester.java @@ -0,0 +1,222 @@ +package com.blarg.gdx.math; + +import com.badlogic.gdx.math.MathUtils; +import com.badlogic.gdx.math.Plane; +import com.badlogic.gdx.math.Vector3; + +public final class SweptSphereCollisionTester { + static final Vector3 tmp1 = new Vector3(); + 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 test(SweptSphereCollisionPacket packet, Vector3 v1, Vector3 v2, Vector3 v3) { + boolean foundCollision = false; + + packet.toEllipsoidSpace(v1, p1); + packet.toEllipsoidSpace(v2, p2); + packet.toEllipsoidSpace(v3, p3); + + trianglePlane.set(p1, p2, p3); + collisionPoint.set(Vector3.Zero); + + // 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 (IntersectionTester.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; + } +}