public void TestMergeOptiTrackBody()
{
var merger = _ninjectKernel.Get <IDataMerger>(HardwareSystems.OptiTrack);
var optiTrackBody = new OptiTrackBody("testbody")
{
Points = new List <Point>()
{
new Point(Vector3.One)
},
BodyType = Body.EBodyType.Marker
};
var frame = new MocapFrame("optitrack", HardwareSystems.OptiTrack)
{
Bodies = new List <Body>()
{
optiTrackBody
}
};
frame = merger.HandleFrame(frame);
var position = frame.Bodies[0].Points[0].Position;
Assert.AreEqual(Body.EBodyType.Marker, frame.Bodies[0].BodyType);
Assert.AreEqual(Vector3.One, position);
}
private void ExtractRigidBodies(FrameOfMocapData data, IDictionary <string, OptiTrackBody> markerSets, ref MocapFrame handledFrame)
{
for (var i = 0; i < data.nRigidBodies; i++)
{
var rbData = data.RigidBodies[i]; // Received rigid body descriptions
if (!rbData.Tracked)
{
continue;
}
// add the rigid body with centroid and rotation
var rb = new OptiTrackBody(rbData.ID.ToString())
{
Centroid = new Vector3(rbData.x, rbData.y, rbData.z) * TranslationUnitMultiplier,
Rotation = new Quaternion(rbData.qx, rbData.qy, rbData.qz, rbData.qw),
BodyType = Body.EBodyType.RigidBody
};
// in addition add each marker as point to the body
var name = _client.NameById(rbData.ID);
if (markerSets.TryGetValue(name, out var ms))
{
rb.Name = name;
foreach (var point in ms.Points)
{
rb.Points.Add(point);
}
markerSets.Remove(name);
}
handledFrame.Bodies.Add(rb); // Add to MocapFrame list of bodies
}
}
private void ExtractSkeletons(FrameOfMocapData data, ref MocapFrame handledFrame)
{
for (var i = 0; i < data.nSkeletons; i++)
{
var sklData = data.Skeletons[i]; // Received skeleton frame data
var skl = new Skeleton(sklData.ID.ToString())
{
BodyType = Body.EBodyType.Skeleton
};
/* Now, for each of the skeleton segments */
for (var j = 0; j < sklData.nRigidBodies; j++)
{
var boneData = sklData.RigidBodies[j];
var bone = new OptiTrackBody(boneData.ID.ToString())
{
Centroid = new Vector3(boneData.x, boneData.y, boneData.z) * TranslationUnitMultiplier,
Rotation = new Quaternion(boneData.qx, boneData.qy, boneData.qz, boneData.qw)
};
skl.RigidBodies.Add(bone); // Add bone to skeleton
}
handledFrame.Bodies.Add(skl);
}
}
private IDictionary <string, OptiTrackBody> ExtractMarkerSets(FrameOfMocapData data)
{
var markerSets = new Dictionary <string, OptiTrackBody>();
for (var i = 0; i < data.nMarkerSets - 1; i++)
{
var msData = data.MarkerSets[i]; // Received marker set descriptions
var ms = new OptiTrackBody(msData.MarkerSetName)
{
BodyType = msData.nMarkers > 1
? Body.EBodyType.MarkerSet
: Body.EBodyType.Marker
};
for (var j = 0; j < msData.nMarkers; j++)
{
var markerData = msData.Markers[j];
var markerId = markerData.ID == -1 ? j : markerData.ID;
var marker = new Point(new Vector3(markerData.x, markerData.y, markerData.z) * TranslationUnitMultiplier)
{
Name = string.Join(PointIdDivider, msData.MarkerSetName, markerId),
};
ms.Points.Add(marker);
}
markerSets.Add(msData.MarkerSetName, ms);
}
return(markerSets);
}
private static Body MergeDefault(OptiTrackBody body)
{
var result = new Body
{
Points = body.Points,
Centroid = body.Centroid,
Rotation = body.Rotation,
Name = body.Name,
Side = body.Side
};
if (result.Points?.Count == 1 && result.BodyType.Equals(Body.EBodyType.MarkerSet))
{
result.BodyType = Body.EBodyType.Marker;
}
return(result);
}
public Body Merge(OptiTrackBody body)
{
Body result;
switch (body.BodyType)
{
case Body.EBodyType.Skeleton:
result = MergeSkeleton(body as Skeleton);
break;
default:
result = MergeDefault(body);
break;
}
return(result);
}
