boop

Co-authored-by: Damien <damguillotin@gmail.com>

src/main/java/com/tocard/cam/LocationQuaternion.java

@@ -1,141 +1,81 @@
 package com.tocard.cam;
 
-import org.bukkit.Bukkit;
 import org.bukkit.Location;
 
-public class LocationQuaternion {
+public class LocationQuaternion extends Location {
-    private double[] coordinates = new double[7];
 
+    private double qw;
+    private double qx;
+    private double qy;
+    private double qz;
+    private float roll = 0;
+
+    private static final float deg2grad = (float) Math.PI / 180.0f;
+
+    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Source_code
     public LocationQuaternion(Location location) {
-        coordinates[0] = location.getX();
+        super(location.getWorld(),
-        coordinates[1] = location.getY();
+                location.getX(),
-        coordinates[2] = location.getZ();
+                location.getY(),
+                location.getZ(),
+                location.getYaw() * deg2grad,
+                location.getPitch() * deg2grad);
 
-        double yaw = location.getYaw() * Math.PI / 180;
+        this.updateQuaternionAngles();
-        double pitch = location.getPitch() * Math.PI / 180;
-        double roll = 0;
-
-        double cy = Math.cos(yaw * 0.5);
-        double sy = Math.sin(yaw * 0.5);
-        double cp = Math.cos(pitch * 0.5);
-        double sp = Math.sin(pitch * 0.5);
-        double cr = Math.cos(roll * 0.5);
-        double sr = Math.sin(roll * 0.5);
-
-        coordinates[3] = cr * cp * cy + sr * sp * sy;
-        coordinates[4] = sr * cp * cy - cr * sp * sy;
-        coordinates[5] = cr * sp * cy + sr * cp * sy;
-        coordinates[6] = cr * cp * sy - sr * sp * cy;
     }
 
-    public LocationQuaternion(double[] coords) {
+    public void updateEulerAngles() {
-        for (int i = 0; i < coordinates.length; i++) {
-            coordinates[i] = coords[i];
-        }
-    }
-
-    public LocationQuaternion add(LocationQuaternion lq) {
-        for (int i = 0; i < coordinates.length; i++) {
-            coordinates[i] += lq.coordinates[i];
-        }
-        return this;
-    }
-
-    public LocationQuaternion subtract(LocationQuaternion lq) {
-        for (int i = 0; i < coordinates.length; i++) {
-            coordinates[i] -= lq.coordinates[i];
-        }
-        return this;
-    }
-
-    public LocationQuaternion add(Location l) {
-        LocationQuaternion lq = new LocationQuaternion(l);
-        for (int i = 0; i < coordinates.length; i++) {
-            coordinates[i] += lq.coordinates[i];
-        }
-        return this;
-    }
-
-    public LocationQuaternion multiply(double m) {
-        for (int i = 0; i < coordinates.length; i++) {
-            coordinates[i] *= m;
-        }
-        return this;
-    }
-
-    public double distance(LocationQuaternion loc) {
-        return Math.sqrt(
-                coordinates[0] * coordinates[0] + coordinates[1] * coordinates[1] + coordinates[2] * coordinates[2]);
-    }
-
-    public Location toLocation() {
         // roll (x-axis rotation)
-        // double sinr_cosp = 2 * (coordinates[3] * coordinates[4] + coordinates[5] *
+        double sinr_cosp = 2 * (qw * qx + qy * qz);
-        // coordinates[6]);
+        double cosr_cosp = 1 - 2 * (qx * qx + qy * qy);
-        // double cosr_cosp = 1 - 2 * (coordinates[4] * coordinates[4] + coordinates[5]
+        this.roll = (float) Math.atan2(sinr_cosp, cosr_cosp);
-        // * coordinates[5]);
-        // double roll = Math.atan2(sinr_cosp, cosr_cosp);
 
         // pitch (y-axis rotation)
-        double pitch;
+        double sinp = 2 * (qw * qy - qz * qx);
-        double sinp = 2 * (coordinates[3] * coordinates[5] - coordinates[6] * coordinates[4]);
+        if (Math.abs(sinp) >= 1) {
-        if (Math.abs(sinp) >= 1)
+            this.setPitch((float) Math.copySign(Math.PI / 2, sinp)); // use 90 degrees if out of range
-            pitch = Math.signum(sinp) * Math.PI / 2; // use 90 degrees if out of range
+        } else {
-        else
+            this.setPitch((float) Math.asin(sinp));
-            pitch = Math.asin(sinp);
+        }
 
         // yaw (z-axis rotation)
-        double siny_cosp = 2 * (coordinates[3] * coordinates[6] + coordinates[4] * coordinates[5]);
+        double siny_cosp = 2 * (qw * qz + qx * qy);
-        double cosy_cosp = 1 - 2 * (coordinates[5] * coordinates[5] + coordinates[6] * coordinates[6]);
+        double cosy_cosp = 1 - 2 * (qy * qy + qz * qz);
-        double yaw = Math.atan2(siny_cosp, cosy_cosp);
+        this.setYaw((float) Math.atan2(siny_cosp, cosy_cosp));
-
-        return new Location(
-                Bukkit.getWorlds().get(0),
-                coordinates[0], coordinates[1], coordinates[2],
-                (float) (yaw * 180 / Math.PI), (float) (pitch * 180 / Math.PI));
     }
 
-    public Location toLocation(double previousYaw) {
+    public void updateQuaternionAngles() {
-        Location loc = toLocation();
+        double cy = Math.cos(getYaw() * 0.5);
+        double sy = Math.sin(getYaw() * 0.5);
+        double cp = Math.cos(getPitch() * 0.5);
+        double sp = Math.sin(getPitch() * 0.5);
+        double cr = Math.cos(getRoll() * 0.5);
+        double sr = Math.sin(getRoll() * 0.5);
 
-        double A = previousYaw;
+        this.qw = cr * cp * cy + sr * sp * sy;
-        double B = loc.getYaw();
+        this.qx = sr * cp * cy - cr * sp * sy;
-        double b = B % 360;
+        this.qy = cr * sp * cy + sr * cp * sy;
-        int wholePart = 360 * (int) (A / 360);
+        this.qz = cr * cp * sy - sr * sp * cy;
-
-        double minDist = -1;
-        double sol = 0;
-        for (int i = -1; i < 2; i++) {
-            double val = wholePart + b + i * 360;
-            double dist = Math.abs(val - A);
-            if (minDist == -1 || dist < minDist) {
-                sol = val;
-                minDist = dist;
-            }
-        }
-        Camera.broadlog("From " + coordinates[3] + " to " + sol);
-        loc.setYaw((float) sol);
-        return loc;
     }
 
     public double dot(LocationQuaternion loc) {
-        double sum = 0;
+        return this.qw * loc.qw + this.qx * loc.qx + this.qy * loc.qy + this.qz * loc.qz;
-        for (int i = 3; i < 7; i++) {
-            sum += coordinates[i] + loc.coordinates[i];
-        }
-        return sum;
-    }
-
-    public LocationQuaternion invertQuaternion(LocationQuaternion previousLocationQuaternion) {
-        if (dot(previousLocationQuaternion) < 0)
-            for (int i = 3; i < 7; i++) {
-                coordinates[i] *= -1;
-            }
-        return this;
     }
 
     @Override
     public LocationQuaternion clone() {
-        return new LocationQuaternion(coordinates);
+        LocationQuaternion cloned = (LocationQuaternion) super.clone();
+
+        cloned.roll = this.roll;
+        cloned.qw = this.qw;
+        cloned.qx = this.qx;
+        cloned.qy = this.qy;
+        cloned.qz = this.qz;
+
+        return cloned;
+    }
+
+    public float getRoll() {
+        return this.roll;
     }
 }

src/main/java/com/tocard/cam/LocationQuaternion2.java

@@ -1,89 +1,91 @@
+// double A = previousLocationQuaternion.coordinates[3];
+// double B = coordinates[3];
+// double b = B % 360;
+// int wholePart = 360 * (int) (A / 360);
+
+// double minDist = -1;
+// double sol = 0;
+// for (int i = -1; i < 2; i++) {
+//     double val = wholePart + b + i * 360;
+//     double dist = Math.abs(val - A);
+//     if (minDist == -1 || dist < minDist) {
+//         sol = val;
+//         minDist = dist;
+//     }
+// }
+// Camera.broadlog("From " + coordinates[3] + " to " + sol);
+// coordinates[3] = sol;
+// return this;
+
 package com.tocard.cam;
 
 import org.bukkit.Bukkit;
 import org.bukkit.Location;
 
-public class LocationQuaternion2 {
+public class LocationQuaternion2 extends Location {
-    public double[] coordinates = new double[5];
 
+    private double qw;
+    private double qx;
+    private double qy;
+    private double qz;
+
+    private static final float deg2grad = (float) Math.PI / 180.0f;
+
+    // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Source_code
     public LocationQuaternion2(Location location) {
-        coordinates[0] = location.getX();
+        super(location.getWorld(),
-        coordinates[1] = location.getY();
+                location.getX(),
-        coordinates[2] = location.getZ();
+                location.getY(),
-        coordinates[3] = location.getYaw();
+                location.getZ(),
-        coordinates[4] = location.getPitch();
+                location.getYaw() * deg2grad,
+                location.getPitch() * deg2grad);
+
+        this.updateQuaternionAngles();
     }
 
-    public LocationQuaternion2(double[] coords) {
+    public void updateEulerAngles() {
-        for (int i = 0; i < coordinates.length; i++) {
+        // roll (x-axis rotation)
-            coordinates[i] = coords[i];
+        double sinr_cosp = 2 * (qw * qx + qy * qz);
-        }
+        double cosr_cosp = 1 - 2 * (qx * qx + qy * qy);
+        this.roll = (float) Math.atan2(sinr_cosp, cosr_cosp);
+
+        // pitch (y-axis rotation)
+        double sinp = 2 * (qw * qy - qz * qx);
+        if (Math.abs(sinp) >= 1) {
+            this.setPitch((float) Math.copySign(Math.PI / 2, sinp)); // use 90 degrees if out of range
+        } else {
+            this.setPitch((float) Math.asin(sinp));
         }
 
-    public LocationQuaternion2 add(LocationQuaternion2 lq) {
+        // yaw (z-axis rotation)
-        for (int i = 0; i < coordinates.length; i++) {
+        double siny_cosp = 2 * (qw * qz + qx * qy);
-            coordinates[i] += lq.coordinates[i];
+        double cosy_cosp = 1 - 2 * (qy * qy + qz * qz);
-        }
+        this.setYaw((float) Math.atan2(siny_cosp, cosy_cosp));
-        return this;
     }
 
-    public LocationQuaternion2 subtract(LocationQuaternion2 lq) {
+    public void updateQuaternionAngles() {
-        for (int i = 0; i < coordinates.length; i++) {
+        double cy = Math.cos(getYaw() * 0.5);
-            coordinates[i] -= lq.coordinates[i];
+        double sy = Math.sin(getYaw() * 0.5);
-        }
+        double cp = Math.cos(getPitch() * 0.5);
-        return this;
+        double sp = Math.sin(getPitch() * 0.5);
-    }
+        double cr = Math.cos(0 * 0.5);
+        double sr = Math.sin(0 * 0.5);
 
-    public LocationQuaternion2 add(Location l) {
+        this.qw = cr * cp * cy + sr * sp * sy;
-        LocationQuaternion2 lq = new LocationQuaternion2(l);
+        this.qx = sr * cp * cy - cr * sp * sy;
-        for (int i = 0; i < coordinates.length; i++) {
+        this.qy = cr * sp * cy + sr * cp * sy;
-            coordinates[i] += lq.coordinates[i];
+        this.qz = cr * cp * sy - sr * sp * cy;
-        }
-        return this;
-    }
-
-    public LocationQuaternion2 multiply(double m) {
-        for (int i = 0; i < coordinates.length; i++) {
-            coordinates[i] *= m;
-        }
-        return this;
-    }
-
-    public double distance(LocationQuaternion2 loc) {
-        return Math.sqrt(
-                coordinates[0] * coordinates[0] + coordinates[1] * coordinates[1] + coordinates[2] * coordinates[2]);
-    }
-
-    public Location toLocation() {
-        return new Location(
-                Bukkit.getWorlds().get(0),
-                coordinates[0], coordinates[1], coordinates[2],
-                (float) (coordinates[3]), (float) (coordinates[4]));
-    }
-
-    public LocationQuaternion2 invertQuaternion(LocationQuaternion2 previousLocationQuaternion) {
-        double A = previousLocationQuaternion.coordinates[3];
-        double B = coordinates[3];
-        double b = B % 360;
-        int wholePart = 360 * (int) (A / 360);
-
-        double minDist = -1;
-        double sol = 0;
-        for (int i = -1; i < 2; i++) {
-            double val = wholePart + b + i * 360;
-            double dist = Math.abs(val - A);
-            if (minDist == -1 || dist < minDist) {
-                sol = val;
-                minDist = dist;
-            }
-        }
-        Camera.broadlog("From " + coordinates[3] + " to " + sol);
-        coordinates[3] = sol;
-        return this;
     }
 
     @Override
     public LocationQuaternion2 clone() {
-        return new LocationQuaternion2(coordinates);
+        LocationQuaternion2 cloned = (LocationQuaternion2) super.clone();
+
+        cloned.qw = this.qw;
+        cloned.qx = this.qx;
+        cloned.qy = this.qy;
+        cloned.qz = this.qz;
+
+        return cloned;
     }
 }