/**
 * Copyright (c) 2008-2021 Bird Dog Games, Inc.
 *
 * This file is part of Ardor3D.
 *
 * Ardor3D is free software: you can redistribute it and/or modify it
 * under the terms of its license which may be found in the accompanying
 * LICENSE file or at <https://git.io/fjRmv>.
 */

package com.ardor3d.math;

import java.io.Externalizable;
import java.io.IOException;
import java.io.ObjectInput;
import java.io.ObjectOutput;

import com.ardor3d.math.type.ReadOnlyMatrix3;
import com.ardor3d.math.type.ReadOnlyQuaternion;
import com.ardor3d.math.type.ReadOnlyVector3;
import com.ardor3d.math.util.EqualsUtil;
import com.ardor3d.math.util.HashUtil;
import com.ardor3d.math.util.MathUtils;
import com.ardor3d.util.export.InputCapsule;
import com.ardor3d.util.export.OutputCapsule;
import com.ardor3d.util.export.Savable;

/**
 * Quaternion represents a 4 value math object used in Ardor3D to describe rotations. It has the
 * advantage of being able to avoid lock by adding a 4th dimension to rotation.
 *
 * Note: some algorithms in this class were ported from Eberly, Wolfram, Game Gems and others to
 * Java.
 */
public class Quaternion implements Cloneable, Savable, Externalizable, ReadOnlyQuaternion, Poolable {

  /**
   * Used with equals method to determine if two Quaternions are close enough to be considered equal.
   */
  public static final double ALLOWED_DEVIANCE = 0.00000001;

  private static final long serialVersionUID = 1L;

  private static final ObjectPool<Quaternion> QUAT_POOL =
      ObjectPool.create(Quaternion.class, MathConstants.maxMathPoolSize);

  /**
   * x=0, y=0, z=0, w=1
   */
  public final static ReadOnlyQuaternion IDENTITY = new Quaternion(0, 0, 0, 1);

  protected double _x = 0;
  protected double _y = 0;
  protected double _z = 0;
  protected double _w = 1;

  /**
   * Constructs a new quaternion set to (0, 0, 0, 1).
   */
  public Quaternion() {
    this(Quaternion.IDENTITY);
  }

  /**
   * Constructs a new quaternion set to the (x, y, z, w) values of the given source quaternion.
   *
   * @param source
   */
  public Quaternion(final ReadOnlyQuaternion source) {
    this(source.getX(), source.getY(), source.getZ(), source.getW());
  }

  /**
   * Constructs a new quaternion set to (x, y, z, w).
   *
   * @param x
   * @param y
   * @param z
   * @param w
   */
  public Quaternion(final double x, final double y, final double z, final double w) {
    _x = x;
    _y = y;
    _z = z;
    _w = w;
  }

  @Override
  public double getX() { return _x; }

  @Override
  public double getY() { return _y; }

  @Override
  public double getZ() { return _z; }

  @Override
  public double getW() { return _w; }

  @Override
  public float getXf() { return (float) _x; }

  @Override
  public float getYf() { return (float) _y; }

  @Override
  public float getZf() { return (float) _z; }

  @Override
  public float getWf() { return (float) _w; }

  /**
   * Stores the double values of this quaternion in the given double array as (x,y,z,w).
   *
   * @param store
   *          The array in which to store the values of this quaternion. If null, a new double[4]
   *          array is created.
   * @return the double array
   * @throws ArrayIndexOutOfBoundsException
   *           if store is not null and is not at least length 4
   */
  @Override
  public double[] toArray(final double[] store) {
    double[] result = store;
    if (result == null) {
      result = new double[4];
    }
    result[3] = getW();
    result[2] = getZ();
    result[1] = getY();
    result[0] = getX();
    return result;
  }

  /**
   * Sets the x component of this quaternion to the given double value.
   *
   * @param x
   */
  public void setX(final double x) { _x = x; }

  /**
   * Sets the y component of this quaternion to the given double value.
   *
   * @param y
   */
  public void setY(final double y) { _y = y; }

  /**
   * Sets the z component of this quaternion to the given double value.
   *
   * @param z
   */
  public void setZ(final double z) { _z = z; }

  /**
   * Sets the w component of this quaternion to the given double value.
   *
   * @param w
   */
  public void setW(final double w) { _w = w; }

  /**
   * Sets the value of this quaternion to (x, y, z, w)
   *
   * @param x
   * @param y
   * @param z
   * @param w
   * @return this quaternion for chaining
   */
  public Quaternion set(final double x, final double y, final double z, final double w) {
    setX(x);
    setY(y);
    setZ(z);
    setW(w);
    return this;
  }

  /**
   * Sets the value of this quaternion to the (x, y, z, w) values of the provided source quaternion.
   *
   * @param source
   * @return this quaternion for chaining
   * @throws NullPointerException
   *           if source is null.
   */
  public Quaternion set(final ReadOnlyQuaternion source) {
    setX(source.getX());
    setY(source.getY());
    setZ(source.getZ());
    setW(source.getW());
    return this;
  }

  /**
   * Updates this quaternion from the given Euler rotation angles, applied in the given order:
   * heading, attitude, bank.
   *
   * @param angles
   *          the Euler angles of rotation (in radians) stored as heading, attitude, and bank.
   * @return this quaternion for chaining
   * @throws ArrayIndexOutOfBoundsException
   *           if angles is less than length 3
   * @throws NullPointerException
   *           if angles is null.
   */
  public Quaternion fromEulerAngles(final double[] angles) {
    return fromEulerAngles(angles[0], angles[1], angles[2]);
  }

  /**
   * Updates this quaternion from the given Euler rotation angles, applied in the given order:
   * heading, attitude, bank.
   *
   * @param heading
   *          the Euler heading angle in radians. (rotation about the y axis)
   * @param attitude
   *          the Euler attitude angle in radians. (rotation about the z axis)
   * @param bank
   *          the Euler bank angle in radians. (rotation about the x axis)
   * @return this quaternion for chaining
   * @see <a href=
   *      "http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm">euclideanspace.com-eulerToQuaternion</a>
   */
  public Quaternion fromEulerAngles(final double heading, final double attitude, final double bank) {
    double angle = heading * 0.5;
    final double sinHeading = Math.sin(angle);
    final double cosHeading = Math.cos(angle);
    angle = attitude * 0.5;
    final double sinAttitude = Math.sin(angle);
    final double cosAttitude = Math.cos(angle);
    angle = bank * 0.5;
    final double sinBank = Math.sin(angle);
    final double cosBank = Math.cos(angle);

    // variables used to reduce multiplication calls.
    final double cosHeadingXcosAttitude = cosHeading * cosAttitude;
    final double sinHeadingXsinAttitude = sinHeading * sinAttitude;
    final double cosHeadingXsinAttitude = cosHeading * sinAttitude;
    final double sinHeadingXcosAttitude = sinHeading * cosAttitude;

    final double w = cosHeadingXcosAttitude * cosBank - sinHeadingXsinAttitude * sinBank;
    final double x = cosHeadingXcosAttitude * sinBank + sinHeadingXsinAttitude * cosBank;
    final double y = sinHeadingXcosAttitude * cosBank + cosHeadingXsinAttitude * sinBank;
    final double z = cosHeadingXsinAttitude * cosBank - sinHeadingXcosAttitude * sinBank;

    set(x, y, z, w);

    return normalizeLocal();
  }

  /**
   * Converts this quaternion to Euler rotation angles in radians (heading, attitude, bank).
   *
   * @param store
   *          the double[] array to store the computed angles in. If null, a new double[] will be
   *          created
   * @return the double[] array, filled with heading, attitude and bank in that order..
   * @throws ArrayIndexOutOfBoundsException
   *           if non-null store is not at least length 3
   * @see <a href=
   *      "http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm">euclideanspace.com-quaternionToEuler</a>
   * @see #fromEulerAngles(double, double, double)
   */
  @Override
  public double[] toEulerAngles(final double[] store) {
    double[] result = store;
    if (result == null) {
      result = new double[3];
    } else if (result.length < 3) {
      throw new ArrayIndexOutOfBoundsException("store array must have at least three elements");
    }

    final double sqw = getW() * getW();
    final double sqx = getX() * getX();
    final double sqy = getY() * getY();
    final double sqz = getZ() * getZ();
    final double unit = sqx + sqy + sqz + sqw; // if normalized is one, otherwise
    // is correction factor
    final double test = getX() * getY() + getZ() * getW();
    if (test > 0.499 * unit) { // singularity at north pole
      result[0] = 2 * Math.atan2(getX(), getW());
      result[1] = MathUtils.HALF_PI;
      result[2] = 0;
    } else if (test < -0.499 * unit) { // singularity at south pole
      result[0] = -2 * Math.atan2(getX(), getW());
      result[1] = -MathUtils.HALF_PI;
      result[2] = 0;
    } else {
      result[0] = Math.atan2(2 * getY() * getW() - 2 * getX() * getZ(), sqx - sqy - sqz + sqw);
      result[1] = Math.asin(2 * test / unit);
      result[2] = Math.atan2(2 * getX() * getW() - 2 * getY() * getZ(), -sqx + sqy - sqz + sqw);
    }
    return result;
  }

  /**
   * Sets the value of this quaternion to the rotation described by the given matrix.
   *
   * @param matrix
   * @return this quaternion for chaining
   * @throws NullPointerException
   *           if matrix is null.
   */
  public Quaternion fromRotationMatrix(final ReadOnlyMatrix3 matrix) {
    return fromRotationMatrix(matrix.getM00(), matrix.getM01(), matrix.getM02(), matrix.getM10(), matrix.getM11(),
        matrix.getM12(), matrix.getM20(), matrix.getM21(), matrix.getM22());
  }

  /**
   * Sets the value of this quaternion to the rotation described by the given matrix values.
   *
   * @param m00
   * @param m01
   * @param m02
   * @param m10
   * @param m11
   * @param m12
   * @param m20
   * @param m21
   * @param m22
   * @return this quaternion for chaining
   */
  public Quaternion fromRotationMatrix(final double m00, final double m01, final double m02, final double m10,
      final double m11, final double m12, final double m20, final double m21, final double m22) {
    // Uses the Graphics Gems code, from
    // ftp://ftp.cis.upenn.edu/pub/graphics/shoemake/quatut.ps.Z
    // *NOT* the "Matrix and Quaternions FAQ", which has errors!

    // the trace is the sum of the diagonal elements; see
    // http://mathworld.wolfram.com/MatrixTrace.html
    final double t = m00 + m11 + m22;

    // we protect the division by s by ensuring that s>=1
    double x, y, z, w;
    if (t >= 0) { // |w| >= .5
      double s = Math.sqrt(t + 1); // |s|>=1 ...
      w = 0.5 * s;
      s = 0.5 / s; // so this division isn't bad
      x = (m21 - m12) * s;
      y = (m02 - m20) * s;
      z = (m10 - m01) * s;
    } else if (m00 > m11 && m00 > m22) {
      double s = Math.sqrt(1.0 + m00 - m11 - m22); // |s|>=1
      x = s * 0.5; // |x| >= .5
      s = 0.5 / s;
      y = (m10 + m01) * s;
      z = (m02 + m20) * s;
      w = (m21 - m12) * s;
    } else if (m11 > m22) {
      double s = Math.sqrt(1.0 + m11 - m00 - m22); // |s|>=1
      y = s * 0.5; // |y| >= .5
      s = 0.5 / s;
      x = (m10 + m01) * s;
      z = (m21 + m12) * s;
      w = (m02 - m20) * s;
    } else {
      double s = Math.sqrt(1.0 + m22 - m00 - m11); // |s|>=1
      z = s * 0.5; // |z| >= .5
      s = 0.5 / s;
      x = (m02 + m20) * s;
      y = (m21 + m12) * s;
      w = (m10 - m01) * s;
    }

    return set(x, y, z, w);
  }

  /**
   * @param store
   *          the matrix to store our result in. If null, a new matrix is created.
   * @return the rotation matrix representation of this quaternion (normalized)
   *
   *         if store is not null and is read only.
   */
  @Override
  public Matrix3 toRotationMatrix(final Matrix3 store) {
    Matrix3 result = store;
    if (result == null) {
      result = new Matrix3();
    }

    final double norm = magnitudeSquared();
    final double s = norm > 0.0 ? 2.0 / norm : 0.0;

    // compute xs/ys/zs first to save 6 multiplications, since xs/ys/zs
    // will be used 2-4 times each.
    final double xs = getX() * s;
    final double ys = getY() * s;
    final double zs = getZ() * s;
    final double xx = getX() * xs;
    final double xy = getX() * ys;
    final double xz = getX() * zs;
    final double xw = getW() * xs;
    final double yy = getY() * ys;
    final double yz = getY() * zs;
    final double yw = getW() * ys;
    final double zz = getZ() * zs;
    final double zw = getW() * zs;

    // using s=2/norm (instead of 1/norm) saves 9 multiplications by 2 here
    result.setM00(1.0 - (yy + zz));
    result.setM01(xy - zw);
    result.setM02(xz + yw);
    result.setM10(xy + zw);
    result.setM11(1.0 - (xx + zz));
    result.setM12(yz - xw);
    result.setM20(xz - yw);
    result.setM21(yz + xw);
    result.setM22(1.0 - (xx + yy));

    return result;
  }

  /**
   * @param store
   *          the matrix to store our result in. If null, a new matrix is created.
   * @return the rotation matrix representation of this quaternion (normalized)
   */
  @Override
  public Matrix4 toRotationMatrix(final Matrix4 store) {
    Matrix4 result = store;
    if (result == null) {
      result = new Matrix4();
    }

    final double norm = magnitudeSquared();
    final double s = norm == 1.0 ? 2.0 : norm > 0.0 ? 2.0 / norm : 0;

    // compute xs/ys/zs first to save 6 multiplications, since xs/ys/zs
    // will be used 2-4 times each.
    final double xs = getX() * s;
    final double ys = getY() * s;
    final double zs = getZ() * s;
    final double xx = getX() * xs;
    final double xy = getX() * ys;
    final double xz = getX() * zs;
    final double xw = getW() * xs;
    final double yy = getY() * ys;
    final double yz = getY() * zs;
    final double yw = getW() * ys;
    final double zz = getZ() * zs;
    final double zw = getW() * zs;

    // using s=2/norm (instead of 1/norm) saves 9 multiplications by 2 here
    result.setM00(1.0 - (yy + zz));
    result.setM01(xy - zw);
    result.setM02(xz + yw);
    result.setM10(xy + zw);
    result.setM11(1.0 - (xx + zz));
    result.setM12(yz - xw);
    result.setM20(xz - yw);
    result.setM21(yz + xw);
    result.setM22(1.0 - (xx + yy));

    return result;
  }

  /**
   * @param index
   *          the 3x3 rotation matrix column to retrieve from this quaternion (normalized). Must be
   *          between 0 and 2.
   * @param store
   *          the vector object to store the result in. if null, a new one is created.
   * @return the column specified by the index.
   */
  @Override
  public Vector3 getRotationColumn(final int index, final Vector3 store) {
    Vector3 result = store;
    if (result == null) {
      result = new Vector3();
    }

    final double norm = magnitudeSquared();
    final double s = norm == 1.0 ? 2.0 : norm > 0.0 ? 2.0 / norm : 0;

    // compute xs/ys/zs first to save 6 multiplications, since xs/ys/zs
    // will be used 2-4 times each.
    final double xs = getX() * s;
    final double ys = getY() * s;
    final double zs = getZ() * s;
    final double xx = getX() * xs;
    final double xy = getX() * ys;
    final double xz = getX() * zs;
    final double xw = getW() * xs;
    final double yy = getY() * ys;
    final double yz = getY() * zs;
    final double yw = getW() * ys;
    final double zz = getZ() * zs;
    final double zw = getW() * zs;

    // using s=2/norm (instead of 1/norm) saves 3 multiplications by 2 here
    double x, y, z;
    switch (index) {
      case 0:
        x = 1.0 - (yy + zz);
        y = xy + zw;
        z = xz - yw;
        break;
      case 1:
        x = xy - zw;
        y = 1.0 - (xx + zz);
        z = yz + xw;
        break;
      case 2:
        x = xz + yw;
        y = yz - xw;
        z = 1.0 - (xx + yy);
        break;
      default:
        throw new IllegalArgumentException("Invalid column index. " + index);
    }

    return result.set(x, y, z);
  }

  /**
   * Sets the values of this quaternion to the values represented by a given angle and axis of
   * rotation. Note that this method creates an object, so use fromAngleNormalAxis if your axis is
   * already normalized. If axis == 0,0,0 the quaternion is set to identity.
   *
   * @param angle
   *          the angle to rotate (in radians).
   * @param axis
   *          the axis of rotation.
   * @return this quaternion for chaining
   * @throws NullPointerException
   *           if axis is null
   */
  public Quaternion fromAngleAxis(final double angle, final ReadOnlyVector3 axis) {
    final Vector3 temp = Vector3.fetchTempInstance();
    final Quaternion quat = fromAngleNormalAxis(angle, axis.normalize(temp));
    Vector3.releaseTempInstance(temp);
    return quat;
  }

  /**
   * Sets the values of this quaternion to the values represented by a given angle and unit length
   * axis of rotation. If axis == 0,0,0 the quaternion is set to identity.
   *
   * @param angle
   *          the angle to rotate (in radians).
   * @param axis
   *          the axis of rotation (already normalized - unit length).
   * @throws NullPointerException
   *           if axis is null
   */
  public Quaternion fromAngleNormalAxis(final double angle, final ReadOnlyVector3 axis) {
    if (axis.equals(Vector3.ZERO)) {
      return setIdentity();
    }

    final double halfAngle = 0.5 * angle;
    final double sin = Math.sin(halfAngle);
    final double w = Math.cos(halfAngle);
    final double x = sin * axis.getX();
    final double y = sin * axis.getY();
    final double z = sin * axis.getZ();
    return set(x, y, z, w);
  }

  /**
   * Returns the rotation angle represented by this quaternion. If a non-null vector is provided, the
   * axis of rotation is stored in that vector as well.
   *
   * @param axisStore
   *          the object we'll store the computed axis in. If null, no computations are done to
   *          determine axis.
   * @return the angle of rotation in radians.
   */
  @Override
  public double toAngleAxis(final Vector3 axisStore) {
    final double sqrLength = getX() * getX() + getY() * getY() + getZ() * getZ();
    double angle;
    if (Math.abs(sqrLength) <= MathUtils.EPSILON) { // length is ~0
      angle = 0.0;
      if (axisStore != null) {
        axisStore.setX(1.0);
        axisStore.setY(0.0);
        axisStore.setZ(0.0);
      }
    } else {
      angle = 2.0 * Math.acos(getW());
      if (axisStore != null) {
        final double invLength = 1.0 / Math.sqrt(sqrLength);
        axisStore.setX(getX() * invLength);
        axisStore.setY(getY() * invLength);
        axisStore.setZ(getZ() * invLength);
      }
    }

    return angle;
  }

  /**
   * Sets this quaternion to that which will rotate vector "from" into vector "to". from and to do not
   * have to be the same length.
   *
   * @param from
   *          the source vector to rotate
   * @param to
   *          the destination vector into which to rotate the source vector
   * @return this quaternion for chaining
   */
  public Quaternion fromVectorToVector(final ReadOnlyVector3 from, final ReadOnlyVector3 to) {
    final ReadOnlyVector3 a = from;
    final ReadOnlyVector3 b = to;
    final double factor = a.length() * b.length();
    if (Math.abs(factor) > MathUtils.EPSILON) {
      // Vectors have length > 0
      final Vector3 pivotVector = Vector3.fetchTempInstance();
      try {
        final double dot = a.dot(b) / factor;
        final double theta = Math.acos(Math.max(-1.0, Math.min(dot, 1.0)));
        a.cross(b, pivotVector);
        if (dot < 0.0 && pivotVector.length() < MathUtils.EPSILON) {
          // Vectors parallel and opposite direction, therefore a rotation of 180 degrees about any vector
          // perpendicular to this vector will rotate vector a onto vector b.
          //
          // The following guarantees the dot-product will be 0.0.
          int dominantIndex;
          if (Math.abs(a.getX()) > Math.abs(a.getY())) {
            if (Math.abs(a.getX()) > Math.abs(a.getZ())) {
              dominantIndex = 0;
            } else {
              dominantIndex = 2;
            }
          } else {
            if (Math.abs(a.getY()) > Math.abs(a.getZ())) {
              dominantIndex = 1;
            } else {
              dominantIndex = 2;
            }
          }
          pivotVector.setValue(dominantIndex, -a.getValue((dominantIndex + 1) % 3));
          pivotVector.setValue((dominantIndex + 1) % 3, a.getValue(dominantIndex));
          pivotVector.setValue((dominantIndex + 2) % 3, 0f);
        }
        return fromAngleAxis(theta, pivotVector);
      } finally {
        Vector3.releaseTempInstance(pivotVector);
      }
    } else {
      return setIdentity();
    }
  }

  /**
   * @param store
   *          the Quaternion to store the result in. if null, a new one is created.
   * @return a new quaternion that represents a unit length version of this Quaternion.
   */
  @Override
  public Quaternion normalize(final Quaternion store) {
    Quaternion result = store;
    if (result == null) {
      result = new Quaternion();
    }

    final double n = 1.0 / magnitude();
    final double x = getX() * n;
    final double y = getY() * n;
    final double z = getZ() * n;
    final double w = getW() * n;
    return result.set(x, y, z, w);
  }

  /**
   * @return this quaternion, modified to be unit length, for chaining.
   */
  public Quaternion normalizeLocal() {
    final double n = 1.0 / magnitude();
    final double x = getX() * n;
    final double y = getY() * n;
    final double z = getZ() * n;
    final double w = getW() * n;
    return set(x, y, z, w);
  }

  /**
   * Calculates the <i>multiplicative inverse</i> <code>Q<sup>-1</sup></code> of this quaternion
   * <code>Q</code> such that <code>QQ<sup>-1</sup> = [0,0,0,1]</code> (the identity quaternion). Note
   * that for unit quaternions, a quaternion's inverse is equal to its (far easier to calculate)
   * conjugate.
   *
   * @param store
   *          the <code>Quaternion</code> to store the result in. If <code>null</code>, a new one is
   *          created.
   * @see #conjugate(Quaternion)
   * @return the multiplicative inverse of this quaternion.
   */
  public Quaternion invert(Quaternion store) {
    if (store == null) {
      store = new Quaternion();
    }
    final double magnitudeSQ = magnitudeSquared();
    conjugate(store);
    if (Math.abs(1.0 - magnitudeSQ) <= MathUtils.EPSILON) {
      return store;
    } else {
      return store.multiplyLocal(1.0 / magnitudeSQ);
    }
  }

  /**
   * Locally sets this quaternion <code>Q</code> to its <i>multiplicative inverse</i>
   * <code>Q<sup>-1</sup></code> such that <code>QQ<sup>-1</sup> = [0,0,0,1]</code> (the identity
   * quaternion). Note that for unit quaternions, a quaternion's inverse is equal to its (far easier
   * to calculate) conjugate.
   *
   * @see #conjugate(Quaternion)
   *
   * @return this <code>Quaternion</code> for chaining.
   */
  public Quaternion invertLocal() {
    final double magnitudeSQ = magnitudeSquared();
    conjugateLocal();
    if (Math.abs(1.0 - magnitudeSQ) <= MathUtils.EPSILON) {
      return this;
    } else {
      return multiplyLocal(1.0 / magnitudeSquared());
    }
  }

  /**
   * Creates a new quaternion that is the conjugate <code>[-x, -y, -z, w]</code> of this quaternion.
   *
   * @param store
   *          the <code>Quaternion</code> to store the result in. If <code>null</code>, a new one is
   *          created.
   * @return the conjugate to this quaternion.
   */
  @Override
  public Quaternion conjugate(Quaternion store) {
    if (store == null) {
      store = new Quaternion();
    }
    return store.set(-getX(), -getY(), -getZ(), getW());
  }

  /**
   * Internally sets this quaternion to its conjugate <code>[-x, -y, -z, w]</code>.
   *
   * @return this <code>Quaternion</code> for chaining.
   */
  public Quaternion conjugateLocal() {
    return set(-getX(), -getY(), -getZ(), getW());
  }

  /**
   * Adds this quaternion to another and places the result in the given store.
   *
   * @param quat
   * @param store
   *          the Quaternion to store the result in. if null, a new one is created.
   * @return a quaternion representing the fields of this quaternion added to those of the given
   *         quaternion.
   */
  @Override
  public Quaternion add(final ReadOnlyQuaternion quat, final Quaternion store) {
    Quaternion result = store;
    if (result == null) {
      result = new Quaternion();
    }

    return result.set(getX() + quat.getX(), getY() + quat.getY(), getZ() + quat.getZ(), getW() + quat.getW());
  }

  /**
   * Internally increments the fields of this quaternion with the field values of the given
   * quaternion.
   *
   * @param quat
   * @return this quaternion for chaining
   */
  public Quaternion addLocal(final ReadOnlyQuaternion quat) {
    setX(getX() + quat.getX());
    setY(getY() + quat.getY());
    setZ(getZ() + quat.getZ());
    setW(getW() + quat.getW());
    return this;
  }

  /**
   * @param quat
   * @param store
   *          the Quaternion to store the result in. if null, a new one is created.
   * @return a quaternion representing the fields of this quaternion subtracted from those of the
   *         given quaternion.
   */
  @Override
  public Quaternion subtract(final ReadOnlyQuaternion quat, final Quaternion store) {
    Quaternion result = store;
    if (result == null) {
      result = new Quaternion();
    }

    return result.set(getX() - quat.getX(), getY() - quat.getY(), getZ() - quat.getZ(), getW() - quat.getW());
  }

  /**
   * Internally decrements the fields of this quaternion by the field values of the given quaternion.
   *
   * @param quat
   * @return this quaternion for chaining.
   */
  public Quaternion subtractLocal(final ReadOnlyQuaternion quat) {
    setX(getX() - quat.getX());
    setY(getY() - quat.getY());
    setZ(getZ() - quat.getZ());
    setW(getW() - quat.getW());
    return this;
  }

  /**
   * Multiplies each value of this quaternion by the given scalar value.
   *
   * @param scalar
   *          the quaternion to multiply this quaternion by.
   * @param store
   *          the Quaternion to store the result in. if null, a new one is created.
   * @return the resulting quaternion.
   */
  @Override
  public Quaternion multiply(final double scalar, final Quaternion store) {
    Quaternion result = store;
    if (result == null) {
      result = new Quaternion();
    }

    return result.set(scalar * getX(), scalar * getY(), scalar * getZ(), scalar * getW());
  }

  /**
   * Multiplies each value of this quaternion by the given scalar value. The result is stored in this
   * quaternion.
   *
   * @param scalar
   *          the quaternion to multiply this quaternion by.
   * @return this quaternion for chaining.
   */
  public Quaternion multiplyLocal(final double scalar) {
    setX(getX() * scalar);
    setY(getY() * scalar);
    setZ(getZ() * scalar);
    setW(getW() * scalar);
    return this;
  }

  /**
   * Multiplies this quaternion by the supplied quaternion. The result is stored in the given store
   * quaternion or a new quaternion if store is null.
   *
   * It IS safe for quat and store to be the same object.
   *
   * @param quat
   *          the quaternion to multiply this quaternion by.
   * @param store
   *          the quaternion to store the result in.
   * @return the new quaternion.
   *
   *         if the given store is read only.
   */
  @Override
  public Quaternion multiply(final ReadOnlyQuaternion quat, Quaternion store) {
    if (store == null) {
      store = new Quaternion();
    }
    final double x = getX() * quat.getW() + getY() * quat.getZ() - getZ() * quat.getY() + getW() * quat.getX();
    final double y = -getX() * quat.getZ() + getY() * quat.getW() + getZ() * quat.getX() + getW() * quat.getY();
    final double z = getX() * quat.getY() - getY() * quat.getX() + getZ() * quat.getW() + getW() * quat.getZ();
    final double w = -getX() * quat.getX() - getY() * quat.getY() - getZ() * quat.getZ() + getW() * quat.getW();
    return store.set(x, y, z, w);
  }

  /**
   * Multiplies this quaternion by the supplied quaternion. The result is stored locally.
   *
   * @param quat
   *          The Quaternion to multiply this one by.
   * @return this quaternion for chaining
   * @throws NullPointerException
   *           if quat is null.
   */
  public Quaternion multiplyLocal(final ReadOnlyQuaternion quat) {
    return multiplyLocal(quat.getX(), quat.getY(), quat.getZ(), quat.getW());
  }

  /**
   * Multiplies this quaternion by the supplied matrix. The result is stored locally.
   *
   * @param matrix
   *          the matrix to apply to this quaternion.
   * @return this quaternion for chaining
   * @throws NullPointerException
   *           if matrix is null.
   */
  public Quaternion multiplyLocal(final ReadOnlyMatrix3 matrix) {
    final double oldX = getX(), oldY = getY(), oldZ = getZ(), oldW = getW();
    fromRotationMatrix(matrix);
    final double tempX = getX(), tempY = getY(), tempZ = getZ(), tempW = getW();

    final double x = oldX * tempW + oldY * tempZ - oldZ * tempY + oldW * tempX;
    final double y = -oldX * tempZ + oldY * tempW + oldZ * tempX + oldW * tempY;
    final double z = oldX * tempY - oldY * tempX + oldZ * tempW + oldW * tempZ;
    final double w = -oldX * tempX - oldY * tempY - oldZ * tempZ + oldW * tempW;
    return set(x, y, z, w);
  }

  /**
   * Multiplies this quaternion by the supplied quaternion values. The result is stored locally.
   *
   * @param qx
   * @param qy
   * @param qz
   * @param qw
   * @return this quaternion for chaining
   */
  public Quaternion multiplyLocal(final double qx, final double qy, final double qz, final double qw) {
    final double x = getX() * qw + getY() * qz - getZ() * qy + getW() * qx;
    final double y = -getX() * qz + getY() * qw + getZ() * qx + getW() * qy;
    final double z = getX() * qy - getY() * qx + getZ() * qw + getW() * qz;
    final double w = -getX() * qx - getY() * qy - getZ() * qz + getW() * qw;
    return set(x, y, z, w);
  }

  /**
   * Multiply this quaternion by a rotational quaternion made from the given angle and axis. The axis
   * must be a normalized vector.
   *
   * @param angle
   *          in radians
   * @param x
   *          x coord of rotation axis
   * @param y
   *          y coord of rotation axis
   * @param z
   *          z coord of rotation axis
   * @return this quaternion for chaining.
   */
  public Quaternion applyRotation(final double angle, final double x, final double y, final double z) {
    if (x == 0 && y == 0 && z == 0) {
      // no change
      return this;
    }

    final double halfAngle = 0.5 * angle;
    final double sin = Math.sin(halfAngle);
    final double qw = Math.cos(halfAngle);
    final double qx = sin * x;
    final double qy = sin * y;
    final double qz = sin * z;

    final double newX = getX() * qw + getY() * qz - getZ() * qy + getW() * qx;
    final double newY = -getX() * qz + getY() * qw + getZ() * qx + getW() * qy;
    final double newZ = getX() * qy - getY() * qx + getZ() * qw + getW() * qz;
    final double newW = -getX() * qx - getY() * qy - getZ() * qz + getW() * qw;

    return set(newX, newY, newZ, newW);
  }

  /**
   * Apply rotation around X
   *
   * @param angle
   *          in radians
   * @return this quaternion for chaining.
   */
  public Quaternion applyRotationX(final double angle) {
    final double halfAngle = 0.5 * angle;
    final double sin = Math.sin(halfAngle);
    final double cos = Math.cos(halfAngle);

    final double newX = getX() * cos + getW() * sin;
    final double newY = getY() * cos + getZ() * sin;
    final double newZ = -getY() * sin + getZ() * cos;
    final double newW = -getX() * sin + getW() * cos;

    return set(newX, newY, newZ, newW);
  }

  /**
   * Apply rotation around Y
   *
   * @param angle
   *          in radians
   * @return this quaternion for chaining.
   */
  public Quaternion applyRotationY(final double angle) {
    final double halfAngle = 0.5 * angle;
    final double sin = Math.sin(halfAngle);
    final double cos = Math.cos(halfAngle);

    final double newX = getX() * cos - getZ() * sin;
    final double newY = getY() * cos + getW() * sin;
    final double newZ = getX() * sin + getZ() * cos;
    final double newW = -getY() * sin + getW() * cos;

    return set(newX, newY, newZ, newW);
  }

  /**
   * Apply rotation around Z
   *
   * @param angle
   *          in radians
   * @return this quaternion for chaining.
   */
  public Quaternion applyRotationZ(final double angle) {
    final double halfAngle = 0.5 * angle;
    final double sin = Math.sin(halfAngle);
    final double cos = Math.cos(halfAngle);

    final double newX = getX() * cos + getY() * sin;
    final double newY = -getX() * sin + getY() * cos;
    final double newZ = getZ() * cos + getW() * sin;
    final double newW = -getZ() * sin + getW() * cos;

    return set(newX, newY, newZ, newW);
  }

  /**
   * Rotates the given vector by this quaternion. If supplied, the result is stored into the supplied
   * "store" vector.
   *
   * @param vec
   *          the vector to multiply this quaternion by.
   * @param store
   *          the vector to store the result in. If store is null, a new vector is created. Note that
   *          it IS safe for vec and store to be the same object.
   * @return the store vector, or a new vector if store is null.
   * @throws NullPointerException
   *           if vec is null
   *
   *           if the given store is read only.
   */
  @Override
  public Vector3 apply(final ReadOnlyVector3 vec, Vector3 store) {
    if (store == null) {
      store = new Vector3();
    }
    if (vec.equals(Vector3.ZERO)) {
      store.set(0, 0, 0);
    } else {
      final double x = getW() * getW() * vec.getX() + 2 * getY() * getW() * vec.getZ()
          - 2 * getZ() * getW() * vec.getY() + getX() * getX() * vec.getX() + 2 * getY() * getX() * vec.getY()
          + 2 * getZ() * getX() * vec.getZ() - getZ() * getZ() * vec.getX() - getY() * getY() * vec.getX();
      final double y = 2 * getX() * getY() * vec.getX() + getY() * getY() * vec.getY()
          + 2 * getZ() * getY() * vec.getZ() + 2 * getW() * getZ() * vec.getX() - getZ() * getZ() * vec.getY()
          + getW() * getW() * vec.getY() - 2 * getX() * getW() * vec.getZ() - getX() * getX() * vec.getY();
      final double z = 2 * getX() * getZ() * vec.getX() + 2 * getY() * getZ() * vec.getY()
          + getZ() * getZ() * vec.getZ() - 2 * getW() * getY() * vec.getX() - getY() * getY() * vec.getZ()
          + 2 * getW() * getX() * vec.getY() - getX() * getX() * vec.getZ() + getW() * getW() * vec.getZ();
      store.set(x, y, z);
    }
    return store;
  }

  /**
   * Updates this quaternion to represent a rotation formed by the given three axes. These axes are
   * assumed to be orthogonal and no error checking is applied. It is the user's job to insure that
   * the three axes being provided indeed represent a proper right handed coordinate system.
   *
   * @param xAxis
   *          vector representing the x-axis of the coordinate system.
   * @param yAxis
   *          vector representing the y-axis of the coordinate system.
   * @param zAxis
   *          vector representing the z-axis of the coordinate system.
   * @return this quaternion for chaining
   */
  public Quaternion fromAxes(final ReadOnlyVector3 xAxis, final ReadOnlyVector3 yAxis, final ReadOnlyVector3 zAxis) {
    return fromRotationMatrix(xAxis.getX(), yAxis.getX(), zAxis.getX(), xAxis.getY(), yAxis.getY(), zAxis.getY(),
        xAxis.getZ(), yAxis.getZ(), zAxis.getZ());
  }

  /**
   * Converts this quaternion to a rotation matrix and then extracts rotation axes.
   *
   * @param axes
   *          the array of vectors to be filled.
   * @throws ArrayIndexOutOfBoundsException
   *           if the given axes array is smaller than 3 elements.
   * @return the axes
   */
  @Override
  public Vector3[] toAxes(final Vector3[] axes) {
    final Matrix3 tempMat = toRotationMatrix(Matrix3.fetchTempInstance());
    axes[2] = tempMat.getColumn(2, axes[2]);
    axes[1] = tempMat.getColumn(1, axes[1]);
    axes[0] = tempMat.getColumn(0, axes[0]);
    Matrix3.releaseTempInstance(tempMat);
    return axes;
  }

  /**
   * Does a spherical linear interpolation between this quaternion and the given end quaternion by the
   * given change amount.
   *
   * @param endQuat
   * @param changeAmnt
   * @param store
   *          the quaternion to store the result in for return. If null, a new quaternion object is
   *          created and returned.
   * @return a new quaternion containing the result.
   */
  @Override
  public Quaternion slerp(final ReadOnlyQuaternion endQuat, final double changeAmnt, final Quaternion store) {
    return Quaternion.slerp(this, endQuat, changeAmnt, store);
  }

  /**
   * Does a spherical linear interpolation between this quaternion and the given end quaternion by the
   * given change amount. Stores the results locally in this quaternion.
   *
   * @param endQuat
   * @param changeAmnt
   * @return this quaternion for chaining.
   */
  public Quaternion slerpLocal(final ReadOnlyQuaternion endQuat, final double changeAmnt) {
    return slerpLocal(this, endQuat, changeAmnt);
  }

  /**
   * Does a spherical linear interpolation between the given start and end quaternions by the given
   * change amount. Returns the result as a new quaternion.
   *
   * @param startQuat
   * @param endQuat
   * @param changeAmnt
   * @param store
   *          the quaternion to store the result in for return. If null, a new quaternion object is
   *          created and returned.
   * @return the new quaternion
   */
  public static Quaternion slerp(final ReadOnlyQuaternion startQuat, final ReadOnlyQuaternion endQuat,
      final double changeAmnt, final Quaternion store) {
    Quaternion result = store;
    if (result == null) {
      result = new Quaternion();
    }

    // check for weighting at either extreme
    if (changeAmnt == 0.0) {
      return result.set(startQuat);
    } else if (changeAmnt == 1.0) {
      return result.set(endQuat);
    }

    result.set(endQuat);
    // Check for equality and skip operation.
    if (startQuat.equals(result)) {
      return result;
    }

    double dotP = startQuat.dot(result);

    if (dotP < 0.0) {
      // Negate the second quaternion and the result of the dot product
      result.multiplyLocal(-1);
      dotP = -dotP;
    }

    // Set the first and second scale for the interpolation
    double scale0 = 1 - changeAmnt;
    double scale1 = changeAmnt;

    // Check if the angle between the 2 quaternions was big enough to
    // warrant such calculations
    if (1 - dotP > 0.1) {// Get the angle between the 2 quaternions,
      // and then store the sin() of that angle
      final double theta = Math.acos(dotP);
      final double invSinTheta = 1f / Math.sin(theta);

      // Calculate the scale for q1 and q2, according to the angle and
      // it's sine value
      scale0 = Math.sin((1 - changeAmnt) * theta) * invSinTheta;
      scale1 = Math.sin(changeAmnt * theta) * invSinTheta;
    }

    // Calculate the x, y, z and w values for the quaternion by using a
    // special form of linear interpolation for quaternions.
    final double x = scale0 * startQuat.getX() + scale1 * result.getX();
    final double y = scale0 * startQuat.getY() + scale1 * result.getY();
    final double z = scale0 * startQuat.getZ() + scale1 * result.getZ();
    final double w = scale0 * startQuat.getW() + scale1 * result.getW();

    // Return the interpolated quaternion
    return result.set(x, y, z, w);
  }

  /**
   * Does a spherical linear interpolation between the given start and end quaternions by the given
   * change amount. Stores the result locally.
   *
   * @param startQuat
   * @param endQuat
   * @param changeAmnt
   * @param workQuat
   *          a Quaternion to use as scratchpad during calculation
   * @return this quaternion for chaining.
   * @throws NullPointerException
   *           if startQuat, endQuat or workQuat are null.
   */
  public Quaternion slerpLocal(final ReadOnlyQuaternion startQuat, final ReadOnlyQuaternion endQuat,
      final double changeAmnt, final Quaternion workQuat) {

    // check for weighting at either extreme
    if (changeAmnt == 0.0) {
      return set(startQuat);
    } else if (changeAmnt == 1.0) {
      return set(endQuat);
    }

    // Check for equality and skip operation.
    if (startQuat.equals(endQuat)) {
      return set(startQuat);
    }

    double result = startQuat.dot(endQuat);
    workQuat.set(endQuat);

    if (result < 0.0) {
      // Negate the second quaternion and the result of the dot product
      workQuat.multiplyLocal(-1);
      result = -result;
    }

    // Set the first and second scale for the interpolation
    double scale0 = 1 - changeAmnt;
    double scale1 = changeAmnt;

    // Check if the angle between the 2 quaternions was big enough to
    // warrant such calculations
    if (1 - result > 0.1) {// Get the angle between the 2 quaternions,
      // and then store the sin() of that angle
      final double theta = Math.acos(result);
      final double invSinTheta = 1f / Math.sin(theta);

      // Calculate the scale for q1 and q2, according to the angle and
      // it's sine value
      scale0 = Math.sin((1 - changeAmnt) * theta) * invSinTheta;
      scale1 = Math.sin(changeAmnt * theta) * invSinTheta;
    }

    // Calculate the x, y, z and w values for the quaternion by using a
    // special form of linear interpolation for quaternions.
    final double x = scale0 * startQuat.getX() + scale1 * workQuat.getX();
    final double y = scale0 * startQuat.getY() + scale1 * workQuat.getY();
    final double z = scale0 * startQuat.getZ() + scale1 * workQuat.getZ();
    final double w = scale0 * startQuat.getW() + scale1 * workQuat.getW();
    set(x, y, z, w);

    // Return the interpolated quaternion
    return this;
  }

  /**
   * Does a spherical linear interpolation between the given start and end quaternions by the given
   * change amount. Stores the result locally.
   *
   * @param startQuat
   * @param endQuat
   * @param changeAmnt
   * @return this quaternion for chaining.
   * @throws NullPointerException
   *           if startQuat or endQuat are null.
   */
  public Quaternion slerpLocal(final ReadOnlyQuaternion startQuat, final ReadOnlyQuaternion endQuat,
      final double changeAmnt) {
    final Quaternion end = Quaternion.fetchTempInstance().set(endQuat);
    slerpLocal(startQuat, endQuat, changeAmnt, end);
    Quaternion.releaseTempInstance(end);
    return this;
  }

  /**
   * Modifies this quaternion to equal the rotation required to point the z-axis at 'direction' and
   * the y-axis to 'up'.
   *
   * @param direction
   *          where to 'look' at
   * @param up
   *          a vector indicating the local up direction.
   * @return this quaternion for chaining.
   */
  public Quaternion lookAt(final ReadOnlyVector3 direction, final ReadOnlyVector3 up) {
    final Vector3 xAxis = Vector3.fetchTempInstance();
    final Vector3 yAxis = Vector3.fetchTempInstance();
    final Vector3 zAxis = Vector3.fetchTempInstance();
    direction.normalize(zAxis);
    up.normalize(xAxis).crossLocal(zAxis);
    zAxis.cross(xAxis, yAxis);

    fromAxes(xAxis, yAxis, zAxis);
    normalizeLocal();

    Vector3.releaseTempInstance(xAxis);
    Vector3.releaseTempInstance(yAxis);
    Vector3.releaseTempInstance(zAxis);
    return this;
  }

  /**
   * @return the squared magnitude of this quaternion.
   */
  @Override
  public double magnitudeSquared() {
    return getW() * getW() + getX() * getX() + getY() * getY() + getZ() * getZ();
  }

  /**
   * @return the magnitude of this quaternion. basically sqrt({@link #magnitude()})
   */
  @Override
  public double magnitude() {
    final double magnitudeSQ = magnitudeSquared();
    if (magnitudeSQ == 1.0) {
      return 1.0;
    }

    return Math.sqrt(magnitudeSQ);
  }

  /**
   * @param x
   * @param y
   * @param z
   * @param w
   * @return the dot product of this quaternion with the given x,y,z and w values.
   */
  @Override
  public double dot(final double x, final double y, final double z, final double w) {
    return getX() * x + getY() * y + getZ() * z + getW() * w;
  }

  /**
   * @param quat
   * @return the dot product of this quaternion with the given quaternion.
   */
  @Override
  public double dot(final ReadOnlyQuaternion quat) {
    return dot(quat.getX(), quat.getY(), quat.getZ(), quat.getW());
  }

  /**
   * Sets the value of this quaternion to (0, 0, 0, 1). Equivalent to calling set(0, 0, 0, 1)
   *
   * @return this quaternion for chaining
   */
  public Quaternion setIdentity() {
    return set(0, 0, 0, 1);
  }

  /**
   * @return true if this quaternion is (0, 0, 0, 1)
   */
  @Override
  public boolean isIdentity() { return strictEquals(Quaternion.IDENTITY); }

  /**
   * Check a quaternion... if it is null or its doubles are NaN or infinite, return false. Else return
   * true.
   *
   * @param quat
   *          the quaternion to check
   * @return true or false as stated above.
   */
  public static boolean isFinite(final ReadOnlyQuaternion quat) {
    if (quat == null) {
      return false;
    }

    return Double.isFinite(quat.getX()) //
        && Double.isFinite(quat.getY()) //
        && Double.isFinite(quat.getZ()) //
        && Double.isFinite(quat.getW());
  }

  /**
   * @return the string representation of this quaternion.
   */
  @Override
  public String toString() {
    return "com.ardor3d.math.Quaternion [X=" + getX() + ", Y=" + getY() + ", Z=" + getZ() + ", W=" + getW() + "]";
  }

  /**
   * @return returns a unique code for this quaternion object based on its values. If two quaternions
   *         are numerically equal, they will return the same hash code value.
   */
  @Override
  public int hashCode() {
    int result = 17;

    result = HashUtil.hash(result, getX());
    result = HashUtil.hash(result, getY());
    result = HashUtil.hash(result, getZ());
    result = HashUtil.hash(result, getW());

    return result;
  }

  /**
   * @param o
   *          the object to compare for equality
   * @return true if this quaternion and the provided quaternion have roughly the same x, y, z and w
   *         values.
   */
  @Override
  public boolean equals(final Object o) {
    if (this == o) {
      return true;
    }
    if (!(o instanceof ReadOnlyQuaternion)) {
      return false;
    }
    final ReadOnlyQuaternion comp = (ReadOnlyQuaternion) o;
    return Math.abs(_x - comp.getX()) < Quaternion.ALLOWED_DEVIANCE
        && Math.abs(_y - comp.getY()) < Quaternion.ALLOWED_DEVIANCE
        && Math.abs(_z - comp.getZ()) < Quaternion.ALLOWED_DEVIANCE
        && Math.abs(_w - comp.getW()) < Quaternion.ALLOWED_DEVIANCE;
  }

  /**
   * @param o
   *          the object to compare for equality
   * @return true if this quaternion and the provided quaternion have the exact same double values.
   */
  @Override
  public boolean strictEquals(final Object o) {
    if (this == o) {
      return true;
    }
    if (!(o instanceof ReadOnlyQuaternion)) {
      return false;
    }
    final ReadOnlyQuaternion comp = (ReadOnlyQuaternion) o;
    return EqualsUtil.areEqual(getX(), comp.getX()) //
        && EqualsUtil.areEqual(getY(), comp.getY()) //
        && EqualsUtil.areEqual(getZ(), comp.getZ()) //
        && EqualsUtil.areEqual(getW(), comp.getW());
  }

  // /////////////////
  // Method for Cloneable
  // /////////////////

  @Override
  public Quaternion clone() {
    return new Quaternion(this);
  }

  // /////////////////
  // Methods for Savable
  // /////////////////

  @Override
  public Class<? extends Quaternion> getClassTag() { return this.getClass(); }

  @Override
  public void write(final OutputCapsule capsule) throws IOException {
    capsule.write(getX(), "x", 0);
    capsule.write(getY(), "y", 0);
    capsule.write(getZ(), "z", 0);
    capsule.write(getW(), "w", 1);
  }

  @Override
  public void read(final InputCapsule capsule) throws IOException {
    setX(capsule.readDouble("x", 0));
    setY(capsule.readDouble("y", 0));
    setZ(capsule.readDouble("z", 0));
    setW(capsule.readDouble("w", 1));
  }

  // /////////////////
  // Methods for Externalizable
  // /////////////////

  /**
   * Used with serialization. Not to be called manually.
   *
   * @param in
   *          ObjectInput
   * @throws IOException
   * @throws ClassNotFoundException
   */
  @Override
  public void readExternal(final ObjectInput in) throws IOException, ClassNotFoundException {
    setX(in.readDouble());
    setY(in.readDouble());
    setZ(in.readDouble());
    setW(in.readDouble());
  }

  /**
   * Used with serialization. Not to be called manually.
   *
   * @param out
   *          ObjectOutput
   * @throws IOException
   */
  @Override
  public void writeExternal(final ObjectOutput out) throws IOException {
    out.writeDouble(getX());
    out.writeDouble(getY());
    out.writeDouble(getZ());
    out.writeDouble(getW());
  }

  // /////////////////
  // Methods for creating temp variables (pooling)
  // /////////////////

  /**
   * @return An instance of Quaternion that is intended for temporary use in calculations and so
   *         forth. Multiple calls to the method should return instances of this class that are not
   *         currently in use.
   */
  public final static Quaternion fetchTempInstance() {
    if (MathConstants.useMathPools) {
      return Quaternion.QUAT_POOL.fetch().setIdentity();
    } else {
      return new Quaternion();
    }
  }

  /**
   * Releases a Quaternion back to be used by a future call to fetchTempInstance. TAKE CARE: this
   * Quaternion object should no longer have other classes referencing it or "Bad Things" will happen.
   *
   * @param mat
   *          the Quaternion to release.
   */
  public final static void releaseTempInstance(final Quaternion mat) {
    if (MathConstants.useMathPools) {
      Quaternion.QUAT_POOL.release(mat);
    }
  }
}
