From 09245defd656f6db602b94c05f4246683f65e8d8 Mon Sep 17 00:00:00 2001
From: Moulberry <james.jenour@protonmail.com>
Date: Mon, 10 Jan 2022 14:40:13 +0800
Subject: [PATCH] Improve PositionUtils and add tests (#582)

---
 .../server/utils/position/PositionUtils.java  | 10 ++--
 .../server/utils/PositionUtilsTest.java       | 51 +++++++++++++++++++
 2 files changed, 58 insertions(+), 3 deletions(-)
 create mode 100644 src/test/java/net/minestom/server/utils/PositionUtilsTest.java

diff --git a/src/main/java/net/minestom/server/utils/position/PositionUtils.java b/src/main/java/net/minestom/server/utils/position/PositionUtils.java
index 2d456e1e5..b00cd2800 100644
--- a/src/main/java/net/minestom/server/utils/position/PositionUtils.java
+++ b/src/main/java/net/minestom/server/utils/position/PositionUtils.java
@@ -13,11 +13,15 @@ public final class PositionUtils {
     }
 
     public static float getLookYaw(double dx, double dz) {
-        final double horizontalAngle = Math.atan2(dz, dx);
-        return (float) (horizontalAngle * (180.0 / Math.PI)) - 90;
+        final double radians = Math.atan2(dz, dx);
+        final float degrees = (float)Math.toDegrees(radians) - 90;
+        if (degrees < -180) return degrees + 360;
+        if (degrees > 180) return degrees - 360;
+        return degrees;
     }
 
     public static float getLookPitch(double dx, double dy, double dz) {
-        return (float) Math.atan2(dy, Math.max(Math.abs(dx), Math.abs(dz)));
+        final double radians = -Math.atan2(dy, Math.max(Math.abs(dx), Math.abs(dz)));
+        return (float) Math.toDegrees(radians);
     }
 }
diff --git a/src/test/java/net/minestom/server/utils/PositionUtilsTest.java b/src/test/java/net/minestom/server/utils/PositionUtilsTest.java
new file mode 100644
index 000000000..888bee67e
--- /dev/null
+++ b/src/test/java/net/minestom/server/utils/PositionUtilsTest.java
@@ -0,0 +1,51 @@
+package net.minestom.server.utils;
+
+import net.minestom.server.utils.position.PositionUtils;
+import org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.*;
+
+public class PositionUtilsTest {
+
+    @Test
+    public void yaw() {
+        float plusX = PositionUtils.getLookYaw(10, 0);
+        assertEquals(-90, plusX, 1E-5);
+
+        float plusZ = PositionUtils.getLookYaw(0, 10);
+        assertEquals(0, plusZ, 1E-5);
+
+        float minusX = PositionUtils.getLookYaw(-10, 0);
+        assertEquals(90, minusX, 1E-5);
+
+        float minusZNegative = PositionUtils.getLookYaw(1E-5, -10);
+        if (minusZNegative < -180) fail();
+        assertEquals(-180, minusZNegative, 1E-4);
+
+        float minusZPositive = PositionUtils.getLookYaw(-1E-5, -10);
+        if (minusZPositive > 180) fail();
+        assertEquals(180, minusZPositive, 1E-4);
+
+        float oneThreeFive = PositionUtils.getLookYaw(-5, -5);
+        assertEquals(135, oneThreeFive, 1E-5);
+
+        float fortyFive = PositionUtils.getLookYaw(5, 5);
+        assertEquals(-45, fortyFive, 1E-5);
+    }
+
+    @Test
+    public void highPitch() {
+        float high = PositionUtils.getLookPitch(0, 999999, 0);
+        assertEquals(-90, high, 1E-5);
+
+        float low = PositionUtils.getLookPitch(0, -999999, 0);
+        assertEquals(90, low, 1E-5);
+
+        float zero = PositionUtils.getLookPitch(-5, 0, 5);
+        assertEquals(0, zero, 1E-5);
+
+        float fortyFive = PositionUtils.getLookPitch(5, 5, 0);
+        assertEquals(-45, fortyFive, 1E-5);
+    }
+
+}