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