kindnick/Streamingle_URP
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix : 옵티 스크립트 업데이트 및 프리펩 업데이트 및 필터 시스템추가

Browse Source
This commit is contained in:
qsxft258@gmail.com 2026-06-02 23:23:10 +09:00
parent 3745b37e85
commit 4b31d6de06
6 changed files with 274 additions and 103 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
Assets/External/OptiTrack Unity Plugin/OptiTrack/Prefabs/Client - OptiTrack.prefab (Stored with Git LFS) vendored
View File

Binary file not shown.

4
Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/Editor/OptitrackStreamingClientEditor.cs vendored
View File

@ -121,7 +121,9 @@ public class OptitrackStreamingClientEditor : Editor
runtimeInfo.Clear();
AddInfoRow(runtimeInfo, "Server Address", client.ServerAddress);
AddInfoRow(runtimeInfo, "Local Address", client.LocalAddress);
AddInfoRow(runtimeInfo, "Local Address", string.IsNullOrEmpty(client.ResolvedLocalAddress)
? client.LocalAddress
: client.ResolvedLocalAddress);
AddInfoRow(runtimeInfo, "Connection Type", client.ConnectionType.ToString());
if (!string.IsNullOrEmpty(client.ServerNatNetVersion))

1
Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/Editor/UXML/OptitrackStreamingClientEditor.uxml vendored
View File

@ -13,7 +13,6 @@
<ui:VisualElement class="section">
<ui:Foldout text="Connection Settings" value="true" class="section-foldout">
<uie:PropertyField binding-path="ServerAddress" label="Server Address"/>
<uie:PropertyField binding-path="LocalAddress" label="Local Address"/>
<uie:PropertyField binding-path="ConnectionType" label="Connection Type"/>
<uie:PropertyField binding-path="SkeletonCoordinates" label="Skeleton Coordinates"/>
<uie:PropertyField binding-path="TMarkersetCoordinates" label="TMarkerset Coordinates"/>

15
Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackFaceDevice.cs vendored
View File

@ -66,13 +66,14 @@ public class OptitrackFaceDevice : MonoBehaviour
// Resolved device names (1 or 2: base, base_A+_B)
private List<string> _resolvedDeviceNames = new List<string>(2);
private Dictionary<string, float> _channelSnapshot = new Dictionary<string, float>();
private float _lastResolveAttempt = -10f;
private const float k_ResolveRetrySeconds = 1.0f;
void Start()
{
if (streamingClient == null)
streamingClient = FindObjectOfType<OptitrackStreamingClient>();
streamingClient = FindFirstObjectByType<OptitrackStreamingClient>();
if (streamingClient != null && !streamingClient.ReceiveDevices)
{
@ -163,16 +164,10 @@ public class OptitrackFaceDevice : MonoBehaviour
int appliedBs = 0;
for (int i = 0; i < _resolvedDeviceNames.Count; ++i)
{
var state = streamingClient.GetLatestDeviceState(_resolvedDeviceNames[i]);
if (state == null) continue;
if (!streamingClient.FillDeviceChannelSnapshot(_resolvedDeviceNames[i], _channelSnapshot))
continue;
// Snapshot to a local list to avoid holding the lock during apply.
// ChannelValues is a Dictionary updated by the NatNet thread; iterating
// it while it mutates would throw. The streaming client's GetLatestDeviceState
// returns the live reference, so we iterate quickly under no lock — accept
// occasional torn reads (one frame stale value) over the perf cost of
// copying the dict every frame.
foreach (var kv in state.ChannelValues)
foreach (var kv in _channelSnapshot)
{
string name = kv.Key;
float v = kv.Value;

314
Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackStreamingClient.cs vendored
View File

@ -17,6 +17,7 @@ limitations under the License.
using System;
using System.Collections.Generic;
using System.Net;
using System.Net.Sockets;
using System.Runtime.InteropServices;
using System.Threading;
using UnityEngine;
@ -312,8 +313,17 @@ public class OptitrackStreamingClient : MonoBehaviour
public string ServerAddress = "127.0.0.1";
[Tooltip("Must be on the same network as the Streaming IP (Local Interface) in Motive.")]
[HideInInspector]
public string LocalAddress = "127.0.0.1";
[Tooltip("Automatically selects the local IPv4 interface that routes to ServerAddress. LocalAddress is used as a fallback if detection fails.")]
[HideInInspector]
public bool AutoDetectLocalAddress = true;
[Tooltip("The local IPv4 address selected for the active connection.")]
[HideInInspector]
public string ResolvedLocalAddress = "";
[Tooltip("Unicast performs subscription reducing your overall data set in some applications.")]
public ClientConnectionType ConnectionType;
@ -340,7 +350,7 @@ public class OptitrackStreamingClient : MonoBehaviour
[Tooltip("Draws force plate visuals in the viewport for debugging and other uses.")]
public bool DrawForcePlates = false;
[Tooltip("Receive analog device data (e.g. iFacialMocap face streams). Each frame the full sFrameOfMocapData is marshaled (~200KB), so leave off if no devices are needed.")]
[Tooltip("Receive analog device data (e.g. iFacialMocap face streams). Leave off if no devices are needed.")]
public bool ReceiveDevices = false;
[Tooltip("Motive will record when the Unity project is played.")]
@ -401,6 +411,9 @@ public class OptitrackStreamingClient : MonoBehaviour
/// <summary>Maps from a streamed skeleton's ID to its most recent available pose data.</summary>
private Dictionary<Int32, OptitrackSkeletonState> m_latestSkeletonStates = new Dictionary<Int32, OptitrackSkeletonState>();
/// <summary>Reusable staging buffers used to validate a complete skeleton frame before committing it.</summary>
private Dictionary<Int32, sRigidBodyData[]> m_skeletonFrameScratch = new Dictionary<Int32, sRigidBodyData[]>();
/// <summary>MirrorMode용: 스켈레톤 ID → (boneId → mirrorBoneId) 매핑 캐시.</summary>
private Dictionary<Int32, Dictionary<Int32, Int32>> m_mirrorBoneIdMaps = new Dictionary<Int32, Dictionary<Int32, Int32>>();
@ -771,6 +784,7 @@ public class OptitrackStreamingClient : MonoBehaviour
for (int attempt = 1; attempt <= maxAttempts; attempt++)
{
m_receivedFrameSinceConnect = false;
float delay = (attempt == 1) ? 1.0f : 5.0f;
Debug.Log(string.Format("{0}: 재연결 시도 {1}/{2} — {3}초 후...", GetType().FullName, attempt, maxAttempts, delay), this);
yield return new WaitForSeconds(delay);
@ -1103,30 +1117,62 @@ public class OptitrackStreamingClient : MonoBehaviour
/// <returns>The most recent available state, or null if none available.</returns>
public OptitrackTMarkersetState GetLatestTMarkersetState(Int32 tmarkersetId)
{
OptitrackTMarkersetState tmarState;
lock (m_frameDataUpdateLock)
{
m_latestTMarkersetStates.TryGetValue(tmarkersetId, out tmarState);
}
if (!m_latestTMarkersetStates.TryGetValue(tmarkersetId, out var source) || source == null)
return null;
if ( MirrorMode && tmarState != null && tmarState.BonePoses != null )
{
var mirrored = new OptitrackTMarkersetState
var snapshot = new OptitrackTMarkersetState
{
BonePoses = new Dictionary<Int32, OptitrackPose>( tmarState.BonePoses.Count ),
LocalBonePoses = tmarState.LocalBonePoses,
BonePoses = CopyPoseDictionary(source.BonePoses, MirrorMode),
LocalBonePoses = CopyPoseDictionary(source.LocalBonePoses, MirrorMode),
};
foreach ( var kvp in tmarState.BonePoses )
mirrored.BonePoses[kvp.Key] = new OptitrackPose
{
Position = MirrorPosition( kvp.Value.Position ),
Orientation = MirrorOrientation( kvp.Value.Orientation ),
};
tmarState = mirrored;
return snapshot;
}
}
return tmarState;
private static Dictionary<Int32, OptitrackPose> CopyPoseDictionary(
Dictionary<Int32, OptitrackPose> source, bool mirror)
{
var snapshot = new Dictionary<Int32, OptitrackPose>(source != null ? source.Count : 0);
if (source == null)
return snapshot;
foreach (var kvp in source)
{
snapshot[kvp.Key] = new OptitrackPose
{
Position = mirror ? MirrorPosition(kvp.Value.Position) : kvp.Value.Position,
Orientation = mirror ? MirrorOrientation(kvp.Value.Orientation) : kvp.Value.Orientation,
};
}
return snapshot;
}
/// <summary>
/// Copies trained markerset poses while holding the NatNet frame lock.
/// </summary>
public bool FillTMarkersetSnapshot(Int32 tmarkersetId, bool useLocalBonePoses,
Dictionary<Int32, Vector3> posOut, Dictionary<Int32, Quaternion> oriOut)
{
lock (m_frameDataUpdateLock)
{
if (!m_latestTMarkersetStates.TryGetValue(tmarkersetId, out var state) || state == null)
return false;
var source = useLocalBonePoses ? state.LocalBonePoses : state.BonePoses;
if (source == null)
return false;
posOut.Clear();
oriOut.Clear();
foreach (var kvp in source)
{
posOut[kvp.Key] = MirrorMode ? MirrorPosition(kvp.Value.Position) : kvp.Value.Position;
oriOut[kvp.Key] = MirrorMode ? MirrorOrientation(kvp.Value.Orientation) : kvp.Value.Orientation;
}
return true;
}
}
@ -1550,42 +1596,38 @@ public class OptitrackStreamingClient : MonoBehaviour
// ----------------------------------
// - Device Definitions (generic analog: iFacialMocap, NIDAQ, etc.)
// ----------------------------------
if (m_dataDescs.DeviceDescriptions != null)
lock (m_frameDataUpdateLock)
{
for (int devIdx = 0; devIdx < m_dataDescs.DeviceDescriptions.Count; ++devIdx)
if (m_dataDescs.DeviceDescriptions != null)
{
sDeviceDescription dev = m_dataDescs.DeviceDescriptions[devIdx];
// dev.ChannelNames is a fixed-size 32 array (NatNet C# wrapper limit).
// Plugin can register devices with > 32 channels (Motive stores them all),
// but only the first 32 names survive the wrapper marshaling.
// IMPORTANT: ChannelCount is preserved as the full count reported by Motive
// (could be > 32, e.g. iFacialMocap primary = 54). Downstream consumers
// need this to match the wire frame's nCh (which may also be > 32 if Motive
// doesn't truncate broadcast). ChannelNames is clamped to the marshaled
// array length to avoid IndexOutOfRangeException.
int marshalNameCount = (dev.ChannelNames != null)
? Math.Min(dev.ChannelCount, dev.ChannelNames.Length)
: 0;
OptitrackDeviceDefinition deviceDef = new OptitrackDeviceDefinition
for (int devIdx = 0; devIdx < m_dataDescs.DeviceDescriptions.Count; ++devIdx)
{
Id = dev.Id,
Name = dev.Name,
SerialNumber = dev.SerialNo,
DeviceType = dev.DeviceType,
ChannelDataType = dev.ChannelDataType,
ChannelCount = dev.ChannelCount,
ChannelNames = new List<string>(marshalNameCount),
};
sDeviceDescription dev = m_dataDescs.DeviceDescriptions[devIdx];
for (int i = 0; i < marshalNameCount; ++i)
{
deviceDef.ChannelNames.Add(dev.ChannelNames[i]);
// dev.ChannelNames is a fixed-size 32 array (NatNet C# wrapper limit).
int marshalNameCount = (dev.ChannelNames != null)
? Math.Min(dev.ChannelCount, dev.ChannelNames.Length)
: 0;
OptitrackDeviceDefinition deviceDef = new OptitrackDeviceDefinition
{
Id = dev.Id,
Name = dev.Name,
SerialNumber = dev.SerialNo,
DeviceType = dev.DeviceType,
ChannelDataType = dev.ChannelDataType,
ChannelCount = dev.ChannelCount,
ChannelNames = new List<string>(marshalNameCount),
};
for (int i = 0; i < marshalNameCount; ++i)
{
deviceDef.ChannelNames.Add(dev.ChannelNames[i]);
}
m_deviceDefinitions.Add(deviceDef);
m_deviceNameToId[deviceDef.Name] = deviceDef.Id;
}
m_deviceDefinitions.Add(deviceDef);
m_deviceNameToId[deviceDef.Name] = deviceDef.Id;
}
}
@ -1725,6 +1767,7 @@ public class OptitrackStreamingClient : MonoBehaviour
private System.Collections.IEnumerator ConnectCoroutine()
{
m_receivedFrameSinceConnect = false;
IPAddress serverAddr;
IPAddress localAddr;
NatNetConnectionType connType;
@ -1732,7 +1775,8 @@ public class OptitrackStreamingClient : MonoBehaviour
try
{
serverAddr = IPAddress.Parse( ServerAddress );
localAddr = IPAddress.Parse( LocalAddress );
localAddr = ResolveLocalAddress( serverAddr );
ResolvedLocalAddress = localAddr.ToString();
connType = ConnectionType == ClientConnectionType.Unicast
? NatNetConnectionType.NatNetConnectionType_Unicast
: NatNetConnectionType.NatNetConnectionType_Multicast;
@ -1795,6 +1839,8 @@ public class OptitrackStreamingClient : MonoBehaviour
SubscribeSkeleton(skel.Value, skel.Key);
foreach (KeyValuePair<string, MonoBehaviour> tmark in m_tmarkersets) // trained markerset added
SubscribeTMarkerset(tmark.Value, tmark.Key);
if (DrawTMarkersetMarkers)
SubscribeTMarkMarkers();
}
// 재연결 중에는 녹화 시작 스킵 — Motive의 Take 파일 반복 열기/닫기 방지
@ -1954,13 +2000,6 @@ public class OptitrackStreamingClient : MonoBehaviour
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);
@ -1982,7 +2021,6 @@ public class OptitrackStreamingClient : MonoBehaviour
// Ensure we have a state corresponding to this skeleton ID.
OptitrackSkeletonState skelState = GetOrCreateSkeletonState( skeletonId );
skelState.DeliveryTimestamp = frameTimestamp;
// Enumerate this skeleton's bone rigid bodies.
Int32 skelRbCount;
@ -2002,17 +2040,47 @@ public class OptitrackStreamingClient : MonoBehaviour
continue;
}
// Motive can occasionally emit an empty or partial skeleton payload. Do not let
// one bad bone overwrite the previous valid pose: stage and validate first.
if (skelRbCount != skelDef.Bones.Count)
{
continue;
}
sRigidBodyData[] stagedBones = GetSkeletonFrameScratch(skeletonId, skelRbCount);
bool isValidSkeletonFrame = true;
for (int boneIdx = 0; boneIdx < skelRbCount; ++boneIdx)
{
sRigidBodyData boneData = new sRigidBodyData();
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_Skeleton_GetRigidBody( pFrame, skelIdx, boneIdx, out boneData );
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_Skeleton_GetRigidBody( pFrame, skelIdx, boneIdx, out stagedBones[boneIdx] );
NatNetException.ThrowIfNotOK( result, "NatNet_Frame_Skeleton_GetRigidBody failed." );
sRigidBodyData boneData = stagedBones[boneIdx];
// 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 );
if (boneSkelId != skeletonId ||
!skelDef.BoneIdToParentIdMap.ContainsKey(boneId) ||
!IsValidSkeletonBoneData(boneData))
{
isValidSkeletonFrame = false;
break;
}
}
if (!isValidSkeletonFrame)
{
continue;
}
// Commit global transforms only after the whole payload has passed validation.
for (int boneIdx = 0; boneIdx < skelRbCount; ++boneIdx)
{
sRigidBodyData boneData = stagedBones[boneIdx];
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)
@ -2029,7 +2097,17 @@ public class OptitrackStreamingClient : MonoBehaviour
Quaternion boneOri = new Quaternion(-boneData.QX, boneData.QY, boneData.QZ, -boneData.QW);
skelState.BonePoses[boneId].Position = bonePos;
skelState.BonePoses[boneId].Orientation = boneOri;
}
// Derive locals in a second pass so parent order in the payload does not matter.
for (int boneIdx = 0; boneIdx < skelRbCount; ++boneIdx)
{
sRigidBodyData boneData = stagedBones[boneIdx];
Int32 boneSkelId, boneId;
NaturalPoint.NatNetLib.NativeMethods.NatNet_DecodeID( boneData.Id, out boneSkelId, out boneId );
Vector3 bonePos = skelState.BonePoses[boneId].Position;
Quaternion boneOri = skelState.BonePoses[boneId].Orientation;
Vector3 parentBonePos = new Vector3(0,0,0);
Quaternion parentBoneOri = new Quaternion(0,0,0,1);
@ -2042,6 +2120,8 @@ public class OptitrackStreamingClient : MonoBehaviour
skelState.LocalBonePoses[boneId].Position = bonePos - parentBonePos;
skelState.LocalBonePoses[boneId].Orientation = Quaternion.Inverse(parentBoneOri) * boneOri;
}
skelState.DeliveryTimestamp = frameTimestamp;
}
// -----------------------------------------------------
@ -2050,6 +2130,7 @@ public class OptitrackStreamingClient : MonoBehaviour
// m_dataDescs를 로컬 변수로 캡처: UpdateDefinitions()(메인 스레드)가 참조를 교체해도 안전
// null 체크: SkipDataDescriptions=true 또는 UpdateDefinitions() 미완료/실패 시 크래시 방지
var dataDescsSnapshot = m_dataDescs;
m_latestTMarkMarkerStates.Clear();
if (dataDescsSnapshot != null && dataDescsSnapshot.AssetDescriptions != null)
for (int tmarkIdx = 0; tmarkIdx < dataDescsSnapshot.AssetDescriptions.Count; ++tmarkIdx)
{
@ -2112,8 +2193,6 @@ public class OptitrackStreamingClient : MonoBehaviour
NatNetException.ThrowIfNotOK(result, "NatNet_Frame_TMarkerset_GetMarkerCount failed.");*/
//Debug.Log("tmark marker count: " + tmarkMarkerCount); // working finally
m_latestTMarkMarkerStates.Clear();
// Update Trained Markerset Marker data
for (int markerIdx = 0; markerIdx < tmarkMarkerCount; ++markerIdx)
{
@ -2232,10 +2311,14 @@ public class OptitrackStreamingClient : MonoBehaviour
devState.DeliveryTimestamp = frameTimestamp;
int chCount = Math.Min(devData.ChannelCount, devDef.ChannelCount);
chCount = Math.Min(chCount, devData.ChannelData != null ? devData.ChannelData.Length : 0);
chCount = Math.Min(chCount, devDef.ChannelNames != null ? devDef.ChannelNames.Count : 0);
for (int ch = 0; ch < chCount; ++ch)
{
// Use subframe 0 — face data is one sample per frame.
sAnalogChannelData chData = devData.ChannelData[ch];
if (chData.Values == null || chData.Values.Length == 0)
continue;
float value = chData.Values[0];
devState.ChannelValues[devDef.ChannelNames[ch]] = value;
}
@ -2270,7 +2353,7 @@ public class OptitrackStreamingClient : MonoBehaviour
Monitor.Enter( m_frameDataUpdateLock );
try
{
return m_latestDeviceStates.TryGetValue(deviceId, out var s) ? s : null;
return m_latestDeviceStates.TryGetValue(deviceId, out var state) ? CopyDeviceState(state) : null;
}
finally
{
@ -2286,8 +2369,8 @@ public class OptitrackStreamingClient : MonoBehaviour
try
{
if (m_deviceNameToId.TryGetValue(deviceName, out Int32 id) &&
m_latestDeviceStates.TryGetValue(id, out var s))
return s;
m_latestDeviceStates.TryGetValue(id, out var state))
return CopyDeviceState(state);
return null;
}
finally
@ -2296,6 +2379,38 @@ public class OptitrackStreamingClient : MonoBehaviour
}
}
private static OptitrackDeviceState CopyDeviceState(OptitrackDeviceState source)
{
if (source == null)
return null;
return new OptitrackDeviceState
{
Id = source.Id,
Name = source.Name,
FrameNumber = source.FrameNumber,
DeliveryTimestamp = source.DeliveryTimestamp,
ChannelValues = new Dictionary<string, float>(source.ChannelValues),
};
}
/// <summary>Copies a device's latest channel values into a caller-owned dictionary.</summary>
public bool FillDeviceChannelSnapshot(string deviceName, Dictionary<string, float> channelValuesOut)
{
lock (m_frameDataUpdateLock)
{
channelValuesOut.Clear();
if (!m_deviceNameToId.TryGetValue(deviceName, out Int32 id) ||
!m_latestDeviceStates.TryGetValue(id, out var state) ||
state == null)
return false;
foreach (var kvp in state.ChannelValues)
channelValuesOut[kvp.Key] = kvp.Value;
return true;
}
}
/// <summary>Returns a snapshot of currently registered device definitions.</summary>
public List<OptitrackDeviceDefinition> GetDeviceDefinitions()
@ -2487,7 +2602,7 @@ public class OptitrackStreamingClient : MonoBehaviour
{
if (m_client != null && ConnectionType == ClientConnectionType.Unicast)
{
bool subscribeSucceeded4 = m_client.RequestCommand("SubscribeToData, TrainedMarkersetMarkers,All", 2000, 3);
bool subscribeSucceeded4 = m_client.RequestCommand("SubscribeToData,TrainedMarkersetMarkers,All", 2000, 3);
//Debug.Log("TMMarkers: " + subscribeSucceeded4);
// Log a warning on the first failure.
@ -2557,6 +2672,71 @@ public class OptitrackStreamingClient : MonoBehaviour
}
}
private IPAddress ResolveLocalAddress(IPAddress serverAddress)
{
if (!AutoDetectLocalAddress)
return IPAddress.Parse(LocalAddress);
if (IPAddress.IsLoopback(serverAddress))
return IPAddress.Loopback;
try
{
using (var socket = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp))
{
socket.Connect(new IPEndPoint(serverAddress, NatNetConstants.DefaultCommandPort));
if (socket.LocalEndPoint is IPEndPoint endpoint &&
endpoint.Address.AddressFamily == AddressFamily.InterNetwork &&
!IPAddress.Any.Equals(endpoint.Address))
{
LocalAddress = endpoint.Address.ToString();
return endpoint.Address;
}
}
}
catch (Exception ex)
{
Debug.LogWarning(GetType().FullName + ": 로컬 주소 자동 탐지 실패. LocalAddress fallback을 사용합니다. " + ex.Message, this);
}
return IPAddress.Parse(LocalAddress);
}
private sRigidBodyData[] GetSkeletonFrameScratch(Int32 skeletonId, Int32 boneCount)
{
if (!m_skeletonFrameScratch.TryGetValue(skeletonId, out var scratch) || scratch.Length != boneCount)
{
scratch = new sRigidBodyData[boneCount];
m_skeletonFrameScratch[skeletonId] = scratch;
}
return scratch;
}
private static bool IsValidSkeletonBoneData(sRigidBodyData boneData)
{
if ((boneData.Params & 0x01) == 0)
return false;
float quaternionMagnitudeSquared =
boneData.QX * boneData.QX +
boneData.QY * boneData.QY +
boneData.QZ * boneData.QZ +
boneData.QW * boneData.QW;
return IsFinite(boneData.X) &&
IsFinite(boneData.Y) &&
IsFinite(boneData.Z) &&
IsFinite(boneData.QX) &&
IsFinite(boneData.QY) &&
IsFinite(boneData.QZ) &&
IsFinite(boneData.QW) &&
quaternionMagnitudeSquared > 0.000001f;
}
private static bool IsFinite(float value)
{
return !float.IsNaN(value) && !float.IsInfinity(value);
}
/// <summary>
/// Returns the <see cref="OptitrackRigidBodyState"/> corresponding to the provided <paramref name="rigidBodyId"/>.
@ -2723,4 +2903,4 @@ public class OptitrackStreamingClient : MonoBehaviour
Monitor.Exit( m_frameDataUpdateLock );
}
#endregion Private methods
}
}

39
Assets/External/OptiTrack Unity Plugin/OptiTrack/Scripts/OptitrackTrainedMarkerset.cs vendored
View File

@ -1,5 +1,5 @@
/*
Copyright © 2016 NaturalPoint Inc.
Copyright © 2016 NaturalPoint Inc.
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
@ -53,6 +53,8 @@ public class OptitrackTrainedMarkerset : MonoBehaviour
private Dictionary<string, Transform> m_cachedBoneNameMap = new Dictionary<string, Transform>(); // Optitrack's skeleton's bone names
private Dictionary<Transform, Transform> m_transformMap = new Dictionary<Transform, Transform>();
private Dictionary<Int32, Vector3> m_snapshotPositions = new Dictionary<Int32, Vector3>();
private Dictionary<Int32, Quaternion> m_snapshotOrientations = new Dictionary<Int32, Quaternion>();
#endregion Private fields
@ -115,36 +117,29 @@ public class OptitrackTrainedMarkerset : MonoBehaviour
private void Update()
{
OptitrackTMarkersetState tmarState = StreamingClient.GetLatestTMarkersetState( m_tmarkersetDef.Id );
if (tmarState != null)
bool useLocalBonePoses = StreamingClient.TMarkersetCoordinates == StreamingCoordinatesValues.Global;
if (StreamingClient.FillTMarkersetSnapshot(m_tmarkersetDef.Id, useLocalBonePoses,
m_snapshotPositions, m_snapshotOrientations))
{
// Update the transforms of the bone GameObjects.
for (int i = 0; i < m_tmarkersetDef.Bones.Count; ++i)
{
Int32 boneId = m_tmarkersetDef.Bones[i].Id;
OptitrackPose bonePose;
GameObject boneObject;
bool foundPose = false;
if (StreamingClient.TMarkersetCoordinates == StreamingCoordinatesValues.Global)
{
// Use global tmarkerset coordinates
foundPose = tmarState.LocalBonePoses.TryGetValue(boneId, out bonePose);
}
else
{
// Use local tmarkerset coordinates
foundPose = tmarState.BonePoses.TryGetValue(boneId, out bonePose);
}
bool foundObject = m_boneObjectMap.TryGetValue(boneId, out boneObject);
if (foundPose && foundObject)
if (foundObject &&
m_snapshotPositions.TryGetValue(boneId, out Vector3 position) &&
m_snapshotOrientations.TryGetValue(boneId, out Quaternion orientation))
{
boneObject.transform.localPosition = bonePose.Position;
boneObject.transform.localRotation = bonePose.Orientation;
m_transformMap[boneObject.transform].transform.localPosition = bonePose.Position;
m_transformMap[boneObject.transform].transform.localRotation = bonePose.Orientation;
boneObject.transform.localPosition = position;
boneObject.transform.localRotation = orientation;
if (m_transformMap.TryGetValue(boneObject.transform, out Transform target))
{
target.localPosition = position;
target.localRotation = orientation;
}
}
}
}
@ -187,4 +182,4 @@ public class OptitrackTrainedMarkerset : MonoBehaviour
}
}
#endregion Private methods
}
}