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;
    }
}