/**
* <code>mult</code> multiplies this quaternion by a parameter quaternion. The result is returned
* as a new quaternion. It should be noted that quaternion multiplication is not commutative so q
* * p != p * q.
*
* <p>It IS safe for q and res to be the same object. It IS safe for this and res to be the same
* object.
*
* @param q the quaternion to multiply this quaternion by.
* @param res the quaternion to store the result in.
* @return the new quaternion.
*/
public Quaternion mult(Quaternion q, Quaternion res) {
if (res == null) res = new Quaternion();
float qw = q.w, qx = q.x, qy = q.y, qz = q.z;
res.x = x * qw + y * qz - z * qy + w * qx;
res.y = -x * qz + y * qw + z * qx + w * qy;
res.z = x * qy - y * qx + z * qw + w * qz;
res.w = -x * qx - y * qy - z * qz + w * qw;
return res;
}
/**
* <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;
}
