public static ushort CalculateYaw(Point a) { return (ushort)(Math.Atan2(a.y, a.x) * 32768 / Math.PI); }