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 bcce778..16df674 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 @@ -6,6 +6,10 @@ public record Quaternion(double x, double y, double z, double w) { public static final Quaternion DEFAULT = new Quaternion(0, 0, 0, 1); + public static Quaternion fromVec (Vec vec, double realPart) { + return new Quaternion(vec.x(), vec.y(), vec.z(), realPart); + } + public float[] toFloatArray() { return new float[] { (float) x, (float) y, (float) z, (float) w }; } @@ -23,6 +27,11 @@ public record Quaternion(double x, double y, double z, double w) { return new Quaternion(-x, -y, -z, w); } + public double dotProduct(Quaternion target) { + return (x * target.x + y * target.y + z * target.z + w * target.w + ); + } + public Quaternion mul(Quaternion target) { return new Quaternion( w() * target.x() + x() * target.w() + y() * target.z() - z() * target.y(), @@ -32,15 +41,34 @@ public record Quaternion(double x, double y, double z, double w) { ); } + public Quaternion add(Quaternion target) { + return new Quaternion( + x + target.x, + y + target.y, + z + target.z, + w + target.w + ); + } + + public Quaternion sub(Quaternion target) { + return new Quaternion( + x - target.x, + y - target.y, + z - target.z, + w - target.w + ); + } + public Quaternion rotate(Vec normal) { var defaultQuaternion = new Quaternion(0, 0, 0, 1); var defaultVector = new Vec(0, 1, 0); var feta = Math.acos(defaultVector.dot(normal)); - var r = defaultVector.cross(normal).normalize(); - var r_sin = r.mul(Math.sin(feta/2)); - - var rotation = new Quaternion(r_sin.x(), r_sin.y(), r_sin.z(), Math.cos(feta/2)); + var r = defaultVector.cross(normal); + if (r.isZero()){ + r = new Vec(0, 0, 1); + } + var rotation = fromVec(r.normalize().mul(Math.sin(feta / 2)), Math.cos(feta / 2)); return defaultQuaternion.mul(rotation); } }