From 5f27758d3a3543253019f558d8f672c5e8b71c2b Mon Sep 17 00:00:00 2001 From: Jesse Morgan Date: Sat, 5 Mar 2011 00:33:40 +0000 Subject: Changed name of alden to common --- src/alden/CollidableObject.java | 533 ------------------------ src/alden/CollisionDetector.java | 672 ------------------------------ src/alden/CollisionInfo.java | 26 -- src/alden/Peer.java | 588 -------------------------- src/alden/PeerCoordinates.java | 53 --- src/alden/PeerInformation.java | 39 -- src/alden/PeerMessage.java | 28 -- src/alden/UnQuat4f.java | 658 ----------------------------- src/common/CollidableObject.java | 533 ++++++++++++++++++++++++ src/common/CollisionDetector.java | 672 ++++++++++++++++++++++++++++++ src/common/CollisionInfo.java | 26 ++ src/common/Peer.java | 588 ++++++++++++++++++++++++++ src/common/PeerCoordinates.java | 53 +++ src/common/PeerInformation.java | 39 ++ src/common/PeerMessage.java | 28 ++ src/common/UnQuat4f.java | 658 +++++++++++++++++++++++++++++ src/tesseract/TesseractServer.java | 3 +- src/tesseract/TesseractUI.java | 2 +- src/tesseract/World.java | 5 +- src/tesseract/objects/PhysicalObject.java | 2 +- src/tesseract/objects/Polygon.java | 3 +- src/tesseract/objects/Sphere.java | 3 +- 22 files changed, 2608 insertions(+), 2604 deletions(-) delete mode 100644 src/alden/CollidableObject.java delete mode 100644 src/alden/CollisionDetector.java delete mode 100644 src/alden/CollisionInfo.java delete mode 100644 src/alden/Peer.java delete mode 100644 src/alden/PeerCoordinates.java delete mode 100644 src/alden/PeerInformation.java delete mode 100644 src/alden/PeerMessage.java delete mode 100644 src/alden/UnQuat4f.java create mode 100644 src/common/CollidableObject.java create mode 100644 src/common/CollisionDetector.java create mode 100644 src/common/CollisionInfo.java create mode 100644 src/common/Peer.java create mode 100644 src/common/PeerCoordinates.java create mode 100644 src/common/PeerInformation.java create mode 100644 src/common/PeerMessage.java create mode 100644 src/common/UnQuat4f.java diff --git a/src/alden/CollidableObject.java b/src/alden/CollidableObject.java deleted file mode 100644 index 9745665..0000000 --- a/src/alden/CollidableObject.java +++ /dev/null @@ -1,533 +0,0 @@ -package alden; -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 vertexCache; - transient private ArrayList 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 getVertices() { - if (vertexCache == null) - vertexCache = CollisionDetector.extractVertices(node); - return vertexCache; - } - - protected ArrayList 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 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/alden/CollisionDetector.java b/src/alden/CollisionDetector.java deleted file mode 100644 index 84682b8..0000000 --- a/src/alden/CollisionDetector.java +++ /dev/null @@ -1,672 +0,0 @@ -package alden; - -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 EMPTY_COLLISION_LIST = new ArrayList(); - - public static class Triangle { - private Vector3f a; - private Vector3f b; - private Vector3f c; - private Vector3f normal; - private float intercept; - - private static class Line { - public Vector3f point; - public Vector3f direction; - - public Line() { - point = new Vector3f(); - direction = new Vector3f(); - } - } - - private static class TPair { - public float t0; - public float t1; - } - - public Triangle(Tuple3f a, Tuple3f b, Tuple3f c) { - this.a = new Vector3f(a); - this.b = new Vector3f(b); - this.c = new Vector3f(c); - Vector3f tmp = new Vector3f(); - tmp.scaleAdd(-1, a, c); - Vector3f tmp2 = new Vector3f(); - tmp2.scaleAdd(-1, b, c); - normal = new Vector3f(); - normal.cross(tmp, tmp2); - if (normal.lengthSquared() == 0) - throw new IllegalArgumentException("Degenerate triangle"); - normal.normalize(); - intercept = normal.dot(this.a); - } - - // Inspired by Tomas Moller's "A Fast Triangle-Triangle Intersection Test" - public CollisionInfo getIntersection(Triangle other) { - float d_0_0 = other.normal.dot(a) - other.intercept; - float d_0_1 = other.normal.dot(b) - other.intercept; - float d_0_2 = other.normal.dot(c) - other.intercept; - if (Math.abs(d_0_0) < EPSILON) - d_0_0 = 0; - if (Math.abs(d_0_1) < EPSILON) - d_0_1 = 0; - if (Math.abs(d_0_2) < EPSILON) - d_0_2 = 0; - if (d_0_0 != 0 && d_0_1 != 0 && d_0_2 != 0 && Math.signum(d_0_0) == Math.signum(d_0_1) && Math.signum(d_0_1) == Math.signum(d_0_2)) - return null; - - float d_1_0 = normal.dot(other.a) - intercept; - float d_1_1 = normal.dot(other.b) - intercept; - float d_1_2 = normal.dot(other.c) - intercept; - if (Math.abs(d_1_0) < EPSILON) - d_1_0 = 0; - if (Math.abs(d_1_1) < EPSILON) - d_1_1 = 0; - if (Math.abs(d_1_2) < EPSILON) - d_1_2 = 0; - if (d_1_0 != 0 && d_1_1 != 0 && d_1_2 != 0 && Math.signum(d_1_0) == Math.signum(d_1_1) && Math.signum(d_1_1) == Math.signum(d_1_2)) - return null; - - // Coplanar, assume no collision - if (d_0_0 == 0 && d_0_1 == 0 && d_0_2 == 0) - return null; - - Line line = calculateLineOfIntersection(other); - TPair r0 = calculateRegionOfIntersection(line, d_0_0, d_0_1, d_0_2); - TPair r1 = other.calculateRegionOfIntersection(line, d_1_0, d_1_1, d_1_2); - - if (r0.t1 < r1.t0 || r0.t0 > r1.t1) - return null; - - Vector3f contactPoint = new Vector3f(); - if (r0.t0 >= r1.t0 && r0.t1 <= r1.t1) - contactPoint.scaleAdd((r0.t0 + r0.t1) / 2, line.direction, line.point); - else if (r0.t0 <= r1.t0 && r0.t1 >= r1.t1) - contactPoint.scaleAdd((r1.t0 + r1.t1) / 2, line.direction, line.point); - else if (r0.t0 < r1.t0) - contactPoint.scaleAdd((r0.t1 + r1.t0) / 2, line.direction, line.point); - else - contactPoint.scaleAdd((r0.t0 + r1.t1) / 2, line.direction, line.point); - - assert(Math.abs(normal.dot(contactPoint) - intercept) < 0.01); - assert(Math.abs(other.normal.dot(contactPoint) - other.intercept) < 0.01); - - float penetration = Float.NEGATIVE_INFINITY; - boolean useThisNormal = false; - if (d_0_0 <= 0 && d_0_0 >= penetration) - penetration = d_0_0; - if (d_0_1 <= 0 && d_0_1 >= penetration) - penetration = d_0_1; - if (d_0_2 <= 0 && d_0_2 >= penetration) - penetration = d_0_2; - if (d_1_0 <= 0 && d_1_0 >= penetration) { - penetration = d_1_0; - useThisNormal = true; - } - if (d_1_1 <= 0 && d_1_1 >= penetration) { - penetration = d_1_1; - useThisNormal = true; - } - if (d_1_2 <= 0 && d_1_2 >= penetration) { - penetration = d_1_2; - useThisNormal = true; - } - Vector3f contactNormal; - if (useThisNormal) - contactNormal = new Vector3f(normal); - else { - contactNormal = new Vector3f(other.normal); - contactNormal.negate(); - } - - return new CollisionInfo(contactPoint, contactNormal, -penetration); - } - - private Line calculateLineOfIntersection(Triangle other) { - Line line = new Line(); - line.direction.cross(normal, other.normal); - if (Math.abs(line.direction.x) < EPSILON) - line.direction.x = 0; - if (Math.abs(line.direction.y) < EPSILON) - line.direction.y = 0; - if (Math.abs(line.direction.z) < EPSILON) - line.direction.z = 0; - line.direction.normalize(); - - if (line.direction.x != 0) { // x <- 0 - if (normal.y != 0) { - line.point.z = (other.normal.y / normal.y * intercept - other.intercept) / (other.normal.y / normal.y * normal.z - other.normal.z); - line.point.y = (intercept - normal.z * line.point.z) / normal.y; - } else { // normal.z != 0 - line.point.y = (other.normal.z / normal.z * intercept - other.intercept) / (other.normal.z / normal.z * normal.y - other.normal.y); - line.point.z = (intercept - normal.y * line.point.y) / normal.z; - } - } else if (line.direction.y != 0) { // y <- 0 - if (normal.x != 0) { - line.point.z = (other.normal.x / normal.x * intercept - other.intercept) / (other.normal.x / normal.x * normal.z - other.normal.z); - line.point.x = (intercept - normal.z * line.point.z) / normal.x; - } else { // normal.z != 0 - line.point.x = (other.normal.z / normal.z * intercept - other.intercept) / (other.normal.z / normal.z * normal.x - other.normal.x); - line.point.z = (intercept - normal.x * line.point.x) / normal.z; - } - } else { // z <- 0 - if (normal.x != 0) { - line.point.y = (other.normal.x / normal.x * intercept - other.intercept) / (other.normal.x / normal.x * normal.y - other.normal.y); - line.point.x = (intercept - normal.y * line.point.y) / normal.x; - } else { // normal.y != 0 - line.point.x = (other.normal.y / normal.y * intercept - other.intercept) / (other.normal.y / normal.y * normal.x - other.normal.x); - line.point.y = (intercept - normal.x * line.point.x) / normal.y; - } - } - - assert(Math.abs(normal.dot(line.point) - intercept) < 0.01); - assert(Math.abs(other.normal.dot(line.point) - other.intercept) < 0.01); - - return line; - } - - private TPair calculateRegionOfIntersection(Line line, float d0, float d1, float d2) { - Vector3f v0, v1, v2; - if (Math.signum(d0) != 0 && Math.signum(d0) != Math.signum(d1) && Math.signum(d0) != Math.signum(d2)) { - v0 = b; v1 = a; v2 = c; - float tmp = d0; d0 = d1; d1 = tmp; - } else if (Math.signum(d1) != 0 && Math.signum(d0) != Math.signum(d1) && Math.signum(d1) != Math.signum(d2)) { - v0 = a; v1 = b; v2 = c; - } else if (Math.signum(d2) != 0 && Math.signum(d0) != Math.signum(d2) && Math.signum(d1) != Math.signum(d2)) { - v0 = a; v1 = c; v2 = b; - float tmp = d1; d1 = d2; d2 = tmp; - } else if (Math.signum(d0) == 0) { - v0 = b; v1 = a; v2 = c; - float tmp = d0; d0 = d1; d1 = tmp; - } else if (Math.signum(d1) == 0) { - v0 = a; v1 = b; v2 = c; - } else { - v0 = a; v1 = c; v2 = b; - float tmp = d1; d1 = d2; d2 = tmp; - } - - Vector3f tmp = new Vector3f(); - tmp.scaleAdd(-1, line.point, v0); - float p0 = line.direction.dot(tmp); - tmp.scaleAdd(-1, line.point, v1); - float p1 = line.direction.dot(tmp); - tmp.scaleAdd(-1, line.point, v2); - float p2 = line.direction.dot(tmp); - - TPair region = new TPair(); - region.t0 = p0 + (p1 - p0) * d0 / (d0 - d1); - region.t1 = p2 + (p1 - p2) * d2 / (d2 - d1); - if (region.t1 < region.t0) { - float tmp2 = region.t0; - region.t0 = region.t1; - region.t1 = tmp2; - } - return region; - } - } - - public static ArrayList calculateCollisions(CollidableObject a, CollidableObject b) { - if (a == b) - return EMPTY_COLLISION_LIST; - if (a instanceof HalfSpace) { - if (b instanceof HalfSpace) - return EMPTY_COLLISION_LIST; - if (b instanceof 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 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 collisions = new ArrayList(); - collisions.add(new CollisionInfo(contactPoint, new Vector3f(a.normal), penetration)); - return collisions; - } - - private static ArrayList 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 collisions = new ArrayList(); - collisions.add(new CollisionInfo(contactPoint, new Vector3f(a.normal), penetration)); - return collisions; - } - - private static ArrayList calculateCollisions(HalfSpace a, ArrayList setB) { - ArrayList collisions = new ArrayList(); - for (Vector3f vertex : setB) { - float penetration = a.intercept - a.normal.dot(vertex); - if (penetration >= 0) { - Vector3f contactPoint = new Vector3f(); - contactPoint.scaleAdd(penetration, a.normal, vertex); - assert(Math.abs(a.normal.dot(contactPoint) - a.intercept) < 0.01); - collisions.add(new CollisionInfo(contactPoint, new Vector3f(a.normal), penetration)); - } - } - return collisions; - } - - private static ArrayList 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 collisions = new ArrayList(); - 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 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 collisions = new ArrayList(); - collisions.add(new CollisionInfo(contactPoint, contactNormal, penetration)); - return collisions; - } - } - return EMPTY_COLLISION_LIST; - } - - private static ArrayList calculateCollisions(Particle a, CollidableObject b) { - ArrayList collisions = new ArrayList(); - 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 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 collisions = new ArrayList(); - delta.normalize(); - Vector3f contactPoint = new Vector3f(); - contactPoint.scaleAdd(a.radius - 0.5f * penetration, delta, a.position); - collisions.add(new CollisionInfo(contactPoint, delta, penetration)); - return collisions; - } - - private static ArrayList calculateCollisions(Polygon a, CollidableObject b) { - ArrayList 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 calculateCollisions(ArrayList setA, ArrayList setB) { - ArrayList collisions = new ArrayList(); - for (int i = 0; i < setA.size(); i++) - for (int j = 0; j < setB.size(); j++) { - CollisionInfo collision = setA.get(i).getIntersection(setB.get(j)); - if (collision != null) - collisions.add(collision); - } - return collisions; - } - - private static ArrayList flipContactNormals(ArrayList collisions) { - for (CollisionInfo collision : collisions) - collision.contactNormal.negate(); - return collisions; - } - - public static ArrayList extractVertices(Node node) { - ArrayList vertices = new ArrayList(); - extractVertices(node, vertices); - return vertices; - } - - private static void extractVertices(Node node, ArrayList vertices) { - if (node instanceof Primitive) { - Primitive prim = (Primitive)node; - int index = 0; - Shape3D shape; - Transform3D L2V = new Transform3D(); - while ((shape = prim.getShape(index++)) != null) { - shape.getLocalToVworld(L2V); - for (int i = 0; i < shape.numGeometries(); i++) - extractVertices(shape.getGeometry(i), L2V, vertices); - } - } else if (node instanceof Shape3D) { - Shape3D shape = (Shape3D)node; - Transform3D L2V = new Transform3D(); - shape.getLocalToVworld(L2V); - for (int i = 0; i < shape.numGeometries(); i++) - extractVertices(shape.getGeometry(i), L2V, vertices); - } else if (node instanceof Group) { - Group group = (Group)node; - for (int i = 0; i < group.numChildren(); i++) - extractVertices(group.getChild(i), vertices); - } else - throw new IllegalArgumentException("Illegal node type for vertex extraction"); - } - - private static void extractVertices(Geometry geometry, Transform3D transform, ArrayList vertices) { - if (geometry instanceof GeometryArray) { - GeometryArray trueGeometry = (GeometryArray)geometry; - vertices.ensureCapacity(vertices.size() + trueGeometry.getValidVertexCount()); - Point3f vertex = new Point3f(); - for (int i = 0; i < trueGeometry.getValidVertexCount(); i++) { - trueGeometry.getCoordinate(i, vertex); - transform.transform(vertex); - vertices.add(new Vector3f(vertex)); - } - } else - throw new IllegalArgumentException("Illegal geometry type for vertex extraction"); - } - - public static ArrayList triangularize(Node node) { - ArrayList triangles = new ArrayList(); - triangularize(node, triangles); - return triangles; - } - - private static void triangularize(Node node, ArrayList triangles) { - if (node instanceof Primitive) { - Primitive prim = (Primitive)node; - triangles.ensureCapacity(prim.getNumTriangles()); - int index = 0; - Shape3D shape; - Transform3D L2V = new Transform3D(); - while ((shape = prim.getShape(index++)) != null) { - shape.getLocalToVworld(L2V); - for (int i = 0; i < shape.numGeometries(); i++) - triangularize(shape.getGeometry(i), L2V, triangles); - } - } else if (node instanceof Shape3D) { - Shape3D shape = (Shape3D)node; - Transform3D L2V = new Transform3D(); - shape.getLocalToVworld(L2V); - for (int i = 0; i < shape.numGeometries(); i++) - triangularize(shape.getGeometry(i), L2V, triangles); - } else if (node instanceof Group) { - Group group = (Group)node; - for (int i = 0; i < group.numChildren(); i++) - triangularize(group.getChild(i), triangles); - } else - throw new IllegalArgumentException("Illegal node type for triangularization"); - } - - private static void triangularize(Geometry geometry, Transform3D transform, ArrayList triangles) { - if (geometry instanceof TriangleArray) { - TriangleArray trueGeometry = (TriangleArray)geometry; - Point3f vertices[] = new Point3f[trueGeometry.getValidVertexCount()]; - for (int i = 0; i < vertices.length; i++) { - vertices[i] = new Point3f(); - trueGeometry.getCoordinate(i, vertices[i]); - transform.transform(vertices[i]); - } - for (int i = 0; i < vertices.length; i += 3) - try { - triangles.add(new Triangle(vertices[i], vertices[i+1], vertices[i+2])); - } catch (IllegalArgumentException e) { - } - } else if (geometry instanceof TriangleStripArray) { - TriangleStripArray trueGeometry = (TriangleStripArray)geometry; - int stripVertexCounts[] = new int[trueGeometry.getNumStrips()]; - trueGeometry.getStripVertexCounts(stripVertexCounts); - Point3f vertices[] = new Point3f[trueGeometry.getValidVertexCount()]; - for (int i = 0; i < vertices.length; i++) { - vertices[i] = new Point3f(); - trueGeometry.getCoordinate(i, vertices[i]); - transform.transform(vertices[i]); - } - int base = 0; - for (int i = 0; i < stripVertexCounts.length; i++) { - boolean reverse = false; - for (int j = 2; j < stripVertexCounts[i]; j++) { - try { - if (reverse) - triangles.add(new Triangle(vertices[base+j-2], vertices[base+j], vertices[base+j-1])); - else - triangles.add(new Triangle(vertices[base+j-2], vertices[base+j-1], vertices[base+j])); - } catch (IllegalArgumentException e) { - } - reverse = !reverse; - } - base += stripVertexCounts[i]; - } - } else if (geometry instanceof TriangleFanArray) { - TriangleFanArray trueGeometry = (TriangleFanArray)geometry; - int stripVertexCounts[] = new int[trueGeometry.getNumStrips()]; - trueGeometry.getStripVertexCounts(stripVertexCounts); - Point3f vertices[] = new Point3f[trueGeometry.getValidVertexCount()]; - for (int i = 0; i < vertices.length; i++) { - vertices[i] = new Point3f(); - trueGeometry.getCoordinate(i, vertices[i]); - transform.transform(vertices[i]); - } - int base = 0; - for (int i = 0; i < stripVertexCounts.length; i++) { - for (int j = 2; j < stripVertexCounts[i]; j++) - try { - triangles.add(new Triangle(vertices[base], vertices[base+j-1], vertices[base+j])); - } catch (IllegalArgumentException e) { - } - base += stripVertexCounts[i]; - } - } else if (geometry instanceof IndexedTriangleArray) { - IndexedTriangleArray trueGeometry = (IndexedTriangleArray)geometry; - Point3f vertices[] = new Point3f[trueGeometry.getValidVertexCount()]; - for (int i = 0; i < vertices.length; i++) { - vertices[i] = new Point3f(); - trueGeometry.getCoordinate(i, vertices[i]); - transform.transform(vertices[i]); - } - int indices[] = new int[trueGeometry.getValidIndexCount()]; - trueGeometry.getCoordinateIndices(0, indices); - for (int i = 0; i < indices.length; i += 3) - try { - triangles.add(new Triangle(vertices[indices[i]], vertices[indices[i+1]], vertices[indices[i+2]])); - } catch (IllegalArgumentException e) { - } - } else if (geometry instanceof IndexedTriangleStripArray) { - IndexedTriangleStripArray trueGeometry = (IndexedTriangleStripArray)geometry; - int stripIndexCounts[] = new int[trueGeometry.getNumStrips()]; - trueGeometry.getStripIndexCounts(stripIndexCounts); - Point3f vertices[] = new Point3f[trueGeometry.getValidVertexCount()]; - for (int i = 0; i < vertices.length; i++) { - vertices[i] = new Point3f(); - trueGeometry.getCoordinate(i, vertices[i]); - transform.transform(vertices[i]); - } - int indices[] = new int[trueGeometry.getValidIndexCount()]; - trueGeometry.getCoordinateIndices(0, indices); - int base = 0; - for (int i = 0; i < stripIndexCounts.length; i++) { - boolean reverse = false; - for (int j = 2; j < stripIndexCounts[i]; j++) { - try { - if (reverse) - triangles.add(new Triangle(vertices[indices[base+j-2]], vertices[indices[base+j]], vertices[indices[base+j-1]])); - else - triangles.add(new Triangle(vertices[indices[base+j-2]], vertices[indices[base+j-1]], vertices[indices[base+j]])); - } catch (IllegalArgumentException e) { - } - reverse = !reverse; - } - base += stripIndexCounts[i]; - } - } else if (geometry instanceof IndexedTriangleFanArray) { - IndexedTriangleFanArray trueGeometry = (IndexedTriangleFanArray)geometry; - int stripIndexCounts[] = new int[trueGeometry.getNumStrips()]; - trueGeometry.getStripIndexCounts(stripIndexCounts); - Point3f vertices[] = new Point3f[trueGeometry.getValidVertexCount()]; - for (int i = 0; i < vertices.length; i++) { - vertices[i] = new Point3f(); - trueGeometry.getCoordinate(i, vertices[i]); - transform.transform(vertices[i]); - } - int indices[] = new int[trueGeometry.getValidIndexCount()]; - trueGeometry.getCoordinateIndices(0, indices); - int base = 0; - for (int i = 0; i < stripIndexCounts.length; i++) { - for (int j = 2; j < stripIndexCounts[i]; j++) - try { - triangles.add(new Triangle(vertices[indices[base]], vertices[indices[base+j-1]], vertices[indices[base+j]])); - } catch (IllegalArgumentException e) { - } - base += stripIndexCounts[i]; - } - } else - throw new IllegalArgumentException("Illegal geometry type for triangularization"); - } - - public static Shape3D createShape(ArrayList triangles) { - TriangleArray geometry = new TriangleArray(3 * triangles.size(), TriangleArray.COORDINATES); - for (int i = 0; i < triangles.size(); i++) { - geometry.setCoordinate(3 * i, new Point3f(triangles.get(i).a)); - geometry.setCoordinate(3 * i + 1, new Point3f(triangles.get(i).b)); - geometry.setCoordinate(3 * i + 2, new Point3f(triangles.get(i).c)); - } - Appearance appearance = new Appearance(); - PolygonAttributes polyAttr = new PolygonAttributes(PolygonAttributes.POLYGON_LINE, PolygonAttributes.CULL_NONE, 0); - appearance.setPolygonAttributes(polyAttr); - return new Shape3D(geometry, appearance); - } -} diff --git a/src/alden/CollisionInfo.java b/src/alden/CollisionInfo.java deleted file mode 100644 index dad74a5..0000000 --- a/src/alden/CollisionInfo.java +++ /dev/null @@ -1,26 +0,0 @@ -package alden; -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/alden/Peer.java b/src/alden/Peer.java deleted file mode 100644 index 931cc10..0000000 --- a/src/alden/Peer.java +++ /dev/null @@ -1,588 +0,0 @@ -package alden; -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 peers; - private ServerSocket serverSocket; - private SwingWorker 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 Peer object. The Peer is - * not connected to a network and sends a log of internal operation - * messages to the console. - */ - public Peer() { - this(true); - } - - /** - * Initializes a new Peer object with message logging - * controlled by the logEnabled parameter. The - * Peer is not connected to a network. - * - * @param logEnabled Initial value for the logEnabled field. - */ - public Peer(boolean logEnabled) { - myInfo = new PeerInformation(); - this.logEnabled = logEnabled; - } - - /** - * Establishes a new peer-to-peer network with this Peer - * as the sole member. The logical coordinates of this - * Peer are chosen randomly. - * - * @return true if the new network was established - * successfully, or false otherwise. - */ - public boolean createNetwork() { - return createNetwork(new PeerCoordinates()); - } - - /** - * Establishes a new peer-to-peer network with this Peer - * as the sole member. The logical coordinates of this - * Peer are specified by parameter location. - * - * @param location Logical coordinates for this Peer - * within the new network. - * - * @return true if the new network was established - * successfully, or false otherwise. - */ - public boolean createNetwork(PeerCoordinates location) { - if (serverSocket == null) - if (!startServer()) - return false; - peers = new ArrayList(); - myInfo.location = location; - logMessage("Established network @ " + myInfo.location); - return true; - } - - /** - * Connects this Peer to an existing peer-to-peer network. The - * port number of the known Peer must be - * {@link #DEFAULT_SERVER_PORT DEFAULT_SERVER_PORT}. The logical coordinates - * of this Peer are chosen randomly. - * - * @param host - * The domain name or IP address of a Peer within - * the network. - * - * @return true if this Peer successfully - * connected to the network, or false otherwise. - */ - public boolean connectToNetwork(String host) { - return connectToNetwork(host, DEFAULT_SERVER_PORT); - } - - /** - * Connects this Peer to an existing peer-to-peer network. The - * port number of the known Peer must be - * {@link #DEFAULT_SERVER_PORT DEFAULT_SERVER_PORT}. The preferred logical - * coordinates of this Peer are specified by parameter - * location, but the actual logical coordinates may be chosen - * randomly to avoid collision with other Peers. - * - * @param host - * The domain name or IP address of a Peer within - * the network. - * @param location - * Preferred logical coordinates for this Peer. - * - * @return true if this Peer successfully - * connected to the network, or false otherwise. - */ - public boolean connectToNetwork(String host, PeerCoordinates location) { - return connectToNetwork(host, DEFAULT_SERVER_PORT, location); - } - - /** - * Connects this Peer to an existing peer-to-peer network. The - * logical coordinates of this Peer are chosen randomly. - * - * @param host - * The domain name or IP address of a Peer within - * the network. - * @param port - * The port number of a Peer within the network. - * - * @return true if this Peer successfully - * connected to the network, or false 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 Peer to an existing peer-to-peer network. The - * preferred logical coordinates of this Peer are specified by - * parameter location, but the actual logical coordinates may - * be chosen randomly to avoid collision with other Peers. - * - * @param host - * The domain name or IP address of a Peer within - * the network. - * @param port - * The port number of a Peer within the network. - * @param location - * Preferred logical coordinates for this Peer. - * - * @return true if this Peer successfully - * connected to the network, or false 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 Peer to an existing peer-to-peer network. The - * port number of the known Peer must be - * {@link #DEFAULT_SERVER_PORT DEFAULT_SERVER_PORT}. The logical coordinates - * of this Peer are chosen randomly. - * - * @param host - * The IP address of a Peer within the network. - * - * @return true if this Peer successfully - * connected to the network, or false otherwise. - */ - public boolean connectToNetwork(InetAddress host) { - return connectToNetwork(host, DEFAULT_SERVER_PORT); - } - - /** - * Connects this Peer to an existing peer-to-peer network. The - * port number of the known Peer must be - * {@link #DEFAULT_SERVER_PORT DEFAULT_SERVER_PORT}. The preferred logical - * coordinates of this Peer are specified by parameter - * location, but the actual logical coordinates may be chosen - * randomly to avoid collision with other Peers. - * - * @param host - * The IP address of a Peer within the network. - * @param location - * Preferred logical coordinates for this Peer. - * - * @return true if this Peer successfully - * connected to the network, or false otherwise. - */ - public boolean connectToNetwork(InetAddress host, PeerCoordinates location) { - return connectToNetwork(host, DEFAULT_SERVER_PORT, location); - } - - /** - * Connects this Peer to an existing peer-to-peer network. The - * logical coordinates of this Peer are chosen randomly. - * - * @param host - * The IP address of a Peer within the network. - * @param port - * The port number of a Peer within the network. - * - * @return true if this Peer successfully - * connected to the network, or false otherwise. - */ - public boolean connectToNetwork(InetAddress host, int port) { - return connectToNetwork(host, port, new PeerCoordinates()); - } - - /** - * Connects this Peer to an existing peer-to-peer network. The - * preferred logical coordinates of this Peer are specified by - * parameter location, but the actual logical coordinates may - * be chosen randomly to avoid collision with other Peers. - * - * @param host - * The IP address of a Peer within the network. - * @param port - * The port number of a Peer within the network. - * @param location - * Preferred logical coordinates for this Peer. - * - * @return true if this Peer successfully connects - * to the network, or false 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 Peer 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 Peer in the network adjacent to this - * Peer in the direction (x,y). - * - * @param x - * X component of a direction relative to the logical coordinates - * of this Peer. - * @param y - * Y component of a direction relative to the logical coordinates - * of this Peer. - * - * @return A PeerInformation object representing the adjacent - * Peer, or null if no adjacent - * Peer 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 - * Peers in the network. - * - * @param payload - * A CollidableObject. - * - * @return true if the message was successfully sent to all - * Peers, or false 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 - * Peers in the network. - * - * @param payload - * An object. - * - * @return true if the message was successfully sent to all - * Peers, or false 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() { - 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/alden/PeerCoordinates.java b/src/alden/PeerCoordinates.java deleted file mode 100644 index 4d0e445..0000000 --- a/src/alden/PeerCoordinates.java +++ /dev/null @@ -1,53 +0,0 @@ -package alden; -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/alden/PeerInformation.java b/src/alden/PeerInformation.java deleted file mode 100644 index 3d94f11..0000000 --- a/src/alden/PeerInformation.java +++ /dev/null @@ -1,39 +0,0 @@ -package alden; -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/alden/PeerMessage.java b/src/alden/PeerMessage.java deleted file mode 100644 index a5af7de..0000000 --- a/src/alden/PeerMessage.java +++ /dev/null @@ -1,28 +0,0 @@ -package alden; -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 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(); - this.id = DEFAULT_ID; - } -} diff --git a/src/alden/UnQuat4f.java b/src/alden/UnQuat4f.java deleted file mode 100644 index 59295a9..0000000 --- a/src/alden/UnQuat4f.java +++ /dev/null @@ -1,658 +0,0 @@ -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); - } - -} - - - - 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 vertexCache; + transient private ArrayList 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 getVertices() { + if (vertexCache == null) + vertexCache = CollisionDetector.extractVertices(node); + return vertexCache; + } + + protected ArrayList 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 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 EMPTY_COLLISION_LIST = new ArrayList(); + + public static class Triangle { + private Vector3f a; + private Vector3f b; + private Vector3f c; + private Vector3f normal; + private float intercept; + + private static class Line { + public Vector3f point; + public Vector3f direction; + + public Line() { + point = new Vector3f(); + direction = new Vector3f(); + } + } + + private static class TPair { + public float t0; + public float t1; + } + + public Triangle(Tuple3f a, Tuple3f b, Tuple3f c) { + this.a = new Vector3f(a); + this.b = new Vector3f(b); + this.c = new Vector3f(c); + Vector3f tmp = new Vector3f(); + tmp.scaleAdd(-1, a, c); + Vector3f tmp2 = new Vector3f(); + tmp2.scaleAdd(-1, b, c); + normal = new Vector3f(); + normal.cross(tmp, tmp2); + if (normal.lengthSquared() == 0) + throw new IllegalArgumentException("Degenerate triangle"); + normal.normalize(); + intercept = normal.dot(this.a); + } + + // Inspired by Tomas Moller's "A Fast Triangle-Triangle Intersection Test" + public CollisionInfo getIntersection(Triangle other) { + float d_0_0 = other.normal.dot(a) - other.intercept; + float d_0_1 = other.normal.dot(b) - other.intercept; + float d_0_2 = other.normal.dot(c) - other.intercept; + if (Math.abs(d_0_0) < EPSILON) + d_0_0 = 0; + if (Math.abs(d_0_1) < EPSILON) + d_0_1 = 0; + if (Math.abs(d_0_2) < EPSILON) + d_0_2 = 0; + if (d_0_0 != 0 && d_0_1 != 0 && d_0_2 != 0 && Math.signum(d_0_0) == Math.signum(d_0_1) && Math.signum(d_0_1) == Math.signum(d_0_2)) + return null; + + float d_1_0 = normal.dot(other.a) - intercept; + float d_1_1 = normal.dot(other.b) - intercept; + float d_1_2 = normal.dot(other.c) - intercept; + if (Math.abs(d_1_0) < EPSILON) + d_1_0 = 0; + if (Math.abs(d_1_1) < EPSILON) + d_1_1 = 0; + if (Math.abs(d_1_2) < EPSILON) + d_1_2 = 0; + if (d_1_0 != 0 && d_1_1 != 0 && d_1_2 != 0 && Math.signum(d_1_0) == Math.signum(d_1_1) && Math.signum(d_1_1) == Math.signum(d_1_2)) + return null; + + // Coplanar, assume no collision + if (d_0_0 == 0 && d_0_1 == 0 && d_0_2 == 0) + return null; + + Line line = calculateLineOfIntersection(other); + TPair r0 = calculateRegionOfIntersection(line, d_0_0, d_0_1, d_0_2); + TPair r1 = other.calculateRegionOfIntersection(line, d_1_0, d_1_1, d_1_2); + + if (r0.t1 < r1.t0 || r0.t0 > r1.t1) + return null; + + Vector3f contactPoint = new Vector3f(); + if (r0.t0 >= r1.t0 && r0.t1 <= r1.t1) + contactPoint.scaleAdd((r0.t0 + r0.t1) / 2, line.direction, line.point); + else if (r0.t0 <= r1.t0 && r0.t1 >= r1.t1) + contactPoint.scaleAdd((r1.t0 + r1.t1) / 2, line.direction, line.point); + else if (r0.t0 < r1.t0) + contactPoint.scaleAdd((r0.t1 + r1.t0) / 2, line.direction, line.point); + else + contactPoint.scaleAdd((r0.t0 + r1.t1) / 2, line.direction, line.point); + + assert(Math.abs(normal.dot(contactPoint) - intercept) < 0.01); + assert(Math.abs(other.normal.dot(contactPoint) - other.intercept) < 0.01); + + float penetration = Float.NEGATIVE_INFINITY; + boolean useThisNormal = false; + if (d_0_0 <= 0 && d_0_0 >= penetration) + penetration = d_0_0; + if (d_0_1 <= 0 && d_0_1 >= penetration) + penetration = d_0_1; + if (d_0_2 <= 0 && d_0_2 >= penetration) + penetration = d_0_2; + if (d_1_0 <= 0 && d_1_0 >= penetration) { + penetration = d_1_0; + useThisNormal = true; + } + if (d_1_1 <= 0 && d_1_1 >= penetration) { + penetration = d_1_1; + useThisNormal = true; + } + if (d_1_2 <= 0 && d_1_2 >= penetration) { + penetration = d_1_2; + useThisNormal = true; + } + Vector3f contactNormal; + if (useThisNormal) + contactNormal = new Vector3f(normal); + else { + contactNormal = new Vector3f(other.normal); + contactNormal.negate(); + } + + return new CollisionInfo(contactPoint, contactNormal, -penetration); + } + + private Line calculateLineOfIntersection(Triangle other) { + Line line = new Line(); + line.direction.cross(normal, other.normal); + if (Math.abs(line.direction.x) < EPSILON) + line.direction.x = 0; + if (Math.abs(line.direction.y) < EPSILON) + line.direction.y = 0; + if (Math.abs(line.direction.z) < EPSILON) + line.direction.z = 0; + line.direction.normalize(); + + if (line.direction.x != 0) { // x <- 0 + if (normal.y != 0) { + line.point.z = (other.normal.y / normal.y * intercept - other.intercept) / (other.normal.y / normal.y * normal.z - other.normal.z); + line.point.y = (intercept - normal.z * line.point.z) / normal.y; + } else { // normal.z != 0 + line.point.y = (other.normal.z / normal.z * intercept - other.intercept) / (other.normal.z / normal.z * normal.y - other.normal.y); + line.point.z = (intercept - normal.y * line.point.y) / normal.z; + } + } else if (line.direction.y != 0) { // y <- 0 + if (normal.x != 0) { + line.point.z = (other.normal.x / normal.x * intercept - other.intercept) / (other.normal.x / normal.x * normal.z - other.normal.z); + line.point.x = (intercept - normal.z * line.point.z) / normal.x; + } else { // normal.z != 0 + line.point.x = (other.normal.z / normal.z * intercept - other.intercept) / (other.normal.z / normal.z * normal.x - other.normal.x); + line.point.z = (intercept - normal.x * line.point.x) / normal.z; + } + } else { // z <- 0 + if (normal.x != 0) { + line.point.y = (other.normal.x / normal.x * intercept - other.intercept) / (other.normal.x / normal.x * normal.y - other.normal.y); + line.point.x = (intercept - normal.y * line.point.y) / normal.x; + } else { // normal.y != 0 + line.point.x = (other.normal.y / normal.y * intercept - other.intercept) / (other.normal.y / normal.y * normal.x - other.normal.x); + line.point.y = (intercept - normal.x * line.point.x) / normal.y; + } + } + + assert(Math.abs(normal.dot(line.point) - intercept) < 0.01); + assert(Math.abs(other.normal.dot(line.point) - other.intercept) < 0.01); + + return line; + } + + private TPair calculateRegionOfIntersection(Line line, float d0, float d1, float d2) { + Vector3f v0, v1, v2; + if (Math.signum(d0) != 0 && Math.signum(d0) != Math.signum(d1) && Math.signum(d0) != Math.signum(d2)) { + v0 = b; v1 = a; v2 = c; + float tmp = d0; d0 = d1; d1 = tmp; + } else if (Math.signum(d1) != 0 && Math.signum(d0) != Math.signum(d1) && Math.signum(d1) != Math.signum(d2)) { + v0 = a; v1 = b; v2 = c; + } else if (Math.signum(d2) != 0 && Math.signum(d0) != Math.signum(d2) && Math.signum(d1) != Math.signum(d2)) { + v0 = a; v1 = c; v2 = b; + float tmp = d1; d1 = d2; d2 = tmp; + } else if (Math.signum(d0) == 0) { + v0 = b; v1 = a; v2 = c; + float tmp = d0; d0 = d1; d1 = tmp; + } else if (Math.signum(d1) == 0) { + v0 = a; v1 = b; v2 = c; + } else { + v0 = a; v1 = c; v2 = b; + float tmp = d1; d1 = d2; d2 = tmp; + } + + Vector3f tmp = new Vector3f(); + tmp.scaleAdd(-1, line.point, v0); + float p0 = line.direction.dot(tmp); + tmp.scaleAdd(-1, line.point, v1); + float p1 = line.direction.dot(tmp); + tmp.scaleAdd(-1, line.point, v2); + float p2 = line.direction.dot(tmp); + + TPair region = new TPair(); + region.t0 = p0 + (p1 - p0) * d0 / (d0 - d1); + region.t1 = p2 + (p1 - p2) * d2 / (d2 - d1); + if (region.t1 < region.t0) { + float tmp2 = region.t0; + region.t0 = region.t1; + region.t1 = tmp2; + } + return region; + } + } + + public static ArrayList calculateCollisions(CollidableObject a, CollidableObject b) { + if (a == b) + return EMPTY_COLLISION_LIST; + if (a instanceof HalfSpace) { + if (b instanceof HalfSpace) + return EMPTY_COLLISION_LIST; + if (b instanceof 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 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 collisions = new ArrayList(); + collisions.add(new CollisionInfo(contactPoint, new Vector3f(a.normal), penetration)); + return collisions; + } + + private static ArrayList 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 collisions = new ArrayList(); + collisions.add(new CollisionInfo(contactPoint, new Vector3f(a.normal), penetration)); + return collisions; + } + + private static ArrayList calculateCollisions(HalfSpace a, ArrayList setB) { + ArrayList collisions = new ArrayList(); + for (Vector3f vertex : setB) { + float penetration = a.intercept - a.normal.dot(vertex); + if (penetration >= 0) { + Vector3f contactPoint = new Vector3f(); + contactPoint.scaleAdd(penetration, a.normal, vertex); + assert(Math.abs(a.normal.dot(contactPoint) - a.intercept) < 0.01); + collisions.add(new CollisionInfo(contactPoint, new Vector3f(a.normal), penetration)); + } + } + return collisions; + } + + private static ArrayList 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 collisions = new ArrayList(); + 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 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 collisions = new ArrayList(); + collisions.add(new CollisionInfo(contactPoint, contactNormal, penetration)); + return collisions; + } + } + return EMPTY_COLLISION_LIST; + } + + private static ArrayList calculateCollisions(Particle a, CollidableObject b) { + ArrayList collisions = new ArrayList(); + 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 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 collisions = new ArrayList(); + delta.normalize(); + Vector3f contactPoint = new Vector3f(); + contactPoint.scaleAdd(a.radius - 0.5f * penetration, delta, a.position); + collisions.add(new CollisionInfo(contactPoint, delta, penetration)); + return collisions; + } + + private static ArrayList calculateCollisions(Polygon a, CollidableObject b) { + ArrayList 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 calculateCollisions(ArrayList setA, ArrayList setB) { + ArrayList collisions = new ArrayList(); + for (int i = 0; i < setA.size(); i++) + for (int j = 0; j < setB.size(); j++) { + CollisionInfo collision = setA.get(i).getIntersection(setB.get(j)); + if (collision != null) + collisions.add(collision); + } + return collisions; + } + + private static ArrayList flipContactNormals(ArrayList collisions) { + for (CollisionInfo collision : collisions) + collision.contactNormal.negate(); + return collisions; + } + + public static ArrayList extractVertices(Node node) { + ArrayList vertices = new ArrayList(); + extractVertices(node, vertices); + return vertices; + } + + private static void extractVertices(Node node, ArrayList vertices) { + if (node instanceof Primitive) { + Primitive prim = (Primitive)node; + int index = 0; + Shape3D shape; + Transform3D L2V = new Transform3D(); + while ((shape = prim.getShape(index++)) != null) { + shape.getLocalToVworld(L2V); + for (int i = 0; i < shape.numGeometries(); i++) + extractVertices(shape.getGeometry(i), L2V, vertices); + } + } else if (node instanceof Shape3D) { + Shape3D shape = (Shape3D)node; + Transform3D L2V = new Transform3D(); + shape.getLocalToVworld(L2V); + for (int i = 0; i < shape.numGeometries(); i++) + extractVertices(shape.getGeometry(i), L2V, vertices); + } else if (node instanceof Group) { + Group group = (Group)node; + for (int i = 0; i < group.numChildren(); i++) + extractVertices(group.getChild(i), vertices); + } else + throw new IllegalArgumentException("Illegal node type for vertex extraction"); + } + + private static void extractVertices(Geometry geometry, Transform3D transform, ArrayList vertices) { + if (geometry instanceof GeometryArray) { + GeometryArray trueGeometry = (GeometryArray)geometry; + vertices.ensureCapacity(vertices.size() + trueGeometry.getValidVertexCount()); + Point3f vertex = new Point3f(); + for (int i = 0; i < trueGeometry.getValidVertexCount(); i++) { + trueGeometry.getCoordinate(i, vertex); + transform.transform(vertex); + vertices.add(new Vector3f(vertex)); + } + } else + throw new IllegalArgumentException("Illegal geometry type for vertex extraction"); + } + + public static ArrayList triangularize(Node node) { + ArrayList triangles = new ArrayList(); + triangularize(node, triangles); + return triangles; + } + + private static void triangularize(Node node, ArrayList triangles) { + if (node instanceof Primitive) { + Primitive prim = (Primitive)node; + triangles.ensureCapacity(prim.getNumTriangles()); + int index = 0; + Shape3D shape; + Transform3D L2V = new Transform3D(); + while ((shape = prim.getShape(index++)) != null) { + shape.getLocalToVworld(L2V); + for (int i = 0; i < shape.numGeometries(); i++) + triangularize(shape.getGeometry(i), L2V, triangles); + } + } else if (node instanceof Shape3D) { + Shape3D shape = (Shape3D)node; + Transform3D L2V = new Transform3D(); + shape.getLocalToVworld(L2V); + for (int i = 0; i < shape.numGeometries(); i++) + triangularize(shape.getGeometry(i), L2V, triangles); + } else if (node instanceof Group) { + Group group = (Group)node; + for (int i = 0; i < group.numChildren(); i++) + triangularize(group.getChild(i), triangles); + } else + throw new IllegalArgumentException("Illegal node type for triangularization"); + } + + private static void triangularize(Geometry geometry, Transform3D transform, ArrayList triangles) { + if (geometry instanceof TriangleArray) { + TriangleArray trueGeometry = (TriangleArray)geometry; + Point3f vertices[] = new Point3f[trueGeometry.getValidVertexCount()]; + for (int i = 0; i < vertices.length; i++) { + vertices[i] = new Point3f(); + trueGeometry.getCoordinate(i, vertices[i]); + transform.transform(vertices[i]); + } + for (int i = 0; i < vertices.length; i += 3) + try { + triangles.add(new Triangle(vertices[i], vertices[i+1], vertices[i+2])); + } catch (IllegalArgumentException e) { + } + } else if (geometry instanceof TriangleStripArray) { + TriangleStripArray trueGeometry = (TriangleStripArray)geometry; + int stripVertexCounts[] = new int[trueGeometry.getNumStrips()]; + trueGeometry.getStripVertexCounts(stripVertexCounts); + Point3f vertices[] = new Point3f[trueGeometry.getValidVertexCount()]; + for (int i = 0; i < vertices.length; i++) { + vertices[i] = new Point3f(); + trueGeometry.getCoordinate(i, vertices[i]); + transform.transform(vertices[i]); + } + int base = 0; + for (int i = 0; i < stripVertexCounts.length; i++) { + boolean reverse = false; + for (int j = 2; j < stripVertexCounts[i]; j++) { + try { + if (reverse) + triangles.add(new Triangle(vertices[base+j-2], vertices[base+j], vertices[base+j-1])); + else + triangles.add(new Triangle(vertices[base+j-2], vertices[base+j-1], vertices[base+j])); + } catch (IllegalArgumentException e) { + } + reverse = !reverse; + } + base += stripVertexCounts[i]; + } + } else if (geometry instanceof TriangleFanArray) { + TriangleFanArray trueGeometry = (TriangleFanArray)geometry; + int stripVertexCounts[] = new int[trueGeometry.getNumStrips()]; + trueGeometry.getStripVertexCounts(stripVertexCounts); + Point3f vertices[] = new Point3f[trueGeometry.getValidVertexCount()]; + for (int i = 0; i < vertices.length; i++) { + vertices[i] = new Point3f(); + trueGeometry.getCoordinate(i, vertices[i]); + transform.transform(vertices[i]); + } + int base = 0; + for (int i = 0; i < stripVertexCounts.length; i++) { + for (int j = 2; j < stripVertexCounts[i]; j++) + try { + triangles.add(new Triangle(vertices[base], vertices[base+j-1], vertices[base+j])); + } catch (IllegalArgumentException e) { + } + base += stripVertexCounts[i]; + } + } else if (geometry instanceof IndexedTriangleArray) { + IndexedTriangleArray trueGeometry = (IndexedTriangleArray)geometry; + Point3f vertices[] = new Point3f[trueGeometry.getValidVertexCount()]; + for (int i = 0; i < vertices.length; i++) { + vertices[i] = new Point3f(); + trueGeometry.getCoordinate(i, vertices[i]); + transform.transform(vertices[i]); + } + int indices[] = new int[trueGeometry.getValidIndexCount()]; + trueGeometry.getCoordinateIndices(0, indices); + for (int i = 0; i < indices.length; i += 3) + try { + triangles.add(new Triangle(vertices[indices[i]], vertices[indices[i+1]], vertices[indices[i+2]])); + } catch (IllegalArgumentException e) { + } + } else if (geometry instanceof IndexedTriangleStripArray) { + IndexedTriangleStripArray trueGeometry = (IndexedTriangleStripArray)geometry; + int stripIndexCounts[] = new int[trueGeometry.getNumStrips()]; + trueGeometry.getStripIndexCounts(stripIndexCounts); + Point3f vertices[] = new Point3f[trueGeometry.getValidVertexCount()]; + for (int i = 0; i < vertices.length; i++) { + vertices[i] = new Point3f(); + trueGeometry.getCoordinate(i, vertices[i]); + transform.transform(vertices[i]); + } + int indices[] = new int[trueGeometry.getValidIndexCount()]; + trueGeometry.getCoordinateIndices(0, indices); + int base = 0; + for (int i = 0; i < stripIndexCounts.length; i++) { + boolean reverse = false; + for (int j = 2; j < stripIndexCounts[i]; j++) { + try { + if (reverse) + triangles.add(new Triangle(vertices[indices[base+j-2]], vertices[indices[base+j]], vertices[indices[base+j-1]])); + else + triangles.add(new Triangle(vertices[indices[base+j-2]], vertices[indices[base+j-1]], vertices[indices[base+j]])); + } catch (IllegalArgumentException e) { + } + reverse = !reverse; + } + base += stripIndexCounts[i]; + } + } else if (geometry instanceof IndexedTriangleFanArray) { + IndexedTriangleFanArray trueGeometry = (IndexedTriangleFanArray)geometry; + int stripIndexCounts[] = new int[trueGeometry.getNumStrips()]; + trueGeometry.getStripIndexCounts(stripIndexCounts); + Point3f vertices[] = new Point3f[trueGeometry.getValidVertexCount()]; + for (int i = 0; i < vertices.length; i++) { + vertices[i] = new Point3f(); + trueGeometry.getCoordinate(i, vertices[i]); + transform.transform(vertices[i]); + } + int indices[] = new int[trueGeometry.getValidIndexCount()]; + trueGeometry.getCoordinateIndices(0, indices); + int base = 0; + for (int i = 0; i < stripIndexCounts.length; i++) { + for (int j = 2; j < stripIndexCounts[i]; j++) + try { + triangles.add(new Triangle(vertices[indices[base]], vertices[indices[base+j-1]], vertices[indices[base+j]])); + } catch (IllegalArgumentException e) { + } + base += stripIndexCounts[i]; + } + } else + throw new IllegalArgumentException("Illegal geometry type for triangularization"); + } + + public static Shape3D createShape(ArrayList triangles) { + TriangleArray geometry = new TriangleArray(3 * triangles.size(), TriangleArray.COORDINATES); + for (int i = 0; i < triangles.size(); i++) { + geometry.setCoordinate(3 * i, new Point3f(triangles.get(i).a)); + geometry.setCoordinate(3 * i + 1, new Point3f(triangles.get(i).b)); + geometry.setCoordinate(3 * i + 2, new Point3f(triangles.get(i).c)); + } + Appearance appearance = new Appearance(); + PolygonAttributes polyAttr = new PolygonAttributes(PolygonAttributes.POLYGON_LINE, PolygonAttributes.CULL_NONE, 0); + appearance.setPolygonAttributes(polyAttr); + return new Shape3D(geometry, appearance); + } +} 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 peers; + private ServerSocket serverSocket; + private SwingWorker 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 Peer object. The Peer is + * not connected to a network and sends a log of internal operation + * messages to the console. + */ + public Peer() { + this(true); + } + + /** + * Initializes a new Peer object with message logging + * controlled by the logEnabled parameter. The + * Peer is not connected to a network. + * + * @param logEnabled Initial value for the logEnabled field. + */ + public Peer(boolean logEnabled) { + myInfo = new PeerInformation(); + this.logEnabled = logEnabled; + } + + /** + * Establishes a new peer-to-peer network with this Peer + * as the sole member. The logical coordinates of this + * Peer are chosen randomly. + * + * @return true if the new network was established + * successfully, or false otherwise. + */ + public boolean createNetwork() { + return createNetwork(new PeerCoordinates()); + } + + /** + * Establishes a new peer-to-peer network with this Peer + * as the sole member. The logical coordinates of this + * Peer are specified by parameter location. + * + * @param location Logical coordinates for this Peer + * within the new network. + * + * @return true if the new network was established + * successfully, or false otherwise. + */ + public boolean createNetwork(PeerCoordinates location) { + if (serverSocket == null) + if (!startServer()) + return false; + peers = new ArrayList(); + myInfo.location = location; + logMessage("Established network @ " + myInfo.location); + return true; + } + + /** + * Connects this Peer to an existing peer-to-peer network. The + * port number of the known Peer must be + * {@link #DEFAULT_SERVER_PORT DEFAULT_SERVER_PORT}. The logical coordinates + * of this Peer are chosen randomly. + * + * @param host + * The domain name or IP address of a Peer within + * the network. + * + * @return true if this Peer successfully + * connected to the network, or false otherwise. + */ + public boolean connectToNetwork(String host) { + return connectToNetwork(host, DEFAULT_SERVER_PORT); + } + + /** + * Connects this Peer to an existing peer-to-peer network. The + * port number of the known Peer must be + * {@link #DEFAULT_SERVER_PORT DEFAULT_SERVER_PORT}. The preferred logical + * coordinates of this Peer are specified by parameter + * location, but the actual logical coordinates may be chosen + * randomly to avoid collision with other Peers. + * + * @param host + * The domain name or IP address of a Peer within + * the network. + * @param location + * Preferred logical coordinates for this Peer. + * + * @return true if this Peer successfully + * connected to the network, or false otherwise. + */ + public boolean connectToNetwork(String host, PeerCoordinates location) { + return connectToNetwork(host, DEFAULT_SERVER_PORT, location); + } + + /** + * Connects this Peer to an existing peer-to-peer network. The + * logical coordinates of this Peer are chosen randomly. + * + * @param host + * The domain name or IP address of a Peer within + * the network. + * @param port + * The port number of a Peer within the network. + * + * @return true if this Peer successfully + * connected to the network, or false 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 Peer to an existing peer-to-peer network. The + * preferred logical coordinates of this Peer are specified by + * parameter location, but the actual logical coordinates may + * be chosen randomly to avoid collision with other Peers. + * + * @param host + * The domain name or IP address of a Peer within + * the network. + * @param port + * The port number of a Peer within the network. + * @param location + * Preferred logical coordinates for this Peer. + * + * @return true if this Peer successfully + * connected to the network, or false 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 Peer to an existing peer-to-peer network. The + * port number of the known Peer must be + * {@link #DEFAULT_SERVER_PORT DEFAULT_SERVER_PORT}. The logical coordinates + * of this Peer are chosen randomly. + * + * @param host + * The IP address of a Peer within the network. + * + * @return true if this Peer successfully + * connected to the network, or false otherwise. + */ + public boolean connectToNetwork(InetAddress host) { + return connectToNetwork(host, DEFAULT_SERVER_PORT); + } + + /** + * Connects this Peer to an existing peer-to-peer network. The + * port number of the known Peer must be + * {@link #DEFAULT_SERVER_PORT DEFAULT_SERVER_PORT}. The preferred logical + * coordinates of this Peer are specified by parameter + * location, but the actual logical coordinates may be chosen + * randomly to avoid collision with other Peers. + * + * @param host + * The IP address of a Peer within the network. + * @param location + * Preferred logical coordinates for this Peer. + * + * @return true if this Peer successfully + * connected to the network, or false otherwise. + */ + public boolean connectToNetwork(InetAddress host, PeerCoordinates location) { + return connectToNetwork(host, DEFAULT_SERVER_PORT, location); + } + + /** + * Connects this Peer to an existing peer-to-peer network. The + * logical coordinates of this Peer are chosen randomly. + * + * @param host + * The IP address of a Peer within the network. + * @param port + * The port number of a Peer within the network. + * + * @return true if this Peer successfully + * connected to the network, or false otherwise. + */ + public boolean connectToNetwork(InetAddress host, int port) { + return connectToNetwork(host, port, new PeerCoordinates()); + } + + /** + * Connects this Peer to an existing peer-to-peer network. The + * preferred logical coordinates of this Peer are specified by + * parameter location, but the actual logical coordinates may + * be chosen randomly to avoid collision with other Peers. + * + * @param host + * The IP address of a Peer within the network. + * @param port + * The port number of a Peer within the network. + * @param location + * Preferred logical coordinates for this Peer. + * + * @return true if this Peer successfully connects + * to the network, or false 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 Peer 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 Peer in the network adjacent to this + * Peer in the direction (x,y). + * + * @param x + * X component of a direction relative to the logical coordinates + * of this Peer. + * @param y + * Y component of a direction relative to the logical coordinates + * of this Peer. + * + * @return A PeerInformation object representing the adjacent + * Peer, or null if no adjacent + * Peer 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 + * Peers in the network. + * + * @param payload + * A CollidableObject. + * + * @return true if the message was successfully sent to all + * Peers, or false 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 + * Peers in the network. + * + * @param payload + * An object. + * + * @return true if the message was successfully sent to all + * Peers, or false 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() { + 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 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(); + 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); + } + +} + + + + diff --git a/src/tesseract/TesseractServer.java b/src/tesseract/TesseractServer.java index 3518c38..6899d7f 100644 --- a/src/tesseract/TesseractServer.java +++ b/src/tesseract/TesseractServer.java @@ -4,7 +4,8 @@ import java.io.BufferedReader; import java.io.IOException; import java.io.InputStreamReader; -import alden.Peer; +import common.Peer; + /** * This class is not part of the deliverable. This class is simply a server to diff --git a/src/tesseract/TesseractUI.java b/src/tesseract/TesseractUI.java index 85f46a1..705dec6 100644 --- a/src/tesseract/TesseractUI.java +++ b/src/tesseract/TesseractUI.java @@ -44,11 +44,11 @@ import tesseract.newmenu.NewSurfBoardMenuItem; import tesseract.newmenu.NewToroidMenuItem; import tesseract.objects.PhysicalObject; import tesseract.objects.Sphere; -import alden.Peer; import com.sun.j3d.utils.picking.PickCanvas; import com.sun.j3d.utils.picking.PickResult; import com.sun.j3d.utils.universe.SimpleUniverse; +import common.Peer; /** * This class is the main UI for the Tesseract Project. diff --git a/src/tesseract/World.java b/src/tesseract/World.java index eb9d26b..9dc5f19 100644 --- a/src/tesseract/World.java +++ b/src/tesseract/World.java @@ -19,11 +19,12 @@ import javax.vecmath.Color3f; import javax.vecmath.Point3d; import javax.vecmath.Vector3f; +import common.CollidableObject; +import common.Peer; + import tesseract.forces.Force; import tesseract.objects.HalfSpace; import tesseract.objects.PhysicalObject; -import alden.CollidableObject; -import alden.Peer; /** * Model of the 3D world. diff --git a/src/tesseract/objects/PhysicalObject.java b/src/tesseract/objects/PhysicalObject.java index d4e4e3b..7b699e3 100644 --- a/src/tesseract/objects/PhysicalObject.java +++ b/src/tesseract/objects/PhysicalObject.java @@ -8,9 +8,9 @@ import javax.media.j3d.TransformGroup; import javax.vecmath.Quat4f; import javax.vecmath.Vector3f; -import alden.CollidableObject; import com.sun.j3d.utils.geometry.Primitive; +import common.CollidableObject; /** * This class is the parent of all objects in the world. diff --git a/src/tesseract/objects/Polygon.java b/src/tesseract/objects/Polygon.java index c8fcf53..7290e04 100644 --- a/src/tesseract/objects/Polygon.java +++ b/src/tesseract/objects/Polygon.java @@ -2,7 +2,8 @@ package tesseract.objects; import javax.media.j3d.Transform3D; import javax.vecmath.*; -import alden.CollidableObject; +import common.CollidableObject; + public abstract class Polygon extends PhysicalObject { public Vector3f normal; diff --git a/src/tesseract/objects/Sphere.java b/src/tesseract/objects/Sphere.java index cb93041..719ecc0 100644 --- a/src/tesseract/objects/Sphere.java +++ b/src/tesseract/objects/Sphere.java @@ -4,7 +4,8 @@ import java.awt.Color; import javax.media.j3d.*; import javax.vecmath.*; -import alden.CollidableObject; +import common.CollidableObject; + /** * Sphere. -- cgit v1.2.3