//=============================================== // helper function to map an actor to a skeleton //=============================================== public void ConvertActor(CapturyActor actor, IntPtr actorData, ref CapturySkeleton skel) { if (skel == null) { Debug.Log("Null skeleton reference"); return; } // copy data over skel.name = System.Text.Encoding.UTF8.GetString(actor.name); skel.id = actor.id; skel.rawData = actorData; // create joints int szStruct = Marshal.SizeOf(typeof(CapturyJoint)); skel.joints = new CapturySkeletonJoint[actor.numJoints]; for (uint i = 0; i < actor.numJoints; i++) { // marshall the joints into a new joint struct CapturyJoint joint = new CapturyJoint(); joint = (CapturyJoint)Marshal.PtrToStructure(new IntPtr(actor.joints.ToInt64() + (szStruct * i)), typeof(CapturyJoint)); skel.joints[i] = new CapturySkeletonJoint(); skel.joints[i].name = System.Text.Encoding.ASCII.GetString(joint.name); int jpos = skel.joints[i].name.IndexOf("\0"); skel.joints[i].name = skel.joints[i].name.Substring(0, jpos); skel.joints[i].offset.Set(joint.ox, joint.oy, joint.oz); skel.joints[i].orientation.Set(joint.rx, joint.ry, joint.rz); skel.joints[i].parent = joint.parent; //Debug.Log ("Got joint " + skel.joints[i].name + " at " + joint.ox + joint.oy + joint.oz); } }
private System.IntPtr ConvertGameObjectToCapturyActor(GameObject g) { CapturyActor actor = new CapturyActor(); Transform[] trafos = g.GetComponentsInChildren <Transform>(); //Debug.Log("have " + (trafos.Length - 1) + " children"); actor.name = System.Text.Encoding.ASCII.GetBytes(g.name); actor.id = 17; actor.numJoints = trafos.Length - 1; System.IntPtr mem = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(CapturyActor)) + actor.numJoints * Marshal.SizeOf(typeof(CapturyJoint))); actor.joints = new System.IntPtr(mem.ToInt64() + Marshal.SizeOf(typeof(CapturyActor))); actor.numBlobs = 0; actor.blobs = System.IntPtr.Zero; Marshal.StructureToPtr(actor, mem, false); CapturyJoint[] joints = new CapturyJoint[trafos.Length - 1]; Vector3[] globalPositions = new Vector3[trafos.Length - 1]; Vector3[] globalPositionsUnity = new Vector3[trafos.Length - 1]; Vector3[] trafoPos = new Vector3[trafos.Length]; Dictionary <string, int> names = new Dictionary <string, int>(); names[trafos[0].name] = -1; // this is the overall parent System.IntPtr j = actor.joints; for (int i = 0; i < actor.numJoints; ++i) { names[trafos[i + 1].name] = i; joints[i].parent = names[trafos[i + 1].parent.name]; joints[i].name = System.Text.Encoding.ASCII.GetBytes(trafos[i + 1].name); Vector3 pos; int parent = joints[i].parent; if (parent != -1) { pos = networkPlugin.ConvertPositionToLive(trafos[i + 1].position - trafos[i + 1].parent.position); } else { pos = networkPlugin.ConvertPositionToLive(trafos[i + 1].position); } joints[i].ox = pos.x; joints[i].oy = pos.y; joints[i].oz = pos.z; Quaternion delta = trafos[i + 1].rotation; Quaternion liveDelta = networkPlugin.ConvertRotationToLive(delta); Vector3 rot = networkPlugin.ConvertToEulerAngles(liveDelta); joints[i].rx = 0.0f; // rot.x; joints[i].ry = 0.0f; // rot.y; joints[i].rz = 0.0f; // rot.z; trafoPos[i] = trafos[i + 1].position; globalPositions[i] = new Vector3(joints[i].ox, joints[i].oy, joints[i].oz); int p = joints[i].parent; if (p > -1) { globalPositions[i] += globalPositions[p]; } globalPositionsUnity[i] = networkPlugin.ConvertPosition(globalPositions[i]) - trafoPos[i]; Marshal.StructureToPtr(joints[i], j, false); j = new System.IntPtr(j.ToInt64() + Marshal.SizeOf(typeof(CapturyJoint))); } return(mem); }
//================================================ // This function continously looks for new actors // It runs in a separate thread //================================================ void lookForActors() { while (!communicationFinished) { // wait for actorCheckTimeout ms before continuing // Debug.Log ("Going to sleep..."); Thread.Sleep(actorCheckTimeout); // Debug.Log ("Waking up..."); // now look for new data // try to connect to captury live if (!isSetup) { if (Captury_connect(host, port) == 1 && Captury_synchronizeTime() != 0) { isSetup = true; Debug.Log("Successfully opened port to Captury Live"); Debug.Log("The time difference is " + Captury_getTimeOffset()); } else { Debug.Log(String.Format("Unable to connect to Captury Live at {0}:{1} ", host, port)); } IntPtr cameraData = IntPtr.Zero; int numCameras = Captury_getCameras(out cameraData); if (numCameras > 0 && cameraData != IntPtr.Zero) { cameraPositions = new Vector3[numCameras]; cameraOrientations = new Quaternion[numCameras]; int szStruct = Marshal.SizeOf(typeof(CapturyCamera)) + 192; // this offset is here to take care of implicit padding for (uint i = 0; i < numCameras; i++) { CapturyCamera camera = new CapturyCamera(); camera = (CapturyCamera)Marshal.PtrToStructure(new IntPtr(cameraData.ToInt64() + (szStruct * i)), typeof(CapturyCamera)); // Debug.Log("camera " + camera.id.ToString("x") + " (" + camera.positionX + ", " + camera.positionY + "," + camera.positionZ + ") (" + // camera.orientationX + ", " + camera.orientationY + "," + camera.orientationZ + ") ss: (" + camera.sensorWidth + ", " + camera.sensorHeight + ") fl:" + // camera.focalLength + " lc: (" + camera.lensCenterX + ", " + camera.lensCenterY + ") "); cameraPositions[i] = ConvertPosition(new Vector3(camera.positionX, camera.positionY, camera.positionZ)); cameraOrientations[i] = ConvertRotation(new Vector3(camera.orientationX, camera.orientationY, camera.orientationZ)); } // Fire cameras changed event if (CamerasChanged != null) { CamerasChanged(cameraPositions, cameraOrientations); } } } if (isSetup) { // grab actors IntPtr actorData = IntPtr.Zero; int numActors = Captury_getActors(out actorData); if (numActors > 0 && actorData != IntPtr.Zero) { Debug.Log(String.Format("Received {0} actors", numActors)); // create actor struct int szStruct = Marshal.SizeOf(typeof(CapturyActor)) + 4; // implicit padding for (uint i = 0; i < numActors; i++) { // get an actor CapturyActor actor = new CapturyActor(); actor = (CapturyActor)Marshal.PtrToStructure(new IntPtr(actorData.ToInt64() + (szStruct * i)), typeof(CapturyActor)); // check if we already have it in our dictionary if (skeletons.ContainsKey(actor.id)) // access to actors does not need to be locked here because the other thread is read-only { communicationMutex.ReleaseMutex(); actorFound[actor.id] = 5; continue; } Debug.Log("Found new actor " + actor.id); // no? we need to convert it IntPtr actorPointer = new IntPtr(actorData.ToInt64() + (szStruct * i)); CapturySkeleton skeleton = new CapturySkeleton(); ConvertActor(actor, actorData, ref skeleton); if (SkeletonFound != null) { SkeletonFound(skeleton); } // and add it to the list of actors we are processing, making sure this is secured by the mutex communicationMutex.WaitOne(); actorPointers.Add(actor.id, actorPointer); skeletons.Add(actor.id, skeleton); actorFound.Add(actor.id, 5); communicationMutex.ReleaseMutex(); } } if (!isConnected) { if (Captury_startStreaming(streamARTags ? 5 : 1) == 1) { Debug.Log("Successfully started streaming data"); isConnected = true; } else { Debug.LogWarning("failed to start streaming"); } } // reduce the actor countdown by one for each actor int[] keys = new int[actorFound.Keys.Count]; actorFound.Keys.CopyTo(keys, 0); foreach (int key in keys) { actorFound[key]--; } } // remove all actors that were not found in the past few actor checks // Debug.Log ("Updating actor structure"); communicationMutex.WaitOne(); List <int> unusedKeys = new List <int>(); foreach (KeyValuePair <int, int> kvp in actorFound) { if (kvp.Value <= 0) { if (SkeletonLost != null) { Debug.Log("lost skeleton. telling all my friends."); SkeletonLost(skeletons[kvp.Key]); } // remove actor actorPointers.Remove(kvp.Key); skeletons.Remove(kvp.Key); unusedKeys.Add(kvp.Key); } } communicationMutex.ReleaseMutex(); // Debug.Log ("Updating actor structure done"); // clear out actorfound structure foreach (int key in unusedKeys) { actorFound.Remove(key); } // look for current transformation of bones with markers - the head /* foreach (KeyValuePair<int, IntPtr> kvp in actorPointers) * { * int id = kvp.Key; * * // find the index of the head joint * int headJointIndex = -1; * for (int i = 0; i < skeletons[id].joints.Length; ++i) * { * if (skeletons[id].joints[i].name.EndsWith(headJointName)) * { * headJointIndex = i; * break; * } * } * if (headJointIndex == -1) * { * Debug.Log("no head joint for skeleton " + id); * continue; * } * * // get the transform and store it in headTransforms * IntPtr trafo = Marshal.AllocHGlobal(Marshal.SizeOf(typeof(CapturyTransform))); * UInt64 timestamp = Captury_getMarkerTransform(kvp.Value, headJointIndex, trafo); * // is there a constraint for this joint that is not older than 500ms? * if (timestamp != 0) * { * CapturyTransform t = (CapturyTransform)Marshal.PtrToStructure(trafo, typeof(CapturyTransform)); * communicationMutex.WaitOne(); * if (headTransforms.ContainsKey(id)) * { * // this is a new transform. the other thread should have a look at it. * if (headTransforms[id].timestamp < timestamp) * headTransforms[id].consumed = false; * } * else * { * headTransforms[id] = new CapturyMarkerTransform(); * headTransforms[id].bestAccuracy = 0.95f; * // if the new transform is actually already old mark it as old directly * if (timestamp > Captury_getTime() - 500000) * headTransforms[id].consumed = false; * else * headTransforms[id].consumed = true; * } * headTransforms[id].rotation = ConvertRotation(new Vector3(t.rx * 180 / (float)Math.PI, t.ry * 180 / (float)Math.PI, t.rz * 180 / (float)Math.PI)); * headTransforms[id].translation = ConvertPosition(new Vector3(t.tx, t.ty, t.tz)); * headTransforms[id].timestamp = timestamp; * communicationMutex.ReleaseMutex(); * // Debug.Log(string.Format("transform for actor.joint {0}.{1} is good, really t {2}, delta now {3}", id, headJointIndex, timestamp, Captury_getTime() - timestamp)); * } * else * { * communicationMutex.WaitOne(); * headTransforms.Remove(id); * communicationMutex.ReleaseMutex(); * } * Marshal.FreeHGlobal(trafo); * }*/ } Debug.Log("Disconnecting"); // make sure we disconnect Captury_disconnect(); isSetup = false; isConnected = 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(); }