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