public void TipOffsets(int? station, double Xoff, double Yoff, double Zoff)
{
Triple co = new Triple(Xoff, Yoff, Zoff);
string c = "N" + (station == null ? "*" : ((int)station).ToString("0")) + co.ToASCII();
SendCommand(c, true);
}
//Cross product
public static Triple Cross(Triple A, Triple B)
{
Triple C = new Triple();
C.v1 = A.v2 * B.v3 - B.v2 * A.v3;
C.v2 = B.v1 * A.v3 - A.v1 * B.v3;
C.v3 = A.v1 * B.v2 - B.v1 * A.v2;
return C;
}
public Triple Get_SourceMountingFrame()
{
SendCommand('G', false);
currentHeader = ReadHeader();
if (currentHeader.Command != 'G')
throw new PolhemusException(0xF3);
Triple mfa;
if (_format == Format.ASCII)
mfa = Triple.FromASCII(TReader, "Sxxx.xxxB");
else
{
if (currentHeader.Length != 12)
throw new PolhemusException(0xF2);
mfa = new Triple(BReader.ReadSingle(), BReader.ReadSingle(), BReader.ReadSingle());
}
return mfa;
}
internal void addPointToView(XYZRecord xyz)
{
Triple t = new Triple(xyz.X, xyz.Y, xyz.Z);
t = projection.PerspectiveProject(t);
if (t.v3 <= 0) return;
Ellipse circle = new Ellipse();
circle.Stroke = System.Windows.Media.Brushes.Transparent;
int pink = Math.Min((int)(10 * Math.Pow(t.v3,0.75)), 180);
circle.Fill = new SolidColorBrush(System.Windows.Media.Color.FromRgb(255, (byte)pink, (byte)pink));
double r = Math.Max(100D / t.v3, 2.5D);
circle.Height = circle.Width = r * 2D;
Canvas.SetTop(circle, Draw.ActualHeight / 2 - viewScale * t.v2 - r);
Canvas.SetLeft(circle, Draw.ActualWidth / 2 + viewScale * t.v1 - r);
Canvas.SetZIndex(circle, (int)(-t.v3 * 100));
circle.ToolTip = new TextBlock(new Run(xyz.Name));
circle.MouseDown+=new MouseButtonEventHandler(circle_MouseDown);
Draw.Children.Add(circle);
}
//----->Polhemus commands start here<-----
//----->Configuration commands<-----
public void AlignmentReferenceFrame(int? station, Triple O, Triple X, Triple Y)
{
string c = "A" + station == null ? "*" : ((int)station).ToString("0") + "," +
O.ToASCII() + X.ToASCII() + Y.ToASCII();
SendCommand(c, true);
}
public Triple[] Get_AlignmentReferenceFrame()
{
SendCommand('A', false);
currentHeader = ReadHeader();
if (currentHeader.Command != 'A')
throw new PolhemusException(0xF3);
Triple[] cc = new Triple[3];
if (_format == Format.ASCII)
{
for (int i = 0; i < 3; i++)
cc[i] = Triple.FromASCII(TReader, "Sxxx.xx");
}
else
{
if (currentHeader.Length != 36)
throw new PolhemusException(0xF2);
for (int i = 0; i < 3; i++)
cc[i] = new Triple(BReader.ReadSingle(), BReader.ReadSingle(), BReader.ReadSingle());
}
return cc;
}
void NormalEndOfFrame()
{
output2.Text = "N = " + Dcount.ToString("0") +
" Mean distance = " + m.ToString("0.000") +
(Dcount > 1 ? " SD = " + sd.ToString("0.0000") : "");
Triple t = (1D / Dcount) * sumP;
Dsum = 0;
Dcount = 0;
Dsumsq = 0;
sumP = new Triple(0, 0, 0);
AcquisitionFinished(sa, new PointAcqusitionFinishedEventArgs(t));
}
public Triple Transform(Triple v)
{
return new Triple(v * Matrix[0], v * Matrix[1], v * Matrix[2]);
}
private Triple DoCoordinateTransform(Triple t)
{
Triple p = t - Origin;
return new Triple(p * Transform[0], p * Transform[1], p * Transform[2]);
}
void ForceEndOfFrame()
{
Dsum = 0;
Dcount = 0;
Dsumsq = 0;
sumP = new Triple(0, 0, 0);
AcquisitionFinished(sa, new PointAcqusitionFinishedEventArgs(null, !executeSkip)); //signal retry or skip
}
private void CreateCoordinateTransform()
{
Origin = 0.5D * (PR + PL);
Triple pr = PR - Origin;
Triple pn = PN - Origin;
Transform[0] = pr.Norm();
Transform[2] = (Triple.Cross(pr, pn)).Norm();
Transform[1] = Triple.Cross(Transform[2], Transform[0]);
}
//Continuous mode callback, modes 1 to 3
void ContinuousPoints(List<IDataFrameType>[] frame, bool final)
{
Skip.IsEnabled = false;
int entryType = (frame == null ? (final ? 0 : 1) : (final ? 2 : 3)); //extrinsic:cancel:intrinsic:continued
#if TRACE
Console.WriteLine("ContinuousPoints " + Dcount.ToString("0") + " " + entryType.ToString("0"));
#endif
if (entryType == 3/*continued*/) //add new point into running averages
{
Triple newP = ((CartesianCoordinates)p.ResponseFrameDescription[0][0]).ToTriple() -
((CartesianCoordinates)p.ResponseFrameDescription[1][0]).ToTriple();
DirectionCosineMatrix t = (DirectionCosineMatrix)p.ResponseFrameDescription[1][1];
Triple P = t.Transform(newP);
sumP += P;
double dr = P.Length();
Dsum += dr;
Dsumsq += dr * dr;
Dcount++;
m = Dsum / Dcount; //running mean
if (Dcount > 1)
sd = Math.Sqrt((Dsumsq - m * m * Dcount) / (Dcount - 1)); //running standard deviation
output2.Text = m.ToString("0.000") + (Dcount > 1 ? "(" + sd.ToString("0.0000") + ")" : "");
//check end of frame criteria
if (mode == 1 && Dcount >= samples || mode == 3 && Dcount >= 2 && sd < threshold)
{
sa.Stop(); //create an extrinsic end to the frame on next entry
}
}
else if (entryType == 0/*extrinsic*/)
if (mode == 1 || mode == 3)
NormalEndOfFrame();
else
ForceEndOfFrame();
else if (entryType == 2/*intrinsic*/)
if (mode == 2)
NormalEndOfFrame();
else
ForceEndOfFrame();
else //if none of above, must be true cancellation or skip
if (executeSkip)
ForceEndOfFrame();
else //handle as cancellation
{
writeElectrodeFile();
Environment.Exit(0);
}
}
//this should be incremented at the time of successful acquistion and not if unsuccessful or cancelled
private void AcquisitionLoop(object sa, PointAcqusitionFinishedEventArgs e)
{
#if TRACE
if (e != null)
Console.WriteLine("AcquisitionLoop " + electrodeNumber.ToString("0") + " " +
(e.result == null) + " " + (e.Retry).ToString());
else
Console.WriteLine("AcquisitionLoop " + electrodeNumber.ToString("0"));
#endif
if (!executeSkip)
{
SystemSounds.Beep.Play();
if (electrodeNumber >= 0)
{
if (!e.Retry)
{
Triple t = DoCoordinateTransform(e.result);
string name = templateList[electrodeNumber].Name;
output1.Text = name + ": " + t.ToString();
XYZRecord xyz = new XYZRecord(name, t.v1, t.v2, t.v3);
if (electrodeLocations.Where(l => l.Name == name).Count() == 0) //assure unique electrode name
{
electrodeLocations.Add(xyz);
addPointToView(xyz);
Delete.IsEnabled = true;
}
else //this is a replacement
{
XYZRecord oldXYZ = electrodeLocations.Where(l => l.Name == name).First(); //get item to be replaced
int n = electrodeLocations.IndexOf(oldXYZ); //and replace old
electrodeLocations.Remove(oldXYZ); //remove
electrodeLocations.Insert(n, xyz); //replace with new
updateView(); //and redraw
}
electrodeNumber++; //on to next electrode location
}
}
else if (electrodeNumber == -3) //first entry
{
if (e != null && !e.Retry)
{
PN = e.result; //save Nasion
electrodeNumber++;
Redo.IsEnabled = true;
}
DoPrompting((e != null) && e.Retry);
((StylusAcquisition)sa).Start();
return;
}
else if (electrodeNumber == -2)
{
//save previous result
if (!e.Retry) //save Right preauricular
{
PR = e.result;
electrodeNumber++;
}
}
else //electrodeNumber == -1
{
//save previous result
if (!e.Retry) //save Left preauricular
{
PL = e.result;
CreateCoordinateTransform(); //calculate coordinate transformtion
electrodeNumber++;
drawAxes();
}
}
}
else //executing skip
{
electrodeNumber++;
executeSkip = false;
}
if (electrodeNumber >= numberOfElectrodes)
{
if (voice)
{
int n = electrodeLocations.Count;
prompt.ClearContent();
prompt.StartSentence();
prompt.AppendText("Completed acquisition of " + n.ToString("0") + " electrodes.");
prompt.EndSentence();
speak.Speak(prompt);
}
ElectrodeName.Text = "";
writeElectrodeFile();
return; //done
}
DoPrompting(e.Retry);
((StylusAcquisition)sa).Start();
return;
}
public static Triple FromASCII(StreamReader sr, string format)
{
Triple cc = new Triple();
cc.v1 = (double)PolhemusController.parseASCIIStream(sr, format);
cc.v2 = (double)PolhemusController.parseASCIIStream(sr, format);
cc.v3 = (double)PolhemusController.parseASCIIStream(sr, format);
return cc;
}
//one shot completed delegate
void SinglePoint(List<IDataFrameType>[] frame)
{
Skip.IsEnabled = false;
#if TRACE
Console.WriteLine("SinglePoint " + pointCount.ToString("0") + " " + (frame == null).ToString());
#endif
if (frame != null)
{
Triple newP = ((CartesianCoordinates)p.ResponseFrameDescription[0][0]).ToTriple() -
((CartesianCoordinates)p.ResponseFrameDescription[1][0]).ToTriple();
DirectionCosineMatrix t = (DirectionCosineMatrix)p.ResponseFrameDescription[1][1];
Triple P = t.Transform(newP);
sumP += P;
double dr = P.Length();
Dsum += dr;
Dsumsq += dr * dr;
Dcount++;
m = Dsum / Dcount; //running mean
if (Dcount > 1)
sd = Math.Sqrt((Dsumsq - m * m * Dcount) / (Dcount - 1)); //running standard deviation
output2.Text = m.ToString("0.000") + (Dcount > 1 ? "(" + sd.ToString("0.0000") + ")" : "");
if (Dcount >= samples) //we've got the requisite number of points
NormalEndOfFrame();
else sa.Start(); //get another point
}
else if (executeSkip)
{
ForceEndOfFrame();
}
}
public DirectionCosineMatrix()
{
for (int i = 0; i < 3; i++)
Matrix[i] = new Triple();
}
public Triple PerspectiveProject(Triple point)
{
//used to calculate rotation of point as observed from and projected onto eye plane
Triple p = new Triple(0, 0, Eye - Tz * point);
if (p.v3 <= 0D) return p; //point behind eye plane: not going to show anyway
double factor = scaleFactor / Math.Pow(p.v3, horizonFactor); //scale towards vanashing point
p.v1 = factor * (Tx * point); //rotate and scale projected point
p.v2 = factor * (Ty * point);
return p;
}
public PointAcqusitionFinishedEventArgs(Triple P, bool retry = false)
{
result = P;
Retry = retry;
}
public Triple Project(Triple point)
{
return new Triple(Tx * point, Ty * point, Tz * point);
}
public void Boresight(int? station, double AzRef, double ElRef, double RlRef, bool ResetOrigin)
{
Triple ra = new Triple(AzRef, ElRef, RlRef);
string c = "B" + station == null ? "*" : ((int)station).ToString("0") + "," +
ra.ToASCII() +
(ResetOrigin ? "1" : "0");
SendCommand(c, true);
}
public void HemisphereOfOperation(int? station, double p1, double p2, double p3)
{
Triple cc = new Triple(p1, p2, p3);
string c = "H" + (station == null ? "*" : ((int)station).ToString("0")) + cc.ToASCII();
SendCommand(c, true);
}
public Triple Get_Boresight(int station)
{
SendCommand("B" + station.ToString("0"), false);
currentHeader = ReadHeader();
if (currentHeader.Command != 'B')
throw new PolhemusException(0xF3);
Triple ra;
if (_format == Format.ASCII)
{
ra = Triple.FromASCII(TReader, "Sxxx.xxB");
PolhemusController.parseASCIIStream(TReader, "<>");
}
else
{
if (currentHeader.Length != 12)
throw new PolhemusException(0xF2);
ra = new Triple(BReader.ReadSingle(), BReader.ReadSingle(), BReader.ReadSingle());
}
return ra;
}
public void SourceMountingFrame(double A, double E, double R)
{
Triple mfa = new Triple(A, E, R);
mfa.v1 = A;
mfa.v2 = E;
mfa.v3 = R;
string c = "G" + mfa.ToString();
SendCommand(c, true);
}
public Triple Get_TipOffsets(int station)
{
SendCommand("N" + station.ToString("0"), false);
currentHeader = ReadHeader();
if (currentHeader.Command != 'N')
throw new PolhemusException(0xF3);
if (currentHeader.Station != station)
throw new PolhemusException(0xF5);
Triple co = new Triple();
if (_format == Format.ASCII)
{
co = Triple.FromASCII(TReader, "Sx.xxxB");
parseASCIIStream("<>");
}
else
{
if (currentHeader.Length != 12)
throw new PolhemusException(0xF2);
co = new Triple(BReader.ReadSingle(), BReader.ReadSingle(), BReader.ReadSingle());
}
return co;
}
public Triple Get_TipOffsets(int station)
{
SendCommand("N" + station.ToString("0"), true);
ResponseHeader r = ReadHeader();
if (r.Command != 'N')
throw new PolhemusException(0xF3);
if (r.Station != station)
throw new PolhemusException(0xF5);
Triple co = new Triple();
if (_format == Format.ASCII)
co = Triple.FromASCII(TReader, "Sx.xxxB");
else
{
if (r.Length != 12)
throw new PolhemusException(0xF2);
co = new Triple(BReader.ReadSingle(), BReader.ReadSingle(), BReader.ReadSingle());
}
return co;
}
