diff --git a/src/main/java/com/tocard/cam/Camera.java b/src/main/java/com/tocard/cam/Camera.java index 098c5e2..a3a3549 100644 --- a/src/main/java/com/tocard/cam/Camera.java +++ b/src/main/java/com/tocard/cam/Camera.java @@ -5,6 +5,7 @@ import java.util.logging.Level; import java.util.logging.Logger; import org.bukkit.Bukkit; +import org.bukkit.Location; import org.bukkit.entity.Player; import org.bukkit.event.EventHandler; import org.bukkit.event.Listener; @@ -118,19 +119,25 @@ public class Camera extends JavaPlugin implements Listener { .add(P.get(i + 1).clone().multiply(t))); } } - curve.add(P.get(0)); } } } - public static String prettyLocation(LocationQuaternion point) { - point.updateEulerAngles(); + public static String prettyLocation(Location point) { return String.format("X=%05.2f, Y=%05.2f, Z=%05.2f, P=%05.2f, Y=%05.2f", point.getX(), point.getY(), point.getZ(), point.getPitch(), point.getYaw()); } + public static String prettyLocation(LocationQuaternion point) { + return String.format( + "X=%05.2f, Y=%05.2f, Z=%05.2f, P=%05.2f, Y=%05.2f, R=%05.2f, w=%05.2f, x=%05.2f, y=%05.2f, z=%05.2f", + point.getX(), point.getY(), point.getZ(), + point.getPitch(), point.getYaw(), point.roll, + point.qw, point.qx, point.qy, point.qz); + } + public static void broadlog(String msg) { Camera.logger.log(Level.INFO, msg); Bukkit.broadcastMessage(msg); diff --git a/src/main/java/com/tocard/cam/LocationQuaternion.java b/src/main/java/com/tocard/cam/LocationQuaternion.java index 5157ba8..af15da5 100644 --- a/src/main/java/com/tocard/cam/LocationQuaternion.java +++ b/src/main/java/com/tocard/cam/LocationQuaternion.java @@ -4,14 +4,13 @@ import org.bukkit.Location; public class LocationQuaternion extends Location { - private double qw; - private double qx; - private double qy; - private double qz; - private float roll = 0; + public double qw; + public double qx; + public double qy; + public double qz; + public float roll = 0; - private static final float deg2grad = (float) Math.PI / 180.0f; - private static final float grad2deg = 1 / deg2grad; + private static final float deg2rad = (float) Math.PI / 180.0f; // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Source_code public LocationQuaternion(Location location) { @@ -26,11 +25,6 @@ public class LocationQuaternion extends Location { } public void updateEulerAngles() { - // roll (x-axis rotation) - double sinr_cosp = 2 * (qw * qx + qy * qz); - double cosr_cosp = 1 - 2 * (qx * qx + qy * qy); - roll = (float) Math.atan2(sinr_cosp, cosr_cosp) * grad2deg; - // pitch (y-axis rotation) double pitch; double sinp = 2 * (qw * qy - qz * qx); @@ -39,19 +33,18 @@ public class LocationQuaternion extends Location { } else { pitch = Math.asin(sinp); } - setPitch((float) pitch * grad2deg); + setPitch((float) pitch / deg2rad); // yaw (z-axis rotation) double siny_cosp = 2 * (qw * qz + qx * qy); double cosy_cosp = 1 - 2 * (qy * qy + qz * qz); double yaw = Math.atan2(siny_cosp, cosy_cosp); - setYaw((float) yaw * grad2deg); - + setYaw((float) yaw / deg2rad); } public void updateQuaternionAngles() { - float yaw = getYaw() * deg2grad; - float pitch = getPitch() * deg2grad; + float yaw = getYaw() * deg2rad; + float pitch = getPitch() * deg2rad; double cy = Math.cos(yaw * 0.5); double sy = Math.sin(yaw * 0.5); @@ -91,7 +84,6 @@ public class LocationQuaternion extends Location { public LocationQuaternion add(LocationQuaternion loc) { super.add(loc); - qw += loc.qw; qx += loc.qx; qy += loc.qy; @@ -103,7 +95,6 @@ public class LocationQuaternion extends Location { public LocationQuaternion subtract(LocationQuaternion loc) { super.subtract(loc); - qw -= loc.qw; qx -= loc.qx; qy -= loc.qy; @@ -115,11 +106,10 @@ public class LocationQuaternion extends Location { public LocationQuaternion multiply(double m) { super.multiply(m); - qw *= m; - qx += m; - qy += m; - qz += m; + qx *= m; + qy *= m; + qz *= m; updateEulerAngles(); return this; @@ -130,4 +120,8 @@ public class LocationQuaternion extends Location { LocationQuaternion cloned = new LocationQuaternion(this); return cloned; } + + public double dot(LocationQuaternion q) { + return qx * q.qx + qy * q.qy + qz * q.qz + qw * q.qw; + } }