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