private void ParseRigidBody(NatNetPacket_In packet, List <Actor> actors)
{
string name = packet.GetString(); // name, TODO: No name in major version < 2
int id = packet.GetInt32(); // ID
// rigid body name should be equal to actor name: search
Actor actor = null;
foreach (Actor a in actors)
{
if (a.name.CompareTo(name) == 0)
{
actor = a;
}
}
if (actor == null)
{
Debug.LogWarning("Rigid Body " + name + " could not be matched to an actor.");
actor = new Actor(scene, id, name);
actors.Add(actor);
}
Bone bone = new Bone(actor, name, id);
packet.GetInt32(); // Parent ID (ignore for rigid body)
bone.parent = null; // rigid bodies should not have a parent
bone.ox = packet.GetFloat(); // X offset
bone.oy = packet.GetFloat(); // Y offset
bone.oz = -packet.GetFloat(); // Z offset
actor.bones = new Bone[1];
actor.bones[0] = bone;
}
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 ParseModelDefinition(NatNetPacket_In packet)
{
int numDescriptions = packet.GetInt32();
List <Actor> actors = new List <Actor>();
List <Device> devices = new List <Device>();
for (int dIdx = 0; dIdx < numDescriptions; dIdx++)
{
int datasetType = packet.GetInt32();
switch (datasetType)
{
case DATASET_TYPE_MARKERSET: ParseMarkerset(packet, actors); break;
case DATASET_TYPE_RIGIDBODY: ParseRigidBody(packet, actors); break;
case DATASET_TYPE_SKELETON: ParseSkeleton(packet, actors); break;
case DATASET_TYPE_FORCEPLATE: ParseForcePlate(packet, devices); break;
default:
{
Debug.LogWarning("Invalid dataset type " + datasetType + " in model definition respose.");
break;
}
}
}
scene.actors = actors.ToArray();
scene.devices = devices.ToArray();
// scene description has possibly changed > update device and actor listeners
RefreshListeners();
return(true);
}
private void ParseMarkerset(NatNetPacket_In packet, List <Actor> actors)
{
int id = 0; // no ID for markersets
string name = packet.GetString(); // markerset name
Actor actor = new Actor(scene, id, name);
int nMarkers = packet.GetInt32(); // marker count
// TODO: Sanity check on the number before allocating that much space
actor.markers = new Marker[nMarkers];
for (int markerIdx = 0; markerIdx < nMarkers; markerIdx++)
{
name = packet.GetString();
Marker marker = new Marker(actor, name);
actor.markers[markerIdx] = marker;
}
actors.Add(actor);
}
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);
}
private void ParseSkeleton(NatNetPacket_In packet, List <Actor> actors)
{
bool includesBoneNames = // starting at v2.0
(serverInfo.versionNatNet[0] >= 2);
String skeletonName = packet.GetString(); // name
int skeletonId = packet.GetInt32(); // ID
// rigid body name should be equal to actor name: search
Actor actor = null;
foreach (Actor a in actors)
{
if (a.name.CompareTo(skeletonName) == 0)
{
actor = a;
actor.id = skeletonId; // associate actor and skeleton
}
}
if (actor == null)
{
// names don't match > try IDs
if ((skeletonId >= 0) && (skeletonId < actors.Count))
{
actor = actors[skeletonId];
}
}
if (actor == null)
{
Debug.LogWarning("Skeleton " + skeletonName + " could not be matched to an actor.");
actor = new Actor(scene, skeletonId, skeletonName);
actors.Add(actor);
}
int nBones = packet.GetInt32(); // Skeleton bone count
// TODO: Sanity check on the number before allocating that much space
actor.bones = new Bone[nBones];
for (int boneIdx = 0; boneIdx < nBones; boneIdx++)
{
String name = "";
if (includesBoneNames)
{
name = packet.GetString(); // bone name
}
int id = packet.GetInt32(); // bone ID
Bone bone = new Bone(actor, name, id);
bone.parent = actor.FindBone(packet.GetInt32()); // Skeleton parent ID
if (bone.parent != null)
{
// if bone has a parent, update child list of parent
bone.parent.children.Add(bone);
}
bone.BuildChain(); // build chain from root to this bone
bone.ox = packet.GetFloat(); // X offset
bone.oy = packet.GetFloat(); // Y offset
bone.oz = -packet.GetFloat(); // Z offset
actor.bones[boneIdx] = bone;
}
}