/*
* Open Source Physics software is free software as described near the bottom of this code file.
*
* For additional information and documentation on Open Source Physics please see:
* <http://www.opensourcephysics.org/>
*/
package org.opensourcephysics.numerics;
import org.opensourcephysics.controls.XML;
import org.opensourcephysics.controls.XMLControl;
import org.opensourcephysics.controls.XMLLoader;
/**
* Quaternion models a unit quaternion and implements quaternion arithmetic.
*/
public class Quaternion implements MatrixTransformation {
static final double SQRT2 = Math.sqrt(2);
protected double q0, q1, q2, q3; // quaternion components
protected double ox = 0, oy = 0, oz = 0; // origin for this rotation
/**
* Constructs and initializes quaternion from the specified components.
*
* @param q0 double
* @param q1 double
* @param q2 double
* @param q3 double
*/
public Quaternion(double q0, double q1, double q2, double q3) {
this.q0 = q0;
this.q1 = q1;
this.q2 = q2;
this.q3 = q3;
normalize();
}
/**
* Constructs and initializes quaternion from the specified components.
*
* @param q0 double
* @param vector sets with q1:vector.x, q2:vector.y, q3:vector.z
*/
public Quaternion(double q0, Vec3D vector) {
this(q0, vector.x, vector.y, vector.z);
}
/**
* Constructs and initializes a quaternion from the array of length 4.
* @param q the array of length 4 containing q0, q1, q2, q3
*/
public Quaternion(double[] q) {
q0 = q[0];
q1 = q[1];
q2 = q[2];
q3 = q[3];
normalize();
}
/**
* Constructs and initializes a Quaternion with the same values as the given quaternion.
* @param q the Vector3d containing the initialization x y z data
*/
public Quaternion(Quaternion q) {
q0 = q.q0;
q1 = q.q1;
q2 = q.q2;
q3 = q.q3;
normalize();
}
/**
* Constructs and initializes a unit quaternion (1,0,0,0).
*/
public Quaternion() {
this(1, 0, 0, 0);
}
/**
* Instantiates a quaternion that aligns the first vector with the second vector.
*
* @param v1 double[]
* @param v2 double[]
* @return Quaternion
*/
public static Quaternion createAlignmentTransformation(double[] v1, double v2[]) {
v1 = VectorMath.normalize(v1.clone());
v2 = VectorMath.normalize(v2.clone());
double[] r = VectorMath.cross3D(v1, v2);
double s = Math.sqrt(2*(1+VectorMath.dot(v1, v2)));
return new Quaternion(s/2, r[0]/s, r[1]/s, r[2]/s);
}
/**
* Sets the origin for this rotation.
*
* @param ox double
* @param oy double
* @param oz double
*/
public void setOrigin(double ox, double oy, double oz) {
this.ox = ox;
this.oy = oy;
this.oz = oz;
}
/**
* Sets the origin for this rotation.
*
* @param origin double[] the new origin
* @return double[]
*/
public double[] setOrigin(double[] origin) {
this.ox = origin[0];
this.oy = origin[1];
this.oz = origin[2];
return origin;
}
/**
* Returns the origin for this rotation
* @return
*/
public double[] getOrigin() {
return new double[] { ox,oy,oz };
}
/**
* Gets the direct rotation matrix of this quaternion rotation.
* Assumes that the quaternion has been normalized.
*
* If the mat parameter is null a double[3][3] array is created;
* otherwise the given array is used.
*
* @param mat double[][] optional matrix
* @return double[][] the matrix
*/
public final double[][] getRotationMatrix(double[][] mat) {
double q0q0 = q0*q0, q0q1 = q0*q1, q0q2 = q0*q2, q0q3 = q0*q3;
double q1q1 = q1*q1, q1q2 = q1*q2, q1q3 = q1*q3;
double q2q2 = q2*q2, q2q3 = q2*q3;
double q3q3 = q3*q3;
if(mat==null) {
mat = new double[3][3];
}
mat[0][0] = (q0q0+q1q1-q2q2-q3q3);
mat[0][1] = 2*(-q0q3+q1q2);
mat[0][2] = 2*(q0q2+q1q3);
mat[1][0] = 2*(q0q3+q1q2);
mat[1][1] = (q0q0-q1q1+q2q2-q3q3);
mat[1][2] = 2*(-q0q1+q2q3);
mat[2][0] = 2*(-q0q2+q1q3);
mat[2][1] = 2*(q0q1+q2q3);
mat[2][2] = (q0q0-q1q1-q2q2+q3q3);
return mat;
}
/**
* Gets the direct homogeneous affine transformation flattened into a 1-d array.
*
* If the mat parameter is null a double[16] array is created;
* otherwise the given array is used.
*
* @param mat double[] optional matrix
* @return double[] the matrix
*/
public final double[] getFlatMatrix(double[] mat) {
double q0q0 = q0*q0, q0q1 = q0*q1, q0q2 = q0*q2, q0q3 = q0*q3;
double q1q1 = q1*q1, q1q2 = q1*q2, q1q3 = q1*q3;
double q2q2 = q2*q2, q2q3 = q2*q3;
double q3q3 = q3*q3;
if(mat==null) {
mat = new double[16];
}
mat[0] = (q0q0+q1q1-q2q2-q3q3);
mat[4] = 2*(-q0q3+q1q2);
mat[8] = 2*(q0q2+q1q3);
mat[1] = 2*(q0q3+q1q2);
mat[5] = (q0q0-q1q1+q2q2-q3q3);
mat[9] = 2*(-q0q1+q2q3);
mat[2] = 2*(-q0q2+q1q3);
mat[3] = 0;
mat[6] = 2*(q0q1+q2q3);
mat[7] = 0;
mat[10] = (q0q0-q1q1-q2q2+q3q3);
mat[11] = 0;
mat[12] = ox-ox*mat[0]-oy*mat[4]-oz*mat[8];
mat[13] = oy-ox*mat[1]-oy*mat[5]-oz*mat[9];
mat[14] = oz-ox*mat[2]-oy*mat[6]-oz*mat[10];
mat[15] = 1;
return mat;
}
/**
* Gets the Quaternion coordinates.
* @return double[]
*/
public double[] getCoordinates() {
return new double[] {q0, q1, q2, q3};
}
/**
* Sets the quaternion coordinates.
*
* @param q0 double
* @param q1 double
* @param q2 double
* @param q3 double
*/
public void setCoordinates(double q0, double q1, double q2, double q3) {
this.q0 = q0;
this.q1 = q1;
this.q2 = q2;
this.q3 = q3;
normalize();
}
/**
* Sets the quaternion coordinates from the array of length 4.
*
* @param q the array of length 4 containing q0, q1, q2, q3
*/
public double[] setCoordinates(double[] q) {
q0 = q[0];
q1 = q[1];
q2 = q[2];
q3 = q[3];
normalize();
return q;
}
/**
* Normalizes this quaternion in place.
*/
public final void normalize() {
double norm = q0*q0+q1*q1+q2*q2+q3*q3;
if(norm==1) {
return; // often doesn't happen due to roundoff
}
norm = 1.0/Math.sqrt(norm); // expensive operation
q0 *= norm;
q1 *= norm;
q2 *= norm;
q3 *= norm;
}
/**
* Conjugates this quaternion in place.
*/
public final void conjugate() {
q1 = -q1;
q2 = -q2;
q3 = -q3;
}
/**
* Adds this quaternion to the given quaternion.
*
* @param q Quaternion
*/
public final void add(Quaternion q) {
q0 += this.q0;
q1 += this.q1;
q2 += this.q2;
q3 += this.q3;
}
/**
* Subtracts this quaternion from the given quaternion.
*
* @param q Quaternion
*/
public final void subtract(Quaternion q) {
q0 -= this.q0;
q1 -= this.q1;
q2 -= this.q2;
q3 -= this.q3;
}
/**
* Multiplies this quaternion with the given quaternion.
*
* @param q Quaternion
*/
public final void multiply(Quaternion q) {
double w = q0*q.q0-q1*q.q1-q2*q.q2-q3*q.q3;
double x = q3*q.q2-q2*q.q3+q1*q.q0+q0*q.q1;
double y = q1*q.q3-q3*q.q1+q2*q.q0+q0*q.q2;
double z = q2*q.q1-q1*q.q2+q3*q.q0+q0*q.q3;
q0 = w;
q1 = x;
q2 = y;
q3 = z;
normalize();
}
/**
* Returns the dot product of this quaternion and quaternion q.
* @param q the other quaternion
* @return the dot product of this and q
*/
public final double dot(Quaternion q) {
return(q0*q.q0+q1*q.q1+q2*q.q2+q3*q.q3);
}
/**
* Returns the squared magnitude of this vector.
* @return the squared magnitude
*/
public final double magnitudeSquared() {
return(q0*q0+q1*q1+q2*q2+q3*q3);
}
/**
* Returns the magnitude of this quaternion.
* @return the magnitude
*/
public final double magnitude() {
return Math.sqrt(q0*q0+q1*q1+q2*q2+q3*q3);
}
/**
* Returns the angle in radians between this quaternion and the given
* quaternion.
*
* @param q the other quaternion
* @return the angle in radians in the range [0,PI]
*/
public final double angle(Quaternion q) {
double norm1 = Math.sqrt(q1*q1+q2*q2+q3*q3); // normalization for vector part of this quaternion
double norm2 = Math.sqrt(q.q1*q.q1+q.q2*q.q2+q.q3*q.q3); // normazliation for vector part for given quaternion
double w = Math.sqrt(1+(q1*q.q1+q2*q.q2+q3*q.q3)/norm1/norm2);
return(2*Math.acos(w/SQRT2));
}
/**
* Instaniates a quaterion whose components are identical to this quaterion.
* @return Object
*/
public Object clone() {
Quaternion q = new Quaternion(q0, q1, q2, q3);
q.setOrigin(ox, oy, oz);
return q;
}
/**
* Transforms (rotates) the coordinates of the given point.
*
* @param p double[]
* @return double[]
*/
public double[] direct(double[] p) { // assumes quaternion is normalized
p[0] -= ox;
p[1] -= oy;
p[2] -= oz;
double pMult = 2*q0*q0-1;
double vMult = 2*(q1*p[0]+q2*p[1]+q3*p[2]);
double crossMult = 2*q0;
double x = pMult*p[0]+vMult*q1+crossMult*(q2*p[2]-q3*p[1]);
double y = pMult*p[1]+vMult*q2+crossMult*(q3*p[0]-q1*p[2]);
p[2] = pMult*p[2]+vMult*q3+crossMult*(q1*p[1]-q2*p[0])+oz;
p[0] = x+ox;
p[1] = y+oy;
return p;
}
public double[] inverse(double[] p) throws UnsupportedOperationException { // assumes quaternion is normalized
p[0] -= ox;
p[1] -= oy;
p[2] -= oz;
double pMult = 2*q0*q0-1;
double vMult = 2*(q1*p[0]+q2*p[1]+q3*p[2]);
double crossMult = -2*q0; // inverse transformation changes sign of cross term
double x = pMult*p[0]+vMult*q1+crossMult*(q2*p[2]-q3*p[1]);
double y = pMult*p[1]+vMult*q2+crossMult*(q3*p[0]-q1*p[2]);
p[2] = pMult*p[2]+vMult*q3+crossMult*(q1*p[1]-q2*p[0])+oz;
p[0] = x+ox;
p[1] = y+oy;
return p;
}
public static XML.ObjectLoader getLoader() {
return new QuaternionLoader();
}
protected static class QuaternionLoader extends XMLLoader {
public void saveObject(XMLControl control, Object obj) {
Quaternion qr = (Quaternion) obj;
control.setValue("q0", qr.q0); //$NON-NLS-1$
control.setValue("q1", qr.q1); //$NON-NLS-1$
control.setValue("q2", qr.q2); //$NON-NLS-1$
control.setValue("q3", qr.q3); //$NON-NLS-1$
control.setValue("ox", qr.ox); //$NON-NLS-1$
control.setValue("oy", qr.oy); //$NON-NLS-1$
control.setValue("oz", qr.oz); //$NON-NLS-1$
}
public Object createObject(XMLControl control) {
return new Quaternion();
}
public Object loadObject(XMLControl control, Object obj) {
Quaternion qr = (Quaternion) obj;
double q0 = control.getDouble("q0"); //$NON-NLS-1$
double q1 = control.getDouble("q0"); //$NON-NLS-1$
double q2 = control.getDouble("q0"); //$NON-NLS-1$
double q3 = control.getDouble("q0"); //$NON-NLS-1$
qr.setCoordinates(q0, q1, q2, q3);
double ox = control.getDouble("ox"); //$NON-NLS-1$
double oy = control.getDouble("oy"); //$NON-NLS-1$
double oz = control.getDouble("oz"); //$NON-NLS-1$
qr.setOrigin(ox, oy, oz);
return obj;
}
}
}
/*
* Open Source Physics software is free software; you can redistribute
* it and/or modify it under the terms of the GNU General Public License (GPL) as
* published by the Free Software Foundation; either version 2 of the License,
* or(at your option) any later version.
* Code that uses any portion of the code in the org.opensourcephysics package
* or any subpackage (subdirectory) of this package must must also be be released
* under the GNU GPL license.
*
* This software is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston MA 02111-1307 USA
* or view the license online at http://www.gnu.org/copyleft/gpl.html
*
* Copyright (c) 2007 The Open Source Physics project
* http://www.opensourcephysics.org
*/