void Update()
{
OptitrackSkeletonState skelState = StreamingClient.GetLatestSkeletonState(m_skeletonDef.Id);
if (skelState != null)
{
// Update the transforms of the bone GameObjects.
for (int i = 0; i < m_skeletonDef.Bones.Count; ++i)
{
Int32 boneId = m_skeletonDef.Bones[i].Id;
OptitrackPose bonePose;
GameObject boneObject;
bool foundPose = skelState.BonePoses.TryGetValue(boneId, out bonePose);
bool foundObject = m_boneObjectMap.TryGetValue(boneId, out boneObject);
if (foundPose && foundObject)
{
boneObject.transform.localPosition = bonePose.Position;
boneObject.transform.localRotation = bonePose.Orientation;
}
}
// Perform Mecanim retargeting.
if (m_srcPoseHandler != null && m_destPoseHandler != null)
{
// Interpret the streamed pose into Mecanim muscle space representation.
m_srcPoseHandler.GetHumanPose(ref m_humanPose);
// Retarget that muscle space pose to the destination avatar.
m_destPoseHandler.SetHumanPose(ref m_humanPose);
}
}
}
public override void UpdateTracker()
{
base.UpdateTracker();
status = Status.Unavailable;
if (!enabled || streamingClient == null)
{
return;
}
status = Status.Present;
deviceView.position = Target.ToVector(trackerTransform.position);
deviceView.orientation = Target.ToRotation(trackerTransform.rotation);
if (trackingType == TrackingType.Skeleton && m_skeletonDef != null)
{
OptitrackSkeletonState skelState = streamingClient.GetLatestSkeletonState(m_skeletonDef.Id);
if (skelState == null)
{
return;
}
status = Status.Tracking;
// Update the transforms of the bone GameObjects.
for (int i = 0; i < m_skeletonDef.Bones.Count; ++i)
{
int optitrackBoneId = m_skeletonDef.Bones[i].Id;
OptitrackPose bonePose;
Bone boneId;
bool foundPose = skelState.BonePoses.TryGetValue(optitrackBoneId, out bonePose);
if (foundPose)
{
bool foundBone = optitrackBoneMapping.TryGetValue(m_skeletonDef.Bones[i].Name, out boneId);
if (foundBone)
{
bonePositions[(int)boneId] = bonePose.Position;
boneRotations[(int)boneId] = bonePose.Orientation;
}
}
}
}
}
/// <summary>
/// Returns the <see cref="OptitrackSkeletonState"/> corresponding to the provided <paramref name="skeletonId"/>.
/// If the requested state object does not exist yet, it will initialize and return a newly-created one.
/// </summary>
/// <remarks>Makes the assumption that the lock on <see cref="m_frameDataUpdateLock"/> is already held.</remarks>
/// <param name="skeletonId">The ID of the skeleton for which to retrieve the corresponding state.</param>
/// <returns>The existing state object, or a newly created one if necessary.</returns>
private OptitrackSkeletonState GetOrCreateSkeletonState(Int32 skeletonId)
{
OptitrackSkeletonState returnedState = null;
if (m_latestSkeletonStates.ContainsKey(skeletonId))
{
returnedState = m_latestSkeletonStates[skeletonId];
}
else
{
OptitrackSkeletonState newSkeletonState = new OptitrackSkeletonState {
BonePoses = new Dictionary <Int32, OptitrackPose>()
};
m_latestSkeletonStates[skeletonId] = newSkeletonState;
returnedState = newSkeletonState;
}
return(returnedState);
}
void Update()
{
if (StreamingClient == null)
{
return;
}
OptitrackSkeletonState skelState = StreamingClient.GetLatestSkeletonState(m_skeletonDef.Id);
if (skelState != null)
{
// Update the transforms of the bone GameObjects.
for (int i = 0; i < m_skeletonDef.Bones.Count; ++i)
{
Int32 boneId = m_skeletonDef.Bones[i].Id;
OptitrackPose bonePose;
GameObject boneObject;
bool foundPose = skelState.BonePoses.TryGetValue(boneId, out bonePose);
bool foundObject = m_boneObjectMap.TryGetValue(boneId, out boneObject);
if (foundPose && foundObject)
{
Vector3 startPos = boneObject.transform.localPosition;
Vector3 endPos = bonePose.Position.V3;
float journey = (startPos - endPos).magnitude;
// if bone position change too large, then move smaller
// if (journey > 0.1f)
// {
// boneObject.transform.localPosition =
// Vector3.Lerp(startPos, endPos, Time.deltaTime / (5 * journey));
// Debug.Log ("lerping skeleton");
// }
// else
// {
// boneObject.transform.localPosition = bonePose.Position.V3;
// }
if (StreamingClient.ConnectionType == OptitrackStreamingClient.ClientConnectionType.Photon)
{
boneObject.transform.localPosition = Vector3.Lerp(startPos, endPos, Time.deltaTime * 15);
}
else
{
boneObject.transform.localPosition = bonePose.Position.V3;
}
// Clamp retargetted optitrack bones
#region Clamp bone rotation
angleX = bonePose.Orientation.Q.eulerAngles.x;
angleY = bonePose.Orientation.Q.eulerAngles.y;
angleZ = bonePose.Orientation.Q.eulerAngles.z;
if (bonePose.Orientation.Q.eulerAngles.x > 180f)
{
angleX = bonePose.Orientation.Q.eulerAngles.x - 360;
}
if (bonePose.Orientation.Q.eulerAngles.y > 180f)
{
angleY = bonePose.Orientation.Q.eulerAngles.y - 360;
}
if (bonePose.Orientation.Q.eulerAngles.z > 180f)
{
angleZ = bonePose.Orientation.Q.eulerAngles.z - 360;
}
if (boneObject.name.Contains("LThumb1"))
{
SetBoneRotation(boneObject, BoneLimit.LThumb1.xMin, BoneLimit.LThumb1.yMin,
BoneLimit.LThumb1.zMin,
BoneLimit.LThumb1.xMax, BoneLimit.LThumb1.yMax, BoneLimit.LThumb1.zMax);
}
else if (boneObject.name.Contains("LThumb2"))
{
SetBoneRotation(boneObject, BoneLimit.LThumb2.xMin, BoneLimit.LThumb2.yMin,
BoneLimit.LThumb2.zMin,
BoneLimit.LThumb2.xMax, BoneLimit.LThumb2.yMax, BoneLimit.LThumb2.zMax);
}
else if (boneObject.name.Contains("RThumb1"))
{
SetBoneRotation(boneObject, BoneLimit.RThumb1.xMin, BoneLimit.RThumb1.yMin,
BoneLimit.RThumb1.zMin,
BoneLimit.RThumb1.xMax, BoneLimit.RThumb1.yMax, BoneLimit.RThumb1.zMax);
}
else if (boneObject.name.Contains("RThumb2"))
{
SetBoneRotation(boneObject, BoneLimit.RThumb2.xMin, BoneLimit.RThumb2.yMin,
BoneLimit.RThumb2.zMin,
BoneLimit.RThumb2.xMax, BoneLimit.RThumb2.yMax, BoneLimit.RThumb2.zMax);
}
else if (boneObject.name.Contains("RIndex1") || boneObject.name.Contains("RRing1") ||
boneObject.name.Contains("RMiddle1") || boneObject.name.Contains("RPinky1"))
{
SetBoneRotation(boneObject, BoneLimit.RRing1.xMin, BoneLimit.RRing1.yMin, BoneLimit.RRing1.zMin,
BoneLimit.RRing1.xMax, BoneLimit.RRing1.yMax, BoneLimit.RRing1.zMax);
}
else if (boneObject.name.Contains("RIndex2") || boneObject.name.Contains("RIndex3") ||
boneObject.name.Contains("RRing2") || boneObject.name.Contains("RRing3") ||
boneObject.name.Contains("RMiddle2") || boneObject.name.Contains("RMiddle3") ||
boneObject.name.Contains("RPinky2") || boneObject.name.Contains("RPinky3"))
{
SetBoneRotation(boneObject, BoneLimit.RRing2.xMin, BoneLimit.RRing2.yMin, BoneLimit.RRing2.zMin,
BoneLimit.RRing2.xMax, BoneLimit.RRing2.yMax, BoneLimit.RRing2.zMax);
}
else if (boneObject.name.Contains("LIndex1") || boneObject.name.Contains("LRing1") ||
boneObject.name.Contains("LMiddle1") || boneObject.name.Contains("LPinky1"))
{
SetBoneRotation(boneObject, BoneLimit.LRing1.xMin, BoneLimit.LRing1.yMin, BoneLimit.LRing1.zMin,
BoneLimit.LRing1.xMax, BoneLimit.LRing1.yMax, BoneLimit.LRing1.zMax);
}
else if (boneObject.name.Contains("LIndex2") || boneObject.name.Contains("LIndex3") ||
boneObject.name.Contains("LRing2") || boneObject.name.Contains("LRing3") ||
boneObject.name.Contains("LMiddle2") || boneObject.name.Contains("LMiddle3") ||
boneObject.name.Contains("LPinky2") || boneObject.name.Contains("LPinky3"))
{
SetBoneRotation(boneObject, BoneLimit.LRing2.xMin, BoneLimit.LRing2.yMin, BoneLimit.LRing2.zMin,
BoneLimit.LRing2.xMax, BoneLimit.LRing2.yMax, BoneLimit.LRing2.zMax);
}
else
{
if (StreamingClient.ConnectionType == OptitrackStreamingClient.ClientConnectionType.Photon)
{
boneObject.transform.localRotation = Quaternion.Lerp(boneObject.transform.localRotation, bonePose.Orientation.Q, Time.deltaTime * 15);
}
else
{
boneObject.transform.localRotation = bonePose.Orientation.Q;
}
}
#endregion
}
}
// Perform Mecanim retargeting.
if (m_srcPoseHandler != null && m_destPoseHandler != null)
{
// Interpret the streamed pose into Mecanim muscle space representation.
m_srcPoseHandler.GetHumanPose(ref m_humanPose);
// Retarget that muscle space pose to the destination avatar.
m_destPoseHandler.SetHumanPose(ref m_humanPose);
}
}
}
/// <summary>
/// Event handler for NatNet frame delivery. Updates our simplified state representations.
/// NOTE: This executes in the context of the NatNetLib network service thread!
/// </summary>
/// <remarks>
/// Because the <see cref="sFrameOfMocapData"/> type is expensive to marshal, we instead utilize the
/// <see cref="NatNetClient.NativeFrameReceivedEventArgs.NativeFramePointer"/>, treating it as as opaque, and
/// passing it to some helper "accessor" functions to retrieve the subset of data we care about, using only
/// blittable types which do not cause any garbage to be allocated.
/// </remarks>
/// <param name="sender"></param>
/// <param name="eventArgs"></param>
private void OnNatNetFrameReceived(object sender, NatNetClient.NativeFrameReceivedEventArgs eventArgs)
{
// In the event of contention, drop the frame being delivered and return immediately.
// We don't want to stall NatNetLib's internal network service thread.
if (!Monitor.TryEnter(m_frameDataUpdateLock))
{
return;
}
try
{
// Update health markers.
m_receivedFrameSinceConnect = true;
Interlocked.Exchange(ref m_lastFrameDeliveryTimestamp.m_ticks, OptitrackHiResTimer.Now().m_ticks);
// Process received frame.
IntPtr pFrame = eventArgs.NativeFramePointer;
NatNetError result = NatNetError.NatNetError_OK;
// Update rigid bodies.
Int32 frameRbCount;
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetRigidBodyCount(pFrame, out frameRbCount);
NatNetException.ThrowIfNotOK(result, "NatNet_Frame_GetRigidBodyCount failed.");
for (int rbIdx = 0; rbIdx < frameRbCount; ++rbIdx)
{
sRigidBodyData rbData = new sRigidBodyData();
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetRigidBody(pFrame, rbIdx, out rbData);
NatNetException.ThrowIfNotOK(result, "NatNet_Frame_GetRigidBody failed.");
bool bTrackedThisFrame = (rbData.Params & 0x01) != 0;
if (bTrackedThisFrame == false)
{
continue;
}
// Ensure we have a state corresponding to this rigid body ID.
OptitrackRigidBodyState rbState = GetOrCreateRigidBodyState(rbData.Id);
rbState.DeliveryTimestamp = OptitrackHiResTimer.Now();
// Flip coordinate handedness from right to left by inverting X and W.
rbState.Pose.Position = new Vector3(-rbData.X, rbData.Y, rbData.Z);
rbState.Pose.Orientation = new Quaternion(-rbData.QX, rbData.QY, rbData.QZ, -rbData.QW);
}
// Update skeletons.
Int32 frameSkeletonCount;
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetSkeletonCount(pFrame, out frameSkeletonCount);
NatNetException.ThrowIfNotOK(result, "NatNet_Frame_GetSkeletonCount failed.");
for (int skelIdx = 0; skelIdx < frameSkeletonCount; ++skelIdx)
{
Int32 skeletonId;
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_Skeleton_GetId(pFrame, skelIdx, out skeletonId);
NatNetException.ThrowIfNotOK(result, "NatNet_Frame_Skeleton_GetId failed.");
// Ensure we have a state corresponding to this skeleton ID.
OptitrackSkeletonState skelState = GetOrCreateSkeletonState(skeletonId);
// Enumerate this skeleton's bone rigid bodies.
Int32 skelRbCount;
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_Skeleton_GetRigidBodyCount(pFrame, skelIdx, out skelRbCount);
NatNetException.ThrowIfNotOK(result, "NatNet_Frame_Skeleton_GetRigidBodyCount failed.");
for (int boneIdx = 0; boneIdx < skelRbCount; ++boneIdx)
{
sRigidBodyData boneData = new sRigidBodyData();
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_Skeleton_GetRigidBody(pFrame, skelIdx, boneIdx, out boneData);
NatNetException.ThrowIfNotOK(result, "NatNet_Frame_Skeleton_GetRigidBody failed.");
// In the context of frame data (unlike in the definition data), this ID value is a
// packed composite of both the asset/entity (skeleton) ID and member (bone) ID.
Int32 boneSkelId, boneId;
NaturalPoint.NatNetLib.NativeMethods.NatNet_DecodeID(boneData.Id, out boneSkelId, out boneId);
// TODO: Could pre-populate this map when the definitions are retrieved.
// Should never allocate after the first frame, at least.
if (skelState.BonePoses.ContainsKey(boneId) == false)
{
skelState.BonePoses[boneId] = new OptitrackPose();
}
// Flip coordinate handedness from right to left by inverting X and W.
skelState.BonePoses[boneId].Position = new Vector3(-boneData.X, boneData.Y, boneData.Z);
skelState.BonePoses[boneId].Orientation = new Quaternion(-boneData.QX, boneData.QY, boneData.QZ, -boneData.QW);
}
}
// Update markers
Int32 MarkerCount;
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetLabeledMarkerCount(pFrame, out MarkerCount);
NatNetException.ThrowIfNotOK(result, "NatNet_Frame_GetSkeletonCount failed.");
m_latestMarkerStates.Clear();
for (int markerIdx = 0; markerIdx < MarkerCount; ++markerIdx)
{
sMarker marker = new sMarker();
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetLabeledMarker(pFrame, markerIdx, out marker);
NatNetException.ThrowIfNotOK(result, "NatNet_Frame_GetLabeledMarker failed.");
// Flip coordinate handedness
Vector3 markerPos = new Vector3(-marker.X, marker.Y, marker.Z);
OptitrackMarkerState markerState = GetOrCreateMarkerState(marker.Id);
markerState.Position = markerPos;
markerState.Size = marker.Size;
markerState.Labeled = (marker.Params & 0x10) == 0;
markerState.Id = marker.Id;
}
}
catch (Exception ex)
{
Debug.LogError(GetType().FullName + ": OnNatNetFrameReceived encountered an exception.", this);
Debug.LogException(ex, this);
}
finally
{
Monitor.Exit(m_frameDataUpdateLock);
}
}
