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;
}
private static Matrix GetRotationMatrix(Vector gravity, Vector geomanetic)
{
Vector A = new Vector(gravity);
Vector E = new Vector(geomanetic);
Vector H = E % A;
double normH = H.GetXYZMagnitude();
if (normH < 0.1)
return null;
H.Normalization();
A.Normalization();
Vector M = A % H;
Matrix ret = new Matrix(H, M, A);
return ret;
}