private void ParseForcePlate(NatNetPacket_In packet, List <Device> devices)
{
int id = packet.GetInt32(); // force plate ID
String name = packet.GetString(); // force plate serial #
Device device = new Device(scene, name, id); // create device
// skip next 652 bytes
// (SDK 2.9 sample code does not explain what this is about)
packet.Skip(652);
int nChannels = packet.GetInt32(); // channel count
device.channels = new Channel[nChannels];
for (int channelIdx = 0; channelIdx < nChannels; channelIdx++)
{
name = packet.GetString();
Channel channel = new Channel(device, name);
device.channels[channelIdx] = channel;
}
devices.Add(device);
}
private bool ParseFrameOfData(NatNetPacket_In packet)
{
// determine special datasets depending on NatNet version
bool includesMarkerIDsAndSizes = // starting at v2.0
(serverInfo.versionNatNet[0] >= 2);
bool includesSkeletonData = // starting at v2.1
((serverInfo.versionNatNet[0] == 2) &&
(serverInfo.versionNatNet[1] >= 1)) ||
(serverInfo.versionNatNet[0] > 2);
bool includesTrackingState = // starting at v2.6
((serverInfo.versionNatNet[0] == 2) &&
(serverInfo.versionNatNet[1] >= 6)) ||
(serverInfo.versionNatNet[0] > 2);
bool includesLabelledMarkers = // starting at v2.3
((serverInfo.versionNatNet[0] == 2) &&
(serverInfo.versionNatNet[1] >= 3)) ||
(serverInfo.versionNatNet[0] > 2);
bool includesLabelledMarkerFlags = // starting at v2.6
((serverInfo.versionNatNet[0] == 2) &&
(serverInfo.versionNatNet[1] >= 6)) ||
(serverInfo.versionNatNet[0] > 2);
bool includesForcePlateData = // starting at v2.9
((serverInfo.versionNatNet[0] == 2) &&
(serverInfo.versionNatNet[1] >= 9)) ||
(serverInfo.versionNatNet[0] > 2);
int frameNumber = packet.GetInt32(); // frame number
// is this an actual update? Or did we receive an older frame
// delta < 10: but do consider looping playback
// when frame numbers suddenly differ significantly
int deltaFrame = frameNumber - scene.frameNumber;
if ((deltaFrame < 0) && (deltaFrame > -10))
{
return(true); // old frame, get out
}
scene.frameNumber = frameNumber;
// Read actor data
int nActors = packet.GetInt32(); // actor count
for (int actorIdx = 0; actorIdx < nActors; actorIdx++)
{
string actorName = packet.GetString();
// find the corresponding actor
Actor actor = scene.FindActor(actorName);
int nMarkers = packet.GetInt32();
for (int markerIdx = 0; markerIdx < nMarkers; markerIdx++)
{
Marker marker = (actor != null) ? actor.markers[markerIdx] : DUMMY_MARKER;
// Read position
marker.px = packet.GetFloat();
marker.py = packet.GetFloat();
marker.pz = packet.GetFloat();
TransformToUnity(ref marker);
// marker is tracked when at least one coordinate is not 0
marker.tracked = (marker.px != 0) ||
(marker.py != 0) ||
(marker.pz != 0);
}
}
// skip unidentified markers
int nUnidentifiedMarkers = packet.GetInt32();
int unidentifiedMarkerDataSize = 3 * 4; // 3 floats
packet.Skip(unidentifiedMarkerDataSize * nUnidentifiedMarkers);
// Read rigid body data
int nRigidBodies = packet.GetInt32(); // bone count
for (int rigidBodyIdx = 0; rigidBodyIdx < nRigidBodies; rigidBodyIdx++)
{
int rigidBodyID = packet.GetInt32(); // get rigid body ID
// find the corresponding actor
Bone bone = DUMMY_BONE;
if ((rigidBodyID >= 0) && (rigidBodyID < scene.actors.Length))
{
//TODO: What if there is no bone in that actor?
bone = scene.actors[rigidBodyID].bones[0];
}
// Read position/rotation
bone.px = packet.GetFloat(); // position
bone.py = packet.GetFloat();
bone.pz = packet.GetFloat();
bone.qx = packet.GetFloat(); // rotation
bone.qy = packet.GetFloat();
bone.qz = packet.GetFloat();
bone.qw = packet.GetFloat();
TransformToUnity(ref bone);
int nMarkers = packet.GetInt32();
for (int i = 0; i < nMarkers; i++)
{
packet.GetFloat(); // Marker X
packet.GetFloat(); // Marker Y
packet.GetFloat(); // Marker Z
}
if (includesMarkerIDsAndSizes)
{
// also, marker IDs and sizes
for (int i = 0; i < nMarkers; i++)
{
packet.GetInt32(); // Marker ID
}
// and sizes
for (int i = 0; i < nMarkers; i++)
{
packet.GetFloat(); // Marker size
}
packet.GetFloat(); // Mean error
}
// Tracking state
if (includesTrackingState)
{
int state = packet.GetInt16();
// 0x01 : rigid body was successfully tracked in this frame
bone.tracked = (state & 0x01) != 0;
}
else
{
// tracking state not sent separately,
// but position = (0,0,0) used as "not tracked" indicator
bone.tracked = (bone.px != 0) ||
(bone.py != 0) ||
(bone.pz != 0);
}
}
// Read skeleton data
if (includesSkeletonData)
{
int nSkeletons = packet.GetInt32();
for (int skeletonIdx = 0; skeletonIdx < nSkeletons; skeletonIdx++)
{
int skeletonId = packet.GetInt32();
// match skeleton and actor ID
Actor actor = scene.FindActor(skeletonId);
if (actor == null)
{
Debug.LogWarning("Could not find actor " + skeletonId);
return(false);
}
// # of bones in skeleton
int nBones = packet.GetInt32();
// TODO: Number sanity check
for (int nBodyIdx = 0; nBodyIdx < nBones; nBodyIdx++)
{
int boneId = packet.GetInt32();
Bone bone = actor.FindBone(boneId);
if (bone == null)
{
bone = DUMMY_BONE;
}
// Read position/rotation
bone.px = packet.GetFloat(); // position
bone.py = packet.GetFloat();
bone.pz = packet.GetFloat();
bone.qx = packet.GetFloat(); // rotation
bone.qy = packet.GetFloat();
bone.qz = packet.GetFloat();
bone.qw = packet.GetFloat();
TransformToUnity(ref bone);
// read/skip rigid marker data
int nMarkers = packet.GetInt32();
for (int i = 0; i < nMarkers; i++)
{
packet.GetFloat(); // X/Y/Z position
packet.GetFloat();
packet.GetFloat();
}
for (int i = 0; i < nMarkers; i++)
{
packet.GetInt32(); // Marker IDs
}
for (int i = 0; i < nMarkers; i++)
{
packet.GetFloat(); // Marker size
}
// ATTENTION: actually "Mean marker error", but used as bone length
bone.length = packet.GetFloat();
// Tracking state
if (includesTrackingState)
{
int state = packet.GetInt16();
// 0x01 : rigid body was successfully tracked in this frame
bone.tracked = (state & 0x01) != 0;
}
else
{
// tracking state not sent separately,
// but position = (0,0,0) used as "not tracked" indicator
bone.tracked = (bone.px != 0) ||
(bone.py != 0) ||
(bone.pz != 0);
}
} // next rigid body
} // next skeleton
// skip labelled markers
if (includesLabelledMarkers)
{
int nLabelledMarkers = packet.GetInt32();
int labelledMarkerDataSize =
includesLabelledMarkerFlags ?
5 * 4 + 1 * 2 : // 1 int, 4 floats, 1 short
5 * 4; // 1 int, 4 floats
packet.Skip(nLabelledMarkers * labelledMarkerDataSize);
// without skipping:
// for ( int markerIdx = 0; markerIdx < nLabeledMarkers; markerIdx++ )
// {
// int id = buf.getInt();
// float x = buf.getFloat();
// float y = buf.getFloat();
// float z = -buf.getFloat();
// float size = buf.getFloat();
// if ( includesLabelledMarkerFlags )
// {
// short params = buf.getShort();
// }
// }
}
// read force plate data
if (includesForcePlateData)
{
int nForcePlates = packet.GetInt32();
for (int forcePlateIdx = 0; forcePlateIdx < nForcePlates; forcePlateIdx++)
{
// read force plate ID and find corresponding device
int forcePlateId = packet.GetInt32();
Device device = scene.FindDevice(forcePlateId);
if (device == null)
{
device = DUMMY_DEVICE;
}
// channel count
int nChannels = packet.GetInt32();
// channel data
for (int chn = 0; chn < nChannels; chn++)
{
int nFrames = packet.GetInt32();
float value = 0;
for (int frameIdx = 0; frameIdx < nFrames; frameIdx++)
{
value = packet.GetFloat();
}
if (chn < device.channels.Length)
{
// effectively only keep the last (or only) value
device.channels[chn].value = value;
}
}
}
}
// read latency and convert from s to ms
scene.latency = (int)(packet.GetFloat() * 1000);
}
NotifyListeners_Update();
return(true);
}