public Quaternion(final Quaternion q) {
set(q);
}
public Quaternion(final float x, final float y, final float z, final float w) {
set(x, y, z, w);
}
/**
* Set this quaternion to a spherical linear interpolation between the given start and end
* quaternions by the given change amount.
*
* <p>Note: Method <i>does not</i> normalize this quaternion!
*
* @param a start quaternion
* @param b end quaternion
* @param changeAmnt float between 0 and 1 representing interpolation.
* @return this quaternion for chaining.
* @see <a
* href="http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/">euclideanspace.com-QuaternionSlerp</a>
*/
public final Quaternion setSlerp(final Quaternion a, final Quaternion b, final float changeAmnt) {
// System.err.println("Slerp.0: A "+a+", B "+b+", t "+changeAmnt);
if (changeAmnt == 0.0f) {
set(a);
} else if (changeAmnt == 1.0f) {
set(b);
} else {
float bx = b.x;
float by = b.y;
float bz = b.z;
float bw = b.w;
// Calculate angle between them (quat dot product)
float cosHalfTheta = a.x * bx + a.y * by + a.z * bz + a.w * bw;
final float scale0, scale1;
if (cosHalfTheta >= 0.95f) {
// quaternions are close, just use linear interpolation
scale0 = 1.0f - changeAmnt;
scale1 = changeAmnt;
// System.err.println("Slerp.1: Linear Interpol; cosHalfTheta "+cosHalfTheta);
} else if (cosHalfTheta <= -0.99f) {
// the quaternions are nearly opposite,
// we can pick any axis normal to a,b to do the rotation
scale0 = 0.5f;
scale1 = 0.5f;
// System.err.println("Slerp.2: Any; cosHalfTheta "+cosHalfTheta);
} else {
// System.err.println("Slerp.3: cosHalfTheta "+cosHalfTheta);
if (cosHalfTheta
<= -FloatUtil.EPSILON) { // FIXME: .. or shall we use the upper bound 'cosHalfTheta <
// FloatUtil.EPSILON' ?
// Negate the second quaternion and the result of the dot product (Inversion)
bx *= -1f;
by *= -1f;
bz *= -1f;
bw *= -1f;
cosHalfTheta *= -1f;
// System.err.println("Slerp.4: Inverted cosHalfTheta "+cosHalfTheta);
}
final float halfTheta = FloatUtil.acos(cosHalfTheta);
final float sinHalfTheta = FloatUtil.sqrt(1.0f - cosHalfTheta * cosHalfTheta);
// if theta = 180 degrees then result is not fully defined
// we could rotate around any axis normal to qa or qb
if (Math.abs(sinHalfTheta) < 0.001f) { // fabs is floating point absolute
scale0 = 0.5f;
scale1 = 0.5f;
// throw new InternalError("XXX"); // FIXME should not be reached due to above inversion ?
} else {
// Calculate the scale for q1 and q2, according to the angle and
// it's sine value
scale0 = FloatUtil.sin((1f - changeAmnt) * halfTheta) / sinHalfTheta;
scale1 = FloatUtil.sin(changeAmnt * halfTheta) / sinHalfTheta;
}
}
x = a.x * scale0 + bx * scale1;
y = a.y * scale0 + by * scale1;
z = a.z * scale0 + bz * scale1;
w = a.w * scale0 + bw * scale1;
}
// System.err.println("Slerp.X: Result "+this);
return this;
}