Fixed gimbal lock

Calendis · Jun 10, 2024 · 080f07f · 080f07f
1 parent 99bb215
commit 080f07f
Showing 1 changed file with 13 additions and 4 deletions.
diff --git a/magician/paint/Renderer.cs b/magician/paint/Renderer.cs
@@ -55,10 +55,19 @@ public static unsafe void PrepareMatrices()
         Node camera = Ref.Perspective;
         //Matrix4X4<double> rot = Matrix4X4.CreateFromYawPitchRoll(camera.yaw, camera.pitch+Math.PI/2, camera.roll);
         //Scribe.Info(camera.roll);
-        Vector3D<double> upV = Vector3D.Transform(new Vector3D<double>(0, 1, 0), Matrix4X4.CreateRotationZ<double>(camera.roll));
-        double targX = camera.X + camera.Heading.X;
-        double targY = camera.Y + camera.Heading.Y;
-        double targZ = camera.Z + camera.Heading.Z;
+        // Use quaternions to avoid gimbal lock
+        Quaternion<double> qYaw = Quaternion<double>.CreateFromYawPitchRoll(camera.yaw, 0, 0);
+        Quaternion<double> qPitch = Quaternion<double>.CreateFromYawPitchRoll(0, camera.pitch, 0);
+        Quaternion<double> qRoll = Quaternion<double>.CreateFromYawPitchRoll(0, 0, camera.roll);
+        Quaternion<double> rot = qPitch * qRoll * qYaw ;
+        Vector3D<double> defaultUp = new(0, 1, 0);
+        Vector3D<double> defaultHeading = new(0, 0, -1);
+        Vector3D<double> heading = Vector3D.Transform(defaultHeading, rot);
+        Vector3D<double> upV = Vector3D.Transform(defaultUp, rot);
+
+        double targX = camera.X + heading.X;
+        double targY = camera.Y + heading.Y;
+        double targZ = camera.Z + heading.Z;
 
         Matrix4X4<float> mview = Matrix4X4.CreateLookAt<float>(
             new((float)camera.X, (float)camera.Y, (float)camera.Z),