private static Vector GetRotationVectorFromGyro(Vector gyro, double time)
{
Vector tmp = new Vector(gyro);
double magnitude = tmp.GetXYZMagnitude();
if (magnitude > 0.00000001)
tmp.Normalization();
double thetaOverTwo = magnitude * time;
double sinThetaOverTwo = Math.Sin(thetaOverTwo);
double cosThetaOverTwo = Math.Cos(thetaOverTwo);
Vector ret = new Vector(4);
for (int i = 0; i < 3; i++)
ret[i] = sinThetaOverTwo * tmp[i];
ret[3] = cosThetaOverTwo;
return ret;
}
