//=========================================
// connect a given transform to a skeleton
//=========================================
private void ConnectSkeleton(int actorId, Transform transform)
{
if (transform == null || !actors.ContainsKey(actorId))
{
return;
}
communicationMutex.WaitOne();
CapturyActor actor = actors[actorId];
CapturySkeleton skel = skeletons[actorId];
skel.reference = transform;
for (uint i = 0; i < actor.numJoints; i++)
{
// check if the joint name matches a reference transform and assign it
ArrayList children = skel.reference.transform.GetAllChildren();
foreach (Transform tra in children)
{
if (tra.name.EndsWith(skel.joints[i].name))
{
skel.joints[i].transform = tra;
continue;
}
}
}
communicationMutex.ReleaseMutex();
}
//===============================================
// helper function to map an actor to a skeleton
//===============================================
private void ConvertActor(CapturyActor actor, 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;
// 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);
//Debug.Log ("Got joint " + skel.joints[i].name + " at " + joint.ox + joint.oy + joint.oz);
}
}
//================================================
// This function continously looks for new actors
// It runs in a separate thread
//================================================
void lookForActors()
{
Debug.Log("looking for actors");
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)
{
Debug.Log("trying to connect to " + host + ":" + port);
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));
}
}
if (isSetup)
{
// grab actors
IntPtr actorData = IntPtr.Zero;
int numActors = Captury_getActors(out actorData);
if (numActors > 0 && actorData != IntPtr.Zero)
{
Debug.Log(String.Format("Successfully received {0} actors", numActors));
// create actor struct
int szStruct = Marshal.SizeOf(typeof(CapturyActor));
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));
Debug.Log(String.Format("actor id {0} {1} ", actor.id, Encoding.UTF8.GetString(actor.name)));
IntPtr ptr = new IntPtr(actorData.ToInt64() + (szStruct * i));
/* for (int x = 0; x < 64; ++x) {
* Debug.Log(String.Format("byte {0} {1}", x, Marshal.ReadByte(ptr, x)));
* }*/
// check if we already have it in our dictionary
if (actors.ContainsKey(actor.id))
{
actorFound[actor.id] = 5;
Debug.Log("Found known actor " + actor.id);
continue;
}
Debug.Log("Found new actor " + actor.id + " with " + actor.numJoints + " joints");
// no? we need to convert it
IntPtr actorPointer = new IntPtr(actorData.ToInt64() + (szStruct * i));
CapturySkeleton skeleton = new CapturySkeleton();
ConvertActor(actor, ref skeleton);
// and add it to the list of actors we are processing, making sure this is secured by the mutex
communicationMutex.WaitOne();
actors.Add(actor.id, actor);
actorPointers.Add(actor.id, actorPointer);
skeletons.Add(actor.id, skeleton);
actorFound.Add(actor.id, 5);
communicationMutex.ReleaseMutex();
}
if (!isConnected && Captury_startStreaming(1) == 1)
{
Debug.Log("Successfully started streaming data");
isConnected = true;
}
}
// 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)
{
// check if this is the current actor
if (kvp.Key == playerActor)
{
playerActor = -1;
}
// remove actor
Debug.Log("Removing actor " + kvp.Key);
actors.Remove(kvp.Key);
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 ar tags - 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;
}
public void sendActorSkeleton(CapturyActor actor, CapturySkeleton skeleton)
{
Debug.Log("received actor " + actor.name + " and skeleton " + skeleton.name);
}