diff --git a/src/main/java/com/github/stephengold/joltjni/Quat.java b/src/main/java/com/github/stephengold/joltjni/Quat.java index 02f6eb21..4941bf11 100644 --- a/src/main/java/com/github/stephengold/joltjni/Quat.java +++ b/src/main/java/com/github/stephengold/joltjni/Quat.java @@ -192,6 +192,31 @@ public void set(float x, float y, float z, float w) { this.w = w; } + /** + * Create a rotation quaternion that rotates {@code from} to {@code to}. + * + * @param from the first direction vector (not null, unaffected) + * @param to the 2nd direction vector (not null, unaffected) + * @return a new quaternion + */ + public static Quat sFromTo(Vec3Arg from, Vec3Arg to) { + float lenV1V2 = (float) Math.sqrt(from.lengthSq() * to.lengthSq()); + float w = lenV1V2 + from.dot(to); + + if (w == 0f) { + if (lenV1V2 == 0f) { // one of the vectors has zero length + return Quat.sIdentity(); + } else { // vectors are perpendicular + Vec3 v = from.getNormalizedPerpendicular(); + return new Quat(v, 0f); + } + } + + Vec3 v = from.cross(to); + Quat result = new Quat(v, w).normalized(); + return result; + } + /** * Create an identity quaternion (0,0,0,1). *