From baac826ceab08a26d0b414fab8d272667a714651 Mon Sep 17 00:00:00 2001
From: sk-zk <sk-zk@users.noreply.github.com>
Date: Sun, 3 Mar 2024 05:39:11 +0100
Subject: [PATCH] Fix node rotations of path items

---
 TruckLib/MathEx.cs                         | 10 +++++-----
 TruckLib/ScsMap/PathItem.cs                |  4 +---
 TruckLibTests/TruckLib/ScsMap/MoverTest.cs | 16 ++++++++--------
 3 files changed, 14 insertions(+), 16 deletions(-)

diff --git a/TruckLib/MathEx.cs b/TruckLib/MathEx.cs
index 8f9041b..61af8a8 100644
--- a/TruckLib/MathEx.cs
+++ b/TruckLib/MathEx.cs
@@ -18,14 +18,14 @@ internal static class MathEx
         public static Quaternion GetNodeRotation(Vector3 a, Vector3 b)
         {
             var direction = Vector3.Normalize(b - a);
-            var yaw = Math.Atan2(-direction.Z, direction.X) - (Math.PI / 2);
-            var pitch = Math.Atan2(direction.Y, Math.Sqrt(direction.X * direction.X + direction.Z * direction.Z));
-            return Quaternion.CreateFromYawPitchRoll((float)yaw, (float)pitch, 0);
+            return GetNodeRotation(direction);
         }
 
-        public static double GetNodeAngle(Vector3 direction)
+        public static Quaternion GetNodeRotation(Vector3 direction)
         {
-            return AngleOffAroundAxis(direction, -Vector3.UnitZ, Vector3.UnitY, false);
+            var yaw = Math.Atan2(-direction.Z, direction.X) - (Math.PI / 2);
+            var pitch = Math.Atan2(direction.Y, Math.Sqrt(direction.X * direction.X + direction.Z * direction.Z));
+            return Quaternion.CreateFromYawPitchRoll((float)yaw, (float)pitch, 0);
         }
 
         public static Vector3 RotatePointAroundPivot(Vector3 point, Vector3 pivot, Quaternion rot)
diff --git a/TruckLib/ScsMap/PathItem.cs b/TruckLib/ScsMap/PathItem.cs
index 05579b3..392c25e 100644
--- a/TruckLib/ScsMap/PathItem.cs
+++ b/TruckLib/ScsMap/PathItem.cs
@@ -119,9 +119,7 @@ protected virtual void SetNodeRotations()
                 var p2 = Nodes[Math.Min(Nodes.Count - 1, i + 1)].Position;
                 var p3 = Nodes[Math.Min(Nodes.Count - 1, i + 2)].Position;
                 var vec = CatmullRomSpline.Derivative(p0, p1, p2, p3, 0);
-                var angle = MathEx.GetNodeAngle(vec);
-                angle = MathEx.Mod(angle, Math.Tau);
-                Nodes[i].Rotation = Quaternion.CreateFromYawPitchRoll((float)angle, 0, 0);
+                Nodes[i].Rotation = MathEx.GetNodeRotation(vec);
             }
         }
 
diff --git a/TruckLibTests/TruckLib/ScsMap/MoverTest.cs b/TruckLibTests/TruckLib/ScsMap/MoverTest.cs
index e9ddb47..7c5f25b 100644
--- a/TruckLibTests/TruckLib/ScsMap/MoverTest.cs
+++ b/TruckLibTests/TruckLib/ScsMap/MoverTest.cs
@@ -14,17 +14,17 @@ public class MoverTest
         public void Add()
         {
             var points = new List<Vector3>() {
-                new(14.45f, 0, 12.32f),
-                new(28.68f, 0, 11.37f),
-                new(28.90f, 0, 25.28f),
-                new(14.98f, 0, 22.84f)
+                new(13, 0, 13),
+                new(38, 5, 5),
+                new(60, 10, 16),
+                new(44, 2, 31)
             };
             var expectedRotations = new List<Quaternion>()
             {
-                new(0, 0.682978f, 0, -0.730439f),
-                new(0, 0.913131f, 0, -0.407665f),
-                new(0, 0.906075f, 0, 0.423116f),
-                new(0, 0.643127f, 0, 0.76576f)
+                new(-0.075905f, 0.586977f, -0.055407f, -0.804131f),
+                new(-0.0714474f, 0.725294f, -0.0761533f, -0.680474f),
+                new(0.00633117f, 0.992021f, 0.0555912f, -0.112979f),
+                new(-0.0691682f, 0.903593f, 0.159656f, 0.391465f)
             };
 
             var map = new Map("foo");