/**
* Performs a great circle interpolation between q1 and q2 quaternions and places the result into
* this quaternion.
*
* @param q1 the initial quaternion
* @param q2 the final quaternion
* @param alpha the alpha angle interpolation parameter. 0 gives q1 as a result 1.0 gives q2 as a
* result
*/
public void interpolate(Quat4d q1, Quat4d q2, double alpha) {
if (q1 == null) throw new IllegalArgumentException("First quaternion cannot be null");
if (q2 == null) throw new IllegalArgumentException("Second quaternion cannot be null");
// From "Advanced Animation and Rendering Techniques"
// by Watt and Watt pg. 364, function as implemented appeared to be
// incorrect. Fails to choose the same quaternion for the double
// covering. Resulting in change of direction for rotations.
// Fixed function to negate the first quaternion in the case that the
// dot product of q1 and this is negative. Second case was not needed.
double dot = q2.x * q1.x + q2.y * q1.y + q2.z * q1.z + q2.angle * q1.angle;
if (dot < 0) {
// negate quaternion
q1.x = -q1.x;
q1.y = -q1.y;
q1.z = -q1.z;
q1.angle = -q1.angle;
dot = -dot;
}
double s1;
double s2;
if ((1.0 - dot) > EPSILON) {
double cosdot = Math.acos(dot);
double sincosdot = Math.sin(cosdot);
s1 = Math.sin((1.0 - alpha) * cosdot) / sincosdot;
s2 = Math.sin(alpha * cosdot) / sincosdot;
} else {
s1 = 1.0 - alpha;
s2 = alpha;
}
x = s1 * q1.x + s2 * q2.x;
y = s1 * q1.y + s2 * q2.y;
z = s1 * q1.z + s2 * q2.z;
angle = s1 * q1.angle + s2 * q2.angle;
}
public static Quat4d makeRotate(double angle, Vector3d axis) {
double epsilon = 0.0000001;
double x = axis.x;
double y = axis.y;
double z = axis.z;
double length = Math.sqrt(x * x + y * y + z * z);
if (length < epsilon) {
// ~zero length axis, so reset rotation to zero.
return new Quat4d();
}
double inversenorm = 1.0 / length;
double coshalfangle = Math.cos(0.5 * angle);
double sinhalfangle = Math.sin(0.5 * angle);
Quat4d res = new Quat4d();
res.x = x * sinhalfangle * inversenorm;
res.y = y * sinhalfangle * inversenorm;
res.z = z * sinhalfangle * inversenorm;
res.w = coshalfangle;
return res;
}
