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;
}
}
}
/// <summary>
/// Transmits a joint to the current tcpClient.
/// </summary>
/// <param name="joint"></param>
public void Transmit(Joint joint)
{
//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('J');
binaryWriter.Write(joint.ID);
binaryWriter.Write(joint.TimeStamp);
binaryWriter.Write(joint.Exists);
binaryWriter.Write(joint.Name);
binaryWriter.Write(joint.Trackable1);
binaryWriter.Write(joint.Trackable2);
binaryWriter.Write(joint.RollOffset);
binaryWriter.Write(joint.PitchOffset);
binaryWriter.Write(joint.YawOffset);
binaryWriter.Write(joint.Roll);
binaryWriter.Write(joint.Pitch);
binaryWriter.Write(joint.Yaw);
binaryWriter.Write(joint.xCoordinate);
binaryWriter.Write(joint.yCoordinate);
binaryWriter.Write(joint.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);
}
}
}
/// <summary>
/// Loads in the joint definitions from an xml based file (Returns 0=Success / -1= Failed to find file)
/// </summary>
/// <param name="file">The file containing the joint definitions</param>
/// <returns></returns>
public static int LoadJointDefinition(string file)
{
Joint newJoint = null;
int result = 0;
jointList = null;
jointList = new List<Joint>();
// START Added for MEng Project
jointCoRList = new List<CoRLocator>();
try
{
//Create the xml reader
XmlTextReader xmlJoints = new XmlTextReader(file);
//While reading
while (xmlJoints.Read())
{
//Get the node type
XmlNodeType nodeType = xmlJoints.NodeType;
if (nodeType == XmlNodeType.Element) // If its an element
{
switch (xmlJoints.Name)
{
case "joint": // New joint
newJoint = new Joint();
break;
case "id":
newJoint.ID = xmlJoints.ReadElementContentAsInt();
break;
case "name":
newJoint.Name = xmlJoints.ReadElementContentAsString();
break;
case "trackable1":
newJoint.Trackable1 = xmlJoints.ReadElementContentAsInt();
break;
case "trackable2":
newJoint.Trackable2 = xmlJoints.ReadElementContentAsInt();
break;
case "yawOffset":
newJoint.YawOffset = xmlJoints.ReadElementContentAsDouble();
break;
case "pitchOffset":
newJoint.PitchOffset = xmlJoints.ReadElementContentAsDouble();
break;
case "rollOffset":
newJoint.RollOffset = xmlJoints.ReadElementContentAsDouble();
break;
}
}
// If its the joint end element add the joint to the joint list
if (nodeType == XmlNodeType.EndElement && xmlJoints.Name == "joint")
{
jointList.Add(newJoint);
// START Added for MEng Project
// Add a corrisponding CoR Locator to the list
jointCoRList.Add(new CoRLocator());
// END Added for MEng Project
}
}
}
catch
{
result = -1; // File not found;
}
return result;
}
void JointProcessor_JointListAvaliableEvent(List<Joint> jointList)
{
List<Joint> tempJointList = new List<Joint>(jointList);
int numJoints = 0;
foreach (Joint joint in tempJointList)
{
if (joint.Name == null) // Prevents binarywriter inside Transmit() trying to write a null name
joint.Name = "Unknown";
if (joint.Exists)
{
Transmit(joint);
numJoints += 1;
}
}
Joint endOfFrameJoint = new Joint();
endOfFrameJoint.TimeStamp = numJoints;
endOfFrameJoint.ID = -2147483648;
endOfFrameJoint.Name = "End";
Transmit(endOfFrameJoint);
}
