/** * <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; }