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