/// <summary>
/// Constructor
/// </summary>
/// <param name="newTrackable">New Trackable</param>
public Trackable(Trackable newTrackable)
{
x = newTrackable.xCoordinate;
y = newTrackable.yCoordinate;
x = newTrackable.zCoordinate;
pitch = newTrackable.Pitch;
yaw = newTrackable.Yaw;
roll = newTrackable.Roll;
qw = newTrackable.qw;
qx = newTrackable.qx;
qy = newTrackable.qy;
qz = newTrackable.qz;
id = newTrackable.ID;
timeStamp = newTrackable.TimeStamp;
name = newTrackable.Name;
trackableIndex = newTrackable.TrackableIndex;
trackableMarkers = null;
}
/// <summary>
/// Constructor
/// </summary>
/// <param name="newTrackable">New Trackable</param>
public Trackable(Trackable newTrackable)
{
x = newTrackable.xCoordinate;
y = newTrackable.yCoordinate;
x = newTrackable.zCoordinate;
pitch = newTrackable.Pitch;
yaw = newTrackable.Yaw;
roll = newTrackable.Roll;
qw = newTrackable.qw;
qx = newTrackable.qx;
qy = newTrackable.qy;
qz = newTrackable.qz;
id = newTrackable.ID;
timeStamp = newTrackable.TimeStamp;
name = newTrackable.Name;
trackableIndex = newTrackable.TrackableIndex;
trackableMarkers = null;
}
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();
}
}
/// <summary>
/// Transmits a trackable to the current tcpClient.
/// </summary>
/// <param name="trackable"></param>
public void Transmit(Trackable trackable)
{
//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('R'); // T is used for Treadmill
binaryWriter.Write(trackable.ID);
binaryWriter.Write(trackable.TimeStamp);
binaryWriter.Write(trackable.TrackableIndex);
binaryWriter.Write(trackable.Name);
binaryWriter.Write(trackable.xCoordinate);
binaryWriter.Write(trackable.yCoordinate);
binaryWriter.Write(trackable.zCoordinate);
binaryWriter.Write(trackable.Roll);
binaryWriter.Write(trackable.Pitch);
binaryWriter.Write(trackable.Yaw);
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_TrackableListAvaliableEvent(List<Trackable> trackableList, long timeStamp)
{
tempTrackableList = new List<Trackable>(trackableList);
tempTrackableListTimeStamp = timeStamp;
foreach (Trackable trackable in tempTrackableList)
{
trackable.TimeStamp = timeStamp;
if (trackable.Name == null) // Prevents binarywriter in Transmit trying to write a null name
trackable.Name = "Unknown";
Transmit(trackable);
}
Trackable endOfFrameTrackable = new Trackable();
endOfFrameTrackable.TimeStamp = trackableList.Count;
endOfFrameTrackable.ID = -2147483648;
endOfFrameTrackable.Name = "End";
Transmit(endOfFrameTrackable);
//Filter Markers - if they are from the same camera update FilteredMarkers
if (tempTrackableListTimeStamp == tempMarkerListTimeStamp && processFilteredMarkers)
FilteredMarkerProcessor.UpdatedFilteredMarkers(tempMarkerList, tempTrackableList);
//Update Joints
JointProcessor.UpdateJoints(tempTrackableList);
}
/// <summary>
/// Returns the Trackable for the index specified
/// </summary>
/// <param name="TrackableIndex">The index of the trackable</param>
/// <returns>The result</returns>
public static Trackable GetTrackableLocation(int TrackableIndex)
{
Trackable trackable = new Trackable();
float x, y, z;
float qx, qy, qz, qw;
float yaw, pitch, roll;
GetTrackableLocation(TrackableIndex, out x, out y, out z, out qx, out qy, out qz, out qw, out yaw, out pitch, out roll);
trackable.xCoordinate = (int)(x * 1000); // Positive faceing the screen going right
trackable.yCoordinate = (int)(y * 1000); // Positive Up
trackable.zCoordinate = (int)(z * 1000); // Positive Towards the screen
trackable.Yaw = -Math.Round(yaw, 1); // invert yaw
trackable.Pitch = Math.Round(roll, 1); // Swap around pitch and roll
trackable.Roll = Math.Round(pitch, 1);
trackable.TimeStamp = System.DateTime.Now.Ticks;
trackable.ID = TrackableID(TrackableIndex);
trackable.Name = TT_TrackableName(TrackableIndex);
trackable.TrackableIndex = TrackableIndex;
trackable.QW = qw;
trackable.QX = qx;
trackable.QY = qy;
trackable.QZ = qz;
Marker trackableMarker = null;
trackable.TrackableMarkers = new List<Marker>();
// Build the rotation matrix
StroMoHab_Matrix.RotationMatrix rotationMatix = new StroMoHab_Matrix.RotationMatrix(Math.PI * trackable.Pitch / 180, Math.PI * trackable.Yaw / 180, Math.PI * trackable.Roll / 180);
//loop through and get all of the markers that are part of the trackable, apply the rotation matrix, and add them
for (int j = 0; j < TrackingTools.TrackableMarkerCount(TrackableIndex); j++)
{
trackableMarker = TrackingTools.TrackableMarker(TrackableIndex, j); // get the marker
// rotate the marker
StroMoHab_Matrix.PointMatrix newPointMatrix = StroMoHab_Matrix.Operations.Rotate(new StroMoHab_Matrix.PointMatrix(trackableMarker.xCoordinate, trackableMarker.yCoordinate, trackableMarker.zCoordinate), rotationMatix);
//get the new marker position
trackableMarker.xCoordinate = Math.Round(newPointMatrix.XCoordinate, 1);
trackableMarker.yCoordinate = Math.Round(newPointMatrix.YCoordinate, 1);
trackableMarker.zCoordinate = Math.Round(newPointMatrix.ZCoordinate, 1);
trackable.TrackableMarkers.Add(trackableMarker);
}
return trackable;
}
public void CalibrateFoot(Trackable calibrationTrackable)
{
//Stromohab_MCE_Connection.TCPProcessor.TrackableListReceivedEvent +=
// new TCPProcessor.TrackableListReceivedHandler(TCPProcessor_TrackableListReceivedEvent);
if (footName == "left") currentID = 1;
if (footName == "right") currentID = 2;
this.calibrationTrackable = calibrationTrackable;
previousTrackable = calibrationTrackable;
PositionData positionCalibration = PositionData.ConvertMarkerPosition((Marker)calibrationTrackable);
this.footPositionList.Add(positionCalibration);
footdown_yCoordinate = calibrationTrackable.yCoordinate;
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 = previousTrackable.zCoordinate;
yPreviousCoordinate = previousTrackable.yCoordinate;
xPreviousCoordinate = previousTrackable.xCoordinate;
}
private static bool RightFoot(Trackable t)
{
if (t.ID == 2)
return true;
else
return false;
}
private static bool LeftFoot(Trackable t)
{
if (t.ID == 1)
return true;
else
return false;
}
/// <summary>
/// Finds an updated CoR for the two Trackables given.
/// </summary>
/// <param name="T1">Trackable1</param>
/// <param name="T2">Trackable2</param>
/// <returns>The CoR Matrix, containg the x, y, and z coordinates</returns>
public Matrix FindCoR(Trackable T1, Trackable T2)
{
//Increase the frame number
frameNumber++;
// Declare rotation matricies
Matrix RT1, RT2;
// Build rotation matrix from quaternion rotation data
RT1 = StroMoHab_Matrix.Operations.BuildRotorFromQuaternionData(T1);
RT2 = StroMoHab_Matrix.Operations.BuildRotorFromQuaternionData(T2);
// Transpose the two rotation matricies
Matrix RT1_Trans = StroMoHab_Matrix.Operations.Transpose(RT1);
Matrix RT2_Trans = StroMoHab_Matrix.Operations.Transpose(RT2);
// Add the next set of data to the summation
RT1_TransRT2 = RT1_TransRT2 + RT1_Trans * RT2;
RT2_TransRT1 = RT2_TransRT1 + RT2_Trans * RT1;
// Put the centre of the Trackables and the distances between them in matrix form
Matrix T1p = Matrix.Create(new double[, ] { { T1.xCoordinate }, { T1.yCoordinate }, { T1.zCoordinate } });
Matrix T2p = Matrix.Create(new double[, ] { { T2.xCoordinate }, { T2.yCoordinate }, { T2.zCoordinate } });
Matrix bT1T2 = T1p - T2p;
// Add the next set of data to the summation
RT1_TransbT1T2 = RT1_TransbT1T2 + RT1_Trans * bT1T2;
RT2_TransbT1T2 = RT2_TransbT1T2 + RT2_Trans * bT1T2;
// Build matrix b
Matrix b = new Matrix(6,1);
for (int i = 0; i < 3; i++)
{
b[i, 0] = RT1_TransbT1T2[i, 0];
b[i + 3, 0] = -RT2_TransbT1T2[i, 0];
}
// Build then get the inverse of matrix M
Matrix M_Inverse = BuildM_Inverse();
if (M_Inverse == null) // Inversion of M failed because its determinant was 0 so just return the previous CoR
return CoR;
// Solve Ma = b to find a
Matrix a = M_Inverse * b;
// Extract the two parts of a
Matrix aT1 = new Matrix(3,1);
Matrix aT2 = new Matrix(3,1);
for (int i = 0; i < 3; i++)
{
aT1[i, 0] = a[i, 0];
aT2[i, 0] = a[i + 3, 0];
}
// Calculate the two centres of rotation
Matrix CoR1 = (RT1 * aT1) + T1p;
Matrix CoR2 = (RT2 * aT2) + T2p;
// The final CoR is the average of the two CoR
for (int i = 0; i < 3; i++)
{
CoR[i, 0] = (CoR1[i, 0] + CoR2[i, 0]) / 2;
}
return CoR;
}
public OpenGlTrackable(Trackable trackable)
: base((float)(trackable.xCoordinate/SCALING_FACTOR)-m_radius,(float)(trackable.yCoordinate/SCALING_FACTOR),-(float)(trackable.zCoordinate /SCALING_FACTOR)-m_radius,(float)(trackable.xCoordinate/SCALING_FACTOR)+m_radius,(float)(trackable.yCoordinate/SCALING_FACTOR),-(float)(trackable.zCoordinate/SCALING_FACTOR)+m_radius)
{
m_trackable = trackable;
}
