fix: quaternion rotation and implemented more operations

api/src/main/java/ru/dragonestia/msb3/api/boot/DefaultBootstrap.java

@@ -3,13 +3,13 @@ package ru.dragonestia.msb3.api.boot;
 import lombok.extern.log4j.Log4j2;
 import net.kyori.adventure.text.Component;
 import net.kyori.adventure.text.format.NamedTextColor;
-import net.minestom.server.coordinate.Point;
 import net.minestom.server.coordinate.Vec;
 import net.minestom.server.entity.EntityType;
 import net.minestom.server.entity.GameMode;
 import net.minestom.server.entity.metadata.display.BlockDisplayMeta;
 import net.minestom.server.instance.Instance;
 import net.minestom.server.instance.block.Block;
+import ru.dragonestia.msb3.api.entity.debug.DebugCollider;
 import ru.dragonestia.msb3.api.entity.debug.DebugLine;
 import ru.dragonestia.msb3.api.entity.debug.DebugMarker;
 import ru.dragonestia.msb3.api.entity.debug.DebugRendererEntity;
@@ -69,11 +69,14 @@ public class DefaultBootstrap extends ServerInitializer {
         drawLine(instance, new Vec(-10, 60, -3), new Vec(10, 60, -3));
         drawLine(instance, new Vec(0, 57, 10), new Vec(0, 57, -10));
         drawLine(instance, new Vec(0, 53, -10), new Vec(0, 53, 10));
+
+        var collider = new DebugCollider("test", Block.ORANGE_STAINED_GLASS, new Vec(5, 5, 5), Component.text("Collider"));
+        collider.setInstance(instance, new Vec(30, 11, 10));
     }
 
     private static final AtomicInteger I = new AtomicInteger(1);
 
-    private void drawLine(Instance instance, Point startPos, Point endPos) {
+    private void drawLine(Instance instance, Vec startPos, Vec endPos) {
         var id = I.getAndIncrement();
         {
             var marker = new DebugMarker("test", Block.RED_CONCRETE, Component.text("[%s] START".formatted(id)));
@@ -88,17 +91,17 @@ public class DefaultBootstrap extends ServerInitializer {
         var dist = startPos.distance(endPos);
         record LineColor(Block block, Vec rot) {}
         for (var rot: List.of(
-                new LineColor(Block.RED_CONCRETE, new Vec(3, 0.1, 0.1)),
+                new LineColor(Block.RED_CONCRETE, new Vec(dist, 0.1, 0.1)),
                 new LineColor(Block.LIME_CONCRETE, new Vec(0.1, dist, 0.1)),
-                new LineColor(Block.LIGHT_BLUE_CONCRETE, new Vec(0.1, 0.1, 3))
+                new LineColor(Block.LIGHT_BLUE_CONCRETE, new Vec(0.1, 0.1, dist))
         )) {
             var entity = new DebugRendererEntity(EntityType.BLOCK_DISPLAY, "test");
             var meta = (BlockDisplayMeta) entity.getEntityMeta();
-            var normal = Vec.fromPoint(endPos.sub(startPos)).normalize();
+            var normal = endPos.sub(startPos).normalize();
             meta.setBlockState(rot.block());
             meta.setScale(rot.rot());
 
-            meta.setLeftRotation(Quaternion.DEFAULT.rotate(normal).toFloatArray());
+            meta.setLeftRotation(Quaternion.DEFAULT.rotate(normal).normalize().toFloatArray());
 
             entity.setInstance(instance, startPos);
         }

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 = defaultVector.cross(normal);
-        var r_sin = r.mul(Math.sin(feta/2));
+        if (r.isZero()){
-
+            r = new Vec(0, 0, 1);
-        var rotation = new Quaternion(r_sin.x(), r_sin.y(), r_sin.z(), Math.cos(feta/2));
+        }
+        var rotation = fromVec(r.normalize().mul(Math.sin(feta / 2)), Math.cos(feta / 2));
         return defaultQuaternion.mul(rotation);
     }
 }