public void TryConnect()
{
Rhino.RhinoApp.Write("Attemping to connect to NatNet server... ");
client = new NatNetClientML();
int res = client.Initialize("127.0.0.1", "127.0.0.1");
if (res != 0)
{
Rhino.RhinoApp.WriteLine("Failed.");
client = null;
IsConnected = false;
return;
}
Rhino.RhinoApp.WriteLine("Success.");
IsConnected = true;
// testing only
/*
* System.Random rnd = new Random();
* for (int i = 0; i < 10; ++i)
* {
* markers.Add(new Point3d((rnd.NextDouble() - 0.5) * 500.0, (rnd.NextDouble() - 0.5) * 500.0, (rnd.NextDouble() - 0.5) * 500.0));
* }
*/
client.OnFrameReady += GetMarkersCallback;
Rhino.Display.DisplayPipeline.PostDrawObjects += DisplayPipeline_DrawMarkers;
Rhino.Display.DisplayPipeline.CalculateBoundingBox += DisplayPipeline_MarkersBoundingBox;
Rhino.RhinoDoc.ActiveDoc.Views.Redraw();
}
private void NatNetClientOnOnFrameReady(FrameOfMocapData data, NatNetClientML client)
{
/* Exception handler for cases where assets are added or removed.
* Data description is re-obtained in the main function so that contents
* in the frame handler is kept minimal. */
if ((data.bTrackingModelsChanged ||
data.nMarkerSets != _markerSets.Count ||
data.nRigidBodies != _rigidBodies.Count ||
data.nSkeletons != _skeletons.Count))
{
Logger.Debug("\n===============================================================================\n");
Logger.Debug("Change in the list of the assets. Refetching the descriptions");
/* Clear out existing lists */
_dataDescriptor.Clear();
_markerSets.Clear();
_rigidBodies.Clear();
_skeletons.Clear();
_nameById.Clear();
/* [NatNet] Re-fetch the updated list of descriptors */
FetchDataDescriptor();
Logger.Debug("===============================================================================\n");
FireDataDescriptionChanged();
}
FireOnFrameReady(data);
}
private void OnOptiTrackFrameReady(FrameOfMocapData data, NatNetClientML client)
{
if (data != null)
{
List <Tuple <RigidBody, RigidBodyData> > rigidBodyInfo = new List <Tuple <RigidBody, RigidBodyData> >();
for (int i = 0; i < data.nRigidBodies; i++)
{
RigidBodyData rigidBodyData = data.RigidBodies[i];
int id = rigidBodyData.ID;
RigidBody rigidBody = GetRigidBody(id);
if (rigidBody != null)
{
rigidBodyInfo.Add(new Tuple <RigidBody, RigidBodyData>(rigidBody, rigidBodyData));
}
}
List <Marker> markerInfo = new List <Marker>();
for (int i = 0; i < data.nOtherMarkers; i++)
{
if (data.OtherMarkers[i].ID == -1)
{
markerInfo.Add(data.OtherMarkers[i]);
}
}
TrackingUpdated?.Invoke(this, new OptiTrackTrackingEventArgs(
rigidBodyInfo, markerInfo, m_natNetClient, data.fTimestamp));
}
}
public OptiTrackTrackingEventArgs(List <Tuple <RigidBody, RigidBodyData> > rigidBodyData,
List <Marker> marekers, NatNetClientML client, double timestamp)
: base(timestamp)
{
m_rigidBodyData = rigidBodyData;
m_markers = marekers;
m_client = client;
}
public static void GetMarkersCallback(FrameOfMocapData frame, NatNetClientML client)
{
markers = new List <Point3d>();
for (int i = 0; i < frame.nOtherMarkers; ++i)
{
Point3d pt = new Point3d(frame.OtherMarkers[i].x * 1000.0, frame.OtherMarkers[i].y * 1000.0, frame.OtherMarkers[i].z * 1000.0);
pt.Transform(xform);
markers.Add(pt);
marker_sizes.Add(1.0f);
//marker_sizes.Add(frame.OtherMarkers[i].size);
}
Rhino.RhinoDoc.ActiveDoc.Views.Redraw();
}
private void Initialize()
{
m_localAddr = IPAddress.Any;
if (m_localAddr != null)
{
if (m_natNetClient != null)
{
m_natNetClient.Uninitialize();
}
m_natNetClient = new NatNetClientML(Constants.MulticastConnectionType);
m_natNetClient.OnFrameReady += new FrameReadyEventHandler(OnOptiTrackFrameReady);
}
}
public RNNContext()
{
doc = Rhino.RhinoDoc.ActiveDoc;
markers = new List <Point3d>();
marker_sizes = new List <float>();
counter = 0;
SetPlane(Plane.WorldXY);
client = null;
Rhino.Display.DisplayPipeline.PostDrawObjects += DisplayPipeline_DrawMarkers;
Rhino.Display.DisplayPipeline.CalculateBoundingBox += DisplayPipeline_MarkersBoundingBox;
m_timer.Start();
}
private void ProcessFrame(FrameOfMocapData data, NatNetClientML client)
{
double frameDeltaTime = data.fTimestamp - frameTimestamp;
frameTimestamp = data.fTimestamp;
var receivedFrame = new InputFrame(data, frameDeltaTime);
var args = new FrameReceivedEventArgs {
ReceivedFrame = receivedFrame,
PrevBallPosition = ballPosition
};
ballPosition = receivedFrame.BallPosition;
FrameReceived?.Invoke(this, args);
}
static void connectToServer()
{
// [NatNet] Instantiate the client object
mNatNet = new NatNetClientML();
// [NatNet] Checking verions of the NatNet SDK library
int[] verNatNet = new int[4]; // Saving NatNet SDK version number
verNatNet = mNatNet.NatNetVersion();
// Console.WriteLine("NatNet SDK Version: {0}.{1}.{2}.{3}", verNatNet[0], verNatNet[1], verNatNet[2], verNatNet[3]);
// [NatNet] Connecting to the Server
// Console.WriteLine("\nConnecting...\n\tLocal IP address: {0}\n\tServer IP Address: {1}\n\n", mStrLocalIP, mStrServerIP);
NatNetClientML.ConnectParams connectParams = new NatNetClientML.ConnectParams();
connectParams.ConnectionType = mConnectionType;
connectParams.ServerAddress = mStrServerIP;
connectParams.LocalAddress = mStrLocalIP;
mNatNet.Connect(connectParams);
}
public void Connect(string clientIp, string serverIp, string connectionType)
{
/* [NatNet] Instantiate the client object */
_natNetClient = new NatNetClientML();
/* [NatNet] Checking verions of the NatNet SDK library */
var natNetVersion = _natNetClient.NatNetVersion();
Logger.Info("NatNet SDK Version: {0}.{1}.{2}.{3}", natNetVersion[0], natNetVersion[1], natNetVersion[2],
natNetVersion[3]);
/* [NatNet] Connecting to the Server */
Logger.Info("\nConnecting...\n\tLocal IP address: {0}\n\tServer IP Address: {1}\n\n",
clientIp, serverIp);
var connectParams = new NatNetClientML.ConnectParams
{
ConnectionType = connectionType == "unicast"
? ConnectionType.Unicast
: ConnectionType.Multicast,
ServerAddress = serverIp,
LocalAddress = clientIp
};
_natNetClient.Connect(connectParams);
_isConnected = FetchServerDescription();
if (_isConnected)
{
_natNetClient.OnFrameReady += NatNetClientOnOnFrameReady;
}
else
{
throw new ApplicationException("Could not connect to optitrack");
}
}
public void TryConnect()
{
if (client != null)
{
//client.Disconnect();
}
Rhino.RhinoApp.Write("Attemping to connect to NatNet server... ");
client = new NatNetClientML();
var cp = new NatNetClientML.ConnectParams();
cp.ConnectionType = ConnectionType.Multicast;
cp.LocalAddress = "127.0.0.1";
cp.ServerAddress = "127.0.0.1";
cp.ServerCommandPort = 1510;
cp.ServerDataPort = 1511;
int res = client.Connect(cp);
if (res != 0)
{
Rhino.RhinoApp.WriteLine("Failed.");
client = null;
IsConnected = false;
return;
}
Rhino.RhinoApp.WriteLine("Success.");
IsConnected = true;
fetchServerDescriptor();
client.OnFrameReady += new NatNetML.FrameReadyEventHandler(fetchFrameData);
Rhino.RhinoDoc.ActiveDoc.Views.Redraw();
}
public OptiTrackClient()
{
_natNetClient = new NatNetClientML();
TranslationUnitMultiplier = 1.0f;
}
public OptiTrackSystem(int connectionType = 0)
{
natNetClient = new NatNetClientML(connectionType);
ServerDescription = new ServerDescription();
ballPosition = Vector <double> .Build.Dense(3);
}
//static uint GPS_LEAPSECONDS_MILLIS = 18000;
static void processFrameData(NatNetML.FrameOfMocapData data)
{
bool data_gogo = true;
/* Parsing Rigid Body Frame Data */
for (int i = 0; i < mRigidBodies.Count; i++)
{
int rbID = mRigidBodies[i].ID; // Fetching rigid body IDs from the saved descriptions
for (int j = 0; j < data.nRigidBodies; j++)
{
if (rbID == data.RigidBodies[j].ID) // When rigid body ID of the descriptions matches rigid body ID of the frame data.
{
NatNetML.RigidBody rb = mRigidBodies[i]; // Saved rigid body descriptions
NatNetML.RigidBodyData rbData = data.RigidBodies[j]; // Received rigid body descriptions
if (rbData.Tracked == true)
{
#if DEBUG_MSG
Console.WriteLine("\tRigidBody ({0}):", rb.Name);
Console.WriteLine("\t\tpos ({0:N3}, {1:N3}, {2:N3})", rbData.x, rbData.y, rbData.z);
// Rigid Body Euler Orientation
float[] quat = new float[4] {
rbData.qx, rbData.qy, rbData.qz, rbData.qw
};
float[] eulers = new float[3];
eulers = NatNetClientML.QuatToEuler(quat, NATEulerOrder.NAT_XYZr); //Converting quat orientation into XYZ Euler representation.
double xrot = RadiansToDegrees(eulers[0]);
double yrot = RadiansToDegrees(eulers[1]);
double zrot = RadiansToDegrees(eulers[2]);
Console.WriteLine("\t\tori ({0:N3}, {1:N3}, {2:N3})", xrot, yrot, zrot);
#endif
if (drones.ContainsKey(rb.Name))
{
DroneData drone = drones[rb.Name];
drone.lost_count = 0;
long cur_ms = stopwatch.ElapsedMilliseconds;
if (drone.send_count > 0)
{
drone.send_count--;
}
else if (data_gogo)
{
drone.send_count = 10;
data_gogo = false;
MAVLink.mavlink_att_pos_mocap_t att_pos = new MAVLink.mavlink_att_pos_mocap_t();
att_pos.time_usec = (ulong)(cur_ms * 1000);
att_pos.x = rbData.x; //north
att_pos.y = rbData.z; //east
att_pos.z = -rbData.y; //down
att_pos.q = new float[4] {
rbData.qw, rbData.qx, rbData.qz, -rbData.qy
};
byte[] pkt = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.ATT_POS_MOCAP, att_pos);
if (drone.lastTime >= 0)
{
MAVLink.mavlink_vision_speed_estimate_t vis_speed = new MAVLink.mavlink_vision_speed_estimate_t();
float total_s = (cur_ms - drone.lastTime) * 0.001f;
vis_speed.x = (rbData.x - drone.lastPosN) / total_s;
vis_speed.y = (rbData.z - drone.lastPosE) / total_s;
vis_speed.z = (-rbData.y - drone.lastPosD) / total_s;
vis_speed.usec = (ulong)(cur_ms * 1000);
byte[] pkt2 = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.VISION_SPEED_ESTIMATE, vis_speed);
int total_len = pkt.Length + pkt2.Length;
byte[] uwb_data = new byte[10 + total_len + 1];
uwb_data[0] = 0x54;
uwb_data[1] = 0xf1;
uwb_data[2] = 0xff;
uwb_data[3] = 0xff;
uwb_data[4] = 0xff;
uwb_data[5] = 0xff;
uwb_data[6] = 2;
uwb_data[7] = drone.uwb_tag_id;
uwb_data[8] = (byte)(total_len & 0xff);
uwb_data[9] = (byte)(total_len >> 16);
Array.Copy(pkt, 0, uwb_data, 10, pkt.Length);
Array.Copy(pkt2, 0, uwb_data, 10 + pkt.Length, pkt2.Length);
byte chk_sum = 0;
foreach (byte uwb_data_byte in uwb_data)
{
chk_sum += uwb_data_byte;
}
uwb_data[uwb_data.Length - 1] = chk_sum;
mavSock.SendTo(uwb_data, drone.ep);
}
}
drone.lastTime = cur_ms;
drone.lastPosN = rbData.x;
drone.lastPosE = rbData.z;
drone.lastPosD = -rbData.y;
}
}
else
{
Console.WriteLine("\t{0} is not tracked in current frame", rb.Name);
/*if (drones.ContainsKey(rb.Name))
* {
* DroneData drone = drones[rb.Name];
* drone.lost_count++;
* if (drone.lost_count > 3)
* {
* drone.lost_count = 0;
* MAVLink.mavlink_gps_input_t gps_input = new MAVLink.mavlink_gps_input_t();
* gps_input.gps_id = 0;
* gps_input.fix_type = (byte)MAVLink.GPS_FIX_TYPE.NO_FIX;
* byte[] pkt = mavlinkParse.GenerateMAVLinkPacket10(MAVLink.MAVLINK_MSG_ID.GPS_INPUT, gps_input);
* mavSock.SendTo(pkt, drone.ep);
* }
* }*/
}
}
}
}
/* Parsing Skeleton Frame Data */
for (int i = 0; i < mSkeletons.Count; i++) // Fetching skeleton IDs from the saved descriptions
{
int sklID = mSkeletons[i].ID;
for (int j = 0; j < data.nSkeletons; j++)
{
if (sklID == data.Skeletons[j].ID) // When skeleton ID of the description matches skeleton ID of the frame data.
{
NatNetML.Skeleton skl = mSkeletons[i]; // Saved skeleton descriptions
NatNetML.SkeletonData sklData = data.Skeletons[j]; // Received skeleton frame data
Console.WriteLine("\tSkeleton ({0}):", skl.Name);
Console.WriteLine("\t\tSegment count: {0}", sklData.nRigidBodies);
/* Now, for each of the skeleton segments */
for (int k = 0; k < sklData.nRigidBodies; k++)
{
NatNetML.RigidBodyData boneData = sklData.RigidBodies[k];
/* Decoding skeleton bone ID */
int skeletonID = HighWord(boneData.ID);
int rigidBodyID = LowWord(boneData.ID);
int uniqueID = skeletonID * 1000 + rigidBodyID;
int key = uniqueID.GetHashCode();
NatNetML.RigidBody bone = (RigidBody)mHtSkelRBs[key]; //Fetching saved skeleton bone descriptions
//Outputting only the hip segment data for the purpose of this sample.
if (k == 0)
{
Console.WriteLine("\t\t{0:N3}: pos({1:N3}, {2:N3}, {3:N3})", bone.Name, boneData.x, boneData.y, boneData.z);
}
}
}
}
}
/* Parsing Force Plate Frame Data */
for (int i = 0; i < mForcePlates.Count; i++)
{
int fpID = mForcePlates[i].ID; // Fetching force plate IDs from the saved descriptions
for (int j = 0; j < data.nForcePlates; j++)
{
if (fpID == data.ForcePlates[j].ID) // When force plate ID of the descriptions matches force plate ID of the frame data.
{
NatNetML.ForcePlate fp = mForcePlates[i]; // Saved force plate descriptions
NatNetML.ForcePlateData fpData = data.ForcePlates[i]; // Received forceplate frame data
Console.WriteLine("\tForce Plate ({0}):", fp.Serial);
// Here we will be printing out only the first force plate "subsample" (index 0) that was collected with the mocap frame.
for (int k = 0; k < fpData.nChannels; k++)
{
Console.WriteLine("\t\tChannel {0}: {1}", fp.ChannelNames[k], fpData.ChannelData[k].Values[0]);
}
}
}
}
#if DEBUG_MSG
Console.WriteLine("\n");
#endif
}
public double[] get_last_frame(string track_type, int object_num)
{
/*
* when asking for data set, default to the first marker set
*/
//grab a frame
FrameOfMocapData data = mNatNet.GetLastFrameOfData();
//
switch (track_type)
{
default:
Console.WriteLine("Unrecognized track object type {0}", track_type);
return(null);
case "markers": //defailt to return first marker of the firsr marker set
NatNetML.MarkerSetData ms = data.MarkerSets[0];
if (object_num < ms.nMarkers)
{
return(new double[3] {
ms.Markers[object_num].x * m_ServerToMillimeters,
ms.Markers[object_num].x * m_ServerToMillimeters,
ms.Markers[object_num].z * m_ServerToMillimeters
});
}
else
{
Console.WriteLine("the specified object number( {0} ) is greater than the total markers ({1})", object_num, ms.nMarkers);
return(null);
}
case "rb":
if (debug)
{
Console.WriteLine("asked for rigid body number {0}", object_num);
Console.WriteLine("number of rigid body: {0}", data.nRigidBodies);
}
if (object_num < data.nRigidBodies)
{
NatNetML.RigidBodyData rb = data.RigidBodies[object_num];
//get the quotenions
float[] quat = new float[4] {
rb.qx, rb.qy, rb.qz, rb.qw
};
float[] eulers = new float[3];
eulers = NatNetClientML.QuatToEuler(quat, NATEulerOrder.NAT_XYZr);
double x = RadiansToDegrees(eulers[0]); // convert to degrees
double y = RadiansToDegrees(eulers[1]);
double z = RadiansToDegrees(eulers[2]);
if (debug)
{
Console.WriteLine("received x y z positions");
Console.WriteLine("{0}, {1}, {2}", x, y, z);
stopwatch.Stop();
Console.WriteLine("Reading the quene takes {0} ms", stopwatch.ElapsedMilliseconds);
}
return(new double[7] {
rb.x *m_ServerToMillimeters, //mm for obvious reaons
rb.y *m_ServerToMillimeters,
rb.z *m_ServerToMillimeters,
x, y, z, data.iFrame
}); //angles in degrees
}
else
{
return(null);
}
}
}
public OptiTrackSystem(int connectionType = 0)
{
natNetClient = new NatNetClientML(connectionType);
serverDescription = new ServerDescription();
}
static void processFrameData(NatNetML.FrameOfMocapData data)
{
/* Parsing Rigid Body Frame Data */
for (int i = 0; i < mRigidBodies.Count; i++)
{
int rbID = mRigidBodies[i].ID; // Fetching rigid body IDs from the saved descriptions
for (int j = 0; j < data.nRigidBodies; j++)
{
if (rbID == data.RigidBodies[j].ID) // When rigid body ID of the descriptions matches rigid body ID of the frame data.
{
NatNetML.RigidBody rb = mRigidBodies[i]; // Saved rigid body descriptions
NatNetML.RigidBodyData rbData = data.RigidBodies[j]; // Received rigid body descriptions
if (rbData.Tracked == true)
{
Console.WriteLine("\tRigidBody ({0}):", rb.Name);
Console.WriteLine("\t\tpos ({0:N3}, {1:N3}, {2:N3})", rbData.x, rbData.y, rbData.z);
// Rigid Body Euler Orientation
float[] quat = new float[4] {
rbData.qx, rbData.qy, rbData.qz, rbData.qw
};
float[] eulers = new float[3];
eulers = NatNetClientML.QuatToEuler(quat, NATEulerOrder.NAT_XYZr); //Converting quat orientation into XYZ Euler representation.
double xrot = RadiansToDegrees(eulers[0]);
double yrot = RadiansToDegrees(eulers[1]);
double zrot = RadiansToDegrees(eulers[2]);
Console.WriteLine("\t\tori ({0:N3}, {1:N3}, {2:N3})", xrot, yrot, zrot);
}
else
{
Console.WriteLine("\t{0} is not tracked in current frame", rb.Name);
}
}
}
}
/* Parsing Skeleton Frame Data */
for (int i = 0; i < mSkeletons.Count; i++) // Fetching skeleton IDs from the saved descriptions
{
int sklID = mSkeletons[i].ID;
for (int j = 0; j < data.nSkeletons; j++)
{
if (sklID == data.Skeletons[j].ID) // When skeleton ID of the description matches skeleton ID of the frame data.
{
NatNetML.Skeleton skl = mSkeletons[i]; // Saved skeleton descriptions
NatNetML.SkeletonData sklData = data.Skeletons[j]; // Received skeleton frame data
Console.WriteLine("\tSkeleton ({0}):", skl.Name);
Console.WriteLine("\t\tSegment count: {0}", sklData.nRigidBodies);
/* Now, for each of the skeleton segments */
for (int k = 0; k < sklData.nRigidBodies; k++)
{
NatNetML.RigidBodyData boneData = sklData.RigidBodies[k];
/* Decoding skeleton bone ID */
int skeletonID = HighWord(boneData.ID);
int rigidBodyID = LowWord(boneData.ID);
int uniqueID = skeletonID * 1000 + rigidBodyID;
int key = uniqueID.GetHashCode();
NatNetML.RigidBody bone = (RigidBody)mHtSkelRBs[key]; //Fetching saved skeleton bone descriptions
//Outputting only the hip segment data for the purpose of this sample.
if (k == 0)
{
Console.WriteLine("\t\t{0:N3}: pos({1:N3}, {2:N3}, {3:N3})", bone.Name, boneData.x, boneData.y, boneData.z);
}
}
}
}
}
/* Parsing Force Plate Frame Data */
for (int i = 0; i < mForcePlates.Count; i++)
{
int fpID = mForcePlates[i].ID; // Fetching force plate IDs from the saved descriptions
for (int j = 0; j < data.nForcePlates; j++)
{
if (fpID == data.ForcePlates[j].ID) // When force plate ID of the descriptions matches force plate ID of the frame data.
{
NatNetML.ForcePlate fp = mForcePlates[i]; // Saved force plate descriptions
NatNetML.ForcePlateData fpData = data.ForcePlates[i]; // Received forceplate frame data
Console.WriteLine("\tForce Plate ({0}):", fp.Serial);
// Here we will be printing out only the first force plate "subsample" (index 0) that was collected with the mocap frame.
for (int k = 0; k < fpData.nChannels; k++)
{
Console.WriteLine("\t\tChannel {0}: {1}", fp.ChannelNames[k], fpData.ChannelData[k].Values[0]);
}
}
}
}
Console.WriteLine("\n");
}
private NatNetPoseData ProcessPoseData(NatNetML.FrameOfMocapData data)
{
NatNetPoseData poseData = new NatNetPoseData();
/* Parsing Simple Marker Data */
for (int i = 0; i < data.nMarkers; i++)
{
Marker marker = data.LabeledMarkers[i];
int mID = marker.ID;
// Console.WriteLine("\tMarker ({0}): \t\tpos ({0:N3}, {1:N3}, {2:N3})", mID, marker.x, marker.y, marker.z);
poseData.mPos = new Vector3(marker.x, marker.y, marker.z);
}
/* Parsing Rigid Body Frame Data */
for (int i = 0; i < mRigidBodies.Count; i++)
{
int rbID = mRigidBodies[i].ID; // Fetching rigid body IDs from the saved descriptions
for (int j = 0; j < data.nRigidBodies; j++)
{
if (rbID == data.RigidBodies[j].ID) // When rigid body ID of the descriptions matches rigid body ID of the frame data.
{
NatNetML.RigidBody rb = mRigidBodies[i]; // Saved rigid body descriptions
NatNetML.RigidBodyData rbData = data.RigidBodies[j]; // Received rigid body descriptions
if (rbData.Tracked == true)
{
poseData.rbPos = new Vector3(rbData.x, rbData.y, rbData.z);
poseData.rbRot = new Quaternion(rbData.qx, rbData.qy, rbData.qz, rbData.qw);
//Console.WriteLine("\tRigidBody ({0}):", rb.Name);
//Console.WriteLine("\t\tpos ({0:N3}, {1:N3}, {2:N3})", rbData.x, rbData.y, rbData.z);
// Rigid Body Euler Orientation
float[] quat = new float[4] {
rbData.qx, rbData.qy, rbData.qz, rbData.qw
};
float[] eulers = new float[3];
eulers = NatNetClientML.QuatToEuler(quat, NATEulerOrder.NAT_XYZr); //Converting quat orientation into XYZ Euler representation.
double xrot = RadiansToDegrees(eulers[0]);
double yrot = RadiansToDegrees(eulers[1]);
double zrot = RadiansToDegrees(eulers[2]);
//Console.WriteLine("\t\tori ({0:N3}, {1:N3}, {2:N3})", xrot, yrot, zrot);
string display = string.Format("\tRigidBody ({0}):", rb.Name);
display += string.Format("\t\tpos ({0:N3}, {1:N3}, {2:N3})", rbData.x, rbData.y, rbData.z);
display += string.Format("\t\tori ({0:N3}, {1:N3}, {2:N3})", xrot, yrot, zrot);
//Console.WriteLine(display);
}
else
{
Console.WriteLine("\t{0} is not tracked in current frame", rb.Name);
}
}
}
}
return(poseData);
}