diff --git a/api/src/main/java/ru/dragonestia/msb3/api/math/Quaternion.java b/api/src/main/java/ru/dragonestia/msb3/api/math/Quaternion.java index 3a010d7..bcce778 100644 --- a/api/src/main/java/ru/dragonestia/msb3/api/math/Quaternion.java +++ b/api/src/main/java/ru/dragonestia/msb3/api/math/Quaternion.java @@ -34,20 +34,13 @@ public record Quaternion(double x, double y, double z, double w) { public Quaternion rotate(Vec normal) { var defaultQuaternion = new Quaternion(0, 0, 0, 1); + var defaultVector = new Vec(0, 1, 0); - var feta_x = Math.atan(normal.z()/normal.y()); + var feta = Math.acos(defaultVector.dot(normal)); + var r = defaultVector.cross(normal).normalize(); + var r_sin = r.mul(Math.sin(feta/2)); - var q0 = new Quaternion((Math.sin(feta_x / 2)), 0, 0, (Math.cos(feta_x / 2))); - var q = q0.mul(defaultQuaternion); - var localPos = new Quaternion(0, 1, 0, 0); - var qvq0 = q0.mul(localPos); - var qvq = qvq0.mul(q0.conjugate()); - var q_vector = new Vec(qvq.x(), qvq.y(), qvq.z()).normalize(); - - var multiply = normal.x() > q_vector.x() ? -1 : 1; - var feta_z = Math.acos(q_vector.dot(normal)); - - var q1 = new Quaternion(0, 0, multiply * (Math.sin(feta_z / 2)), (Math.cos(feta_z / 2))); - return q.mul(q1); + var rotation = new Quaternion(r_sin.x(), r_sin.y(), r_sin.z(), Math.cos(feta/2)); + return defaultQuaternion.mul(rotation); } }