diff options
Diffstat (limited to 'src/common')
| -rw-r--r-- | src/common/CollidableObject.java | 533 | ||||
| -rw-r--r-- | src/common/CollisionDetector.java | 672 | ||||
| -rw-r--r-- | src/common/CollisionInfo.java | 26 | ||||
| -rw-r--r-- | src/common/Peer.java | 588 | ||||
| -rw-r--r-- | src/common/PeerCoordinates.java | 53 | ||||
| -rw-r--r-- | src/common/PeerInformation.java | 39 | ||||
| -rw-r--r-- | src/common/PeerMessage.java | 28 | ||||
| -rw-r--r-- | src/common/UnQuat4f.java | 658 |
8 files changed, 2597 insertions, 0 deletions
diff --git a/src/common/CollidableObject.java b/src/common/CollidableObject.java new file mode 100644 index 0000000..d95c8b3 --- /dev/null +++ b/src/common/CollidableObject.java @@ -0,0 +1,533 @@ +package common;
+import java.io.IOException;
+import java.io.ObjectInputStream;
+import java.io.ObjectOutputStream;
+import java.io.Serializable;
+import java.util.*;
+
+import javax.media.j3d.*;
+import javax.vecmath.*;
+
+import com.sun.j3d.utils.geometry.GeometryInfo;
+import com.sun.j3d.utils.geometry.Primitive;
+
+@SuppressWarnings("restriction")
+public abstract class CollidableObject implements Serializable {
+ private static final long serialVersionUID = 3667108226485766929L;
+ 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 previousRotationalVelocity;
+ protected Vector3f torqueAccumulator;
+ protected Matrix3f inverseInertiaTensor;
+ protected float coefficientOfRestitution;
+ protected float penetrationCorrection;
+ protected float dynamicFriction;
+ protected float rotationalFriction;
+ transient protected BranchGroup BG;
+ transient protected TransformGroup TG;
+ transient protected Node node;
+ transient private ArrayList<Vector3f> vertexCache;
+ transient private ArrayList<CollisionDetector.Triangle> triangleCache;
+ transient private Bounds boundsCache;
+ // The inverse inertia tensor in world coordinates
+ transient 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();
+ previousRotationalVelocity = new Vector3f();
+ torqueAccumulator = new Vector3f();
+ inverseInertiaTensor = new Matrix3f();
+ coefficientOfRestitution = 0.75f;
+ penetrationCorrection = 1.05f;
+ dynamicFriction = 0.02f;
+ rotationalFriction = 0.05f;
+ 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(node)));
+ }
+
+ 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() {
+ Vector3f com = new Vector3f(-centerOfMass.x, -centerOfMass.y, -centerOfMass.z);
+ Transform3D tmp = new Transform3D();
+ tmp.setTranslation(com);
+ Transform3D tmp2 = new Transform3D();
+ tmp2.setRotation(orientation);
+ com.negate();
+ com.add(position);
+ tmp2.setTranslation(com);
+ tmp2.mul(tmp);
+ TG.setTransform(tmp2);
+ clearCaches();
+ }
+
+ public 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);
+ Matrix3f tmp = new Matrix3f(inverseInertiaTensor);
+ Matrix3f tmp2 = new Matrix3f(inverseInertiaTensorCache);
+ tmp2.invert();
+ tmp.mul(tmp2);
+ inverseInertiaTensorCache.mul(tmp);
+ }
+ 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) {
+ 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 otherRelativeContactPosition = new Vector3f();
+ otherRelativeContactPosition.scaleAdd(-1, other.position, ci.contactPoint);
+ otherRelativeContactPosition.scaleAdd(-1, other.centerOfMass, otherRelativeContactPosition);
+
+ Vector3f thisContactVelocity = new Vector3f();
+ thisContactVelocity.cross(angularVelocity, thisRelativeContactPosition);
+ thisContactVelocity.add(previousVelocity);
+
+ 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;
+
+ /* Dynamic Friction */
+ if (dynamicFriction > 0) {
+ Vector3f acceleration = new Vector3f();
+ Vector3f perpVelocity = new Vector3f();
+ float contactSpeed = ci.contactNormal.dot(velocity) - ci.contactNormal.dot(other.velocity);
+
+ perpVelocity.scaleAdd(-contactSpeed, ci.contactNormal, previousVelocity);
+ if (perpVelocity.length() > 0) {
+ perpVelocity.normalize();
+ acceleration.scaleAdd(-1, previousVelocity, velocity);
+ velocity.scaleAdd(dynamicFriction * acceleration.dot(ci.contactNormal), perpVelocity, velocity);
+ }
+
+ perpVelocity.scaleAdd(contactSpeed, ci.contactNormal, other.previousVelocity);
+ if (perpVelocity.length() > 0) {
+ perpVelocity.normalize();
+ acceleration.scaleAdd(-1, other.previousVelocity, other.velocity);
+ other.velocity.scaleAdd(dynamicFriction * acceleration.dot(ci.contactNormal), perpVelocity, other.velocity);
+ }
+ }
+
+ Vector3f thisMovementUnit = new Vector3f();
+ thisMovementUnit.cross(thisRelativeContactPosition, ci.contactNormal);
+ getInverseInertiaTensor().transform(thisMovementUnit);
+ Vector3f thisAngularVelocityUnit = new Vector3f();
+ thisAngularVelocityUnit.cross(thisMovementUnit, thisRelativeContactPosition);
+ totalInverseMass += thisAngularVelocityUnit.dot(ci.contactNormal);
+
+ Vector3f otherMovementUnit = new Vector3f();
+ otherMovementUnit.cross(otherRelativeContactPosition, ci.contactNormal);
+ other.getInverseInertiaTensor().transform(otherMovementUnit);
+ Vector3f otherAngularVelocityUnit = new Vector3f();
+ otherAngularVelocityUnit.cross(otherMovementUnit, 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);
+ getInverseInertiaTensor().transform(tmp);
+ angularVelocity.add(tmp);
+ position.scaleAdd(-ci.penetration * penetrationCorrection * inverseMass / totalInverseMass, ci.contactNormal, position);
+ thisMovementUnit.scale(-ci.penetration * penetrationCorrection / totalInverseMass);
+ UnQuat4f tmp2 = new UnQuat4f(thisMovementUnit.x, thisMovementUnit.y, thisMovementUnit.z, 0);
+ tmp2.scale(0.5f);
+ tmp2.mul(orientation);
+ orientation.add(tmp2);
+ orientation.normalize();
+
+ impulse.negate();
+ other.velocity.scaleAdd(other.inverseMass, impulse, other.velocity);
+ tmp.cross(otherRelativeContactPosition, impulse);
+ other.getInverseInertiaTensor().transform(tmp);
+ other.angularVelocity.add(tmp);
+ other.position.scaleAdd(ci.penetration * penetrationCorrection * other.inverseMass / totalInverseMass, ci.contactNormal, other.position);
+ otherMovementUnit.scale(ci.penetration * penetrationCorrection / totalInverseMass);
+ tmp2.set(otherMovementUnit.x, otherMovementUnit.y, otherMovementUnit.z, 0);
+ tmp2.scale(0.5f);
+ tmp2.mul(other.orientation);
+ other.orientation.add(tmp2);
+ other.orientation.normalize();
+
+ if (rotationalFriction > 0) {
+ /* Rotational Friction */
+ Vector3f w = new Vector3f();
+
+ /*float radius = thisRelativeContactPosition.length();
+ w.cross(angularVelocity, ci.contactNormal);
+ velocity.scaleAdd(-1, previousRotationalVelocity, velocity);
+ previousRotationalVelocity.scale(radius, w);
+ velocity.scaleAdd(radius, w, velocity);
+ w.cross(previousRotationalVelocity, ci.contactNormal);
+ angularVelocity.scaleAdd(-0.5f * w.dot(angularVelocity), w, angularVelocity);
+
+ radius = otherRelativeContactPosition.length();
+ w.cross(other.angularVelocity, ci.contactNormal);
+ other.velocity.scaleAdd(-1, other.previousRotationalVelocity, other.velocity);
+ other.previousRotationalVelocity.scale(radius, w);
+ other.velocity.scaleAdd(radius , w, other.velocity);
+ w.cross(other.previousRotationalVelocity, ci.contactNormal);
+ other.angularVelocity.scaleAdd(-0.5f * w.dot(other.angularVelocity), w, other.angularVelocity);
+ */
+
+ angularVelocity.scaleAdd(-rotationalFriction * ci.contactNormal.dot(angularVelocity), ci.contactNormal, angularVelocity);
+ other.angularVelocity.scaleAdd(-rotationalFriction * ci.contactNormal.dot(other.angularVelocity), ci.contactNormal, other.angularVelocity);
+
+ }
+ }
+
+private static final int NODE_TYPE_BRANCH = 1;
+ private static final int NODE_TYPE_TRANSFORM = 2;
+ private static final int NODE_TYPE_PRIMITIVE = 3;
+ private static final int NODE_TYPE_SHAPE = 4;
+
+ private void writeObject(ObjectOutputStream out) throws IOException {
+ out.defaultWriteObject();
+ writeObject(out, node);
+ }
+
+ private static void writeObject(ObjectOutputStream out, Node node) throws IOException {
+ if (node instanceof BranchGroup) {
+ out.writeInt(NODE_TYPE_BRANCH);
+ out.writeInt(((BranchGroup) node).numChildren());
+ for (int i = 0; i < ((BranchGroup) node).numChildren(); i++) {
+ Node childNode = ((BranchGroup) node).getChild(i);
+ writeObject(out, childNode);
+ }
+ } else if (node instanceof TransformGroup) {
+ out.writeInt(NODE_TYPE_TRANSFORM);
+ Transform3D tgT = new Transform3D();
+ Matrix4f matrix = new Matrix4f();
+ ((TransformGroup) node).getTransform(tgT);
+ tgT.get(matrix);
+ out.writeObject(matrix);
+ out.writeInt(((TransformGroup) node).numChildren());
+ for (int i = 0; i < ((TransformGroup) node).numChildren(); i++) {
+ Node childNode = ((TransformGroup) node).getChild(i);
+ writeObject(out, childNode);
+ }
+ } else if (node instanceof Primitive) {
+ out.writeInt(NODE_TYPE_PRIMITIVE);
+ Primitive prim = (Primitive)node;
+ int index = 0;
+ Shape3D shape;
+ out.writeInt(prim.numChildren());
+ while ((shape = prim.getShape(index++)) != null) {
+ Appearance appearance = shape.getAppearance();
+ if (appearance != null) {
+ out.writeBoolean(true);
+ writeObject(out, appearance);
+ } else
+ out.writeBoolean(false);
+ out.writeInt(shape.numGeometries());
+ for (int i = 0; i < shape.numGeometries(); i++)
+ writeObject(out, shape.getGeometry(i));
+ }
+
+ } else if (node instanceof Shape3D) {
+ out.writeInt(NODE_TYPE_SHAPE);
+ Shape3D shape = (Shape3D) node;
+ Appearance appearance = shape.getAppearance();
+ if (appearance != null) {
+ out.writeBoolean(true);
+ writeObject(out, appearance);
+ } else
+ out.writeBoolean(false);
+ out.writeInt(shape.numGeometries());
+ for (int i = 0; i < shape.numGeometries(); i++)
+ writeObject(out, shape.getGeometry(i));
+
+ } else
+ throw new IllegalArgumentException("Illegal node type for serialization");
+ }
+
+ private static void writeObject(ObjectOutputStream out, Geometry geometry) throws IOException {
+ GeometryInfo gi = new GeometryInfo((GeometryArray)geometry);
+ gi.convertToIndexedTriangles();
+ geometry = gi.getIndexedGeometryArray();
+ IndexedTriangleArray trueGeometry = (IndexedTriangleArray)geometry;
+
+ int format = trueGeometry.getVertexFormat() & (IndexedTriangleArray.COORDINATES | IndexedTriangleArray.NORMALS);
+ out.writeInt(format);
+ Point3f vertices[] = new Point3f[trueGeometry.getValidVertexCount()];
+ for (int i = 0; i < vertices.length; i++) {
+ vertices[i] = new Point3f();
+ trueGeometry.getCoordinate(i, vertices[i]);
+ }
+ out.writeObject(vertices);
+ int indices[] = new int[trueGeometry.getValidIndexCount()];
+ trueGeometry.getCoordinateIndices(0, indices);
+ out.writeObject(indices);
+
+ if ((format & IndexedTriangleArray.NORMALS) != 0) {
+ Vector3f normals[] = new Vector3f[trueGeometry.getValidVertexCount()];
+ for (int i = 0; i < normals.length; i++) {
+ normals[i] = new Vector3f();
+ trueGeometry.getNormal(i, normals[i]);
+ }
+ out.writeObject(normals);
+ trueGeometry.getNormalIndices(0, indices);
+ out.writeObject(indices);
+ }
+ }
+
+ private static void writeObject(ObjectOutputStream out, Appearance appearance) throws IOException {
+ Material material = appearance.getMaterial();
+ if (material != null) {
+ out.writeBoolean(true);
+ Color3f color = new Color3f();
+ material.getAmbientColor(color);
+ out.writeObject(color);
+ color = new Color3f();
+ material.getDiffuseColor(color);
+ out.writeObject(color);
+ color = new Color3f();
+ material.getSpecularColor(color);
+ out.writeObject(color);
+ color = new Color3f();
+ material.getEmissiveColor(color);
+ out.writeObject(color);
+ out.writeFloat(material.getShininess());
+ out.writeInt(material.getColorTarget());
+ } else
+ out.writeBoolean(false);
+ }
+
+ private void readObject(ObjectInputStream in) throws IOException, ClassNotFoundException {
+ in.defaultReadObject();
+ TG = new TransformGroup();
+ TG.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE);
+ BG = new BranchGroup();
+ BG.setCapability(BranchGroup.ALLOW_DETACH);
+ BG.addChild(TG);
+ setShape(readNode(in));
+ }
+
+ private static Node readNode(ObjectInputStream in) throws IOException, ClassNotFoundException {
+ int type = in.readInt();
+ switch (type) {
+ case NODE_TYPE_BRANCH:
+ BranchGroup bgroup = new BranchGroup();
+ int numTGChildren = in.readInt();
+ for (int i = 0; i < numTGChildren; i++) {
+ bgroup.addChild(readNode(in));
+ }
+ return bgroup;
+ case NODE_TYPE_TRANSFORM:
+ TransformGroup tgroup = new TransformGroup();
+ Matrix4f matrix = (Matrix4f) in.readObject();
+ Transform3D tgT = new Transform3D(matrix);
+ tgroup.setTransform(tgT);
+ int numChildren = in.readInt();
+ for (int i = 0; i < numChildren; i++) {
+ tgroup.addChild(readNode(in));
+ }
+ return tgroup;
+ case NODE_TYPE_PRIMITIVE:
+ BranchGroup bg = new BranchGroup();
+ int shapes = in.readInt();
+ for (int i = 0; i < shapes; i++) {
+ Shape3D shape = new Shape3D();
+ shape.removeAllGeometries();
+ if (in.readBoolean())
+ shape.setAppearance(readAppearance(in));
+ int geometries = in.readInt();
+ for (int j = 0; j < geometries; j++)
+ shape.addGeometry(readGeometry(in));
+ bg.addChild(shape);
+ }
+ return bg;
+ case NODE_TYPE_SHAPE:
+ BranchGroup shapeBG = new BranchGroup();
+ Shape3D shape = new Shape3D();
+ shape.removeAllGeometries();
+ boolean hasAppearance = in.readBoolean();
+ Appearance shapeApp = new Appearance();
+ if(hasAppearance) {
+ shapeApp = readAppearance(in);
+ }
+ int geometries = in.readInt();
+ for (int i = 0; i < geometries; i++)
+ shape.addGeometry(readGeometry(in));
+ shapeBG.addChild(shape);
+ return shapeBG;
+ default:
+ throw new IllegalArgumentException("Illegal node type for serialization");
+ }
+ }
+
+ private static GeometryArray readGeometry(ObjectInputStream in) throws IOException, ClassNotFoundException {
+ int format = in.readInt();
+ Point3f vertices[] = (Point3f[])in.readObject();
+ int indices[] = (int[])in.readObject();
+ IndexedTriangleArray geometry = new IndexedTriangleArray(vertices.length, format, indices.length);
+ geometry.setCoordinates(0, vertices);
+ geometry.setCoordinateIndices(0, indices);
+
+ if ((format & IndexedTriangleArray.NORMALS) != 0) {
+ Vector3f normals[] = (Vector3f[])in.readObject();
+ indices = (int[])in.readObject();
+ geometry.setNormals(0, normals);
+ geometry.setNormalIndices(0, indices);
+ }
+
+ return geometry;
+ }
+
+ private static Appearance readAppearance(ObjectInputStream in) throws IOException, ClassNotFoundException {
+ Appearance appearance = new Appearance();
+ if (in.readBoolean()) {
+ Material material = new Material();
+ material.setAmbientColor((Color3f)in.readObject());
+ material.setDiffuseColor((Color3f)in.readObject());
+ material.setSpecularColor((Color3f)in.readObject());
+ material.setEmissiveColor((Color3f)in.readObject());
+ material.setShininess(in.readFloat());
+ material.setColorTarget(in.readInt());
+ appearance.setMaterial(material);
+ }
+ return appearance;
+ }
+}
diff --git a/src/common/CollisionDetector.java b/src/common/CollisionDetector.java new file mode 100644 index 0000000..c4d03e8 --- /dev/null +++ b/src/common/CollisionDetector.java @@ -0,0 +1,672 @@ +package common;
+
+import java.util.ArrayList;
+
+import javax.media.j3d.Appearance;
+import javax.media.j3d.Geometry;
+import javax.media.j3d.GeometryArray;
+import javax.media.j3d.Group;
+import javax.media.j3d.IndexedTriangleArray;
+import javax.media.j3d.IndexedTriangleFanArray;
+import javax.media.j3d.IndexedTriangleStripArray;
+import javax.media.j3d.Node;
+import javax.media.j3d.PolygonAttributes;
+import javax.media.j3d.Shape3D;
+import javax.media.j3d.Transform3D;
+import javax.media.j3d.TriangleArray;
+import javax.media.j3d.TriangleFanArray;
+import javax.media.j3d.TriangleStripArray;
+import javax.vecmath.Matrix3f;
+import javax.vecmath.Point3f;
+import javax.vecmath.Tuple3f;
+import javax.vecmath.Vector3f;
+
+import tesseract.objects.HalfSpace;
+import tesseract.objects.Particle;
+import tesseract.objects.Polygon;
+import tesseract.objects.Sphere;
+
+import com.sun.j3d.utils.geometry.Primitive;
+
+@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 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 Particle)
+ return calculateCollisions((HalfSpace)a, (Particle)b);
+ if (b instanceof Sphere)
+ return calculateCollisions((HalfSpace)a, (Sphere)b);
+ return calculateCollisions((HalfSpace)a, b.getVertices());
+ }
+ if (b instanceof HalfSpace) {
+ if (a instanceof Particle)
+ return flipContactNormals(calculateCollisions((HalfSpace)b, (Particle)a));
+ if (a instanceof Sphere)
+ return flipContactNormals(calculateCollisions((HalfSpace)b, (Sphere)a));
+ return flipContactNormals(calculateCollisions((HalfSpace)b, a.getVertices()));
+ }
+ if (a instanceof Particle) {
+ if (b instanceof Particle)
+ return EMPTY_COLLISION_LIST;
+ if (b instanceof Sphere)
+ return calculateCollisions((Particle)a, (Sphere)b);
+ if (b instanceof Polygon)
+ return calculateCollisions((Particle)a, (Polygon)b);
+ }
+ if (b instanceof Particle) {
+ if (a instanceof Sphere)
+ return flipContactNormals(calculateCollisions((Particle)b, (Sphere)a));
+ if (a instanceof Polygon)
+ return flipContactNormals(calculateCollisions((Particle)b, (Polygon)a));
+ }
+ if (a instanceof Sphere && b instanceof Sphere)
+ return calculateCollisions((Sphere)a, (Sphere)b);
+
+ if (!a.getBounds().intersect(b.getBounds()))
+ return EMPTY_COLLISION_LIST;
+
+ if (a instanceof Particle)
+ return calculateCollisions((Particle)a, b);
+ if (b instanceof Particle)
+ return flipContactNormals(calculateCollisions((Particle)b, a));
+ if (a instanceof Polygon)
+ return calculateCollisions((Polygon)a, b);
+ if (b instanceof Polygon)
+ return calculateCollisions((Polygon)b, a);
+ return CollisionDetector.calculateCollisions(a.getCollisionTriangles(), b.getCollisionTriangles());
+ }
+
+ private static ArrayList<CollisionInfo> calculateCollisions(HalfSpace a, Particle b) {
+ float penetration = a.intercept - a.normal.dot(b.position);
+ if (penetration < 0)
+ return EMPTY_COLLISION_LIST;
+ Vector3f contactPoint = new Vector3f();
+ contactPoint.scaleAdd(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, new Vector3f(a.normal), penetration));
+ return collisions;
+ }
+
+ private static ArrayList<CollisionInfo> calculateCollisions(HalfSpace a, Sphere b) {
+ float penetration = b.radius - (a.normal.dot(b.position) - a.intercept);
+ if (penetration < 0)
+ return EMPTY_COLLISION_LIST;
+ 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, new Vector3f(a.normal), penetration));
+ return collisions;
+ }
+
+ 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, new Vector3f(a.normal), penetration));
+ }
+ }
+ return collisions;
+ }
+
+ private static ArrayList<CollisionInfo> calculateCollisions(Particle a, Sphere b) {
+ Vector3f delta = new Vector3f();
+ delta.scaleAdd(-1, a.position, b.position);
+ float penetration = b.radius - delta.length();
+ if (penetration < 0)
+ return EMPTY_COLLISION_LIST;
+
+ ArrayList<CollisionInfo> collisions = new ArrayList<CollisionInfo>();
+ delta.normalize();
+ Vector3f contactPoint = new Vector3f();
+ contactPoint.scaleAdd(-(b.radius - 0.5f * penetration), delta, b.position);
+ collisions.add(new CollisionInfo(contactPoint, delta, penetration));
+ return collisions;
+ }
+
+ private static ArrayList<CollisionInfo> calculateCollisions(Particle a, Polygon b) {
+ float penetration = b.intercept - b.normal.dot(a.position);
+ float previousPenetration = b.intercept - b.normal.dot(a.previousPosition);
+ if (Math.signum(penetration) == Math.signum(previousPenetration))
+ return EMPTY_COLLISION_LIST;
+
+ for (Triangle triangle : b.getCollisionTriangles()) {
+ Matrix3f tmp = new Matrix3f(a.previousPosition.x - a.position.x, triangle.b.x - triangle.a.x, triangle.c.x - triangle.a.x,
+ a.previousPosition.y - a.position.y, triangle.b.y - triangle.a.y, triangle.c.y - triangle.a.y,
+ a.previousPosition.z - a.position.z, triangle.b.z - triangle.a.z, triangle.c.z - triangle.a.z);
+ tmp.invert();
+ Vector3f intercept = new Vector3f();
+ intercept.scaleAdd(-1, triangle.a, a.previousPosition);
+ tmp.transform(intercept);
+
+ assert(intercept.x >= 0 && intercept.x <= 1);
+
+ if (intercept.y >= 0 && intercept.y <= 1 && intercept.z >= 0 && intercept.z <= 1 && (intercept.y + intercept.z) <= 1) {
+ Vector3f contactPoint = new Vector3f();
+ contactPoint.scaleAdd(-1, a.previousPosition, a.position);
+ contactPoint.scale(intercept.x);
+ contactPoint.add(a.previousPosition);
+ assert(Math.abs(b.normal.dot(contactPoint) - b.intercept) < 0.01);
+ Vector3f contactNormal = new Vector3f(b.normal);
+ if (penetration - previousPenetration > 0)
+ contactNormal.negate();
+ else
+ penetration = -penetration;
+ ArrayList<CollisionInfo> collisions = new ArrayList<CollisionInfo>();
+ collisions.add(new CollisionInfo(contactPoint, contactNormal, penetration));
+ return collisions;
+ }
+ }
+ return EMPTY_COLLISION_LIST;
+ }
+
+ private static ArrayList<CollisionInfo> calculateCollisions(Particle a, CollidableObject b) {
+ ArrayList<CollisionInfo> collisions = new ArrayList<CollisionInfo>();
+ for (Triangle triangle : b.getCollisionTriangles()) {
+ float penetration = triangle.intercept - triangle.normal.dot(a.position);
+ if (penetration < 0 || (!collisions.isEmpty() && penetration <= collisions.get(0).penetration))
+ continue;
+ float previousPenetration = triangle.intercept - triangle.normal.dot(a.previousPosition);
+ if (Math.signum(penetration) == Math.signum(previousPenetration))
+ continue;
+
+ Matrix3f tmp = new Matrix3f(a.previousPosition.x - a.position.x, triangle.b.x - triangle.a.x, triangle.c.x - triangle.a.x,
+ a.previousPosition.y - a.position.y, triangle.b.y - triangle.a.y, triangle.c.y - triangle.a.y,
+ a.previousPosition.z - a.position.z, triangle.b.z - triangle.a.z, triangle.c.z - triangle.a.z);
+ tmp.invert();
+ Vector3f intercept = new Vector3f();
+ intercept.scaleAdd(-1, triangle.a, a.previousPosition);
+ tmp.transform(intercept);
+
+ assert(intercept.x >= 0 && intercept.x <= 1);
+
+ if (intercept.y >= 0 && intercept.y <= 1 && intercept.z >= 0 && intercept.z <= 1 && (intercept.y + intercept.z) <= 1) {
+ Vector3f contactPoint = new Vector3f();
+ contactPoint.scaleAdd(-1, a.previousPosition, a.position);
+ contactPoint.scale(intercept.x);
+ contactPoint.add(a.previousPosition);
+ assert(Math.abs(triangle.normal.dot(contactPoint) - triangle.intercept) < 0.01);
+ Vector3f contactNormal = new Vector3f(triangle.normal);
+ if (penetration - previousPenetration > 0)
+ contactNormal.negate();
+ else
+ penetration = -penetration;
+ collisions.clear();
+ collisions.add(new CollisionInfo(contactPoint, contactNormal, 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 = a.radius + b.radius - delta.length();
+ 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(Polygon a, CollidableObject b) {
+ ArrayList<CollisionInfo> collisions = calculateCollisions(a.getCollisionTriangles(), b.getCollisionTriangles());
+ int size = collisions.size();
+ collisions.ensureCapacity(2 * size);
+ for (int i = 0; i < size; i++) {
+ collisions.add(collisions.get(i).clone());
+ collisions.get(collisions.size() - 1).contactNormal.negate();
+ }
+ 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;
+ }
+
+ private static ArrayList<CollisionInfo> flipContactNormals(ArrayList<CollisionInfo> collisions) {
+ for (CollisionInfo collision : collisions)
+ collision.contactNormal.negate();
+ 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/common/CollisionInfo.java b/src/common/CollisionInfo.java new file mode 100644 index 0000000..6b81967 --- /dev/null +++ b/src/common/CollisionInfo.java @@ -0,0 +1,26 @@ +package common;
+import javax.vecmath.*;
+
+@SuppressWarnings("restriction")
+public class CollisionInfo implements Cloneable {
+ 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;
+ }
+
+ public CollisionInfo clone() {
+ try {
+ CollisionInfo copy = (CollisionInfo)super.clone();
+ copy.contactPoint = new Vector3f(contactPoint);
+ copy.contactNormal = new Vector3f(contactNormal);
+ return copy;
+ } catch (CloneNotSupportedException e) {
+ return null;
+ }
+ }
+}
\ No newline at end of file diff --git a/src/common/Peer.java b/src/common/Peer.java new file mode 100644 index 0000000..732043b --- /dev/null +++ b/src/common/Peer.java @@ -0,0 +1,588 @@ +package common;
+import java.io.IOException;
+import java.io.ObjectInputStream;
+import java.io.ObjectOutputStream;
+import java.net.InetAddress;
+import java.net.InetSocketAddress;
+import java.net.ServerSocket;
+import java.net.Socket;
+import java.net.SocketException;
+import java.net.UnknownHostException;
+import java.util.ArrayList;
+import java.util.Date;
+import java.util.Observable;
+
+import javax.swing.SwingWorker;
+import javax.vecmath.Vector2f;
+
+
+@SuppressWarnings("restriction")
+public class Peer extends Observable {
+ /**
+ * The default port number for incoming network connections.
+ */
+ public static final int DEFAULT_SERVER_PORT = 5507;
+
+ private PeerInformation myInfo;
+ private ArrayList<PeerInformation> peers;
+ private ServerSocket serverSocket;
+ private SwingWorker<Object,Object> worker;
+
+ /**
+ * A flag indicating whether internal operation messages (useful for
+ * debugging) are logged to the console. This field may be modified at any
+ * time.
+ */
+ public boolean logEnabled;
+
+ /**
+ * Initializes a new <code>Peer</code> object. The <code>Peer</code> is
+ * not connected to a network and sends a log of internal operation
+ * messages to the console.
+ */
+ public Peer() {
+ this(true);
+ }
+
+ /**
+ * Initializes a new <code>Peer</code> object with message logging
+ * controlled by the <code>logEnabled</code> parameter. The
+ * <code>Peer</code> is not connected to a network.
+ *
+ * @param logEnabled Initial value for the <code>logEnabled</code> field.
+ */
+ public Peer(boolean logEnabled) {
+ myInfo = new PeerInformation();
+ this.logEnabled = logEnabled;
+ }
+
+ /**
+ * Establishes a new peer-to-peer network with this <code>Peer</code>
+ * as the sole member. The logical coordinates of this
+ * <code>Peer</code> are chosen randomly.
+ *
+ * @return <code>true</code> if the new network was established
+ * successfully, or <code>false</code> otherwise.
+ */
+ public boolean createNetwork() {
+ return createNetwork(new PeerCoordinates());
+ }
+
+ /**
+ * Establishes a new peer-to-peer network with this <code>Peer</code>
+ * as the sole member. The logical coordinates of this
+ * <code>Peer</code> are specified by parameter <code>location</code>.
+ *
+ * @param location Logical coordinates for this <code>Peer</code>
+ * within the new network.
+ *
+ * @return <code>true</code> if the new network was established
+ * successfully, or <code>false</code> otherwise.
+ */
+ public boolean createNetwork(PeerCoordinates location) {
+ if (serverSocket == null)
+ if (!startServer())
+ return false;
+ peers = new ArrayList<PeerInformation>();
+ myInfo.location = location;
+ logMessage("Established network @ " + myInfo.location);
+ return true;
+ }
+
+ /**
+ * Connects this <code>Peer</code> to an existing peer-to-peer network. The
+ * port number of the known <code>Peer</code> must be
+ * {@link #DEFAULT_SERVER_PORT DEFAULT_SERVER_PORT}. The logical coordinates
+ * of this <code>Peer</code> are chosen randomly.
+ *
+ * @param host
+ * The domain name or IP address of a <code>Peer</code> within
+ * the network.
+ *
+ * @return <code>true</code> if this <code>Peer</code> successfully
+ * connected to the network, or <code>false</code> otherwise.
+ */
+ public boolean connectToNetwork(String host) {
+ return connectToNetwork(host, DEFAULT_SERVER_PORT);
+ }
+
+ /**
+ * Connects this <code>Peer</code> to an existing peer-to-peer network. The
+ * port number of the known <code>Peer</code> must be
+ * {@link #DEFAULT_SERVER_PORT DEFAULT_SERVER_PORT}. The preferred logical
+ * coordinates of this <code>Peer</code> are specified by parameter
+ * <code>location</code>, but the actual logical coordinates may be chosen
+ * randomly to avoid collision with other <code>Peer</code>s.
+ *
+ * @param host
+ * The domain name or IP address of a <code>Peer</code> within
+ * the network.
+ * @param location
+ * Preferred logical coordinates for this <code>Peer</code>.
+ *
+ * @return <code>true</code> if this <code>Peer</code> successfully
+ * connected to the network, or <code>false</code> otherwise.
+ */
+ public boolean connectToNetwork(String host, PeerCoordinates location) {
+ return connectToNetwork(host, DEFAULT_SERVER_PORT, location);
+ }
+
+ /**
+ * Connects this <code>Peer</code> to an existing peer-to-peer network. The
+ * logical coordinates of this <code>Peer</code> are chosen randomly.
+ *
+ * @param host
+ * The domain name or IP address of a <code>Peer</code> within
+ * the network.
+ * @param port
+ * The port number of a <code>Peer</code> within the network.
+ *
+ * @return <code>true</code> if this <code>Peer</code> successfully
+ * connected to the network, or <code>false</code> otherwise.
+ */
+ public boolean connectToNetwork(String host, int port) {
+ try {
+ return connectToNetwork(InetAddress.getByName(host), port);
+ } catch (UnknownHostException e) {
+ System.err.println(e);
+ return false;
+ }
+ }
+
+ /**
+ * Connects this <code>Peer</code> to an existing peer-to-peer network. The
+ * preferred logical coordinates of this <code>Peer</code> are specified by
+ * parameter <code>location</code>, but the actual logical coordinates may
+ * be chosen randomly to avoid collision with other <code>Peer</code>s.
+ *
+ * @param host
+ * The domain name or IP address of a <code>Peer</code> within
+ * the network.
+ * @param port
+ * The port number of a <code>Peer</code> within the network.
+ * @param location
+ * Preferred logical coordinates for this <code>Peer</code>.
+ *
+ * @return <code>true</code> if this <code>Peer</code> successfully
+ * connected to the network, or <code>false</code> otherwise.
+ */
+ public boolean connectToNetwork(String host, int port, PeerCoordinates location) {
+ try {
+ return connectToNetwork(InetAddress.getByName(host), port, location);
+ } catch (UnknownHostException e) {
+ System.err.println(e);
+ return false;
+ }
+ }
+
+ /**
+ * Connects this <code>Peer</code> to an existing peer-to-peer network. The
+ * port number of the known <code>Peer</code> must be
+ * {@link #DEFAULT_SERVER_PORT DEFAULT_SERVER_PORT}. The logical coordinates
+ * of this <code>Peer</code> are chosen randomly.
+ *
+ * @param host
+ * The IP address of a <code>Peer</code> within the network.
+ *
+ * @return <code>true</code> if this <code>Peer</code> successfully
+ * connected to the network, or <code>false</code> otherwise.
+ */
+ public boolean connectToNetwork(InetAddress host) {
+ return connectToNetwork(host, DEFAULT_SERVER_PORT);
+ }
+
+ /**
+ * Connects this <code>Peer</code> to an existing peer-to-peer network. The
+ * port number of the known <code>Peer</code> must be
+ * {@link #DEFAULT_SERVER_PORT DEFAULT_SERVER_PORT}. The preferred logical
+ * coordinates of this <code>Peer</code> are specified by parameter
+ * <code>location</code>, but the actual logical coordinates may be chosen
+ * randomly to avoid collision with other <code>Peer</code>s.
+ *
+ * @param host
+ * The IP address of a <code>Peer</code> within the network.
+ * @param location
+ * Preferred logical coordinates for this <code>Peer</code>.
+ *
+ * @return <code>true</code> if this <code>Peer</code> successfully
+ * connected to the network, or <code>false</code> otherwise.
+ */
+ public boolean connectToNetwork(InetAddress host, PeerCoordinates location) {
+ return connectToNetwork(host, DEFAULT_SERVER_PORT, location);
+ }
+
+ /**
+ * Connects this <code>Peer</code> to an existing peer-to-peer network. The
+ * logical coordinates of this <code>Peer</code> are chosen randomly.
+ *
+ * @param host
+ * The IP address of a <code>Peer</code> within the network.
+ * @param port
+ * The port number of a <code>Peer</code> within the network.
+ *
+ * @return <code>true</code> if this <code>Peer</code> successfully
+ * connected to the network, or <code>false</code> otherwise.
+ */
+ public boolean connectToNetwork(InetAddress host, int port) {
+ return connectToNetwork(host, port, new PeerCoordinates());
+ }
+
+ /**
+ * Connects this <code>Peer</code> to an existing peer-to-peer network. The
+ * preferred logical coordinates of this <code>Peer</code> are specified by
+ * parameter <code>location</code>, but the actual logical coordinates may
+ * be chosen randomly to avoid collision with other <code>Peer</code>s.
+ *
+ * @param host
+ * The IP address of a <code>Peer</code> within the network.
+ * @param port
+ * The port number of a <code>Peer</code> within the network.
+ * @param location
+ * Preferred logical coordinates for this <code>Peer</code>.
+ *
+ * @return <code>true</code> if this <code>Peer</code> successfully connects
+ * to the network, or <code>false</code> otherwise.
+ */
+ public boolean connectToNetwork(InetAddress host, int port, PeerCoordinates location) {
+ if (serverSocket == null)
+ if (!startServer())
+ return false;
+ try {
+ Socket socket = new Socket();
+ try {
+ socket.connect(new InetSocketAddress(host, port), 10000);
+ } catch (IOException e) {
+ System.out.println("Unable to connect to " + host + ":" + port);
+ return false;
+ }
+ ObjectOutputStream socketOut = new ObjectOutputStream(socket.getOutputStream());
+ socketOut.writeObject(createJoinMessage(location));
+ socket.close();
+ while (myInfo.location == null)
+ Thread.sleep(1000);
+ logMessage("Joined network @ " + myInfo.location);
+ return true;
+ } catch (Exception e) {
+ System.err.println(e);
+ return false;
+ }
+ }
+
+ /**
+ * Disconnects this <code>Peer</code> from the peer-to-peer network.
+ */
+ public synchronized void disconnectFromNetwork() {
+ PeerMessage mesg = createAddPeersMessage();
+ mesg.peers = peers;
+ for (PeerInformation peer : peers)
+ sendMessage(mesg, peer);
+ mesg = createRemovePeersMessage();
+ for (PeerInformation peer : peers)
+ sendMessage(mesg, peer);
+ worker.cancel(true);
+ try {
+ serverSocket.close();
+ } catch (IOException e) {
+ }
+ serverSocket = null;
+ System.out.println(myInfo + " disconnected");
+ }
+
+ /**
+ * Identifies the <code>Peer</code> in the network adjacent to this
+ * <code>Peer</code> in the direction (<code>x</code>,<code>y</code>).
+ *
+ * @param x
+ * X component of a direction relative to the logical coordinates
+ * of this <code>Peer</code>.
+ * @param y
+ * Y component of a direction relative to the logical coordinates
+ * of this <code>Peer</code>.
+ *
+ * @return A <code>PeerInformation</code> object representing the adjacent
+ * <code>Peer</code>, or <code>null</code> if no adjacent
+ * <code>Peer</code> exists.
+ */
+ public synchronized PeerInformation getPeerInDirection(float x, float y) {
+ float minDistance = Float.POSITIVE_INFINITY;
+ PeerInformation minPeer = null;
+
+ Vector2f startPoint = new Vector2f(myInfo.location.getX(), myInfo.location.getY());
+ Vector2f direction = new Vector2f(x, y);
+ for (PeerInformation peer : peers) {
+ Vector2f normal = new Vector2f(peer.location.getX() - startPoint.x, peer.location.getY() - startPoint.y);
+ Vector2f midpoint = new Vector2f((peer.location.getX() + startPoint.x) / 2f, (peer.location.getY() + startPoint.y) / 2f);
+
+ float denominator = direction.dot(normal);
+ if (denominator == 0)
+ continue;
+ midpoint.scaleAdd(-1, startPoint, midpoint);
+ float distance = midpoint.dot(normal) / denominator;
+ if (distance > 0 && distance < minDistance) {
+ minDistance = distance;
+ minPeer = peer;
+ }
+ }
+
+ // Tuples for fixed boundaries: normal.x, normal.y, point.x, point.y
+ float boundaries[] = {1, 0, PeerCoordinates.MIN_X, PeerCoordinates.MIN_Y,
+ 0, 1, PeerCoordinates.MIN_X, PeerCoordinates.MIN_Y,
+ -1, 0, PeerCoordinates.MAX_X, PeerCoordinates.MAX_Y,
+ 0, -1, PeerCoordinates.MAX_X, PeerCoordinates.MAX_X};
+ for (int i = 0; i < boundaries.length; i += 4) {
+ Vector2f normal = new Vector2f(boundaries[i], boundaries[i+1]);
+ Vector2f point = new Vector2f(boundaries[i+2], boundaries[i+3]);
+
+ float denominator = direction.dot(normal);
+ if (denominator == 0)
+ continue;
+ point.scaleAdd(-1, startPoint, point);
+ float distance = point.dot(normal) / denominator;
+ if (distance > 0 && distance < minDistance)
+ return null;
+ }
+
+ return minPeer;
+ }
+
+ /**
+ * A method that sends a CollidableObject to all
+ * <code>Peer</code>s in the network.
+ *
+ * @param payload
+ * A CollidableObject.
+ *
+ * @return <code>true</code> if the message was successfully sent to all
+ * <code>Peer</code>s, or <code>false</code> otherwise.
+ */
+ public synchronized boolean sendToAllPeers(CollidableObject payload) {
+ PeerMessage message = createPayloadMessage(payload);
+ boolean success = true;
+ for (PeerInformation peer : peers)
+ success = sendMessage(message, peer) && success;
+ return success;
+ }
+
+ /**
+ * A method that sends an object to all
+ * <code>Peer</code>s in the network.
+ *
+ * @param payload
+ * An object.
+ *
+ * @return <code>true</code> if the message was successfully sent to all
+ * <code>Peer</code>s, or <code>false</code> otherwise.
+ */
+ public synchronized boolean sendExtraToAllPeers(Object payload) {
+ PeerMessage message = createExtraMessage(payload);
+ boolean success = true;
+ for (PeerInformation peer : peers)
+ success = sendMessage(message, peer) && success;
+ return success;
+ }
+
+ private boolean startServer() {
+ return startServer(DEFAULT_SERVER_PORT);
+ }
+
+ private boolean startServer(int port) {
+ while (true) {
+ try {
+ serverSocket = new ServerSocket(port);
+ } catch (IOException e) {
+ port++;
+ continue;
+ } catch (SecurityException e) {
+ System.err.println(e);
+ return false;
+ }
+ try {
+ myInfo.address = InetAddress.getLocalHost();
+ } catch (UnknownHostException e) {
+ System.err.println(e);
+ try {
+ serverSocket.close();
+ } catch (IOException e1) {
+ }
+ serverSocket = null;
+ return false;
+ }
+ myInfo.port = port;
+ break;
+ }
+
+ System.out.println("Listening on " + myInfo.address + ":" + myInfo.port);
+
+ worker = new SwingWorker<Object,Object>() {
+ protected Object doInBackground() throws Exception {
+ while (!isCancelled()) {
+ logMessage("Waiting for peer connection...");
+ try {
+ Socket socket = serverSocket.accept();
+ processConnection(socket);
+ } catch (SocketException e) {
+ } catch (IOException e) {
+ System.err.println(e);
+ }
+ }
+ return null;
+ }
+ };
+ worker.execute();
+
+ return true;
+ }
+
+ private synchronized void processConnection(Socket socket) {
+ ObjectInputStream socketIn;
+ PeerMessage mesg;
+ try {
+ socketIn = new ObjectInputStream(socket.getInputStream());
+ mesg = (PeerMessage)socketIn.readObject();
+ } catch (Exception e) {
+ System.err.println(e);
+ return;
+ }
+ if (mesg.sender.address == null || mesg.type != PeerMessage.Type.JOIN)
+ mesg.sender.address = socket.getInetAddress();
+ switch (mesg.type) {
+ case JOIN:
+ logMessage("Received JOIN message from " + mesg.sender);
+ while (true) {
+ PeerInformation peer = lookup(mesg.sender.location);
+ if (peer != myInfo) {
+ sendMessage(mesg, peer);
+ break;
+ } else if (myInfo.location.equals(mesg.sender.location))
+ mesg.sender.location.setToRandomCoordinate();
+ else {
+ sendMessage(createJoinResultMessage(mesg.sender.location), mesg.sender);
+ break;
+ }
+ }
+ break;
+ case JOIN_RESULT:
+ logMessage("Received JOIN_RESULT message from " + mesg.sender);
+ assert(peers == null);
+ peers = mesg.peers;
+ assert(myInfo.location == null);
+ myInfo.location = mesg.location;
+ PeerMessage newMesg = createAddPeersMessage();
+ for (PeerInformation peer : peers) {
+ logMessage("Adding peer " + peer);
+ sendMessage(newMesg, peer);
+ }
+ logMessage("Adding peer " + mesg.sender);
+ sendMessage(newMesg, mesg.sender);
+ peers.add(mesg.sender);
+ break;
+ case ADD_PEERS:
+ logMessage("Received ADD_PEERS message from " + mesg.sender);
+ if (!peers.contains(mesg.sender)) {
+ logMessage("Adding peer " + mesg.sender);
+ peers.add(mesg.sender);
+ }
+ for (PeerInformation peer : mesg.peers)
+ if (!peers.contains(peer) && !myInfo.equals(peer)) {
+ logMessage("Adding peer " + peer);
+ peers.add(peer);
+ }
+ break;
+ case REMOVE_PEERS:
+ logMessage("Received REMOVE_PEERS message from " + mesg.sender);
+ if (peers.contains(mesg.sender)) {
+ logMessage("Removing peer " + mesg.sender);
+ peers.remove(mesg.sender);
+ }
+ for (PeerInformation peer : mesg.peers)
+ if (peers.contains(peer)) {
+ logMessage("Removing peer " + peer);
+ peers.remove(peer);
+ }
+ break;
+
+ case PAYLOAD:
+ logMessage("Received PAYLOAD message from " + mesg.sender);
+
+ setChanged();
+ notifyObservers(mesg.payload);
+
+ break;
+
+ case EXTRA:
+ logMessage("Received EXTRA message from " + mesg.sender);
+
+ if (mesg.id != null && mesg.id.equals(PeerMessage.DEFAULT_ID)) {
+ setChanged();
+ notifyObservers(mesg.extra);
+ }
+
+ break;
+ }
+ }
+
+ private synchronized PeerInformation lookup(PeerCoordinates location) {
+ double minDistance = myInfo.location.distanceTo(location);
+ int min = -1;
+ for (int i = 0; i < peers.size(); i++) {
+ double distance = peers.get(i).location.distanceTo(location);
+ if (distance < minDistance) {
+ minDistance = distance;
+ min = i;
+ }
+ }
+ if (min == -1)
+ return myInfo;
+ return peers.get(min);
+ }
+
+ private boolean sendMessage(PeerMessage message, PeerInformation destination) {
+ try {
+ Socket socket = new Socket(destination.address, destination.port);
+ ObjectOutputStream socketOut = new ObjectOutputStream(socket.getOutputStream());
+ socketOut.writeObject(message);
+ socket.close();
+ } catch (IOException e) {
+ System.err.println(e);
+ return false;
+ }
+ return true;
+ }
+
+ private PeerMessage createJoinMessage(PeerCoordinates location) {
+ PeerInformation tmp = new PeerInformation(null, myInfo.port, location);
+ return new PeerMessage(PeerMessage.Type.JOIN, tmp);
+ }
+
+ private PeerMessage createJoinResultMessage(PeerCoordinates location) {
+ PeerMessage mesg = new PeerMessage(PeerMessage.Type.JOIN_RESULT, myInfo);
+ mesg.peers = peers;
+ mesg.location = location;
+ return mesg;
+ }
+
+ private PeerMessage createAddPeersMessage() {
+ return new PeerMessage(PeerMessage.Type.ADD_PEERS, myInfo);
+ }
+
+ private PeerMessage createRemovePeersMessage() {
+ return new PeerMessage(PeerMessage.Type.REMOVE_PEERS, myInfo);
+ }
+
+ private PeerMessage createPayloadMessage(CollidableObject payload) {
+ PeerMessage mesg = new PeerMessage(PeerMessage.Type.PAYLOAD, myInfo);
+ mesg.payload = payload;
+ return mesg;
+ }
+
+ private PeerMessage createExtraMessage(Object payload) {
+ PeerMessage mesg = new PeerMessage(PeerMessage.Type.EXTRA, myInfo);
+ mesg.extra = payload;
+ return mesg;
+ }
+
+ private void logMessage(String text) {
+ if (logEnabled)
+ System.out.println(new Date() + " -- " + text);
+ }
+}
diff --git a/src/common/PeerCoordinates.java b/src/common/PeerCoordinates.java new file mode 100644 index 0000000..13c32e1 --- /dev/null +++ b/src/common/PeerCoordinates.java @@ -0,0 +1,53 @@ +package common;
+import java.io.*;
+
+public class PeerCoordinates implements Serializable {
+ private static final long serialVersionUID = 3667108226485766929L;
+ public static final int MIN_X = 0;
+ public static final int MAX_X = 99;
+ public static final int MIN_Y = 0;
+ public static final int MAX_Y = 99;
+
+ private int x;
+ private int y;
+
+ public PeerCoordinates() {
+ setToRandomCoordinate();
+ }
+
+ public PeerCoordinates(int x, int y) {
+ if (x < MIN_X || x > MAX_X || y < MIN_Y || y > MAX_Y)
+ throw new IllegalArgumentException();
+
+ this.x = x;
+ this.y = y;
+ }
+
+ public int getX() {
+ return x;
+ }
+
+ public int getY() {
+ return y;
+ }
+
+ public double distanceTo(PeerCoordinates other) {
+ return Math.sqrt((x - other.x) * (x - other.x) + (y - other.y) * (y - other.y));
+ }
+
+ public String toString() {
+ return "(" + x + ", " + y + ")";
+ }
+
+ public boolean equals(Object other) {
+ if (!(other instanceof PeerCoordinates))
+ return false;
+
+ return x == ((PeerCoordinates)other).x && y == ((PeerCoordinates)other).y;
+ }
+
+ public void setToRandomCoordinate() {
+ x = MIN_X + (int)((MAX_X - MIN_X + 1) * Math.random());
+ y = MIN_Y + (int)((MAX_Y - MIN_Y + 1) * Math.random());
+ }
+}
diff --git a/src/common/PeerInformation.java b/src/common/PeerInformation.java new file mode 100644 index 0000000..5721e35 --- /dev/null +++ b/src/common/PeerInformation.java @@ -0,0 +1,39 @@ +package common;
+import java.io.*;
+import java.net.*;
+
+public class PeerInformation implements Serializable {
+ private static final long serialVersionUID = 3667108226485766929L;
+ public static final String DEFAULT_ID = "something unique";
+
+ public InetAddress address;
+ public int port;
+ public PeerCoordinates location;
+ public String id;
+
+ public PeerInformation() {
+ id = DEFAULT_ID;
+ }
+
+ public PeerInformation(PeerInformation other) {
+ this(other.address, other.port, other.location);
+ }
+
+ public PeerInformation(InetAddress address, int port, PeerCoordinates location) {
+ this();
+ this.address = address;
+ this.port = port;
+ this.location = location;
+ }
+
+ public String toString() {
+ return address + ":" + port + " @ " + location;
+ }
+
+ public boolean equals(Object other) {
+ if (!(other instanceof PeerInformation))
+ return false;
+
+ return location.equals(((PeerInformation)other).location);
+ }
+}
diff --git a/src/common/PeerMessage.java b/src/common/PeerMessage.java new file mode 100644 index 0000000..4e292b9 --- /dev/null +++ b/src/common/PeerMessage.java @@ -0,0 +1,28 @@ +package common;
+import java.io.*;
+import java.util.*;
+
+
+public class PeerMessage implements Serializable {
+ private static final long serialVersionUID = 3667108226485766929L;
+ public static final String DEFAULT_ID = "TesseractProject";
+
+ public enum Type {
+ JOIN, JOIN_RESULT, ADD_PEERS, REMOVE_PEERS, PAYLOAD, EXTRA;
+ }
+
+ public Type type;
+ public PeerInformation sender;
+ public PeerCoordinates location;
+ public ArrayList<PeerInformation> peers;
+ public CollidableObject payload;
+ public Object extra;
+ public String id;
+
+ public PeerMessage(Type type, PeerInformation sender) {
+ this.type = type;
+ this.sender = sender;
+ peers = new ArrayList<PeerInformation>();
+ this.id = DEFAULT_ID;
+ }
+}
diff --git a/src/common/UnQuat4f.java b/src/common/UnQuat4f.java new file mode 100644 index 0000000..f32546c --- /dev/null +++ b/src/common/UnQuat4f.java @@ -0,0 +1,658 @@ +package common;
+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);
+ }
+
+}
+
+
+
+
|