gered/gdx-toolbox
1
0
Fork 0
Code Issues Pull requests Packages Projects Releases Wiki Activity

add swept-sphere collision check implementation

Browse source
This commit is contained in:
Gered 2013-09-14 17:33:54 -04:00
parent b2882c44e0
commit 79bae46cb1
3 changed files with 288 additions and 0 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

215
src/com/blarg/gdx/math/IntersectionTester.java
View file

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

32
src/com/blarg/gdx/math/MathHelpers.java
View file

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

41
src/com/blarg/gdx/math/SweptSphereCollisionPacket.java Normal file
View file

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