Initial creation of vecmath sources in CVS repository

git-svn-id: https://svn.java.net/svn/vecmath~svn/trunk@5 dd45e54d-f42e-c781-df72-dca083a658b1

1 files changed, 662 insertions, 0 deletions

diff --git a/src/javax/vecmath/Quat4d.java b/src/javax/vecmath/Quat4d.java
new file mode 100644
index 0000000..c6232f6
--- /dev/null
+++ b/src/javax/vecmath/Quat4d.java
@@ -0,0 +1,662 @@
+/*
+ * $RCSfile$
+ *
+ * Copyright (c) 2004 Sun Microsystems, Inc. All rights reserved.
+ *
+ * Use is subject to license terms.
+ *
+ * $Revision$
+ * $Date$
+ * $State$
+ */
+
+package javax.vecmath;
+
+import java.lang.Math;
+
+/**
+ * A 4-element quaternion represented by double precision floating 
+ * point x,y,z,w coordinates.  The quaternion is always normalized.
+ *
+ */
+public class Quat4d extends Tuple4d implements java.io.Serializable {
+
+  // Combatible with 1.1
+  static final long serialVersionUID = 7577479888820201099L;
+
+  final static double EPS = 0.000001;
+  final static double EPS2 = 1.0e-30;
+  final static double PIO2 = 1.57079632679;
+
+  /**
+   * Constructs and initializes a Quat4d 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 Quat4d(double x, double y, double z, double w)
+  {
+      double mag;
+      mag = 1.0/Math.sqrt( x*x + y*y + z*z + w*w );
+      this.x =  x*mag;
+      this.y =  y*mag;
+      this.z =  z*mag;
+      this.w =  w*mag;
+
+  }
+
+  /**
+   * Constructs and initializes a Quat4d from the array of length 4. 
+   * @param q the array of length 4 containing xyzw in order
+   */
+  public Quat4d(double[] q)
+  {
+      double mag; 
+      mag = 1.0/Math.sqrt( q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3] );
+      x =  q[0]*mag;
+      y =  q[1]*mag;
+      z =  q[2]*mag;
+      w =  q[3]*mag;
+
+  }
+
+  /**
+   * Constructs and initializes a Quat4d from the specified Quat4d.
+   * @param q1 the Quat4d containing the initialization x y z w data
+   */
+  public Quat4d(Quat4d q1)
+  {
+       super(q1);
+  }
+
+  /**
+   * Constructs and initializes a Quat4d from the specified Quat4f.
+   * @param q1 the Quat4f containing the initialization x y z w data
+   */
+  public Quat4d(Quat4f q1)
+  {
+     super(q1);
+  }
+
+
+    /** 
+     * Constructs and initializes a Quat4d from the specified Tuple4f. 
+     * @param t1 the Tuple4f containing the initialization x y z w data 
+     */  
+    public Quat4d(Tuple4f t1)  
+    { 
+      double mag;
+      mag = 1.0/Math.sqrt( t1.x*t1.x + t1.y*t1.y + t1.z*t1.z + t1.w*t1.w );
+      x =  t1.x*mag;
+      y =  t1.y*mag;
+      z =  t1.z*mag;
+      w =  t1.w*mag;
+
+    }
+ 
+ 
+    /** 
+     * Constructs and initializes a Quat4d from the specified Tuple4d.  
+     * @param t1 the Tuple4d containing the initialization x y z w data 
+     */  
+    public Quat4d(Tuple4d t1)
+    {
+      double mag;
+      mag = 1.0/Math.sqrt( t1.x*t1.x + t1.y*t1.y + t1.z*t1.z + t1.w*t1.w );
+      x =  t1.x*mag;
+      y =  t1.y*mag;
+      z =  t1.z*mag;
+      w =  t1.w*mag;
+    }
+
+
+  /**
+   * Constructs and initializes a Quat4d to (0,0,0,0).
+   */
+  public Quat4d()
+  {
+     super();
+  }
+
+
+  /**
+   * Sets the value of this quaternion to the conjugate of quaternion q1.
+   * @param q1 the source vector
+   */
+  public final void conjugate(Quat4d q1)
+  {
+    this.x = -q1.x;
+    this.y = -q1.y;
+    this.z = -q1.z;
+    this.w = q1.w;
+  }
+
+
+  /**
+   * Negate the value of of each of this quaternion's x,y,z coordinates 
+   *  in place.
+   */
+  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(Quat4d q1, Quat4d 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 {
+      double	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(Quat4d q1)
+  {
+      double     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(Quat4d q1, Quat4d q2) 
+  {   
+      Quat4d  tempQuat = new Quat4d(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(Quat4d q1)
+  {  
+      Quat4d  tempQuat = new Quat4d(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(Quat4d q1)
+  {
+    double norm;
+
+    norm = 1.0/(q1.w*q1.w + q1.x*q1.x + q1.y*q1.y + q1.z*q1.z);
+    this.w =  norm*q1.w;
+    this.x = -norm*q1.x;
+    this.y = -norm*q1.y;
+    this.z = -norm*q1.z;
+  }
+
+
+  /**
+   * Sets the value of this quaternion to the quaternion inverse of itself.
+   */
+  public final void inverse()
+  {
+    double norm;  
+ 
+    norm = 1.0/(this.w*this.w + this.x*this.x + this.y*this.y + this.z*this.z);
+    this.w *=  norm;
+    this.x *= -norm;
+    this.y *= -norm;
+    this.z *= -norm;
+  }
+
+
+  /**
+   * Sets the value of this quaternion to the normalized value
+   * of quaternion q1.
+   * @param q1 the quaternion to be normalized.
+   */
+  public final void normalize(Quat4d q1)
+  {
+    double norm;
+
+    norm = (q1.x*q1.x + q1.y*q1.y + q1.z*q1.z + q1.w*q1.w);
+
+    if (norm > 0.0) {
+      norm = 1.0/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 =  0.0;
+      this.y =  0.0;
+      this.z =  0.0;
+      this.w =  0.0;
+    }
+  }
+
+
+  /**
+   * Normalizes the value of this quaternion in place.
+   */
+  public final void normalize()
+  {
+    double norm;
+
+    norm = (this.x*this.x + this.y*this.y + this.z*this.z + this.w*this.w);
+
+    if (norm > 0.0) {
+      norm = 1.0 / Math.sqrt(norm);
+      this.x *= norm;
+      this.y *= norm;
+      this.z *= norm;
+      this.w *= norm;
+    } else {
+      this.x =  0.0;
+      this.y =  0.0;
+      this.z =  0.0;
+      this.w =  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) 
+    {
+	double ww = 0.25*(m1.m00 + m1.m11 + m1.m22 + m1.m33);
+
+	if (ww >= 0) {
+	    if (ww >= EPS2) {
+		this.w = Math.sqrt(ww);
+		ww =  0.25/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.5*(m1.m11 + m1.m22);
+	if (ww >= 0) {
+	    if (ww >= EPS2) {
+		this.x =  Math.sqrt(ww);
+		ww = 1.0/(2.0*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.5*(1.0 - m1.m22);
+	if (ww >= EPS2) {
+	    this.y = Math.sqrt(ww);
+	    this.z = (m1.m21)/(2.0*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 = Math.sqrt(ww);
+		ww = 0.25/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.5*(m1.m11 + m1.m22);
+	if (ww >= 0) {
+	    if (ww >= EPS2){
+		this.x =  Math.sqrt(ww);
+		ww = 0.5/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.0;
+	ww = 0.5*(1.0 - m1.m22);
+	if (ww >= EPS2) {
+	    this.y =  Math.sqrt(ww);
+	    this.z = m1.m21/(2.0*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)
+    {
+	double ww = 0.25*(m1.m00 + m1.m11 + m1.m22 + 1.0);
+
+	if (ww >= 0) {
+	    if (ww >= EPS2) {
+		this.w = Math.sqrt(ww);
+		ww = 0.25/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.5*(m1.m11 + m1.m22);
+	if (ww >= 0) {
+	    if (ww >= EPS2) {
+		this.x = Math.sqrt(ww);
+		ww = 0.5/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.5*(1.0 - m1.m22);
+	if (ww >= EPS2) {
+	    this.y = Math.sqrt(ww);
+	    this.z = (m1.m21/(2.0*this.y));
+	}
+     
+	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.0);
+
+	if (ww >= 0) {
+	    if (ww >= EPS2) {
+		this.w = Math.sqrt(ww);
+		ww = 0.25/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.5*(m1.m11 + m1.m22);
+	if (ww >= 0) {
+	    if (ww >= EPS2) {
+		this.x =  Math.sqrt(ww);
+		ww = 0.5/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.5*(1.0 - m1.m22);
+	if (ww >= EPS2) {
+	    this.y =  Math.sqrt(ww);
+	    this.z = m1.m21/(2.0*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)
+    {
+	double mag,amag;
+	// Quat = cos(theta/2) + sin(theta/2)(roation_axis) 
+
+	amag = Math.sqrt( a.x*a.x + a.y*a.y + a.z*a.z);
+	if( amag < EPS ) {
+	    w = 0.0;
+	    x = 0.0;
+	    y = 0.0;
+	    z = 0.0;
+	} else {
+	    mag = Math.sin(a.angle/2.0);
+	    amag = 1.0/amag;
+	    w = 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)
+    {
+	double mag,amag;
+	// Quat = cos(theta/2) + sin(theta/2)(roation_axis) 
+	
+	amag = Math.sqrt( a.x*a.x + a.y*a.y + a.z*a.z);
+	if( amag < EPS ) {
+	    w = 0.0;
+	    x = 0.0;
+	    y = 0.0;
+	    z = 0.0;
+	} else {  
+	    amag = 1.0/amag; 
+	    mag = Math.sin(a.angle/2.0);
+	    w = Math.cos(a.angle/2.0);
+       x = a.x*amag*mag;
+       y = a.y*amag*mag;
+       z = 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(Quat4d q1, double 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 ) {
+        // switch the quaterion values
+        q1.x = -q1.x;  q1.y = -q1.y;  q1.z = -q1.z;  q1.w = -q1.w;
+      }
+
+      if ( (1.0 - Math.abs(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 = s1*w + s2*q1.w;
+      x = s1*x + s2*q1.x;
+      y = s1*y + s2*q1.y;
+      z = 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(Quat4d q1, Quat4d q2, double 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 ) {
+        // switch the quaterion values
+        q1.x = -q1.x;  q1.y = -q1.y;  q1.z = -q1.z;  q1.w = -q1.w;
+      }
+
+      if ( (1.0 - Math.abs(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 = s1*q1.w + s2*q2.w;
+      x = s1*q1.x + s2*q2.x;
+      y = s1*q1.y + s2*q2.y;
+      z = s1*q1.z + s2*q2.z;
+    }
+
+}