//rotates points from one frame of reference to another using a rotation matrix
public static void RotatePoints(Matrix3x3 rotMatrix, Vector3[] bodyFramePoints, Vector3[] referenceFramePoints)
{
for (int i = 0; i < referenceFramePoints.Length; i++)
{
bodyFramePoints[i] = Matrix3x3.Multiply(referenceFramePoints[i], rotMatrix);
}
}
//computes a rotation matrix based on a previous rotation matrix and a series of angle rotations
//better algorithm then nextRotMatrix - still need to keep rotation < 180 degrees
//This uses the rectangular rule
public static Matrix3x3 nextRotMatrix2(Matrix3x3 rotMatrix, Vector3 rotations)
{
//This uses C2 = C1( I + (sin(w)/w)B + ((1 - cos(w))/w)B^2 )
//where w is the total rotation, I is the identity matrix and B is the scew symmetric form of the rotation vector
Matrix3x3 I = new Matrix3x3();
I.matrix[0, 0] = 1; I.matrix[1, 0] = 0; I.matrix[2, 0] = 0;
I.matrix[0, 1] = 0; I.matrix[1, 1] = 1; I.matrix[2, 1] = 0;
I.matrix[0, 2] = 0; I.matrix[1, 2] = 0; I.matrix[2, 2] = 1;
Matrix3x3 B = new Matrix3x3();
B.matrix[0, 0] = 0; B.matrix[1, 0] = -rotations.Z; B.matrix[2, 0] = rotations.Y;
B.matrix[0, 1] = rotations.Z; B.matrix[1, 1] = 0; B.matrix[2, 1] = -rotations.X;
B.matrix[0, 2] = -rotations.Y; B.matrix[1, 2] = rotations.X; B.matrix[2, 2] = 0;
double totalRotation = Vector3.Length(rotations);
Matrix3x3 smallRot;
//Don't divide by 0
if (totalRotation > 0)
{
smallRot = Matrix3x3.Add(Matrix3x3.Add(
I,
Matrix3x3.Multiply(Math.Sin(totalRotation) / totalRotation, B)),
Matrix3x3.Multiply((1 - Math.Cos(totalRotation)) / (totalRotation * totalRotation), Matrix3x3.Multiply(B, B))
);
}
else
{
smallRot = I;
}
Matrix3x3 newRotMatrix = Matrix3x3.Multiply(rotMatrix, smallRot);
//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(newRotMatrix)
//This should give an Identity matrix
//Matrix3x3 I = Matrix3x3.Multiply(Matrix3x3.Transpose(newRotMatrix), newRotMatrix);
//Normalize to the the vectors Unit length
//return newRotMatrix;
return(Matrix3x3.Normalize(newRotMatrix));
//TODO: We should really be doing an orthonormalization
}
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);
}
}
//computes a rotation matrix based on a previous rotation matrix and a series of angle rotations
public static Matrix3x3 nextRotMatrix(Matrix3x3 rotMatrix, Vector3 rotations)
{
//assuming C(t2) = C(t1)A(t1) where A(t1) is the rotation matrix relating the body frame between time t1 and t2 (I + B)
//A(t1) = [ 1 y z ] for small angles (<180 degrees). x, y and z are rotations about the axes
// [ -y 1 x ]
// [ -z -x 1 ]
Matrix3x3 A = new Matrix3x3();
A.matrix[0, 0] = 1; A.matrix[1, 0] = rotations.Y; A.matrix[2, 0] = rotations.Z;
A.matrix[0, 1] = -rotations.Y; A.matrix[1, 1] = 1; A.matrix[2, 1] = rotations.X;
A.matrix[0, 2] = -rotations.Z; A.matrix[1, 2] = -rotations.X; A.matrix[2, 2] = 1;
//Normalized to keep the vectors unit length
Matrix3x3 newRotMatrix = Matrix3x3.Normalize(Matrix3x3.Multiply(rotMatrix, A));
return(newRotMatrix);
}
private void finishZeroing()
{
//align body rotation matrix with reference frame
Matrix3x3 rotMatrix = new Matrix3x3();
if (initialRotWithGravity.Checked)
{
//base the initial rotation matrix on the gravity measurement - keep the y axis (up-down) rotated so the cord is facing out
//Calculate the angles and make sure they are -1 <= x <= 1
//get a normalized version of the gravity vector to find angles
gravityTemp = Vector3.Normalize(gravityRef);
double xAngle = Math.Asin(-gravityTemp.X);
double zAngle = Math.Asin(gravityTemp.Z);
//The board is up-side down
if (gravityRef.Y > 0)
{
xAngle = -xAngle;
zAngle = -zAngle;
}
Matrix3x3 xRotMatrix = new Matrix3x3();
xRotMatrix.matrix[0, 0] = Math.Cos(xAngle); xRotMatrix.matrix[1, 0] = -Math.Sin(xAngle); xRotMatrix.matrix[2, 0] = 0;
xRotMatrix.matrix[0, 1] = Math.Sin(xAngle); xRotMatrix.matrix[1, 1] = Math.Cos(xAngle); xRotMatrix.matrix[2, 1] = 0;
xRotMatrix.matrix[0, 2] = 0; xRotMatrix.matrix[1, 2] = 0; xRotMatrix.matrix[2, 2] = 1;
//no rotation
Matrix3x3 yRotMatrix = new Matrix3x3();
yRotMatrix.matrix[0, 0] = 1; yRotMatrix.matrix[1, 0] = 0; yRotMatrix.matrix[2, 0] = 0;
yRotMatrix.matrix[0, 1] = 0; yRotMatrix.matrix[1, 1] = 1; yRotMatrix.matrix[2, 1] = 0;
yRotMatrix.matrix[0, 2] = 0; yRotMatrix.matrix[1, 2] = 0; yRotMatrix.matrix[2, 2] = 1;
Matrix3x3 zRotMatrix = new Matrix3x3();
zRotMatrix.matrix[0, 0] = 1; zRotMatrix.matrix[1, 0] = 0; zRotMatrix.matrix[2, 0] = 0;
zRotMatrix.matrix[0, 1] = 0; zRotMatrix.matrix[1, 1] = Math.Cos(zAngle); zRotMatrix.matrix[2, 1] = -Math.Sin(zAngle);
zRotMatrix.matrix[0, 2] = 0; zRotMatrix.matrix[1, 2] = Math.Sin(zAngle); zRotMatrix.matrix[2, 2] = Math.Cos(zAngle);
rotMatrix = Matrix3x3.Multiply(Matrix3x3.Multiply(xRotMatrix, yRotMatrix), zRotMatrix);
//The board is up-side down
if (gravityRef.Y < 0)
{
rotMatrix = Matrix3x3.Multiply(-1, rotMatrix);
}
//now rotate gravity into reference frame
gravityRef = Matrix3x3.Multiply(gravityRef, rotMatrix);
magRef = Matrix3x3.Multiply(magRef, rotMatrix);
}
//Assume initial rotation is flat
else
{
rotMatrix.matrix[0, 0] = 1; rotMatrix.matrix[1, 0] = 0; rotMatrix.matrix[2, 0] = 0;
rotMatrix.matrix[0, 1] = 0; rotMatrix.matrix[1, 1] = 1; rotMatrix.matrix[2, 1] = 0;
rotMatrix.matrix[0, 2] = 0; rotMatrix.matrix[1, 2] = 0; rotMatrix.matrix[2, 2] = 1;
}
timer = DateTime.Now;
milliseconds = 0;
milliseconds2 = 0;
p.rotMatrix = rotMatrix;
xGravTxt.Text = gravityRef.X.ToString("F4");
yGravTxt.Text = gravityRef.Y.ToString("F4");
zGravTxt.Text = gravityRef.Z.ToString("F4");
totGravTxt.Text = Vector3.Length(gravityRef).ToString("F4");
Math3D.RotatePoints(p.rotMatrix, p.vertexBuffer, p.originalVertices);
zeroStatusTxt.Text = "Done.";
}
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;
}
