static (WAngle, WAngle, WAngle) QuaternionToEuler(int x, int y, int z, int w)
{
// Theoretically 1024 squared, but may differ slightly due to rounding
var lsq = x * x + y * y + z * z + w * w;
var srcp = 2 * (w * x + y * z);
var crcp = lsq - 2 * (x * x + y * y);
var sp = (w * y - z * x) / 512;
var sycp = 2 * (w * z + x * y);
var cycp = lsq - 2 * (y * y + z * z);
var roll = -WAngle.ArcTan(srcp, crcp);
var pitch = -(Math.Abs(sp) >= 1024 ? new WAngle(Math.Sign(sp) * 256) : WAngle.ArcSin(sp));
var yaw = -WAngle.ArcTan(sycp, cycp);
return(roll, pitch, yaw);
}
WRot(int x, int y, int z, int w)
{
this.x = x;
this.y = y;
this.z = z;
this.w = w;
// Theoretically 1024 squared, but may differ slightly due to rounding
var lsq = x * x + y * y + z * z + w * w;
var srcp = 2 * (w * x + y * z);
var crcp = lsq - 2 * (x * x + y * y);
var sp = (w * y - z * x) / 512;
var sycp = 2 * (w * z + x * y);
var cycp = lsq - 2 * (y * y + z * z);
Roll = -WAngle.ArcTan(srcp, crcp);
Pitch = -(Math.Abs(sp) >= 1024 ? new WAngle(Math.Sign(sp) * 256) : WAngle.ArcSin(sp));
Yaw = -WAngle.ArcTan(sycp, cycp);
}
