/**
* <code>slerp</code> sets this quaternion's value as an interpolation between two other
* quaternions.
*
* @param q1 the first quaternion.
* @param q2 the second quaternion.
* @param t the amount to interpolate between the two quaternions.
*/
public Quaternion slerp(Quaternion q1, Quaternion q2, float t) {
// Create a local quaternion to store the interpolated quaternion
if (q1.x == q2.x && q1.y == q2.y && q1.z == q2.z && q1.w == q2.w) {
this.set(q1);
return this;
}
float result = (q1.x * q2.x) + (q1.y * q2.y) + (q1.z * q2.z) + (q1.w * q2.w);
if (result < 0.0f) {
// Negate the second quaternion and the result of the dot product
q2.x = -q2.x;
q2.y = -q2.y;
q2.z = -q2.z;
q2.w = -q2.w;
result = -result;
}
// Set the first and second scale for the interpolation
float scale0 = 1 - t;
float scale1 = t;
// Check if the angle between the 2 quaternions was big enough to
// warrant such calculations
if ((1 - result) > 0.1f) { // Get the angle between the 2 quaternions,
// and then store the sin() of that angle
float theta = FastMath.acos(result);
float invSinTheta = 1f / FastMath.sin(theta);
// Calculate the scale for q1 and q2, according to the angle and
// it's sine value
scale0 = FastMath.sin((1 - t) * theta) * invSinTheta;
scale1 = FastMath.sin((t * theta)) * invSinTheta;
}
// Calculate the x, y, z and w values for the quaternion by using a
// special
// form of linear interpolation for quaternions.
this.x = (scale0 * q1.x) + (scale1 * q2.x);
this.y = (scale0 * q1.y) + (scale1 * q2.y);
this.z = (scale0 * q1.z) + (scale1 * q2.z);
this.w = (scale0 * q1.w) + (scale1 * q2.w);
// Return the interpolated quaternion
return this;
}
/**
* <code>toAngleAxis</code> sets a given angle and axis to that represented by the current
* quaternion. The values are stored as following: The axis is provided as a parameter and built
* by the method, the angle is returned as a float.
*
* @param axisStore the object we'll store the computed axis in.
* @return the angle of rotation in radians.
*/
public float toAngleAxis(Vector3f axisStore) {
float sqrLength = x * x + y * y + z * z;
float angle;
if (sqrLength == 0.0f) {
angle = 0.0f;
if (axisStore != null) {
axisStore.x = 1.0f;
axisStore.y = 0.0f;
axisStore.z = 0.0f;
}
} else {
angle = (2.0f * FastMath.acos(w));
if (axisStore != null) {
float invLength = (1.0f / FastMath.sqrt(sqrLength));
axisStore.x = x * invLength;
axisStore.y = y * invLength;
axisStore.z = z * invLength;
}
}
return angle;
}