/// <summary>
/// Checks if the given <see cref="ARTag"/> is attached to a <see cref="CapturySkeleton"/> in <see cref="trackedSkeletons"/>
/// </summary>
/// <param name="tag"></param>
/// <returns>The skeleton which tag is attached to. null if there's none.</returns>
private CapturySkeleton GetAttachedSkeleton(ARTag tag)
{
foreach (var skel in trackedSkeletons)
{
if (IsAttachedToSkeleton(tag, skel))
{
return(skel);
}
}
return(null);
}
/// <summary>
/// Checks if the <see cref="ARTag"/> is attached to the <see cref="CapturySkeleton"/> by comparing their positions
/// </summary>
/// <param name="tag"></param>
/// <param name="skel"></param>
/// <returns></returns>
private bool IsAttachedToSkeleton(ARTag tag, CapturySkeleton skel)
{
float threshold = 0.5f;
foreach (var joint in skel.joints)
{
if (joint != null && joint.transform != null)
{
// TODO check if local / global position
if (Vector3.Distance(tag.translation, joint.transform.position) < threshold)
{
return(true);
}
}
}
return(false);
}
//============================
// this is run once per frame
//============================
void Update()
{
// only perform if we are actually connected
if (!isConnected)
{
return;
}
// make sure we lock access before doing anything
// Debug.Log ("Starting pose update...");
communicationMutex.WaitOne();
// fetch current pose for all skeletons
foreach (KeyValuePair <int, CapturySkeleton> kvp in skeletons)
{
// get the actor id
int actorID = kvp.Key;
// check if the actor is mapped to something, if not, ignore
if (skeletons[actorID].mesh == null)
{
continue;
}
// get pointer to pose
IntPtr poseData = Captury_getCurrentPose(actorPointers[actorID]);
// check if we actually got data, if not, continue
if (poseData == IntPtr.Zero)
{
// something went wrong, get error message
IntPtr msg = Captury_getLastErrorMessage();
string errmsg = Marshal.PtrToStringAnsi(msg);
Debug.Log("Stream error: " + errmsg);
Captury_freeErrorMessage(msg);
}
else
{
//Debug.Log("received pose for " + actorID);
// convert the pose
CapturyPose pose;
if (skeletons[actorID].retargetTarget != IntPtr.Zero)
{
if (!skeletons[actorID].upToDate)
{
CapturyActor actor = (CapturyActor)Marshal.PtrToStructure(skeletons[actorID].retargetTarget, typeof(CapturyActor));
CapturySkeleton skel = skeletons[actorID];
String[] jointNames = new String[skel.joints.Length];
for (int i = 0; i < jointNames.Length; ++i)
{
jointNames[i] = skel.joints[i].name;
}
IntPtr rawData = skel.rawData;
ConvertActor(actor, skeletons[actorID].retargetTarget, ref skel);
skeletons[actorID].joints = skel.joints;
skeletons[actorID].rawData = rawData;
skeletons[actorID].mesh = skel.mesh;
skeletons[actorID].originalMesh = skel.originalMesh;
skeletons[actorID].upToDate = true;
for (int i = 0; i < skel.joints.Length; ++i)
{
bool found = false;
for (int n = 0; n < jointNames.Length; ++n)
{
if (skel.joints[i].name.EndsWith(jointNames[n]) || jointNames[n].EndsWith(skel.joints[i].name))
{
found = true;
break;
}
}
if (!found)
{
skel.joints[i].transform = null;
skel.joints[i].originalTransform = null;
}
}
}
IntPtr retargetedPose = liveRetarget(skeletons[actorID].rawData, skeletons[actorID].retargetTarget, poseData);
pose = (CapturyPose)Marshal.PtrToStructure(retargetedPose, typeof(CapturyPose));
}
else
{
pose = (CapturyPose)Marshal.PtrToStructure(poseData, typeof(CapturyPose));
}
// copy the data into a float array
float[] values = new float[pose.numValues * 6];
Marshal.Copy(pose.values, values, 0, pose.numValues * 6);
Vector3 pos = new Vector3();
Vector3 rot = new Vector3();
// now loop over all joints
for (int jointID = 0; jointID < skeletons[actorID].joints.Length; jointID++)
{
// ignore any joints that do not map to a transform
if (skeletons[actorID].joints[jointID].transform == null)
{
continue;
}
// set offset and rotation
int baseIndex = jointID * 6;
pos.Set(values[baseIndex + 0], values[baseIndex + 1], values[baseIndex + 2]);
skeletons[actorID].joints[jointID].transform.position = ConvertPosition(pos) + offsetToWorld;
rot.Set(values[baseIndex + 3], values[baseIndex + 4], values[baseIndex + 5]);
skeletons[actorID].joints[jointID].transform.rotation = ConvertRotation(rot) * skeletons[actorID].joints[jointID].originalTransform.rotation;
}
// finally, free the pose data again, as we are finished
Captury_freePose(poseData);
}
}
// get artags
IntPtr arTagData = Captury_getCurrentARTags();
// check if we actually got data, if not, continue
if (arTagData == IntPtr.Zero)
{
// something went wrong, get error message
//IntPtr msg = Captury_getLastErrorMessage();
//string errmsg = Marshal.PtrToStringAnsi(msg);
//Captury_freeErrorMessage(msg);
}
else
{
IntPtr at = arTagData;
int num;
for (num = 0; num < 100; ++num)
{
CapturyARTag arTag = (CapturyARTag)Marshal.PtrToStructure(at, typeof(CapturyARTag));
if (arTag.id == -1)
{
break;
}
Array.Resize(ref arTags, num + 1);
arTags[num] = new ARTag();
arTags[num].id = arTag.id;
arTags[num].translation = ConvertPosition(new Vector3(arTag.ox, arTag.oy, arTag.oz));
arTags[num].rotation = ConvertRotation(Quaternion.LookRotation(new Vector3(arTag.nx, arTag.ny, arTag.nz)).eulerAngles);
at = new IntPtr(at.ToInt64() + Marshal.SizeOf(typeof(CapturyARTag)));
}
if (num != 0 && ARTagsDetected != null)
{
ARTagsDetected(arTags);
}
else
{
Array.Resize(ref arTags, 0);
}
Captury_freeARTags(arTagData);
}
communicationMutex.ReleaseMutex();
}
