/// <summary>
/// Updates a camera in the list
/// </summary>
/// <param name="camera">The camera</param>
private static void UpdateCameraInList(OptiTrackCamera camera)
{
listOfCameras[camera.CameraNumber].CameraNumber = camera.CameraNumber;
listOfCameras[camera.CameraNumber].xCoordinate = camera.xCoordinate;
listOfCameras[camera.CameraNumber].yCoordinate = camera.yCoordinate;
listOfCameras[camera.CameraNumber].zCoordinate = camera.zCoordinate;
for (int i = 0; i < 9; i++)
{
listOfCameras[camera.CameraNumber].RotationMatrix[i] = camera.RotationMatrix[i];
}
}
/// <summary>
/// Constructor
/// </summary>
public OptiTrackCamera(OptiTrackCamera newCamera)
{
cameraNumber = newCamera.CameraNumber;
cameraName = newCamera.cameraName;
x = newCamera.xCoordinate;
y = newCamera.yCoordinate;
z = newCamera.zCoordinate;
for (int i = 0; i < 9; i++)
{
rotationMatrix[i] = newCamera.rotationMatrix[i];
}
}
/// <summary>
/// Constructor
/// </summary>
public OptiTrackCamera(OptiTrackCamera newCamera)
{
cameraNumber = newCamera.CameraNumber;
cameraName = newCamera.cameraName;
x = newCamera.xCoordinate;
y = newCamera.yCoordinate;
z = newCamera.zCoordinate;
for (int i = 0; i < 9; i++)
{
rotationMatrix[i] = newCamera.rotationMatrix[i];
}
}
/// <summary>
/// Updates a camera in the list
/// </summary>
/// <param name="camera">The camera</param>
private static void UpdateCameraInList(OptiTrackCamera camera)
{
listOfCameras[camera.CameraNumber].CameraNumber = camera.CameraNumber;
listOfCameras[camera.CameraNumber].xCoordinate = camera.xCoordinate;
listOfCameras[camera.CameraNumber].yCoordinate = camera.yCoordinate;
listOfCameras[camera.CameraNumber].zCoordinate = camera.zCoordinate;
for (int i = 0; i < 9; i++)
{
listOfCameras[camera.CameraNumber].RotationMatrix[i] = camera.RotationMatrix[i];
}
}
/// <summary>
/// Adds a camera
/// </summary>
/// <param name="camera">The camera to add</param>
public static void AddCamera(OptiTrackCamera camera)
{
bool cameraFound = false;
foreach (OptiTrackCamera currentCamera in listOfCameras)
{
if (camera.CameraNumber == currentCamera.CameraNumber)
{
cameraFound = true;
if (currentCamera.CameraNumber != -1)
{
UpdateCameraInList(currentCamera);
}
}
}
if (cameraFound == false)
{
listOfCameras.Add(new OptiTrackCamera(camera));
}
}
/// <summary>
/// Adds a camera
/// </summary>
/// <param name="camera">The camera to add</param>
public static void AddCamera(OptiTrackCamera camera)
{
bool cameraFound = false;
foreach (OptiTrackCamera currentCamera in listOfCameras)
{
if (camera.CameraNumber == currentCamera.CameraNumber)
{
cameraFound = true;
if (currentCamera.CameraNumber != -1)
{
UpdateCameraInList(currentCamera);
}
}
}
if (cameraFound == false)
{
listOfCameras.Add(new OptiTrackCamera(camera));
}
}
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 camera to the current tcpClient
/// </summary>
/// <param name="camera"></param>
public void Transmit(OptiTrackCamera camera)
{
lock (m_lock)
{
MotionCaptureController.MarkerListAvaliable -= new MotionCaptureController.MarkerListAvaliableEventHandler(MotionCapture_MarkerListAvaliableEvent);
//if there is a client connection
if (clientConnectionList.Count != 0)
{
//and if the client is connected
if (clientConnectionList[0].Connected == true)
{
foreach (TcpClient singleClientConnection in clientConnectionList)
{
BinaryWriter binaryWriter = new BinaryWriter(new BufferedStream(singleClientConnection.GetStream()));
binaryWriter.Write('C');
binaryWriter.Write(camera.CameraNumber);
binaryWriter.Write(camera.xCoordinate);
binaryWriter.Write(camera.yCoordinate);
binaryWriter.Write(camera.zCoordinate);
double[] rotationMatrixBuffer = camera.RotationMatrix;
System.Diagnostics.Debug.WriteLine("Camera rotation matrix: ");
for (int i = 0; i < 9; i++)
{
System.Diagnostics.Debug.WriteLine(rotationMatrixBuffer[i].ToString());
binaryWriter.Write(rotationMatrixBuffer[i]);
}
binaryWriter.Flush();
}
}
}
MotionCaptureController.MarkerListAvaliable += new MotionCaptureController.MarkerListAvaliableEventHandler(MotionCapture_MarkerListAvaliableEvent);
}
}
private void OptitrackCameraList_TransmitCameraEvent(OptiTrackCamera camera)
{
Transmit(camera);
}
/// <summary>
/// Updates the camera list
/// </summary>
public static void UpdateCameraList()
{
if (APIRunning)
{
numberOfCameras = TrackingTools.CameraCount();
OptiTrackCamera camera = new OptiTrackCamera();
connectedCameraDetails = " Number of Cameras Connected: " + numberOfCameras + "\n";
double[] tempRotationMatrix = new double[9];
int j = 0;
for (int i = 0; i < numberOfCameras; i++)
{
camera.CameraNumber = i;
camera.CameraName = TrackingTools.CameraName(i);
camera.xCoordinate = TrackingTools.CameraLocationX(i) * 1000;
camera.yCoordinate = TrackingTools.CameraLocationY(i) * 1000;
camera.zCoordinate = TrackingTools.CameraLocationZ(i) * 1000;
connectedCameraDetails += " Camera : " + camera.CameraName + "\n";
j = 0;
for (j = 0; j < 9; j++)
{
tempRotationMatrix[j] = TrackingTools.CameraOrientation(i, j);
}
camera.RotationMatrix = tempRotationMatrix;
OptitrackCameraList.AddCamera(camera);
}
}
}
