private static void UpdateMarkerInList(Marker currentMarker)
{
listOfMarkers[currentMarker.MarkerId].TimeStamp = currentMarker.TimeStamp;
listOfMarkers[currentMarker.MarkerId].xCoordinate = currentMarker.xCoordinate;
listOfMarkers[currentMarker.MarkerId].yCoordinate = currentMarker.yCoordinate;
listOfMarkers[currentMarker.MarkerId].zCoordinate = currentMarker.zCoordinate;
}
private void GaitProcessor_PeakDetected(string foot, int direction, Marker footMarker)
{
if (enabled == true)
{
generateMetrics(foot, direction, footMarker);
}
}
/// <summary>
/// Constructor - creates new marker using parameter marker value
/// </summary>
/// <param name="newMarker"></param>
public Marker(Marker newMarker)
{
markerId = newMarker.MarkerId;
timeStamp = newMarker.TimeStamp;
xCoordinate = newMarker.xCoordinate;
yCoordinate = newMarker.yCoordinate;
zCoordinate = newMarker.zCoordinate;
}
public Foot(string footName, Marker calibrationMarker)
{
this.calibrationMarker = calibrationMarker;
this.footName = footName;
previousMarker = calibrationMarker;
footdown_yCoordinate = calibrationMarker.yCoordinate;
currentID = calibrationMarker.MarkerId;
xDirection = 0;// -1 -> indicates heading in -x direction
yDirection = 0;// +1 -> +y direction etc
zDirection = 0;// +1 (+z) is forwards, towards screen; -1 -> movving towards back of treadmill
xPrevDirection = 0;
yPrevDirection = 0;
zPrevDirection = 0;
TCPProcessor.WholeFrameReceivedEvent +=
new TCPProcessor.WholeFrameReceivedHandler(TCPProcessor_WholeFrameReceivedEvent);
}
private void Foot_FootDownDetected(Foot foot)
{
if (eventcount++ > 2) //ignore initial events to allow for treadmill speed up etc
{
if (foot.FootName == "left")
{
if (rightFlag == true)
{
rightFlag = false;
leftFlag = true;
outfile.Write(storedRight.xCoordinate.ToString() + "," + foot.CurrentMarker.xCoordinate.ToString() + "\n");
}
else
{
leftFlag = true;
storedLeft = new Marker(foot.CurrentMarker);
}
}
else
{
leftFlag = false;
}
if (foot.FootName == "right")
{
if (leftFlag == true)
{
leftFlag = false;
rightFlag = true;
outfile.Write(foot.CurrentMarker.xCoordinate.ToString() + "," + storedLeft.xCoordinate.ToString() + "\n");
}
else
{
rightFlag = true;
storedRight = new Marker(foot.CurrentMarker);
}
}
else
{
rightFlag = false;
}
//debug output
Console.WriteLine(foot.FootName + "Down " + foot.CurrentMarker.MarkerId.ToString() + " x" + foot.CurrentMarker.xCoordinate.ToString()
+ " y" + foot.CurrentMarker.yCoordinate.ToString() + " z" + foot.CurrentMarker.zCoordinate.ToString() + " NP" + foot.NearestPosition.ToString() + "\n");
sound.Play();
}
}
/// <summary>
/// Updates the list of markers with lastest marker coordinates
/// </summary>
public static void UpdateCoordinates()
{
PointCloud.NPPointCloudFrame frame = new PointCloud.NPPointCloudFrame();
pointcloud.GetFrame(out frame);
Marker marker = new Marker();
while (frame != null)
{
for (int i = 0; i < frame.Count; i++)
{
marker.MarkerId = i;
marker.TimeStamp = Environment.TickCount;
marker.xCoordinate = frame.Item(i).X * 1000;
marker.yCoordinate = frame.Item(i).Y * 1000;
marker.zCoordinate = frame.Item(i).Z * 1000;
//MarkerList.RefreshMarkerList(marker, frame.Count);
if (CoordinatesAvailableEvent != null)
{
CoordinatesAvailableEvent(marker);
m_endOfFrameSent = false; ;
}
}
//Send number of markers with marker data
marker.TimeStamp = frame.Count;
marker.MarkerId = -2147483648;
if (CoordinatesAvailableEvent != null && m_endOfFrameSent == false)
{
CoordinatesAvailableEvent(marker);
m_endOfFrameSent = true;
}
pointcloud.GetFrame(out frame);
}
}
/// <summary>
/// Adds a given marker to the marker list
/// </summary>
/// <param name="currentMarker"></param>
public static void AddMarker(Marker currentMarker)
{
bool markerFound = false;
foreach (Marker marker in listOfMarkers)
{
if (marker.MarkerId == currentMarker.MarkerId)
{
markerFound = true;
if (currentMarker.MarkerId != -1)
{
UpdateMarkerInList(currentMarker);
}
}
}
if (markerFound == false)
{
listOfMarkers.Add(new Marker(currentMarker));
}
}
//private static void metronome_beat(object source, ElapsedEventArgs e)
//{
//
//}
private void Foot_PeakDetected(string foot, int direction, Marker footMarker)
{
if ((leftHeel == true) && (direction == -1) && (foot == "left"))
{
leftHeel_Sound.Play();
Console.WriteLine("Left: " + footMarker.MarkerId.ToString() + "\t" + footMarker.zCoordinate.ToString() + "\t" +
footMarker.prevZCoordinate.ToString() + "\t" + footMarker.TimeStamp.ToString());
}
if ((rightHeel == true) && (direction == -1) && (foot == "right"))
{
rightHeel_Sound.Play();
Console.WriteLine("Right: " + footMarker.MarkerId.ToString() + "\t" + footMarker.zCoordinate.ToString() + "\t" +
footMarker.prevZCoordinate.ToString() + "\t" + footMarker.TimeStamp.ToString());
}
if ((leftToe == true) && (direction == 1) && (foot == "left"))
{
leftToe_Sound.Play();
}
if ((rightToe == true) && (direction == 1) && (foot == "right"))
{
rightToe_Sound.Play();
}
}
private static void HandleDataFromServer(object client)
{
TcpClient serverConnection = (TcpClient)client;
//NetworkStream serverStream = serverConnection.GetStream();
Marker receivedMarker = new Marker();
OptiTrackCamera receivedCamera = new OptiTrackCamera();
BinaryReader binaryReader = new BinaryReader(new BufferedStream(serverConnection.GetStream()));
int bytesRead;
while (true)
{
bytesRead = 0;
bool readTimestamp = false;
try
{
//blocks until a client sends a message
char messageType = binaryReader.ReadChar();
bytesRead = 1;
switch (messageType)
{
case 'M':
receivedMarker.MarkerId = binaryReader.ReadInt32();
//if have been sent End of Frame indication
//TimeStamp field of End of Frame Indicator contains number of markers in the frame
//ie. receivedMarker.TimeStamp = number of markers in list
if (receivedMarker.MarkerId == -2147483648)
{
receivedMarker.TimeStamp = binaryReader.ReadInt32();
readTimestamp = true;
MarkerList.RemoveExcessMarkersFromList(receivedMarker.TimeStamp);
//MarkerList.MarkerBinList();
if (WholeFrameReceivedEvent != null)
{
WholeFrameReceivedEvent();
}
}
if (readTimestamp == false)
{
receivedMarker.TimeStamp = binaryReader.ReadInt32();
}
receivedMarker.xCoordinate = binaryReader.ReadDouble();
receivedMarker.yCoordinate = binaryReader.ReadDouble();
receivedMarker.zCoordinate = binaryReader.ReadDouble();
bytesRead = 36;
if (receivedMarker.MarkerId != -2147483648)
{
MarkerList.AddMarker(receivedMarker);
}
break;
case 'C':
receivedCamera.CameraNumber = binaryReader.ReadInt32();
receivedCamera.xCoordinate = binaryReader.ReadDouble();
receivedCamera.yCoordinate = binaryReader.ReadDouble();
receivedCamera.zCoordinate = binaryReader.ReadDouble();
double[] rotationMatrixBuffer = new double[9];
for (int i = 0; i < 9; i++)
{
rotationMatrixBuffer[i] = binaryReader.ReadDouble();
}
receivedCamera.RotationMatrix = rotationMatrixBuffer;
bytesRead = 108;
OptitrackCameraList.AddCamera(receivedCamera);
break;
case 'T':
if (AvatarSpeedChangedEvent != null)
{
AvatarSpeedChangedEvent((float)binaryReader.ReadDouble());
bytesRead = 8;
}
else
{
binaryReader.ReadDouble();
bytesRead = 8;
}
break;
default:
System.Diagnostics.Debug.WriteLine("A transmit error has occured. " + messageType.ToString() + " recieved");
bytesRead = 5;
break;
}
}
catch (System.IO.IOException)
{
// a socket error has occured
System.Diagnostics.Debug.WriteLine("Socket error has occured in TCPServer.HandleClientComm");
break;
}
if (bytesRead == 0)
{
// the client has disconnected from the server
System.Diagnostics.Debug.WriteLine("The client has disconnected from the server");
break;
}
//else message has been successfully recieved
//OptitrackCommandParser commandParser = new OptitrackCommandParser();
//commandParser.handleCommand(recievedCommand[0]);
}
}
private void TCPProcessor_TrackableListReceivedEvent(List<Stromohab_MCE.Trackable> newTrackableList)
{
trackableList = new List<Stromohab_MCE.Trackable>(newTrackableList);
Marker m1 = (Marker)(trackableList.Find(LeftFoot));
Marker m2 = (Marker)(trackableList.Find(RightFoot));
if (currentID == 1) currentMarker = m1;
if (currentID == 2) currentMarker = m2;
MarkerTrack();
FrontFoot(m1 ,m2);
}
private void TCPProcessor_WholeFrameReceivedEvent()
{
//Assign the appropriate MarkerID for left and right feet
//Marker_Bin returns the left- or right-most Marker for LeftFoot/RightFoot properties
leftFootMarker = Marker_Bin.LeftFoot;
rightFootMarker = Marker_Bin.RightFoot;
if ((leftFootMarker != null) && (rightFootMarker != null))
{
if (MarkerList.listOfMarkers.Count > 0)
{
if (sampleCount == 0)
{
//timeStart = MarkerList.listOfMarkers[footMarker].TimeStamp;
timeStart = leftFootMarker.TimeStamp;
sampleCount++;
}
time = (leftFootMarker.TimeStamp - timeStart) / 1000.0; //Time is measured in seconds
if (continuePlotting == false)
{
endPlotting();
}
else
{
zDataL.Add(time, leftFootMarker.zCoordinate);
yDataL.Add(time, leftFootMarker.yCoordinate);
xDataL.Add(time, leftFootMarker.xCoordinate);
zDataR.Add(time, rightFootMarker.zCoordinate);
yDataR.Add(time, rightFootMarker.yCoordinate);
xDataR.Add(time, rightFootMarker.xCoordinate);
// Keep the X scale at a rolling 30 second interval, with one
// major step between the max X value and the end of the axis
Scale xScaleLeft = zedGraphControl1.GraphPane.XAxis.Scale;
Scale xScaleRight = zedGraphControl2.GraphPane.XAxis.Scale;
if (time > xScaleLeft.Max - xScaleLeft.MajorStep)
{
xScaleLeft.Max = time + xScaleLeft.MajorStep;
xScaleLeft.Min = xScaleLeft.Max - 6.0;
}
if (time > xScaleRight.Max - xScaleRight.MajorStep)
{
xScaleRight.Max = time + xScaleRight.MajorStep;
xScaleRight.Min = xScaleRight.Max - 6.0;
}
// Force a redraw
zedGraphControl1.Invalidate();
zedGraphControl2.Invalidate();
}
}
}
}
private void FootTrack()
{
//For each new frame need to determine which Marker 'describes' this foot instance
//An extension of Marker_Bin - it has to allow for feet crossing so can not rely on
//which marker is most left/right but has to use historical information to determine
//the track of this foot instance. Initially previous position only but Kalman filter may be necessary.
//This function can also determine foot events eg. peaks, foot-down etc.
double nearestPosition = 9999;
double relativePosition = 0;
//if (MarkerList.listOfMarkers.Count != 2)
foreach (Marker singleMarker in MarkerList.listOfMarkers)
{
int xDiff = ((int)(singleMarker.xCoordinate + (0.5 * Math.Sign(singleMarker.xCoordinate)))) - ((int)(previousMarker.xCoordinate + (0.5 * Math.Sign(previousMarker.xCoordinate))));
int yDiff = ((int)(singleMarker.yCoordinate + (0.5 * Math.Sign(singleMarker.yCoordinate)))) - ((int)(previousMarker.yCoordinate + (0.5 * Math.Sign(previousMarker.yCoordinate))));
int zDiff = ((int)(singleMarker.zCoordinate + (0.5 * Math.Sign(singleMarker.zCoordinate)))) - ((int)(previousMarker.zCoordinate + (0.5 * Math.Sign(previousMarker.zCoordinate))));
relativePosition = Math.Sqrt(Math.Pow(xDiff, 2) + Math.Pow(zDiff, 2) + Math.Pow(yDiff, 2));
//Console.WriteLine(this.footName + singleMarker.MarkerId.ToString() + ">>" + " x" + ((int)singleMarker.xCoordinate).ToString() +
//" y" + ((int)singleMarker.yCoordinate).ToString() + " z" + ((int)singleMarker.zCoordinate).ToString() + " ><" + relativePosition.ToString());
//Console.WriteLine("Previous: x" + previousMarker.xCoordinate.ToString() + " y" + previousMarker.yCoordinate.ToString() + " z" + previousMarker.zCoordinate.ToString());
if (relativePosition < nearestPosition)
{
nearestPosition = relativePosition;
markerShift = (int)nearestPosition;
currentMarker = new Marker (singleMarker);
//if (Math.Abs(xDiff) > 1) xChange = xDiff;//check current-previous>1 to illiminate small errors
//if (Math.Abs(yDiff) > 1) yChange = yDiff;//
//if (Math.Abs(zDiff) > 1) zChange = zDiff;//
xChange = xDiff;
yChange = yDiff;
zChange = zDiff;
//Console.WriteLine("zDiff = " + zDiff.ToString());
}
}
//Console.WriteLine("==" + footName + currentMarker.MarkerId.ToString() + ">nearest<" + nearestPosition.ToString());
if (nearestPosition < 50)
{
//Console.WriteLine(this.footName + this.currentMarker.MarkerId.ToString());
xDirection = Math.Sign(xChange);
yDirection = Math.Sign(yChange);
zDirection = Math.Sign(zChange);
if ((xDirection != xPrevDirection) && (xFootPeakDetected != null) && (xDirection != 0))
{
xFootPeakDetected(this);
}
if ((yDirection != yPrevDirection) && (yFootPeakDetected != null) && (yDirection != 0))
{
yFootPeakDetected(this);
}
if ((zDirection != zPrevDirection) && (zFootPeakDetected != null) && (zDirection != 0))
{
zFootPeakDetected(this);
}
if ((zDirection == 1))
{
footDownEventIsFired = false;//prevent further footdown events for the current step
}
//Console.WriteLine("DownEventData: " + "E" + footDownEventIsFired.ToString() + " zD" + zDirection.ToString() +
//" y" + currentMarker.yCoordinate.ToString() + " xChange" + (Math.Abs(xChange)).ToString() + " yChange" + (Math.Abs(yChange)).ToString());
if ((footDownEventIsFired == false) && (zDirection == -1) && (currentMarker.yCoordinate < (footdown_yCoordinate + 5)) && (Math.Abs(xChange) < 10) && (Math.Abs(yChange) < 10))
{
footDownEventIsFired = true;
FootDownDetected(this);
}
previousMarker = new Marker(currentMarker);
xPrevDirection = xDirection;
yPrevDirection = yDirection;
zPrevDirection = zDirection;
//Console.WriteLine("Passing previous: x" + previousMarker.xCoordinate + " y" + previousMarker.yCoordinate + " z" + previousMarker.zCoordinate + "\n");
}
//else
//{
//currentMarker = null;
//}
}
private void FrontFoot(Marker m1, Marker m2)
{
if (m1.zCoordinate >= m2.zCoordinate)
{
if (this.Equals(m1))
{
frontfoot = this;
}
}
else
{
if (this.Equals(m2))
{
frontfoot = this;
}
}
}
private void TCPProcessor_WholeFrameReceivedEvent()
{
if (MarkerList.listOfMarkers.Count == 2)
{
currentMarker = MarkerList.listOfMarkers[currentID];
MarkerTrack();
FrontFoot(MarkerList.listOfMarkers[0], MarkerList.listOfMarkers[1]);
}
}
/// <summary>
/// Transmits a marker to the current tcpClient.
/// </summary>
/// <param name="marker"></param>
public void Transmit(Marker marker)
{
//lock this code so that if multiple Transmits are called (marker + speed for example), only one thread can access the connection stream at a time
lock (m_lock)
{
//create a list to store any dead connections in (for removal from the main list at the end of the function)
List<TcpClient> connectionsToRemoveFromList = new List<TcpClient>(clientConnectionList.Count);
foreach (TcpClient singleClientConnection in clientConnectionList)
{
try
{
if (singleClientConnection.Connected == false)
{
throw new System.IO.IOException(singleClientConnection.Client.RemoteEndPoint.ToString() + " is no longer connected to the server");
}
else
{
//send the data
BinaryWriter binaryWriter = new BinaryWriter(new BufferedStream(singleClientConnection.GetStream()));
binaryWriter.Write('M');
binaryWriter.Write(marker.MarkerId);
binaryWriter.Write(marker.TimeStamp);
binaryWriter.Write(marker.xCoordinate);
binaryWriter.Write(marker.yCoordinate);
binaryWriter.Write(marker.zCoordinate);
binaryWriter.Flush();
}
}
catch (System.IO.IOException ex)
{
System.Diagnostics.Debug.WriteLine("The client has disconnected. Exception message: " + ex.Message);
connectionsToRemoveFromList.Add(singleClientConnection);
}
catch (InvalidOperationException ex)
{
System.Diagnostics.Debug.WriteLine("The client has disconnected. Exception message: " + ex.Message);
connectionsToRemoveFromList.Add(singleClientConnection);
}
}
foreach (TcpClient connectionToRemove in connectionsToRemoveFromList)
{
clientConnectionList.Remove(connectionToRemove);
}
}
}
private void MotionCapture_CoordinatesAvailableEvent(Marker marker)
{
Transmit(marker);
}
public RawData(Marker calibrationMarker)
{
//TCPProcessor.WholeFrameReceivedEvent +=
// new TCPProcessor.WholeFrameReceivedHandler(TCPProcessor_WholeFrameReceivedEvent);
}
private void generateMetrics(string foot, int direction, Marker footMarker)
{
switch (strideElement)
{
case 0:
if ((foot == "left") && (direction == 1)) //Left toe off
{
time[0] = footMarker.TimeStamp;
strideStart = footMarker.zCoordinate;
strideElement = 1;
}
break;
case 1:
strideElement = 0; //stride data frame is reset if stride stages are detected out of sequence
if ((foot == "left") && (direction == -1)) //left heel down
{
time[1] = footMarker.TimeStamp;
strideElement = 2;
}
break;
case 2:
strideElement = 0;
if ((foot == "right") && (direction == 1)) //right toe off
{
time[2] = footMarker.TimeStamp;
strideElement = 3;
}
break;
case 3:
strideElement = 0;
if ((foot == "right") && (direction == -1)) //right heel down
{
time[3] = footMarker.TimeStamp;
strideElement = 4;
}
break;
case 4:
strideElement = 0;
if ((foot == "left") && (direction == 1)) //left toe off etc...
{
time[4] = footMarker.TimeStamp;
strideElement = 5;
}
break;
case 5:
strideElement = 0;
if ((foot == "left") && (direction == -1))
{
time[5] = footMarker.TimeStamp;
strideElement = 6;
}
break;
case 6:
strideElement = 0;
if ((foot == "right") && (direction == 1))
{
time[6] = footMarker.TimeStamp;
strideElement = 7;
}
break;
case 7:
strideElement = 0;
if ((foot == "right") && (direction == -1))
{
time[7] = footMarker.TimeStamp;
strideStop = footMarker.zCoordinate;
//now have full set of data for 2 strides for symmetry calculations
//do calculations and fire an event
symmetryCalculation();
}
break;
}
}
