/// <summary>Retrieves the definition of the skeleton with the specified asset name.</summary> /// <param name="skeletonAssetName">The name of the skeleton for which to retrieve the definition.</param> /// <returns>The specified skeleton definition, or null if not found.</returns> public OptitrackSkeletonDefinition GetSkeletonDefinitionByName(string skeletonAssetName) { if (ConnectionType == ClientConnectionType.Photon) { for (int i = 0; i < m_photonisedSkeletonDefinitions.Count; ++i) { OptitrackSkeletonDefinition skelDef = m_photonisedSkeletonDefinitions [i]; if (skelDef.Name.Equals(skeletonAssetName, StringComparison.InvariantCultureIgnoreCase)) { return(skelDef); } } } else { for (int i = 0; i < m_skeletonDefinitions.Count; ++i) { OptitrackSkeletonDefinition skelDef = m_skeletonDefinitions [i]; if (skelDef.Name.Equals(skeletonAssetName, StringComparison.InvariantCultureIgnoreCase)) { return(skelDef); } } } return(null); }
/// <summary>Request data descriptions from the host, then update our definitions.</summary> /// <exception cref="NatNetException"> /// Thrown by <see cref="NatNetClient.GetDataDescriptions"/> if the request to the server fails. /// </exception> public void UpdateDefinitions() { // This may throw an exception if the server request times out or otherwise fails. m_dataDescs = m_client.GetDataDescriptions(); m_skeletonDefinitions.Clear(); for (int nativeDescIdx = 0; nativeDescIdx < m_dataDescs.SkeletonDescriptions.Count; ++nativeDescIdx) { sSkeletonDescription nativeSkel = m_dataDescs.SkeletonDescriptions[nativeDescIdx]; OptitrackSkeletonDefinition skelDef = new OptitrackSkeletonDefinition { Id = nativeSkel.Id, Name = nativeSkel.Name, Bones = new List <OptitrackSkeletonDefinition.BoneDefinition>(nativeSkel.RigidBodyCount), }; // Populate nested bone definitions. for (int nativeBoneIdx = 0; nativeBoneIdx < nativeSkel.RigidBodyCount; ++nativeBoneIdx) { sRigidBodyDescription nativeBone = nativeSkel.RigidBodies[nativeBoneIdx]; OptitrackSkeletonDefinition.BoneDefinition boneDef = new OptitrackSkeletonDefinition.BoneDefinition { Id = nativeBone.Id, ParentId = nativeBone.ParentId, Name = nativeBone.Name, Offset = new Vector3(nativeBone.OffsetX, nativeBone.OffsetY, nativeBone.OffsetZ), }; skelDef.Bones.Add(boneDef); } m_skeletonDefinitions.Add(skelDef); } }
void Start() { // If the user didn't explicitly associate a client, find a suitable default. if (this.StreamingClient == null) { this.StreamingClient = OptitrackStreamingClient.FindDefaultClient(); // If we still couldn't find one, disable this component. if (this.StreamingClient == null) { Debug.LogError(GetType().FullName + ": Streaming client not set, and no " + typeof(OptitrackStreamingClient).FullName + " components found in scene; disabling this component.", this); this.enabled = false; return; } } this.StreamingClient.RegisterSkeleton(this, this.SkeletonAssetName); // Create a lookup from Mecanim anatomy bone names to OptiTrack streaming bone names. CacheBoneNameMap(this.StreamingClient.BoneNamingConvention, this.SkeletonAssetName); // Retrieve the OptiTrack skeleton definition. m_skeletonDef = this.StreamingClient.GetSkeletonDefinitionByName(this.SkeletonAssetName); if (m_skeletonDef == null) { Debug.LogError(GetType().FullName + ": Could not find skeleton definition with the name \"" + this.SkeletonAssetName + "\"", this); this.enabled = false; return; } // Create a hierarchy of GameObjects that will receive the skeletal pose data. string rootObjectName = "OptiTrack Skeleton - " + this.SkeletonAssetName; m_rootObject = new GameObject(rootObjectName); m_boneObjectMap = new Dictionary <Int32, GameObject>(m_skeletonDef.Bones.Count); for (int boneDefIdx = 0; boneDefIdx < m_skeletonDef.Bones.Count; ++boneDefIdx) { OptitrackSkeletonDefinition.BoneDefinition boneDef = m_skeletonDef.Bones[boneDefIdx]; GameObject boneObject = new GameObject(boneDef.Name); boneObject.transform.parent = boneDef.ParentId == 0 ? m_rootObject.transform : m_boneObjectMap[boneDef.ParentId].transform; boneObject.transform.localPosition = boneDef.Offset; m_boneObjectMap[boneDef.Id] = boneObject; } // Hook up retargeting between those GameObjects and the destination Avatar. MecanimSetup(rootObjectName); // Can't re-parent this until after Mecanim setup, or else Mecanim gets confused. m_rootObject.transform.parent = this.StreamingClient.transform; m_rootObject.transform.localPosition = Vector3.zero; m_rootObject.transform.localRotation = Quaternion.identity; }
void redirectSkeleton() { if (this.StreamingClient == null) { return; } // Create a lookup from Mecanim anatomy bone names to OptiTrack streaming bone names. CacheBoneNameMap(this.StreamingClient.BoneNamingConvention, this.SkeletonAssetName); // Retrieve the OptiTrack skeleton definition. m_skeletonDef = this.StreamingClient.GetSkeletonDefinitionByName(this.SkeletonAssetName); if (m_skeletonDef == null) { StreamingClient.TriggerUpdateDefinitions(); Debug.LogError( GetType().FullName + ": Could not find skeleton definition with the name \"" + this.SkeletonAssetName + "\"", this); this.enabled = false; return; } else { Debug.Log("find a skeleton"); } // Create a hierarchy of GameObjects that will receive the skeletal pose data. string rootObjectName = "OptiTrack Skeleton - " + this.SkeletonAssetName; m_rootObject = new GameObject(rootObjectName); m_boneObjectMap = new Dictionary <Int32, GameObject>(m_skeletonDef.Bones.Count); for (int boneDefIdx = 0; boneDefIdx < m_skeletonDef.Bones.Count; ++boneDefIdx) { OptitrackSkeletonDefinition.BoneDefinition boneDef = m_skeletonDef.Bones[boneDefIdx]; GameObject boneObject = new GameObject(boneDef.Name); boneObject.transform.parent = boneDef.ParentId == 0 ? m_rootObject.transform : m_boneObjectMap[boneDef.ParentId].transform; boneObject.transform.localPosition = boneDef.Offset.V3; m_boneObjectMap[boneDef.Id] = boneObject; } // Hook up retargeting between those GameObjects and the destination Avatar. MecanimSetup(rootObjectName); // Can't reparent this until after Mecanim setup, or else Mecanim gets confused. m_rootObject.transform.parent = this.StreamingClient.transform; m_rootObject.transform.localPosition = Vector3.zero; m_rootObject.transform.localRotation = Quaternion.identity; }
/// <summary>Retrieves the definition of the skeleton with the specified asset name.</summary> /// <param name="skeletonAssetName">The name of the skeleton for which to retrieve the definition.</param> /// <returns>The specified skeleton definition, or null if not found.</returns> public OptitrackSkeletonDefinition GetSkeletonDefinitionByName(string skeletonAssetName) { for (int i = 0; i < m_skeletonDefinitions.Count; ++i) { OptitrackSkeletonDefinition skelDef = m_skeletonDefinitions[i]; if (skelDef.Name.Equals(skeletonAssetName, StringComparison.InvariantCultureIgnoreCase)) { return(skelDef); } } return(null); }
/// <summary>Retrieves the definition of the skeleton with the specified asset name.</summary> /// <param name="skeletonAssetName">The name of the skeleton for which to retrieve the definition.</param> /// <returns>The specified skeleton definition, or null if not found.</returns> public OptitrackSkeletonDefinition GetSkeletonDefinitionByName( string skeletonAssetName ) { for ( int i = 0; i < m_skeletonDefinitions.Count; ++i ) { OptitrackSkeletonDefinition skelDef = m_skeletonDefinitions[i]; if ( skelDef.Name.Equals( skeletonAssetName, StringComparison.OrdinalIgnoreCase ) ) { return skelDef; } } return null; }
/// <summary>Retrieves the definition of the skeleton with the specified skeleton id.</summary> /// <param name="skeletonId">The id of the skeleton for which to retrieve the definition.</param> /// <returns>The specified skeleton definition, or null if not found.</returns> public OptitrackSkeletonDefinition GetSkeletonDefinitionById(Int32 skeletonId) { for (int i = 0; i < m_skeletonDefinitions.Count; ++i) { OptitrackSkeletonDefinition skelDef = m_skeletonDefinitions[i]; if (skelDef.Id == skeletonId) { return(skelDef); } } return(null); }
/// <summary>Request data descriptions from the host, then update our definitions.</summary> /// <exception cref="NatNetException"> /// Thrown by <see cref="NatNetClient.GetDataDescriptions"/> if the request to the server fails. /// </exception> public void UpdateDefinitions() { if (m_client == null) { return; } // This may throw an exception if the server request times out or otherwise fails. m_dataDescs = m_client.GetDataDescriptions(); m_skeletonDefinitions.Clear(); for (int nativeDescIdx = 0; nativeDescIdx < m_dataDescs.SkeletonDescriptions.Count; ++nativeDescIdx) { sSkeletonDescription nativeSkel = m_dataDescs.SkeletonDescriptions[nativeDescIdx]; OptitrackSkeletonDefinition skelDef = new OptitrackSkeletonDefinition { Id = nativeSkel.Id, Name = nativeSkel.Name, Bones = new List <OptitrackSkeletonDefinition.BoneDefinition>(nativeSkel.RigidBodyCount), }; // Populate nested bone definitions. for (int nativeBoneIdx = 0; nativeBoneIdx < nativeSkel.RigidBodyCount; ++nativeBoneIdx) { sRigidBodyDescription nativeBone = nativeSkel.RigidBodies[nativeBoneIdx]; OptitrackSkeletonDefinition.BoneDefinition boneDef = new OptitrackSkeletonDefinition.BoneDefinition { Id = nativeBone.Id, ParentId = nativeBone.ParentId, Name = nativeBone.Name, Offset = new Vector3Serializer(nativeBone.OffsetX, nativeBone.OffsetY, nativeBone.OffsetZ), }; skelDef.Bones.Add(boneDef); } m_skeletonDefinitions.Add(skelDef); } BinaryFormatter binFormatter = new BinaryFormatter(); MemoryStream mStream = new MemoryStream(); binFormatter.Serialize(mStream, m_skeletonDefinitions); photonView.RPC("RPC_UpdateDefinitions", PhotonTargets.AllBuffered, new object[] { mStream.ToArray() }); }
public override void StartTracker(HumanoidControl _humanoid) { humanoid = _humanoid; if (!enabled) { return; } //if (device == null) { // device = new NativeOptitrackDevice(); // device.Init(localAddress, serverAddress, serverCommandPort, serverDataPort); //} if (trackingType == TrackingType.Skeleton) { CacheHumanBodyBonesMap(streamingClient.BoneNamingConvention, skeletonName); m_skeletonDef = streamingClient.GetSkeletonDefinitionByName(skeletonName); } }
/// <summary>Request data descriptions from the host, then update our definitions.</summary> /// <exception cref="NatNetException"> /// Thrown by <see cref="NatNetClient.GetDataDescriptions"/> if the request to the server fails. /// </exception> public void UpdateDefinitions() { // This may throw an exception if the server request times out or otherwise fails. m_dataDescs = m_client.GetDataDescriptions(); m_rigidBodyDefinitions.Clear(); m_skeletonDefinitions.Clear(); // Translate rigid body definitions. for (int nativeRbDescIdx = 0; nativeRbDescIdx < m_dataDescs.RigidBodyDescriptions.Count; ++nativeRbDescIdx) { sRigidBodyDescription nativeRb = m_dataDescs.RigidBodyDescriptions[nativeRbDescIdx]; OptitrackRigidBodyDefinition rbDef = new OptitrackRigidBodyDefinition { Id = nativeRb.Id, Name = nativeRb.Name, Markers = new List <OptitrackRigidBodyDefinition.MarkerDefinition>(nativeRb.MarkerCount), }; // Populate nested marker definitions. for (int nativeMarkerIdx = 0; nativeMarkerIdx < nativeRb.MarkerCount; ++nativeMarkerIdx) { int positionOffset = nativeMarkerIdx * Marshal.SizeOf(typeof(MarkerDataVector)); IntPtr positionPtr = new IntPtr(nativeRb.MarkerPositions.ToInt64() + positionOffset); int labelOffset = nativeMarkerIdx * Marshal.SizeOf(typeof(Int32)); IntPtr labelPtr = new IntPtr(nativeRb.MarkerRequiredLabels.ToInt64() + labelOffset); MarkerDataVector nativePos = (MarkerDataVector)Marshal.PtrToStructure(positionPtr, typeof(MarkerDataVector)); Int32 nativeLabel = Marshal.ReadInt32(labelPtr); OptitrackRigidBodyDefinition.MarkerDefinition markerDef = new OptitrackRigidBodyDefinition.MarkerDefinition { Position = new Vector3(nativePos.Values[0], nativePos.Values[1], nativePos.Values[2]), RequiredLabel = nativeLabel, }; rbDef.Markers.Add(markerDef); } m_rigidBodyDefinitions.Add(rbDef); } // Translate skeleton definitions. for (int nativeSkelDescIdx = 0; nativeSkelDescIdx < m_dataDescs.SkeletonDescriptions.Count; ++nativeSkelDescIdx) { sSkeletonDescription nativeSkel = m_dataDescs.SkeletonDescriptions[nativeSkelDescIdx]; OptitrackSkeletonDefinition skelDef = new OptitrackSkeletonDefinition { Id = nativeSkel.Id, Name = nativeSkel.Name, Bones = new List <OptitrackSkeletonDefinition.BoneDefinition>(nativeSkel.RigidBodyCount), }; // Populate nested bone definitions. for (int nativeBoneIdx = 0; nativeBoneIdx < nativeSkel.RigidBodyCount; ++nativeBoneIdx) { sRigidBodyDescription nativeBone = nativeSkel.RigidBodies[nativeBoneIdx]; OptitrackSkeletonDefinition.BoneDefinition boneDef = new OptitrackSkeletonDefinition.BoneDefinition { Id = nativeBone.Id, ParentId = nativeBone.ParentId, Name = nativeBone.Name, Offset = new Vector3(-nativeBone.OffsetX, nativeBone.OffsetY, nativeBone.OffsetZ), }; skelDef.Bones.Add(boneDef); } m_skeletonDefinitions.Add(skelDef); } }
/// <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; // get timestamp UInt64 transmitTimestamp; result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetTransmitTimestamp(pFrame, out transmitTimestamp); // get and decode timecode (if available) UInt32 timecode; UInt32 timecodeSubframe; result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetTimecode(pFrame, out timecode, out timecodeSubframe); Int32 hour, minute, second, frameNumber, subframeNumber; NaturalPoint.NatNetLib.NativeMethods.NatNet_DecodeTimecode(timecode, timecodeSubframe, out hour, out minute, out second, out frameNumber, out subframeNumber); // 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); RigidBodyDataToState(rbData, OptitrackHiResTimer.Now(), rbState); } // 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(); } if (skelState.LocalBonePoses.ContainsKey(boneId) == false) { skelState.LocalBonePoses[boneId] = new OptitrackPose(); } // Flip coordinate handedness from right to left by inverting X and W. Vector3 bonePos = new Vector3(-boneData.X, boneData.Y, boneData.Z); Quaternion boneOri = new Quaternion(-boneData.QX, boneData.QY, boneData.QZ, -boneData.QW); skelState.BonePoses[boneId].Position = bonePos; skelState.BonePoses[boneId].Orientation = boneOri; Vector3 parentBonePos = new Vector3(0, 0, 0); Quaternion parentBoneOri = new Quaternion(0, 0, 0, 1); OptitrackSkeletonDefinition skelDef = GetSkeletonDefinitionById(skeletonId); if (skelDef == null) { Debug.LogError(GetType().FullName + ": OnNatNetFrameReceived, no corresponding skeleton definition for received skeleton frame data.", this); continue; } Int32 pId = skelDef.BoneIdToParentIdMap[boneId]; if (pId != 0) { parentBonePos = skelState.BonePoses[pId].Position; parentBoneOri = skelState.BonePoses[pId].Orientation; } skelState.LocalBonePoses[boneId].Position = bonePos - parentBonePos; skelState.LocalBonePoses[boneId].Orientation = Quaternion.Inverse(parentBoneOri) * boneOri; } } // 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 OptitrackMarkerState markerState = GetOrCreateMarkerState(marker.Id); markerState.Position = new Vector3(-marker.X, marker.Y, marker.Z); 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); } }