Initial commit

srcs/RobotNet10/Shared/RobotNet10.Shared/Geometry/GeometryExtensions.cs

@@ -0,0 +1,18 @@
+using RobotNet10.Shared.Numbers;
+
+namespace RobotNet10.Shared.Geometry;
+
+public static class GeometryExtensions
+{
+    extension(Quaternion q)
+    {
+        public double ToYawRadian()
+        {
+            var sinYaw = 2.0 * (q.W * q.Z + q.X * q.Y);
+            var cosYaw = 1.0 - 2.0 * (q.Y * q.Y + q.Z * q.Z);
+            return Math.Atan2(sinYaw, cosYaw);
+        }
+
+        public double ToYawDegrees() => q.ToYawRadian() * 180 / Math.PI;
+    }
+}