aboutsummaryrefslogtreecommitdiffstats
path: root/src/jogl/classes/com/jogamp/opengl
diff options
context:
space:
mode:
Diffstat (limited to 'src/jogl/classes/com/jogamp/opengl')
-rw-r--r--src/jogl/classes/com/jogamp/opengl/math/Quaternion.java443
1 files changed, 239 insertions, 204 deletions
diff --git a/src/jogl/classes/com/jogamp/opengl/math/Quaternion.java b/src/jogl/classes/com/jogamp/opengl/math/Quaternion.java
index 0cc5f5ae7..409176101 100644
--- a/src/jogl/classes/com/jogamp/opengl/math/Quaternion.java
+++ b/src/jogl/classes/com/jogamp/opengl/math/Quaternion.java
@@ -27,191 +27,204 @@
*/
package com.jogamp.opengl.math;
-
-
public class Quaternion {
- protected float x,y,z,w;
-
- public Quaternion(){
+ protected float x, y, z, w;
+ public Quaternion() {
+ setIdentity();
}
+ public Quaternion(Quaternion q) {
+ x = q.x;
+ y = q.y;
+ z = q.z;
+ w = q.w;
+ }
+
public Quaternion(float x, float y, float z, float w) {
this.x = x;
this.y = y;
this.z = z;
this.w = w;
}
-
- /** Constructor to create a rotation based quaternion from two vectors
+
+ /**
+ * Constructor to create a rotation based quaternion from two vectors
+ *
* @param vector1
* @param vector2
*/
- public Quaternion(float[] vector1, float[] vector2)
- {
- float theta = (float)FloatUtil.acos(dot(vector1, vector2));
- float[] cross = cross(vector1,vector2);
- cross = normalizeVec(cross);
-
- this.x = (float)FloatUtil.sin(theta/2)*cross[0];
- this.y = (float)FloatUtil.sin(theta/2)*cross[1];
- this.z = (float)FloatUtil.sin(theta/2)*cross[2];
- this.w = (float)FloatUtil.cos(theta/2);
- this.normalize();
+ public Quaternion(float[] vector1, float[] vector2) {
+ float theta = FloatUtil.acos(VectorUtil.dot(vector1, vector2));
+ float[] cross = VectorUtil.cross(vector1, vector2);
+ fromAxis(cross, theta);
+ }
+
+ /***
+ * Constructor to create a rotation based quaternion from axis vector and angle
+ * @param vector axis vector
+ * @param angle rotation angle (rads)
+ * @see #fromAxis(float[], float)
+ */
+ public Quaternion(float[] vector, float angle) {
+ fromAxis(vector, angle);
}
- /** Transform the rotational quaternion to axis based rotation angles
+ /***
+ * Initialize this quaternion with given axis vector and rotation angle
+ *
+ * @param vector axis vector
+ * @param angle rotation angle (rads)
+ */
+ public void fromAxis(float[] vector, float angle) {
+ float halfangle = angle * 0.5f;
+ float sin = FloatUtil.sin(halfangle);
+ float[] nv = VectorUtil.normalize(vector);
+ x = (nv[0] * sin);
+ y = (nv[1] * sin);
+ z = (nv[2] * sin);
+ w = FloatUtil.cos(halfangle);
+ }
+
+ /**
+ * Transform the rotational quaternion to axis based rotation angles
+ *
* @return new float[4] with ,theta,Rx,Ry,Rz
*/
- public float[] toAxis()
- {
+ public float[] toAxis() {
float[] vec = new float[4];
- float scale = (float)FloatUtil.sqrt(x * x + y * y + z * z);
- vec[0] =(float) FloatUtil.acos(w) * 2.0f;
+ float scale = FloatUtil.sqrt(x * x + y * y + z * z);
+ vec[0] = FloatUtil.acos(w) * 2.0f;
vec[1] = x / scale;
vec[2] = y / scale;
vec[3] = z / scale;
return vec;
}
-
- /** Normalize a vector
- * @param vector input vector
- * @return normalized vector
- */
- private float[] normalizeVec(float[] vector)
- {
- float[] newVector = new float[3];
-
- float d = FloatUtil.sqrt(vector[0]*vector[0] + vector[1]*vector[1] + vector[2]*vector[2]);
- if(d> 0.0f)
- {
- newVector[0] = vector[0]/d;
- newVector[1] = vector[1]/d;
- newVector[2] = vector[2]/d;
- }
- return newVector;
- }
- /** compute the dot product of two points
- * @param vec1 vector 1
- * @param vec2 vector 2
- * @return the dot product as float
- */
- private float dot(float[] vec1, float[] vec2)
- {
- return (vec1[0]*vec2[0] + vec1[1]*vec2[1] + vec1[2]*vec2[2]);
- }
- /** cross product vec1 x vec2
- * @param vec1 vector 1
- * @param vec2 vecttor 2
- * @return the resulting vector
- */
- private float[] cross(float[] vec1, float[] vec2)
- {
- float[] out = new float[3];
-
- out[0] = vec2[2]*vec1[1] - vec2[1]*vec1[2];
- out[1] = vec2[0]*vec1[2] - vec2[2]*vec1[0];
- out[2] = vec2[1]*vec1[0] - vec2[0]*vec1[1];
- return out;
- }
public float getW() {
return w;
}
+
public void setW(float w) {
this.w = w;
}
+
public float getX() {
return x;
}
+
public void setX(float x) {
this.x = x;
}
+
public float getY() {
return y;
}
+
public void setY(float y) {
this.y = y;
}
+
public float getZ() {
return z;
}
+
public void setZ(float z) {
this.z = z;
}
- /** Add a quaternion
+ /**
+ * Add a quaternion
+ *
* @param q quaternion
*/
- public void add(Quaternion q)
- {
- x+=q.x;
- y+=q.y;
- z+=q.z;
+ public void add(Quaternion q) {
+ x += q.x;
+ y += q.y;
+ z += q.z;
}
-
- /** Subtract a quaternion
+
+ /**
+ * Subtract a quaternion
+ *
* @param q quaternion
*/
- public void subtract(Quaternion q)
- {
- x-=q.x;
- y-=q.y;
- z-=q.z;
+ public void subtract(Quaternion q) {
+ x -= q.x;
+ y -= q.y;
+ z -= q.z;
}
-
- /** Divide a quaternion by a constant
+
+ /**
+ * Divide a quaternion by a constant
+ *
* @param n a float to divide by
*/
- public void divide(float n)
- {
- x/=n;
- y/=n;
- z/=n;
+ public void divide(float n) {
+ x /= n;
+ y /= n;
+ z /= n;
}
-
- /** Multiply this quaternion by
- * the param quaternion
+
+ /**
+ * Multiply this quaternion by the param quaternion
+ *
* @param q a quaternion to multiply with
*/
- public void mult(Quaternion q)
- {
- float w1 = w*q.w - x*q.x - y*q.y - z*q.z;
+ public void mult(Quaternion q) {
+ float w1 = w * q.w - x * q.x - y * q.y - z * q.z;
- float x1 = w*q.x + x*q.w + y*q.z - z*q.y;
- float y1 = w*q.y - x*q.z + y*q.w + z*q.x;
- float z1 = w*q.z + x*q.y - y*q.x + z*q.w;
+ float x1 = w * q.x + x * q.w + y * q.z - z * q.y;
+ float y1 = w * q.y - x * q.z + y * q.w + z * q.x;
+ float z1 = w * q.z + x * q.y - y * q.x + z * q.w;
w = w1;
x = x1;
y = y1;
- z = z1;
+ z = z1;
}
-
- /** Multiply a quaternion by a constant
+
+ /**
+ * Multiply a quaternion by a constant
+ *
* @param n a float constant
*/
- public void mult(float n)
- {
- x*=n;
- y*=n;
- z*=n;
+ public void mult(float n) {
+ x *= n;
+ y *= n;
+ z *= n;
}
- /** Normalize a quaternion required if
- * to be used as a rotational quaternion
+ /***
+ * Rotate given vector by this quaternion
+ *
+ * @param vector input vector
+ * @return rotated vector
*/
- public void normalize()
- {
- float norme = (float)FloatUtil.sqrt(w*w + x*x + y*y + z*z);
- if (norme == 0.0f)
- {
- w = 1.0f;
- x = y = z = 0.0f;
- }
- else
- {
- float recip = 1.0f/norme;
+ public float[] mult(float[] vector) {
+ // TODO : optimalize
+ float[] res = new float[3];
+ Quaternion a = new Quaternion(vector[0], vector[1], vector[2], 0.0f);
+ Quaternion b = new Quaternion(this);
+ Quaternion c = new Quaternion(this);
+ b.inverse();
+ a.mult(b);
+ c.mult(a);
+ res[0] = c.x;
+ res[1] = c.y;
+ res[2] = c.z;
+ return res;
+ }
+
+ /**
+ * Normalize a quaternion required if to be used as a rotational quaternion
+ */
+ public void normalize() {
+ float norme = (float) FloatUtil.sqrt(w * w + x * x + y * y + z * z);
+ if (norme == 0.0f) {
+ setIdentity();
+ } else {
+ float recip = 1.0f / norme;
w *= recip;
x *= recip;
@@ -219,42 +232,42 @@ public class Quaternion {
z *= recip;
}
}
-
- /** Invert the quaternion If rotational,
- * will produce a the inverse rotation
+
+ /**
+ * Invert the quaternion If rotational, will produce a the inverse rotation
*/
- public void inverse()
- {
- float norm = w*w + x*x + y*y + z*z;
+ public void inverse() {
+ float norm = w * w + x * x + y * y + z * z;
- float recip = 1.0f/norm;
+ float recip = 1.0f / norm;
w *= recip;
- x = -1*x*recip;
- y = -1*y*recip;
- z = -1*z*recip;
+ x = -1 * x * recip;
+ y = -1 * y * recip;
+ z = -1 * z * recip;
}
-
- /** Transform this quaternion to a
- * 4x4 column matrix representing the rotation
- * @return new float[16] column matrix 4x4
+
+ /**
+ * Transform this quaternion to a 4x4 column matrix representing the
+ * rotation
+ *
+ * @return new float[16] column matrix 4x4
*/
- public float[] toMatrix()
- {
+ public float[] toMatrix() {
float[] matrix = new float[16];
- matrix[0] = 1.0f - 2*y*y - 2*z*z;
- matrix[1] = 2*x*y + 2*w*z;
- matrix[2] = 2*x*z - 2*w*y;
+ matrix[0] = 1.0f - 2 * y * y - 2 * z * z;
+ matrix[1] = 2 * x * y + 2 * w * z;
+ matrix[2] = 2 * x * z - 2 * w * y;
matrix[3] = 0;
- matrix[4] = 2*x*y - 2*w*z;
- matrix[5] = 1.0f - 2*x*x - 2*z*z;
- matrix[6] = 2*y*z + 2*w*x;
+ matrix[4] = 2 * x * y - 2 * w * z;
+ matrix[5] = 1.0f - 2 * x * x - 2 * z * z;
+ matrix[6] = 2 * y * z + 2 * w * x;
matrix[7] = 0;
- matrix[8] = 2*x*z + 2*w*y;
- matrix[9] = 2*y*z - 2*w*x;
- matrix[10] = 1.0f - 2*x*x - 2*y*y;
+ matrix[8] = 2 * x * z + 2 * w * y;
+ matrix[9] = 2 * y * z - 2 * w * x;
+ matrix[10] = 1.0f - 2 * x * x - 2 * y * y;
matrix[11] = 0;
matrix[12] = 0;
@@ -263,15 +276,18 @@ public class Quaternion {
matrix[15] = 1;
return matrix;
}
-
- /** Set this quaternion from a Sphereical interpolation
- * of two param quaternion, used mostly for rotational animation.
- * <p>
- * Note: Method does not normalize this quaternion!
- * </p>
- * <p>
- * See http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/
- * </p>
+
+ /**
+ * Set this quaternion from a Sphereical interpolation of two param
+ * quaternion, used mostly for rotational animation.
+ * <p>
+ * Note: Method does not normalize this quaternion!
+ * </p>
+ * <p>
+ * See http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/
+ * quaternions/slerp/
+ * </p>
+ *
* @param a initial quaternion
* @param b target quaternion
* @param t float between 0 and 1 representing interp.
@@ -279,7 +295,7 @@ public class Quaternion {
public void slerp(Quaternion a, Quaternion b, float t) {
final float cosom = a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
final float t1 = 1.0f - t;
-
+
// if the two quaternions are close, just use linear interpolation
if (cosom >= 0.95f) {
x = a.x * t1 + b.x * t;
@@ -288,8 +304,9 @@ public class Quaternion {
w = a.w * t1 + b.w * t;
return;
}
-
- // the quaternions are nearly opposite, we can pick any axis normal to a,b
+
+ // the quaternions are nearly opposite, we can pick any axis normal to
+ // a,b
// to do the rotation
if (cosom <= -0.99f) {
x = 0.5f * (a.x + b.x);
@@ -298,94 +315,112 @@ public class Quaternion {
w = 0.5f * (a.w + b.w);
return;
}
-
+
// cosom is now withion range of acos, do a SLERP
final float sinom = FloatUtil.sqrt(1.0f - cosom * cosom);
final float omega = FloatUtil.acos(cosom);
-
+
final float scla = FloatUtil.sin(t1 * omega) / sinom;
- final float sclb = FloatUtil.sin( t * omega) / sinom;
-
+ final float sclb = FloatUtil.sin(t * omega) / sinom;
+
x = a.x * scla + b.x * sclb;
y = a.y * scla + b.y * sclb;
z = a.z * scla + b.z * sclb;
w = a.w * scla + b.w * sclb;
}
-
- /** Check if this quaternion is empty, ie (0,0,0,1)
+
+ /**
+ * Check if this quaternion is empty, ie (0,0,0,1)
+ *
* @return true if empty, false otherwise
+ * @deprecated use {@link #isIdentity()} instead
*/
- public boolean isEmpty()
- {
- if (w==1 && x==0 && y==0 && z==0)
+ @Deprecated
+ public boolean isEmpty() {
+ if (w == 1 && x == 0 && y == 0 && z == 0)
return true;
return false;
}
-
- /** Check if this quaternion represents an identity
- * matrix, for rotation.
+
+ /**
+ * Check if this quaternion represents an identity matrix, for rotation.
+ *
* @return true if it is an identity rep., false otherwise
*/
- public boolean isIdentity()
- {
- if (w==0 && x==0 && y==0 && z==0)
- return true;
- return false;
+ public boolean isIdentity() {
+ return w == 1 && x == 0 && y == 0 && z == 0;
}
- /** compute the quaternion from a 3x3 column matrix
- * @param m 3x3 column matrix
+ /***
+ * Set this quaternion to identity (x=0,y=0,z=0,w=1)
+ */
+ public void setIdentity() {
+ x = y = z = 0;
+ w = 1;
+ }
+
+ /**
+ * compute the quaternion from a 3x3 column matrix
+ *
+ * @param m 3x3 column matrix
*/
public void setFromMatrix(float[] m) {
- float T= m[0] + m[4] + m[8] + 1;
- if (T>0){
- float S = 0.5f / (float)FloatUtil.sqrt(T);
+ float T = m[0] + m[4] + m[8] + 1;
+ if (T > 0) {
+ float S = 0.5f / (float) FloatUtil.sqrt(T);
w = 0.25f / S;
- x = ( m[5] - m[7]) * S;
- y = ( m[6] - m[2]) * S;
- z = ( m[1] - m[3] ) * S;
- }
- else{
- if ((m[0] > m[4])&(m[0] > m[8])) {
- float S = FloatUtil.sqrt( 1.0f + m[0] - m[4] - m[8] ) * 2f; // S=4*qx
+ x = (m[5] - m[7]) * S;
+ y = (m[6] - m[2]) * S;
+ z = (m[1] - m[3]) * S;
+ } else {
+ if ((m[0] > m[4]) & (m[0] > m[8])) {
+ float S = FloatUtil.sqrt(1.0f + m[0] - m[4] - m[8]) * 2f; // S=4*qx
w = (m[7] - m[5]) / S;
x = 0.25f * S;
- y = (m[3] + m[1]) / S;
- z = (m[6] + m[2]) / S;
- }
- else if (m[4] > m[8]) {
- float S = FloatUtil.sqrt( 1.0f + m[4] - m[0] - m[8] ) * 2f; // S=4*qy
+ y = (m[3] + m[1]) / S;
+ z = (m[6] + m[2]) / S;
+ } else if (m[4] > m[8]) {
+ float S = FloatUtil.sqrt(1.0f + m[4] - m[0] - m[8]) * 2f; // S=4*qy
w = (m[6] - m[2]) / S;
- x = (m[3] + m[1]) / S;
+ x = (m[3] + m[1]) / S;
y = 0.25f * S;
- z = (m[7] + m[5]) / S;
- }
- else {
- float S = FloatUtil.sqrt( 1.0f + m[8] - m[0] - m[4] ) * 2f; // S=4*qz
+ z = (m[7] + m[5]) / S;
+ } else {
+ float S = FloatUtil.sqrt(1.0f + m[8] - m[0] - m[4]) * 2f; // S=4*qz
w = (m[3] - m[1]) / S;
- x = (m[6] + m[2]) / S;
- y = (m[7] + m[5]) / S;
+ x = (m[6] + m[2]) / S;
+ y = (m[7] + m[5]) / S;
z = 0.25f * S;
- }
+ }
}
}
-
- /** Check if the the 3x3 matrix (param) is in fact
- * an affine rotational matrix
+
+ /**
+ * Check if the the 3x3 matrix (param) is in fact an affine rotational
+ * matrix
+ *
* @param m 3x3 column matrix
* @return true if representing a rotational matrix, false otherwise
*/
public boolean isRotationMatrix(float[] m) {
double epsilon = 0.01; // margin to allow for rounding errors
- if (FloatUtil.abs(m[0]*m[3] + m[3]*m[4] + m[6]*m[7]) > epsilon) return false;
- if (FloatUtil.abs(m[0]*m[2] + m[3]*m[5] + m[6]*m[8]) > epsilon) return false;
- if (FloatUtil.abs(m[1]*m[2] + m[4]*m[5] + m[7]*m[8]) > epsilon) return false;
- if (FloatUtil.abs(m[0]*m[0] + m[3]*m[3] + m[6]*m[6] - 1) > epsilon) return false;
- if (FloatUtil.abs(m[1]*m[1] + m[4]*m[4] + m[7]*m[7] - 1) > epsilon) return false;
- if (FloatUtil.abs(m[2]*m[2] + m[5]*m[5] + m[8]*m[8] - 1) > epsilon) return false;
- return (FloatUtil.abs(determinant(m)-1) < epsilon);
+ if (FloatUtil.abs(m[0] * m[3] + m[3] * m[4] + m[6] * m[7]) > epsilon)
+ return false;
+ if (FloatUtil.abs(m[0] * m[2] + m[3] * m[5] + m[6] * m[8]) > epsilon)
+ return false;
+ if (FloatUtil.abs(m[1] * m[2] + m[4] * m[5] + m[7] * m[8]) > epsilon)
+ return false;
+ if (FloatUtil.abs(m[0] * m[0] + m[3] * m[3] + m[6] * m[6] - 1) > epsilon)
+ return false;
+ if (FloatUtil.abs(m[1] * m[1] + m[4] * m[4] + m[7] * m[7] - 1) > epsilon)
+ return false;
+ if (FloatUtil.abs(m[2] * m[2] + m[5] * m[5] + m[8] * m[8] - 1) > epsilon)
+ return false;
+ return (FloatUtil.abs(determinant(m) - 1) < epsilon);
}
+
private float determinant(float[] m) {
- return m[0]*m[4]*m[8] + m[3]*m[7]*m[2] + m[6]*m[1]*m[5] - m[0]*m[7]*m[5] - m[3]*m[1]*m[8] - m[6]*m[4]*m[2];
+ return m[0] * m[4] * m[8] + m[3] * m[7] * m[2] + m[6] * m[1] * m[5]
+ - m[0] * m[7] * m[5] - m[3] * m[1] * m[8] - m[6] * m[4] * m[2];
}
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-12-15 02:50:21 +0000