void calculatePosition(SpatialEventData data, double timeChangeSeconds)
{
Vector3 accelForcesBody = new Vector3(data.Acceleration[0], data.Acceleration[2], -data.Acceleration[1]);
Vector3 accelForcesRef = Matrix3x3.Multiply(accelForcesBody, p.rotMatrix);
Vector3 accelForcesRefWithoutGravity = Vector3.Subtract(accelForcesRef, gravityRef);
//convert from g's to m/s^2 - also, X is backwards
accelForcesRefWithoutGravity.X = accelForcesRefWithoutGravity.X * g;
accelForcesRefWithoutGravity.Y = -accelForcesRefWithoutGravity.Y * g;
accelForcesRefWithoutGravity.Z = accelForcesRefWithoutGravity.Z * g;
//Integrate accelerations into velocities in m/s: v2 = v1 + at
velocities.X += timeChangeSeconds * accelForcesRefWithoutGravity.X;
velocities.Y += timeChangeSeconds * accelForcesRefWithoutGravity.Y;
velocities.Z += timeChangeSeconds * accelForcesRefWithoutGravity.Z;
//Integrate velocities into positions in m: s2 = s1 + v2t
positions.X += timeChangeSeconds * velocities.X;
positions.Y += timeChangeSeconds * velocities.Y;
positions.Z += timeChangeSeconds * velocities.Z;
TimeSpan passed = DateTime.Now.Subtract(timer);
//print every 50 ms
if (passed.TotalMilliseconds > milliseconds + 50)
{
milliseconds = passed.TotalMilliseconds;
xVelTxt.Text = velocities.X.ToString("F4");
yVelTxt.Text = velocities.Y.ToString("F4");
zVelTxt.Text = velocities.Z.ToString("F4");
xPosnTxt.Text = positions.X.ToString("F4");
yPosnTxt.Text = positions.Y.ToString("F4");
zPosnTxt.Text = positions.Z.ToString("F4");
xAccelTxt.Text = accelForcesRefWithoutGravity.X.ToString("F4");
yAccelTxt.Text = accelForcesRefWithoutGravity.Y.ToString("F4");
zAccelTxt.Text = accelForcesRefWithoutGravity.Z.ToString("F4");
totVelTxt.Text = Vector3.Length(velocities).ToString("F4");
totPosnTxt.Text = Vector3.Length(positions).ToString("F4");
totAccelTxt.Text = Vector3.Length(accelForcesRefWithoutGravity).ToString("F4");
timeTxt.Text = "" + passed.Minutes.ToString().PadLeft(2) + ":" + passed.Seconds.ToString().PadLeft(2) + "." + passed.Milliseconds.ToString().PadLeft(2);
}
}
void calculatePosition(SpatialEventData data, double timeChangeSeconds)
{
Vector3 accelForcesBody = new Vector3(data.Acceleration[0], data.Acceleration[2], -data.Acceleration[1]);
Vector3 accelForcesRef = Matrix3x3.Multiply(accelForcesBody, p.rotMatrix);
Vector3 accelForcesRefWithoutGravity = Vector3.Subtract(accelForcesRef, gravityRef);
//convert from g's to m/s^2 - also, X is backwards
accelForcesRefWithoutGravity.X = accelForcesRefWithoutGravity.X * g;
accelForcesRefWithoutGravity.Y = -accelForcesRefWithoutGravity.Y * g;
accelForcesRefWithoutGravity.Z = accelForcesRefWithoutGravity.Z * g;
//Integrate accelerations into velocities in m/s: v2 = v1 + at
velocities.X += timeChangeSeconds * accelForcesRefWithoutGravity.X;
velocities.Y += timeChangeSeconds * accelForcesRefWithoutGravity.Y;
velocities.Z += timeChangeSeconds * accelForcesRefWithoutGravity.Z;
//Integrate velocities into positions in m: s2 = s1 + v2t
positions.X += timeChangeSeconds * velocities.X;
positions.Y += timeChangeSeconds * velocities.Y;
positions.Z += timeChangeSeconds * velocities.Z;
TimeSpan passed = DateTime.Now.Subtract(timer);
//print every 50 ms
if (passed.TotalMilliseconds > milliseconds + 50)
{
milliseconds = passed.TotalMilliseconds;
xVelTxt.Text = velocities.X.ToString("F4");
yVelTxt.Text = velocities.Y.ToString("F4");
zVelTxt.Text = velocities.Z.ToString("F4");
xPosnTxt.Text = positions.X.ToString("F4");
yPosnTxt.Text = positions.Y.ToString("F4");
zPosnTxt.Text = positions.Z.ToString("F4");
xAccelTxt.Text = accelForcesRefWithoutGravity.X.ToString("F4");
yAccelTxt.Text = accelForcesRefWithoutGravity.Y.ToString("F4");
zAccelTxt.Text = accelForcesRefWithoutGravity.Z.ToString("F4");
totVelTxt.Text = Vector3.Length(velocities).ToString("F4");
totPosnTxt.Text = Vector3.Length(positions).ToString("F4");
totAccelTxt.Text = Vector3.Length(accelForcesRefWithoutGravity).ToString("F4");
timeTxt.Text = "" + passed.Minutes.ToString().PadLeft(2) + ":" + passed.Seconds.ToString().PadLeft(2) + "." + passed.Milliseconds.ToString().PadLeft(2);
}
}
void calculateAttitude(SpatialEventData data, double timeChangeSeconds)
{
Vector3 rots = new Vector3(0, 0, 0);
foreach(double angRate in data.AngularRate)
if ((angRate >= 400) || (angRate <= -400))
{
Pipeline.fillpen = Color.Red;
overRotCount++;
overRotsTxt.Text = overRotCount.ToString();
}
rots.X = -(timeChangeSeconds * data.AngularRate[0] * Math.PI / 180);
rots.Y = -(timeChangeSeconds * data.AngularRate[2] * Math.PI / 180);
rots.Z = (timeChangeSeconds * data.AngularRate[1] * Math.PI / 180);
Matrix3x3 nextRotMatrix = Math3D.nextRotMatrix2(p.rotMatrix, rots);
TimeSpan passed = DateTime.Now.Subtract(timer);
//accumulate magnetic data
if (data.MagneticField.Length > 0)
{
magTemp.X += data.MagneticField[0];
magTemp.Y += data.MagneticField[2];
magTemp.Z -= data.MagneticField[1];
magSamplesTaken++;
//factor into rotations at some interval
if (passed.TotalMilliseconds > milliseconds2 + 100)
{
milliseconds2 = passed.TotalMilliseconds;
//convert vector in reference frame to body frame
Vector3 expectedMag = Matrix3x3.Multiply(magRef, Matrix3x3.Transpose(nextRotMatrix));
//actual magnetic vector
magTemp.X /= magSamplesTaken;
magTemp.Y /= magSamplesTaken;
magTemp.Z /= magSamplesTaken;
magSamplesTaken = 0;
if (doMag)
{
//find the angles between the two magnetic vectors. This gives a rotation matrix
Matrix3x3 magRot = Math3D.getRotationMatrix(magTemp, expectedMag);
//If these are off, it's because of slight errors - these are no longer Rotation matrices, strictly speaking
//The determinant should be 1
double det = Matrix3x3.Determinant(magRot);
//This should give an Identity matrix
Matrix3x3 I = Matrix3x3.Multiply(Matrix3x3.Transpose(magRot), magRot);
Vector3 magTempRot = Matrix3x3.Multiply(magTemp, magRot);
Vector3 magDiff = Vector3.Subtract(Vector3.Normalize(magTemp), Vector3.Normalize(expectedMag));
// These should be close to 0
xMagdiffTxt.Text = (magDiff.X * 180.0 / Math.PI).ToString("F4");
yMagdiffTxt.Text = (magDiff.Y * 180.0 / Math.PI).ToString("F4");
zMagdiffTxt.Text = (magDiff.Z * 180.0 / Math.PI).ToString("F4");
totMagDivTxt.Text = (Math.Acos(Vector3.DotProduct(Vector3.Normalize(magTemp), Vector3.Normalize(expectedMag))) * 180.0 / Math.PI).ToString("F4");
//correct the rotation matrix
if (compassCorrections.Checked)
{
nextRotMatrix = Matrix3x3.Normalize(Matrix3x3.Multiply(Matrix3x3.Normalize(nextRotMatrix), Matrix3x3.Normalize(magRot)));
}
}
magTemp.X = 0;
magTemp.Y = 0;
magTemp.Z = 0;
doMag = true;
}
}
p.rotMatrix = nextRotMatrix;
}
//This integrates gyro angular rate into heading over time
void calculateGyroHeading(SpatialEventData[] data, int index)
{
double gyro = 0;
for (int i = 0; i < data.Length; i++)
{
gyro = data[i].AngularRate[index];
if (lastMsCountGood[index])
{
//calculate heading
double timechange = data[i].Timestamp.TotalMilliseconds - lastMsCount[index]; // in ms
double timeChangeSeconds = (double)timechange / 1000.0;
//if (index == 0)
// Console.WriteLine("X Gyro rate: " + gyro.ToString() + " Time: " + timeChangeSeconds.ToString() + ", " + data[i].Timestamp.TotalMilliseconds.ToString());
gyroHeading[index] += timeChangeSeconds * gyro;
}
lastMsCount[index] = data[i].Timestamp.TotalMilliseconds;
lastMsCountGood[index] = true;
}
}
void calculateAttitude(SpatialEventData data, double timeChangeSeconds)
{
Vector3 rots = new Vector3(0, 0, 0);
foreach (double angRate in data.AngularRate)
{
if ((angRate >= phid.gyroAxes[0].AngularRateMax) || (angRate <= phid.gyroAxes[0].AngularRateMin))
{
Pipeline.fillpen = Color.Red;
overRotCount++;
overRotsTxt.Text = overRotCount.ToString();
}
}
rots.X = -(timeChangeSeconds * data.AngularRate[0] * Math.PI / 180);
rots.Y = -(timeChangeSeconds * data.AngularRate[2] * Math.PI / 180);
rots.Z = (timeChangeSeconds * data.AngularRate[1] * Math.PI / 180);
Matrix3x3 nextRotMatrix = Math3D.nextRotMatrix2(p.rotMatrix, rots);
TimeSpan passed = DateTime.Now.Subtract(timer);
//accumulate magnetic data
if (data.MagneticField.Length > 0)
{
magTemp.X += data.MagneticField[0];
magTemp.Y += data.MagneticField[2];
magTemp.Z -= data.MagneticField[1];
magSamplesTaken++;
//factor into rotations at some interval
if (passed.TotalMilliseconds > milliseconds2 + 100)
{
milliseconds2 = passed.TotalMilliseconds;
//convert vector in reference frame to body frame
Vector3 expectedMag = Matrix3x3.Multiply(magRef, Matrix3x3.Transpose(nextRotMatrix));
//actual magnetic vector
magTemp.X /= magSamplesTaken;
magTemp.Y /= magSamplesTaken;
magTemp.Z /= magSamplesTaken;
magSamplesTaken = 0;
if (doMag)
{
//find the angles between the two magnetic vectors. This gives a rotation matrix
Matrix3x3 magRot = Math3D.getRotationMatrix(magTemp, expectedMag);
//If these are off, it's because of slight errors - these are no longer Rotation matrices, strictly speaking
//The determinant should be 1
double det = Matrix3x3.Determinant(magRot);
//This should give an Identity matrix
Matrix3x3 I = Matrix3x3.Multiply(Matrix3x3.Transpose(magRot), magRot);
Vector3 magTempRot = Matrix3x3.Multiply(magTemp, magRot);
Vector3 magDiff = Vector3.Subtract(Vector3.Normalize(magTemp), Vector3.Normalize(expectedMag));
// These should be close to 0
xMagdiffTxt.Text = (magDiff.X * 180.0 / Math.PI).ToString("F4");
yMagdiffTxt.Text = (magDiff.Y * 180.0 / Math.PI).ToString("F4");
zMagdiffTxt.Text = (magDiff.Z * 180.0 / Math.PI).ToString("F4");
totMagDivTxt.Text = (Math.Acos(Vector3.DotProduct(Vector3.Normalize(magTemp), Vector3.Normalize(expectedMag))) * 180.0 / Math.PI).ToString("F4");
//correct the rotation matrix
if (compassCorrections.Checked)
{
nextRotMatrix = Matrix3x3.Normalize(Matrix3x3.Multiply(Matrix3x3.Normalize(nextRotMatrix), Matrix3x3.Normalize(magRot)));
}
}
magTemp.X = 0;
magTemp.Y = 0;
magTemp.Z = 0;
doMag = true;
}
}
p.rotMatrix = nextRotMatrix;
}
