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); } }