/// <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(); }