Copied JOGL_2_SANDBOX r350 on to trunk; JOGL_2_SANDBOX branch is now closed

git-svn-id: file:///usr/local/projects/SUN/JOGL/git-svn/../svn-server-sync/jogl-demos/trunk@352 3298f667-5e0e-4b4a-8ed4-a3559d26a5f4

1 files changed, 309 insertions, 0 deletions

diff --git a/src/gleem/linalg/Rotf.java b/src/gleem/linalg/Rotf.java
new file mode 100644
index 0000000..4e39c21
--- /dev/null
+++ b/src/gleem/linalg/Rotf.java
@@ -0,0 +1,309 @@
+/*
+ * gleem -- OpenGL Extremely Easy-To-Use Manipulators.
+ * Copyright (C) 1998-2003 Kenneth B. Russell ([email protected])
+ *
+ * Copying, distribution and use of this software in source and binary
+ * forms, with or without modification, is permitted provided that the
+ * following conditions are met:
+ *
+ * Distributions of source code must reproduce the copyright notice,
+ * this list of conditions and the following disclaimer in the source
+ * code header files; and Distributions of binary code must reproduce
+ * the copyright notice, this list of conditions and the following
+ * disclaimer in the documentation, Read me file, license file and/or
+ * other materials provided with the software distribution.
+ *
+ * The names of Sun Microsystems, Inc. ("Sun") and/or the copyright
+ * holder may not be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED "AS IS," WITHOUT A WARRANTY OF ANY
+ * KIND. ALL EXPRESS OR IMPLIED CONDITIONS, REPRESENTATIONS AND
+ * WARRANTIES, INCLUDING ANY IMPLIED WARRANTY OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE, NON-INTERFERENCE, ACCURACY OF
+ * INFORMATIONAL CONTENT OR NON-INFRINGEMENT, ARE HEREBY EXCLUDED. THE
+ * COPYRIGHT HOLDER, SUN AND SUN'S LICENSORS SHALL NOT BE LIABLE FOR
+ * ANY DAMAGES SUFFERED BY LICENSEE AS A RESULT OF USING, MODIFYING OR
+ * DISTRIBUTING THIS SOFTWARE OR ITS DERIVATIVES. IN NO EVENT WILL THE
+ * COPYRIGHT HOLDER, SUN OR SUN'S LICENSORS BE LIABLE FOR ANY LOST
+ * REVENUE, PROFIT OR DATA, OR FOR DIRECT, INDIRECT, SPECIAL,
+ * CONSEQUENTIAL, INCIDENTAL OR PUNITIVE DAMAGES, HOWEVER CAUSED AND
+ * REGARDLESS OF THE THEORY OF LIABILITY, ARISING OUT OF THE USE OF OR
+ * INABILITY TO USE THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY
+ * OF SUCH DAMAGES. YOU ACKNOWLEDGE THAT THIS SOFTWARE IS NOT
+ * DESIGNED, LICENSED OR INTENDED FOR USE IN THE DESIGN, CONSTRUCTION,
+ * OPERATION OR MAINTENANCE OF ANY NUCLEAR FACILITY. THE COPYRIGHT
+ * HOLDER, SUN AND SUN'S LICENSORS DISCLAIM ANY EXPRESS OR IMPLIED
+ * WARRANTY OF FITNESS FOR SUCH USES.
+ */
+
+package gleem.linalg;
+
+/** Represents a rotation with single-precision components */
+
+public class Rotf {
+  private static float EPSILON = 1.0e-7f;
+
+  // Representation is a quaternion. Element 0 is the scalar part (=
+  // cos(theta/2)), elements 1..3 the imaginary/"vector" part (=
+  // sin(theta/2) * axis).
+  private float q0;
+  private float q1;
+  private float q2;
+  private float q3;
+
+  /** Default constructor initializes to the identity quaternion */
+  public Rotf() {
+    init();
+  }
+
+  public Rotf(Rotf arg) {
+    set(arg);
+  }
+
+  /** Axis does not need to be normalized but must not be the zero
+      vector. Angle is in radians. */
+  public Rotf(Vec3f axis, float angle) {
+    set(axis, angle);
+  }
+
+  /** Creates a rotation which will rotate vector "from" into vector
+      "to". */
+  public Rotf(Vec3f from, Vec3f to) {
+    set(from, to);
+  }
+
+  /** Re-initialize this quaternion to be the identity quaternion "e"
+      (i.e., no rotation) */
+  public void init() {
+    q0 = 1;
+    q1 = q2 = q3 = 0;
+  }
+
+  /** Test for "approximate equality" -- performs componentwise test
+      to see whether difference between all components is less than
+      epsilon. */
+  public boolean withinEpsilon(Rotf arg, float epsilon) {
+    return ((Math.abs(q0 - arg.q0) < epsilon) &&
+            (Math.abs(q1 - arg.q1) < epsilon) &&
+            (Math.abs(q2 - arg.q2) < epsilon) &&
+            (Math.abs(q3 - arg.q3) < epsilon));
+  }
+
+  /** Axis does not need to be normalized but must not be the zero
+      vector. Angle is in radians. */
+  public void set(Vec3f axis, float angle) {
+    float halfTheta = angle / 2.0f;
+    q0 = (float) Math.cos(halfTheta);
+    float sinHalfTheta = (float) Math.sin(halfTheta);
+    Vec3f realAxis = new Vec3f(axis);
+    realAxis.normalize();
+    q1 = realAxis.x() * sinHalfTheta;
+    q2 = realAxis.y() * sinHalfTheta;
+    q3 = realAxis.z() * sinHalfTheta;
+  }
+
+  public void set(Rotf arg) {
+    q0 = arg.q0;
+    q1 = arg.q1;
+    q2 = arg.q2;
+    q3 = arg.q3;
+  }
+
+  /** Sets this rotation to that which will rotate vector "from" into
+      vector "to". from and to do not have to be the same length. */
+  public void set(Vec3f from, Vec3f to) {
+    Vec3f axis = from.cross(to);
+    if (axis.lengthSquared() < EPSILON) {
+      init();
+      return;
+    }
+    float dotp = from.dot(to);
+    float denom = from.length() * to.length();
+    if (denom < EPSILON) {
+      init();
+      return;
+    }
+    dotp /= denom;
+    set(axis, (float) Math.acos(dotp));
+  }
+
+  /** Returns angle (in radians) and mutates the given vector to be
+      the axis. */
+  public float get(Vec3f axis) {
+    // FIXME: Is this numerically stable? Is there a better way to
+    // extract the angle from a quaternion?
+    // NOTE: remove (float) to illustrate compiler bug
+    float retval = (float) (2.0f * Math.acos(q0));
+    axis.set(q1, q2, q3);
+    float len = axis.length();
+    if (len == 0.0f) {
+      axis.set(0, 0, 1);
+    } else {
+      axis.scale(1.0f / len);
+    }
+    return retval;
+  }
+
+  /** Returns inverse of this rotation; creates new rotation */
+  public Rotf inverse() {
+    Rotf tmp = new Rotf(this);
+    tmp.invert();
+    return tmp;
+  }
+
+  /** Mutate this quaternion to be its inverse. This is equivalent to
+      the conjugate of the quaternion. */
+  public void invert() {
+    q1 = -q1;
+    q2 = -q2;
+    q3 = -q3;
+  }
+  
+  /** Length of this quaternion in four-space */
+  public float length() {
+    return (float) Math.sqrt(lengthSquared());
+  }
+
+  /** This dotted with this */
+  public float lengthSquared() {
+    return (q0 * q0 +
+            q1 * q1 +
+            q2 * q2 +
+            q3 * q3);
+  }
+
+  /** Make this quaternion a unit quaternion again. If you are
+      composing dozens of quaternions you probably should call this
+      periodically to ensure that you have a valid rotation. */
+  public void normalize() {
+    float len = length();
+    q0 /= len;
+    q1 /= len;
+    q2 /= len;
+    q3 /= len;
+  }
+
+  /** Returns this * b, in that order; creates new rotation */
+  public Rotf times(Rotf b) {
+    Rotf tmp = new Rotf();
+    tmp.mul(this, b);
+    return tmp;
+  }
+
+  /** Compose two rotations: this = A * B in that order. NOTE that
+      because we assume a column vector representation that this
+      implies that a vector rotated by the cumulative rotation will be
+      rotated first by B, then A. NOTE: "this" must be different than
+      both a and b. */
+  public void mul(Rotf a, Rotf b) {
+    q0 = (a.q0 * b.q0 - a.q1 * b.q1 -
+          a.q2 * b.q2 - a.q3 * b.q3);
+    q1 = (a.q0 * b.q1 + a.q1 * b.q0 +
+          a.q2 * b.q3 - a.q3 * b.q2);
+    q2 = (a.q0 * b.q2 + a.q2 * b.q0 -
+          a.q1 * b.q3 + a.q3 * b.q1);
+    q3 = (a.q0 * b.q3 + a.q3 * b.q0 +
+          a.q1 * b.q2 - a.q2 * b.q1);
+  }
+
+  /** Turns this rotation into a 3x3 rotation matrix. NOTE: only
+      mutates the upper-left 3x3 of the passed Mat4f. Implementation
+      from B. K. P. Horn's <u>Robot Vision</u> textbook. */
+  public void toMatrix(Mat4f mat) {
+    float q00 = q0 * q0;
+    float q11 = q1 * q1;
+    float q22 = q2 * q2;
+    float q33 = q3 * q3;
+    // Diagonal elements
+    mat.set(0, 0, q00 + q11 - q22 - q33);
+    mat.set(1, 1, q00 - q11 + q22 - q33);
+    mat.set(2, 2, q00 - q11 - q22 + q33);
+    // 0,1 and 1,0 elements
+    float q03 = q0 * q3;
+    float q12 = q1 * q2;
+    mat.set(0, 1, 2.0f * (q12 - q03));
+    mat.set(1, 0, 2.0f * (q03 + q12));
+    // 0,2 and 2,0 elements
+    float q02 = q0 * q2;
+    float q13 = q1 * q3;
+    mat.set(0, 2, 2.0f * (q02 + q13));
+    mat.set(2, 0, 2.0f * (q13 - q02));
+    // 1,2 and 2,1 elements
+    float q01 = q0 * q1;
+    float q23 = q2 * q3;
+    mat.set(1, 2, 2.0f * (q23 - q01));
+    mat.set(2, 1, 2.0f * (q01 + q23));
+  }
+
+  /** Turns the upper left 3x3 of the passed matrix into a rotation.
+      Implementation from Watt and Watt, <u>Advanced Animation and
+      Rendering Techniques</u>.
+      @see gleem.linalg.Mat4f#getRotation */
+  public void fromMatrix(Mat4f mat) {
+    // FIXME: Should reimplement to follow Horn's advice of using
+    // eigenvector decomposition to handle roundoff error in given
+    // matrix.
+    
+    float tr, s;
+    int i, j, k;
+  
+    tr = mat.get(0, 0) + mat.get(1, 1) + mat.get(2, 2);
+    if (tr > 0.0) {
+      s = (float) Math.sqrt(tr + 1.0f);
+      q0 = s * 0.5f;
+      s = 0.5f / s;
+      q1 = (mat.get(2, 1) - mat.get(1, 2)) * s;
+      q2 = (mat.get(0, 2) - mat.get(2, 0)) * s;
+      q3 = (mat.get(1, 0) - mat.get(0, 1)) * s;
+    } else {
+      i = 0;
+      if (mat.get(1, 1) > mat.get(0, 0))
+        i = 1;
+      if (mat.get(2, 2) > mat.get(i, i))
+        i = 2;
+      j = (i+1)%3;
+      k = (j+1)%3;
+      s = (float) Math.sqrt( (mat.get(i, i) - (mat.get(j, j) + mat.get(k, k))) + 1.0f);
+      setQ(i+1, s * 0.5f);
+      s = 0.5f / s;
+      q0 = (mat.get(k, j) - mat.get(j, k)) * s;
+      setQ(j+1, (mat.get(j, i) + mat.get(i, j)) * s);
+      setQ(k+1, (mat.get(k, i) + mat.get(i, k)) * s);
+    }
+  }
+
+  /** Rotate a vector by this quaternion. Implementation is from
+      Horn's <u>Robot Vision</u>. NOTE: src and dest must be different
+      vectors. */
+  public void rotateVector(Vec3f src, Vec3f dest) {
+    Vec3f qVec = new Vec3f(q1, q2, q3);
+    Vec3f qCrossX = qVec.cross(src);
+    Vec3f qCrossXCrossQ = qCrossX.cross(qVec);
+    qCrossX.scale(2.0f * q0);
+    qCrossXCrossQ.scale(-2.0f);
+    dest.add(src, qCrossX);
+    dest.add(dest, qCrossXCrossQ);
+  }
+
+  /** Rotate a vector by this quaternion, returning newly-allocated result. */
+  public Vec3f rotateVector(Vec3f src) {
+    Vec3f tmp = new Vec3f();
+    rotateVector(src, tmp);
+    return tmp;
+  }
+
+  public String toString() {
+    return "(" + q0 + ", " + q1 + ", " + q2 + ", " + q3 + ")";
+  }
+
+  private void setQ(int i, float val) {
+    switch (i) {
+    case 0: q0 = val; break;
+    case 1: q1 = val; break;
+    case 2: q2 = val; break;
+    case 3: q3 = val; break;
+    default: throw new IndexOutOfBoundsException();
+    }
+  }
+}