19 lines
472 B
C#
19 lines
472 B
C#
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;
|
|
}
|
|
}
|