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;
}
public static PositionData ConvertMarkerPosition(Marker marker)
{
PositionData position = new PositionData();
position.MarkerID = marker.MarkerId;
position.Time = marker.TimeStamp;
position.XCoordinate = marker.xCoordinate;
position.YCoordinate = marker.yCoordinate;
position.ZCoordinate = marker.zCoordinate;
return position;
}
/// <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;
prevX = newMarker.prevXCoordinate;
prevY = newMarker.prevYCoordinate;
prevZ = newMarker.prevZCoordinate;
m_prevTimestamp = newMarker.PrevTimestamp;
}
/// <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);
return;
}
}
}
if (markerFound == false)
{
listOfMarkers.Add(new Marker(currentMarker));
}
}
public void CalibrateFoot(Marker calibrationMarker)
{
footPositionList = new List<PositionData>();
//TCPProcessor.WholeFrameReceivedEvent +=
// new TCPProcessor.WholeFrameReceivedHandler(TCPProcessor_WholeFrameReceivedEvent);
this.calibrationMarker = calibrationMarker;
previousMarker = calibrationMarker;
//
currentMarker = new Marker(previousMarker);
//
PositionData positionCalibration = PositionData.ConvertMarkerPosition(calibrationMarker);
this.footPositionList.Add(positionCalibration);
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 -> moving towards back of treadmill
xPrevDirection = 0;
yPrevDirection = 0;
zPrevDirection = 0;
zPreviousCoordinate = previousMarker.zCoordinate;
yPreviousCoordinate = previousMarker.yCoordinate;
xPreviousCoordinate = previousMarker.xCoordinate;
}
private static void HandleDataFromServer(object client)
{
TcpClient serverConnection = (TcpClient)client;
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': // Marker
Marker receivedMarker = new Marker();
receivedMarker.MarkerId = binaryReader.ReadInt32();
//if have been sent End of Frame indication
if (receivedMarker.MarkerId == -2147483648)
{
receivedMarker.TimeStamp = binaryReader.ReadInt64();
readTimestamp = true;
MarkerList.RemoveExcessMarkersFromList(receivedMarker.TimeStamp);
if (WholeFrameReceivedEvent != null)
{
WholeFrameReceivedEvent();
}
}
if (readTimestamp == false)
{
receivedMarker.TimeStamp = binaryReader.ReadInt64();
}
receivedMarker.xCoordinate = binaryReader.ReadDouble();
receivedMarker.yCoordinate = binaryReader.ReadDouble();
receivedMarker.zCoordinate = binaryReader.ReadDouble();
bytesRead = 36;
if (receivedMarker.MarkerId != -2147483648)
{
MarkerList.AddMarker(receivedMarker);
}
break;
case 'F': // Filtered Marker
Marker receivedFilteredMarker = new Marker();
receivedFilteredMarker.MarkerId = binaryReader.ReadInt32();
//if have been sent End of Frame indication
if (receivedFilteredMarker.MarkerId == -2147483648)
{
receivedFilteredMarker.TimeStamp = binaryReader.ReadInt64();
readTimestamp = true;
FilteredMarkerList.RemoveExcessMarkersFromList(receivedFilteredMarker.TimeStamp);
if (FilteredMarkerListReceivedEvent != null)
{
FilteredMarkerListReceivedEvent();
}
}
if (readTimestamp == false)
{
receivedFilteredMarker.TimeStamp = binaryReader.ReadInt64();
}
receivedFilteredMarker.xCoordinate = binaryReader.ReadDouble();
receivedFilteredMarker.yCoordinate = binaryReader.ReadDouble();
receivedFilteredMarker.zCoordinate = binaryReader.ReadDouble();
bytesRead = 36;
if (receivedFilteredMarker.MarkerId != -2147483648)
{
FilteredMarkerList.AddMarker(receivedFilteredMarker);
}
break;
case 'R': // Trackable
Trackable receivedTrackable = new Trackable();
receivedTrackable.ID = binaryReader.ReadInt32();
receivedTrackable.TimeStamp = binaryReader.ReadInt64();
if (receivedTrackable.ID == -2147483648)
{
if (receivedTrackable.TimeStamp == 0)
trackableList.Clear();
//End of List
if (TrackableListReceivedEvent != null)
TrackableListReceivedEvent(trackableList);
//Read in any remaining data out of the buffer
receivedTrackable.TrackableIndex = binaryReader.ReadInt32();
receivedTrackable.Name = binaryReader.ReadString();
receivedTrackable.xCoordinate = binaryReader.ReadInt32();
receivedTrackable.yCoordinate = binaryReader.ReadInt32();
receivedTrackable.zCoordinate = binaryReader.ReadInt32();
receivedTrackable.Roll = binaryReader.ReadDouble();
receivedTrackable.Pitch = binaryReader.ReadDouble();
receivedTrackable.Yaw = binaryReader.ReadDouble();
}
else
{
if (trackableList.Count != 0) // alread contains data
{
//If from a previous time frame, clear the list
if (receivedTrackable.TimeStamp != trackableList[0].TimeStamp)
{
trackableList.Clear();
}
}
receivedTrackable.TrackableIndex = binaryReader.ReadInt32();
receivedTrackable.Name = binaryReader.ReadString();
receivedTrackable.xCoordinate = binaryReader.ReadInt32();
receivedTrackable.yCoordinate = binaryReader.ReadInt32();
receivedTrackable.zCoordinate = binaryReader.ReadInt32();
receivedTrackable.Roll = binaryReader.ReadDouble();
receivedTrackable.Pitch = binaryReader.ReadDouble();
receivedTrackable.Yaw = binaryReader.ReadDouble();
trackableList.Add(receivedTrackable);
}
break;
case 'J': // Joint
Joint receivedJoint = new Joint();
receivedJoint.ID = binaryReader.ReadInt32();
receivedJoint.TimeStamp = binaryReader.ReadInt64();
if (receivedJoint.ID == -2147483648)
{
if (receivedJoint.TimeStamp == 0)
jointList.Clear();
//End of List
if (JointListReceivedEvent != null)
JointListReceivedEvent(jointList);
//Read in any remaining data out of the buffer
receivedJoint.Exists = binaryReader.ReadBoolean();
receivedJoint.Name = binaryReader.ReadString();
receivedJoint.Trackable1 = binaryReader.ReadInt32();
receivedJoint.Trackable2 = binaryReader.ReadInt32();
receivedJoint.RollOffset = binaryReader.ReadDouble();
receivedJoint.PitchOffset = binaryReader.ReadDouble();
receivedJoint.YawOffset = binaryReader.ReadDouble();
receivedJoint.Roll = binaryReader.ReadDouble();
receivedJoint.Pitch = binaryReader.ReadDouble();
receivedJoint.Yaw = binaryReader.ReadDouble();
receivedJoint.xCoordinate = binaryReader.ReadInt32();
receivedJoint.yCoordinate = binaryReader.ReadInt32();
receivedJoint.zCoordinate = binaryReader.ReadInt32();
}
else
{
if (jointList.Count != 0) // alread contains data
{
//If from a previous time frame, clear the list
if (receivedJoint.TimeStamp != jointList[0].TimeStamp)
{
jointList.Clear();
}
}
receivedJoint.Exists = binaryReader.ReadBoolean();
receivedJoint.Name = binaryReader.ReadString();
receivedJoint.Trackable1 = binaryReader.ReadInt32();
receivedJoint.Trackable2 = binaryReader.ReadInt32();
receivedJoint.RollOffset = binaryReader.ReadDouble();
receivedJoint.PitchOffset = binaryReader.ReadDouble();
receivedJoint.YawOffset = binaryReader.ReadDouble();
receivedJoint.Roll = binaryReader.ReadDouble();
receivedJoint.Pitch = binaryReader.ReadDouble();
receivedJoint.Yaw = binaryReader.ReadDouble();
receivedJoint.xCoordinate = binaryReader.ReadInt32();
receivedJoint.yCoordinate = binaryReader.ReadInt32();
receivedJoint.zCoordinate = binaryReader.ReadInt32();
jointList.Add(receivedJoint);
}
break;
case 'C': // Camera
OptiTrackCamera receivedCamera = new OptiTrackCamera();
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': // Treadmill (Speed)
if (AvatarSpeedChangedEvent != null)
{
AvatarSpeedChangedEvent((float)binaryReader.ReadDouble());
bytesRead = 8;
}
else
{
binaryReader.ReadDouble();
bytesRead = 8;
}
break;
case 'B': //flag type and flag
{
System.Diagnostics.Debug.WriteLine("MessageType: " + messageType.ToString());
char type = binaryReader.ReadChar();
System.Diagnostics.Debug.WriteLine("Type: " + type.ToString());
bool flag = binaryReader.ReadBoolean();
System.Diagnostics.Debug.WriteLine("Flag: " + flag.ToString());
bytesRead = 9;
switch (type)
{
case 'F': //Toggle Feet
{
if (ToggleDrawingOfFeetEvent != null)
{
ToggleDrawingOfFeetEvent(flag);
}
break;
}
}
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;
}
}
}
public void Reset()
{
m_leftFootMarker = null;
m_rightFootMarker = null;
m_leftFootTrackable = null;
m_rightFootTrackable = null;
}
/// <summary>
/// Stores the calibration (inital) positions of the left and right feet.
/// </summary>
public void Store(Trackable leftFoot, Trackable rightFoot)
{
m_leftFootTrackable = new Trackable(leftFoot);
m_rightFootTrackable = new Trackable(rightFoot);
m_leftFootMarker = null;
m_rightFootMarker = null;
if (m_calibrationUpdated != null)
{
m_calibrationUpdated();
}
}
/// <summary>
/// Stores the calibration (inital) positions of the left and right feet.
/// </summary>
public void Store(Marker leftFoot, Marker rightFoot)
{
m_leftFootMarker = new Marker(leftFoot);
m_rightFootMarker = new Marker(rightFoot);
m_leftFootTrackable = null;
m_rightFootTrackable = null;
if (m_calibrationUpdated != null)
{
m_calibrationUpdated();
}
}
private void TCPProcessor_TrackableListReceivedEvent(List<Trackable> newTrackableList)
{
trackableList = new List<Trackable>(newTrackableList);
Marker m1 = (Marker)(trackableList.Find(LeftFoot));
Marker m2 = (Marker)(trackableList.Find(RightFoot));
if (currentID == 1) currentMarker = m1;
if (currentID == 2) currentMarker = m2;
PositionData footData = PositionData.ConvertMarkerPosition(currentMarker);
this.footPositionList.Add(footData);
MarkerTrack();
//FrontFoot(m1, m2);
}
/// <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_MarkerListAvaliableEvent(List<Marker> markerList, long timeStamp)
{
tempMarkerList = new List<Marker>(markerList);
tempMarkerListTimeStamp = timeStamp;
foreach (Marker marker in tempMarkerList)
{
Transmit(marker);
}
Marker endOfFrameMarker = new Marker();
endOfFrameMarker.TimeStamp = markerList.Count;
endOfFrameMarker.MarkerId = -2147483648;
Transmit(endOfFrameMarker);
}
void FilteredMarkerProcessor_FilteredMarkerListAvaliableEvent()
{
List<Marker> tempFilteredMarkerList = new List<Marker>(FilteredMarkerList.listOfMarkers);
int[] treadmillMakrerIDNumbers;
if (mcTreadmill.DetectTreadmillSpeed)
{
treadmillMakrerIDNumbers = mcTreadmill.FindSpeed(tempFilteredMarkerList); // Find the speed and get the 1st and (if applicable) 2nd treadmill marker id
if (treadmillMakrerIDNumbers != null) // If a marker number was returned then strip them from the list
{
int newMarkerNumber = 0;
List<Marker> newTempFilteredMarkerList = new List<Marker>();
foreach (Marker tempMarker in tempFilteredMarkerList)
{
//If the marker ID doesn't match the first or second tredmill marker then add it to the new filtered makrer lsit
//or the 3rd or 4th - now extended to allow for upto 4 visible treadmill markers
if ((tempMarker.MarkerId != treadmillMakrerIDNumbers[0]) && (tempMarker.MarkerId != treadmillMakrerIDNumbers[1]))
//&& (tempMarker.MarkerId != treadmillMakrerIDNumbers[2]) && (tempMarker.MarkerId != treadmillMakrerIDNumbers[3]))
{
Marker newTempMarker = new Marker(tempMarker);
newTempMarker.MarkerId = newMarkerNumber;
newMarkerNumber++;
newTempFilteredMarkerList.Add(newTempMarker);
System.Diagnostics.Debug.WriteLine("Filtered marker x/y/z:" + newTempMarker.xCoordinate + " " + newTempMarker.yCoordinate + " " + newTempMarker.zCoordinate);
}
}
//System.Diagnostics.Debug.WriteLine("\n");
// Save the newly generated list
tempFilteredMarkerList = new List<Marker>(newTempFilteredMarkerList);
}
}
foreach (Marker marker in tempFilteredMarkerList)
{
TransmitFiltered(marker);
}
Marker endOfFrameMarker = new Marker();
endOfFrameMarker.TimeStamp = tempFilteredMarkerList.Count;
endOfFrameMarker.MarkerId = -2147483648;
TransmitFiltered(endOfFrameMarker);
}
/// <summary>
/// Updates the list of markers with lastest marker coordinates
/// </summary>
public static void UpdateCoordinates(bool updateMarker, bool updateTrackable)
{
if (APIRunning)
{
// Get the timestamp to apply to all markers and trackables
long timeStamp = System.DateTime.Now.Ticks;
try
{
if (TrackingTools.UpdateAll() == 0) // If it updates correctly
{
if (updateMarker)
{
int markerCount = TrackingTools.FrameMarkerCount();
Marker marker = new Marker();
for (int i = 0; i < markerCount; i++)
{
//Get the marker details from TrackingTools
marker.MarkerId = i;
marker.TimeStamp = timeStamp;
marker.xCoordinate = -(int)(TrackingTools.FrameMarkerX(i) * 1000);
marker.yCoordinate = (int)(TrackingTools.FrameMarkerY(i) * 1000);
marker.zCoordinate = (int)(TrackingTools.FrameMarkerZ(i) * 1000);
MarkerList.AddMarker(marker);
marker = new Marker();
}
MarkerList.RemoveExcessMarkersFromList(markerCount);
OnMarkerListAvaliable(MarkerList.listOfMarkers, timeStamp);
}
if (updateTrackable)
{
int trackableCount = TrackingTools.TrackableCount();
trackableList.Clear();
Trackable newTrackable = null;
// Go through the loaded trackables and if they are in the current frame
// add them to the trackableList
for (int i = 0; i < trackableCount; i++)
{
if (TrackingTools.IsTrackableTracked(i))
{
newTrackable = TrackingTools.GetTrackableLocation(i);
newTrackable.TimeStamp = timeStamp;
trackableList.Add(newTrackable);
}
}
OnTrackableListAvaliable(trackableList, timeStamp);
}
}
}
catch (AccessViolationException ex)
{
System.Diagnostics.Debug.WriteLine("Error With UpdateALL() " + ex + " Time Stamp : " + timeStamp);
}
catch (System.Runtime.InteropServices.SEHException ex)
{
System.Diagnostics.Debug.WriteLine("Error With UpdateALL() " + ex + " Time Stamp : " + timeStamp);
}
}
}
/// <summary>
/// Constructor. Creates OpenGlMarker from an optitrack Marker.
/// </summary>
/// <param name="marker"></param>
public OpenGLMarker(Marker marker)
: base((float)(marker.xCoordinate / SCALING_FACTOR) - m_radius, (float)(marker.yCoordinate / SCALING_FACTOR), -(float)(marker.zCoordinate / SCALING_FACTOR) - m_radius, (float)(marker.xCoordinate / SCALING_FACTOR) + m_radius, (float)(marker.yCoordinate / SCALING_FACTOR), -(float)(marker.zCoordinate / SCALING_FACTOR) + m_radius)
{
m_marker = marker;
}
/// <summary>
/// Gets the marker that makes up the trackable
/// </summary>
/// <param name="TrackableIndex">The trackable index</param>
/// <param name="MarkerIndex">The marker index</param>
/// <returns>The marker</returns>
public static Marker TrackableMarker(int TrackableIndex, int MarkerIndex)
{
Marker marker = new Marker();
float x, y, z;
TrackableMarker(TrackableIndex, MarkerIndex, out x, out y, out z);
marker.TimeStamp = System.DateTime.Now.Ticks;
marker.xCoordinate = (int)(x * 1000);
marker.yCoordinate = (int)(y * 1000);
marker.zCoordinate = (int)(z * 1000);
return marker;
}
/// <summary>
/// Filters out all of the markers that are part of a trackable and leaves just single markers
/// </summary>
public static void UpdatedFilteredMarkers(List<Marker> markerList, List<Trackable> trackableList)
{
int markerCount = 0;
filteredMarkerList = new List<Marker>();
Marker trackableMarker = null;
if (trackableList.Count != 0) // If there are trackable
{
foreach (Marker marker in markerList) // Go through every marker
{
bool singleMarker = true;
foreach (Trackable trackable in trackableList) // Go through every trackable
{
foreach (Marker tempTrackableMarker in trackable.TrackableMarkers) // Go through every marker making up the trackable
{
trackableMarker = tempTrackableMarker;
if ((marker.yCoordinate >= trackableMarker.yCoordinate + trackable.yCoordinate - markerTollerence))
{
if ((marker.yCoordinate <= trackableMarker.yCoordinate + trackable.yCoordinate + markerTollerence))
{
if ((marker.xCoordinate >= trackableMarker.xCoordinate + trackable.xCoordinate - markerTollerence))
{
if ((marker.xCoordinate <= trackableMarker.xCoordinate + trackable.xCoordinate + markerTollerence))
{
if ((marker.zCoordinate >= trackableMarker.zCoordinate + trackable.zCoordinate - markerTollerence))
{
if ((marker.zCoordinate <= trackableMarker.zCoordinate + trackable.zCoordinate + markerTollerence))
singleMarker = false;
// This marker is part of a trackable and isn't a single marker
}
}
}
}
}
if (!singleMarker)
break; // If its been found the break out
}
if (!singleMarker)
break; // If its been found the break out
}
if (singleMarker) // If its a single marker make a copy and add it to the new list
{
markerCount++;
Marker filteredMarker = new Marker(marker);
filteredMarker.MarkerId= markerCount -1;
filteredMarkerList.Add(filteredMarker);
}
}
FilteredMarkerList.listOfMarkers = new List<Marker>(filteredMarkerList);
}
else // If there are no trackables then just copy across the list
{
FilteredMarkerList.listOfMarkers = new List<Marker>(markerList);
}
OnFilteredMarkerListAvaliable();
}
/*private PositionData ConvertMarkerPosition(Marker calibrationMarker)
{
PositionData position = new PositionData();
position.Time = calibrationMarker.TimeStamp;
position.XCoordinate = calibrationMarker.xCoordinate;
position.YCoordinate = calibrationMarker.yCoordinate;
position.ZCoordinate = calibrationMarker.zCoordinate;
return position;
}
*/
#endregion
//public void FootCleanUp()
//{
// TCPProcessor.WholeFrameReceivedEvent -=
// new TCPProcessor.WholeFrameReceivedHandler(TCPProcessor_WholeFrameReceivedEvent);
//}
#region Foot tracking code (depracated)
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 + 15)))// && (Math.Abs(xChange) < 3) && (Math.Abs(yChange) < 3))
{
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.currentMarker.zCoordinate == m1.zCoordinate)
{
frontfoot = this;
}
}
else
{
if (this.currentMarker.zCoordinate == m2.zCoordinate)
{
frontfoot = this;
}
}
}
/// <summary>
/// Saves the new marker data
/// </summary>
/// <param name="markerList"></param>
/// <param name="timeStamp"></param>
static void MotionCaptureController_MarkerListAvaliable(List<Marker> markerList, long timeStamp)
{
//Deep copy the list
_currentMarkerList = new List<Marker>();
foreach (Marker m in markerList)
{
Marker newM = new Marker(m);
_currentMarkerList.Add(newM);
}
_currentMarkerTimeStamp = timeStamp;
if (_currentMarkerTimeStamp == _currentTrackableTimeStamp)
SaveFrame();
}
void TCPProcessor_FilteredMarkerListReceivedEvent(List<StroMoHab_Objects.Objects.Marker> filteredMarkerList)
{
TCPProcessor.FilteredMarkerListReceivedEvent -= new TCPProcessor.FilteredMarkerListReceivedHandler(TCPProcessor_FilteredMarkerListReceivedEvent);
_filteredMarkerList = new List<Marker>();
foreach (Marker currentMarker in filteredMarkerList)
{
Marker copyOfMarker = new Marker(currentMarker);
_filteredMarkerList.Add(copyOfMarker);
}
//if (_waitForVisualiser.Enabled)
//{
// return;
//}
if (!_updatingGraph)
{
//_filteredMarkerList = new List<Marker>(filteredMarkerList);
if (_filteredMarkerList.Count > 0)
{
if (_filteredMarkerList.Count > 1)
{
//Make sure index 0 is always the leftmost so can keep GUI consistent.
if (_filteredMarkerList[0].xCoordinate < _filteredMarkerList[1].xCoordinate) //this feels backwards to me, but after 2 80+ hour weeks me brain can't figure out the logic. TODO: Investigate and resolve as necessary.
{
Marker tempMarker = new Marker(_filteredMarkerList[1]);
_filteredMarkerList[1] = new Marker(_filteredMarkerList[0]);
_filteredMarkerList[0] = new Marker(tempMarker);
}
}
#region ZedGraphControl1
if (zedGraphControl1.GraphPane != null)
{
if (zedGraphControl1.MasterPane.PaneList.Count > 0)
{
if (zedGraphControl1.MasterPane.PaneList[0] != null)
{
PopulatePositionGraph(zedGraphControl1.MasterPane.PaneList[0], 0);
}
}
if (zedGraphControl1.MasterPane.PaneList.Count > 1)
{
if (zedGraphControl1.MasterPane.PaneList[1] != null)
{
PopulatePositionGraph(zedGraphControl1.MasterPane.PaneList[1], 1);
}
}
if (zedGraphControl1.MasterPane.PaneList.Count > 2)
{
if (zedGraphControl1.MasterPane.PaneList[2] != null)
{
PopulateVelocityGraph(zedGraphControl1.MasterPane.PaneList[2], 2);
}
}
if (zedGraphControl1.MasterPane.PaneList.Count > 3)
{
if (zedGraphControl1.MasterPane.PaneList[3] != null)
{
PopulateSymmetryGraph(zedGraphControl1.MasterPane.PaneList[3]);
}
}
if (zedGraphControl1.MasterPane.PaneList.Count > 4)
{
if (zedGraphControl1.MasterPane.PaneList[4] != null)
{
PopulateVelocityGraph(zedGraphControl1.MasterPane.PaneList[4], 4);
}
}
}
#endregion ZedGraphControl1
}
}
TCPProcessor.FilteredMarkerListReceivedEvent += new TCPProcessor.FilteredMarkerListReceivedHandler(TCPProcessor_FilteredMarkerListReceivedEvent);
}
