/**
* Interpolates between this Rotation and a given Rotation by a given interpolation factor.
*
* @param rotation The terminal point of interpolation.
* @param factor The interpolation factor. This value will be clamped to the range [0.0, 1.0].
*/
public Rotation slerp(Rotation rotation, double factor) {
// store quaternion components for easy access
Quaternion startQuat = this.quaternion;
Quaternion endQuat = rotation.getQuaternion();
double x1 = startQuat.getX();
double y1 = startQuat.getY();
double z1 = startQuat.getZ();
double w1 = startQuat.getW();
double x2 = endQuat.getX();
double y2 = endQuat.getY();
double z2 = endQuat.getZ();
double w2 = endQuat.getW();
// calculate angle between start and end
double cosHalfTheta = w1 * w2 + x1 * x2 + y1 * y2 + z1 * z2;
// if start=end or start=-end, we can return start
if (Math.abs(cosHalfTheta) >= 1.0) return this;
// calculate temporary values
double halfTheta = Math.acos(cosHalfTheta);
double sinHalfTheta = Math.sqrt(1.0 - cosHalfTheta * cosHalfTheta);
// if theta is 180°, then the result is not fully defined
// we could rotate around any axis normal to start or end
if (Math.abs(sinHalfTheta) < 0.001) {
Vector3D v1 = startQuat.vectorComponent();
Vector3D v2 = endQuat.vectorComponent();
Vector3D vect = v1.add(v2).multiply(0.5);
double scalar = (w1 + w2) * 0.5;
return new Rotation(new Quaternion(vect, scalar));
}
double ratioA = Math.sin((1 - factor) * halfTheta) / sinHalfTheta;
double ratioB = Math.sin(factor * halfTheta) / sinHalfTheta;
// compute and return quaternion
Vector3D v1 = startQuat.vectorComponent();
Vector3D v2 = endQuat.vectorComponent();
Vector3D vect = v1.multiply(ratioA).add((v2).multiply(ratioB));
double scalar = (w1 * ratioA) + (w2 * ratioB);
return new Rotation(new Quaternion(vect, scalar));
}
/**
* Creates a string to describe this Rotation as a quaternion.
*
* @return A string that describes this Rotation as a quaternion.
*/
public String toQuaternionString() {
return "("
+ quaternion.getX()
+ ", "
+ quaternion.getY()
+ ", "
+ quaternion.getZ()
+ ", "
+ quaternion.getW()
+ ")";
}
public Quaternion multiply(Quaternion other) {
double a, b, c, d;
double x1, y1, z1;
double x2, y2, z2, w2;
x1 = v.getX();
y1 = v.getY();
z1 = v.getZ();
w2 = other.getW();
x2 = other.getX();
y2 = other.getY();
z2 = other.getZ();
a = w * w2 - x1 * x2 - y1 * y2 - z1 * z2;
b = w * x2 + x1 * w2 + y1 * z2 - z1 * y2;
c = w * y2 - x1 * z2 + y1 * w2 + z1 * x2;
d = w * z2 + x1 * y2 - y1 * x2 + z1 * w2;
return new Quaternion(a, b, c, d);
}
public Quaternion add(Quaternion other) {
return new Quaternion(
w + other.getW(), getX() + other.getX(), getY() + other.getY(), getZ() + other.getZ());
}
