void AsQuarternion(out int x, out int y, out int z, out int w)
{
// Angles increase clockwise
var roll = new WAngle(-Roll.Angle / 2);
var pitch = new WAngle(-Pitch.Angle / 2);
var yaw = new WAngle(-Yaw.Angle / 2);
var cr = (long)roll.Cos();
var sr = (long)roll.Sin();
var cp = (long)pitch.Cos();
var sp = (long)pitch.Sin();
var cy = (long)yaw.Cos();
var sy = (long)yaw.Sin();
// Normalized to 1024 == 1.0
x = (int)((sr * cp * cy - cr * sp * sy) / 1048576);
y = (int)((cr * sp * cy + sr * cp * sy) / 1048576);
z = (int)((cr * cp * sy - sr * sp * cy) / 1048576);
w = (int)((cr * cp * cy + sr * sp * sy) / 1048576);
}
public int[] AsQuarternion()
{
// Angles increase clockwise
var r = new WAngle(-Roll.Angle / 2);
var p = new WAngle(-Pitch.Angle / 2);
var y = new WAngle(-Yaw.Angle / 2);
var cr = (long)r.Cos();
var sr = (long)r.Sin();
var cp = (long)p.Cos();
var sp = (long)p.Sin();
var cy = (long)y.Cos();
var sy = (long)y.Sin();
// Normalized to 1024 == 1.0
return(new int[4]
{
(int)((sr * cp * cy - cr * sp * sy) / 1048576), // x
(int)((cr * sp * cy + sr * cp * sy) / 1048576), // y
(int)((cr * cp * sy - sr * sp * cy) / 1048576), // z
(int)((cr * cp * cy + sr * sp * sy) / 1048576) // w
});
}
/// <summary>
/// Construct a rotation from euler angles.
/// </summary>
public WRot(WAngle roll, WAngle pitch, WAngle yaw)
{
Roll = roll;
Pitch = pitch;
Yaw = yaw;
// Angles increase clockwise
var qr = new WAngle(-Roll.Angle / 2);
var qp = new WAngle(-Pitch.Angle / 2);
var qy = new WAngle(-Yaw.Angle / 2);
var cr = (long)qr.Cos();
var sr = (long)qr.Sin();
var cp = (long)qp.Cos();
var sp = (long)qp.Sin();
var cy = (long)qy.Cos();
var sy = (long)qy.Sin();
// Normalized to 1024 == 1.0
x = (int)((sr * cp * cy - cr * sp * sy) / 1048576);
y = (int)((cr * sp * cy + sr * cp * sy) / 1048576);
z = (int)((cr * cp * sy - sr * sp * cy) / 1048576);
w = (int)((cr * cp * cy + sr * sp * sy) / 1048576);
}
public int[] AsQuarternion()
{
// Angles increase clockwise
var r = new WAngle(-Roll.Angle / 2);
var p = new WAngle(-Pitch.Angle / 2);
var y = new WAngle(-Yaw.Angle / 2);
var cr = (long)r.Cos();
var sr = (long)r.Sin();
var cp = (long)p.Cos();
var sp = (long)p.Sin();
var cy = (long)y.Cos();
var sy = (long)y.Sin();
// Normalized to 1024 == 1.0
return new int[4]
{
(int)((sr*cp*cy - cr*sp*sy) / 1048576), // x
(int)((cr*sp*cy + sr*cp*sy) / 1048576), // y
(int)((cr*cp*sy - sr*sp*cy) / 1048576), // z
(int)((cr*cp*cy + sr*sp*sy) / 1048576) // w
};
}
