4104 lines
173 KiB
C#
4104 lines
173 KiB
C#
/*
|
||
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.
|
||
You may obtain a copy of the License at
|
||
|
||
http://www.apache.org/licenses/LICENSE-2.0
|
||
|
||
Unless required by applicable law or agreed to in writing, software
|
||
distributed under the License is distributed on an "AS IS" BASIS,
|
||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||
See the License for the specific language governing permissions and
|
||
limitations under the License.
|
||
*/
|
||
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Net;
|
||
using System.Net.Sockets;
|
||
using System.Runtime.InteropServices;
|
||
using System.Text;
|
||
using System.Threading;
|
||
using UnityEngine;
|
||
using NaturalPoint;
|
||
using NaturalPoint.NatNetLib;
|
||
|
||
/// <summary>Skeleton naming conventions supported by OptiTrack Motive.</summary>
|
||
public enum OptitrackBoneNameConvention
|
||
{
|
||
Motive,
|
||
FBX,
|
||
BVH,
|
||
}
|
||
|
||
public enum StreamingCoordinatesValues
|
||
{
|
||
Local,
|
||
Global
|
||
}
|
||
|
||
/// <summary>Describes the position and orientation of a streamed tracked object.</summary>
|
||
public class OptitrackPose
|
||
{
|
||
public Vector3 Position;
|
||
public Quaternion Orientation;
|
||
}
|
||
|
||
/// <summary>Represents the state of a streamed marker.</summary>
|
||
public class OptitrackMarkerState
|
||
{
|
||
public string Name;
|
||
public Vector3 Position;
|
||
public float Size;
|
||
public bool Labeled;
|
||
public Int32 Id;
|
||
public bool IsActive;
|
||
}
|
||
|
||
/// <summary>Represents the state of a streamed rigid body at an instant in time.</summary>
|
||
public class OptitrackRigidBodyState
|
||
{
|
||
public OptitrackHiResTimer.Timestamp DeliveryTimestamp;
|
||
public OptitrackPose Pose;
|
||
public bool IsTracked;
|
||
}
|
||
|
||
/// <summary>Represents the state of a streamed skeleton at an instant in time.</summary>
|
||
public class OptitrackSkeletonState
|
||
{
|
||
/// <summary>Maps from OptiTrack bone IDs to their corresponding bone poses.</summary>
|
||
public Dictionary<Int32, OptitrackPose> BonePoses;
|
||
public Dictionary<Int32, OptitrackPose> LocalBonePoses;
|
||
/// <summary>NatNet ?꾨젅?꾩씠 ?섏떊???쒓컖 (怨좏빐?곷룄 ?섎뱶?⑥뼱 ??대㉧ 湲곕컲).</summary>
|
||
public OptitrackHiResTimer.Timestamp DeliveryTimestamp;
|
||
}
|
||
|
||
/// <summary>Represents the state of a streamed trained markerset at an instant in time.</summary>
|
||
public class OptitrackTMarkersetState // trained markerset added
|
||
{
|
||
/// <summary>Maps from OptiTrack bone IDs to their corresponding bone poses.</summary>
|
||
public Dictionary<Int32, OptitrackPose> BonePoses;
|
||
public Dictionary<Int32, OptitrackPose> LocalBonePoses;
|
||
}
|
||
|
||
public class OptitrackRigidBodyDefinition
|
||
{
|
||
public class MarkerDefinition
|
||
{
|
||
public Vector3 Position;
|
||
public Int32 RequiredLabel;
|
||
}
|
||
|
||
public Int32 Id;
|
||
public string Name;
|
||
public List<MarkerDefinition> Markers;
|
||
}
|
||
|
||
/// <summary>Describes the hierarchy and neutral pose of a streamed skeleton.</summary>
|
||
public class OptitrackSkeletonDefinition
|
||
{
|
||
public class BoneDefinition
|
||
{
|
||
/// <summary>The ID of this bone within this skeleton.</summary>
|
||
public Int32 Id;
|
||
|
||
/// <summary>The ID of this bone's parent bone. A value of 0 means that this is the root bone.</summary>
|
||
public Int32 ParentId;
|
||
|
||
/// <summary>The name of this bone.</summary>
|
||
public string Name;
|
||
|
||
/// <summary>
|
||
/// This bone's position offset from its parent in the skeleton's neutral pose.
|
||
/// (The neutral orientation is always <see cref="Quaternion.identity"/>.)
|
||
/// </summary>
|
||
public Vector3 Offset;
|
||
}
|
||
|
||
/// <summary>Skeleton ID. Used as an argument to <see cref="OptitrackStreamingClient.GetLatestSkeletonState"/>.</summary>
|
||
public Int32 Id;
|
||
|
||
/// <summary>Skeleton asset name.</summary>
|
||
public string Name;
|
||
|
||
/// <summary>Bone names, hierarchy, and neutral pose position information.</summary>
|
||
public List<BoneDefinition> Bones;
|
||
|
||
/// <summary>Bone hierarchy information</summary>
|
||
public Dictionary<Int32, Int32> BoneIdToParentIdMap;
|
||
}
|
||
|
||
public class OptitrackTMarkersetDefinition // trained markerset added // check where this is coming from
|
||
{
|
||
public class BoneDefinition
|
||
{
|
||
/// <summary>The ID of this bone within this trained markerset.</summary>
|
||
public Int32 Id;
|
||
|
||
/// <summary>The ID of this bone's parent bone. A value of 0 means that this is the root bone.</summary>
|
||
public Int32 ParentId;
|
||
|
||
/// <summary>The name of this bone.</summary>
|
||
public string Name;
|
||
|
||
/// <summary>
|
||
/// This bone's position offset from its parent in the skeleton's neutral pose.
|
||
/// (The neutral orientation is always <see cref="Quaternion.identity"/>.)
|
||
/// </summary>
|
||
public Vector3 Offset;
|
||
}
|
||
|
||
public class MarkerDefinition
|
||
{
|
||
/// <summary>The name of this marker.</summary>
|
||
public string Name;
|
||
public Vector3 Position;
|
||
public Int32 Id;
|
||
|
||
//public float Size;
|
||
//public bool Labeled;
|
||
//public bool IsActive;
|
||
}
|
||
|
||
/// <summary>Asset ID. Used as an argument to <see cref="OptitrackStreamingClient.GetLatestTMarkersetState"/>.</summary>
|
||
public Int32 Id;
|
||
|
||
/// <summary>Skeleton asset name.</summary>
|
||
public string Name;
|
||
|
||
/// <summary>Bone names, hierarchy, and neutral pose position information.</summary>
|
||
public List<BoneDefinition> Bones;
|
||
|
||
/// <summary>Bone hierarchy information</summary>
|
||
public Dictionary<Int32, Int32> BoneIdToParentIdMap;
|
||
|
||
public List<MarkerDefinition> Markers;
|
||
}
|
||
|
||
public class OptitrackMarkersDefinition
|
||
{
|
||
/// <summary>The name of this bone.</summary>
|
||
public string Name;
|
||
}
|
||
|
||
public class OptitrackForcePlateDefinition
|
||
{
|
||
/// <summary>The ID of this force plate.</summary>
|
||
public Int32 Id;
|
||
|
||
/// <summary>The serial number of this force plate.</summary>
|
||
public string SerialNumber;
|
||
|
||
/// <summary>The width of the force plate.</summary>
|
||
public float Width;
|
||
|
||
/// <summary>The length of the force plate.</summary>
|
||
public float Length;
|
||
|
||
/// <summary>The electrical offset of this force plate.</summary>
|
||
public Vector3 ElectricalOffset;
|
||
|
||
/// <summary>The calibration matrix of this force plate.</summary>
|
||
public List<float> CalibrationMatrix;
|
||
|
||
/// <summary>The corner locations of this force plate.</summary>
|
||
public List<float> Corners;
|
||
|
||
/// <summary>The force plate type.</summary>
|
||
public Int32 PlateType;
|
||
|
||
/// <summary>The force plate channel data type.</summary>
|
||
public Int32 ChannelDataType;
|
||
|
||
/// <summary>The force plate channel count.</summary>
|
||
public Int32 ChannelCount;
|
||
|
||
/// <summary>The channel names for the force plate.</summary>
|
||
public List<string> ChannelNames;
|
||
}
|
||
|
||
public class OptitrackCameraDefinition
|
||
{
|
||
/// <summary>The name of this camera.</summary>
|
||
public string Name;
|
||
|
||
/// <summary>The name of this camera.</summary>
|
||
public Vector3 Position;
|
||
|
||
/// <summary>The name of this camera.</summary>
|
||
public Quaternion Orientation;
|
||
}
|
||
|
||
/// <summary>Definition of a generic analog device (e.g. iFacialMocap face stream).</summary>
|
||
public class OptitrackDeviceDefinition
|
||
{
|
||
public Int32 Id;
|
||
public string Name;
|
||
public string SerialNumber;
|
||
public Int32 DeviceType;
|
||
public Int32 ChannelDataType;
|
||
public Int32 ChannelCount;
|
||
public List<string> ChannelNames;
|
||
}
|
||
|
||
/// <summary>Latest sampled frame for a generic analog device.</summary>
|
||
public class OptitrackDeviceState
|
||
{
|
||
public Int32 Id;
|
||
public string Name;
|
||
/// <summary>Channel values keyed by channel name (matches ChannelNames in definition).</summary>
|
||
public Dictionary<string, float> ChannelValues;
|
||
public Int32 FrameNumber;
|
||
public OptitrackHiResTimer.Timestamp DeliveryTimestamp;
|
||
}
|
||
|
||
public static class OptitrackHiResTimer
|
||
{
|
||
public struct Timestamp
|
||
{
|
||
internal Int64 m_ticks;
|
||
|
||
public float AgeSeconds
|
||
{
|
||
get
|
||
{
|
||
return Now().SecondsSince( this );
|
||
}
|
||
}
|
||
|
||
public float SecondsSince( Timestamp reference )
|
||
{
|
||
Int64 deltaTicks = m_ticks - reference.m_ticks;
|
||
return deltaTicks / (float)System.Diagnostics.Stopwatch.Frequency;
|
||
}
|
||
}
|
||
|
||
public static Timestamp Now()
|
||
{
|
||
return new Timestamp {
|
||
m_ticks = System.Diagnostics.Stopwatch.GetTimestamp()
|
||
};
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Connects to a NatNet streaming server and makes the data available in lightweight Unity-friendly representations.
|
||
/// </summary>
|
||
public class OptitrackStreamingClient : MonoBehaviour
|
||
{
|
||
private const int k_DefaultNatNetCommandPort = 1510;
|
||
private const int k_DefaultNatNetDataPort = 1511;
|
||
private const int k_MaxNatNetDevices = 32;
|
||
|
||
public enum ClientConnectionType
|
||
{
|
||
Multicast,
|
||
Unicast
|
||
}
|
||
|
||
[Header("Connection Settings")]
|
||
|
||
[Tooltip("The Streaming IP (Local Interface) in Motive")]
|
||
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;
|
||
|
||
[Header("NatNet Endpoint Override")]
|
||
[Tooltip("NatNet command port. Motive/MMRP default = 1510.")]
|
||
[Range(1, 65535)]
|
||
public int CommandPort = k_DefaultNatNetCommandPort;
|
||
|
||
[Tooltip("NatNet data port. Motive/MMRP default = 1511.")]
|
||
[Range(1, 65535)]
|
||
public int DataPort = k_DefaultNatNetDataPort;
|
||
|
||
[Tooltip("Optional multicast group override. Empty uses the NatNet default group (usually 239.255.42.99). Use a separate group for replay, e.g. 239.255.42.100.")]
|
||
public string MulticastAddress = "";
|
||
|
||
[Header("Replay Priority Quick Setup")]
|
||
[Tooltip("Enable Live Motive + MMRP Replay priority switching from this Streaming Client inspector.")]
|
||
public bool EnableReplayPriority = false;
|
||
|
||
[Tooltip("MMRP / recording PC IP address. Replay frames from this IP override live frames while fresh.")]
|
||
public string ReplayServerAddress = "127.0.0.1";
|
||
|
||
[Tooltip("Replay frames are preferred while newer than this many seconds.")]
|
||
[Range(0.05f, 5.0f)]
|
||
public float ReplayFreshnessSeconds = 0.35f;
|
||
|
||
[Tooltip("Actor/skeleton name to use when the NatNet command channel is unavailable and names cannot be read from MODELDEF. Must match OptitrackSkeletonAnimator_Mingle.SkeletonAssetName.")]
|
||
public string DirectSyntheticSkeletonName = "002";
|
||
|
||
[Tooltip("Controls whether skeleton data is streamed with local or global coordinates.")]
|
||
public StreamingCoordinatesValues SkeletonCoordinates = StreamingCoordinatesValues.Local;
|
||
|
||
[Tooltip("Controls whether tmarkerset data is streamed with local or global coordinates.")]
|
||
public StreamingCoordinatesValues TMarkersetCoordinates = StreamingCoordinatesValues.Local;
|
||
|
||
[Tooltip("Controls the Bone Naming Convention in the streamed data.")]
|
||
public OptitrackBoneNameConvention BoneNamingConvention = OptitrackBoneNameConvention.Motive;
|
||
|
||
[Header("Extra Features")]
|
||
|
||
[Tooltip("Draws marker visuals in the viewport for debugging and other uses. Using this will increase the data rate in Unicast mode.")]
|
||
public bool DrawMarkers = false;
|
||
|
||
[Tooltip("Draws trained markerset marker visuals in the viewport for debugging and other uses. Using this will increase the data rate in Unicast mode.")]
|
||
public bool DrawTMarkersetMarkers = false; // trained markerset added
|
||
|
||
[Tooltip("Draws camera visuals in the viewport for debugging and other uses. Motive 3.0+ only.")]
|
||
public bool DrawCameras = false;
|
||
|
||
[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). Leave off if no devices are needed.")]
|
||
public bool ReceiveDevices = false;
|
||
|
||
[Tooltip("Motive will record when the Unity project is played.")]
|
||
public bool RecordOnPlay = false;
|
||
|
||
[Tooltip("Skips getting data descriptions. Skeletons will not work with this feature turned on, but it will reduce network usage with a large number of rigid bodies.")]
|
||
public bool SkipDataDescriptions = false;
|
||
|
||
[Tooltip("Automatically retries the initial connection and reconnects when streaming frames stop.")]
|
||
public bool AutoReconnect = true;
|
||
|
||
[Tooltip("Changes to the version of Natnet used by the server")]
|
||
public string ServerNatNetVersion = "";
|
||
public string ClientNatNetVersion = "";
|
||
|
||
//[Tooltip("Timecode Provider")]
|
||
//public bool TimecodeProvider = false;
|
||
|
||
[Header("Mirror Mode")]
|
||
[Tooltip("Mirrors streamed skeleton bones and retargeted avatar motion left/right.")]
|
||
public bool MirrorMode = false;
|
||
|
||
[Header("Skeleton Frame Filter")]
|
||
[Tooltip("Strict skeleton frame filter.\nON: drops the whole skeleton frame when any bone is untracked or invalid.\nOFF: valid bones update and invalid bones keep the previous pose. Recommended for live use.")]
|
||
public bool EnableSkeletonFrameFilter = false;
|
||
|
||
#region Private fields
|
||
//private UInt16 ServerCommandPort = NatNetConstants.DefaultCommandPort;
|
||
//private UInt16 ServerDataPort = NatNetConstants.DefaultDataPort;
|
||
|
||
private bool m_doneSubscriptionNotice = false;
|
||
private bool m_receivedFrameSinceConnect = false;
|
||
private bool m_hasDrawnCameras = false;
|
||
private bool m_hasDrawnForcePlates = false;
|
||
private bool m_subscribedToMarkers = false;
|
||
private float m_markerSubscribeRetryCooldown = 0f; // 留덉빱 援щ룆 ?ㅽ뙣 ???ъ떆??荑⑤떎??
|
||
private const float k_MarkerSubscribeRetryInterval = 10f; // 10珥?媛꾧꺽 ?ъ떆??
|
||
|
||
private OptitrackHiResTimer.Timestamp m_lastFrameDeliveryTimestamp;
|
||
private Coroutine m_connectionHealthCoroutine = null;
|
||
private Coroutine m_replayConnectCoroutine = null;
|
||
private UdpClient m_directFrameUdp = null;
|
||
private Thread m_directFrameThread = null;
|
||
private volatile bool m_directFrameRunning = false;
|
||
private bool m_directFrameLogged = false;
|
||
private volatile bool m_directFrameLogPending = false;
|
||
private volatile bool m_directTimeoutLogPending = false;
|
||
private volatile bool m_directSocketErrorLogPending = false;
|
||
private string m_directSocketErrorMessage = "";
|
||
private int m_directTimeoutCount = 0;
|
||
private bool m_directNatNetConnected = false;
|
||
private bool m_directNoModelDefWarned = false;
|
||
|
||
private NatNetClient m_client;
|
||
private NatNetClient m_replayClient;
|
||
private bool m_replayReceivedFrameSinceConnect = false;
|
||
private bool m_replayConnectWarned = false;
|
||
private OptitrackHiResTimer.Timestamp m_lastReplayFrameDeliveryTimestamp;
|
||
private NatNetClient.DataDescriptions m_dataDescs;
|
||
private List<OptitrackRigidBodyDefinition> m_rigidBodyDefinitions = new List<OptitrackRigidBodyDefinition>();
|
||
private List<OptitrackSkeletonDefinition> m_skeletonDefinitions = new List<OptitrackSkeletonDefinition>();
|
||
private List<OptitrackTMarkersetDefinition> m_tmarkersetDefinitions = new List<OptitrackTMarkersetDefinition>(); // trained markerset added
|
||
private List<OptitrackMarkersDefinition> m_tmarkmarkersDefinitions = new List<OptitrackMarkersDefinition>(); // trained markerset added
|
||
private List<OptitrackMarkersDefinition> m_markersDefinitions = new List<OptitrackMarkersDefinition>();
|
||
private List<OptitrackCameraDefinition> m_cameraDefinitions = new List<OptitrackCameraDefinition>();
|
||
private List<OptitrackForcePlateDefinition> m_forcePlateDefinitions = new List<OptitrackForcePlateDefinition>();
|
||
private List<OptitrackDeviceDefinition> m_deviceDefinitions = new List<OptitrackDeviceDefinition>();
|
||
|
||
/// <summary>Maps from a streamed device's ID to its most recent channel values.</summary>
|
||
private Dictionary<Int32, OptitrackDeviceState> m_latestDeviceStates = new Dictionary<Int32, OptitrackDeviceState>();
|
||
/// <summary>Name -> Id lookup for devices (built from descriptions).</summary>
|
||
private Dictionary<string, Int32> m_deviceNameToId = new Dictionary<string, Int32>();
|
||
|
||
/// <summary>Maps from a streamed rigid body's ID to its most recent available pose data.</summary>
|
||
private Dictionary<Int32, OptitrackRigidBodyState> m_latestRigidBodyStates = new Dictionary<Int32, OptitrackRigidBodyState>();
|
||
|
||
/// <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>>();
|
||
|
||
/// <summary>Maps from a streamed trained markerset's ID to its most recent available pose data.</summary>
|
||
private Dictionary<Int32, OptitrackTMarkersetState> m_latestTMarkersetStates = new Dictionary<Int32, OptitrackTMarkersetState>(); // trained markerset added
|
||
|
||
/// <summary>Maps from a streamed marker's ID to its most recent available position.</summary>
|
||
private Dictionary<Int32, OptitrackMarkerState> m_latestMarkerStates = new Dictionary<Int32, OptitrackMarkerState>();
|
||
|
||
/// <summary>Maps from a streamed trained markerset marker's ID to its most recent available position.</summary>
|
||
private Dictionary<Int32, OptitrackMarkerState> m_latestTMarkMarkerStates = new Dictionary<Int32, OptitrackMarkerState>(); // trained markerset added
|
||
|
||
/// <summary>Maps from a streamed rigid body's ID to its component.</summary>
|
||
private Dictionary<Int32, MonoBehaviour> m_rigidBodies = new Dictionary<Int32, MonoBehaviour>();
|
||
|
||
/// <summary>Maps from a streamed skeleton names to its component.</summary>
|
||
private Dictionary<string, MonoBehaviour> m_skeletons = new Dictionary<string, MonoBehaviour>();
|
||
|
||
/// <summary>Maps from a streamed trained markerset names to its component.</summary>
|
||
private Dictionary<string, MonoBehaviour> m_tmarkersets = new Dictionary<string, MonoBehaviour>(); // trained markerset added
|
||
|
||
/// <summary>Maps from a streamed marker's ID to its sphere game object. Used for drawing markers.</summary>
|
||
private Dictionary<Int32, GameObject> m_latestMarkerSpheres = new Dictionary<Int32, GameObject>();
|
||
|
||
/// <summary>Maps from a streamed trained markerset marker's ID to its sphere game object. Used for drawing markers.</summary>
|
||
private Dictionary<Int32, GameObject> m_latestTMarkMarkerSpheres = new Dictionary<Int32, GameObject>(); // trained markerset added
|
||
|
||
/// <summary>
|
||
/// Lock held during access to fields which are potentially modified by <see cref="OnNatNetFrameReceived"/> (which
|
||
/// executes on a separate thread). Note while the lock is held, any frame updates received are simply dropped.
|
||
/// </summary>
|
||
private object m_frameDataUpdateLock = new object();
|
||
|
||
/// <summary>assetID ??assetName 罹먯떆 (GetMarkerName ??3以??좏삎 ?먯깋 ?쒓굅??.</summary>
|
||
private Dictionary<Int32, string> m_assetIdToNameCache = new Dictionary<Int32, string>();
|
||
|
||
// 以묎컙?????ㅼ펷?덊넠???앹꽦??寃쎌슦 ?뺤쓽 ?ъ“???뚮옒洹?(NatNet ?ㅻ젅?쒖뿉?쒕룄 ? ??volatile)
|
||
private volatile bool m_pendingDefinitionRefresh = false;
|
||
private float m_definitionRefreshCooldown = 0f;
|
||
|
||
// ?먮룞 ?ъ뿰寃?吏꾪뻾 以??щ? (以묐났 ?ъ뿰寃?諛⑹?)
|
||
private bool m_isReconnecting = false;
|
||
|
||
// ?뱁솕 ?곹깭 異붿쟻 (ToggleRecording?먯꽌 遺덊븘?뷀븳 ?댁쨷 紐낅졊 諛⑹?)
|
||
private bool m_isRecording = false;
|
||
|
||
// SetProperty ?먭꺽 紐낅졊??理쒖큹 ?곌껐?먯꽌留??꾩넚 (?ъ뿰寃???Motive ?ㅼ젙 諛섎났 蹂寃?諛⑹?)
|
||
private bool m_hasAppliedServerSettings = false;
|
||
|
||
// ?뺤쓽 ?ъ“???잛닔 ?쒗븳 (Motive 遺??諛⑹?: 理쒕? ?잛닔 珥덇낵 ???섎룞 ?몃━嫄??꾩슂)
|
||
private int m_definitionRefreshCount = 0;
|
||
private const int k_MaxAutoDefinitionRefreshes = 10;
|
||
#endregion Private fields
|
||
|
||
/// <summary>
|
||
/// ?몃??먯꽌 ?뺤쓽 ?ъ“?뚮? ?붿껌?⑸땲?? 荑⑤떎??5珥? ?댁뿉??臾댁떆?⑸땲??
|
||
/// OptitrackRigidBody, OptitrackSkeletonAnimator_Mingle ?깆뿉???먯뀑??李얠? 紐삵븷 ???몄텧.
|
||
/// </summary>
|
||
public void RequestDefinitionRefresh()
|
||
{
|
||
m_pendingDefinitionRefresh = true;
|
||
}
|
||
|
||
private void Update()
|
||
{
|
||
if (m_directFrameLogPending)
|
||
{
|
||
m_directFrameLogPending = false;
|
||
Debug.Log(GetType().FullName + ": receiving NatNet frames through direct UDP fallback.", this);
|
||
}
|
||
if (m_directTimeoutLogPending)
|
||
{
|
||
m_directTimeoutLogPending = false;
|
||
Debug.LogWarning(GetType().FullName + ": direct NatNet receiver is joined but has not received multicast packets yet. If Python receives frames on this PC, check Unity Editor firewall permission or disable the separate NatNetDeviceListener objects for this test.", this);
|
||
}
|
||
if (m_directSocketErrorLogPending)
|
||
{
|
||
m_directSocketErrorLogPending = false;
|
||
Debug.LogWarning(GetType().FullName + ": direct NatNet receiver socket warning: " + m_directSocketErrorMessage, this);
|
||
}
|
||
|
||
if (EnableReplayPriority && m_replayConnectCoroutine == null && ConnectionType != ClientConnectionType.Multicast)
|
||
m_replayConnectCoroutine = StartCoroutine(ReplayConnectLoop());
|
||
else if (!EnableReplayPriority && m_replayConnectCoroutine != null)
|
||
{
|
||
StopCoroutine(m_replayConnectCoroutine);
|
||
m_replayConnectCoroutine = null;
|
||
CleanupReplayClient();
|
||
}
|
||
|
||
if (DrawMarkers)
|
||
{
|
||
// 留덉빱 援щ룆 ?ㅽ뙣 ??留??꾨젅???ъ떆??諛⑹? ??荑⑤떎???곸슜
|
||
if (m_client != null && ConnectionType == ClientConnectionType.Unicast && !m_subscribedToMarkers)
|
||
{
|
||
if (m_markerSubscribeRetryCooldown <= 0f)
|
||
{
|
||
SubscribeMarkers();
|
||
if (!m_subscribedToMarkers)
|
||
m_markerSubscribeRetryCooldown = k_MarkerSubscribeRetryInterval;
|
||
}
|
||
else
|
||
{
|
||
m_markerSubscribeRetryCooldown -= Time.deltaTime;
|
||
}
|
||
}
|
||
|
||
// ??踰붿쐞 理쒖냼?? ?곗씠?곕쭔 蹂듭궗 ?????댁젣 ??GameObject ?앹꽦/?뚭눼
|
||
var markerSnapshot = new Dictionary<Int32, (Vector3 pos, float size, string name, bool isActive)>();
|
||
lock (m_frameDataUpdateLock)
|
||
{
|
||
foreach (var markerEntry in m_latestMarkerStates)
|
||
{
|
||
markerSnapshot[markerEntry.Key] = (
|
||
markerEntry.Value.Position,
|
||
markerEntry.Value.Size,
|
||
markerEntry.Value.Name,
|
||
markerEntry.Value.IsActive
|
||
);
|
||
}
|
||
}
|
||
|
||
// ??諛뽰뿉??GameObject ?낅뜲?댄듃/?앹꽦
|
||
var activeIds = new HashSet<Int32>(markerSnapshot.Count);
|
||
foreach (var kvp in markerSnapshot)
|
||
{
|
||
activeIds.Add(kvp.Key);
|
||
if (m_latestMarkerSpheres.ContainsKey(kvp.Key))
|
||
{
|
||
m_latestMarkerSpheres[kvp.Key].transform.position = kvp.Value.pos;
|
||
}
|
||
else
|
||
{
|
||
var sphere = GameObject.CreatePrimitive(PrimitiveType.Cube);
|
||
sphere.transform.parent = this.transform;
|
||
sphere.transform.localScale = new Vector3(kvp.Value.size, kvp.Value.size, kvp.Value.size);
|
||
sphere.transform.position = kvp.Value.pos;
|
||
sphere.name = kvp.Value.name;
|
||
if (kvp.Value.isActive)
|
||
sphere.GetComponent<Renderer>().material.SetColor("_Color", Color.cyan);
|
||
m_latestMarkerSpheres[kvp.Key] = sphere;
|
||
}
|
||
}
|
||
// ??諛뽰뿉??stale ?ㅻ툕?앺듃 ?쒓굅
|
||
var staleIds = new List<Int32>();
|
||
foreach (var sphereEntry in m_latestMarkerSpheres)
|
||
{
|
||
if (!activeIds.Contains(sphereEntry.Key))
|
||
staleIds.Add(sphereEntry.Key);
|
||
}
|
||
foreach (var id in staleIds)
|
||
{
|
||
Destroy(m_latestMarkerSpheres[id]);
|
||
m_latestMarkerSpheres.Remove(id);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// not drawing markers, remove all marker spheres
|
||
foreach (KeyValuePair<Int32, GameObject> markerSphereEntry in m_latestMarkerSpheres)
|
||
{
|
||
Destroy( m_latestMarkerSpheres[markerSphereEntry.Key] );
|
||
}
|
||
m_latestMarkerSpheres.Clear();
|
||
}
|
||
|
||
// ??諛뽰뿉??移대찓??吏?ㅻ찓?몃━ ?앹꽦 (1?뚯꽦 ???곗씠?곕뒗 m_cameraDefinitions???대? 蹂듭궗??
|
||
if (DrawCameras && !m_hasDrawnCameras )
|
||
{
|
||
if (m_client.ServerAppVersion >= new Version(3, 0, 0))
|
||
{
|
||
// m_cameraDefinitions??硫붿씤 ?ㅻ젅?쒖쓽 UpdateDefinitions()?먯꽌 梨꾩썙吏?????遺덊븘??
|
||
var cameraGroup = new GameObject("Cameras");
|
||
foreach (OptitrackCameraDefinition camera in m_cameraDefinitions)
|
||
{
|
||
var geometry = GameObject.CreatePrimitive(PrimitiveType.Cube);
|
||
geometry.transform.parent = cameraGroup.transform;
|
||
geometry.transform.localScale = new Vector3(0.1f, 0.1f, 0.1f);
|
||
geometry.transform.position = camera.Position;
|
||
geometry.transform.rotation = camera.Orientation;
|
||
geometry.name = camera.Name;
|
||
geometry.GetComponent<Renderer>().material.SetColor("_Color", Color.black);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
Debug.LogWarning("Drawing cameras is only supported in Motive 3.0+.");
|
||
}
|
||
|
||
m_hasDrawnCameras = true;
|
||
}
|
||
|
||
// ??諛뽰뿉???ъ뒪 ?뚮젅?댄듃 吏?ㅻ찓?몃━ ?앹꽦 (1?뚯꽦)
|
||
if (DrawForcePlates && !m_hasDrawnForcePlates)
|
||
{
|
||
var forcePlateGroup = new GameObject("Force Plates");
|
||
foreach (OptitrackForcePlateDefinition plate in m_forcePlateDefinitions)
|
||
{
|
||
Vector3 p0 = new Vector3(-plate.Corners[0], plate.Corners[1], plate.Corners[2]);
|
||
Vector3 p1 = new Vector3(-plate.Corners[3], plate.Corners[4], plate.Corners[5]);
|
||
Vector3 p2 = new Vector3(-plate.Corners[6], plate.Corners[7], plate.Corners[8]);
|
||
Vector3 p3 = new Vector3(-plate.Corners[9], plate.Corners[10], plate.Corners[11]);
|
||
Vector3 pAverage = (p0 + p1 + p2 + p3) / 4;
|
||
|
||
var geometry = GameObject.CreatePrimitive(PrimitiveType.Cube);
|
||
geometry.transform.parent = forcePlateGroup.transform;
|
||
geometry.transform.localScale = new Vector3(plate.Length * 0.0254f, 0.03f, plate.Width * 0.0254f);
|
||
geometry.transform.position = pAverage;
|
||
geometry.transform.rotation = Quaternion.LookRotation(p2 - p1);
|
||
geometry.name = plate.SerialNumber;
|
||
geometry.GetComponent<Renderer>().material.SetColor("_Color", Color.blue);
|
||
}
|
||
|
||
m_hasDrawnForcePlates = true;
|
||
}
|
||
|
||
//if (TimecodeProvider)
|
||
//{
|
||
// Debug.Log("");
|
||
//}
|
||
|
||
// Trained Markerset Markers if requested to draw // trained markerset added
|
||
if (DrawTMarkersetMarkers)
|
||
{
|
||
// ??踰붿쐞 理쒖냼?? ?곗씠?곕쭔 蹂듭궗 ?????댁젣 ??GameObject ?앹꽦/?뚭눼
|
||
var tmarkSnapshot = new Dictionary<Int32, (Vector3 pos, float size, string name, bool isActive)>();
|
||
lock (m_frameDataUpdateLock)
|
||
{
|
||
foreach (var markerEntry in m_latestTMarkMarkerStates)
|
||
{
|
||
tmarkSnapshot[markerEntry.Key] = (
|
||
markerEntry.Value.Position,
|
||
markerEntry.Value.Size,
|
||
markerEntry.Value.Name,
|
||
markerEntry.Value.IsActive
|
||
);
|
||
}
|
||
}
|
||
|
||
// ??諛뽰뿉??GameObject ?낅뜲?댄듃/?앹꽦
|
||
var activeTMarkIds = new HashSet<Int32>(tmarkSnapshot.Count);
|
||
foreach (var kvp in tmarkSnapshot)
|
||
{
|
||
activeTMarkIds.Add(kvp.Key);
|
||
if (m_latestTMarkMarkerSpheres.ContainsKey(kvp.Key))
|
||
{
|
||
m_latestTMarkMarkerSpheres[kvp.Key].transform.position = kvp.Value.pos;
|
||
}
|
||
else
|
||
{
|
||
var cube = GameObject.CreatePrimitive(PrimitiveType.Cube);
|
||
cube.transform.parent = this.transform;
|
||
cube.transform.localScale = new Vector3(kvp.Value.size, kvp.Value.size, kvp.Value.size);
|
||
cube.transform.position = kvp.Value.pos;
|
||
cube.name = kvp.Value.name;
|
||
if (kvp.Value.isActive)
|
||
cube.GetComponent<Renderer>().material.SetColor("_Color", Color.cyan);
|
||
m_latestTMarkMarkerSpheres[kvp.Key] = cube;
|
||
}
|
||
}
|
||
// ??諛뽰뿉??stale ?ㅻ툕?앺듃 ?쒓굅
|
||
var staleTMarkIds = new List<Int32>();
|
||
foreach (var cubeEntry in m_latestTMarkMarkerSpheres)
|
||
{
|
||
if (!activeTMarkIds.Contains(cubeEntry.Key))
|
||
staleTMarkIds.Add(cubeEntry.Key);
|
||
}
|
||
foreach (var id in staleTMarkIds)
|
||
{
|
||
Destroy(m_latestTMarkMarkerSpheres[id]);
|
||
m_latestTMarkMarkerSpheres.Remove(id);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// not drawing markers, remove all marker spheres
|
||
foreach (KeyValuePair<Int32, GameObject> markerCubeEntry in m_latestTMarkMarkerSpheres)
|
||
{
|
||
Destroy(m_latestTMarkMarkerSpheres[markerCubeEntry.Key]);
|
||
}
|
||
m_latestTMarkMarkerSpheres.Clear();
|
||
}
|
||
|
||
// 誘몃벑濡??ㅼ펷?덊넠???꾨젅?꾩뿉 ?깆옣?덉쓣 ???뺤쓽 ?먮룞 ?ъ“??(Motive 以묎컙 ?앹꽦 ???
|
||
// 荑⑤떎??15珥?+ 理쒕? ?잛닔 ?쒗븳?쇰줈 Motive DataDescription ?붿껌 ??뭾 諛⑹?
|
||
if (m_pendingDefinitionRefresh && m_definitionRefreshCooldown <= 0f && m_client != null && !SkipDataDescriptions)
|
||
{
|
||
m_pendingDefinitionRefresh = false;
|
||
|
||
if (m_definitionRefreshCount < k_MaxAutoDefinitionRefreshes)
|
||
{
|
||
m_definitionRefreshCooldown = 15f;
|
||
m_definitionRefreshCount++;
|
||
try { UpdateDefinitions(); }
|
||
catch (Exception ex) { Debug.LogException(ex, this); }
|
||
}
|
||
else
|
||
{
|
||
// 理쒕? ?잛닔 珥덇낵 ??寃쎄퀬 濡쒓렇 異쒕젰 ???먮룞 ?ъ“??以묐떒
|
||
m_definitionRefreshCooldown = 60f; // 1遺????ㅼ떆 ?쒕룄 ?덉슜
|
||
Debug.LogWarning(GetType().FullName + ": Automatic definition refresh limit reached (" + k_MaxAutoDefinitionRefreshes + "). Check the streamed assets in Motive.", this);
|
||
}
|
||
}
|
||
if (m_definitionRefreshCooldown > 0f)
|
||
m_definitionRefreshCooldown -= Time.deltaTime;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Returns the first <see cref="OptitrackStreamingClient"/> component located in the scene.
|
||
/// Provides a convenient, sensible default in the common case where only a single client exists.
|
||
/// Issues a warning if more than one such component is found.
|
||
/// </summary>
|
||
/// <returns>An arbitrary OptitrackClient from the scene, or null if none are found.</returns>
|
||
public static OptitrackStreamingClient FindDefaultClient()
|
||
{
|
||
OptitrackStreamingClient[] allClients = FindObjectsByType<OptitrackStreamingClient>(FindObjectsSortMode.None);
|
||
|
||
if ( allClients.Length == 0 )
|
||
{
|
||
Debug.LogError( "Unable to locate any " + typeof( OptitrackStreamingClient ).FullName + " components." );
|
||
return null;
|
||
}
|
||
else if ( allClients.Length > 1 )
|
||
{
|
||
Debug.LogWarning( "Multiple " + typeof( OptitrackStreamingClient ).FullName + " components found in scene; defaulting to first available." );
|
||
}
|
||
|
||
return allClients[0];
|
||
}
|
||
|
||
/// <summary>
|
||
/// Sends a message to Motive to start recording
|
||
/// </summary>
|
||
/// <returns>A boolean indicating if message was successful.</returns>
|
||
public bool StartRecording()
|
||
{
|
||
if(m_client != null)
|
||
{
|
||
bool result = m_client.RequestCommand("StartRecording");
|
||
if (result) m_isRecording = true;
|
||
return result;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Sends a message to Motive to stop recording
|
||
/// </summary>
|
||
/// <returns>A boolean indicating if message was successful.</returns>
|
||
public bool StopRecording()
|
||
{
|
||
if (m_client != null)
|
||
{
|
||
bool result = m_client.RequestCommand("StopRecording");
|
||
if (result) m_isRecording = false;
|
||
return result;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Reconnects to the OptiTrack streaming server by disconnecting and reconnecting.
|
||
/// </summary>
|
||
public void Reconnect()
|
||
{
|
||
if (m_isReconnecting)
|
||
{
|
||
Debug.LogWarning(GetType().FullName + ": Reconnect is already in progress.", this);
|
||
return;
|
||
}
|
||
|
||
Debug.Log("OptiTrack: reconnect requested.");
|
||
|
||
// 湲곗〈 ?곌껐 ?뺣━ (StopAllCoroutines ?ы븿)
|
||
OnDisable();
|
||
|
||
StartCoroutine(ReconnectCoroutine());
|
||
}
|
||
|
||
/// <summary>
|
||
/// Coroutine for handling the reconnection process with a retry loop.
|
||
/// Attempts up to 5 times: 1s delay on first attempt, 5s on subsequent.
|
||
/// Last known pose is preserved during reconnect (m_latestSkeletonStates not cleared).
|
||
/// </summary>
|
||
private System.Collections.IEnumerator ReconnectCoroutine()
|
||
{
|
||
m_isReconnecting = true;
|
||
const int maxAttempts = 5;
|
||
|
||
for (int attempt = 1; attempt <= maxAttempts; attempt++)
|
||
{
|
||
m_receivedFrameSinceConnect = false;
|
||
float delay = (attempt == 1) ? 1.0f : 5.0f;
|
||
Debug.Log(string.Format("{0}: reconnect attempt {1}/{2} in {3} seconds...", GetType().FullName, attempt, maxAttempts, delay), this);
|
||
yield return new WaitForSeconds(delay);
|
||
|
||
// ConnectCoroutine ?ㅽ뻾: ?곌껐 ?섎┰ + CheckConnectionHealth ?쒖옉
|
||
yield return StartCoroutine(ConnectCoroutine());
|
||
|
||
// 泥??꾨젅???섏떊源뚯? 理쒕? 3珥??湲?
|
||
float deadline = Time.realtimeSinceStartup + 3.0f;
|
||
while (!m_receivedFrameSinceConnect && Time.realtimeSinceStartup < deadline)
|
||
yield return null;
|
||
|
||
if (m_receivedFrameSinceConnect)
|
||
{
|
||
Debug.Log(GetType().FullName + ": reconnect succeeded.", this);
|
||
// ?ъ뿰寃??깃났 ?꾩뿉留??뱁솕 ?ъ떆??
|
||
if (RecordOnPlay)
|
||
StartRecording();
|
||
m_isReconnecting = false;
|
||
yield break;
|
||
}
|
||
|
||
Debug.LogWarning(string.Format("{0}: reconnect attempt {1}/{2} failed; no streaming frames received.", GetType().FullName, attempt, maxAttempts), this);
|
||
|
||
// ?ㅼ쓬 ?쒕룄 ??遺遺??곌껐 ?뺣━ (ReconnectCoroutine ?먯떊? 以묒??섏? ?딅룄濡?StopCoroutine ?ъ슜)
|
||
if (m_connectionHealthCoroutine != null)
|
||
{
|
||
StopCoroutine(m_connectionHealthCoroutine);
|
||
m_connectionHealthCoroutine = null;
|
||
}
|
||
if (m_client != null || m_directNatNetConnected)
|
||
CleanupClient();
|
||
}
|
||
|
||
Debug.LogError(string.Format("{0}: all {1} reconnect attempts failed. Reconnect manually from the Inspector.", GetType().FullName, maxAttempts), this);
|
||
m_isReconnecting = false;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Gets the current connection status of the client.
|
||
/// </summary>
|
||
/// <returns>True if connected and receiving data, false otherwise.</returns>
|
||
public bool IsConnected()
|
||
{
|
||
return ((m_client != null || m_directNatNetConnected) && m_receivedFrameSinceConnect) || IsReplayFrameFresh();
|
||
}
|
||
|
||
/// <summary>
|
||
/// Gets the connection status as a readable string.
|
||
/// </summary>
|
||
/// <returns>Connection status description.</returns>
|
||
public string GetConnectionStatus()
|
||
{
|
||
if (IsReplayFrameFresh())
|
||
{
|
||
return "Replay active (MMRP)";
|
||
}
|
||
|
||
if (m_client == null && !m_directNatNetConnected)
|
||
{
|
||
return "Disconnected";
|
||
}
|
||
else if (!m_receivedFrameSinceConnect)
|
||
{
|
||
return "Connected, waiting for data";
|
||
}
|
||
else
|
||
{
|
||
float lastFrameAge = m_lastFrameDeliveryTimestamp.AgeSeconds;
|
||
if (lastFrameAge < 5.0f)
|
||
{
|
||
return "Connected, receiving data";
|
||
}
|
||
else
|
||
{
|
||
return "Connected, data interrupted";
|
||
}
|
||
}
|
||
}
|
||
|
||
#region Recording Control Functions
|
||
/// <summary>
|
||
/// Starts recording with enhanced logging.
|
||
/// </summary>
|
||
public void StartRecordingWithLog()
|
||
{
|
||
bool result = StartRecording();
|
||
if (result)
|
||
{
|
||
Debug.Log("OptiTrack: recording started.");
|
||
}
|
||
else
|
||
{
|
||
Debug.LogWarning("OptiTrack: failed to start recording.");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Stops recording with enhanced logging.
|
||
/// </summary>
|
||
public void StopRecordingWithLog()
|
||
{
|
||
bool result = StopRecording();
|
||
if (result)
|
||
{
|
||
Debug.Log("OptiTrack: recording stopped.");
|
||
}
|
||
else
|
||
{
|
||
Debug.LogWarning("OptiTrack: failed to stop recording.");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Toggles recording state on this client.
|
||
/// </summary>
|
||
public void ToggleRecording()
|
||
{
|
||
if (m_client != null)
|
||
{
|
||
// ?곹깭 異붿쟻 湲곕컲 ?좉? ??湲곗〈: "?쇰떒 Start ???ㅽ뙣?섎㈃ Stop" ?댁쨷 紐낅졊 ??1??紐낅졊?쇰줈 媛쒖꽑
|
||
if (m_isRecording)
|
||
{
|
||
if (StopRecording())
|
||
Debug.Log("OptiTrack: recording stopped.");
|
||
else
|
||
Debug.LogWarning("OptiTrack: failed to stop recording.");
|
||
}
|
||
else
|
||
{
|
||
if (StartRecording())
|
||
Debug.Log("OptiTrack: recording started.");
|
||
else
|
||
Debug.LogWarning("OptiTrack: failed to start recording.");
|
||
}
|
||
}
|
||
else
|
||
{
|
||
Debug.LogError("OptiTrack: client is not connected.");
|
||
}
|
||
}
|
||
|
||
public void ToggleDrawMarkers()
|
||
{
|
||
DrawMarkers = !DrawMarkers;
|
||
Debug.Log($"OptiTrack: DrawMarkers = {DrawMarkers}");
|
||
}
|
||
#endregion
|
||
|
||
/// <summary>Get the most recently received state for the specified rigid body.</summary>
|
||
/// <param name="rigidBodyId">Corresponds to the "User ID" field in Motive.</param>
|
||
/// <returns>The most recent available state, or null if none available.</returns>
|
||
public OptitrackRigidBodyState GetLatestRigidBodyState( Int32 rigidBodyId, bool networkCompensation = true )
|
||
{
|
||
OptitrackRigidBodyState rbState;
|
||
|
||
if ( ! networkCompensation || m_client == null )
|
||
{
|
||
lock ( m_frameDataUpdateLock )
|
||
{
|
||
m_latestRigidBodyStates.TryGetValue( rigidBodyId, out rbState );
|
||
}
|
||
}
|
||
else
|
||
{
|
||
sRigidBodyData rbData;
|
||
m_client.GetPredictedRigidBodyPose( rigidBodyId, out rbData, 0.0 );
|
||
|
||
rbState = new OptitrackRigidBodyState();
|
||
RigidBodyDataToState( rbData, OptitrackHiResTimer.Now(), rbState );
|
||
}
|
||
|
||
if ( MirrorMode && rbState != null )
|
||
{
|
||
rbState = new OptitrackRigidBodyState
|
||
{
|
||
DeliveryTimestamp = rbState.DeliveryTimestamp,
|
||
IsTracked = rbState.IsTracked,
|
||
Pose = new OptitrackPose
|
||
{
|
||
Position = MirrorPosition( rbState.Pose.Position ),
|
||
Orientation = MirrorOrientation( rbState.Pose.Orientation ),
|
||
}
|
||
};
|
||
}
|
||
|
||
return rbState;
|
||
}
|
||
|
||
/// <summary>Get the most recently received state for the specified skeleton.</summary>
|
||
/// <param name="skeletonId">
|
||
/// Taken from the corresponding <see cref="OptitrackSkeletonDefinition.Id"/> field.
|
||
/// To find the appropriate skeleton definition, use <see cref="GetSkeletonDefinitionByName"/>.
|
||
/// </param>
|
||
/// <returns>The most recent available state, or null if none available.</returns>
|
||
public OptitrackSkeletonState GetLatestSkeletonState( Int32 skeletonId )
|
||
{
|
||
OptitrackSkeletonState skelState;
|
||
|
||
lock ( m_frameDataUpdateLock )
|
||
{
|
||
m_latestSkeletonStates.TryGetValue( skeletonId, out skelState );
|
||
}
|
||
|
||
return skelState;
|
||
}
|
||
|
||
/// <summary>
|
||
/// ?쎌쓣 ?좎???梨꾨줈 ?ㅼ펷?덊넠 ?ъ쫰瑜?pre-allocated ?뺤뀛?덈━??蹂듭궗.
|
||
/// GetLatestSkeletonState() ?????놁씠 BonePoses瑜??쒗쉶?섎뒗 torn read瑜?諛⑹?.
|
||
/// </summary>
|
||
/// <returns>?ㅼ펷?덊넠 ?곗씠?곌? 議댁옱?섎㈃ true, ?놁쑝硫?false.</returns>
|
||
public bool FillBoneSnapshot( Int32 skeletonId,
|
||
Dictionary<Int32, Vector3> posOut,
|
||
Dictionary<Int32, Quaternion> oriOut,
|
||
out OptitrackHiResTimer.Timestamp deliveryTimestamp )
|
||
{
|
||
deliveryTimestamp = default( OptitrackHiResTimer.Timestamp );
|
||
lock ( m_frameDataUpdateLock )
|
||
{
|
||
OptitrackSkeletonState state;
|
||
if ( !m_latestSkeletonStates.TryGetValue( skeletonId, out state ) || state == null )
|
||
return false;
|
||
|
||
deliveryTimestamp = state.DeliveryTimestamp;
|
||
posOut.Clear();
|
||
oriOut.Clear();
|
||
|
||
if ( MirrorMode )
|
||
{
|
||
Dictionary<Int32, Int32> mirrorMap = GetOrBuildMirrorBoneIdMap( skeletonId );
|
||
foreach ( var kvp in state.BonePoses )
|
||
{
|
||
Int32 targetId = mirrorMap != null && mirrorMap.TryGetValue( kvp.Key, out Int32 mid ) ? mid : kvp.Key;
|
||
|
||
if ( targetId == kvp.Key )
|
||
{
|
||
// ?移?蹂?(Hip, 泥숈텛, 紐? 癒몃━ ??: YZ ?됰㈃ 諛섏궗 ?곸슜
|
||
posOut[kvp.Key] = MirrorPosition( kvp.Value.Position );
|
||
oriOut[kvp.Key] = MirrorOrientation( kvp.Value.Orientation );
|
||
}
|
||
else
|
||
{
|
||
// L/R ??蹂? ?꾩튂???먭린 ?먮━ ?좎?, ?뚯쟾? YZ 諛섏궗 ??誘몃윭 蹂몄쑝濡??ㅼ솑
|
||
posOut[kvp.Key] = kvp.Value.Position;
|
||
oriOut[targetId] = MirrorOrientation( kvp.Value.Orientation );
|
||
}
|
||
}
|
||
}
|
||
else
|
||
{
|
||
foreach ( var kvp in state.BonePoses )
|
||
{
|
||
posOut[kvp.Key] = kvp.Value.Position;
|
||
oriOut[kvp.Key] = kvp.Value.Orientation;
|
||
}
|
||
}
|
||
return true;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 蹂??대쫫??L/R ?묐몢?щ? 諛섏쟾???대쫫??諛섑솚. ?移?蹂몄씠硫?null.
|
||
/// ?? "LUArm" ??"RUArm", "RShin" ??"LShin", "Hip" ??null
|
||
/// </summary>
|
||
private static string GetMirrorBoneName( string name )
|
||
{
|
||
if ( string.IsNullOrEmpty( name ) || name.Length < 2 ) return null;
|
||
if ( name[0] == 'L' && char.IsUpper( name[1] ) ) return "R" + name.Substring( 1 );
|
||
if ( name[0] == 'R' && char.IsUpper( name[1] ) ) return "L" + name.Substring( 1 );
|
||
return null;
|
||
}
|
||
|
||
/// <summary>
|
||
/// ?ㅼ펷?덊넠 ?뺤쓽瑜?湲곕컲?쇰줈 boneId ??mirrorBoneId 留ㅽ븨??鍮뚮뱶?섍퀬 罹먯떆?????
|
||
/// m_frameDataUpdateLock ?댁뿉???몄텧?댁빞 ??
|
||
/// </summary>
|
||
private Dictionary<Int32, Int32> GetOrBuildMirrorBoneIdMap( Int32 skeletonId )
|
||
{
|
||
if ( m_mirrorBoneIdMaps.TryGetValue( skeletonId, out var cached ) )
|
||
return cached;
|
||
|
||
OptitrackSkeletonDefinition skelDef = GetSkeletonDefinitionById( skeletonId );
|
||
if ( skelDef == null ) return null;
|
||
|
||
// ?대쫫 ??ID 猷⑹뾽 ?뚯씠釉?
|
||
var nameToId = new Dictionary<string, Int32>( skelDef.Bones.Count );
|
||
foreach ( var bone in skelDef.Bones )
|
||
nameToId[bone.Name] = bone.Id;
|
||
|
||
var map = new Dictionary<Int32, Int32>( skelDef.Bones.Count );
|
||
foreach ( var bone in skelDef.Bones )
|
||
{
|
||
// "SkeletonName_BoneName" ?뺤떇 吏?? "_" ?ㅼ쓽 吏㏃? ?대쫫?먯꽌 L/R ?묐몢??泥섎━
|
||
string fullName = bone.Name;
|
||
int sep = fullName.IndexOf( '_' );
|
||
string prefix = sep >= 0 ? fullName.Substring( 0, sep + 1 ) : ""; // "Skeleton1_"
|
||
string shortName = sep >= 0 ? fullName.Substring( sep + 1 ) : fullName; // "LUArm"
|
||
|
||
string mirrorShort = GetMirrorBoneName( shortName );
|
||
string mirrorFull = mirrorShort != null ? prefix + mirrorShort : null;
|
||
|
||
if ( mirrorFull != null && nameToId.TryGetValue( mirrorFull, out Int32 mirrorId ) )
|
||
map[bone.Id] = mirrorId;
|
||
else
|
||
map[bone.Id] = bone.Id; // ?移?蹂몄? ?먭린 ?먯떊??留ㅽ븨
|
||
}
|
||
|
||
m_mirrorBoneIdMaps[skeletonId] = map;
|
||
return map;
|
||
}
|
||
|
||
/// <summary>YZ ?됰㈃ 湲곗? ?뚯쟾 誘몃윭 (R' = M쨌R쨌M, M=diag(-1,1,1)): Y, Z ?깅텇 遺??諛섏쟾.</summary>
|
||
private static Quaternion MirrorOrientation( Quaternion q )
|
||
=> new Quaternion( q.x, -q.y, -q.z, q.w );
|
||
|
||
/// <summary>YZ ?됰㈃ 湲곗? ?꾩튂 誘몃윭: X ?깅텇 遺??諛섏쟾.</summary>
|
||
private static Vector3 MirrorPosition( Vector3 pos )
|
||
=> new Vector3( -pos.x, pos.y, pos.z );
|
||
|
||
/// <summary>Get the most recently received state for the specified trained markerset.</summary>
|
||
/// <param name="tmarkersetId">
|
||
/// Taken from the corresponding <see cref="OptitrackTMarkersetDefinition.Id"/> field.
|
||
/// To find the appropriate skeleton definition, use <see cref="GetTMarkersetDefinitionByName"/>.
|
||
/// </param>
|
||
/// <returns>The most recent available state, or null if none available.</returns>
|
||
public OptitrackTMarkersetState GetLatestTMarkersetState(Int32 tmarkersetId)
|
||
{
|
||
lock (m_frameDataUpdateLock)
|
||
{
|
||
if (!m_latestTMarkersetStates.TryGetValue(tmarkersetId, out var source) || source == null)
|
||
return null;
|
||
|
||
var snapshot = new OptitrackTMarkersetState
|
||
{
|
||
BonePoses = CopyPoseDictionary(source.BonePoses, MirrorMode),
|
||
LocalBonePoses = CopyPoseDictionary(source.LocalBonePoses, MirrorMode),
|
||
};
|
||
return snapshot;
|
||
}
|
||
}
|
||
|
||
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;
|
||
}
|
||
}
|
||
|
||
/// <summary>Get the most recently received state for streamed markers.</summary>
|
||
/// <returns>The most recent available marker states, or null if none available.</returns>
|
||
public List<OptitrackMarkerState> GetLatestMarkerStates()
|
||
{
|
||
List<OptitrackMarkerState> markerStates = new List<OptitrackMarkerState>();
|
||
|
||
lock (m_frameDataUpdateLock)
|
||
{
|
||
foreach (KeyValuePair<Int32, OptitrackMarkerState> markerEntry in m_latestMarkerStates)
|
||
{
|
||
OptitrackMarkerState newMarkerState = new OptitrackMarkerState
|
||
{
|
||
Position = MirrorMode ? MirrorPosition( markerEntry.Value.Position ) : markerEntry.Value.Position,
|
||
Labeled = markerEntry.Value.Labeled,
|
||
Size = markerEntry.Value.Size,
|
||
Id = markerEntry.Value.Id
|
||
};
|
||
markerStates.Add( newMarkerState );
|
||
}
|
||
}
|
||
|
||
return markerStates;
|
||
}
|
||
|
||
/// <summary>Retrieves the definition of the rigid body with the specified streaming ID.</summary>
|
||
/// <param name="rigidBodyId"></param>
|
||
/// <returns>The specified rigid body definition, or null if not found.</returns>
|
||
public OptitrackRigidBodyDefinition GetRigidBodyDefinitionById( Int32 rigidBodyId )
|
||
{
|
||
return m_rigidBodyDefinitions.Find( def => def.Id == rigidBodyId );
|
||
}
|
||
|
||
/// <summary>
|
||
/// Gets the rigid body ID by its name.
|
||
/// </summary>
|
||
/// <param name="rigidBodyName">The name of the rigid body to find.</param>
|
||
/// <returns>The ID of the rigid body if found, -1 otherwise.</returns>
|
||
public int GetRigidBodyIdByName(string rigidBodyName)
|
||
{
|
||
var definition = m_rigidBodyDefinitions.Find(def => def.Name == rigidBodyName);
|
||
return definition != null ? definition.Id : -1;
|
||
}
|
||
|
||
/// <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 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>Retrieves the definition of the tmarkerset with the specified asset name.</summary>
|
||
/// <param name="TMarkersetName">The name of the tmarkerset for which to retrieve the definition.</param>
|
||
/// <returns>The specified tmarkerset definition, or null if not found.</returns> // trained markerset added
|
||
public OptitrackTMarkersetDefinition GetTMarkersetDefinitionByName(string TMarkersetName)
|
||
{
|
||
for (int i = 0; i < m_tmarkersetDefinitions.Count; i++)
|
||
{
|
||
OptitrackTMarkersetDefinition tmarDef = m_tmarkersetDefinitions[i];
|
||
|
||
if (tmarDef.Name.Equals(TMarkersetName, StringComparison.InvariantCultureIgnoreCase))
|
||
{
|
||
return tmarDef;
|
||
}
|
||
}
|
||
|
||
return null;
|
||
}
|
||
|
||
/// <summary>Retrieves the definition of the tmarkerset with the specified tmarkerset id.</summary>
|
||
/// <param name="tmarkersetId">The id of the tmarkerset for which to retrieve the definition.</param>
|
||
/// <returns>The specified tmarkerset definition, or null if not found.</returns> // trained markerset added
|
||
public OptitrackTMarkersetDefinition GetTMarkersetDefinitionById(Int32 tmarkersetId)
|
||
{
|
||
for (int i = 0; i < m_tmarkersetDefinitions.Count; ++i)
|
||
{
|
||
OptitrackTMarkersetDefinition tmarDef = m_tmarkersetDefinitions[i];
|
||
|
||
if (tmarDef.Id == tmarkersetId)
|
||
{
|
||
return tmarDef;
|
||
}
|
||
}
|
||
|
||
return null;
|
||
}
|
||
|
||
/// <summary>Get the most recently received state for streamed trained markerset markers.</summary>
|
||
/// <returns>The most recent available trained markerset marker states, or null if none available.</returns>
|
||
public List<OptitrackMarkerState> GetLatestTMarkMarkerStates() // trained markerset added
|
||
{
|
||
List<OptitrackMarkerState> tmarkmarkerStates = new List<OptitrackMarkerState>();
|
||
|
||
lock (m_frameDataUpdateLock)
|
||
{
|
||
foreach (KeyValuePair<Int32, OptitrackMarkerState> markerEntry in m_latestTMarkMarkerStates)
|
||
{
|
||
OptitrackMarkerState newMarkerState = new OptitrackMarkerState
|
||
{
|
||
Position = MirrorMode ? MirrorPosition( markerEntry.Value.Position ) : markerEntry.Value.Position,
|
||
Labeled = markerEntry.Value.Labeled,
|
||
Size = markerEntry.Value.Size,
|
||
Id = markerEntry.Value.Id
|
||
};
|
||
tmarkmarkerStates.Add(newMarkerState);
|
||
}
|
||
}
|
||
|
||
return tmarkmarkerStates;
|
||
}
|
||
|
||
/// <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.
|
||
UInt32 descriptionTypeMask = 0;
|
||
descriptionTypeMask |= (1 << (int)NatNetDataDescriptionType.NatNetDataDescriptionType_RigidBody);
|
||
descriptionTypeMask |= (1 << (int)NatNetDataDescriptionType.NatNetDataDescriptionType_Skeleton);
|
||
descriptionTypeMask |= (1 << (int)NatNetDataDescriptionType.NatNetDataDescriptionType_Asset); // trained markerset added
|
||
if (DrawMarkers)
|
||
{
|
||
descriptionTypeMask |= (1 << (int)NatNetDataDescriptionType.NatNetDataDescriptionType_MarkerSet);
|
||
}
|
||
if (DrawCameras)
|
||
{
|
||
descriptionTypeMask |= (1 << (int)NatNetDataDescriptionType.NatNetDataDescriptionType_Camera);
|
||
}
|
||
if( DrawForcePlates)
|
||
{
|
||
descriptionTypeMask |= (1 << (int)NatNetDataDescriptionType.NatNetDataDescriptionType_ForcePlate);
|
||
}
|
||
if( ReceiveDevices )
|
||
{
|
||
descriptionTypeMask |= (1 << (int)NatNetDataDescriptionType.NatNetDataDescriptionType_Device);
|
||
}
|
||
m_dataDescs = m_client.GetDataDescriptions(descriptionTypeMask);
|
||
|
||
// ?뺤쓽瑜??깃났?곸쑝濡?諛쏆븯?쇰?濡??먮룞 ?ъ“??移댁슫??由ъ뀑
|
||
m_definitionRefreshCount = 0;
|
||
|
||
m_rigidBodyDefinitions.Clear();
|
||
m_skeletonDefinitions.Clear();
|
||
m_tmarkersetDefinitions.Clear();
|
||
m_mirrorBoneIdMaps.Clear(); // ?ㅼ펷?덊넠 ?뺤쓽 蹂寃???mirror map 罹먯떆 臾댄슚??
|
||
// Device caches are accessed from NatNet thread; lock them.
|
||
lock (m_frameDataUpdateLock)
|
||
{
|
||
m_deviceDefinitions.Clear();
|
||
m_deviceNameToId.Clear();
|
||
m_latestDeviceStates.Clear();
|
||
}
|
||
|
||
// NatNet ?ㅻ젅?쒓? ?묎렐?섎뒗 罹먯떆???쎌쑝濡?蹂댄샇?섏뿬 ?덉씠??諛⑹?
|
||
lock (m_frameDataUpdateLock)
|
||
{
|
||
m_assetIdToNameCache.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),
|
||
BoneIdToParentIdMap = new Dictionary<int, int>(),
|
||
};
|
||
|
||
// 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 );
|
||
skelDef.BoneIdToParentIdMap[boneDef.Id] = boneDef.ParentId;
|
||
}
|
||
|
||
m_skeletonDefinitions.Add( skelDef );
|
||
}
|
||
|
||
// --------------------------------------------------------------------
|
||
// - Translate Trained Markerset Definitions // trained markerset added
|
||
// --------------------------------------------------------------------
|
||
for (int nativeTmarkDescIdx = 0; nativeTmarkDescIdx < m_dataDescs.AssetDescriptions.Count; ++nativeTmarkDescIdx)
|
||
{
|
||
sAssetDescription nativeTmark = m_dataDescs.AssetDescriptions[nativeTmarkDescIdx];
|
||
// Debug.Log("#rbs: " + nativeTmark.RigidBodyCount); // correct
|
||
|
||
OptitrackTMarkersetDefinition tmarkDef = new OptitrackTMarkersetDefinition
|
||
{
|
||
Id = nativeTmark.AssetID,
|
||
Name = nativeTmark.Name,
|
||
Bones = new List<OptitrackTMarkersetDefinition.BoneDefinition>(nativeTmark.RigidBodyCount),
|
||
BoneIdToParentIdMap = new Dictionary<int, int>(),
|
||
Markers = new List<OptitrackTMarkersetDefinition.MarkerDefinition>(nativeTmark.MarkerCount),
|
||
};
|
||
|
||
// Populate nested bone definitions.
|
||
for (int nativeBoneIdx = 0; nativeBoneIdx < nativeTmark.RigidBodyCount; ++nativeBoneIdx)
|
||
{
|
||
sRigidBodyDescription nativeBone = nativeTmark.RigidBodies[nativeBoneIdx];
|
||
|
||
OptitrackTMarkersetDefinition.BoneDefinition boneDef =
|
||
new OptitrackTMarkersetDefinition.BoneDefinition
|
||
{
|
||
Id = nativeBone.Id,
|
||
ParentId = nativeBone.ParentId,
|
||
Name = nativeBone.Name,
|
||
Offset = new Vector3(-nativeBone.OffsetX, nativeBone.OffsetY, nativeBone.OffsetZ),
|
||
};
|
||
|
||
tmarkDef.Bones.Add(boneDef);
|
||
tmarkDef.BoneIdToParentIdMap[boneDef.Id] = boneDef.ParentId;
|
||
}
|
||
|
||
// Populate nested marker definitions
|
||
for (int nativeMarkerIdx = 0; nativeMarkerIdx < nativeTmark.MarkerCount; ++nativeMarkerIdx)
|
||
{
|
||
sMarkerDescription nativeMarker = nativeTmark.Markers[nativeMarkerIdx];
|
||
//Debug.Log("TMarkerset (X, Y, Z): " + nativeMarker.X + " " + nativeMarker.Y + " " + nativeMarker.Z);
|
||
//Debug.Log(nativeMarker.Id + " " + nativeMarker.Name);
|
||
|
||
OptitrackTMarkersetDefinition.MarkerDefinition markerDef =
|
||
new OptitrackTMarkersetDefinition.MarkerDefinition
|
||
{
|
||
Name = nativeMarker.Name,
|
||
Id = nativeMarker.Id,
|
||
Position = new Vector3(-nativeMarker.X, nativeMarker.Y, nativeMarker.Z),
|
||
};
|
||
tmarkDef.Markers.Add(markerDef);
|
||
|
||
}
|
||
|
||
//foreach (KeyValuePair<int, int> kvp in tmarkDef.BoneIdToParentIdMap)
|
||
//{
|
||
// Debug.Log("Key: " + kvp.Key + "Value: " + kvp.Value);
|
||
//} // correct
|
||
|
||
m_tmarkersetDefinitions.Add(tmarkDef);
|
||
}
|
||
|
||
// ----------------------------------
|
||
// - Get Marker Definitions (ToDo)
|
||
// ----------------------------------
|
||
//for (int markersetNumber = 0; markersetNumber < m_dataDescs.MarkerSetDescriptions.Count; ++markersetNumber)
|
||
//{
|
||
// sMarkerSetDescription markerDescription = m_dataDescs.MarkerSetDescriptions[markersetNumber];
|
||
|
||
// if(markerDescription.Name == "all")
|
||
// {
|
||
// Int32 nMarkers = markerDescription.MarkerCount;
|
||
|
||
// for( int i = 0; i < nMarkers; ++i)
|
||
// {
|
||
// int nameOffset = i * NatNetConstants.MaxNameLength; //Marshal.SizeOf(typeof(Char**));
|
||
// IntPtr namePtr = new IntPtr(markerDescription.MarkerNames.ToInt64() + nameOffset);
|
||
|
||
// // FIXME: Need to de-construct the char array of names to use for marker naming later.
|
||
// // This throws an exception, thus doesn't work.
|
||
// // MarkerNames is a char** of size [MarkerCount][MaxNameLength]
|
||
// //string nativeLabel = Marshal.PtrToStringAnsi(namePtr);
|
||
// }
|
||
|
||
// OptitrackMarkersDefinition markersDef = new OptitrackMarkersDefinition
|
||
// {
|
||
// Name = markerDescription.Name
|
||
|
||
// };
|
||
|
||
// m_markersDefinitions.Add(markersDef);
|
||
// }
|
||
//}
|
||
|
||
// ----------------------------------
|
||
// - Camera Definitions
|
||
// ----------------------------------
|
||
for (int cameraIndex = 0; cameraIndex < m_dataDescs.CameraDescriptions.Count; ++cameraIndex)
|
||
{
|
||
sCameraDescription camera = m_dataDescs.CameraDescriptions[cameraIndex];
|
||
|
||
OptitrackCameraDefinition cameraDef = new OptitrackCameraDefinition
|
||
{
|
||
Name = camera.Name,
|
||
//Unity and motive use opposite positive-x directions, so positionX needs to be set to negative
|
||
Position = new Vector3(-camera.PositionX, camera.PositionY, camera.PositionZ),
|
||
Orientation = new Quaternion(camera.RotationX, camera.RotationY, camera.RotationZ, camera.RotationW),
|
||
};
|
||
|
||
m_cameraDefinitions.Add(cameraDef);
|
||
}
|
||
|
||
// ----------------------------------
|
||
// - Force Plate Definitions
|
||
// ----------------------------------
|
||
for (int plateIndex = 0; plateIndex < m_dataDescs.ForcePlateDescriptions.Count; ++plateIndex)
|
||
{
|
||
sForcePlateDescription plate = m_dataDescs.ForcePlateDescriptions[plateIndex];
|
||
|
||
OptitrackForcePlateDefinition forcePlateDef = new OptitrackForcePlateDefinition
|
||
{
|
||
Id = plate.Id,
|
||
SerialNumber = plate.SerialNo,
|
||
Width = plate.Width, // in inches
|
||
Length = plate.Length, // in inches
|
||
ElectricalOffset = new Vector3(plate.OriginX, plate.OriginY, plate.OriginZ),
|
||
CalibrationMatrix = new List<float>(12 * 12),
|
||
Corners = new List<float>(4 * 3),
|
||
PlateType = plate.PlateType,
|
||
ChannelDataType = plate.ChannelDataType,
|
||
ChannelCount = plate.ChannelCount,
|
||
ChannelNames = new List<string>(plate.ChannelCount),
|
||
};
|
||
|
||
// Populate corner locations
|
||
for( int i = 0; i < 12; ++i )
|
||
{
|
||
forcePlateDef.Corners.Add( plate.Corners[i] );
|
||
}
|
||
|
||
//Populate Channel Names
|
||
for (int i = 0; i < forcePlateDef.ChannelCount; ++i)
|
||
{
|
||
forcePlateDef.ChannelNames.Add(plate.ChannelNames[i]);
|
||
}
|
||
|
||
m_forcePlateDefinitions.Add(forcePlateDef);
|
||
}
|
||
|
||
// ----------------------------------
|
||
// - Device Definitions (generic analog: iFacialMocap, NIDAQ, etc.)
|
||
// ----------------------------------
|
||
lock (m_frameDataUpdateLock)
|
||
{
|
||
if (m_dataDescs.DeviceDescriptions != null)
|
||
{
|
||
for (int devIdx = 0; devIdx < m_dataDescs.DeviceDescriptions.Count; ++devIdx)
|
||
{
|
||
sDeviceDescription dev = m_dataDescs.DeviceDescriptions[devIdx];
|
||
|
||
// 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;
|
||
}
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
public void RegisterRigidBody( MonoBehaviour component, Int32 rigidBodyId )
|
||
{
|
||
if ( m_rigidBodies.ContainsKey( rigidBodyId ) )
|
||
{
|
||
#if false
|
||
MonoBehaviour existingRb = m_rigidBodies[rigidBodyId];
|
||
Debug.LogError( GetType().FullName + ": " + rb.GetType().FullName + " has duplicate rigid body ID " + rigidBodyId, component );
|
||
Debug.LogError( GetType().FullName + ": (Existing " + existingRb.GetType().FullName + " was already registered with that ID)", existingRb );
|
||
rb.enabled = false;
|
||
#endif
|
||
return;
|
||
}
|
||
|
||
m_rigidBodies[rigidBodyId] = component;
|
||
|
||
SubscribeRigidBody(component, rigidBodyId);
|
||
}
|
||
|
||
public void RegisterSkeleton(MonoBehaviour component, string name)
|
||
{
|
||
if (m_skeletons.ContainsKey(name))
|
||
{
|
||
#if false
|
||
MonoBehaviour existingSkel = m_skeletons[rigidBodyId];
|
||
Debug.LogError( "Duplicate skeleton detected, " + GetType().FullName + ": (Existing " + existingRb.GetType().FullName + " was already registered with that ID)", existingRb );
|
||
#endif
|
||
return;
|
||
}
|
||
|
||
m_skeletons[name] = component;
|
||
|
||
SubscribeSkeleton(component, name);
|
||
}
|
||
|
||
public void RegisterTMarkerset(MonoBehaviour component, string name) // trained markerset added
|
||
{
|
||
if (m_tmarkersets.ContainsKey(name))
|
||
{
|
||
return;
|
||
}
|
||
|
||
m_tmarkersets[name] = component;
|
||
|
||
SubscribeTMarkerset(component, name);
|
||
}
|
||
|
||
/// <summary>
|
||
/// (Re)initializes <see cref="m_client"/> and connects to the configured streaming server.
|
||
/// ?곌껐 珥덇린?붾뒗 肄붾（?댁쑝濡??꾩엫?섏뿬 Thread.Sleep?쇰줈 硫붿씤 ?ㅻ젅?쒕? 釉붾씫?섏? ?딅뒗??
|
||
/// </summary>
|
||
void OnEnable()
|
||
{
|
||
m_receivedFrameSinceConnect = false;
|
||
// AutoReconnect媛 耳쒖졇?덉쑝硫?珥덇린 ?곌껐 ?ㅽ뙣 ?쒖뿉???먮룞 ?ъ떆??
|
||
if (AutoReconnect)
|
||
StartCoroutine( InitialConnectWithRetry() );
|
||
else
|
||
StartCoroutine( ConnectCoroutine() );
|
||
|
||
if (EnableReplayPriority && m_replayConnectCoroutine == null && ConnectionType != ClientConnectionType.Multicast)
|
||
m_replayConnectCoroutine = StartCoroutine( ReplayConnectLoop() );
|
||
}
|
||
|
||
/// <summary>
|
||
/// 珥덇린 ?곌껐??諛깆삤?꾩? ?④퍡 ?먮룞 ?ъ떆?꾪빀?덈떎.
|
||
/// 泥섎━?섎뒗 耳?댁뒪:
|
||
/// A) NatNet_Client_Connect ?먯껜媛 throw (NatNetError_Network ?? ??Motive PC 誘몄쓳?? 諛⑺솕踰? ?섎せ??IP ??
|
||
/// B) Connect???깃났?덉쑝??泥??꾨젅?꾩씠 ?곸쁺 ??????Motive Streaming 誘명솢?깊솕, Multicast ?쇱슦??李⑤떒 ??
|
||
/// 耳?댁뒪 C(?곌껐 ???꾨젅???딄?)??湲곗〈 CheckConnectionHealth+ReconnectCoroutine?먯꽌 泥섎━??
|
||
/// </summary>
|
||
private System.Collections.IEnumerator InitialConnectWithRetry()
|
||
{
|
||
const int maxAttempts = 10;
|
||
// 諛깆삤?? 1, 2, 3, 5, 10, 10, ... 珥?(珥???60珥덇컙 ?쒕룄)
|
||
float[] retryDelays = { 1f, 2f, 3f, 5f, 10f };
|
||
|
||
for (int attempt = 1; attempt <= maxAttempts; attempt++)
|
||
{
|
||
// ConnectCoroutine ?ㅽ뻾
|
||
yield return StartCoroutine(ConnectCoroutine());
|
||
|
||
bool transportConnected = m_client != null || m_directNatNetConnected;
|
||
if (transportConnected)
|
||
{
|
||
if (m_directNatNetConnected && m_client == null)
|
||
{
|
||
if (attempt > 1)
|
||
Debug.Log(string.Format("{0}: initial direct NatNet receiver started (attempt {1}/{2}).", GetType().FullName, attempt, maxAttempts), this);
|
||
yield break;
|
||
}
|
||
|
||
// 耳?댁뒪 B ?뺤씤: Connect???깃났 ??泥??꾨젅???꾩갑源뚯? 5珥??湲?
|
||
float deadline = Time.realtimeSinceStartup + 5.0f;
|
||
while (!m_receivedFrameSinceConnect && Time.realtimeSinceStartup < deadline)
|
||
yield return null;
|
||
|
||
if (m_receivedFrameSinceConnect)
|
||
{
|
||
if (attempt > 1)
|
||
Debug.Log(string.Format("{0}: initial connection succeeded (attempt {1}/{2}).", GetType().FullName, attempt, maxAttempts), this);
|
||
yield break; // ?깃났
|
||
}
|
||
|
||
// 耳?댁뒪 B: ?곌껐?먯?留??꾨젅??誘몄닔?????대씪?댁뼵???뺣━ ???ъ떆??
|
||
Debug.LogWarning(string.Format("{0}: initial connection attempt {1}/{2} connected to {3} but received no frames for 5 seconds. ServerNatNet={4}, ClientNatNet={5}, Local={6}, Connection={7}. Check Motive streaming mode, multicast interface, or NatNet SDK compatibility.", GetType().FullName, attempt, maxAttempts, ServerAddress, ServerNatNetVersion, ClientNatNetVersion, ResolvedLocalAddress, ConnectionType), this);
|
||
CleanupClient();
|
||
}
|
||
|
||
if (attempt == maxAttempts)
|
||
{
|
||
Debug.LogError(string.Format("{0}: all {1} initial connection attempts failed. Reconnect manually from the Inspector.", GetType().FullName, maxAttempts), this);
|
||
yield break;
|
||
}
|
||
|
||
float delay = retryDelays[Mathf.Min(attempt - 1, retryDelays.Length - 1)];
|
||
Debug.Log(string.Format("{0}: Initial connection attempt {1}/{2} failed; retrying in {3} seconds.", GetType().FullName, attempt, maxAttempts, delay), this);
|
||
yield return new WaitForSeconds(delay);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// m_client? 愿??肄붾（???대깽?몃? ?덉쟾?섍쾶 ?뺣━?⑸땲??
|
||
/// InitialConnectWithRetry??耳?댁뒪 B ?ъ떆?????ъ슜.
|
||
/// </summary>
|
||
private void CleanupClient()
|
||
{
|
||
StopDirectFrameReceiver();
|
||
m_directNatNetConnected = false;
|
||
if (m_connectionHealthCoroutine != null)
|
||
{
|
||
StopCoroutine(m_connectionHealthCoroutine);
|
||
m_connectionHealthCoroutine = null;
|
||
}
|
||
if (m_client != null)
|
||
{
|
||
try { m_client.NativeFrameReceived -= OnNatNetFrameReceived; } catch (Exception) { }
|
||
try { m_client.Disconnect(); } catch (Exception) { }
|
||
try { m_client.Dispose(); } catch (Exception) { }
|
||
m_client = null;
|
||
}
|
||
}
|
||
|
||
|
||
private System.Collections.IEnumerator ReplayConnectLoop()
|
||
{
|
||
var retryDelay = new WaitForSeconds(2.0f);
|
||
while (EnableReplayPriority)
|
||
{
|
||
if (m_replayClient == null && !string.IsNullOrWhiteSpace(ReplayServerAddress))
|
||
yield return StartCoroutine(ConnectReplayCoroutine());
|
||
else if (m_replayClient != null &&
|
||
m_replayReceivedFrameSinceConnect &&
|
||
m_lastReplayFrameDeliveryTimestamp.AgeSeconds > 5.0f)
|
||
{
|
||
CleanupReplayClient();
|
||
}
|
||
yield return retryDelay;
|
||
}
|
||
CleanupReplayClient();
|
||
m_replayConnectCoroutine = null;
|
||
}
|
||
|
||
private System.Collections.IEnumerator ConnectReplayCoroutine()
|
||
{
|
||
IPAddress serverAddr;
|
||
IPAddress localAddr;
|
||
try
|
||
{
|
||
serverAddr = IPAddress.Parse(ReplayServerAddress.Trim());
|
||
localAddr = ResolveLocalAddress(serverAddr);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
Debug.LogException(ex, this);
|
||
yield break;
|
||
}
|
||
|
||
NatNetClient connectedClient = null;
|
||
Exception connectError = null;
|
||
System.Threading.Tasks.Task connectTask = System.Threading.Tasks.Task.Run(() =>
|
||
{
|
||
NatNetClient c = null;
|
||
try
|
||
{
|
||
c = new NatNetClient();
|
||
c.Connect(
|
||
NatNetConnectionType.NatNetConnectionType_Multicast,
|
||
localAddr,
|
||
serverAddr,
|
||
k_DefaultNatNetCommandPort,
|
||
k_DefaultNatNetDataPort,
|
||
IPAddress.Parse("239.255.42.100"));
|
||
connectedClient = c;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
connectError = ex;
|
||
if (c != null) { try { c.Dispose(); } catch { } }
|
||
}
|
||
});
|
||
|
||
while (!connectTask.IsCompleted)
|
||
yield return null;
|
||
|
||
if (connectError != null || connectedClient == null)
|
||
{
|
||
if (!m_replayConnectWarned)
|
||
{
|
||
Debug.LogWarning(GetType().FullName + ": Replay NatNet is not available yet. Server=" + ReplayServerAddress + ", local=" + localAddr + ", command=1510, data=1511, multicast=239.255.42.100. This is normal while MMRP is not replaying.", this);
|
||
m_replayConnectWarned = true;
|
||
}
|
||
yield break;
|
||
}
|
||
|
||
CleanupReplayClient();
|
||
m_replayClient = connectedClient;
|
||
m_replayReceivedFrameSinceConnect = false;
|
||
m_replayConnectWarned = false;
|
||
m_replayClient.NativeFrameReceived += OnNatNetFrameReceived;
|
||
Debug.Log(GetType().FullName + ": Replay NatNet connected to " + ReplayServerAddress + " (239.255.42.100).", this);
|
||
}
|
||
|
||
private void CleanupReplayClient()
|
||
{
|
||
if (m_replayClient != null)
|
||
{
|
||
try { m_replayClient.NativeFrameReceived -= OnNatNetFrameReceived; } catch { }
|
||
try { m_replayClient.Disconnect(); } catch { }
|
||
try { m_replayClient.Dispose(); } catch { }
|
||
m_replayClient = null;
|
||
}
|
||
m_replayReceivedFrameSinceConnect = false;
|
||
}
|
||
|
||
private bool IsReplayFrameFresh()
|
||
{
|
||
return EnableReplayPriority &&
|
||
m_replayReceivedFrameSinceConnect &&
|
||
m_lastReplayFrameDeliveryTimestamp.AgeSeconds <= ReplayFreshnessSeconds;
|
||
}
|
||
|
||
private void StartDirectFrameReceiver(string localAddress, string multicastAddress, UInt16 dataPort)
|
||
{
|
||
StopDirectFrameReceiver();
|
||
|
||
try
|
||
{
|
||
IPAddress group = IPAddress.Parse(multicastAddress);
|
||
IPAddress local = IPAddress.Parse(localAddress);
|
||
|
||
m_directFrameUdp = new UdpClient(AddressFamily.InterNetwork);
|
||
m_directFrameUdp.ExclusiveAddressUse = false;
|
||
m_directFrameUdp.Client.ReceiveBufferSize = 1024 * 1024;
|
||
m_directFrameUdp.Client.SetSocketOption(SocketOptionLevel.Socket, SocketOptionName.ReuseAddress, true);
|
||
m_directFrameUdp.Client.Bind(new IPEndPoint(IPAddress.Any, dataPort));
|
||
m_directFrameUdp.Client.SetSocketOption(SocketOptionLevel.IP, SocketOptionName.AddMembership, new MulticastOption(group, local));
|
||
if (EnableReplayPriority)
|
||
{
|
||
try
|
||
{
|
||
IPAddress replayGroup = IPAddress.Parse("239.255.42.100");
|
||
if (!replayGroup.Equals(group))
|
||
m_directFrameUdp.Client.SetSocketOption(SocketOptionLevel.IP, SocketOptionName.AddMembership, new MulticastOption(replayGroup, local));
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
Debug.LogWarning(GetType().FullName + ": direct NatNet replay multicast join failed: " + ex.Message, this);
|
||
}
|
||
}
|
||
m_directFrameUdp.Client.ReceiveTimeout = 1000;
|
||
|
||
m_directFrameRunning = true;
|
||
m_directNatNetConnected = true;
|
||
m_directFrameLogged = false;
|
||
m_directTimeoutCount = 0;
|
||
m_directTimeoutLogPending = false;
|
||
m_directSocketErrorLogPending = false;
|
||
m_directFrameThread = new Thread(DirectFrameThreadLoop)
|
||
{
|
||
IsBackground = true,
|
||
Name = "OptitrackStreamingClient.DirectNatNet"
|
||
};
|
||
m_directFrameThread.Start();
|
||
Debug.Log(GetType().FullName + ": direct NatNet frame receiver joined " + multicastAddress + ":" + dataPort + " on " + localAddress + (EnableReplayPriority ? " with replay priority group 239.255.42.100." : "."), this);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
StopDirectFrameReceiver();
|
||
Debug.LogWarning(GetType().FullName + ": direct NatNet frame receiver failed to start: " + ex.Message, this);
|
||
}
|
||
}
|
||
|
||
private void StopDirectFrameReceiver()
|
||
{
|
||
m_directFrameRunning = false;
|
||
m_directFrameLogPending = false;
|
||
m_directTimeoutLogPending = false;
|
||
m_directSocketErrorLogPending = false;
|
||
m_directNatNetConnected = false;
|
||
try { m_directFrameUdp?.Close(); } catch { }
|
||
m_directFrameUdp = null;
|
||
|
||
if (m_directFrameThread != null && m_directFrameThread.IsAlive)
|
||
m_directFrameThread.Join(500);
|
||
m_directFrameThread = null;
|
||
}
|
||
|
||
private void DirectFrameThreadLoop()
|
||
{
|
||
IPEndPoint ep = new IPEndPoint(IPAddress.Any, 0);
|
||
while (m_directFrameRunning)
|
||
{
|
||
try
|
||
{
|
||
byte[] data = m_directFrameUdp.Receive(ref ep);
|
||
m_directTimeoutCount = 0;
|
||
ParseDirectFramePacket(data, ep.Address);
|
||
}
|
||
catch (SocketException se)
|
||
{
|
||
if (!m_directFrameRunning) break;
|
||
if (se.SocketErrorCode == SocketError.TimedOut)
|
||
{
|
||
int timeoutCount = Interlocked.Increment(ref m_directTimeoutCount);
|
||
if (timeoutCount == 5 || timeoutCount == 15)
|
||
m_directTimeoutLogPending = true;
|
||
continue;
|
||
}
|
||
m_directSocketErrorMessage = se.SocketErrorCode + " / " + se.Message;
|
||
m_directSocketErrorLogPending = true;
|
||
}
|
||
catch (ObjectDisposedException)
|
||
{
|
||
break;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
m_directSocketErrorMessage = ex.GetType().Name + " / " + ex.Message;
|
||
m_directSocketErrorLogPending = true;
|
||
}
|
||
}
|
||
}
|
||
|
||
private void ParseDirectFramePacket(byte[] data, IPAddress sourceAddress)
|
||
{
|
||
if (data == null || data.Length < 12) return;
|
||
if (BitConverter.ToUInt16(data, 0) != 7) return; // NAT_FRAMEOFDATA
|
||
|
||
int o = 4;
|
||
int frameNumber = BitConverter.ToInt32(data, o); o += 4;
|
||
OptitrackHiResTimer.Timestamp frameTimestamp = OptitrackHiResTimer.Now();
|
||
if (!m_directFrameLogged)
|
||
{
|
||
m_directFrameLogged = true;
|
||
m_directFrameLogPending = true;
|
||
}
|
||
bool isReplayFrame = EnableReplayPriority &&
|
||
!string.IsNullOrWhiteSpace(ReplayServerAddress) &&
|
||
sourceAddress != null &&
|
||
sourceAddress.ToString() == ReplayServerAddress.Trim();
|
||
|
||
if (!isReplayFrame && IsReplayFrameFresh())
|
||
return;
|
||
|
||
if (isReplayFrame)
|
||
{
|
||
m_replayReceivedFrameSinceConnect = true;
|
||
Interlocked.Exchange(ref m_lastReplayFrameDeliveryTimestamp.m_ticks, frameTimestamp.m_ticks);
|
||
}
|
||
else
|
||
{
|
||
m_receivedFrameSinceConnect = true;
|
||
Interlocked.Exchange(ref m_lastFrameDeliveryTimestamp.m_ticks, frameTimestamp.m_ticks);
|
||
}
|
||
|
||
if (!Monitor.TryEnter(m_frameDataUpdateLock, 1))
|
||
return;
|
||
|
||
try
|
||
{
|
||
int rbCount, rbStart, rbEnd;
|
||
if (!ReadDirectSection(data, ref o, out rbCount, out rbStart, out rbEnd)) return; // markerSets
|
||
if (!ReadDirectSection(data, ref o, out rbCount, out rbStart, out rbEnd)) return; // unlabeled
|
||
if (!ReadDirectSection(data, ref o, out rbCount, out rbStart, out rbEnd)) return; // rigidBodies
|
||
ParseDirectRigidBodies(data, rbCount, rbStart, rbEnd, frameTimestamp);
|
||
|
||
int skelCount, skelStart, skelEnd;
|
||
if (!ReadDirectSection(data, ref o, out skelCount, out skelStart, out skelEnd)) return; // skeletons
|
||
ParseDirectSkeletons(data, skelCount, skelStart, skelEnd, frameTimestamp);
|
||
|
||
int skipCount, skipStart, skipEnd;
|
||
if (!ReadDirectSection(data, ref o, out skipCount, out skipStart, out skipEnd)) return; // assets
|
||
if (!ReadDirectSection(data, ref o, out skipCount, out skipStart, out skipEnd)) return; // labeled markers
|
||
if (!ReadDirectSection(data, ref o, out skipCount, out skipStart, out skipEnd)) return; // force plates
|
||
|
||
int deviceCount, deviceStart, deviceEnd;
|
||
if (!ReadDirectSection(data, ref o, out deviceCount, out deviceStart, out deviceEnd)) return; // devices
|
||
ParseDirectDevices(data, deviceCount, deviceStart, deviceEnd, frameNumber, frameTimestamp);
|
||
|
||
}
|
||
finally
|
||
{
|
||
Monitor.Exit(m_frameDataUpdateLock);
|
||
}
|
||
}
|
||
|
||
private void ParseDirectDevices(byte[] data, int count, int start, int end, int frameNumber, OptitrackHiResTimer.Timestamp frameTimestamp)
|
||
{
|
||
if (!ReceiveDevices)
|
||
return;
|
||
|
||
m_latestDeviceStates.Clear();
|
||
|
||
int o = start;
|
||
for (int i = 0; i < count && o + 8 <= end; i++)
|
||
{
|
||
int id = BitConverter.ToInt32(data, o); o += 4;
|
||
int channelCount = BitConverter.ToInt32(data, o); o += 4;
|
||
if (channelCount < 0 || channelCount > 4096)
|
||
return;
|
||
|
||
OptitrackDeviceDefinition definition = m_deviceDefinitions.Find(d => d.Id == id);
|
||
Dictionary<string, float> values = new Dictionary<string, float>(channelCount);
|
||
|
||
for (int ch = 0; ch < channelCount; ch++)
|
||
{
|
||
if (o + 4 > end) return;
|
||
int sampleCount = BitConverter.ToInt32(data, o); o += 4;
|
||
if (sampleCount < 0 || sampleCount > 1024 || o + sampleCount * 4 > end) return;
|
||
|
||
float value = sampleCount > 0 ? BitConverter.ToSingle(data, o) : 0.0f;
|
||
o += sampleCount * 4;
|
||
|
||
string channelName = (definition != null && definition.ChannelNames != null && ch < definition.ChannelNames.Count)
|
||
? definition.ChannelNames[ch]
|
||
: "Channel" + ch;
|
||
values[channelName] = value;
|
||
}
|
||
|
||
int stateKey = DirectDeviceStateKey(id, i);
|
||
string deviceName = definition != null ? definition.Name : ("Device" + stateKey);
|
||
m_latestDeviceStates[stateKey] = new OptitrackDeviceState
|
||
{
|
||
Id = stateKey,
|
||
Name = deviceName,
|
||
ChannelValues = values,
|
||
FrameNumber = frameNumber,
|
||
DeliveryTimestamp = frameTimestamp,
|
||
};
|
||
}
|
||
}
|
||
|
||
private static int DirectDeviceStateKey(int deviceId, int wireIndex)
|
||
{
|
||
return deviceId != 0 ? deviceId : -1 - wireIndex;
|
||
}
|
||
|
||
private static bool ReadDirectSection(byte[] data, ref int o, out int count, out int start, out int end)
|
||
{
|
||
count = 0;
|
||
start = end = o;
|
||
if (o + 8 > data.Length) return false;
|
||
count = BitConverter.ToInt32(data, o); o += 4;
|
||
int size = BitConverter.ToInt32(data, o); o += 4;
|
||
if (count < 0 || size < 0 || o + size > data.Length) return false;
|
||
start = o;
|
||
end = o + size;
|
||
o = end;
|
||
return true;
|
||
}
|
||
|
||
private static bool ReadDirectRigidBody(byte[] data, ref int o, int end, out sRigidBodyData rb)
|
||
{
|
||
rb = new sRigidBodyData();
|
||
if (o + 38 > end || o + 38 > data.Length) return false;
|
||
rb.Id = BitConverter.ToInt32(data, o); o += 4;
|
||
rb.X = BitConverter.ToSingle(data, o); o += 4;
|
||
rb.Y = BitConverter.ToSingle(data, o); o += 4;
|
||
rb.Z = BitConverter.ToSingle(data, o); o += 4;
|
||
rb.QX = BitConverter.ToSingle(data, o); o += 4;
|
||
rb.QY = BitConverter.ToSingle(data, o); o += 4;
|
||
rb.QZ = BitConverter.ToSingle(data, o); o += 4;
|
||
rb.QW = BitConverter.ToSingle(data, o); o += 4;
|
||
rb.MeanError = BitConverter.ToSingle(data, o); o += 4;
|
||
rb.Params = BitConverter.ToInt16(data, o); o += 2;
|
||
return true;
|
||
}
|
||
|
||
private void ParseDirectRigidBodies(byte[] data, int count, int start, int end, OptitrackHiResTimer.Timestamp frameTimestamp)
|
||
{
|
||
int o = start;
|
||
for (int i = 0; i < count && o < end; i++)
|
||
{
|
||
sRigidBodyData rbData;
|
||
if (!ReadDirectRigidBody(data, ref o, end, out rbData)) return;
|
||
OptitrackRigidBodyState rbState = GetOrCreateRigidBodyState(rbData.Id);
|
||
RigidBodyDataToState(rbData, frameTimestamp, rbState);
|
||
}
|
||
}
|
||
|
||
private void ParseDirectSkeletons(byte[] data, int count, int start, int end, OptitrackHiResTimer.Timestamp frameTimestamp)
|
||
{
|
||
int o = start;
|
||
for (int i = 0; i < count && o + 8 <= end; i++)
|
||
{
|
||
int skeletonId = BitConverter.ToInt32(data, o); o += 4;
|
||
int boneCount = BitConverter.ToInt32(data, o); o += 4;
|
||
if (boneCount < 0 || boneCount > 4096 || o + boneCount * 38 > end) return;
|
||
|
||
sRigidBodyData[] stagedBones = GetSkeletonFrameScratch(skeletonId, boneCount);
|
||
for (int b = 0; b < boneCount; b++)
|
||
{
|
||
if (!ReadDirectRigidBody(data, ref o, end, out stagedBones[b])) return;
|
||
}
|
||
|
||
CommitDirectSkeletonFrame(skeletonId, stagedBones, boneCount, frameTimestamp);
|
||
}
|
||
}
|
||
|
||
private void CommitDirectSkeletonFrame(int skeletonId, sRigidBodyData[] stagedBones, int boneCount, OptitrackHiResTimer.Timestamp frameTimestamp)
|
||
{
|
||
OptitrackSkeletonDefinition skelDef = GetSkeletonDefinitionById(skeletonId);
|
||
if (skelDef == null)
|
||
{
|
||
skelDef = EnsureDirectSyntheticSkeletonDefinition(skeletonId, stagedBones, boneCount);
|
||
}
|
||
|
||
if (skelDef == null)
|
||
{
|
||
if (!m_pendingDefinitionRefresh && m_definitionRefreshCount < k_MaxAutoDefinitionRefreshes)
|
||
m_pendingDefinitionRefresh = true;
|
||
return;
|
||
}
|
||
|
||
if (EnableSkeletonFrameFilter && boneCount != skelDef.Bones.Count)
|
||
return;
|
||
|
||
OptitrackSkeletonState skelState = GetOrCreateSkeletonState(skeletonId);
|
||
|
||
if (EnableSkeletonFrameFilter)
|
||
{
|
||
for (int b = 0; b < boneCount; b++)
|
||
{
|
||
int boneSkelId, boneId;
|
||
DirectDecodeId(stagedBones[b].Id, out boneSkelId, out boneId);
|
||
if (boneSkelId != skeletonId ||
|
||
!skelDef.BoneIdToParentIdMap.ContainsKey(boneId) ||
|
||
!IsSkeletonBoneTracked(stagedBones[b]) ||
|
||
!IsBoneDataUsable(stagedBones[b]))
|
||
{
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
|
||
for (int b = 0; b < boneCount; b++)
|
||
{
|
||
sRigidBodyData boneData = stagedBones[b];
|
||
if (!TryGetDirectCommittableBoneId(boneData, skeletonId, skelDef, out int boneId))
|
||
continue;
|
||
|
||
if (!skelState.BonePoses.ContainsKey(boneId))
|
||
skelState.BonePoses[boneId] = new OptitrackPose();
|
||
if (!skelState.LocalBonePoses.ContainsKey(boneId))
|
||
skelState.LocalBonePoses[boneId] = new OptitrackPose();
|
||
|
||
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);
|
||
}
|
||
|
||
for (int b = 0; b < boneCount; b++)
|
||
{
|
||
sRigidBodyData boneData = stagedBones[b];
|
||
if (!TryGetDirectCommittableBoneId(boneData, skeletonId, skelDef, out int boneId))
|
||
continue;
|
||
|
||
Vector3 bonePos = skelState.BonePoses[boneId].Position;
|
||
Quaternion boneOri = skelState.BonePoses[boneId].Orientation;
|
||
Vector3 parentBonePos = Vector3.zero;
|
||
Quaternion parentBoneOri = Quaternion.identity;
|
||
|
||
Int32 parentId = skelDef.BoneIdToParentIdMap[boneId];
|
||
if (parentId != 0 && skelState.BonePoses.TryGetValue(parentId, out OptitrackPose parentPose))
|
||
{
|
||
parentBonePos = parentPose.Position;
|
||
parentBoneOri = parentPose.Orientation;
|
||
}
|
||
|
||
skelState.LocalBonePoses[boneId].Position = bonePos - parentBonePos;
|
||
skelState.LocalBonePoses[boneId].Orientation = Quaternion.Inverse(parentBoneOri) * boneOri;
|
||
}
|
||
skelState.DeliveryTimestamp = frameTimestamp;
|
||
}
|
||
|
||
private static void DirectDecodeId(int encodedId, out int assetId, out int memberId)
|
||
{
|
||
assetId = (encodedId >> 16) & 0x7FFF;
|
||
memberId = encodedId & 0xFFFF;
|
||
}
|
||
|
||
private static bool TryGetDirectCommittableBoneId(sRigidBodyData boneData, Int32 skeletonId, OptitrackSkeletonDefinition skelDef, out int boneId)
|
||
{
|
||
int boneSkelId;
|
||
DirectDecodeId(boneData.Id, out boneSkelId, out boneId);
|
||
return boneSkelId == skeletonId &&
|
||
skelDef.BoneIdToParentIdMap.ContainsKey(boneId) &&
|
||
IsBoneDataUsable(boneData);
|
||
}
|
||
|
||
private OptitrackSkeletonDefinition EnsureDirectSyntheticSkeletonDefinition(int skeletonId, sRigidBodyData[] stagedBones, int boneCount)
|
||
{
|
||
OptitrackSkeletonDefinition existing = GetSkeletonDefinitionById(skeletonId);
|
||
if (existing != null)
|
||
return existing;
|
||
|
||
string skeletonName = ChooseSyntheticSkeletonName(skeletonId);
|
||
OptitrackSkeletonDefinition skelDef = new OptitrackSkeletonDefinition
|
||
{
|
||
Id = skeletonId,
|
||
Name = skeletonName,
|
||
Bones = new List<OptitrackSkeletonDefinition.BoneDefinition>(boneCount),
|
||
BoneIdToParentIdMap = new Dictionary<int, int>(),
|
||
};
|
||
|
||
for (int i = 0; i < boneCount; i++)
|
||
{
|
||
int boneSkelId, boneId;
|
||
DirectDecodeId(stagedBones[i].Id, out boneSkelId, out boneId);
|
||
if (boneSkelId != skeletonId)
|
||
continue;
|
||
|
||
string shortName = DirectMotiveBoneName(boneId);
|
||
int parentId = DirectMotiveParentId(boneId);
|
||
OptitrackSkeletonDefinition.BoneDefinition boneDef = new OptitrackSkeletonDefinition.BoneDefinition
|
||
{
|
||
Id = boneId,
|
||
ParentId = parentId,
|
||
Name = skeletonName + "_" + shortName,
|
||
Offset = Vector3.zero,
|
||
};
|
||
skelDef.Bones.Add(boneDef);
|
||
skelDef.BoneIdToParentIdMap[boneDef.Id] = boneDef.ParentId;
|
||
}
|
||
|
||
if (skelDef.Bones.Count == 0)
|
||
return null;
|
||
|
||
m_skeletonDefinitions.Add(skelDef);
|
||
m_mirrorBoneIdMaps.Clear();
|
||
return skelDef;
|
||
}
|
||
|
||
private string ChooseSyntheticSkeletonName(int skeletonId)
|
||
{
|
||
if (!string.IsNullOrWhiteSpace(DirectSyntheticSkeletonName))
|
||
return DirectSyntheticSkeletonName.Trim();
|
||
return "Skeleton" + skeletonId;
|
||
}
|
||
|
||
private static string DirectMotiveBoneName(int boneId)
|
||
{
|
||
switch (boneId)
|
||
{
|
||
case 1: return "Hip";
|
||
case 2: return "Ab";
|
||
case 52: return "Spine2";
|
||
case 53: return "Spine3";
|
||
case 54: return "Spine4";
|
||
case 3: return "Chest";
|
||
case 4: return "Neck";
|
||
case 58: return "Neck2";
|
||
case 5: return "Head";
|
||
case 6: return "LShoulder";
|
||
case 7: return "LUArm";
|
||
case 8: return "LFArm";
|
||
case 9: return "LHand";
|
||
case 22: return "LThumb1";
|
||
case 23: return "LThumb2";
|
||
case 24: return "LThumb3";
|
||
case 25: return "LIndex1";
|
||
case 26: return "LIndex2";
|
||
case 27: return "LIndex3";
|
||
case 28: return "LMiddle1";
|
||
case 29: return "LMiddle2";
|
||
case 30: return "LMiddle3";
|
||
case 31: return "LRing1";
|
||
case 32: return "LRing2";
|
||
case 33: return "LRing3";
|
||
case 34: return "LPinky1";
|
||
case 35: return "LPinky2";
|
||
case 36: return "LPinky3";
|
||
case 10: return "RShoulder";
|
||
case 11: return "RUArm";
|
||
case 12: return "RFArm";
|
||
case 13: return "RHand";
|
||
case 37: return "RThumb1";
|
||
case 38: return "RThumb2";
|
||
case 39: return "RThumb3";
|
||
case 40: return "RIndex1";
|
||
case 41: return "RIndex2";
|
||
case 42: return "RIndex3";
|
||
case 43: return "RMiddle1";
|
||
case 44: return "RMiddle2";
|
||
case 45: return "RMiddle3";
|
||
case 46: return "RRing1";
|
||
case 47: return "RRing2";
|
||
case 48: return "RRing3";
|
||
case 49: return "RPinky1";
|
||
case 50: return "RPinky2";
|
||
case 51: return "RPinky3";
|
||
case 14: return "LThigh";
|
||
case 15: return "LShin";
|
||
case 16: return "LFoot";
|
||
case 17: return "LToe";
|
||
case 18: return "RThigh";
|
||
case 19: return "RShin";
|
||
case 20: return "RFoot";
|
||
case 21: return "RToe";
|
||
default: return "Bone" + boneId;
|
||
}
|
||
}
|
||
|
||
private static int DirectMotiveParentId(int boneId)
|
||
{
|
||
switch (boneId)
|
||
{
|
||
case 1: return 0;
|
||
case 2: return 1;
|
||
case 52: return 2;
|
||
case 53: return 52;
|
||
case 54: return 53;
|
||
case 3: return 54;
|
||
case 4: return 3;
|
||
case 58: return 4;
|
||
case 5: return 58;
|
||
case 6: return 3;
|
||
case 7: return 6;
|
||
case 8: return 7;
|
||
case 9: return 8;
|
||
case 22: return 9;
|
||
case 23: return 22;
|
||
case 24: return 23;
|
||
case 25: return 9;
|
||
case 26: return 25;
|
||
case 27: return 26;
|
||
case 28: return 9;
|
||
case 29: return 28;
|
||
case 30: return 29;
|
||
case 31: return 9;
|
||
case 32: return 31;
|
||
case 33: return 32;
|
||
case 34: return 9;
|
||
case 35: return 34;
|
||
case 36: return 35;
|
||
case 10: return 3;
|
||
case 11: return 10;
|
||
case 12: return 11;
|
||
case 13: return 12;
|
||
case 37: return 13;
|
||
case 38: return 37;
|
||
case 39: return 38;
|
||
case 40: return 13;
|
||
case 41: return 40;
|
||
case 42: return 41;
|
||
case 43: return 13;
|
||
case 44: return 43;
|
||
case 45: return 44;
|
||
case 46: return 13;
|
||
case 47: return 46;
|
||
case 48: return 47;
|
||
case 49: return 13;
|
||
case 50: return 49;
|
||
case 51: return 50;
|
||
case 14: return 1;
|
||
case 15: return 14;
|
||
case 16: return 15;
|
||
case 17: return 16;
|
||
case 18: return 1;
|
||
case 19: return 18;
|
||
case 20: return 19;
|
||
case 21: return 20;
|
||
default: return 0;
|
||
}
|
||
}
|
||
|
||
private bool ConnectDirectNatNet(IPAddress serverAddr, IPAddress localAddr, UInt16 commandPort, UInt16 dataPort, IPAddress multicastAddr)
|
||
{
|
||
StopDirectFrameReceiver();
|
||
|
||
string hostName;
|
||
byte[] appVersion;
|
||
byte[] natNetVersion;
|
||
byte[] modelDefPacket;
|
||
UInt16 negotiatedDataPort = dataPort;
|
||
IPAddress negotiatedMulticast = multicastAddr;
|
||
|
||
bool hasServerInfo = DirectRequestServerInfo(serverAddr, localAddr, commandPort, out hostName, out appVersion, out natNetVersion, out negotiatedDataPort, out negotiatedMulticast);
|
||
if (!hasServerInfo)
|
||
{
|
||
hostName = "NatNet multicast source";
|
||
appVersion = new byte[4];
|
||
natNetVersion = new byte[] { 4, 2, 0, 0 };
|
||
negotiatedDataPort = dataPort;
|
||
negotiatedMulticast = multicastAddr ?? IPAddress.Parse("239.255.42.99");
|
||
if (!m_directNoModelDefWarned)
|
||
{
|
||
Debug.LogWarning(GetType().FullName + ": NatNet command channel did not respond on " + serverAddr + ":" + commandPort + ". Continuing with multicast frames and synthetic skeleton definitions.", this);
|
||
m_directNoModelDefWarned = true;
|
||
}
|
||
}
|
||
|
||
if (negotiatedDataPort == 0)
|
||
negotiatedDataPort = dataPort;
|
||
if (negotiatedMulticast == null)
|
||
negotiatedMulticast = multicastAddr ?? IPAddress.Parse("239.255.42.99");
|
||
|
||
if (hasServerInfo && DirectRequestModelDef(serverAddr, localAddr, commandPort, out modelDefPacket))
|
||
{
|
||
if (!SkipDataDescriptions && !DirectUpdateDefinitions(modelDefPacket))
|
||
Debug.LogWarning(GetType().FullName + ": direct NatNet MODELDEF parse failed. Falling back to synthetic skeleton definitions from incoming frames.", this);
|
||
}
|
||
else if (hasServerInfo && !m_directNoModelDefWarned)
|
||
{
|
||
Debug.LogWarning(GetType().FullName + ": direct NatNet MODELDEF request failed. Falling back to synthetic skeleton definitions from incoming frames.", this);
|
||
m_directNoModelDefWarned = true;
|
||
}
|
||
|
||
ServerNatNetVersion = natNetVersion[0] + "." + natNetVersion[1] + "." + natNetVersion[2] + "." + natNetVersion[3];
|
||
ClientNatNetVersion = "Direct UDP";
|
||
m_directNatNetConnected = true;
|
||
StartDirectFrameReceiver(localAddr.ToString(), negotiatedMulticast.ToString(), negotiatedDataPort);
|
||
Debug.Log(GetType().FullName + ": Connected to direct NatNet server. Server=" + serverAddr + ", host=" + hostName + ", local=" + localAddr + ", serverNatNet=" + ServerNatNetVersion + ", multicast=" + negotiatedMulticast + ":" + negotiatedDataPort + ".", this);
|
||
return true;
|
||
}
|
||
|
||
private bool DirectRequestServerInfo(IPAddress serverAddr, IPAddress localAddr, UInt16 commandPort, out string hostName, out byte[] appVersion, out byte[] natNetVersion, out UInt16 dataPort, out IPAddress multicastAddress)
|
||
{
|
||
hostName = "";
|
||
appVersion = new byte[4];
|
||
natNetVersion = new byte[4];
|
||
dataPort = 0;
|
||
multicastAddress = null;
|
||
|
||
byte[] response;
|
||
byte[] pingPayload = DirectBuildPingPayload();
|
||
if (!DirectCommandRequest(serverAddr, localAddr, commandPort, 0, pingPayload, 2000, out response)) // NAT_PING
|
||
return false;
|
||
if (response.Length < 4 || BitConverter.ToUInt16(response, 0) != 1) // NAT_PINGRESPONSE / server info
|
||
return false;
|
||
|
||
int payloadLen = BitConverter.ToUInt16(response, 2);
|
||
if (payloadLen < 268 || response.Length < 4 + payloadLen)
|
||
return false;
|
||
|
||
int o = 4;
|
||
hostName = DirectReadFixedString(response, o, 256);
|
||
o += 256;
|
||
Buffer.BlockCopy(response, o, appVersion, 0, 4); o += 4;
|
||
Buffer.BlockCopy(response, o, natNetVersion, 0, 4); o += 4;
|
||
|
||
// NatNet 4.x server info may append clock frequency, data port, multicast flag and multicast address.
|
||
if (response.Length >= o + 8)
|
||
o += 8;
|
||
if (response.Length >= o + 2)
|
||
{
|
||
dataPort = BitConverter.ToUInt16(response, o);
|
||
o += 2;
|
||
}
|
||
if (response.Length >= o + 1)
|
||
o += 1; // multicast flag
|
||
if (response.Length >= o + 4)
|
||
{
|
||
byte a = response[o], b = response[o + 1], c = response[o + 2], d = response[o + 3];
|
||
if (a != 0 || b != 0 || c != 0 || d != 0)
|
||
multicastAddress = new IPAddress(new byte[] { a, b, c, d });
|
||
}
|
||
return true;
|
||
}
|
||
|
||
private bool DirectRequestModelDef(IPAddress serverAddr, IPAddress localAddr, UInt16 commandPort, out byte[] modelDefPacket)
|
||
{
|
||
modelDefPacket = null;
|
||
byte[] response;
|
||
if (!DirectCommandRequest(serverAddr, localAddr, commandPort, 4, null, 3000, out response)) // NAT_REQUEST_MODELDEF
|
||
return false;
|
||
if (response.Length < 8 || BitConverter.ToUInt16(response, 0) != 5) // NAT_MODELDEF
|
||
return false;
|
||
modelDefPacket = response;
|
||
return true;
|
||
}
|
||
|
||
private bool DirectCommandRequest(IPAddress serverAddr, IPAddress localAddr, UInt16 commandPort, UInt16 messageId, byte[] payload, int timeoutMs, out byte[] response)
|
||
{
|
||
response = null;
|
||
try
|
||
{
|
||
using (UdpClient udp = new UdpClient())
|
||
{
|
||
udp.Client.ReceiveTimeout = timeoutMs;
|
||
udp.Client.Bind(new IPEndPoint(localAddr, 0));
|
||
byte[] packet = DirectBuildPacket(messageId, payload);
|
||
udp.Send(packet, packet.Length, new IPEndPoint(serverAddr, commandPort));
|
||
IPEndPoint remote = new IPEndPoint(IPAddress.Any, 0);
|
||
response = udp.Receive(ref remote);
|
||
return response != null && response.Length >= 4;
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
Debug.LogWarning(GetType().FullName + ": direct NatNet command " + messageId + " failed against " + serverAddr + ":" + commandPort + " (" + ex.Message + ").", this);
|
||
return false;
|
||
}
|
||
}
|
||
|
||
private static byte[] DirectBuildPacket(UInt16 messageId, byte[] payload)
|
||
{
|
||
int payloadLen = payload != null ? payload.Length : 0;
|
||
byte[] packet = new byte[4 + payloadLen];
|
||
Array.Copy(BitConverter.GetBytes(messageId), 0, packet, 0, 2);
|
||
Array.Copy(BitConverter.GetBytes((UInt16)payloadLen), 0, packet, 2, 2);
|
||
if (payloadLen > 0)
|
||
Buffer.BlockCopy(payload, 0, packet, 4, payloadLen);
|
||
return packet;
|
||
}
|
||
|
||
private static byte[] DirectBuildPingPayload()
|
||
{
|
||
byte[] payload = new byte[264];
|
||
byte[] name = Encoding.ASCII.GetBytes("UnityDirectNatNet");
|
||
Buffer.BlockCopy(name, 0, payload, 0, Math.Min(name.Length, 255));
|
||
payload[256] = 4;
|
||
payload[257] = 0;
|
||
payload[258] = 0;
|
||
payload[259] = 0;
|
||
payload[260] = 4;
|
||
payload[261] = 2;
|
||
payload[262] = 0;
|
||
payload[263] = 0;
|
||
return payload;
|
||
}
|
||
|
||
private bool DirectUpdateDefinitions(byte[] modelDefPacket)
|
||
{
|
||
if (modelDefPacket == null || modelDefPacket.Length < 8)
|
||
return false;
|
||
|
||
int payloadLen = BitConverter.ToUInt16(modelDefPacket, 2);
|
||
int end = Math.Min(modelDefPacket.Length, 4 + payloadLen);
|
||
int o = 4;
|
||
if (o + 4 > end)
|
||
return false;
|
||
|
||
List<OptitrackRigidBodyDefinition> rigidBodies = new List<OptitrackRigidBodyDefinition>();
|
||
List<OptitrackSkeletonDefinition> skeletons = new List<OptitrackSkeletonDefinition>();
|
||
List<OptitrackDeviceDefinition> devices = new List<OptitrackDeviceDefinition>();
|
||
|
||
int dataSetCount = BitConverter.ToInt32(modelDefPacket, o); o += 4;
|
||
for (int i = 0; i < dataSetCount && o + 8 <= end; i++)
|
||
{
|
||
int dataSetType = BitConverter.ToInt32(modelDefPacket, o); o += 4;
|
||
int dataSetSize = BitConverter.ToInt32(modelDefPacket, o); o += 4;
|
||
if (dataSetSize < 0 || o + dataSetSize > end)
|
||
return false;
|
||
|
||
int dataSetEnd = o + dataSetSize;
|
||
try
|
||
{
|
||
if (dataSetType == 1)
|
||
{
|
||
rigidBodies.Add(DirectReadRigidBodyDefinition(modelDefPacket, ref o, dataSetEnd));
|
||
}
|
||
else if (dataSetType == 2)
|
||
{
|
||
skeletons.Add(DirectReadSkeletonDefinition(modelDefPacket, ref o, dataSetEnd));
|
||
}
|
||
else if (dataSetType == 4)
|
||
{
|
||
devices.Add(DirectReadDeviceDefinition(modelDefPacket, ref o, dataSetEnd));
|
||
}
|
||
}
|
||
catch
|
||
{
|
||
return false;
|
||
}
|
||
|
||
o = dataSetEnd;
|
||
}
|
||
|
||
m_definitionRefreshCount = 0;
|
||
m_rigidBodyDefinitions.Clear();
|
||
m_skeletonDefinitions.Clear();
|
||
m_tmarkersetDefinitions.Clear();
|
||
m_mirrorBoneIdMaps.Clear();
|
||
m_rigidBodyDefinitions.AddRange(rigidBodies);
|
||
m_skeletonDefinitions.AddRange(skeletons);
|
||
|
||
lock (m_frameDataUpdateLock)
|
||
{
|
||
m_deviceDefinitions.Clear();
|
||
m_deviceNameToId.Clear();
|
||
m_latestDeviceStates.Clear();
|
||
m_assetIdToNameCache.Clear();
|
||
|
||
foreach (OptitrackDeviceDefinition deviceDef in devices)
|
||
{
|
||
m_deviceDefinitions.Add(deviceDef);
|
||
if (!string.IsNullOrEmpty(deviceDef.Name))
|
||
m_deviceNameToId[deviceDef.Name] = deviceDef.Id;
|
||
}
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
private static OptitrackRigidBodyDefinition DirectReadRigidBodyDefinition(byte[] data, ref int o, int end)
|
||
{
|
||
string name;
|
||
int id, parentId, markerCount;
|
||
Vector3 offset;
|
||
DirectReadRigidBodyDesc(data, ref o, end, out name, out id, out parentId, out offset, out markerCount);
|
||
|
||
OptitrackRigidBodyDefinition rbDef = new OptitrackRigidBodyDefinition
|
||
{
|
||
Id = id,
|
||
Name = name,
|
||
Markers = new List<OptitrackRigidBodyDefinition.MarkerDefinition>(Math.Max(0, markerCount)),
|
||
};
|
||
|
||
for (int i = 0; i < markerCount; i++)
|
||
rbDef.Markers.Add(new OptitrackRigidBodyDefinition.MarkerDefinition());
|
||
return rbDef;
|
||
}
|
||
|
||
private static OptitrackSkeletonDefinition DirectReadSkeletonDefinition(byte[] data, ref int o, int end)
|
||
{
|
||
string name = DirectReadCString(data, ref o, end);
|
||
int id = DirectReadInt32(data, ref o, end);
|
||
int boneCount = DirectReadInt32(data, ref o, end);
|
||
|
||
OptitrackSkeletonDefinition skelDef = new OptitrackSkeletonDefinition
|
||
{
|
||
Id = id,
|
||
Name = name,
|
||
Bones = new List<OptitrackSkeletonDefinition.BoneDefinition>(Math.Max(0, boneCount)),
|
||
BoneIdToParentIdMap = new Dictionary<int, int>(),
|
||
};
|
||
|
||
for (int i = 0; i < boneCount; i++)
|
||
{
|
||
string boneName;
|
||
int boneId, parentId, markerCount;
|
||
Vector3 offset;
|
||
DirectReadRigidBodyDesc(data, ref o, end, out boneName, out boneId, out parentId, out offset, out markerCount);
|
||
OptitrackSkeletonDefinition.BoneDefinition boneDef = new OptitrackSkeletonDefinition.BoneDefinition
|
||
{
|
||
Id = boneId,
|
||
ParentId = parentId,
|
||
Name = boneName,
|
||
Offset = offset,
|
||
};
|
||
skelDef.Bones.Add(boneDef);
|
||
skelDef.BoneIdToParentIdMap[boneDef.Id] = boneDef.ParentId;
|
||
}
|
||
|
||
return skelDef;
|
||
}
|
||
|
||
private static OptitrackDeviceDefinition DirectReadDeviceDefinition(byte[] data, ref int o, int end)
|
||
{
|
||
int id = DirectReadInt32(data, ref o, end);
|
||
string name = DirectReadCString(data, ref o, end);
|
||
string serial = DirectReadCString(data, ref o, end);
|
||
int deviceType = DirectReadInt32(data, ref o, end);
|
||
int channelDataType = DirectReadInt32(data, ref o, end);
|
||
int channelCount = DirectReadInt32(data, ref o, end);
|
||
|
||
OptitrackDeviceDefinition deviceDef = new OptitrackDeviceDefinition
|
||
{
|
||
Id = id,
|
||
Name = name,
|
||
SerialNumber = serial,
|
||
DeviceType = deviceType,
|
||
ChannelDataType = channelDataType,
|
||
ChannelCount = channelCount,
|
||
ChannelNames = new List<string>(Math.Max(0, channelCount)),
|
||
};
|
||
|
||
for (int i = 0; i < channelCount; i++)
|
||
deviceDef.ChannelNames.Add(DirectReadCString(data, ref o, end));
|
||
return deviceDef;
|
||
}
|
||
|
||
private static void DirectReadRigidBodyDesc(byte[] data, ref int o, int end, out string name, out int id, out int parentId, out Vector3 offset, out int markerCount)
|
||
{
|
||
name = DirectReadCString(data, ref o, end);
|
||
id = DirectReadInt32(data, ref o, end);
|
||
parentId = DirectReadInt32(data, ref o, end);
|
||
float x = DirectReadFloat(data, ref o, end);
|
||
float y = DirectReadFloat(data, ref o, end);
|
||
float z = DirectReadFloat(data, ref o, end);
|
||
offset = new Vector3(-x, y, z);
|
||
|
||
// Orientation offset is present in NatNet 4.x descriptions.
|
||
DirectReadFloat(data, ref o, end);
|
||
DirectReadFloat(data, ref o, end);
|
||
DirectReadFloat(data, ref o, end);
|
||
DirectReadFloat(data, ref o, end);
|
||
|
||
markerCount = DirectReadInt32(data, ref o, end);
|
||
int markerBytes = markerCount * (12 + 4);
|
||
if (markerCount < 0 || o + markerBytes > end)
|
||
throw new ArgumentOutOfRangeException("markerCount");
|
||
o += markerBytes;
|
||
for (int i = 0; i < markerCount; i++)
|
||
DirectReadCString(data, ref o, end);
|
||
}
|
||
|
||
private static int DirectReadInt32(byte[] data, ref int o, int end)
|
||
{
|
||
if (o + 4 > end) throw new ArgumentOutOfRangeException("o");
|
||
int v = BitConverter.ToInt32(data, o);
|
||
o += 4;
|
||
return v;
|
||
}
|
||
|
||
private static float DirectReadFloat(byte[] data, ref int o, int end)
|
||
{
|
||
if (o + 4 > end) throw new ArgumentOutOfRangeException("o");
|
||
float v = BitConverter.ToSingle(data, o);
|
||
o += 4;
|
||
return v;
|
||
}
|
||
|
||
private static string DirectReadCString(byte[] data, ref int o, int end)
|
||
{
|
||
if (o >= end) return "";
|
||
int start = o;
|
||
while (o < end && data[o] != 0)
|
||
o++;
|
||
string value = Encoding.UTF8.GetString(data, start, o - start);
|
||
if (o < end)
|
||
o++;
|
||
return value;
|
||
}
|
||
|
||
private static string DirectReadFixedString(byte[] data, int o, int maxLen)
|
||
{
|
||
int len = 0;
|
||
while (len < maxLen && o + len < data.Length && data[o + len] != 0)
|
||
len++;
|
||
return Encoding.UTF8.GetString(data, o, len);
|
||
}
|
||
|
||
private System.Collections.IEnumerator ConnectCoroutine()
|
||
{
|
||
m_receivedFrameSinceConnect = false;
|
||
|
||
// --- 硫붿씤 ?ㅻ젅?? 二쇱냼/紐⑤뱶 ?뚯떛 (Unity API 誘몄궗?? 利됱떆 ?꾨즺) ---
|
||
IPAddress serverAddr;
|
||
IPAddress localAddr;
|
||
IPAddress multicastAddr = null;
|
||
UInt16 commandPort;
|
||
UInt16 dataPort;
|
||
NatNetConnectionType connType;
|
||
try
|
||
{
|
||
serverAddr = IPAddress.Parse( ServerAddress );
|
||
commandPort = (UInt16)Mathf.Clamp( CommandPort, 1, 65535 );
|
||
dataPort = (UInt16)Mathf.Clamp( DataPort, 1, 65535 );
|
||
if ( !string.IsNullOrWhiteSpace( MulticastAddress ) )
|
||
multicastAddr = IPAddress.Parse( MulticastAddress.Trim() );
|
||
localAddr = ResolveLocalAddress( serverAddr );
|
||
ResolvedLocalAddress = localAddr.ToString();
|
||
connType = ConnectionType == ClientConnectionType.Unicast
|
||
? NatNetConnectionType.NatNetConnectionType_Unicast
|
||
: NatNetConnectionType.NatNetConnectionType_Multicast;
|
||
}
|
||
catch ( Exception ex )
|
||
{
|
||
Debug.LogException( ex, this );
|
||
Debug.LogError( GetType().FullName + ": Error parsing connection settings. Server=" + ServerAddress + ", command=" + CommandPort + ", data=" + DataPort + ", multicast=" + MulticastAddress + ".", this );
|
||
yield break;
|
||
}
|
||
|
||
if (ConnectionType == ClientConnectionType.Multicast)
|
||
{
|
||
IPAddress directMulticastAddr = multicastAddr ?? IPAddress.Parse("239.255.42.99");
|
||
if (!ConnectDirectNatNet(serverAddr, localAddr, commandPort, dataPort, directMulticastAddr))
|
||
{
|
||
Debug.LogError(GetType().FullName + ": Error connecting to direct NatNet server. Server=" + ServerAddress + ", local=" + localAddr + ", command=" + commandPort + ", data=" + dataPort + ", multicast=" + directMulticastAddr + ". Check Motive streaming, firewall, and network interface.", this);
|
||
yield break;
|
||
}
|
||
|
||
if (m_replayConnectCoroutine != null)
|
||
{
|
||
StopCoroutine(m_replayConnectCoroutine);
|
||
m_replayConnectCoroutine = null;
|
||
CleanupReplayClient();
|
||
}
|
||
|
||
if (RecordOnPlay && !m_isReconnecting)
|
||
Debug.LogWarning(GetType().FullName + ": RecordOnPlay requires the NatNet SDK command path and is skipped in direct multicast mode.", this);
|
||
|
||
m_connectionHealthCoroutine = StartCoroutine(CheckConnectionHealth());
|
||
yield break;
|
||
}
|
||
|
||
Debug.LogError(GetType().FullName + ": Unicast/NatNet SDK connection is disabled. This project now uses the direct UDP multicast NatNet path only.", this);
|
||
yield break;
|
||
|
||
#pragma warning disable 162
|
||
// --- 諛깃렇?쇱슫???ㅻ젅?? 釉붾줈???ㅼ씠?곕툕 ?곌껐 + SetProperty ---
|
||
// NatNet_Client_Connect ???쒕쾭媛 利됱떆 ?묐떟?섏? ?딆쑝硫??대? ??꾩븘?껉퉴吏 釉붾줈?밸맂??
|
||
// 肄붾（?댁? 硫붿씤 ?ㅻ젅?쒖뿉???ㅽ뻾?섎?濡??ш린??吏곸젒 ?몄텧?섎㈃ 留??ъ젒?띾쭏???붾㈃??硫덉텣??
|
||
// Task 濡?遺꾨━?섍퀬 硫붿씤 ?ㅻ젅?쒕뒗 留??꾨젅???묐낫(yield)?섎ʼn ?꾨즺瑜??대쭅?쒕떎.
|
||
bool applyServerSettings = !m_hasAppliedServerSettings;
|
||
NatNetClient connectedClient = null;
|
||
Exception connectError = null;
|
||
|
||
System.Threading.Tasks.Task connectTask = System.Threading.Tasks.Task.Run( () =>
|
||
{
|
||
NatNetClient c = null;
|
||
try
|
||
{
|
||
c = new NatNetClient();
|
||
if (commandPort == k_DefaultNatNetCommandPort &&
|
||
dataPort == k_DefaultNatNetDataPort &&
|
||
multicastAddr == null)
|
||
{
|
||
c.Connect( connType, localAddr, serverAddr );
|
||
}
|
||
else
|
||
{
|
||
c.Connect( connType, localAddr, serverAddr, commandPort, dataPort, multicastAddr );
|
||
}
|
||
|
||
// SetProperty??理쒖큹 ?곌껐?먯꽌留??꾩넚 ???ъ뿰寃???Motive 湲濡쒕쾶 ?ㅼ젙 諛섎났 蹂寃?諛⑹?
|
||
if ( applyServerSettings )
|
||
{
|
||
// Remotely change the Skeleton Coordinate property to Global/Local
|
||
if (SkeletonCoordinates == StreamingCoordinatesValues.Global)
|
||
c.RequestCommand("SetProperty,,Skeleton Coordinates,false");
|
||
else
|
||
c.RequestCommand("SetProperty,,Skeleton Coordinates,true");
|
||
|
||
// Remotely change the Bone Naming Convention to Motive/FBX/BVH
|
||
if (BoneNamingConvention == OptitrackBoneNameConvention.Motive)
|
||
c.RequestCommand("SetProperty,,Bone Naming Convention,0");
|
||
else if (BoneNamingConvention == OptitrackBoneNameConvention.FBX)
|
||
c.RequestCommand("SetProperty,,Bone Naming Convention,1");
|
||
else if (BoneNamingConvention == OptitrackBoneNameConvention.BVH)
|
||
c.RequestCommand("SetProperty,,Bone Naming Convention,2");
|
||
}
|
||
|
||
connectedClient = c;
|
||
}
|
||
catch ( Exception ex )
|
||
{
|
||
connectError = ex;
|
||
if ( c != null ) { try { c.Dispose(); } catch { } }
|
||
}
|
||
} );
|
||
|
||
// 硫붿씤 ?ㅻ젅?쒕뒗 留??꾨젅???묐낫?섎ʼn ?곌껐 ?꾨즺瑜??湲????뚮뜑留??낅젰??硫덉텛吏 ?딆쓬
|
||
while ( !connectTask.IsCompleted )
|
||
yield return null;
|
||
|
||
if ( connectError != null || connectedClient == null )
|
||
{
|
||
if ( connectError != null )
|
||
Debug.LogException( connectError, this );
|
||
Debug.LogError( GetType().FullName + ": Error connecting to NatNet server. Server=" + ServerAddress + ", local=" + localAddr + ", command=" + commandPort + ", data=" + dataPort + ", multicast=" + (multicastAddr != null ? multicastAddr.ToString() : "default") + ". Check Motive streaming, firewall, and network interface.", this );
|
||
yield break;
|
||
}
|
||
|
||
m_client = connectedClient;
|
||
if ( applyServerSettings )
|
||
m_hasAppliedServerSettings = true;
|
||
else
|
||
Debug.Log(GetType().FullName + ": reconnect skipped SetProperty commands; they were already applied on the first connection.", this);
|
||
|
||
// SetProperty 紐낅졊???쒕쾭???곸슜???뚭퉴吏 ?湲?(?ъ뿰寃???SetProperty ?ㅽ궢?덉쑝硫??湲?遺덊븘??
|
||
if (!m_isReconnecting)
|
||
yield return new UnityEngine.WaitForSeconds( 0.1f );
|
||
|
||
try
|
||
{
|
||
if (!SkipDataDescriptions)
|
||
UpdateDefinitions();
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
Debug.LogException(ex, this);
|
||
}
|
||
|
||
if (ConnectionType == ClientConnectionType.Unicast)
|
||
{
|
||
ResetStreamingSubscriptions();
|
||
foreach (KeyValuePair<Int32, MonoBehaviour> rb in m_rigidBodies)
|
||
SubscribeRigidBody(rb.Value, rb.Key);
|
||
foreach (KeyValuePair<string, MonoBehaviour> skel in m_skeletons)
|
||
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 ?뚯씪 諛섎났 ?닿린/?リ린 諛⑹?
|
||
if (RecordOnPlay && !m_isReconnecting)
|
||
StartRecording();
|
||
|
||
byte[] NatNetVersion = m_client.ServerDescription.NatNetVersion;
|
||
ServerNatNetVersion = NatNetVersion[0] + "." + NatNetVersion[1] + "." + NatNetVersion[2] + "." + NatNetVersion[3];
|
||
ClientNatNetVersion = "" + NatNetClient.NatNetLibVersion;
|
||
Debug.Log(GetType().FullName + ": Connected to NatNet server. Server=" + ServerAddress + ", local=" + ResolvedLocalAddress + ", serverNatNet=" + ServerNatNetVersion + ", clientNatNet=" + ClientNatNetVersion + ", connection=" + ConnectionType + ".", this);
|
||
|
||
m_client.NativeFrameReceived += OnNatNetFrameReceived;
|
||
m_connectionHealthCoroutine = StartCoroutine( CheckConnectionHealth() );
|
||
#pragma warning restore 162
|
||
}
|
||
|
||
/// <summary>
|
||
/// Disconnects from the streaming server and cleans up <see cref="m_client"/>.
|
||
/// </summary>
|
||
void OnDisable()
|
||
{
|
||
// ConnectCoroutine ?ы븿 紐⑤뱺 肄붾（???뺤?
|
||
StopAllCoroutines();
|
||
StopDirectFrameReceiver();
|
||
m_connectionHealthCoroutine = null;
|
||
m_replayConnectCoroutine = null;
|
||
m_isReconnecting = false;
|
||
|
||
CleanupReplayClient();
|
||
|
||
if (m_client != null)
|
||
{
|
||
if (RecordOnPlay)
|
||
StopRecording();
|
||
|
||
m_client.NativeFrameReceived -= OnNatNetFrameReceived;
|
||
try { m_client.Disconnect(); } catch { }
|
||
m_client.Dispose();
|
||
m_client = null;
|
||
}
|
||
}
|
||
|
||
System.Collections.IEnumerator CheckConnectionHealth()
|
||
{
|
||
const float kHealthCheckIntervalSeconds = 1.0f;
|
||
const float kRecentFrameThresholdSeconds = 5.0f;
|
||
|
||
// The lifespan of these variables is tied to the lifespan of a single connection session.
|
||
// The coroutine is stopped on disconnect and restarted on connect.
|
||
YieldInstruction checkIntervalYield = new WaitForSeconds( kHealthCheckIntervalSeconds );
|
||
OptitrackHiResTimer.Timestamp connectionInitiatedTimestamp = OptitrackHiResTimer.Now();
|
||
OptitrackHiResTimer.Timestamp lastFrameReceivedTimestamp;
|
||
bool wasReceivingFrames = false;
|
||
bool warnedPendingFirstFrame = false;
|
||
|
||
while ( true )
|
||
{
|
||
yield return checkIntervalYield;
|
||
|
||
if ( m_receivedFrameSinceConnect == false )
|
||
{
|
||
// Still waiting for first frame. Warn exactly once if this takes too long.
|
||
if ( connectionInitiatedTimestamp.AgeSeconds > kRecentFrameThresholdSeconds )
|
||
{
|
||
if ( warnedPendingFirstFrame == false )
|
||
{
|
||
if (m_directNatNetConnected)
|
||
Debug.LogWarning( GetType().FullName + ": Direct NatNet receiver is running but no frames have arrived in Unity yet. Check Unity Editor firewall permission, multicast routing, or temporarily disable separate NatNetDeviceListener objects.", this );
|
||
else
|
||
Debug.LogWarning( GetType().FullName + ": No frames received from the server yet. Verify your connection settings are correct and that the server is streaming.", this );
|
||
warnedPendingFirstFrame = true;
|
||
}
|
||
|
||
continue;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// We've received at least one frame, do ongoing checks for changes in connection health.
|
||
lastFrameReceivedTimestamp.m_ticks = Interlocked.Read( ref m_lastFrameDeliveryTimestamp.m_ticks );
|
||
bool receivedRecentFrame = lastFrameReceivedTimestamp.AgeSeconds < kRecentFrameThresholdSeconds;
|
||
|
||
if ( wasReceivingFrames == false && receivedRecentFrame == true )
|
||
{
|
||
// Transition: Bad health -> good health.
|
||
wasReceivingFrames = true;
|
||
Debug.Log( GetType().FullName + ": Receiving streaming data from the server.", this );
|
||
continue;
|
||
}
|
||
else if ( wasReceivingFrames == true && receivedRecentFrame == false )
|
||
{
|
||
// Transition: Good health -> bad health.
|
||
wasReceivingFrames = false;
|
||
Debug.LogWarning( GetType().FullName + ": No streaming frames received from the server recently.", this );
|
||
if ( AutoReconnect )
|
||
{
|
||
Debug.Log( GetType().FullName + ": starting automatic reconnect.", this );
|
||
Reconnect();
|
||
// OnDisable() ??StopAllCoroutines() 濡???肄붾（?댁? ?ㅼ쓬 yield ?먯꽌 以묒???
|
||
}
|
||
continue;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
#region Private methods
|
||
/// <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 )
|
||
{
|
||
bool isReplayFrame = ReferenceEquals(sender, m_replayClient);
|
||
if (!isReplayFrame && IsReplayFrameFresh())
|
||
{
|
||
return;
|
||
}
|
||
|
||
// 寃쏀빀 ??理쒕? 1ms ?湲????쒕∼.
|
||
// FillBoneSnapshot????蹂댁쑀 ?쒓컙??~0.1ms?대?濡??뺤긽 ?곹솴?먯꽌???쒕∼ ?놁쓬.
|
||
// GC Pause ??1ms 珥덇낵 ?곹솴?먯꽌留??쒕∼ (?덉슜 媛??.
|
||
if ( ! Monitor.TryEnter( m_frameDataUpdateLock, 1 ) )
|
||
{
|
||
return;
|
||
}
|
||
|
||
try
|
||
{
|
||
// Update health markers.
|
||
OptitrackHiResTimer.Timestamp frameTimestamp = OptitrackHiResTimer.Now();
|
||
if (isReplayFrame)
|
||
{
|
||
m_replayReceivedFrameSinceConnect = true;
|
||
Interlocked.Exchange(ref m_lastReplayFrameDeliveryTimestamp.m_ticks, frameTimestamp.m_ticks);
|
||
}
|
||
m_receivedFrameSinceConnect = true;
|
||
Interlocked.Exchange( ref m_lastFrameDeliveryTimestamp.m_ticks, frameTimestamp.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);
|
||
//Debug.Log(hour + "......" + minute + second + frameNumber + 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." );
|
||
|
||
// 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." );
|
||
|
||
// ?ㅼ펷?덊넠 ?뺤쓽 寃?됱쓣 蹂?猷⑦봽 諛뽰뿉??1?뚮쭔 ?섑뻾 (湲곗〈: 留?蹂몃쭏???좏삎 ?먯깋)
|
||
OptitrackSkeletonDefinition skelDef = GetSkeletonDefinitionById(skeletonId);
|
||
if (skelDef == null)
|
||
{
|
||
// Motive?먯꽌 以묎컙???ㅼ펷?덊넠???앹꽦??寃쎌슦 ??硫붿씤 ?ㅻ젅?쒖뿉???뺤쓽 ?ъ“???덉빟 (以묐났 濡쒓렇 諛⑹?)
|
||
if (!m_pendingDefinitionRefresh && m_definitionRefreshCount < k_MaxAutoDefinitionRefreshes)
|
||
{
|
||
Debug.LogWarning(GetType().FullName + ": missing skeleton definition for streamed skeleton ID " + skeletonId + "; scheduling definition refresh.", this);
|
||
m_pendingDefinitionRefresh = true;
|
||
}
|
||
continue;
|
||
}
|
||
|
||
// === ?ㅼ펷?덊넠 ?꾨젅???꾪꽣 (EnableSkeletonFrameFilter) ===
|
||
// Motive媛 鍮?遺遺??ㅼ펷?덊넠 ?섏씠濡쒕뱶瑜??대낫?????덈떎.
|
||
// ON : 蹂?媛쒖닔媛 ?ㅻⅤ硫??꾨젅???꾩껜 ?먭린 (吏곸쟾 ?ъ쫰 ?좎? ???⑤┝/遺遺꾪봽?덉엫 諛⑹?)
|
||
// OFF: ?ㅼ뼱??蹂?媛쒖닔留뚰겮 洹몃?濡?泥섎━ (紐⑥뀡 ?딄? 諛⑹?)
|
||
if (EnableSkeletonFrameFilter && skelRbCount != skelDef.Bones.Count)
|
||
{
|
||
continue;
|
||
}
|
||
|
||
sRigidBodyData[] stagedBones = GetSkeletonFrameScratch(skeletonId, skelRbCount);
|
||
for (int boneIdx = 0; boneIdx < skelRbCount; ++boneIdx)
|
||
{
|
||
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_Skeleton_GetRigidBody( pFrame, skelIdx, boneIdx, out stagedBones[boneIdx] );
|
||
NatNetException.ThrowIfNotOK( result, "NatNet_Frame_Skeleton_GetRigidBody failed." );
|
||
}
|
||
|
||
// ON ???뚮쭔: 蹂??섎굹?쇰룄 ?몃옒???ㅽ뙣/?먯긽?대㈃ ?꾨젅???꾩껜 ?먭린
|
||
if (EnableSkeletonFrameFilter)
|
||
{
|
||
bool isValidSkeletonFrame = true;
|
||
for (int boneIdx = 0; boneIdx < skelRbCount; ++boneIdx)
|
||
{
|
||
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) ||
|
||
!IsSkeletonBoneTracked(boneData) ||
|
||
!IsBoneDataUsable(boneData))
|
||
{
|
||
isValidSkeletonFrame = false;
|
||
break;
|
||
}
|
||
}
|
||
|
||
if (!isValidSkeletonFrame)
|
||
{
|
||
continue;
|
||
}
|
||
}
|
||
|
||
// === 湲濡쒕쾶 ?몃옖?ㅽ뤌 而ㅻ컠 ===
|
||
// ?꾪꽣 OFF ?먯꽌???먯긽(NaN쨌0荑쇳꽣?덉뼵)쨌誘몃ℓ??蹂몄? 媛쒕퀎濡?嫄대꼫?곗뼱 吏곸쟾 ?ъ쫰瑜?蹂댁〈?쒕떎.
|
||
for (int boneIdx = 0; boneIdx < skelRbCount; ++boneIdx)
|
||
{
|
||
sRigidBodyData boneData = stagedBones[boneIdx];
|
||
if (!TryGetCommittableBoneId(boneData, skeletonId, skelDef, out int boneId))
|
||
continue;
|
||
|
||
// 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;
|
||
}
|
||
|
||
// 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];
|
||
if (!TryGetCommittableBoneId(boneData, skeletonId, skelDef, out int boneId))
|
||
continue;
|
||
|
||
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);
|
||
|
||
Int32 pId = skelDef.BoneIdToParentIdMap[boneId];
|
||
if (pId != 0 && skelState.BonePoses.TryGetValue(pId, out OptitrackPose parentPose))
|
||
{
|
||
parentBonePos = parentPose.Position;
|
||
parentBoneOri = parentPose.Orientation;
|
||
}
|
||
skelState.LocalBonePoses[boneId].Position = bonePos - parentBonePos;
|
||
skelState.LocalBonePoses[boneId].Orientation = Quaternion.Inverse(parentBoneOri) * boneOri;
|
||
}
|
||
|
||
skelState.DeliveryTimestamp = frameTimestamp;
|
||
}
|
||
|
||
// -----------------------------------------------------
|
||
// - Update trained markerset // trained markerset added
|
||
// ----------------------------------------------------
|
||
// 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)
|
||
{
|
||
Int32 tmarkersetId = dataDescsSnapshot.AssetDescriptions[tmarkIdx].AssetID;
|
||
|
||
// Ensure we have a state corresponding to this tmarkerset ID.
|
||
OptitrackTMarkersetState tmarkState = GetOrCreateTMarkersetState(tmarkersetId);
|
||
|
||
// TMarkerset ?뺤쓽 寃?됱쓣 蹂?猷⑦봽 諛뽰뿉??1?뚮쭔 ?섑뻾 (湲곗〈: 留?蹂몃쭏???좏삎 ?먯깋)
|
||
Int32 tmarkRbCount = dataDescsSnapshot.AssetDescriptions[tmarkIdx].RigidBodyCount;
|
||
OptitrackTMarkersetDefinition tmarkDef = GetTMarkersetDefinitionById(tmarkersetId);
|
||
if (tmarkDef == null)
|
||
{
|
||
Debug.LogError(GetType().FullName + ": OnNatNetFrameReceived, no corresponding tmarkerset definition for received tmarkerset frame data.", this);
|
||
continue;
|
||
}
|
||
|
||
for (int boneIdx = 0; boneIdx < tmarkRbCount; ++boneIdx)
|
||
{
|
||
sRigidBodyData boneData = new sRigidBodyData();
|
||
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_TMarkerset_GetRigidBody(pFrame, tmarkIdx, boneIdx, out boneData);
|
||
NatNetException.ThrowIfNotOK(result, "NatNet_Frame_TMarkerset_GetRigidBody failed.");
|
||
|
||
Int32 boneTMarkId, boneId;
|
||
NaturalPoint.NatNetLib.NativeMethods.NatNet_DecodeID(boneData.Id, out boneTMarkId, out boneId);
|
||
|
||
if (tmarkState.BonePoses.ContainsKey(boneId) == false)
|
||
{
|
||
tmarkState.BonePoses[boneId] = new OptitrackPose();
|
||
}
|
||
if (tmarkState.LocalBonePoses.ContainsKey(boneId) == false)
|
||
{
|
||
tmarkState.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);
|
||
tmarkState.BonePoses[boneId].Position = bonePos;
|
||
tmarkState.BonePoses[boneId].Orientation = boneOri;
|
||
|
||
Vector3 parentBonePos = new Vector3(0, 0, 0);
|
||
Quaternion parentBoneOri = new Quaternion(0, 0, 0, 1);
|
||
|
||
Int32 pId = tmarkDef.BoneIdToParentIdMap[boneId];
|
||
if (pId != -1)
|
||
{
|
||
parentBonePos = tmarkState.BonePoses[pId].Position;
|
||
parentBoneOri = tmarkState.BonePoses[pId].Orientation;
|
||
}
|
||
tmarkState.LocalBonePoses[boneId].Position = bonePos - parentBonePos;
|
||
tmarkState.LocalBonePoses[boneId].Orientation = Quaternion.Inverse(parentBoneOri) * boneOri;
|
||
}
|
||
|
||
// --------------------------------------------
|
||
// - Update trained markerset markers
|
||
// --------------------------------------------
|
||
Int32 tmarkMarkerCount = dataDescsSnapshot.AssetDescriptions[tmarkIdx].MarkerCount;
|
||
/*result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_TMarkerset_GetMarkerCount(pFrame, tmarkIdx, out tmarkMarkerCount);
|
||
NatNetException.ThrowIfNotOK(result, "NatNet_Frame_TMarkerset_GetMarkerCount failed.");*/
|
||
//Debug.Log("tmark marker count: " + tmarkMarkerCount); // working finally
|
||
|
||
// Update Trained Markerset Marker data
|
||
for (int markerIdx = 0; markerIdx < tmarkMarkerCount; ++markerIdx)
|
||
{
|
||
sMarker tmarker = new sMarker(); // markerData
|
||
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_TMarkerset_GetMarker(pFrame, tmarkIdx, markerIdx, out tmarker);
|
||
NatNetException.ThrowIfNotOK(result, "NatNet_Frame_TMarkerset_GetMarker failed.");
|
||
//Debug.Log("result: " + result);
|
||
|
||
// Flip coordinate handedness
|
||
OptitrackMarkerState tmarkerState = GetOrCreateTMarkMarkerState(tmarker.Id);
|
||
tmarkerState.Name = GetMarkerName(tmarker);
|
||
tmarkerState.Position = new Vector3(-tmarker.X, tmarker.Y, tmarker.Z);
|
||
tmarkerState.Size = tmarker.Size;
|
||
tmarkerState.Labeled = (tmarker.Params & 0x10) == 0;
|
||
tmarkerState.Id = tmarker.Id;
|
||
tmarkerState.IsActive = (tmarker.Params & 0x20) != 0;
|
||
}
|
||
|
||
}
|
||
|
||
// ----------------------
|
||
// - Update markers
|
||
// ----------------------
|
||
Int32 MarkerCount;
|
||
result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetLabeledMarkerCount( pFrame, out MarkerCount );
|
||
NatNetException.ThrowIfNotOK( result, "NatNet_Frame_GetLabeledMarkerCount failed.");
|
||
|
||
m_latestMarkerStates.Clear();
|
||
//Debug.Log("marker count: " + MarkerCount);
|
||
|
||
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.Name = GetMarkerName( marker );
|
||
markerState.Position = new Vector3( -marker.X, marker.Y, marker.Z );
|
||
markerState.Size = marker.Size;
|
||
markerState.Labeled = (marker.Params & 0x10) == 0;
|
||
markerState.Id = marker.Id;
|
||
markerState.IsActive = (marker.Params & 0x20) != 0;
|
||
}
|
||
|
||
// ----------------------------------
|
||
// - Update analog devices (iFacialMocap face, NIDAQ, etc.)
|
||
//
|
||
// We CANNOT use eventArgs.MarshaledFrame here ??that triggers a full
|
||
// sFrameOfMocapData marshal which throws on the fixed-size MarkerSets[]
|
||
// array (uninitialized bytes in unused slots fail ANSI string decoding).
|
||
//
|
||
// Instead: unsafe pointer arithmetic to read DeviceCount + each sDeviceData
|
||
// directly from native memory, skipping the problematic sections entirely.
|
||
// sDeviceData / sAnalogChannelData have no string fields, so PtrToStructure
|
||
// on individual entries is safe.
|
||
// ----------------------------------
|
||
if (m_deviceDefinitions.Count > 0)
|
||
{
|
||
ProcessDeviceData(eventArgs.NativeFramePointer, frameTimestamp);
|
||
}
|
||
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
Debug.LogError( GetType().FullName + ": OnNatNetFrameReceived encountered an exception.", this );
|
||
Debug.LogException( ex, this );
|
||
}
|
||
finally
|
||
{
|
||
Monitor.Exit( m_frameDataUpdateLock );
|
||
}
|
||
}
|
||
|
||
// Cached struct offsets/sizes for ProcessDeviceData. Computed once via reflection,
|
||
// then reused ??Marshal.OffsetOf / SizeOf don't allocate on subsequent calls.
|
||
private static int s_DeviceCountOffset = -1;
|
||
private static int s_DevicesArrayOffset;
|
||
private static int s_DeviceDataSize;
|
||
private static int s_FrameNumberOffset;
|
||
|
||
private static void EnsureDeviceOffsetsInitialized()
|
||
{
|
||
if (s_DeviceCountOffset >= 0) return;
|
||
s_DeviceCountOffset = (int)Marshal.OffsetOf(typeof(sFrameOfMocapData), "DeviceCount");
|
||
s_DevicesArrayOffset = (int)Marshal.OffsetOf(typeof(sFrameOfMocapData), "Devices");
|
||
s_FrameNumberOffset = (int)Marshal.OffsetOf(typeof(sFrameOfMocapData), "FrameNumber");
|
||
s_DeviceDataSize = Marshal.SizeOf(typeof(sDeviceData));
|
||
}
|
||
|
||
private void ProcessDeviceData(IntPtr pFrame, OptitrackHiResTimer.Timestamp frameTimestamp)
|
||
{
|
||
if (pFrame == IntPtr.Zero) return;
|
||
EnsureDeviceOffsetsInitialized();
|
||
|
||
int deviceCount = Marshal.ReadInt32(pFrame, s_DeviceCountOffset);
|
||
|
||
if (deviceCount <= 0) return;
|
||
if (deviceCount > k_MaxNatNetDevices) deviceCount = k_MaxNatNetDevices;
|
||
|
||
int frameNumber = Marshal.ReadInt32(pFrame, s_FrameNumberOffset);
|
||
|
||
for (int devIdx = 0; devIdx < deviceCount; ++devIdx)
|
||
{
|
||
IntPtr pDevice = IntPtr.Add(pFrame, s_DevicesArrayOffset + devIdx * s_DeviceDataSize);
|
||
sDeviceData devData = (sDeviceData)Marshal.PtrToStructure(pDevice, typeof(sDeviceData));
|
||
|
||
OptitrackDeviceDefinition devDef = GetDeviceDefinitionById(devData.Id);
|
||
if (devDef == null) continue;
|
||
|
||
OptitrackDeviceState devState = GetOrCreateDeviceState(devData.Id);
|
||
devState.Name = devDef.Name;
|
||
devState.FrameNumber = frameNumber;
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
|
||
private OptitrackDeviceDefinition GetDeviceDefinitionById( Int32 id )
|
||
{
|
||
for (int i = 0; i < m_deviceDefinitions.Count; ++i)
|
||
if (m_deviceDefinitions[i].Id == id) return m_deviceDefinitions[i];
|
||
return null;
|
||
}
|
||
|
||
private OptitrackDeviceState GetOrCreateDeviceState( Int32 id )
|
||
{
|
||
if (m_latestDeviceStates.TryGetValue(id, out OptitrackDeviceState state))
|
||
return state;
|
||
state = new OptitrackDeviceState
|
||
{
|
||
Id = id,
|
||
ChannelValues = new Dictionary<string, float>(),
|
||
};
|
||
m_latestDeviceStates[id] = state;
|
||
return state;
|
||
}
|
||
|
||
/// <summary>Get the latest channel values for a device by ID.</summary>
|
||
public OptitrackDeviceState GetLatestDeviceState( Int32 deviceId )
|
||
{
|
||
Monitor.Enter( m_frameDataUpdateLock );
|
||
try
|
||
{
|
||
return m_latestDeviceStates.TryGetValue(deviceId, out var state) ? CopyDeviceState(state) : null;
|
||
}
|
||
finally
|
||
{
|
||
Monitor.Exit( m_frameDataUpdateLock );
|
||
}
|
||
}
|
||
|
||
/// <summary>Get the latest channel values for a device by Motive name (e.g. "iFacialMocap_40001").</summary>
|
||
public OptitrackDeviceState GetLatestDeviceState( string deviceName )
|
||
{
|
||
Monitor.Enter( m_frameDataUpdateLock );
|
||
try
|
||
{
|
||
if (m_deviceNameToId.TryGetValue(deviceName, out Int32 id) &&
|
||
m_latestDeviceStates.TryGetValue(id, out var state))
|
||
return CopyDeviceState(state);
|
||
return null;
|
||
}
|
||
finally
|
||
{
|
||
Monitor.Exit( m_frameDataUpdateLock );
|
||
}
|
||
}
|
||
|
||
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 = source.ChannelValues != null
|
||
? new Dictionary<string, float>(source.ChannelValues)
|
||
: new Dictionary<string, float>(),
|
||
};
|
||
}
|
||
|
||
/// <summary>Returns snapshots of all latest NatNet device samples, including direct UDP fallback devices without MODELDEF names.</summary>
|
||
public List<OptitrackDeviceState> GetLatestDeviceStates()
|
||
{
|
||
lock (m_frameDataUpdateLock)
|
||
{
|
||
var snapshots = new List<OptitrackDeviceState>(m_latestDeviceStates.Count);
|
||
foreach (var kvp in m_latestDeviceStates)
|
||
snapshots.Add(CopyDeviceState(kvp.Value));
|
||
return snapshots;
|
||
}
|
||
}
|
||
|
||
/// <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()
|
||
{
|
||
Monitor.Enter( m_frameDataUpdateLock );
|
||
try
|
||
{
|
||
return new List<OptitrackDeviceDefinition>(m_deviceDefinitions);
|
||
}
|
||
finally
|
||
{
|
||
Monitor.Exit( m_frameDataUpdateLock );
|
||
}
|
||
}
|
||
|
||
private string GetMarkerName( sMarker marker )
|
||
{
|
||
int assetID = marker.Id >> 16; // high word = Asset ID Number
|
||
int memberID = marker.Id & 0x00ffff; // low word = Member ID Number (constraint number)
|
||
|
||
// assetID?믪씠由?罹먯떆 ?ъ슜 (湲곗〈: 留?留덉빱留덈떎 3媛?由ъ뒪???좏삎 ?먯깋)
|
||
string assetName;
|
||
if (!m_assetIdToNameCache.TryGetValue(assetID, out assetName))
|
||
{
|
||
assetName = "";
|
||
OptitrackRigidBodyDefinition rigidBodyDef = GetRigidBodyDefinitionById( assetID );
|
||
if (rigidBodyDef != null)
|
||
assetName = rigidBodyDef.Name;
|
||
else
|
||
{
|
||
OptitrackSkeletonDefinition skeletonDef = GetSkeletonDefinitionById( assetID );
|
||
if (skeletonDef != null)
|
||
assetName = skeletonDef.Name;
|
||
else
|
||
{
|
||
OptitrackTMarkersetDefinition tmarkersetDef = GetTMarkersetDefinitionById( assetID );
|
||
if (tmarkersetDef != null)
|
||
assetName = tmarkersetDef.Name;
|
||
}
|
||
}
|
||
m_assetIdToNameCache[assetID] = assetName;
|
||
}
|
||
|
||
bool IsLabeled = (marker.Params & 0x10) == 0;
|
||
bool IsActive = (marker.Params & 0x20) != 0;
|
||
|
||
if (IsActive && !IsLabeled)
|
||
return "Active " + marker.Id.ToString();
|
||
else if (IsActive && IsLabeled)
|
||
return "Active " + marker.Id.ToString() + " (" + assetName + " Member ID: " + memberID + " )";
|
||
else if (!IsActive && !IsLabeled)
|
||
return "Passive " + marker.Id.ToString();
|
||
else
|
||
return "Passive (" + assetName + " Member ID: " + memberID + ")";
|
||
}
|
||
|
||
private void RigidBodyDataToState(sRigidBodyData rbData, OptitrackHiResTimer.Timestamp timestamp, OptitrackRigidBodyState rbState)
|
||
{
|
||
rbState.DeliveryTimestamp = timestamp;
|
||
rbState.Pose = new OptitrackPose
|
||
{
|
||
Position = new Vector3(-rbData.X, rbData.Y, rbData.Z),
|
||
Orientation = new Quaternion(-rbData.QX, rbData.QY, rbData.QZ, -rbData.QW),
|
||
};
|
||
rbState.IsTracked = (rbData.Params & 0x01) != 0;
|
||
}
|
||
|
||
private void ResetStreamingSubscriptions()
|
||
{
|
||
// 1媛?紐낅졊?쇰줈 ?듯빀: "SubscribeToData"??紐⑤뱺 ?꾪꽣瑜??대━?댄븯怨?湲곕낯 ?곹깭(援щ룆 ?놁쓬)濡?由ъ뀑
|
||
m_client.RequestCommand( "SubscribeToData" );
|
||
}
|
||
|
||
private void SubscribeRigidBody( MonoBehaviour component, Int32 rigidBodyId )
|
||
{
|
||
if ( m_client != null && ConnectionType == ClientConnectionType.Unicast )
|
||
{
|
||
// Try subscribing up to 3 times with a 2000 ms timeout before giving up.
|
||
bool subscribeSucceeded = m_client.RequestCommand( "SubscribeByID,RigidBody," + rigidBodyId, 2000, 3 );
|
||
|
||
// Log a warning on the first failure.
|
||
if ( ! subscribeSucceeded && ! m_doneSubscriptionNotice )
|
||
{
|
||
if ( m_client.ServerDescription.HostApp == "Motive" )
|
||
{
|
||
// Host app is Motive: If new enough to support subscription, failure is an error.
|
||
// Otherwise, warn them that they may want to update Motive to reduce bandwidth consumption.
|
||
if ( m_client.ServerAppVersion >= new Version(2, 2, 0) )
|
||
{
|
||
Debug.LogError( "Failed to subscribe to rigid body streaming data for component", component);
|
||
}
|
||
else
|
||
{
|
||
Debug.LogWarning("Your version of Motive is too old to support NatNet rigid body data subscription; streaming bandwidth consumption may be higher than necessary. This feature works in Motive 2.2.0+.");
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// Not Motive, we don't know whether it "should" support this. Warning instead of error.
|
||
Debug.LogWarning( "Failed to subscribe to rigid body streaming data for component", component );
|
||
}
|
||
|
||
m_doneSubscriptionNotice = true;
|
||
}
|
||
}
|
||
}
|
||
|
||
private void SubscribeSkeleton(MonoBehaviour component, string name )
|
||
{
|
||
if (m_client != null && ConnectionType == ClientConnectionType.Unicast)
|
||
{
|
||
if (m_client.ServerAppVersion >= new Version(2, 2, 1))
|
||
{
|
||
// Try subscribing up to 3 times with a 2000 ms timeout before giving up.
|
||
bool subscribeSucceeded = m_client.RequestCommand("SubscribeToData,Skeleton," + name, 2000, 3);
|
||
|
||
// Log a warning on the first failure.
|
||
if (!subscribeSucceeded && !m_doneSubscriptionNotice)
|
||
{
|
||
Debug.LogError("Failed to subscribe to skeleton streaming data for component", component);
|
||
m_doneSubscriptionNotice = true;
|
||
}
|
||
}
|
||
else if (m_client.ServerAppVersion == new Version(2, 2, 0, 0))
|
||
{
|
||
// Motive 2.2.0 has a bug were Motive says it subscribes successfully, but doesn't.
|
||
// Subscribing to all skeletons still works, so for this version that is done instead.
|
||
|
||
// Try subscribing up to 3 times with a 2000 ms timeout before giving up.
|
||
bool subscribeSucceeded = m_client.RequestCommand("SubscribeToData,Skeleton,All" + name, 2000, 3);
|
||
|
||
if (!subscribeSucceeded && !m_doneSubscriptionNotice)
|
||
{
|
||
Debug.LogError("Failed to subscribe to all skeletons streaming data some unknown reason.", component);
|
||
m_doneSubscriptionNotice = true;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
Debug.LogWarning("Your version of Motive is too old to support NatNet skeleton data subscription; streaming bandwidth consumption may be higher than necessary. This feature works in Motive 2.2.1+.");
|
||
m_doneSubscriptionNotice = true;
|
||
}
|
||
}
|
||
}
|
||
|
||
private void SubscribeTMarkerset(MonoBehaviour component, string name) // check the version numbers // trained markerset added
|
||
{
|
||
if (m_client != null && ConnectionType == ClientConnectionType.Unicast)
|
||
{
|
||
if (m_client.ServerAppVersion >= new Version(2, 2, 1))
|
||
{
|
||
// Try subscribing up to 3 times with a 2000 ms timeout before giving up.
|
||
bool subscribeSucceeded = m_client.RequestCommand("SubscribeToData,TrainedMarkersets," + name, 2000, 3);
|
||
|
||
// Log a warning on the first failure.
|
||
if (!subscribeSucceeded && !m_doneSubscriptionNotice)
|
||
{
|
||
Debug.LogError("Failed to subscribe to trained markerset streaming data for component", component);
|
||
m_doneSubscriptionNotice = true;
|
||
}
|
||
}
|
||
|
||
else if (m_client.ServerAppVersion == new Version(2, 2, 0, 0))
|
||
{
|
||
// Motive 2.2.0 has a bug were Motive says it subscribes successfully, but doesn't.
|
||
// Subscribing to all skeletons still works, so for this version that is done instead.
|
||
|
||
// Try subscribing up to 3 times with a 2000 ms timeout before giving up.
|
||
bool subscribeSucceeded = m_client.RequestCommand("SubscribeToData,TrainedMarkersets,All" + name, 2000, 3);
|
||
|
||
if (!subscribeSucceeded && !m_doneSubscriptionNotice)
|
||
{
|
||
Debug.LogError("Failed to subscribe to all trained markersets streaming data some unknown reason.", component);
|
||
m_doneSubscriptionNotice = true;
|
||
}
|
||
}
|
||
|
||
else
|
||
{
|
||
Debug.LogWarning("Your version of Motive is too old to support NatNet skeleton data subscription; streaming bandwidth consumption may be higher than necessary. This feature works in Motive 2.2.1+.");
|
||
m_doneSubscriptionNotice = true;
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
private void SubscribeTMarkMarkers()
|
||
{
|
||
if (m_client != null && ConnectionType == ClientConnectionType.Unicast)
|
||
{
|
||
bool subscribeSucceeded4 = m_client.RequestCommand("SubscribeToData,TrainedMarkersetMarkers,All", 2000, 3);
|
||
//Debug.Log("TMMarkers: " + subscribeSucceeded4);
|
||
|
||
// Log a warning on the first failure.
|
||
if (!subscribeSucceeded4 && !m_doneSubscriptionNotice)
|
||
{
|
||
if (m_client.ServerDescription.HostApp == "Motive")
|
||
{
|
||
// Host app is Motive: If new enough to support subscription, failure is an error.
|
||
// Otherwise, warn them that they may want to update Motive to reduce bandwidth consumption.
|
||
if (m_client.ServerAppVersion >= new Version(2, 2, 0))
|
||
{
|
||
Debug.LogError("Failed to subscribe to tmark marker streaming data");
|
||
}
|
||
else
|
||
{
|
||
Debug.LogWarning("Your version of Motive is too old to support NatNet tmark marker data subscription; streaming bandwidth consumption may be higher than necessary. This feature works in Motive 2.2.0+.");
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// Not Motive, we don't know whether it "should" support this. Warning instead of error.
|
||
Debug.LogWarning("Failed to subscribe to tmark marker streaming data");
|
||
}
|
||
|
||
m_doneSubscriptionNotice = true;
|
||
}
|
||
}
|
||
}
|
||
|
||
private void SubscribeMarkers( )
|
||
{
|
||
if (m_client != null && ConnectionType == ClientConnectionType.Unicast)
|
||
{
|
||
// Try subscribing up to 3 times with a 2000 ms timeout before giving up.
|
||
bool subscribeSucceeded = m_client.RequestCommand("SubscribeToData,MarkerSetMarkers,All", 2000, 3);
|
||
bool subscribeSucceeded2 = m_client.RequestCommand("SubscribeToData,LabeledMarkers,All", 2000, 3);
|
||
bool subscribeSucceeded3 = m_client.RequestCommand("SubscribeToData,LegacyUnlabeledMarkers,All", 2000, 3);
|
||
//bool subscribeSucceeded4 = m_client.RequestCommand("SubscribeToData, TrainedMarkersetMarkers,All", 2000, 3);
|
||
//bool allSubscribeSucceeded = subscribeSucceeded4;
|
||
bool allSubscribeSucceeded = subscribeSucceeded && subscribeSucceeded2 && subscribeSucceeded3;
|
||
m_subscribedToMarkers = allSubscribeSucceeded;
|
||
|
||
// Log a warning on the first failure.
|
||
if (!allSubscribeSucceeded && !m_doneSubscriptionNotice)
|
||
{
|
||
if (m_client.ServerDescription.HostApp == "Motive")
|
||
{
|
||
// Host app is Motive: If new enough to support subscription, failure is an error.
|
||
// Otherwise, warn them that they may want to update Motive to reduce bandwidth consumption.
|
||
if (m_client.ServerAppVersion >= new Version(2, 2, 0))
|
||
{
|
||
Debug.LogError("Failed to subscribe to marker streaming data");
|
||
}
|
||
else
|
||
{
|
||
Debug.LogWarning("Your version of Motive is too old to support NatNet rigid body data subscription; streaming bandwidth consumption may be higher than necessary. This feature works in Motive 2.2.0+.");
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// Not Motive, we don't know whether it "should" support this. Warning instead of error.
|
||
Debug.LogWarning("Failed to subscribe to marker streaming data");
|
||
}
|
||
|
||
m_doneSubscriptionNotice = true;
|
||
}
|
||
}
|
||
}
|
||
|
||
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, Mathf.Clamp(CommandPort, 1, 65535)));
|
||
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 + ": failed to auto-detect local address. Falling back to LocalAddress. " + 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;
|
||
}
|
||
|
||
/// <summary>Motive "???꾨젅???몃옒?밸맖" 鍮꾪듃(0x01). ?꾧꺽 ?꾪꽣(ON)?먯꽌留??꾨젅???먭린 議곌굔?쇰줈 ?곗씤??</summary>
|
||
private static bool IsSkeletonBoneTracked(sRigidBodyData boneData)
|
||
{
|
||
return (boneData.Params & 0x01) != 0;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 蹂??곗씠?곌? ?ъ쫰濡?而ㅻ컠?대룄 ?덉쟾?쒖? 寃?ы븳??醫뚰몴 ?좏븳 + 荑쇳꽣?덉뼵 ?뺤긽).
|
||
/// ?몃옒??鍮꾪듃? 臾닿? ???꾪꽣 OFF?먯꽌???먯긽 蹂몄씠 吏곸쟾 ?ъ쫰瑜???뼱?곗? 紐삵븯寃?留됰뒗 ?덉쟾?μ튂.
|
||
/// </summary>
|
||
private static bool IsBoneDataUsable(sRigidBodyData boneData)
|
||
{
|
||
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;
|
||
}
|
||
|
||
/// <summary>
|
||
/// ?꾨젅??蹂??곗씠?곌? ???ㅼ펷?덊넠??而ㅻ컠 媛?ν븳吏 寃?ы븯怨?boneId瑜?異쒕젰?쒕떎.
|
||
/// ?ㅼ펷?덊넠 ID ?쇱튂 + ?뺤쓽??留ㅽ븨??蹂?+ ?곗씠???뺤긽(NaN/0-荑쇳꽣?덉뼵 ?꾨떂)???뚮쭔 true.
|
||
/// (?꾪꽣 ON/OFF 怨듯넻?쇰줈 而ㅻ컠 ?④퀎?먯꽌 ?ъ슜 ??OFF?먯꽌 ?먯긽쨌誘몃ℓ??蹂?媛쒕퀎 ?ㅽ궢, 2李??⑥뒪 KeyNotFound 諛⑹?)
|
||
/// </summary>
|
||
private static bool TryGetCommittableBoneId(sRigidBodyData boneData, Int32 skeletonId, OptitrackSkeletonDefinition skelDef, out int boneId)
|
||
{
|
||
Int32 boneSkelId;
|
||
NaturalPoint.NatNetLib.NativeMethods.NatNet_DecodeID( boneData.Id, out boneSkelId, out boneId );
|
||
return boneSkelId == skeletonId
|
||
&& skelDef.BoneIdToParentIdMap.ContainsKey(boneId)
|
||
&& IsBoneDataUsable(boneData);
|
||
}
|
||
|
||
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"/>.
|
||
/// 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="rigidBodyId">The ID of the rigid body for which to retrieve the corresponding state.</param>
|
||
/// <returns>The existing state object, or a newly created one if necessary.</returns>
|
||
private OptitrackRigidBodyState GetOrCreateRigidBodyState( Int32 rigidBodyId )
|
||
{
|
||
OptitrackRigidBodyState returnedState = null;
|
||
|
||
if ( m_latestRigidBodyStates.ContainsKey( rigidBodyId ) )
|
||
{
|
||
returnedState = m_latestRigidBodyStates[rigidBodyId];
|
||
}
|
||
else
|
||
{
|
||
OptitrackRigidBodyState newRbState = new OptitrackRigidBodyState {
|
||
Pose = new OptitrackPose(),
|
||
};
|
||
|
||
m_latestRigidBodyStates[rigidBodyId] = newRbState;
|
||
|
||
returnedState = newRbState;
|
||
}
|
||
|
||
return returnedState;
|
||
}
|
||
|
||
/// <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>(),
|
||
LocalBonePoses = new Dictionary<int, OptitrackPose>(),
|
||
};
|
||
|
||
m_latestSkeletonStates[skeletonId] = newSkeletonState;
|
||
|
||
returnedState = newSkeletonState;
|
||
}
|
||
|
||
return returnedState;
|
||
}
|
||
|
||
private OptitrackTMarkersetState GetOrCreateTMarkersetState(Int32 tmarkersetId)
|
||
{
|
||
OptitrackTMarkersetState returnedState = null;
|
||
|
||
if (m_latestTMarkersetStates.ContainsKey(tmarkersetId))
|
||
{
|
||
returnedState = m_latestTMarkersetStates[tmarkersetId];
|
||
}
|
||
else
|
||
{
|
||
OptitrackTMarkersetState newTMarkersetState = new OptitrackTMarkersetState
|
||
{
|
||
BonePoses = new Dictionary<Int32, OptitrackPose>(),
|
||
LocalBonePoses = new Dictionary<int, OptitrackPose>(),
|
||
};
|
||
|
||
m_latestTMarkersetStates[tmarkersetId] = newTMarkersetState;
|
||
|
||
returnedState = newTMarkersetState;
|
||
}
|
||
|
||
return returnedState;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Returns the <see cref="OptitrackMarkerState"/> corresponding to the provided <paramref name="markerId"/>.
|
||
/// 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="markerId">The ID of the bone in trained markerset for which to retrieve the corresponding state.</param>
|
||
/// <returns>The existing state object, or a newly created one if necessary.</returns>
|
||
private OptitrackMarkerState GetOrCreateTMarkMarkerState(Int32 markerId)
|
||
{
|
||
OptitrackMarkerState returnedState = null;
|
||
|
||
if (m_latestTMarkMarkerStates.ContainsKey(markerId))
|
||
{
|
||
returnedState = m_latestTMarkMarkerStates[markerId];
|
||
}
|
||
else
|
||
{
|
||
OptitrackMarkerState newMarkerState = new OptitrackMarkerState
|
||
{
|
||
Position = new Vector3(),
|
||
};
|
||
|
||
m_latestTMarkMarkerStates[markerId] = newMarkerState;
|
||
|
||
returnedState = newMarkerState;
|
||
}
|
||
|
||
//Debug.Log(returnedState);
|
||
return returnedState;
|
||
}
|
||
|
||
/// <summary>
|
||
/// Returns the <see cref="OptitrackMarkerState"/> corresponding to the provided <paramref name="markerId"/>.
|
||
/// 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="markerId">The ID of the rigid body for which to retrieve the corresponding state.</param>
|
||
/// <returns>The existing state object, or a newly created one if necessary.</returns>
|
||
private OptitrackMarkerState GetOrCreateMarkerState(Int32 markerId)
|
||
{
|
||
OptitrackMarkerState returnedState = null;
|
||
|
||
if (m_latestMarkerStates.ContainsKey( markerId ))
|
||
{
|
||
returnedState = m_latestMarkerStates[markerId];
|
||
}
|
||
else
|
||
{
|
||
OptitrackMarkerState newMarkerState = new OptitrackMarkerState
|
||
{
|
||
Position = new Vector3(),
|
||
};
|
||
|
||
m_latestMarkerStates[markerId] = newMarkerState;
|
||
|
||
returnedState = newMarkerState;
|
||
}
|
||
|
||
return returnedState;
|
||
}
|
||
|
||
/// <summary>
|
||
/// ?대? ?꾨젅???곗씠????吏꾩엯. 諛섎뱶??try-finally ?⑦꽩?쇰줈 _ExitFrameDataUpdateLock()怨??띿쑝濡??ъ슜?섏꽭??
|
||
/// Exit ?놁씠 ?몄텧?섎㈃ NatNet ?ㅻ젅?쒓? ?곴뎄 ?곕뱶?쎈맗?덈떎.
|
||
/// </summary>
|
||
[System.Obsolete("吏곸젒 ??議곗옉 ???FillBoneSnapshot() ???ㅻ젅???덉쟾 API瑜??ъ슜?섏꽭??")]
|
||
internal void _EnterFrameDataUpdateLock()
|
||
{
|
||
Monitor.Enter( m_frameDataUpdateLock );
|
||
}
|
||
|
||
/// <summary>
|
||
/// ?대? ?꾨젅???곗씠?????댁젣. 諛섎뱶??_EnterFrameDataUpdateLock()怨??띿쑝濡??ъ슜?섏꽭??
|
||
/// </summary>
|
||
[System.Obsolete("吏곸젒 ??議곗옉 ???FillBoneSnapshot() ???ㅻ젅???덉쟾 API瑜??ъ슜?섏꽭??")]
|
||
internal void _ExitFrameDataUpdateLock()
|
||
{
|
||
Monitor.Exit( m_frameDataUpdateLock );
|
||
}
|
||
#endregion Private methods
|
||
}
|