public float GetRoll()
{
FloatVectorStruct ypr = new FloatVectorStruct();
GetYawPitchRoll(ypr);
return(ypr.z);
}
void GetYawPitchRoll(FloatVectorStruct ypr)
{
FloatVectorStruct gravity = new FloatVectorStruct();
GetGravity(ypr, this);
GetYawPitchRoll(this, gravity, ypr);
}
static void GetYawPitchRoll(Quaternion q, FloatVectorStruct gravity, FloatVectorStruct ypr)
{
// yaw: (about Z axis)
ypr.x = (float)Math.Atan2(2 * q.x * q.y - 2 * q.w * q.z, 2 * q.w * q.w + 2 * q.x * q.x - 1);
// pitch: (node up/down, about X axis)
ypr.y = (float)Math.Atan(gravity.y / Math.Sqrt(gravity.x * gravity.x + gravity.z * gravity.z));
// roll: (tilt left/right, about Y axis)
ypr.z = (float)Math.Atan(gravity.x / Math.Sqrt(gravity.y * gravity.y + gravity.z * gravity.z));
}
static void GetGravity(FloatVectorStruct v, Quaternion q)
{
v.x = 2 * (q.x * q.z - q.w * q.y);
v.y = 2 * (q.w * q.x + q.y * q.z);
v.z = q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z;
}