public SensorFusion(RiftHeadsetDevice sensor = null)
{
Stage = 0;
RunningTime = 0;
DeltaT = 0.001f;
Gain = 0.05f;
EnableGravity = true;
EnablePrediction = true;
PredictionDT = 0.03f;
PredictionTimeIncrement = 0.001f;
FRawMag = new SensorFilter(10);
FAngV = new SensorFilter(20);
GyroOffset = new Vector3f();
TiltAngleFilter = new SensorFilterBase_float(1000);
EnableYawCorrection = false;
MagCalibrated = false;
MagNumReferences = 0;
MagRefIdx = -1;
MagRefScore = 0;
MotionTrackingEnabled = true;
if (sensor != null)
AttachToSensor(sensor);
MagCalibrationMatrix = new Matrix4f();
MagCalibrationMatrix.SetIdentity();
}
// Store the calibration matrix for the magnetometer
public void SetMagCalibration(Matrix4f m)
{
MagCalibrationMatrix = m;
MagCalibrationTime = DateTime.Now; // time stamp the calibration
MagCalibrated = true;
}
public static Matrix4f operator /(Matrix4f a, float s)
{
Matrix4f result = new Matrix4f();
for (int i = 0; i < 4; i++){
for (int j = 0; j < 4; j++){
result[i, j] = a[i, j] / s;
}
}
return result;
}
// Multiplies two matrices into destination with minimum copying.
public static Matrix4f operator *(Matrix4f a, Matrix4f b)
{
Matrix4f result = new Matrix4f();
int i = 0;
do {
result[i, 0] = a.M[i, 0] * b.M[0, 0] + a.M[i, 1] * b.M[1, 0] + a.M[i, 2] * b.M[2, 0] + a.M[i, 3] * b.M[3, 0];
result[i, 1] = a.M[i, 0] * b.M[0, 1] + a.M[i, 1] * b.M[1, 1] + a.M[i, 2] * b.M[2, 1] + a.M[i, 3] * b.M[3, 1];
result[i, 2] = a.M[i, 0] * b.M[0, 2] + a.M[i, 1] * b.M[1, 2] + a.M[i, 2] * b.M[2, 2] + a.M[i, 3] * b.M[3, 2];
result[i, 3] = a.M[i, 0] * b.M[0, 3] + a.M[i, 1] * b.M[1, 3] + a.M[i, 2] * b.M[2, 3] + a.M[i, 3] * b.M[3, 3];
} while((++i) < 4);
return result;
}
public static Matrix4f operator -(Matrix4f a, Matrix4f b)
{
Matrix4f result = new Matrix4f();
for (int i = 0; i < 4; i++){
for (int j = 0; j < 4; j++){
result[i, j] = a[i, j] - b[i, j];
}
}
return result;
}