diff options
Diffstat (limited to 'src/alden')
| -rw-r--r-- | src/alden/CollidableObject.java | 236 | ||||
| -rw-r--r-- | src/alden/CollisionDetector.java | 514 | ||||
| -rw-r--r-- | src/alden/CollisionDetectorDemo.java | 237 | ||||
| -rw-r--r-- | src/alden/CollisionInfo.java | 15 | ||||
| -rw-r--r-- | src/alden/HalfSpace.java | 28 | ||||
| -rw-r--r-- | src/alden/Sphere.java | 34 | ||||
| -rw-r--r-- | src/alden/UnQuat4f.java | 658 |
7 files changed, 1722 insertions, 0 deletions
diff --git a/src/alden/CollidableObject.java b/src/alden/CollidableObject.java new file mode 100644 index 0000000..d207f8f --- /dev/null +++ b/src/alden/CollidableObject.java @@ -0,0 +1,236 @@ +package alden;
+import java.util.*;
+import javax.media.j3d.*;
+import javax.vecmath.*;
+
+@SuppressWarnings("restriction")
+public abstract class CollidableObject {
+ protected float inverseMass;
+ // The center of mass in the local coordinate system
+ protected Vector3f centerOfMass;
+ protected Vector3f position, previousPosition;
+ protected Vector3f velocity, previousVelocity;
+ protected Vector3f forceAccumulator;
+ protected Quat4f orientation;
+ protected Vector3f angularVelocity;
+ protected Vector3f torqueAccumulator;
+ protected Matrix3f inverseInertiaTensor;
+ protected float coefficientOfRestitution;
+ protected float penetrationCorrection;
+ protected float dynamicFriction;
+ protected BranchGroup BG;
+ protected TransformGroup TG;
+ protected Node node;
+ private ArrayList<Vector3f> vertexCache;
+ private ArrayList<CollisionDetector.Triangle> triangleCache;
+ private Bounds boundsCache;
+ // The inverse inertia tensor in world coordinates
+ private Matrix3f inverseInertiaTensorCache;
+
+ public CollidableObject() {
+ this(1);
+ }
+
+ public CollidableObject(float mass) {
+ if (mass <= 0)
+ throw new IllegalArgumentException();
+ inverseMass = 1 / mass;
+ centerOfMass = new Vector3f();
+ position = new Vector3f();
+ previousPosition = new Vector3f();
+ velocity = new Vector3f();
+ previousVelocity = new Vector3f();
+ forceAccumulator = new Vector3f();
+ orientation = new Quat4f(0, 0, 0, 1);
+ angularVelocity = new Vector3f();
+ torqueAccumulator = new Vector3f();
+ inverseInertiaTensor = new Matrix3f();
+ coefficientOfRestitution = 0.65f;
+ penetrationCorrection = 1.05f;
+ dynamicFriction = 0.02f;
+ TG = new TransformGroup();
+ TG.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
+ BG = new BranchGroup();
+ BG.setCapability(BranchGroup.ALLOW_DETACH);
+ BG.addChild(TG);
+ }
+
+ protected void setShape(Node node) {
+ this.node = node;
+ TG.addChild(node);
+// TG.addChild(CollisionDetector.createShape(CollisionDetector.triangularize(shapeNode)));
+ }
+
+ public Group getGroup() {
+ return BG;
+ }
+
+ public void detach() {
+ BG.detach();
+ }
+
+ public void updateState(float duration) {
+ previousPosition.set(position);
+ previousVelocity.set(velocity);
+ // The force vector now becomes the acceleration vector.
+ forceAccumulator.scale(inverseMass);
+ position.scaleAdd(duration, velocity, position);
+ position.scaleAdd(duration * duration / 2, forceAccumulator, position);
+ velocity.scaleAdd(duration, forceAccumulator, velocity);
+ // The force vector is cleared.
+ forceAccumulator.set(0, 0, 0);
+ angularVelocity.scaleAdd(duration, torqueAccumulator, angularVelocity);
+ torqueAccumulator.set(0, 0, 0);
+ UnQuat4f tmp = new UnQuat4f(angularVelocity.x, angularVelocity.y, angularVelocity.z, 0);
+ tmp.scale(duration / 2);
+ tmp.mul(orientation);
+ orientation.add(tmp);
+ orientation.normalize();
+ }
+
+ protected void updateTransformGroup() {
+ Transform3D tmp = new Transform3D();
+ tmp.setRotation(orientation);
+ tmp.setTranslation(position);
+ TG.setTransform(tmp);
+ clearCaches();
+ }
+
+ protected ArrayList<Vector3f> getVertices() {
+ if (vertexCache == null)
+ vertexCache = CollisionDetector.extractVertices(node);
+ return vertexCache;
+ }
+
+ protected ArrayList<CollisionDetector.Triangle> getCollisionTriangles() {
+ if (triangleCache == null)
+ triangleCache = CollisionDetector.triangularize(node);
+ return triangleCache;
+ }
+
+ protected Bounds getBounds() {
+ if (boundsCache == null) {
+ boundsCache = node.getBounds();
+ Transform3D tmp = new Transform3D();
+ node.getLocalToVworld(tmp);
+ boundsCache.transform(tmp);
+ }
+ return boundsCache;
+ }
+
+ protected Matrix3f getInverseInertiaTensor() {
+ if (inverseInertiaTensorCache == null) {
+ inverseInertiaTensorCache = new Matrix3f();
+ inverseInertiaTensorCache.set(orientation);
+ inverseInertiaTensorCache.invert();
+ inverseInertiaTensorCache.mul(inverseInertiaTensor);
+ }
+ return inverseInertiaTensorCache;
+ }
+
+ protected void clearCaches() {
+ vertexCache = null;
+ triangleCache = null;
+ boundsCache = null;
+ inverseInertiaTensorCache = null;
+ }
+
+ public void resolveCollisions(CollidableObject other) {
+ ArrayList<CollisionInfo> collisions = CollisionDetector.calculateCollisions(this, other);
+ if (collisions.isEmpty())
+ return;
+
+ CollisionInfo finalCollision = null;
+ float max = Float.NEGATIVE_INFINITY;
+ int count = 0;
+ for (CollisionInfo collision : collisions) {
+// float speed = collision.contactNormal.dot(previousVelocity) - collision.contactNormal.dot(other.previousVelocity);
+ Vector3f thisRelativeContactPosition = new Vector3f();
+ thisRelativeContactPosition.scaleAdd(-1, position, collision.contactPoint);
+ thisRelativeContactPosition.scaleAdd(-1, centerOfMass, thisRelativeContactPosition);
+ Vector3f thisContactVelocity = new Vector3f();
+ thisContactVelocity.cross(angularVelocity, thisRelativeContactPosition);
+ thisContactVelocity.add(previousVelocity);
+ Vector3f otherRelativeContactPosition = new Vector3f();
+ otherRelativeContactPosition.scaleAdd(-1, other.position, collision.contactPoint);
+ otherRelativeContactPosition.scaleAdd(-1, other.centerOfMass, otherRelativeContactPosition);
+ Vector3f otherContactVelocity = new Vector3f();
+ otherContactVelocity.cross(other.angularVelocity, otherRelativeContactPosition);
+ otherContactVelocity.add(other.previousVelocity);
+ float speed = collision.contactNormal.dot(thisContactVelocity) - collision.contactNormal.dot(otherContactVelocity);
+ if (speed > 0)
+ if (speed > max + CollisionDetector.EPSILON) {
+ finalCollision = collision;
+ max = speed;
+ count = 1;
+ } else if (speed >= max - CollisionDetector.EPSILON) {
+ finalCollision.contactPoint.add(collision.contactPoint);
+ finalCollision.penetration += collision.penetration;
+ count++;
+ }
+ }
+ if (finalCollision != null) {
+ finalCollision.contactPoint.scale(1f / count);
+ finalCollision.penetration /= count;
+ resolveCollision(other, finalCollision);
+ updateTransformGroup();
+ other.updateTransformGroup();
+ }
+ }
+
+ public void resolveCollision(CollidableObject other, CollisionInfo ci) {
+ if (ci.penetration <= 0)
+ return;
+
+ Vector3f thisRelativeContactPosition = new Vector3f();
+ thisRelativeContactPosition.scaleAdd(-1, position, ci.contactPoint);
+ thisRelativeContactPosition.scaleAdd(-1, centerOfMass, thisRelativeContactPosition);
+ Vector3f thisContactVelocity = new Vector3f();
+ thisContactVelocity.cross(angularVelocity, thisRelativeContactPosition);
+ thisContactVelocity.add(previousVelocity);
+
+ Vector3f otherRelativeContactPosition = new Vector3f();
+ otherRelativeContactPosition.scaleAdd(-1, other.position, ci.contactPoint);
+ otherRelativeContactPosition.scaleAdd(-1, other.centerOfMass, otherRelativeContactPosition);
+ Vector3f otherContactVelocity = new Vector3f();
+ otherContactVelocity.cross(other.angularVelocity, otherRelativeContactPosition);
+ otherContactVelocity.add(other.previousVelocity);
+
+ float initialClosingSpeed = ci.contactNormal.dot(thisContactVelocity) - ci.contactNormal.dot(otherContactVelocity);
+ float finalClosingSpeed = -initialClosingSpeed * coefficientOfRestitution;
+ float deltaClosingSpeed = finalClosingSpeed - initialClosingSpeed;
+ float totalInverseMass = inverseMass + other.inverseMass;
+ if (totalInverseMass == 0)
+ return;
+
+ Vector3f thisAngularVelocityUnit = new Vector3f();
+ thisAngularVelocityUnit.cross(thisRelativeContactPosition, ci.contactNormal);
+ getInverseInertiaTensor().transform(thisAngularVelocityUnit);
+ thisAngularVelocityUnit.cross(thisAngularVelocityUnit, thisRelativeContactPosition);
+ totalInverseMass += thisAngularVelocityUnit.dot(ci.contactNormal);
+
+ Vector3f otherAngularVelocityUnit = new Vector3f();
+ otherAngularVelocityUnit.cross(otherRelativeContactPosition, ci.contactNormal);
+ other.getInverseInertiaTensor().transform(otherAngularVelocityUnit);
+ otherAngularVelocityUnit.cross(otherAngularVelocityUnit, otherRelativeContactPosition);
+ totalInverseMass += otherAngularVelocityUnit.dot(ci.contactNormal);
+
+ Vector3f impulse = new Vector3f(ci.contactNormal);
+ impulse.scale(deltaClosingSpeed / totalInverseMass);
+ velocity.scaleAdd(inverseMass, impulse, velocity);
+ Vector3f tmp = new Vector3f();
+ tmp.cross(thisRelativeContactPosition, impulse);
+ tmp.scale(thisAngularVelocityUnit.dot(ci.contactNormal));
+ getInverseInertiaTensor().transform(tmp);
+ angularVelocity.add(tmp);
+ position.scaleAdd(-ci.penetration * penetrationCorrection * inverseMass / (inverseMass + other.inverseMass), ci.contactNormal, position);
+
+ impulse.negate();
+ other.velocity.scaleAdd(other.inverseMass, impulse, other.velocity);
+ tmp.cross(otherRelativeContactPosition, impulse);
+ tmp.scale(otherAngularVelocityUnit.dot(ci.contactNormal));
+ other.getInverseInertiaTensor().transform(tmp);
+ other.angularVelocity.add(tmp);
+ other.position.scaleAdd(ci.penetration * penetrationCorrection * other.inverseMass / (inverseMass + other.inverseMass), ci.contactNormal, other.position);
+ }
+}
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<CollisionInfo> EMPTY_COLLISION_LIST = new ArrayList<CollisionInfo>();
+
+ 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<CollisionInfo> 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<CollisionInfo> 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<CollisionInfo> collisions = new ArrayList<CollisionInfo>();
+ collisions.add(new CollisionInfo(contactPoint, a.normal, penetration));
+ return collisions;
+ }
+ return EMPTY_COLLISION_LIST;
+ }
+
+ private static ArrayList<CollisionInfo> calculateCollisions(HalfSpace a, ArrayList<Vector3f> setB) {
+ ArrayList<CollisionInfo> collisions = new ArrayList<CollisionInfo>();
+ 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<CollisionInfo> 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<CollisionInfo> collisions = new ArrayList<CollisionInfo>();
+ 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<CollisionInfo> calculateCollisions(ArrayList<Triangle> setA, ArrayList<Triangle> setB) {
+ ArrayList<CollisionInfo> collisions = new ArrayList<CollisionInfo>();
+ 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<Vector3f> extractVertices(Node node) {
+ ArrayList<Vector3f> vertices = new ArrayList<Vector3f>();
+ extractVertices(node, vertices);
+ return vertices;
+ }
+
+ private static void extractVertices(Node node, ArrayList<Vector3f> 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<Vector3f> 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<Triangle> triangularize(Node node) {
+ ArrayList<Triangle> triangles = new ArrayList<Triangle>();
+ triangularize(node, triangles);
+ return triangles;
+ }
+
+ private static void triangularize(Node node, ArrayList<Triangle> 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<Triangle> 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<Triangle> 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);
+ }
+}
diff --git a/src/alden/CollisionDetectorDemo.java b/src/alden/CollisionDetectorDemo.java new file mode 100644 index 0000000..b2268a4 --- /dev/null +++ b/src/alden/CollisionDetectorDemo.java @@ -0,0 +1,237 @@ +package alden;
+import com.sun.j3d.utils.geometry.*;
+import com.sun.j3d.utils.geometry.Box;
+import com.sun.j3d.utils.picking.*;
+import com.sun.j3d.utils.picking.behaviors.*;
+import com.sun.j3d.utils.universe.*;
+import java.awt.*;
+import java.awt.event.*;
+import java.util.*;
+import java.util.List;
+import javax.media.j3d.*;
+import javax.swing.*;
+import javax.swing.Timer;
+import javax.vecmath.*;
+
+@SuppressWarnings("restriction")
+public class CollisionDetectorDemo {
+ private JFrame appFrame;
+ private Canvas3D canvas3D;
+ private BranchGroup scene;
+ private BoundingLeaf originLeaf;
+ // Bounding box defining the periphery of the virtual world.
+ private BoundingBox virtualWorldBounds;
+ // Data needed for adjusting the camera position.
+ private TransformGroup cameraTG;
+ private double cameraXRotation, cameraYRotation, cameraDistance;
+ private MouseEvent lastDragEvent;
+ // A list of all the objects in the world
+ private List<CollidableObject> objects;
+ // A list of all the visual collision points
+ private List<BranchGroup> collisionPoints;
+ // Number of state updates per second
+ private final int UPDATE_RATE = 30;
+
+ public static void main(String[] args) {
+ SwingUtilities.invokeLater(new Runnable() {
+ public void run() {
+ new CollisionDetectorDemo().createAndShowGUI();
+ }
+ });
+ }
+
+ private CollisionDetectorDemo() {
+ final double UNIT = 1f;
+ virtualWorldBounds = new BoundingBox(new Point3d(-UNIT/2, -UNIT/2, -UNIT/2), new Point3d(UNIT/2, UNIT/2, UNIT/2));
+ cameraDistance = 3 * UNIT;
+ objects = new ArrayList<CollidableObject>();
+ collisionPoints = new ArrayList<BranchGroup>();
+ }
+
+ private void createAndShowGUI() {
+ GraphicsConfiguration config = SimpleUniverse.getPreferredConfiguration();
+ canvas3D = new Canvas3D(config);
+ SimpleUniverse universe = new SimpleUniverse(canvas3D);
+ cameraTG = universe.getViewingPlatform().getViewPlatformTransform();
+ updateCamera();
+ universe.getViewer().getView().setSceneAntialiasingEnable(true);
+
+ scene = new BranchGroup();
+ scene.setCapability(BranchGroup.ALLOW_CHILDREN_EXTEND);
+ scene.setCapability(BranchGroup.ALLOW_CHILDREN_WRITE);
+ originLeaf = new BoundingLeaf(new BoundingSphere());
+ scene.addChild(originLeaf);
+ scene.addChild(createVirtualWorldBoundsShape());
+ addObjects();
+ scene.compile();
+ universe.addBranchGraph(scene);
+
+ appFrame = new JFrame("Collision Detector Demo");
+ appFrame.setDefaultCloseOperation(WindowConstants.EXIT_ON_CLOSE);
+ appFrame.add(canvas3D);
+ appFrame.pack();
+ if (Toolkit.getDefaultToolkit().isFrameStateSupported(JFrame.MAXIMIZED_BOTH))
+ appFrame.setExtendedState(appFrame.getExtendedState() | JFrame.MAXIMIZED_BOTH);
+
+ canvas3D.addMouseMotionListener(new MouseMotionAdapter() {
+ public void mouseDragged(MouseEvent e) {
+ if ((e.getModifiersEx() & MouseEvent.BUTTON1_DOWN_MASK) == 0)
+ return;
+ if (lastDragEvent != null) {
+ cameraXRotation += Math.toRadians(e.getY() - lastDragEvent.getY()) / 3;
+ if (cameraXRotation > Math.PI / 2)
+ cameraXRotation = Math.PI / 2;
+ else if (cameraXRotation < -Math.PI / 2)
+ cameraXRotation = -Math.PI / 2;
+ cameraYRotation += Math.toRadians(e.getX() - lastDragEvent.getX()) / 3;
+ updateCamera();
+ }
+ lastDragEvent = e;
+ }
+ public void mouseMoved(MouseEvent e) {
+ lastDragEvent = null;
+ }});
+ canvas3D.addMouseWheelListener(new MouseWheelListener() {
+ public void mouseWheelMoved(MouseWheelEvent e) {
+ if (e.getWheelRotation() > 0)
+ cameraDistance *= 1.05;
+ else if (e.getWheelRotation() < 0)
+ cameraDistance *= 0.95;
+ updateCamera();
+ }
+ });
+ new Timer(1000 / UPDATE_RATE, new ActionListener() {
+ public void actionPerformed(ActionEvent e) {
+ canvas3D.stopRenderer();
+ tick();
+ canvas3D.startRenderer();
+ }
+ }).start();
+
+ appFrame.setVisible(true);
+ }
+
+ private Node createVirtualWorldBoundsShape() {
+ Point3d lower = new Point3d();
+ Point3d upper = new Point3d();
+ virtualWorldBounds.getLower(lower);
+ virtualWorldBounds.getUpper(upper);
+
+ double coordinates[] = {lower.x, lower.y, lower.z, upper.x, lower.y, lower.z,
+ upper.x, lower.y, upper.z, lower.x, lower.y, upper.z,
+ lower.x, upper.y, lower.z, upper.x, upper.y, lower.z,
+ upper.x, upper.y, upper.z, lower.x, upper.y, upper.z};
+ int coordinateIndices[] = {0, 1, 1, 2, 2, 3, 3, 0,
+ 4, 5, 5, 6, 6, 7, 7, 4,
+ 0, 4, 1, 5, 2, 6, 3, 7};
+
+ IndexedLineArray geometry = new IndexedLineArray(coordinates.length / 3, IndexedLineArray.COORDINATES, coordinateIndices.length);
+ geometry.setCoordinates(0, coordinates);
+ geometry.setCoordinateIndices(0, coordinateIndices);
+
+ return new Shape3D(geometry);
+ }
+
+ private void addObjects() {
+ BranchGroup pickables = new BranchGroup();
+ pickables.addChild(new PickTranslateBehavior(pickables, canvas3D, virtualWorldBounds, PickTool.GEOMETRY));
+ pickables.addChild(new PickZoomBehavior(pickables, canvas3D, virtualWorldBounds, PickTool.GEOMETRY));
+ scene.addChild(pickables);
+
+ Appearance appearance = new Appearance();
+ appearance.setTransparencyAttributes(new TransparencyAttributes(TransparencyAttributes.NICEST, 0.2f));
+ appearance.setColoringAttributes(new ColoringAttributes(1, 0.7f, 0.7f, ColoringAttributes.FASTEST));
+ Primitive prim = new com.sun.j3d.utils.geometry.Sphere(0.2f, 0, 15, appearance);
+ objects.add(new GenericCollidableObject(prim, new Vector3f(0, 0, 0.3f), originLeaf));
+
+ appearance = new Appearance();
+ appearance.setTransparencyAttributes(new TransparencyAttributes(TransparencyAttributes.NICEST, 0.2f));
+ appearance.setColoringAttributes(new ColoringAttributes(0.7f, 1, 0.7f, ColoringAttributes.FASTEST));
+ prim = new Box(0.2f, 0.15f, 0.1f, 0, appearance);
+ objects.add(new GenericCollidableObject(prim, new Vector3f(0.3f, 0, 0), originLeaf));
+
+ appearance = new Appearance();
+ appearance.setTransparencyAttributes(new TransparencyAttributes(TransparencyAttributes.NICEST, 0.2f));
+ appearance.setColoringAttributes(new ColoringAttributes(0.7f, 0.7f, 1, ColoringAttributes.FASTEST));
+ prim = new Cylinder(0.2f, 0.4f, 0, 15, 1, appearance);
+ objects.add(new GenericCollidableObject(prim, new Vector3f(0, 0, -0.3f), originLeaf));
+
+ appearance = new Appearance();
+ appearance.setTransparencyAttributes(new TransparencyAttributes(TransparencyAttributes.NICEST, 0.2f));
+ appearance.setColoringAttributes(new ColoringAttributes(1, 1, 0.7f, ColoringAttributes.FASTEST));
+ prim = new com.sun.j3d.utils.geometry.Cone(0.15f, 0.3f, 0, 15, 1, appearance);
+ objects.add(new GenericCollidableObject(prim, new Vector3f(-0.3f, 0, 0), originLeaf));
+
+ for (CollidableObject object : objects)
+ pickables.addChild(object.getGroup());
+ }
+
+ private void tick() {
+ for (BranchGroup BG : collisionPoints)
+ BG.detach();
+ collisionPoints.clear();
+
+ for (int i = 0; i < objects.size() - 1; i++)
+ for (int j = i + 1; j < objects.size(); j++) {
+ ArrayList<CollisionInfo> collisions = CollisionDetector.calculateCollisions(objects.get(i), objects.get(j));
+ Appearance appearance = new Appearance();
+ ColoringAttributes cAttr = new ColoringAttributes(new Color3f(1, 0, 0), ColoringAttributes.FASTEST);
+ appearance.setColoringAttributes(cAttr);
+ for (CollisionInfo ci : collisions) {
+ Transform3D tmp = new Transform3D();
+ tmp.setTranslation(ci.contactPoint);
+ TransformGroup TG = new TransformGroup(tmp);
+ TG.addChild(new com.sun.j3d.utils.geometry.Sphere(0.002f, 0, 8, appearance));
+ BranchGroup BG = new BranchGroup();
+ BG.setCapability(BranchGroup.ALLOW_DETACH);
+ BG.addChild(TG);
+ collisionPoints.add(BG);
+ scene.addChild(BG);
+ }
+ }
+ }
+
+ private void updateCamera() {
+ Transform3D camera3D = new Transform3D();
+ camera3D.setTranslation(new Vector3f(0f, 0f, -(float)cameraDistance));
+ Transform3D tmp = new Transform3D();
+ tmp.rotX(cameraXRotation);
+ camera3D.mul(tmp);
+ tmp.rotY(cameraYRotation);
+ camera3D.mul(tmp);
+ camera3D.invert();
+ cameraTG.setTransform(camera3D);
+ }
+
+ private static class CollisionUpdateBehavior extends Behavior {
+ private GenericCollidableObject gco;
+ private TransformGroup TG;
+
+ public CollisionUpdateBehavior(GenericCollidableObject gco, TransformGroup TG, BoundingLeaf boundingLeaf) {
+ this.gco = gco;
+ this.TG = TG;
+ setSchedulingBoundingLeaf(boundingLeaf);
+ }
+
+ public void initialize() {
+ wakeupOn(new WakeupOnTransformChange(TG));
+ }
+
+ public void processStimulus(Enumeration e) {
+ gco.clearCaches();
+ wakeupOn(new WakeupOnTransformChange(TG));
+ }
+ }
+
+ private static class GenericCollidableObject extends CollidableObject {
+ public GenericCollidableObject(Node shapeNode, Vector3f position, BoundingLeaf boundingLeaf) {
+ setShape(shapeNode);
+ this.position.set(position);
+ TG.setCapability(TransformGroup.ALLOW_TRANSFORM_READ);
+ TG.setCapability(TransformGroup.ENABLE_PICK_REPORTING);
+ TG.addChild(CollisionDetector.createShape(CollisionDetector.triangularize(shapeNode)));
+ BG.addChild(new CollisionUpdateBehavior(this, TG, boundingLeaf));
+ updateTransformGroup();
+ }
+ }
+}
diff --git a/src/alden/CollisionInfo.java b/src/alden/CollisionInfo.java new file mode 100644 index 0000000..9cefcd0 --- /dev/null +++ b/src/alden/CollisionInfo.java @@ -0,0 +1,15 @@ +package alden;
+import javax.vecmath.*;
+
+@SuppressWarnings("restriction")
+public class CollisionInfo {
+ public Vector3f contactPoint;
+ public Vector3f contactNormal;
+ public float penetration;
+
+ public CollisionInfo(Vector3f contactPoint, Vector3f contactNormal, float penetration) {
+ this.contactPoint = contactPoint;
+ this.contactNormal = contactNormal;
+ this.penetration = penetration;
+ }
+}
\ No newline at end of file diff --git a/src/alden/HalfSpace.java b/src/alden/HalfSpace.java new file mode 100644 index 0000000..efab5ea --- /dev/null +++ b/src/alden/HalfSpace.java @@ -0,0 +1,28 @@ +package alden;
+import javax.media.j3d.*;
+import javax.vecmath.*;
+
+@SuppressWarnings("restriction")
+public class HalfSpace extends CollidableObject {
+ protected Vector3f normal;
+ // Right-hand side of the plane equation: Ax + By + Cz = D
+ protected float intercept;
+
+ public HalfSpace(Vector3f position, Vector3f normal) {
+ super(Float.POSITIVE_INFINITY);
+ this.position.set(position);
+ this.normal = new Vector3f(normal);
+ this.normal.normalize();
+ intercept = this.normal.dot(position);
+ }
+
+/* public CollisionInfo calculateCollision(Particle particle) {
+ if (Math.signum(normal.dot(particle.getPosition()) - intercept) == Math.signum(normal.dot(particle.getPreviousPosition()) - intercept))
+ return null;
+
+ Vector3f projection = new Vector3f();
+ projection.scaleAdd(-1, position, particle.getPosition());
+ float penetration = -projection.dot(normal);
+ return new CollisionInfo(normal, penetration);
+ }*/
+}
diff --git a/src/alden/Sphere.java b/src/alden/Sphere.java new file mode 100644 index 0000000..df063a3 --- /dev/null +++ b/src/alden/Sphere.java @@ -0,0 +1,34 @@ +package alden;
+import javax.media.j3d.*;
+import javax.vecmath.*;
+
+@SuppressWarnings("restriction")
+public class Sphere extends CollidableObject {
+ protected float radius;
+
+ public Sphere(float radius, Vector3f position) {
+ this(1, radius, position);
+ }
+
+ public Sphere(float mass, float radius, Vector3f position) {
+ super(mass);
+ setShape(createShape(radius, 22));
+ this.radius = radius;
+ this.position.set(position);
+ if (inverseMass != 0) {
+ inverseInertiaTensor.m00 = 2f / 5 / inverseMass * radius * radius;
+ inverseInertiaTensor.m11 = inverseInertiaTensor.m00;
+ inverseInertiaTensor.m22 = inverseInertiaTensor.m00;
+ inverseInertiaTensor.invert();
+ }
+ updateTransformGroup();
+ }
+
+ protected Node createShape(float radius, int divisions) {
+ Appearance appearance = new Appearance();
+ Material material = new Material();
+ material.setDiffuseColor(0.7f, 0.7f, 1);
+ appearance.setMaterial(material);
+ return new com.sun.j3d.utils.geometry.Sphere(radius, com.sun.j3d.utils.geometry.Sphere.GENERATE_NORMALS, divisions, appearance);
+ }
+}
diff --git a/src/alden/UnQuat4f.java b/src/alden/UnQuat4f.java new file mode 100644 index 0000000..59295a9 --- /dev/null +++ b/src/alden/UnQuat4f.java @@ -0,0 +1,658 @@ +package alden;
+import javax.vecmath.*;
+
+/**
+ * A 4 element unit quaternion represented by single precision floating
+ * point x,y,z,w coordinates.
+ *
+ */
+@SuppressWarnings("restriction")
+public class UnQuat4f extends Tuple4f implements java.io.Serializable {
+
+ // Combatible with 1.1
+ static final long serialVersionUID = 2675933778405442383L;
+
+ final static double EPS = 0.000001;
+ final static double EPS2 = 1.0e-30;
+ final static double PIO2 = 1.57079632679;
+
+ /**
+ * Constructs and initializes a Quat4f from the specified xyzw coordinates.
+ * @param x the x coordinate
+ * @param y the y coordinate
+ * @param z the z coordinate
+ * @param w the w scalar component
+ */
+ public UnQuat4f(float x, float y, float z, float w)
+ {
+ this.x = x;
+ this.y = y;
+ this.z = z;
+ this.w = w;
+
+ }
+
+ /**
+ * Constructs and initializes a Quat4f from the array of length 4.
+ * @param q the array of length 4 containing xyzw in order
+ */
+ public UnQuat4f(float[] q)
+ {
+ x = q[0];
+ y = q[1];
+ z = q[2];
+ w = q[3];
+
+ }
+
+
+ /**
+ * Constructs and initializes a Quat4f from the specified Quat4f.
+ * @param q1 the Quat4f containing the initialization x y z w data
+ */
+ public UnQuat4f(UnQuat4f q1)
+ {
+ super(q1);
+ }
+
+
+ /**
+ * Constructs and initializes a Quat4f from the specified Tuple4f.
+ * @param t1 the Tuple4f containing the initialization x y z w data
+ */
+ public UnQuat4f(Tuple4f t1)
+ {
+ x = t1.x;
+ y = t1.y;
+ z = t1.z;
+ w = t1.w;
+
+ }
+
+
+ /**
+ * Constructs and initializes a Quat4f from the specified Tuple4d.
+ * @param t1 the Tuple4d containing the initialization x y z w data
+ */
+ public UnQuat4f(Tuple4d t1)
+ {
+ x = (float)(t1.x);
+ y = (float)(t1.y);
+ z = (float)(t1.z);
+ w = (float)(t1.w);
+ }
+
+
+ /**
+ * Constructs and initializes a Quat4f to (0.0,0.0,0.0,0.0).
+ */
+ public UnQuat4f()
+ {
+ super();
+ }
+
+
+ /**
+ * Sets the value of this quaternion to the conjugate of quaternion q1.
+ * @param q1 the source vector
+ */
+ public final void conjugate(UnQuat4f q1)
+ {
+ this.x = -q1.x;
+ this.y = -q1.y;
+ this.z = -q1.z;
+ this.w = q1.w;
+ }
+
+ /**
+ * Sets the value of this quaternion to the conjugate of itself.
+ */
+ public final void conjugate()
+ {
+ this.x = -this.x;
+ this.y = -this.y;
+ this.z = -this.z;
+ }
+
+
+ /**
+ * Sets the value of this quaternion to the quaternion product of
+ * quaternions q1 and q2 (this = q1 * q2).
+ * Note that this is safe for aliasing (e.g. this can be q1 or q2).
+ * @param q1 the first quaternion
+ * @param q2 the second quaternion
+ */
+ public final void mul(UnQuat4f q1, UnQuat4f q2)
+ {
+ if (this != q1 && this != q2) {
+ this.w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z;
+ this.x = q1.w*q2.x + q2.w*q1.x + q1.y*q2.z - q1.z*q2.y;
+ this.y = q1.w*q2.y + q2.w*q1.y - q1.x*q2.z + q1.z*q2.x;
+ this.z = q1.w*q2.z + q2.w*q1.z + q1.x*q2.y - q1.y*q2.x;
+ } else {
+ float x, y, w;
+
+ w = q1.w*q2.w - q1.x*q2.x - q1.y*q2.y - q1.z*q2.z;
+ x = q1.w*q2.x + q2.w*q1.x + q1.y*q2.z - q1.z*q2.y;
+ y = q1.w*q2.y + q2.w*q1.y - q1.x*q2.z + q1.z*q2.x;
+ this.z = q1.w*q2.z + q2.w*q1.z + q1.x*q2.y - q1.y*q2.x;
+ this.w = w;
+ this.x = x;
+ this.y = y;
+ }
+ }
+
+
+ /**
+ * Sets the value of this quaternion to the quaternion product of
+ * itself and q1 (this = this * q1).
+ * @param q1 the other quaternion
+ */
+ public final void mul(UnQuat4f q1)
+ {
+ float x, y, w;
+
+ w = this.w*q1.w - this.x*q1.x - this.y*q1.y - this.z*q1.z;
+ x = this.w*q1.x + q1.w*this.x + this.y*q1.z - this.z*q1.y;
+ y = this.w*q1.y + q1.w*this.y - this.x*q1.z + this.z*q1.x;
+ this.z = this.w*q1.z + q1.w*this.z + this.x*q1.y - this.y*q1.x;
+ this.w = w;
+ this.x = x;
+ this.y = y;
+ }
+
+ /**
+ * Sets the value of this quaternion to the quaternion product of
+ * itself and q1 (this = this * q1).
+ * @param q1 the other quaternion
+ */
+ public final void mul(Quat4f q1)
+ {
+ float x, y, w;
+
+ w = this.w*q1.w - this.x*q1.x - this.y*q1.y - this.z*q1.z;
+ x = this.w*q1.x + q1.w*this.x + this.y*q1.z - this.z*q1.y;
+ y = this.w*q1.y + q1.w*this.y - this.x*q1.z + this.z*q1.x;
+ this.z = this.w*q1.z + q1.w*this.z + this.x*q1.y - this.y*q1.x;
+ this.w = w;
+ this.x = x;
+ this.y = y;
+ }
+
+ /**
+ * Multiplies quaternion q1 by the inverse of quaternion q2 and places
+ * the value into this quaternion. The value of both argument quaternions
+ * is preservered (this = q1 * q2^-1).
+ * @param q1 the first quaternion
+ * @param q2 the second quaternion
+ */
+ public final void mulInverse(UnQuat4f q1, UnQuat4f q2)
+ {
+ UnQuat4f tempQuat = new UnQuat4f(q2);
+
+ tempQuat.inverse();
+ this.mul(q1, tempQuat);
+ }
+
+
+
+ /**
+ * Multiplies this quaternion by the inverse of quaternion q1 and places
+ * the value into this quaternion. The value of the argument quaternion
+ * is preserved (this = this * q^-1).
+ * @param q1 the other quaternion
+ */
+ public final void mulInverse(UnQuat4f q1)
+ {
+ UnQuat4f tempQuat = new UnQuat4f(q1);
+
+ tempQuat.inverse();
+ this.mul(tempQuat);
+ }
+
+
+
+ /**
+ * Sets the value of this quaternion to quaternion inverse of quaternion q1.
+ * @param q1 the quaternion to be inverted
+ */
+ public final void inverse(UnQuat4f q1)
+ {
+ this.w = q1.w;
+ this.x = -q1.x;
+ this.y = -q1.y;
+ this.z = -q1.z;
+ }
+
+
+ /**
+ * Sets the value of this quaternion to the quaternion inverse of itself.
+ */
+ public final void inverse()
+ {
+ this.w *= 1;
+ this.x *= -1;
+ this.y *= -1;
+ this.z *= -1;
+ }
+
+
+ /**
+ * Sets the value of this quaternion to the normalized value
+ * of quaternion q1.
+ * @param q1 the quaternion to be normalized.
+ */
+ public final void normalize(UnQuat4f q1)
+ {
+ float norm;
+
+ norm = (q1.x*q1.x + q1.y*q1.y + q1.z*q1.z + q1.w*q1.w);
+
+ if (norm > 0.0f) {
+ norm = 1.0f/(float)Math.sqrt(norm);
+ this.x = norm*q1.x;
+ this.y = norm*q1.y;
+ this.z = norm*q1.z;
+ this.w = norm*q1.w;
+ } else {
+ this.x = (float) 0.0;
+ this.y = (float) 0.0;
+ this.z = (float) 0.0;
+ this.w = (float) 0.0;
+ }
+ }
+
+
+ /**
+ * Normalizes the value of this quaternion in place.
+ */
+ public final void normalize()
+ {
+ float norm;
+
+ norm = (this.x*this.x + this.y*this.y + this.z*this.z + this.w*this.w);
+
+ if (norm > 0.0f) {
+ norm = 1.0f / (float)Math.sqrt(norm);
+ this.x *= norm;
+ this.y *= norm;
+ this.z *= norm;
+ this.w *= norm;
+ } else {
+ this.x = (float) 0.0;
+ this.y = (float) 0.0;
+ this.z = (float) 0.0;
+ this.w = (float) 0.0;
+ }
+ }
+
+
+ /**
+ * Sets the value of this quaternion to the rotational component of
+ * the passed matrix.
+ * @param m1 the Matrix4f
+ */
+ public final void set(Matrix4f m1)
+ {
+ float ww = 0.25f*(m1.m00 + m1.m11 + m1.m22 + m1.m33);
+
+ if (ww >= 0) {
+ if (ww >= EPS2) {
+ this.w = (float) Math.sqrt((double)ww);
+ ww = 0.25f/this.w;
+ this.x = (m1.m21 - m1.m12)*ww;
+ this.y = (m1.m02 - m1.m20)*ww;
+ this.z = (m1.m10 - m1.m01)*ww;
+ return;
+ }
+ } else {
+ this.w = 0;
+ this.x = 0;
+ this.y = 0;
+ this.z = 1;
+ return;
+ }
+
+ this.w = 0;
+ ww = -0.5f*(m1.m11 + m1.m22);
+
+ if (ww >= 0) {
+ if (ww >= EPS2) {
+ this.x = (float) Math.sqrt((double) ww);
+ ww = 1.0f/(2.0f*this.x);
+ this.y = m1.m10*ww;
+ this.z = m1.m20*ww;
+ return;
+ }
+ } else {
+ this.x = 0;
+ this.y = 0;
+ this.z = 1;
+ return;
+ }
+
+ this.x = 0;
+ ww = 0.5f*(1.0f - m1.m22);
+
+ if (ww >= EPS2) {
+ this.y = (float) Math.sqrt((double) ww);
+ this.z = m1.m21/(2.0f*this.y);
+ return;
+ }
+
+ this.y = 0;
+ this.z = 1;
+ }
+
+
+ /**
+ * Sets the value of this quaternion to the rotational component of
+ * the passed matrix.
+ * @param m1 the Matrix4d
+ */
+ public final void set(Matrix4d m1)
+ {
+ double ww = 0.25*(m1.m00 + m1.m11 + m1.m22 + m1.m33);
+
+ if (ww >= 0) {
+ if (ww >= EPS2) {
+ this.w = (float) Math.sqrt(ww);
+ ww = 0.25/this.w;
+ this.x = (float) ((m1.m21 - m1.m12)*ww);
+ this.y = (float) ((m1.m02 - m1.m20)*ww);
+ this.z = (float) ((m1.m10 - m1.m01)*ww);
+ return;
+ }
+ } else {
+ this.w = 0;
+ this.x = 0;
+ this.y = 0;
+ this.z = 1;
+ return;
+ }
+
+ this.w = 0;
+ ww = -0.5*(m1.m11 + m1.m22);
+ if (ww >= 0) {
+ if (ww >= EPS2) {
+ this.x = (float) Math.sqrt(ww);
+ ww = 0.5/this.x;
+ this.y = (float)(m1.m10*ww);
+ this.z = (float)(m1.m20*ww);
+ return;
+ }
+ } else {
+ this.x = 0;
+ this.y = 0;
+ this.z = 1;
+ return;
+ }
+
+ this.x = 0;
+ ww = 0.5*(1.0 - m1.m22);
+ if (ww >= EPS2) {
+ this.y = (float) Math.sqrt(ww);
+ this.z = (float) (m1.m21/(2.0*(double)(this.y)));
+ return;
+ }
+
+ this.y = 0;
+ this.z = 1;
+ }
+
+
+ /**
+ * Sets the value of this quaternion to the rotational component of
+ * the passed matrix.
+ * @param m1 the Matrix3f
+ */
+ public final void set(Matrix3f m1)
+ {
+ float ww = 0.25f*(m1.m00 + m1.m11 + m1.m22 + 1.0f);
+
+ if (ww >= 0) {
+ if (ww >= EPS2) {
+ this.w = (float) Math.sqrt((double) ww);
+ ww = 0.25f/this.w;
+ this.x = (m1.m21 - m1.m12)*ww;
+ this.y = (m1.m02 - m1.m20)*ww;
+ this.z = (m1.m10 - m1.m01)*ww;
+ return;
+ }
+ } else {
+ this.w = 0;
+ this.x = 0;
+ this.y = 0;
+ this.z = 1;
+ return;
+ }
+
+ this.w = 0;
+ ww = -0.5f*(m1.m11 + m1.m22);
+ if (ww >= 0) {
+ if (ww >= EPS2) {
+ this.x = (float) Math.sqrt((double) ww);
+ ww = 0.5f/this.x;
+ this.y = m1.m10*ww;
+ this.z = m1.m20*ww;
+ return;
+ }
+ } else {
+ this.x = 0;
+ this.y = 0;
+ this.z = 1;
+ return;
+ }
+
+ this.x = 0;
+ ww = 0.5f*(1.0f - m1.m22);
+ if (ww >= EPS2) {
+ this.y = (float) Math.sqrt((double) ww);
+ this.z = m1.m21/(2.0f*this.y);
+ return;
+ }
+
+ this.y = 0;
+ this.z = 1;
+ }
+
+
+ /**
+ * Sets the value of this quaternion to the rotational component of
+ * the passed matrix.
+ * @param m1 the Matrix3d
+ */
+ public final void set(Matrix3d m1)
+ {
+ double ww = 0.25*(m1.m00 + m1.m11 + m1.m22 + 1.0f);
+
+ if (ww >= 0) {
+ if (ww >= EPS2) {
+ this.w = (float) Math.sqrt(ww);
+ ww = 0.25/this.w;
+ this.x = (float) ((m1.m21 - m1.m12)*ww);
+ this.y = (float) ((m1.m02 - m1.m20)*ww);
+ this.z = (float) ((m1.m10 - m1.m01)*ww);
+ return;
+ }
+ } else {
+ this.w = 0;
+ this.x = 0;
+ this.y = 0;
+ this.z = 1;
+ return;
+ }
+
+ this.w = 0;
+ ww = -0.5*(m1.m11 + m1.m22);
+ if (ww >= 0) {
+ if (ww >= EPS2) {
+ this.x = (float) Math.sqrt(ww);
+ ww = 0.5/this.x;
+ this.y = (float) (m1.m10*ww);
+ this.z = (float) (m1.m20*ww);
+ return;
+ }
+ } else {
+ this.x = 0;
+ this.y = 0;
+ this.z = 1;
+ return;
+ }
+
+ this.x = 0;
+ ww = 0.5*(1.0 - m1.m22);
+ if (ww >= EPS2) {
+ this.y = (float) Math.sqrt(ww);
+ this.z = (float) (m1.m21/(2.0*(double)(this.y)));
+ return;
+ }
+
+ this.y = 0;
+ this.z = 1;
+ }
+
+
+ /**
+ * Sets the value of this quaternion to the equivalent rotation
+ * of the AxisAngle argument.
+ * @param a the AxisAngle to be emulated
+ */
+ public final void set(AxisAngle4f a)
+ {
+ float mag,amag;
+ // Quat = cos(theta/2) + sin(theta/2)(roation_axis)
+ amag = (float)Math.sqrt( a.x*a.x + a.y*a.y + a.z*a.z);
+ if (amag < EPS ) {
+ w = 0.0f;
+ x = 0.0f;
+ y = 0.0f;
+ z = 0.0f;
+ } else {
+ amag = 1.0f/amag;
+ mag = (float)Math.sin(a.angle/2.0);
+ w = (float)Math.cos(a.angle/2.0);
+ x = a.x*amag*mag;
+ y = a.y*amag*mag;
+ z = a.z*amag*mag;
+ }
+ }
+
+
+ /**
+ * Sets the value of this quaternion to the equivalent rotation
+ * of the AxisAngle argument.
+ * @param a the AxisAngle to be emulated
+ */
+ public final void set(AxisAngle4d a)
+ {
+ float mag,amag;
+ // Quat = cos(theta/2) + sin(theta/2)(roation_axis)
+
+ amag = (float)(1.0/Math.sqrt( a.x*a.x + a.y*a.y + a.z*a.z));
+
+ if (amag < EPS ) {
+ w = 0.0f;
+ x = 0.0f;
+ y = 0.0f;
+ z = 0.0f;
+ } else {
+ amag = 1.0f/amag;
+ mag = (float)Math.sin(a.angle/2.0);
+ w = (float)Math.cos(a.angle/2.0);
+ x = (float)a.x*amag*mag;
+ y = (float)a.y*amag*mag;
+ z = (float)a.z*amag*mag;
+ }
+
+ }
+
+
+ /**
+ * Performs a great circle interpolation between this quaternion
+ * and the quaternion parameter and places the result into this
+ * quaternion.
+ * @param q1 the other quaternion
+ * @param alpha the alpha interpolation parameter
+ */
+ public final void interpolate(UnQuat4f q1, float alpha) {
+ // From "Advanced Animation and Rendering Techniques"
+ // by Watt and Watt pg. 364, function as implemented appeared to be
+ // incorrect. Fails to choose the same quaternion for the double
+ // covering. Resulting in change of direction for rotations.
+ // Fixed function to negate the first quaternion in the case that the
+ // dot product of q1 and this is negative. Second case was not needed.
+
+ double dot,s1,s2,om,sinom;
+
+ dot = x*q1.x + y*q1.y + z*q1.z + w*q1.w;
+
+ if ( dot < 0 ) {
+ // negate quaternion
+ q1.x = -q1.x; q1.y = -q1.y; q1.z = -q1.z; q1.w = -q1.w;
+ dot = -dot;
+ }
+
+ if ( (1.0 - dot) > EPS ) {
+ om = Math.acos(dot);
+ sinom = Math.sin(om);
+ s1 = Math.sin((1.0-alpha)*om)/sinom;
+ s2 = Math.sin( alpha*om)/sinom;
+ } else{
+ s1 = 1.0 - alpha;
+ s2 = alpha;
+ }
+
+ w = (float)(s1*w + s2*q1.w);
+ x = (float)(s1*x + s2*q1.x);
+ y = (float)(s1*y + s2*q1.y);
+ z = (float)(s1*z + s2*q1.z);
+ }
+
+
+
+ /**
+ * Performs a great circle interpolation between quaternion q1
+ * and quaternion q2 and places the result into this quaternion.
+ * @param q1 the first quaternion
+ * @param q2 the second quaternion
+ * @param alpha the alpha interpolation parameter
+ */
+ public final void interpolate(UnQuat4f q1, UnQuat4f q2, float alpha) {
+ // From "Advanced Animation and Rendering Techniques"
+ // by Watt and Watt pg. 364, function as implemented appeared to be
+ // incorrect. Fails to choose the same quaternion for the double
+ // covering. Resulting in change of direction for rotations.
+ // Fixed function to negate the first quaternion in the case that the
+ // dot product of q1 and this is negative. Second case was not needed.
+
+ double dot,s1,s2,om,sinom;
+
+ dot = q2.x*q1.x + q2.y*q1.y + q2.z*q1.z + q2.w*q1.w;
+
+ if ( dot < 0 ) {
+ // negate quaternion
+ q1.x = -q1.x; q1.y = -q1.y; q1.z = -q1.z; q1.w = -q1.w;
+ dot = -dot;
+ }
+
+ if ( (1.0 - dot) > EPS ) {
+ om = Math.acos(dot);
+ sinom = Math.sin(om);
+ s1 = Math.sin((1.0-alpha)*om)/sinom;
+ s2 = Math.sin( alpha*om)/sinom;
+ } else{
+ s1 = 1.0 - alpha;
+ s2 = alpha;
+ }
+ w = (float)(s1*q1.w + s2*q2.w);
+ x = (float)(s1*q1.x + s2*q2.x);
+ y = (float)(s1*q1.y + s2*q2.y);
+ z = (float)(s1*q1.z + s2*q2.z);
+ }
+
+}
+
+
+
+
|