//rotates points from one frame of reference to another using a rotation matrix
public static void RotatePoints(Matrix3x3 rotMatrix, List <Vector3> bodyFramePoints, List <Vector3> referenceFramePoints)
{
for (int i = 0; i < referenceFramePoints.Count; i++)
{
bodyFramePoints[i] = Matrix3x3.Multiply(referenceFramePoints[i], rotMatrix);
}
}
//Runs the calibration algorithm
void calibrateThreadFunction()
{
addMessage("Starting Calibration with " + compassSamples.Count.ToString() + " samples.");
cal = new Calibration(this);
bool calSuccess = false;
if (threeAxisButton.Checked)
{
calSuccess = cal.calibrate3(compassSamples);
}
else
{
calSuccess = cal.calibrate2(compassSamples);
}
if (calSuccess)
{
//Calibration was successful - redraw all the samples in green.
addMessage("Calibration successful.");
if (localFieldTextBox.Text == "")
{
setLocalField(cal.estimatedLocalField.ToString("F5"));
}
for (int i = 0; i < compassSamples.Count; i++)
{
//apply calibration
Vector3 rawData = new Vector3(compassSamples[i].data[0], compassSamples[i].data[1], compassSamples[i].data[2]);
Vector3 calibratedData = Matrix3x3.Multiply(Vector3.Subtract(rawData, cal.offset), cal.gainAndTransform);
//apply local mag field - calibrate returns a unit (mag 1) field
calibratedData = Vector3.Multiply(double.Parse(localFieldTextBox.Text), calibratedData);
lock (p)
{
if (twoAxisButton.Checked)
{
p.addPoint(new Vector3(
calibratedData.X,
calibratedData.Y,
-calibratedData.Z), Brushes.Green);
}
else
{
p.addPoint(new Vector3(
calibratedData.X,
calibratedData.Z,
-calibratedData.Y), Brushes.Green);
}
}
}
calibrated = true;
}
else
{
//failed
addMessage("Calibration failed, try again.");
}
setButtonEnabled(sampleDoneButton, true, "Start");
}
//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
}
//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);
}
void spatial_SpatialData(object sender, Phidgets.Events.SpatialDataEventArgs e)
{
//This displays a vector representing the measured field in red, and after calibration the calibrated field in green
if (e.spatialData[0].MagneticField.Length == 3)
{
lock (p)
{
if (twoAxisButton.Checked)
{
p.originalVertices[4] = new Vector3(
e.spatialData[0].MagneticField[0],
e.spatialData[0].MagneticField[1],
-e.spatialData[0].MagneticField[2]);
if (calibrated)
{
Vector3 rawData = new Vector3(e.spatialData[0].MagneticField[0], e.spatialData[0].MagneticField[1], e.spatialData[0].MagneticField[2]);
Vector3 calibratedData = Matrix3x3.Multiply(Vector3.Subtract(rawData, cal.offset), cal.gainAndTransform);
//apply local mag field - calibrate returns a unit (mag 1) field
calibratedData = Vector3.Multiply(double.Parse(localFieldTextBox.Text), calibratedData);
p.originalVertices[5] = new Vector3(
calibratedData.X,
calibratedData.Y,
-calibratedData.Z);
}
}
else
{
p.originalVertices[4] = new Vector3(
e.spatialData[0].MagneticField[0],
e.spatialData[0].MagneticField[2],
-e.spatialData[0].MagneticField[1]);
if (calibrated)
{
Vector3 rawData = new Vector3(e.spatialData[0].MagneticField[0], e.spatialData[0].MagneticField[1], e.spatialData[0].MagneticField[2]);
Vector3 calibratedData = Matrix3x3.Multiply(Vector3.Subtract(rawData, cal.offset), cal.gainAndTransform);
//apply local mag field - calibrate returns a unit (mag 1) field
calibratedData = Vector3.Multiply(double.Parse(localFieldTextBox.Text), calibratedData);
p.originalVertices[5] = new Vector3(
calibratedData.X,
calibratedData.Z,
-calibratedData.Y);
}
}
}
}
//This samples magnetic field data
if (sampling)
{
lock (compassSamples)
{
if (sampling)
{
sampleCounter++;
if (sampleCounter >= samplesPerSample)
{
sampleCounter = 0;
if (e.spatialData[0].MagneticField.Length == 3)
{
compassSamples.Add(
new compassDataPoint(
e.spatialData[0].MagneticField[0],
e.spatialData[0].MagneticField[1],
e.spatialData[0].MagneticField[2]
)
);
//This draws the magnetic vectors as dots
lock (p)
{
if (twoAxisButton.Checked)
{
p.addPoint(new Vector3(
e.spatialData[0].MagneticField[0],
e.spatialData[0].MagneticField[1],
-e.spatialData[0].MagneticField[2]), Brushes.Red);
}
else
{
p.addPoint(new Vector3(
e.spatialData[0].MagneticField[0],
e.spatialData[0].MagneticField[2],
-e.spatialData[0].MagneticField[1]), Brushes.Red);
}
}
}
}
}
}
}
}
