// Update is called once per frame
void Update()
{
List <RigidBodyGeneric> rbs = new List <RigidBodyGeneric>();
List <Vector3> unidentifiedMarkerPositions = new List <Vector3>();
if (mocapServer == MocapServer.Cube_QualisysTrackManager)
{
QTMInterface.Report(ref rbs, ref unidentifiedMarkerPositions);
}
else if (mocapServer == MocapServer.Perform_OptitrackMotiveBody)
{
//OMBInterface.Update();
OMBInterface.Report(ref rbs);
}
// iterate over the rigid bodies to adjust transform
for (int i = 0; i < rbs.Count; i++)
{
Vector3 pos = new Vector3();
pos = rbs [i].position;
pos.x /= scaleX;
pos.x += offsetX;
pos.y /= scaleY;
pos.y += offsetY;
pos.z /= scaleZ;
pos.z += offsetZ;
RigidBodyGeneric rb = new RigidBodyGeneric();
rb.position = pos;
rb.rotation = rbs [i].rotation;
rb.name = rbs [i].name;
rbs [i] = rb;
}
// iterate over the incoming rigid bodies. if a Unity user has registered a delegate to associate with the name/ID of the incoming rigid body, call that delegate and pass it the established parameters
//Debug.Log("rbs.Count: " + rbs.Count);
for (int i = 0; i < rbs.Count; i++)
{
List <RegisteredRigidBodyDelegate> delegates;
if (registeredRigidBodyDelegatesByName.TryGetValue(rbs [i].name, out delegates))
{
for (int delegateCounter = 0; delegateCounter < delegates.Count; delegateCounter++)
{
delegates [delegateCounter] (rbs [i].position, rbs[i].rotation);
}
}
}
// iterate over the incoming unidentified markers, passing them to all event callers
for (int i = 0; i < registeredUnidentifiedMarkersDelegates.Count; i++)
{
registeredUnidentifiedMarkersDelegates [i] (unidentifiedMarkerPositions);
}
}
// Process Data Frame OscBundle
private void ParsePacket(OscPacket packet, ref List <RigidBodyGeneric> rbs)
{
if (packet.IsBundle)
{
//OSC Data frame packet; defined in section 6.8 of the QTM RT Server Protocol Documentation.
foreach (OscMessage message in ((OscBundle)packet).Messages)
{
//LogMessage (message);
// make sure the message is containing a 6DOF RB
string[] messageParts = message.Address.Split('/');
if (messageParts[2] == "6d")
{
Vector3 position = new Vector3();
Quaternion rotation = new Quaternion();
ExtractPositionAndRotation(message, ref position, ref rotation);
RigidBodyGeneric rb = new RigidBodyGeneric();
rb.name = messageParts [3];
rb.position = position;
rb.rotation = rotation;
rbs.Add(rb);
}
/*
* //Check if this rigid body name was given in rbNames[]
* for (int i = 0; i < rbNames.Length; i++) {
* if (rbNames[i] == message.Address) {
* Vector3 position = new Vector3();
* Quaternion rotation = new Quaternion();
* ExtractPositionAndRotation(message, ref position, ref rotation);
* positionsReport[i] = position;
* rotationsReport[i] = rotation;
* }
* }
*/
}
}
else
{
LogMessage((OscMessage)packet);
if (String.Compare(((OscMessage)packet).Address, "/qtm/cmd_res") == 0)
{
//OSC Command response packet; defined in section 6.6 of the QTM RT Server Protocol Documentation.
connected = true;
//Upon receiving response from server, start streaming data frames.
SendCommand("StreamFrames AllFrames 6D");
}
}
}
// zach added for MotionCaptureStreamingReceiver
public void Report(ref List <RigidBodyGeneric> rbs)
{
if (bDataReceived)
{
//OptitrackRigidBodyState rbState;
lock ( m_frameDataUpdateLock )
{
foreach (KeyValuePair <Int32, OptitrackRigidBodyState> entry in m_latestRigidBodyStates)
{
RigidBodyGeneric rb = new RigidBodyGeneric();
rb.position = entry.Value.Pose.Position;
rb.rotation = entry.Value.Pose.Orientation;
rb.name = entry.Key.ToString();
rbs.Add(rb);
}
}
}
}
// Process Data Frame OscBundle
private void ParsePacket(OscPacket packet, ref List <RigidBodyGeneric> rbs, ref List <Vector3> unidentifiedMarkerPositions)
{
if (packet.IsBundle)
{
//OSC Data frame packet; defined in section 6.8 of the QTM RT Server Protocol Documentation.
foreach (OscMessage message in ((OscBundle)packet).Messages)
{
//LogMessage (message);
// make sure the message is containing a 6DOF RB
string[] messageParts = message.Address.Split('/');
if (messageParts[2] == "3d_no_labels")
{
ExtractPositionsFrom3DNoLabels(message, ref unidentifiedMarkerPositions);
}
if (messageParts[2] == "6d")
{
Vector3 position = new Vector3();
Quaternion rotation = new Quaternion();
ExtractPositionAndRotationFrom6D(message, ref position, ref rotation);
RigidBodyGeneric rb = new RigidBodyGeneric();
rb.name = messageParts[3];
rb.position = position;
rb.rotation = rotation;
rbs.Add(rb);
}
}
}
else
{
LogMessage((OscMessage)packet);
if (String.Compare(((OscMessage)packet).Address, "/qtm/cmd_res") == 0)
{
//OSC Command response packet; defined in section 6.6 of the QTM RT Server Protocol Documentation.
connected = true;
//Upon receiving response from server, start streaming data frames.
SendCommand("StreamFrames AllFrames 3DNoLabels 6D");
}
}
}
// zach added for MotionCaptureStreamingReceiver
public void Report(ref List <RigidBodyGeneric> rbs)
{
if (bDataReceived)
{
for (int i = 0; i < msgData.rigid_bodies.Count; i++)
{
RigidBodyGeneric rb = new RigidBodyGeneric();
rb.position = convertRightHandedToLeftHandedPosition(((RigidBody)msgData.rigid_bodies [i]).position);
rb.rotation = convertRightHandedToLeftHandedRotation(((RigidBody)msgData.rigid_bodies [i]).rotation);
string rbName = "";
if (rigidBodyIDtoName.TryGetValue(((RigidBody)msgData.rigid_bodies [i]).id, out rbName))
{
rb.name = rbName;
}
else
{
rb.name = "none";
}
rbs.Add(rb);
}
}
}
