From 354041e8d61571b25c6eeb672537a013d3e0fa60 Mon Sep 17 00:00:00 2001 From: Jesse Morgan Date: Fri, 11 Feb 2011 18:53:32 +0000 Subject: Broke the grabbing code but added Alden's collision code. Not sure if its working yet since my Mac doesn't like it. --- src/alden/CollisionDetector.java | 514 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 514 insertions(+) create mode 100644 src/alden/CollisionDetector.java (limited to 'src/alden/CollisionDetector.java') diff --git a/src/alden/CollisionDetector.java b/src/alden/CollisionDetector.java new file mode 100644 index 0000000..f05e9b8 --- /dev/null +++ b/src/alden/CollisionDetector.java @@ -0,0 +1,514 @@ +package alden; +import com.sun.j3d.utils.geometry.*; +import java.util.*; + +import javax.media.j3d.*; +import javax.vecmath.*; + +@SuppressWarnings("restriction") +public class CollisionDetector { + public static final float EPSILON = 0.0001f; + private static final ArrayList EMPTY_COLLISION_LIST = new ArrayList(); + + public static class Triangle { + private Vector3f a; + private Vector3f b; + private Vector3f c; + private Vector3f normal; + private float intercept; + + private static class Line { + public Vector3f point; + public Vector3f direction; + + public Line() { + point = new Vector3f(); + direction = new Vector3f(); + } + } + + private static class TPair { + public float t0; + public float t1; + } + + public Triangle(Tuple3f a, Tuple3f b, Tuple3f c) { + this.a = new Vector3f(a); + this.b = new Vector3f(b); + this.c = new Vector3f(c); + Vector3f tmp = new Vector3f(); + tmp.scaleAdd(-1, a, c); + Vector3f tmp2 = new Vector3f(); + tmp2.scaleAdd(-1, b, c); + normal = new Vector3f(); + normal.cross(tmp, tmp2); + if (normal.lengthSquared() == 0) + throw new IllegalArgumentException("Degenerate triangle"); + normal.normalize(); + intercept = normal.dot(this.a); + } + + // Inspired by Tomas Moller's "A Fast Triangle-Triangle Intersection Test" + public CollisionInfo getIntersection(Triangle other) { + float d_0_0 = other.normal.dot(a) - other.intercept; + float d_0_1 = other.normal.dot(b) - other.intercept; + float d_0_2 = other.normal.dot(c) - other.intercept; + if (Math.abs(d_0_0) < EPSILON) + d_0_0 = 0; + if (Math.abs(d_0_1) < EPSILON) + d_0_1 = 0; + if (Math.abs(d_0_2) < EPSILON) + d_0_2 = 0; + if (d_0_0 != 0 && d_0_1 != 0 && d_0_2 != 0 && Math.signum(d_0_0) == Math.signum(d_0_1) && Math.signum(d_0_1) == Math.signum(d_0_2)) + return null; + + float d_1_0 = normal.dot(other.a) - intercept; + float d_1_1 = normal.dot(other.b) - intercept; + float d_1_2 = normal.dot(other.c) - intercept; + if (Math.abs(d_1_0) < EPSILON) + d_1_0 = 0; + if (Math.abs(d_1_1) < EPSILON) + d_1_1 = 0; + if (Math.abs(d_1_2) < EPSILON) + d_1_2 = 0; + if (d_1_0 != 0 && d_1_1 != 0 && d_1_2 != 0 && Math.signum(d_1_0) == Math.signum(d_1_1) && Math.signum(d_1_1) == Math.signum(d_1_2)) + return null; + + // Coplanar, assume no collision + if (d_0_0 == 0 && d_0_1 == 0 && d_0_2 == 0) + return null; + + Line line = calculateLineOfIntersection(other); + TPair r0 = calculateRegionOfIntersection(line, d_0_0, d_0_1, d_0_2); + TPair r1 = other.calculateRegionOfIntersection(line, d_1_0, d_1_1, d_1_2); + + if (r0.t1 < r1.t0 || r0.t0 > r1.t1) + return null; + + Vector3f contactPoint = new Vector3f(); + if (r0.t0 >= r1.t0 && r0.t1 <= r1.t1) + contactPoint.scaleAdd((r0.t0 + r0.t1) / 2, line.direction, line.point); + else if (r0.t0 <= r1.t0 && r0.t1 >= r1.t1) + contactPoint.scaleAdd((r1.t0 + r1.t1) / 2, line.direction, line.point); + else if (r0.t0 < r1.t0) + contactPoint.scaleAdd((r0.t1 + r1.t0) / 2, line.direction, line.point); + else + contactPoint.scaleAdd((r0.t0 + r1.t1) / 2, line.direction, line.point); + + assert(Math.abs(normal.dot(contactPoint) - intercept) < 0.01); + assert(Math.abs(other.normal.dot(contactPoint) - other.intercept) < 0.01); + + float penetration = Float.NEGATIVE_INFINITY; + boolean useThisNormal = false; + if (d_0_0 <= 0 && d_0_0 >= penetration) + penetration = d_0_0; + if (d_0_1 <= 0 && d_0_1 >= penetration) + penetration = d_0_1; + if (d_0_2 <= 0 && d_0_2 >= penetration) + penetration = d_0_2; + if (d_1_0 <= 0 && d_1_0 >= penetration) { + penetration = d_1_0; + useThisNormal = true; + } + if (d_1_1 <= 0 && d_1_1 >= penetration) { + penetration = d_1_1; + useThisNormal = true; + } + if (d_1_2 <= 0 && d_1_2 >= penetration) { + penetration = d_1_2; + useThisNormal = true; + } + Vector3f contactNormal; + if (useThisNormal) + contactNormal = new Vector3f(normal); + else { + contactNormal = new Vector3f(other.normal); + contactNormal.negate(); + } + + return new CollisionInfo(contactPoint, contactNormal, -penetration); + } + + private Line calculateLineOfIntersection(Triangle other) { + Line line = new Line(); + line.direction.cross(normal, other.normal); + if (Math.abs(line.direction.x) < EPSILON) + line.direction.x = 0; + if (Math.abs(line.direction.y) < EPSILON) + line.direction.y = 0; + if (Math.abs(line.direction.z) < EPSILON) + line.direction.z = 0; + line.direction.normalize(); + + if (line.direction.x != 0) { // x <- 0 + if (normal.y != 0) { + line.point.z = (other.normal.y / normal.y * intercept - other.intercept) / (other.normal.y / normal.y * normal.z - other.normal.z); + line.point.y = (intercept - normal.z * line.point.z) / normal.y; + } else { // normal.z != 0 + line.point.y = (other.normal.z / normal.z * intercept - other.intercept) / (other.normal.z / normal.z * normal.y - other.normal.y); + line.point.z = (intercept - normal.y * line.point.y) / normal.z; + } + } else if (line.direction.y != 0) { // y <- 0 + if (normal.x != 0) { + line.point.z = (other.normal.x / normal.x * intercept - other.intercept) / (other.normal.x / normal.x * normal.z - other.normal.z); + line.point.x = (intercept - normal.z * line.point.z) / normal.x; + } else { // normal.z != 0 + line.point.x = (other.normal.z / normal.z * intercept - other.intercept) / (other.normal.z / normal.z * normal.x - other.normal.x); + line.point.z = (intercept - normal.x * line.point.x) / normal.z; + } + } else { // z <- 0 + if (normal.x != 0) { + line.point.y = (other.normal.x / normal.x * intercept - other.intercept) / (other.normal.x / normal.x * normal.y - other.normal.y); + line.point.x = (intercept - normal.y * line.point.y) / normal.x; + } else { // normal.y != 0 + line.point.x = (other.normal.y / normal.y * intercept - other.intercept) / (other.normal.y / normal.y * normal.x - other.normal.x); + line.point.y = (intercept - normal.x * line.point.x) / normal.y; + } + } + + assert(Math.abs(normal.dot(line.point) - intercept) < 0.01); + assert(Math.abs(other.normal.dot(line.point) - other.intercept) < 0.01); + + return line; + } + + private TPair calculateRegionOfIntersection(Line line, float d0, float d1, float d2) { + Vector3f v0, v1, v2; + if (Math.signum(d0) != 0 && Math.signum(d0) != Math.signum(d1) && Math.signum(d0) != Math.signum(d2)) { + v0 = b; v1 = a; v2 = c; + float tmp = d0; d0 = d1; d1 = tmp; + } else if (Math.signum(d1) != 0 && Math.signum(d0) != Math.signum(d1) && Math.signum(d1) != Math.signum(d2)) { + v0 = a; v1 = b; v2 = c; + } else if (Math.signum(d2) != 0 && Math.signum(d0) != Math.signum(d2) && Math.signum(d1) != Math.signum(d2)) { + v0 = a; v1 = c; v2 = b; + float tmp = d1; d1 = d2; d2 = tmp; + } else if (Math.signum(d0) == 0) { + v0 = b; v1 = a; v2 = c; + float tmp = d0; d0 = d1; d1 = tmp; + } else if (Math.signum(d1) == 0) { + v0 = a; v1 = b; v2 = c; + } else { + v0 = a; v1 = c; v2 = b; + float tmp = d1; d1 = d2; d2 = tmp; + } + + Vector3f tmp = new Vector3f(); + tmp.scaleAdd(-1, line.point, v0); + float p0 = line.direction.dot(tmp); + tmp.scaleAdd(-1, line.point, v1); + float p1 = line.direction.dot(tmp); + tmp.scaleAdd(-1, line.point, v2); + float p2 = line.direction.dot(tmp); + + TPair region = new TPair(); + region.t0 = p0 + (p1 - p0) * d0 / (d0 - d1); + region.t1 = p2 + (p1 - p2) * d2 / (d2 - d1); + if (region.t1 < region.t0) { + float tmp2 = region.t0; + region.t0 = region.t1; + region.t1 = tmp2; + } + return region; + } + + public boolean isAdjacent(Triangle other) { + if (a.equals(other.a)) { + if (b.equals(other.b) || b.equals(other.c) || c.equals(other.b) || c.equals(other.c)) + return true; + } else if (a.equals(other.b)) { + if (b.equals(other.a) || b.equals(other.c) || c.equals(other.a) || c.equals(other.c)) + return true; + } else if (a.equals(other.c)) { + if (b.equals(other.a) || b.equals(other.b) || c.equals(other.a) || c.equals(other.b)) + return true; + } else if ((b.equals(other.b) || b.equals(other.c)) && (c.equals(other.b) || c.equals(other.c))) + return true; + return false; + } + } + + public static ArrayList calculateCollisions(CollidableObject a, CollidableObject b) { + if (a == b) + return EMPTY_COLLISION_LIST; + if (a instanceof HalfSpace) { + if (b instanceof HalfSpace) + return EMPTY_COLLISION_LIST; + if (b instanceof Sphere) + return calculateCollisions((HalfSpace)a, (Sphere)b); + return calculateCollisions((HalfSpace)a, b.getVertices()); + } + if (!a.getBounds().intersect(b.getBounds())) + return EMPTY_COLLISION_LIST; + if (a instanceof Sphere && b instanceof Sphere) + return calculateCollisions((Sphere)a, (Sphere)b); + return CollisionDetector.calculateCollisions(a.getCollisionTriangles(), b.getCollisionTriangles()); + } + + private static ArrayList calculateCollisions(HalfSpace a, Sphere b) { + float penetration = b.radius - (a.normal.dot(b.position) - a.intercept); + if (penetration >= 0) { + Vector3f contactPoint = new Vector3f(); + contactPoint.scaleAdd(-(b.radius - penetration), a.normal, b.position); + assert(Math.abs(a.normal.dot(contactPoint) - a.intercept) < 0.01); + ArrayList collisions = new ArrayList(); + collisions.add(new CollisionInfo(contactPoint, a.normal, penetration)); + return collisions; + } + return EMPTY_COLLISION_LIST; + } + + private static ArrayList calculateCollisions(HalfSpace a, ArrayList setB) { + ArrayList collisions = new ArrayList(); + for (Vector3f vertex : setB) { + float penetration = a.intercept - a.normal.dot(vertex); + if (penetration >= 0) { + Vector3f contactPoint = new Vector3f(); + contactPoint.scaleAdd(penetration, a.normal, vertex); + assert(Math.abs(a.normal.dot(contactPoint) - a.intercept) < 0.01); + collisions.add(new CollisionInfo(contactPoint, a.normal, penetration)); + } + } + return collisions; + } + + private static ArrayList calculateCollisions(Sphere a, Sphere b) { + Vector3f delta = new Vector3f(); + delta.scaleAdd(-1, a.position, b.position); + float penetration = delta.length() - a.radius - b.radius; + if (penetration > 0) + return EMPTY_COLLISION_LIST; + + ArrayList collisions = new ArrayList(); + delta.normalize(); + Vector3f contactPoint = new Vector3f(); + contactPoint.scaleAdd(a.radius + 0.5f * penetration, delta, a.position); + collisions.add(new CollisionInfo(contactPoint, delta, -penetration)); + return collisions; + } + + private static ArrayList calculateCollisions(ArrayList setA, ArrayList setB) { + ArrayList collisions = new ArrayList(); + for (int i = 0; i < setA.size(); i++) + for (int j = 0; j < setB.size(); j++) { + CollisionInfo collision = setA.get(i).getIntersection(setB.get(j)); + if (collision != null) + collisions.add(collision); + } + return collisions; + } + + public static ArrayList extractVertices(Node node) { + ArrayList vertices = new ArrayList(); + extractVertices(node, vertices); + return vertices; + } + + private static void extractVertices(Node node, ArrayList vertices) { + if (node instanceof Primitive) { + Primitive prim = (Primitive)node; + int index = 0; + Shape3D shape; + Transform3D L2V = new Transform3D(); + while ((shape = prim.getShape(index++)) != null) { + shape.getLocalToVworld(L2V); + for (int i = 0; i < shape.numGeometries(); i++) + extractVertices(shape.getGeometry(i), L2V, vertices); + } + } else if (node instanceof Shape3D) { + Shape3D shape = (Shape3D)node; + Transform3D L2V = new Transform3D(); + shape.getLocalToVworld(L2V); + for (int i = 0; i < shape.numGeometries(); i++) + extractVertices(shape.getGeometry(i), L2V, vertices); + } else if (node instanceof Group) { + Group group = (Group)node; + for (int i = 0; i < group.numChildren(); i++) + extractVertices(group.getChild(i), vertices); + } else + throw new IllegalArgumentException("Illegal node type for vertex extraction"); + } + + private static void extractVertices(Geometry geometry, Transform3D transform, ArrayList vertices) { + if (geometry instanceof GeometryArray) { + GeometryArray trueGeometry = (GeometryArray)geometry; + vertices.ensureCapacity(vertices.size() + trueGeometry.getValidVertexCount()); + Point3f vertex = new Point3f(); + for (int i = 0; i < trueGeometry.getValidVertexCount(); i++) { + trueGeometry.getCoordinate(i, vertex); + transform.transform(vertex); + vertices.add(new Vector3f(vertex)); + } + } else + throw new IllegalArgumentException("Illegal geometry type for vertex extraction"); + } + + public static ArrayList triangularize(Node node) { + ArrayList triangles = new ArrayList(); + triangularize(node, triangles); + return triangles; + } + + private static void triangularize(Node node, ArrayList triangles) { + if (node instanceof Primitive) { + Primitive prim = (Primitive)node; + triangles.ensureCapacity(prim.getNumTriangles()); + int index = 0; + Shape3D shape; + Transform3D L2V = new Transform3D(); + while ((shape = prim.getShape(index++)) != null) { + shape.getLocalToVworld(L2V); + for (int i = 0; i < shape.numGeometries(); i++) + triangularize(shape.getGeometry(i), L2V, triangles); + } + } else if (node instanceof Shape3D) { + Shape3D shape = (Shape3D)node; + Transform3D L2V = new Transform3D(); + shape.getLocalToVworld(L2V); + for (int i = 0; i < shape.numGeometries(); i++) + triangularize(shape.getGeometry(i), L2V, triangles); + } else if (node instanceof Group) { + Group group = (Group)node; + for (int i = 0; i < group.numChildren(); i++) + triangularize(group.getChild(i), triangles); + } else + throw new IllegalArgumentException("Illegal node type for triangularization"); + } + + private static void triangularize(Geometry geometry, Transform3D transform, ArrayList triangles) { + if (geometry instanceof TriangleArray) { + TriangleArray trueGeometry = (TriangleArray)geometry; + Point3f vertices[] = new Point3f[trueGeometry.getValidVertexCount()]; + for (int i = 0; i < vertices.length; i++) { + vertices[i] = new Point3f(); + trueGeometry.getCoordinate(i, vertices[i]); + transform.transform(vertices[i]); + } + for (int i = 0; i < vertices.length; i += 3) + try { + triangles.add(new Triangle(vertices[i], vertices[i+1], vertices[i+2])); + } catch (IllegalArgumentException e) { + } + } else if (geometry instanceof TriangleStripArray) { + TriangleStripArray trueGeometry = (TriangleStripArray)geometry; + int stripVertexCounts[] = new int[trueGeometry.getNumStrips()]; + trueGeometry.getStripVertexCounts(stripVertexCounts); + Point3f vertices[] = new Point3f[trueGeometry.getValidVertexCount()]; + for (int i = 0; i < vertices.length; i++) { + vertices[i] = new Point3f(); + trueGeometry.getCoordinate(i, vertices[i]); + transform.transform(vertices[i]); + } + int base = 0; + for (int i = 0; i < stripVertexCounts.length; i++) { + boolean reverse = false; + for (int j = 2; j < stripVertexCounts[i]; j++) { + try { + if (reverse) + triangles.add(new Triangle(vertices[base+j-2], vertices[base+j], vertices[base+j-1])); + else + triangles.add(new Triangle(vertices[base+j-2], vertices[base+j-1], vertices[base+j])); + } catch (IllegalArgumentException e) { + } + reverse = !reverse; + } + base += stripVertexCounts[i]; + } + } else if (geometry instanceof TriangleFanArray) { + TriangleFanArray trueGeometry = (TriangleFanArray)geometry; + int stripVertexCounts[] = new int[trueGeometry.getNumStrips()]; + trueGeometry.getStripVertexCounts(stripVertexCounts); + Point3f vertices[] = new Point3f[trueGeometry.getValidVertexCount()]; + for (int i = 0; i < vertices.length; i++) { + vertices[i] = new Point3f(); + trueGeometry.getCoordinate(i, vertices[i]); + transform.transform(vertices[i]); + } + int base = 0; + for (int i = 0; i < stripVertexCounts.length; i++) { + for (int j = 2; j < stripVertexCounts[i]; j++) + try { + triangles.add(new Triangle(vertices[base], vertices[base+j-1], vertices[base+j])); + } catch (IllegalArgumentException e) { + } + base += stripVertexCounts[i]; + } + } else if (geometry instanceof IndexedTriangleArray) { + IndexedTriangleArray trueGeometry = (IndexedTriangleArray)geometry; + Point3f vertices[] = new Point3f[trueGeometry.getValidVertexCount()]; + for (int i = 0; i < vertices.length; i++) { + vertices[i] = new Point3f(); + trueGeometry.getCoordinate(i, vertices[i]); + transform.transform(vertices[i]); + } + int indices[] = new int[trueGeometry.getValidIndexCount()]; + trueGeometry.getCoordinateIndices(0, indices); + for (int i = 0; i < indices.length; i += 3) + try { + triangles.add(new Triangle(vertices[indices[i]], vertices[indices[i+1]], vertices[indices[i+2]])); + } catch (IllegalArgumentException e) { + } + } else if (geometry instanceof IndexedTriangleStripArray) { + IndexedTriangleStripArray trueGeometry = (IndexedTriangleStripArray)geometry; + int stripIndexCounts[] = new int[trueGeometry.getNumStrips()]; + trueGeometry.getStripIndexCounts(stripIndexCounts); + Point3f vertices[] = new Point3f[trueGeometry.getValidVertexCount()]; + for (int i = 0; i < vertices.length; i++) { + vertices[i] = new Point3f(); + trueGeometry.getCoordinate(i, vertices[i]); + transform.transform(vertices[i]); + } + int indices[] = new int[trueGeometry.getValidIndexCount()]; + trueGeometry.getCoordinateIndices(0, indices); + int base = 0; + for (int i = 0; i < stripIndexCounts.length; i++) { + boolean reverse = false; + for (int j = 2; j < stripIndexCounts[i]; j++) { + try { + if (reverse) + triangles.add(new Triangle(vertices[indices[base+j-2]], vertices[indices[base+j]], vertices[indices[base+j-1]])); + else + triangles.add(new Triangle(vertices[indices[base+j-2]], vertices[indices[base+j-1]], vertices[indices[base+j]])); + } catch (IllegalArgumentException e) { + } + reverse = !reverse; + } + base += stripIndexCounts[i]; + } + } else if (geometry instanceof IndexedTriangleFanArray) { + IndexedTriangleFanArray trueGeometry = (IndexedTriangleFanArray)geometry; + int stripIndexCounts[] = new int[trueGeometry.getNumStrips()]; + trueGeometry.getStripIndexCounts(stripIndexCounts); + Point3f vertices[] = new Point3f[trueGeometry.getValidVertexCount()]; + for (int i = 0; i < vertices.length; i++) { + vertices[i] = new Point3f(); + trueGeometry.getCoordinate(i, vertices[i]); + transform.transform(vertices[i]); + } + int indices[] = new int[trueGeometry.getValidIndexCount()]; + trueGeometry.getCoordinateIndices(0, indices); + int base = 0; + for (int i = 0; i < stripIndexCounts.length; i++) { + for (int j = 2; j < stripIndexCounts[i]; j++) + try { + triangles.add(new Triangle(vertices[indices[base]], vertices[indices[base+j-1]], vertices[indices[base+j]])); + } catch (IllegalArgumentException e) { + } + base += stripIndexCounts[i]; + } + } else + throw new IllegalArgumentException("Illegal geometry type for triangularization"); + } + + public static Shape3D createShape(ArrayList triangles) { + TriangleArray geometry = new TriangleArray(3 * triangles.size(), TriangleArray.COORDINATES); + for (int i = 0; i < triangles.size(); i++) { + geometry.setCoordinate(3 * i, new Point3f(triangles.get(i).a)); + geometry.setCoordinate(3 * i + 1, new Point3f(triangles.get(i).b)); + geometry.setCoordinate(3 * i + 2, new Point3f(triangles.get(i).c)); + } + Appearance appearance = new Appearance(); + PolygonAttributes polyAttr = new PolygonAttributes(PolygonAttributes.POLYGON_LINE, PolygonAttributes.CULL_NONE, 0); + appearance.setPolygonAttributes(polyAttr); + return new Shape3D(geometry, appearance); + } +} -- cgit v1.2.3