/// <summary>
/// Called when a Captury AR tags (markers) are detected
/// </summary>
/// <param name="tags"></param>
private void OnARTagsDetected(ARTag[] tags)
{
// check player/skeleton assignment with AR tags
if (playerSkeleton == null)
{
// get the AR tags which are assigned to the player
List <ARTag> playerARTags = tags.Where(t => capturyConfig.arTagIDs.Contains(t.id)).ToList();
foreach (var tag in playerARTags)
{
CapturySkeleton skel = GetAttachedSkeleton(tag);
if (skel != null)
{
if (skel.playerID == -1)
{
AssignPlayerToSkeleton(skel);
}
else
{
Debug.Log("Skeleton " + skel.id + " is already assigned to player " + skel.playerID);
}
}
}
}
if (debugARTags)
{
ShowARTags(tags);
arTagsUpdated = true;
}
}
/// <summary>
/// Instantiates and sets the given avatarPrefab for the CapturySkeleton
/// </summary>
/// <param name="skel"></param>
/// <param name="avatarPrefab"></param>
private void SetAvatar(CapturySkeleton skel, GameObject avatarPrefab)
{
GameObject avatar = Instantiate(avatarPrefab);
DontDestroyOnLoad(avatar);
avatar.SetActive(true);
if (skel.mesh != null)
{
// destroy old avatar
DestroyImmediate(skel.mesh);
}
skel.mesh = avatar;
skel.retargetTarget = ConvertGameObjectToCapturyActor(avatar);
skel.originalMesh = avatarPrefab;
/* CapturyActor actor = new CapturyActor();
* actor = (CapturyActor)Marshal.PtrToStructure(x, typeof(CapturyActor));
*
* // no? we need to convert it
* CapturySkeleton skeleton = new CapturySkeleton();
* networkPlugin.ConvertActor(actor, x, ref skeleton);
*
*
* CapturyActor srcActor = new CapturyActor();
* srcActor = (CapturyActor)Marshal.PtrToStructure(skel.rawData, typeof(CapturyActor));
* CapturySkeleton srcSkel = new CapturySkeleton();
* networkPlugin.ConvertActor(srcActor, skel.rawData, ref srcSkel);
*/
liveGenerateMapping(skel.rawData, skel.retargetTarget);
}
//===============================================
// 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);
}
}
/// <summary>
/// Called when a new captury skeleton is found
/// </summary>
/// <param name="skeleton"></param>
private void OnSkeletonFound(CapturySkeleton skeleton)
{
Debug.Log("CapturyAvatarManager found skeleton with id " + skeleton.id + " and name " + skeleton.name);
lock (newSkeletons)
{
newSkeletons.Add(skeleton);
}
}
/// <summary>
/// Can be called from a multiplayer manager to set the skeletons playerID
/// </summary>
/// <param name="skeletonID">Captury Skeleton id</param>
/// <param name="playerID">Networking Player id</param>
public void SetSkeletonPlayerID(int skeletonID, int playerID)
{
CapturySkeleton skel = trackedSkeletons.First(s => s.id == skeletonID);
if (skel != null)
{
skel.playerID = playerID;
}
}
/// <summary>
/// Assigns the local player to the given <see cref="CapturySkeleton"/>.
/// </summary>
/// <param name="skeleton"></param>
private void AssignPlayerToSkeleton(CapturySkeleton skeleton)
{
// instantiate the local player avatar
GameObject avatarPrefab = GetAvatarPrefab(capturyConfig.avatarID, true);
SetAvatar(skeleton, avatarPrefab);
playerSkeleton = skeleton;
Transform left = null;
Transform right = null;
Transform head = null;
GameObject avatar = skeleton.mesh.gameObject;
Component[] trafos = avatar.transform.GetComponentsInChildren <Transform>();
foreach (Transform child in trafos)
{
if (child.name.EndsWith(AVATAR_LEFT_HAND_TRANSFORM_NAME))
{
left = child;
}
if (child.name.EndsWith(AVATAR_RIGHT_HAND_TRANSFORM_NAME))
{
right = child;
}
if (child.name.EndsWith(AVATAR_HEAD_TRANSFORM_NAME))
{
head = child;
}
}
if (left != null && right != null)
{
capturyLeapIntegration.setTargetModel(left, right, skeleton.id);
}
else
{
Debug.Log("Cannot find hands on target avatar with name '" + AVATAR_LEFT_HAND_TRANSFORM_NAME + "' and '" + AVATAR_RIGHT_HAND_TRANSFORM_NAME + "'");
}
if (head != null)
{
transformFollower.Target = head;
}
else
{
Debug.Log("Cannot find head on target avatar with name " + AVATAR_HEAD_TRANSFORM_NAME);
}
if (PlayerAssignmentChanged != null)
{
PlayerAssignmentChanged(playerSkeleton.id, true);
}
Debug.Log("Assigned local player to skeleton with name " + skeleton.name + " and id " + skeleton.id);
}
/// <summary>
/// Called when a captury skeleton is lost
/// </summary>
/// <param name="skeleton"></param>
private void OnSkeletonLost(CapturySkeleton skeleton)
{
Debug.Log("CapturyAvatarManager lost skeleton with id " + skeleton.id + " and name " + skeleton.name);
lock (lostSkeletons)
{
lostSkeletons.Add(skeleton);
}
// clear the assignment between local player and the skelton if it's lost
if (IsLocalPlayer(skeleton))
{
ClearPlayerAssignment();
}
}
public void setRotationConstraint(int id, string jointName, Transform t)
{
if (skeletons.ContainsKey(id))
{
Debug.Log("Cannot set rotation for " + jointName + ": no skeleton with id " + id);
return;
}
else
{
Debug.Log("Set " + jointName + "-rotation to " + t);
}
communicationMutex.WaitOne();
CapturySkeleton skel = skeletons[id];
communicationMutex.ReleaseMutex();
int index;
if (jointsWithConstraints.ContainsKey(jointName))
{
index = jointsWithConstraints[jointName];
}
else
{
index = 0;
foreach (CapturySkeletonJoint j in skel.joints)
{
if (j.name == jointName)
{
break;
}
++index;
}
if (index == skel.joints.Length)
{
Debug.Log("Cannot set constraint for joint " + jointName + ": no such joint");
return;
}
}
// CapturySkeletonJoint jnt = skel.joints[index];
Vector3 euler = ConvertToEulerAngles(ConvertRotationToLive(t.rotation));
IntPtr rotation = Marshal.AllocHGlobal(12);
Marshal.StructureToPtr(euler, rotation, false);
Captury_setRotationConstraint(id, index, rotation, Captury_getTime(), 1.0f);
Marshal.FreeHGlobal(rotation);
}
/// <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);
}
/// <summary>
/// Clears the assignment of the local player with an skeleton
/// </summary>
private void ClearPlayerAssignment()
{
if (playerSkeleton == null)
{
Debug.LogError("Trying to clear player assignment, but playerSkeleton == null");
return;
}
if (PlayerAssignmentChanged != null)
{
PlayerAssignmentChanged(playerSkeleton.id, false);
}
playerSkeleton.playerID = -1;
capturyLeapIntegration.setTargetModel(null, null, -1);
transformFollower.Target = null;
playerSkeleton = null;
}
/// <summary>
/// Return true if the given skeleton is the local player skeleton
/// </summary>
/// <param name="skeleton"></param>
/// <returns></returns>
private bool IsLocalPlayer(CapturySkeleton skeleton)
{
return(skeleton.Equals(playerSkeleton));
}
//================================================
// 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();
}
