159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta/* 259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * Copyright (c) 2009-2010 jMonkeyEngine 359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * All rights reserved. 459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * Redistribution and use in source and binary forms, with or without 659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * modification, are permitted provided that the following conditions are 759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * met: 859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * * Redistributions of source code must retain the above copyright 1059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * notice, this list of conditions and the following disclaimer. 1159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 1259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * * Redistributions in binary form must reproduce the above copyright 1359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * notice, this list of conditions and the following disclaimer in the 1459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * documentation and/or other materials provided with the distribution. 1559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 1659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * * Neither the name of 'jMonkeyEngine' nor the names of its contributors 1759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * may be used to endorse or promote products derived from this software 1859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * without specific prior written permission. 1959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 2059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 2159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 2259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 2359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR 2459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 2559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 2659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 2759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 2859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 2959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 3059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 3159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 3259b2e6871c65f58fdad78cd7229c292f6a177578Scott Bartapackage com.jme3.math; 3359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 3459b2e6871c65f58fdad78cd7229c292f6a177578Scott Bartaimport com.jme3.export.*; 3559b2e6871c65f58fdad78cd7229c292f6a177578Scott Bartaimport com.jme3.util.TempVars; 3659b2e6871c65f58fdad78cd7229c292f6a177578Scott Bartaimport java.io.Externalizable; 3759b2e6871c65f58fdad78cd7229c292f6a177578Scott Bartaimport java.io.IOException; 3859b2e6871c65f58fdad78cd7229c292f6a177578Scott Bartaimport java.io.ObjectInput; 3959b2e6871c65f58fdad78cd7229c292f6a177578Scott Bartaimport java.io.ObjectOutput; 4059b2e6871c65f58fdad78cd7229c292f6a177578Scott Bartaimport java.util.logging.Logger; 4159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 4259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta/** 4359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>Quaternion</code> defines a single example of a more general class of 4459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * hypercomplex numbers. Quaternions extends a rotation in three dimensions to a 4559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * rotation in four dimensions. This avoids "gimbal lock" and allows for smooth 4659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * continuous rotation. 4759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 4859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>Quaternion</code> is defined by four floating point numbers: {x y z 4959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * w}. 5059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 5159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @author Mark Powell 5259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @author Joshua Slack 5359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 5459b2e6871c65f58fdad78cd7229c292f6a177578Scott Bartapublic final class Quaternion implements Savable, Cloneable, java.io.Serializable { 5559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 5659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta static final long serialVersionUID = 1; 5759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 5859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta private static final Logger logger = Logger.getLogger(Quaternion.class.getName()); 5959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 6059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * Represents the identity quaternion rotation (0, 0, 0, 1). 6159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 6259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public static final Quaternion IDENTITY = new Quaternion(); 6359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public static final Quaternion DIRECTION_Z = new Quaternion(); 6459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public static final Quaternion ZERO = new Quaternion(0, 0, 0, 0); 6559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 6659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta static { 6759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta DIRECTION_Z.fromAxes(Vector3f.UNIT_X, Vector3f.UNIT_Y, Vector3f.UNIT_Z); 6859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 6959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta protected float x, y, z, w; 7059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 7159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 7259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * Constructor instantiates a new <code>Quaternion</code> object 7359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * initializing all values to zero, except w which is initialized to 1. 7459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 7559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 7659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion() { 7759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta x = 0; 7859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta y = 0; 7959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta z = 0; 8059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta w = 1; 8159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 8259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 8359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 8459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * Constructor instantiates a new <code>Quaternion</code> object from the 8559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * given list of parameters. 8659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 8759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param x 8859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the x value of the quaternion. 8959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param y 9059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the y value of the quaternion. 9159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param z 9259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the z value of the quaternion. 9359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param w 9459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the w value of the quaternion. 9559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 9659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion(float x, float y, float z, float w) { 9759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.x = x; 9859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.y = y; 9959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.z = z; 10059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.w = w; 10159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 10259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 10359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public float getX() { 10459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return x; 10559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 10659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 10759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public float getY() { 10859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return y; 10959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 11059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 11159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public float getZ() { 11259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return z; 11359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 11459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 11559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public float getW() { 11659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return w; 11759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 11859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 11959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 12059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * sets the data in a <code>Quaternion</code> object from the given list 12159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * of parameters. 12259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 12359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param x 12459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the x value of the quaternion. 12559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param y 12659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the y value of the quaternion. 12759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param z 12859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the z value of the quaternion. 12959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param w 13059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the w value of the quaternion. 13159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return this 13259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 13359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion set(float x, float y, float z, float w) { 13459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.x = x; 13559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.y = y; 13659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.z = z; 13759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.w = w; 13859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return this; 13959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 14059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 14159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 14259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * Sets the data in this <code>Quaternion</code> object to be equal to the 14359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * passed <code>Quaternion</code> object. The values are copied producing 14459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * a new object. 14559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 14659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param q 14759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * The Quaternion to copy values from. 14859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return this 14959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 15059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion set(Quaternion q) { 15159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.x = q.x; 15259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.y = q.y; 15359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.z = q.z; 15459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.w = q.w; 15559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return this; 15659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 15759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 15859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 15959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * Constructor instantiates a new <code>Quaternion</code> object from a 16059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * collection of rotation angles. 16159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 16259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param angles 16359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the angles of rotation (x, y, z) that will define the 16459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>Quaternion</code>. 16559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 16659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion(float[] angles) { 16759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta fromAngles(angles); 16859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 16959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 17059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 17159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * Constructor instantiates a new <code>Quaternion</code> object from an 17259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * interpolation between two other quaternions. 17359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 17459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param q1 17559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the first quaternion. 17659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param q2 17759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the second quaternion. 17859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param interp 17959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the amount to interpolate between the two quaternions. 18059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 18159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion(Quaternion q1, Quaternion q2, float interp) { 18259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta slerp(q1, q2, interp); 18359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 18459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 18559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 18659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * Constructor instantiates a new <code>Quaternion</code> object from an 18759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * existing quaternion, creating a copy. 18859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 18959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param q 19059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the quaternion to copy. 19159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 19259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion(Quaternion q) { 19359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.x = q.x; 19459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.y = q.y; 19559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.z = q.z; 19659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.w = q.w; 19759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 19859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 19959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 20059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * Sets this Quaternion to {0, 0, 0, 1}. Same as calling set(0,0,0,1). 20159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 20259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public void loadIdentity() { 20359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta x = y = z = 0; 20459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta w = 1; 20559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 20659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 20759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 20859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return true if this Quaternion is {0,0,0,1} 20959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 21059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public boolean isIdentity() { 21159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (x == 0 && y == 0 && z == 0 && w == 1) { 21259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return true; 21359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } else { 21459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return false; 21559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 21659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 21759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 21859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 21959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>fromAngles</code> builds a quaternion from the Euler rotation 22059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * angles (y,r,p). 22159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 22259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param angles 22359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the Euler angles of rotation (in radians). 22459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 22559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion fromAngles(float[] angles) { 22659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (angles.length != 3) { 22759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta throw new IllegalArgumentException( 22859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta "Angles array must have three elements"); 22959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 23059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 23159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return fromAngles(angles[0], angles[1], angles[2]); 23259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 23359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 23459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 23559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>fromAngles</code> builds a Quaternion from the Euler rotation 23659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * angles (y,r,p). Note that we are applying in order: roll, pitch, yaw but 23759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * we've ordered them in x, y, and z for convenience. 23859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @see <a href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm">http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm</a> 23959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 24059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param yaw 24159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the Euler yaw of rotation (in radians). (aka Bank, often rot 24259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * around x) 24359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param roll 24459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the Euler roll of rotation (in radians). (aka Heading, often 24559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * rot around y) 24659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param pitch 24759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the Euler pitch of rotation (in radians). (aka Attitude, often 24859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * rot around z) 24959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 25059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion fromAngles(float yaw, float roll, float pitch) { 25159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float angle; 25259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float sinRoll, sinPitch, sinYaw, cosRoll, cosPitch, cosYaw; 25359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta angle = pitch * 0.5f; 25459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta sinPitch = FastMath.sin(angle); 25559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta cosPitch = FastMath.cos(angle); 25659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta angle = roll * 0.5f; 25759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta sinRoll = FastMath.sin(angle); 25859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta cosRoll = FastMath.cos(angle); 25959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta angle = yaw * 0.5f; 26059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta sinYaw = FastMath.sin(angle); 26159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta cosYaw = FastMath.cos(angle); 26259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 26359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // variables used to reduce multiplication calls. 26459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float cosRollXcosPitch = cosRoll * cosPitch; 26559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float sinRollXsinPitch = sinRoll * sinPitch; 26659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float cosRollXsinPitch = cosRoll * sinPitch; 26759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float sinRollXcosPitch = sinRoll * cosPitch; 26859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 26959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta w = (cosRollXcosPitch * cosYaw - sinRollXsinPitch * sinYaw); 27059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta x = (cosRollXcosPitch * sinYaw + sinRollXsinPitch * cosYaw); 27159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta y = (sinRollXcosPitch * cosYaw + cosRollXsinPitch * sinYaw); 27259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta z = (cosRollXsinPitch * cosYaw - sinRollXcosPitch * sinYaw); 27359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 27459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta normalize(); 27559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return this; 27659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 27759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 27859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 27959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>toAngles</code> returns this quaternion converted to Euler 28059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * rotation angles (yaw,roll,pitch).<br/> 28159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @see <a href="http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm">http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm</a> 28259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 28359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param angles 28459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the float[] in which the angles should be stored, or null if 28559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * you want a new float[] to be created 28659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return the float[] in which the angles are stored. 28759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 28859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public float[] toAngles(float[] angles) { 28959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (angles == null) { 29059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta angles = new float[3]; 29159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } else if (angles.length != 3) { 29259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta throw new IllegalArgumentException("Angles array must have three elements"); 29359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 29459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 29559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float sqw = w * w; 29659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float sqx = x * x; 29759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float sqy = y * y; 29859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float sqz = z * z; 29959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise 30059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // is correction factor 30159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float test = x * y + z * w; 30259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (test > 0.499 * unit) { // singularity at north pole 30359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta angles[1] = 2 * FastMath.atan2(x, w); 30459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta angles[2] = FastMath.HALF_PI; 30559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta angles[0] = 0; 30659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } else if (test < -0.499 * unit) { // singularity at south pole 30759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta angles[1] = -2 * FastMath.atan2(x, w); 30859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta angles[2] = -FastMath.HALF_PI; 30959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta angles[0] = 0; 31059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } else { 31159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta angles[1] = FastMath.atan2(2 * y * w - 2 * x * z, sqx - sqy - sqz + sqw); // roll or heading 31259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta angles[2] = FastMath.asin(2 * test / unit); // pitch or attitude 31359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta angles[0] = FastMath.atan2(2 * x * w - 2 * y * z, -sqx + sqy - sqz + sqw); // yaw or bank 31459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 31559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return angles; 31659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 31759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 31859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 31959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 32059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>fromRotationMatrix</code> generates a quaternion from a supplied 32159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * matrix. This matrix is assumed to be a rotational matrix. 32259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 32359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param matrix 32459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the matrix that defines the rotation. 32559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 32659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion fromRotationMatrix(Matrix3f matrix) { 32759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return fromRotationMatrix(matrix.m00, matrix.m01, matrix.m02, matrix.m10, 32859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta matrix.m11, matrix.m12, matrix.m20, matrix.m21, matrix.m22); 32959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 33059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 33159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion fromRotationMatrix(float m00, float m01, float m02, 33259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float m10, float m11, float m12, 33359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float m20, float m21, float m22) { 33459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // Use the Graphics Gems code, from 33559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z 33659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // *NOT* the "Matrix and Quaternions FAQ", which has errors! 33759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 33859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // the trace is the sum of the diagonal elements; see 33959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // http://mathworld.wolfram.com/MatrixTrace.html 34059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float t = m00 + m11 + m22; 34159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 34259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // we protect the division by s by ensuring that s>=1 34359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (t >= 0) { // |w| >= .5 34459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float s = FastMath.sqrt(t + 1); // |s|>=1 ... 34559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta w = 0.5f * s; 34659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta s = 0.5f / s; // so this division isn't bad 34759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta x = (m21 - m12) * s; 34859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta y = (m02 - m20) * s; 34959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta z = (m10 - m01) * s; 35059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } else if ((m00 > m11) && (m00 > m22)) { 35159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float s = FastMath.sqrt(1.0f + m00 - m11 - m22); // |s|>=1 35259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta x = s * 0.5f; // |x| >= .5 35359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta s = 0.5f / s; 35459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta y = (m10 + m01) * s; 35559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta z = (m02 + m20) * s; 35659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta w = (m21 - m12) * s; 35759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } else if (m11 > m22) { 35859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float s = FastMath.sqrt(1.0f + m11 - m00 - m22); // |s|>=1 35959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta y = s * 0.5f; // |y| >= .5 36059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta s = 0.5f / s; 36159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta x = (m10 + m01) * s; 36259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta z = (m21 + m12) * s; 36359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta w = (m02 - m20) * s; 36459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } else { 36559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float s = FastMath.sqrt(1.0f + m22 - m00 - m11); // |s|>=1 36659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta z = s * 0.5f; // |z| >= .5 36759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta s = 0.5f / s; 36859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta x = (m02 + m20) * s; 36959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta y = (m21 + m12) * s; 37059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta w = (m10 - m01) * s; 37159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 37259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 37359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return this; 37459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 37559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 37659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 37759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>toRotationMatrix</code> converts this quaternion to a rotational 37859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * matrix. Note: the result is created from a normalized version of this quat. 37959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 38059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return the rotation matrix representation of this quaternion. 38159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 38259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Matrix3f toRotationMatrix() { 38359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta Matrix3f matrix = new Matrix3f(); 38459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return toRotationMatrix(matrix); 38559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 38659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 38759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 38859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>toRotationMatrix</code> converts this quaternion to a rotational 38959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * matrix. The result is stored in result. 39059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 39159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param result 39259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * The Matrix3f to store the result in. 39359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return the rotation matrix representation of this quaternion. 39459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 39559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Matrix3f toRotationMatrix(Matrix3f result) { 39659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 39759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float norm = norm(); 39859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // we explicitly test norm against one here, saving a division 39959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // at the cost of a test and branch. Is it worth it? 40059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float s = (norm == 1f) ? 2f : (norm > 0f) ? 2f / norm : 0; 40159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 40259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // compute xs/ys/zs first to save 6 multiplications, since xs/ys/zs 40359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // will be used 2-4 times each. 40459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float xs = x * s; 40559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float ys = y * s; 40659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float zs = z * s; 40759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float xx = x * xs; 40859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float xy = x * ys; 40959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float xz = x * zs; 41059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float xw = w * xs; 41159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float yy = y * ys; 41259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float yz = y * zs; 41359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float yw = w * ys; 41459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float zz = z * zs; 41559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float zw = w * zs; 41659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 41759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // using s=2/norm (instead of 1/norm) saves 9 multiplications by 2 here 41859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta result.m00 = 1 - (yy + zz); 41959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta result.m01 = (xy - zw); 42059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta result.m02 = (xz + yw); 42159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta result.m10 = (xy + zw); 42259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta result.m11 = 1 - (xx + zz); 42359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta result.m12 = (yz - xw); 42459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta result.m20 = (xz - yw); 42559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta result.m21 = (yz + xw); 42659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta result.m22 = 1 - (xx + yy); 42759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 42859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return result; 42959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 43059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 43159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 43259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>toRotationMatrix</code> converts this quaternion to a rotational 43359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * matrix. The result is stored in result. 4th row and 4th column values are 43459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * untouched. Note: the result is created from a normalized version of this quat. 43559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 43659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param result 43759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * The Matrix4f to store the result in. 43859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return the rotation matrix representation of this quaternion. 43959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 44059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Matrix4f toRotationMatrix(Matrix4f result) { 44159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 44259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float norm = norm(); 44359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // we explicitly test norm against one here, saving a division 44459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // at the cost of a test and branch. Is it worth it? 44559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float s = (norm == 1f) ? 2f : (norm > 0f) ? 2f / norm : 0; 44659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 44759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // compute xs/ys/zs first to save 6 multiplications, since xs/ys/zs 44859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // will be used 2-4 times each. 44959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float xs = x * s; 45059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float ys = y * s; 45159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float zs = z * s; 45259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float xx = x * xs; 45359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float xy = x * ys; 45459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float xz = x * zs; 45559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float xw = w * xs; 45659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float yy = y * ys; 45759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float yz = y * zs; 45859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float yw = w * ys; 45959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float zz = z * zs; 46059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float zw = w * zs; 46159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 46259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // using s=2/norm (instead of 1/norm) saves 9 multiplications by 2 here 46359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta result.m00 = 1 - (yy + zz); 46459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta result.m01 = (xy - zw); 46559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta result.m02 = (xz + yw); 46659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta result.m10 = (xy + zw); 46759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta result.m11 = 1 - (xx + zz); 46859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta result.m12 = (yz - xw); 46959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta result.m20 = (xz - yw); 47059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta result.m21 = (yz + xw); 47159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta result.m22 = 1 - (xx + yy); 47259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 47359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return result; 47459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 47559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 47659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 47759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>getRotationColumn</code> returns one of three columns specified 47859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * by the parameter. This column is returned as a <code>Vector3f</code> 47959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * object. 48059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 48159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param i 48259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the column to retrieve. Must be between 0 and 2. 48359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return the column specified by the index. 48459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 48559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Vector3f getRotationColumn(int i) { 48659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return getRotationColumn(i, null); 48759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 48859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 48959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 49059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>getRotationColumn</code> returns one of three columns specified 49159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * by the parameter. This column is returned as a <code>Vector3f</code> 49259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * object. The value is retrieved as if this quaternion was first normalized. 49359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 49459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param i 49559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the column to retrieve. Must be between 0 and 2. 49659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param store 49759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the vector object to store the result in. if null, a new one 49859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * is created. 49959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return the column specified by the index. 50059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 50159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Vector3f getRotationColumn(int i, Vector3f store) { 50259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (store == null) { 50359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta store = new Vector3f(); 50459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 50559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 50659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float norm = norm(); 50759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (norm != 1.0f) { 50859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta norm = FastMath.invSqrt(norm); 50959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 51059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 51159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float xx = x * x * norm; 51259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float xy = x * y * norm; 51359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float xz = x * z * norm; 51459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float xw = x * w * norm; 51559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float yy = y * y * norm; 51659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float yz = y * z * norm; 51759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float yw = y * w * norm; 51859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float zz = z * z * norm; 51959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float zw = z * w * norm; 52059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 52159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta switch (i) { 52259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta case 0: 52359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta store.x = 1 - 2 * (yy + zz); 52459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta store.y = 2 * (xy + zw); 52559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta store.z = 2 * (xz - yw); 52659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta break; 52759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta case 1: 52859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta store.x = 2 * (xy - zw); 52959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta store.y = 1 - 2 * (xx + zz); 53059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta store.z = 2 * (yz + xw); 53159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta break; 53259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta case 2: 53359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta store.x = 2 * (xz + yw); 53459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta store.y = 2 * (yz - xw); 53559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta store.z = 1 - 2 * (xx + yy); 53659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta break; 53759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta default: 53859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta logger.warning("Invalid column index."); 53959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta throw new IllegalArgumentException("Invalid column index. " + i); 54059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 54159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 54259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return store; 54359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 54459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 54559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 54659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>fromAngleAxis</code> sets this quaternion to the values specified 54759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * by an angle and an axis of rotation. This method creates an object, so 54859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * use fromAngleNormalAxis if your axis is already normalized. 54959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 55059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param angle 55159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the angle to rotate (in radians). 55259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param axis 55359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the axis of rotation. 55459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return this quaternion 55559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 55659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion fromAngleAxis(float angle, Vector3f axis) { 55759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta Vector3f normAxis = axis.normalize(); 55859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta fromAngleNormalAxis(angle, normAxis); 55959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return this; 56059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 56159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 56259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 56359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>fromAngleNormalAxis</code> sets this quaternion to the values 56459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * specified by an angle and a normalized axis of rotation. 56559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 56659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param angle 56759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the angle to rotate (in radians). 56859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param axis 56959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the axis of rotation (already normalized). 57059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 57159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion fromAngleNormalAxis(float angle, Vector3f axis) { 57259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (axis.x == 0 && axis.y == 0 && axis.z == 0) { 57359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta loadIdentity(); 57459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } else { 57559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float halfAngle = 0.5f * angle; 57659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float sin = FastMath.sin(halfAngle); 57759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta w = FastMath.cos(halfAngle); 57859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta x = sin * axis.x; 57959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta y = sin * axis.y; 58059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta z = sin * axis.z; 58159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 58259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return this; 58359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 58459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 58559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 58659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>toAngleAxis</code> sets a given angle and axis to that 58759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * represented by the current quaternion. The values are stored as 58859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * following: The axis is provided as a parameter and built by the method, 58959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the angle is returned as a float. 59059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 59159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param axisStore 59259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the object we'll store the computed axis in. 59359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return the angle of rotation in radians. 59459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 59559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public float toAngleAxis(Vector3f axisStore) { 59659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float sqrLength = x * x + y * y + z * z; 59759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float angle; 59859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (sqrLength == 0.0f) { 59959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta angle = 0.0f; 60059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (axisStore != null) { 60159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta axisStore.x = 1.0f; 60259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta axisStore.y = 0.0f; 60359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta axisStore.z = 0.0f; 60459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 60559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } else { 60659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta angle = (2.0f * FastMath.acos(w)); 60759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (axisStore != null) { 60859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float invLength = (1.0f / FastMath.sqrt(sqrLength)); 60959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta axisStore.x = x * invLength; 61059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta axisStore.y = y * invLength; 61159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta axisStore.z = z * invLength; 61259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 61359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 61459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 61559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return angle; 61659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 61759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 61859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 61959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>slerp</code> sets this quaternion's value as an interpolation 62059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * between two other quaternions. 62159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 62259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param q1 62359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the first quaternion. 62459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param q2 62559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the second quaternion. 62659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param t 62759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the amount to interpolate between the two quaternions. 62859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 62959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion slerp(Quaternion q1, Quaternion q2, float t) { 63059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // Create a local quaternion to store the interpolated quaternion 63159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (q1.x == q2.x && q1.y == q2.y && q1.z == q2.z && q1.w == q2.w) { 63259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.set(q1); 63359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return this; 63459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 63559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 63659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float result = (q1.x * q2.x) + (q1.y * q2.y) + (q1.z * q2.z) 63759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta + (q1.w * q2.w); 63859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 63959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (result < 0.0f) { 64059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // Negate the second quaternion and the result of the dot product 64159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta q2.x = -q2.x; 64259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta q2.y = -q2.y; 64359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta q2.z = -q2.z; 64459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta q2.w = -q2.w; 64559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta result = -result; 64659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 64759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 64859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // Set the first and second scale for the interpolation 64959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float scale0 = 1 - t; 65059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float scale1 = t; 65159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 65259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // Check if the angle between the 2 quaternions was big enough to 65359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // warrant such calculations 65459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if ((1 - result) > 0.1f) {// Get the angle between the 2 quaternions, 65559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // and then store the sin() of that angle 65659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float theta = FastMath.acos(result); 65759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float invSinTheta = 1f / FastMath.sin(theta); 65859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 65959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // Calculate the scale for q1 and q2, according to the angle and 66059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // it's sine value 66159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta scale0 = FastMath.sin((1 - t) * theta) * invSinTheta; 66259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta scale1 = FastMath.sin((t * theta)) * invSinTheta; 66359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 66459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 66559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // Calculate the x, y, z and w values for the quaternion by using a 66659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // special 66759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // form of linear interpolation for quaternions. 66859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.x = (scale0 * q1.x) + (scale1 * q2.x); 66959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.y = (scale0 * q1.y) + (scale1 * q2.y); 67059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.z = (scale0 * q1.z) + (scale1 * q2.z); 67159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.w = (scale0 * q1.w) + (scale1 * q2.w); 67259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 67359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // Return the interpolated quaternion 67459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return this; 67559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 67659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 67759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 67859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * Sets the values of this quaternion to the slerp from itself to q2 by 67959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * changeAmnt 68059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 68159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param q2 68259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * Final interpolation value 68359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param changeAmnt 68459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * The amount diffrence 68559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 68659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public void slerp(Quaternion q2, float changeAmnt) { 68759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (this.x == q2.x && this.y == q2.y && this.z == q2.z 68859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta && this.w == q2.w) { 68959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return; 69059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 69159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 69259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float result = (this.x * q2.x) + (this.y * q2.y) + (this.z * q2.z) 69359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta + (this.w * q2.w); 69459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 69559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (result < 0.0f) { 69659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // Negate the second quaternion and the result of the dot product 69759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta q2.x = -q2.x; 69859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta q2.y = -q2.y; 69959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta q2.z = -q2.z; 70059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta q2.w = -q2.w; 70159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta result = -result; 70259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 70359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 70459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // Set the first and second scale for the interpolation 70559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float scale0 = 1 - changeAmnt; 70659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float scale1 = changeAmnt; 70759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 70859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // Check if the angle between the 2 quaternions was big enough to 70959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // warrant such calculations 71059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if ((1 - result) > 0.1f) { 71159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // Get the angle between the 2 quaternions, and then store the sin() 71259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // of that angle 71359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float theta = FastMath.acos(result); 71459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float invSinTheta = 1f / FastMath.sin(theta); 71559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 71659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // Calculate the scale for q1 and q2, according to the angle and 71759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // it's sine value 71859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta scale0 = FastMath.sin((1 - changeAmnt) * theta) * invSinTheta; 71959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta scale1 = FastMath.sin((changeAmnt * theta)) * invSinTheta; 72059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 72159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 72259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // Calculate the x, y, z and w values for the quaternion by using a 72359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // special 72459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // form of linear interpolation for quaternions. 72559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.x = (scale0 * this.x) + (scale1 * q2.x); 72659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.y = (scale0 * this.y) + (scale1 * q2.y); 72759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.z = (scale0 * this.z) + (scale1 * q2.z); 72859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.w = (scale0 * this.w) + (scale1 * q2.w); 72959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 73059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 73159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 73259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * Sets the values of this quaternion to the nlerp from itself to q2 by blend. 73359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param q2 73459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param blend 73559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 73659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public void nlerp(Quaternion q2, float blend) { 73759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float dot = dot(q2); 73859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float blendI = 1.0f - blend; 73959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (dot < 0.0f) { 74059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta x = blendI * x - blend * q2.x; 74159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta y = blendI * y - blend * q2.y; 74259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta z = blendI * z - blend * q2.z; 74359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta w = blendI * w - blend * q2.w; 74459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } else { 74559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta x = blendI * x + blend * q2.x; 74659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta y = blendI * y + blend * q2.y; 74759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta z = blendI * z + blend * q2.z; 74859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta w = blendI * w + blend * q2.w; 74959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 75059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta normalizeLocal(); 75159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 75259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 75359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 75459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>add</code> adds the values of this quaternion to those of the 75559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * parameter quaternion. The result is returned as a new quaternion. 75659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 75759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param q 75859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the quaternion to add to this. 75959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return the new quaternion. 76059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 76159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion add(Quaternion q) { 76259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return new Quaternion(x + q.x, y + q.y, z + q.z, w + q.w); 76359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 76459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 76559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 76659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>add</code> adds the values of this quaternion to those of the 76759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * parameter quaternion. The result is stored in this Quaternion. 76859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 76959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param q 77059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the quaternion to add to this. 77159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return This Quaternion after addition. 77259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 77359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion addLocal(Quaternion q) { 77459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.x += q.x; 77559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.y += q.y; 77659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.z += q.z; 77759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.w += q.w; 77859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return this; 77959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 78059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 78159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 78259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>subtract</code> subtracts the values of the parameter quaternion 78359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * from those of this quaternion. The result is returned as a new 78459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * quaternion. 78559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 78659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param q 78759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the quaternion to subtract from this. 78859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return the new quaternion. 78959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 79059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion subtract(Quaternion q) { 79159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return new Quaternion(x - q.x, y - q.y, z - q.z, w - q.w); 79259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 79359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 79459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 79559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>subtract</code> subtracts the values of the parameter quaternion 79659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * from those of this quaternion. The result is stored in this Quaternion. 79759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 79859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param q 79959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the quaternion to subtract from this. 80059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return This Quaternion after subtraction. 80159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 80259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion subtractLocal(Quaternion q) { 80359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.x -= q.x; 80459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.y -= q.y; 80559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.z -= q.z; 80659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta this.w -= q.w; 80759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return this; 80859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 80959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 81059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 81159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>mult</code> multiplies this quaternion by a parameter quaternion. 81259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * The result is returned as a new quaternion. It should be noted that 81359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * quaternion multiplication is not commutative so q * p != p * q. 81459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 81559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param q 81659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the quaternion to multiply this quaternion by. 81759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return the new quaternion. 81859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 81959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion mult(Quaternion q) { 82059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return mult(q, null); 82159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 82259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 82359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 82459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>mult</code> multiplies this quaternion by a parameter quaternion. 82559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * The result is returned as a new quaternion. It should be noted that 82659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * quaternion multiplication is not commutative so q * p != p * q. 82759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 82859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * It IS safe for q and res to be the same object. 82959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * It IS safe for this and res to be the same object. 83059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 83159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param q 83259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the quaternion to multiply this quaternion by. 83359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param res 83459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the quaternion to store the result in. 83559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return the new quaternion. 83659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 83759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion mult(Quaternion q, Quaternion res) { 83859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (res == null) { 83959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta res = new Quaternion(); 84059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 84159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float qw = q.w, qx = q.x, qy = q.y, qz = q.z; 84259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta res.x = x * qw + y * qz - z * qy + w * qx; 84359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta res.y = -x * qz + y * qw + z * qx + w * qy; 84459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta res.z = x * qy - y * qx + z * qw + w * qz; 84559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta res.w = -x * qx - y * qy - z * qz + w * qw; 84659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return res; 84759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 84859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 84959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 85059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>apply</code> multiplies this quaternion by a parameter matrix 85159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * internally. 85259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 85359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param matrix 85459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the matrix to apply to this quaternion. 85559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 85659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public void apply(Matrix3f matrix) { 85759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float oldX = x, oldY = y, oldZ = z, oldW = w; 85859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta fromRotationMatrix(matrix); 85959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float tempX = x, tempY = y, tempZ = z, tempW = w; 86059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 86159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta x = oldX * tempW + oldY * tempZ - oldZ * tempY + oldW * tempX; 86259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta y = -oldX * tempZ + oldY * tempW + oldZ * tempX + oldW * tempY; 86359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta z = oldX * tempY - oldY * tempX + oldZ * tempW + oldW * tempZ; 86459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta w = -oldX * tempX - oldY * tempY - oldZ * tempZ + oldW * tempW; 86559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 86659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 86759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 86859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 86959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>fromAxes</code> creates a <code>Quaternion</code> that 87059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * represents the coordinate system defined by three axes. These axes are 87159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * assumed to be orthogonal and no error checking is applied. Thus, the user 87259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * must insure that the three axes being provided indeed represents a proper 87359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * right handed coordinate system. 87459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 87559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param axis 87659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the array containing the three vectors representing the 87759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * coordinate system. 87859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 87959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion fromAxes(Vector3f[] axis) { 88059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (axis.length != 3) { 88159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta throw new IllegalArgumentException( 88259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta "Axis array must have three elements"); 88359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 88459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return fromAxes(axis[0], axis[1], axis[2]); 88559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 88659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 88759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 88859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 88959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>fromAxes</code> creates a <code>Quaternion</code> that 89059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * represents the coordinate system defined by three axes. These axes are 89159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * assumed to be orthogonal and no error checking is applied. Thus, the user 89259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * must insure that the three axes being provided indeed represents a proper 89359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * right handed coordinate system. 89459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 89559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param xAxis vector representing the x-axis of the coordinate system. 89659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param yAxis vector representing the y-axis of the coordinate system. 89759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param zAxis vector representing the z-axis of the coordinate system. 89859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 89959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion fromAxes(Vector3f xAxis, Vector3f yAxis, Vector3f zAxis) { 90059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return fromRotationMatrix(xAxis.x, yAxis.x, zAxis.x, xAxis.y, yAxis.y, 90159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta zAxis.y, xAxis.z, yAxis.z, zAxis.z); 90259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 90359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 90459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 90559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 90659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>toAxes</code> takes in an array of three vectors. Each vector 90759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * corresponds to an axis of the coordinate system defined by the quaternion 90859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * rotation. 90959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 91059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param axis 91159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the array of vectors to be filled. 91259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 91359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public void toAxes(Vector3f axis[]) { 91459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta Matrix3f tempMat = toRotationMatrix(); 91559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta axis[0] = tempMat.getColumn(0, axis[0]); 91659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta axis[1] = tempMat.getColumn(1, axis[1]); 91759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta axis[2] = tempMat.getColumn(2, axis[2]); 91859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 91959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 92059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 92159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>mult</code> multiplies this quaternion by a parameter vector. The 92259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * result is returned as a new vector. 92359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 92459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param v 92559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the vector to multiply this quaternion by. 92659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return the new vector. 92759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 92859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Vector3f mult(Vector3f v) { 92959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return mult(v, null); 93059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 93159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 93259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 93359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>mult</code> multiplies this quaternion by a parameter vector. The 93459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * result is stored in the supplied vector 93559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 93659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param v 93759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the vector to multiply this quaternion by. 93859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return v 93959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 94059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Vector3f multLocal(Vector3f v) { 94159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float tempX, tempY; 94259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta tempX = w * w * v.x + 2 * y * w * v.z - 2 * z * w * v.y + x * x * v.x 94359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta + 2 * y * x * v.y + 2 * z * x * v.z - z * z * v.x - y * y * v.x; 94459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta tempY = 2 * x * y * v.x + y * y * v.y + 2 * z * y * v.z + 2 * w * z 94559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * v.x - z * z * v.y + w * w * v.y - 2 * x * w * v.z - x * x 94659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * v.y; 94759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta v.z = 2 * x * z * v.x + 2 * y * z * v.y + z * z * v.z - 2 * w * y * v.x 94859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta - y * y * v.z + 2 * w * x * v.y - x * x * v.z + w * w * v.z; 94959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta v.x = tempX; 95059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta v.y = tempY; 95159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return v; 95259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 95359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 95459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 95559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * Multiplies this Quaternion by the supplied quaternion. The result is 95659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * stored in this Quaternion, which is also returned for chaining. Similar 95759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * to this *= q. 95859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 95959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param q 96059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * The Quaternion to multiply this one by. 96159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return This Quaternion, after multiplication. 96259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 96359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion multLocal(Quaternion q) { 96459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float x1 = x * q.w + y * q.z - z * q.y + w * q.x; 96559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float y1 = -x * q.z + y * q.w + z * q.x + w * q.y; 96659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float z1 = x * q.y - y * q.x + z * q.w + w * q.z; 96759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta w = -x * q.x - y * q.y - z * q.z + w * q.w; 96859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta x = x1; 96959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta y = y1; 97059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta z = z1; 97159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return this; 97259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 97359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 97459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 97559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * Multiplies this Quaternion by the supplied quaternion. The result is 97659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * stored in this Quaternion, which is also returned for chaining. Similar 97759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * to this *= q. 97859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 97959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param qx - 98059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * quat x value 98159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param qy - 98259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * quat y value 98359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param qz - 98459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * quat z value 98559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param qw - 98659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * quat w value 98759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 98859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return This Quaternion, after multiplication. 98959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 99059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion multLocal(float qx, float qy, float qz, float qw) { 99159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float x1 = x * qw + y * qz - z * qy + w * qx; 99259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float y1 = -x * qz + y * qw + z * qx + w * qy; 99359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float z1 = x * qy - y * qx + z * qw + w * qz; 99459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta w = -x * qx - y * qy - z * qz + w * qw; 99559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta x = x1; 99659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta y = y1; 99759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta z = z1; 99859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return this; 99959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 100059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 100159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 100259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>mult</code> multiplies this quaternion by a parameter vector. The 100359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * result is returned as a new vector. 100459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 100559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param v 100659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the vector to multiply this quaternion by. 100759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param store 100859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the vector to store the result in. It IS safe for v and store 100959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * to be the same object. 101059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return the result vector. 101159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 101259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Vector3f mult(Vector3f v, Vector3f store) { 101359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (store == null) { 101459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta store = new Vector3f(); 101559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 101659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (v.x == 0 && v.y == 0 && v.z == 0) { 101759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta store.set(0, 0, 0); 101859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } else { 101959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float vx = v.x, vy = v.y, vz = v.z; 102059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta store.x = w * w * vx + 2 * y * w * vz - 2 * z * w * vy + x * x 102159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * vx + 2 * y * x * vy + 2 * z * x * vz - z * z * vx - y 102259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * y * vx; 102359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta store.y = 2 * x * y * vx + y * y * vy + 2 * z * y * vz + 2 * w 102459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * z * vx - z * z * vy + w * w * vy - 2 * x * w * vz - x 102559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * x * vy; 102659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta store.z = 2 * x * z * vx + 2 * y * z * vy + z * z * vz - 2 * w 102759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * y * vx - y * y * vz + 2 * w * x * vy - x * x * vz + w 102859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * w * vz; 102959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 103059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return store; 103159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 103259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 103359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 103459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>mult</code> multiplies this quaternion by a parameter scalar. The 103559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * result is returned as a new quaternion. 103659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 103759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param scalar 103859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the quaternion to multiply this quaternion by. 103959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return the new quaternion. 104059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 104159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion mult(float scalar) { 104259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return new Quaternion(scalar * x, scalar * y, scalar * z, scalar * w); 104359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 104459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 104559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 104659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>mult</code> multiplies this quaternion by a parameter scalar. The 104759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * result is stored locally. 104859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 104959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param scalar 105059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the quaternion to multiply this quaternion by. 105159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return this. 105259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 105359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion multLocal(float scalar) { 105459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta w *= scalar; 105559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta x *= scalar; 105659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta y *= scalar; 105759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta z *= scalar; 105859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return this; 105959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 106059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 106159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 106259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>dot</code> calculates and returns the dot product of this 106359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * quaternion with that of the parameter quaternion. 106459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 106559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param q 106659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the quaternion to calculate the dot product of. 106759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return the dot product of this and the parameter quaternion. 106859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 106959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public float dot(Quaternion q) { 107059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return w * q.w + x * q.x + y * q.y + z * q.z; 107159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 107259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 107359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 107459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>norm</code> returns the norm of this quaternion. This is the dot 107559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * product of this quaternion with itself. 107659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 107759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return the norm of the quaternion. 107859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 107959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public float norm() { 108059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return w * w + x * x + y * y + z * z; 108159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 108259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 108359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 108459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>normalize</code> normalizes the current <code>Quaternion</code> 108559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @deprecated The naming of this method doesn't follow convention. 108659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * Please use {@link Quaternion#normalizeLocal() } instead. 108759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 108859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta @Deprecated 108959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public void normalize() { 109059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float n = FastMath.invSqrt(norm()); 109159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta x *= n; 109259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta y *= n; 109359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta z *= n; 109459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta w *= n; 109559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 109659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 109759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 109859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>normalize</code> normalizes the current <code>Quaternion</code> 109959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 110059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public void normalizeLocal() { 110159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float n = FastMath.invSqrt(norm()); 110259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta x *= n; 110359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta y *= n; 110459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta z *= n; 110559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta w *= n; 110659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 110759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 110859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 110959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>inverse</code> returns the inverse of this quaternion as a new 111059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * quaternion. If this quaternion does not have an inverse (if its normal is 111159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 0 or less), then null is returned. 111259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 111359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return the inverse of this quaternion or null if the inverse does not 111459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * exist. 111559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 111659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion inverse() { 111759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float norm = norm(); 111859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (norm > 0.0) { 111959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float invNorm = 1.0f / norm; 112059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return new Quaternion(-x * invNorm, -y * invNorm, -z * invNorm, w 112159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * invNorm); 112259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 112359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta // return an invalid result to flag the error 112459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return null; 112559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 112659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 112759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 112859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>inverse</code> calculates the inverse of this quaternion and 112959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * returns this quaternion after it is calculated. If this quaternion does 113059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * not have an inverse (if it's norma is 0 or less), nothing happens 113159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 113259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return the inverse of this quaternion 113359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 113459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion inverseLocal() { 113559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float norm = norm(); 113659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (norm > 0.0) { 113759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float invNorm = 1.0f / norm; 113859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta x *= -invNorm; 113959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta y *= -invNorm; 114059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta z *= -invNorm; 114159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta w *= invNorm; 114259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 114359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return this; 114459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 114559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 114659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 114759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>negate</code> inverts the values of the quaternion. 114859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 114959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 115059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public void negate() { 115159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta x *= -1; 115259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta y *= -1; 115359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta z *= -1; 115459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta w *= -1; 115559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 115659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 115759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 115859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 115959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>toString</code> creates the string representation of this 116059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>Quaternion</code>. The values of the quaternion are displace (x, 116159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * y, z, w), in the following manner: <br> 116259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * (x, y, z, w) 116359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 116459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return the string representation of this object. 116559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @see java.lang.Object#toString() 116659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 116759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta @Override 116859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public String toString() { 116959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return "(" + x + ", " + y + ", " + z + ", " + w + ")"; 117059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 117159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 117259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 117359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>equals</code> determines if two quaternions are logically equal, 117459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * that is, if the values of (x, y, z, w) are the same for both quaternions. 117559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 117659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param o 117759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the object to compare for equality 117859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return true if they are equal, false otherwise. 117959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 118059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta @Override 118159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public boolean equals(Object o) { 118259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (!(o instanceof Quaternion)) { 118359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return false; 118459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 118559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 118659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (this == o) { 118759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return true; 118859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 118959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 119059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta Quaternion comp = (Quaternion) o; 119159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (Float.compare(x, comp.x) != 0) { 119259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return false; 119359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 119459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (Float.compare(y, comp.y) != 0) { 119559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return false; 119659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 119759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (Float.compare(z, comp.z) != 0) { 119859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return false; 119959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 120059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (Float.compare(w, comp.w) != 0) { 120159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return false; 120259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 120359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return true; 120459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 120559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 120659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 120759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 120859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>hashCode</code> returns the hash code value as an integer and is 120959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * supported for the benefit of hashing based collection classes such as 121059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * Hashtable, HashMap, HashSet etc. 121159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 121259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return the hashcode for this instance of Quaternion. 121359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @see java.lang.Object#hashCode() 121459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 121559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta @Override 121659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public int hashCode() { 121759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta int hash = 37; 121859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta hash = 37 * hash + Float.floatToIntBits(x); 121959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta hash = 37 * hash + Float.floatToIntBits(y); 122059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta hash = 37 * hash + Float.floatToIntBits(z); 122159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta hash = 37 * hash + Float.floatToIntBits(w); 122259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return hash; 122359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 122459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 122559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 122659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 122759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>readExternal</code> builds a quaternion from an 122859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>ObjectInput</code> object. <br> 122959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * NOTE: Used with serialization. Not to be called manually. 123059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 123159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param in 123259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the ObjectInput value to read from. 123359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @throws IOException 123459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * if the ObjectInput value has problems reading a float. 123559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @see java.io.Externalizable 123659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 123759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public void readExternal(ObjectInput in) throws IOException { 123859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta x = in.readFloat(); 123959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta y = in.readFloat(); 124059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta z = in.readFloat(); 124159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta w = in.readFloat(); 124259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 124359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 124459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 124559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>writeExternal</code> writes this quaternion out to a 124659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>ObjectOutput</code> object. NOTE: Used with serialization. Not to 124759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * be called manually. 124859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 124959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param out 125059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the object to write to. 125159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @throws IOException 125259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * if writing to the ObjectOutput fails. 125359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @see java.io.Externalizable 125459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 125559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public void writeExternal(ObjectOutput out) throws IOException { 125659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta out.writeFloat(x); 125759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta out.writeFloat(y); 125859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta out.writeFloat(z); 125959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta out.writeFloat(w); 126059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 126159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 126259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 126359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * <code>lookAt</code> is a convienence method for auto-setting the 126459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * quaternion based on a direction and an up vector. It computes 126559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * the rotation to transform the z-axis to point into 'direction' 126659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * and the y-axis to 'up'. 126759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * 126859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param direction 126959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * where to look at in terms of local coordinates 127059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param up 127159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * a vector indicating the local up direction. 127259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * (typically {0, 1, 0} in jME.) 127359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 127459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public void lookAt(Vector3f direction, Vector3f up) { 127559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta TempVars vars = TempVars.get(); 127659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta vars.vect3.set(direction).normalizeLocal(); 127759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta vars.vect1.set(up).crossLocal(direction).normalizeLocal(); 127859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta vars.vect2.set(direction).crossLocal(vars.vect1).normalizeLocal(); 127959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta fromAxes(vars.vect1, vars.vect2, vars.vect3); 128059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta vars.release(); 128159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 128259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 128359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public void write(JmeExporter e) throws IOException { 128459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta OutputCapsule cap = e.getCapsule(this); 128559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta cap.write(x, "x", 0); 128659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta cap.write(y, "y", 0); 128759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta cap.write(z, "z", 0); 128859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta cap.write(w, "w", 1); 128959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 129059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 129159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public void read(JmeImporter e) throws IOException { 129259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta InputCapsule cap = e.getCapsule(this); 129359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta x = cap.readFloat("x", 0); 129459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta y = cap.readFloat("y", 0); 129559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta z = cap.readFloat("z", 0); 129659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta w = cap.readFloat("w", 1); 129759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 129859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 129959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 130059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return A new quaternion that describes a rotation that would point you 130159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * in the exact opposite direction of this Quaternion. 130259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 130359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion opposite() { 130459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return opposite(null); 130559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 130659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 130759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 130859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * FIXME: This seems to have singularity type issues with angle == 0, possibly others such as PI. 130959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @param store 131059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * A Quaternion to store our result in. If null, a new one is 131159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * created. 131259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return The store quaternion (or a new Quaterion, if store is null) that 131359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * describes a rotation that would point you in the exact opposite 131459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * direction of this Quaternion. 131559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 131659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion opposite(Quaternion store) { 131759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta if (store == null) { 131859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta store = new Quaternion(); 131959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 132059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 132159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta Vector3f axis = new Vector3f(); 132259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta float angle = toAngleAxis(axis); 132359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 132459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta store.fromAngleAxis(FastMath.PI + angle, axis); 132559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return store; 132659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 132759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 132859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta /** 132959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * @return This Quaternion, altered to describe a rotation that would point 133059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * you in the exact opposite direction of where it is pointing 133159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta * currently. 133259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta */ 133359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion oppositeLocal() { 133459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return opposite(this); 133559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 133659b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta 133759b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta @Override 133859b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta public Quaternion clone() { 133959b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta try { 134059b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta return (Quaternion) super.clone(); 134159b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } catch (CloneNotSupportedException e) { 134259b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta throw new AssertionError(); // can not happen 134359b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 134459b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta } 134559b2e6871c65f58fdad78cd7229c292f6a177578Scott Barta} 1346