void Update() { OptitrackRigidBodyState rbState = StreamingClient.GetLatestRigidBodyState(RigidBodyId); if (rbState != null) { this.transform.localPosition = rbState.Pose.Position; this.transform.localRotation = rbState.Pose.Orientation; if (localToParent) { this.transform.localPosition = transform.parent.InverseTransformPoint(this.transform.localPosition); this.transform.localRotation = Quaternion.Inverse(transform.parent.localRotation) * this.transform.localRotation; } if (markers == null || markers.Length != rbState.Markers.Count) { markers = new Vector3[rbState.Markers.Count]; markerSize = new float[rbState.Markers.Count]; } try { for (int m = 0; m < markers.Length; ++m) { markers[m] = rbState.Markers[m].Position; markerSize[m] = rbState.Markers[m].Size; } } catch (Exception e) { } } }
//Get marker IDs for cue for cue stick private void getCueMarkers() { OptitrackRigidBodyState rbState = StreamingClient.GetLatestRigidBodyState(RigidBodyId); //access rigidbody List <OptitrackMarkerState> markers = new List <OptitrackMarkerState>(); //initialize list of marker objects from rigidbody markers = StreamingClient.GetLatestMarkerStates(); //get objects of all markers from rigidbody SortedList <float, int> sortedMarkers = new SortedList <float, int>(); for (int j = 0; j < markers.Count; j++) { OptitrackMarkerState m = markers[j]; int mID = m.Id; Vector3 pos = m.Position; Debug.Log(pos.ToString("f4")); Debug.Log(mID); float zpos = pos.z; sortedMarkers.Add(zpos, mID); } backID1 = sortedMarkers.Values[1]; backID2 = sortedMarkers.Values[2]; frontID1 = sortedMarkers.Values[4]; frontID2 = sortedMarkers.Values[5]; markerIDs = false; Debug.Log("bid " + backID1); Debug.Log("bid2 " + backID2); Debug.Log(frontID1); Debug.Log("fid2 " + frontID2); }
public static void GetPositionRotation(OptitrackStreamingClient streamingClient, int trackerId, out Vector position, out Rotation rotation) { OptitrackRigidBodyState rbState = streamingClient.GetLatestRigidBodyState(trackerId); position = Passer.Target.ToVector(rbState.Pose.Position); rotation = Passer.Target.ToRotation(rbState.Pose.Orientation); }
private const float maxAge_sec = 0.01F; // if a measurement is older then this value, it will not be used public void Update(int trackerId) { if (streamingClient == null) { return; } OptitrackRigidBodyState rbState = streamingClient.GetLatestRigidBodyState(trackerId); if (rbState == null || rbState.DeliveryTimestamp.AgeSeconds > maxAge_sec) { _rotationConfidence = 0; _positionConfidence = 0; status = Status.Present; return; } _localSensorPosition = Passer.Target.ToVector(rbState.Pose.Position); _localSensorRotation = Passer.Target.ToRotation(rbState.Pose.Orientation); _rotationConfidence = 0.99F; // only native tracking is better _positionConfidence = 1; UpdateSensor(); status = Status.Tracking; }
// Update is called once per frame void Update() { // create the color for the square new_color = new Color(color_factor, color_factor, color_factor, 1f); // put it on the square tracking_square.GetComponent <Renderer>().material.SetColor("_Color", new_color); // Define the color for the next iteration (switch it) if (color_factor > 0.0f) { color_factor = 0.0f; } else { color_factor = 1.0f; } // Get the status of the rigid body OptitrackRigidBodyState rbState = StreamingClient.GetLatestRigidBodyState(RigidBodyId); if (rbState != null) { // get the position of the mouse RB Mouse_Position = rbState.Pose.Position; // change the transform position of the game object this.transform.localPosition = Mouse_Position; // also change its rotation this.transform.localRotation = rbState.Pose.Orientation; // turn the angles into Euler (for later printing) Mouse_Orientation = this.transform.eulerAngles; // get the timestamp Time_stamp = rbState.DeliveryTimestamp.SecondsSince(reference); } // get the position of single trackable points in the arena, used for labeled real crickets List <OptitrackMarkerState> markerStates = StreamingClient.GetLatestMarkerStates(); // for each detected marker foreach (OptitrackMarkerState marker in markerStates) { // if it's not part of a rigid body, save the position as cricket if (marker.Labeled == false) { Cricket = marker.Position; } else { // if not, ignore it Cricket = new Vector3(10.0f, 10.0f, 10.0f); } } // Write the line of data writer.WriteLine(string.Concat(Time_stamp.ToString(), ',', Mouse_Position.x.ToString(), ',', Mouse_Position.y.ToString(), ',', Mouse_Position.z.ToString(), ',', Mouse_Orientation.x.ToString(), ',', Mouse_Orientation.y.ToString(), ',', Mouse_Orientation.z.ToString(), ',', Cricket.x.ToString(), ',', Cricket.y.ToString(), ',', Cricket.z.ToString(), ',', color_factor.ToString())); }
// Update is called once per frame void Update() { if (Input.GetKeyDown("c") && (calibrateMethod == CalibrationLoadMethod.CALIBRATION) && !dataInitiate) { StartCoroutine(RunCalibration()); } if (sampling) { OptitrackRigidBodyState rbState = StreamingClient.GetLatestRigidBodyState(ControllerRigidbodyID); if (rbState != null) { calibrationData.addPositionData(rbState.Pose.Position, ControllerGameobject.transform.position); samplingCount++; //new 0524 Instantiate(generate_rbState_Objects_0, rbState.Pose.Position, Quaternion.identity); Instantiate(generate_ControllerGameobject_Objects_0, ControllerGameobject.transform.position, Quaternion.identity); // log some data Debug.Log(samplingCount + " - OptitrackRB: " + rbState.Pose.Position + ", GameObj: " + ControllerGameobject.transform.position); string path = "Assets/Resources/test.txt"; //Write some text to the test.txt file StreamWriter writer = new StreamWriter(path, true); writer.WriteLine(rbState.Pose.Position); writer.Close(); if (samplingCount >= samplingPoints) { sampling = false; calibrationData.calibrateTransformationMatrix(); print("Finish Calibrate"); ShowCalibrationInfo("Finish Calibrate"); dataInitiate = true; float error = 0.0f; print("Calculating Error"); for (int i = 0; i < samplingCount; i++) { Vector3 temp = calibrationData.PositiontransformationMatrix.MultiplyPoint3x4(calibrationData.oriPosition[i]); error += Vector3.Distance(temp, calibrationData.targetPosition[i]); } float errorMean = error / samplingCount; Debug.Log(errorMean.ToString("F6")); ShowCalibrationInfo("Calculating Error: " + errorMean.ToString("F6")); } } } }
void UpdatePose() { OptitrackRigidBodyState rbState = StreamingClient.GetLatestRigidBodyState(RigidBodyId); if (rbState != null) { this.transform.localPosition = rbState.Pose.Position; this.transform.localRotation = rbState.Pose.Orientation; } }
//update the transform position, rotation and timestamp every frame void Update() { rbState = StreamingClient.GetLatestRigidBodyState(RigidBodyId); if (rbState != null) { this.transform.localPosition = rbState.Pose.Position; this.transform.localRotation = rbState.Pose.Orientation; this.rbTimestamp = rbState.DeliveryTimestamp; } }
void UpdatePose() { Vector3 lastPos = transform.position; OptitrackRigidBodyState rbState = StreamingClient.GetLatestRigidBodyState(RigidBodyId); if (rbState != null) { this.transform.localPosition = rbState.Pose.Position; this.transform.localRotation = rbState.Pose.Orientation; } tracked = Vector3.Distance(lastPos, transform.position) > 0; }
void UpdatePose() { OptitrackRigidBodyState rbState = StreamingClient.GetLatestRigidBodyState(RigidBodyId); if (rbState != null) { Vector3 position = rbState.Pose.Position * moveFactor; Vector2 newPos = new Vector2(position.z, position.x); GetComponent <Rigidbody2D> ().AddForce(oldPos - newPos); oldPos = newPos; } }
void UpdatePose() { OptitrackRigidBodyState rbState = StreamingClient.GetLatestRigidBodyState(RigidBodyId); if (rbState != null) { trans.UpdateValue(rbState.Pose.Position, rbState.Pose.Orientation); this.transform.localPosition = Filtering ? trans.GetFilteredTransform().Position : rbState.Pose.Position; this.transform.localRotation = Filtering ? trans.GetFilteredTransform().Rotation : rbState.Pose.Orientation; } }
void Update() { OptitrackRigidBodyState rbState = StreamingClient.GetLatestRigidBodyState(RigidBodyId); if (rbState != null) { this.transform.localPosition = Vector3.Lerp(transform.localPosition, rbState.Pose.Position.V3, Time.deltaTime * lerpSpeed); // this.transform.localPosition = rbState.Pose.Position.V3; this.transform.localRotation = Quaternion.Lerp(transform.rotation, rbState.Pose.Orientation.Q, Time.deltaTime * lerpSpeed); // this.transform.localRotation = rbState.Pose.Orientation.Q; } }
void UpdatePose() { OptitrackRigidBodyState rbState = StreamingClient.GetLatestRigidBodyState(RigidBodyId); if (rbState != null) { // Fix Unity rigidbody in z-plane. rbState.Pose.Position.z = 0.0f; // Amplify Unity rigidbody movement. this.transform.localPosition = rbState.Pose.Position * ampFactor; this.transform.localRotation = rbState.Pose.Orientation; } }
void Update() { OptitrackRigidBodyState rbState = StreamingClient.GetLatestRigidBodyState(RigidBodyId); if (rbState != null) { float new_x = Mathf.Clamp(rbState.Pose.Position.x, x_bound.x, x_bound.y); float new_y = Mathf.Clamp(rbState.Pose.Position.y, y_bound.x, y_bound.y); float new_z = Mathf.Clamp(rbState.Pose.Position.z, z_bound.x, z_bound.y); // Debug.Log (new_y); this.transform.localPosition = new Vector3(new_x, new_y, new_z); // this.transform.localPosition = rbState.Pose.Position; this.transform.localRotation = rbState.Pose.Orientation; //Debug.Log(this.transform.localPosition); } }
void Update() { OptitrackRigidBodyState rbState = StreamingClient.GetLatestRigidBodyState(RigidBodyId); if (rbState != null && rbState.DeliveryTimestamp.AgeSeconds < 1.0f) { // Update position. this.transform.localPosition = rbState.Pose.Position; // Update drift correction. if (m_driftCorrHandle != IntPtr.Zero && m_hmdCameraObject) { NpHmdQuaternion opticalOri = new NpHmdQuaternion(rbState.Pose.Orientation); NpHmdQuaternion inertialOri = new NpHmdQuaternion(m_hmdCameraObject.transform.localRotation); NpHmdResult result = NativeMethods.NpHmd_MeasurementUpdate( m_driftCorrHandle, ref opticalOri, // const ref inertialOri, // const Time.deltaTime ); if (result == NpHmdResult.OK) { NpHmdQuaternion newCorrection; result = NativeMethods.NpHmd_GetOrientationCorrection(m_driftCorrHandle, out newCorrection); if (result == NpHmdResult.OK) { this.transform.localRotation = newCorrection; } else { Debug.LogError(GetType().FullName + ": NpHmd_GetOrientationCorrection failed.", this); this.enabled = false; return; } } else { Debug.LogError(GetType().FullName + ": NpHmd_MeasurementUpdate failed.", this); this.enabled = false; return; } } } }
// Start is called before the first frame update void Start() { OptitrackRigidBodyState rigidBodyState = streamingClient.GetLatestRigidBodyState(rigidBodyId); if (rigidBodyState != null) { Debug.Log(string.Format("Align position of rigid body with id {0}", rigidBodyId)); gameObject.transform.SetPositionAndRotation(rigidBodyState.Pose.Position, rigidBodyState.Pose.Orientation); path = string.Format("{0}_rigidBody{1}.txt", DateTime.Now.ToString("yyyy-MM-dd-HH-mm-ss-fff"), rigidBodyId); sw = new StreamWriter(path, true); Debug.Log("Print rigid body position to file"); sw.WriteLine(string.Format("{0}\t{1}", rigidBodyState.Pose.Position, rigidBodyState.Pose.Orientation)); sw.Close(); } }
void UpdatePose() { OptitrackRigidBodyState rbState = StreamingClient.GetLatestRigidBodyState(RigidBodyId); if (rbState != null) { this.transform.localPosition = rbState.Pose.Position; this.transform.localRotation = rbState.Pose.Orientation; if (onlyRotation) { Vector3 localRotation = transform.localRotation.eulerAngles; localRotation.x = 0; localRotation.z = 0; transform.localRotation = Quaternion.Euler(localRotation); } } }
/// <summary> /// Draws marker visualizations in the editor viewport for any selected OptitrackRigidBody component. /// </summary> void OnSceneGUI() { OptitrackRigidBody rb = target as OptitrackRigidBody; if (!rb || rb.StreamingClient == null) { return; } rb.StreamingClient._EnterFrameDataUpdateLock(); try { OptitrackRigidBodyDefinition rbDef = rb.StreamingClient.GetRigidBodyDefinitionById(rb.RigidBodyId); OptitrackRigidBodyState rbState = rb.StreamingClient.GetLatestRigidBodyState(rb.RigidBodyId); if (rbDef != null && rbState != null) { for (int iMarker = 0; iMarker < rbDef.Markers.Count; ++iMarker) { OptitrackRigidBodyDefinition.MarkerDefinition marker = rbDef.Markers[iMarker]; // Effectively treat the RB GameObject transform as a rigid transform by negating its local scale. Vector3 markerPos = marker.Position; markerPos.Scale(new Vector3(1.0f / rb.transform.localScale.x, 1.0f / rb.transform.localScale.y, 1.0f / rb.transform.localScale.z)); markerPos = rb.transform.TransformPoint(markerPos); float kMarkerSize = 0.02f; Matrix4x4 markerTransform = Matrix4x4.TRS(markerPos, Quaternion.identity, new Vector3(kMarkerSize, kMarkerSize, kMarkerSize)); for (int iPass = 0; iPass < m_markerMaterial.passCount; ++iPass) { if (m_markerMaterial.SetPass(iPass)) { Graphics.DrawMeshNow(m_markerMesh, markerTransform); } } } } } finally { rb.StreamingClient._ExitFrameDataUpdateLock(); } }
public override void Update() { if (!humanoid.optitrack.enabled || !enabled) { return; } //float confidence = 1; OptitrackRigidBodyState rbState = streamingClient.GetLatestRigidBodyState(trackerId); if (rbState != null) { targetTransform.position = rbState.Pose.Position; targetTransform.rotation = rbState.Pose.Orientation; // architecture does not allow confidence here... } }
void UpdateGO(int sname, GameObject go, ref OptitrackRigidBodyState otRBState, ref List <bool> readies) { float s = 0.05f; otRBState = StreamingClient.GetLatestRigidBodyState(sname); readies.Add(otRBState != null); go.transform.localScale = new Vector3(s, s, s); if (otRBState != null) { go.transform.position = otRBState.Pose.Position; go.transform.rotation = otRBState.Pose.Orientation; if (drawRotation) { Debug.DrawRay(go.transform.position, go.transform.TransformDirection(Vector3.forward), Color.blue); Debug.DrawRay(go.transform.position, go.transform.TransformDirection(Vector3.up), Color.green); Debug.DrawRay(go.transform.position, go.transform.TransformDirection(Vector3.right), Color.red); } } }
/// <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) { OptitrackRigidBodyState rbState = null; lock ( m_frameDataUpdateLock ) { if (ConnectionType == ClientConnectionType.Photon) { m_photonisedLatestRigidBodyStates.TryGetValue(rigidBodyId, out rbState); } else { m_latestRigidBodyStates.TryGetValue(rigidBodyId, out rbState); } } return(rbState); }
// Update is called once per frame void Update() { // set the color of the square for tracking the frames new_color = new Color(color_factor, color_factor, color_factor, 1f); tracking_square.GetComponent <Renderer> ().material.SetColor("_Color", new_color); if (color_factor > 0.0f) { color_factor = 0.0f; } else { color_factor = 1.0f; } OptitrackRigidBodyState rbState = StreamingClient.GetLatestRigidBodyState(RigidBodyId); if (rbState != null) { Mouse_Position = rbState.Pose.Position; this.transform.localPosition = Mouse_Position; this.transform.localRotation = rbState.Pose.Orientation; Mouse_Orientation = this.transform.eulerAngles; Time_stamp = rbState.DeliveryTimestamp.SecondsSince(reference); } OptitrackRigidBodyState rbState2 = StreamingClient.GetLatestRigidBodyState(RigidBodyId2); if (rbState2 != null) { Mouse_Position2 = rbState2.Pose.Position; mouse2.transform.localPosition = Mouse_Position2; mouse2.transform.localRotation = rbState2.Pose.Orientation; Mouse_Orientation2 = mouse2.transform.eulerAngles; } writer.WriteLine(string.Concat(Time_stamp.ToString(), ',', Mouse_Position.x.ToString(), ',', Mouse_Position.y.ToString(), ',', Mouse_Position.z.ToString(), ',', Mouse_Orientation.x.ToString(), ',', Mouse_Orientation.y.ToString(), ',', Mouse_Orientation.z.ToString(), ',', Mouse_Position2.x.ToString(), ',', Mouse_Position2.y.ToString(), ',', Mouse_Position2.z.ToString(), ',', Mouse_Orientation2.x.ToString(), ',', Mouse_Orientation2.y.ToString(), ',', Mouse_Orientation2.z.ToString(), ',', color_factor.ToString())); }
//Function to get IDs of markers in corresponding pockets for calibration private void getCalMarkerId() { OptitrackRigidBodyState rbState = StreamingClient.GetLatestRigidBodyState(RigidBodyId); //access rigidbody List <OptitrackMarkerState> markers = new List <OptitrackMarkerState>(); //initialize list of marker objects from rigidbody markers = StreamingClient.GetLatestMarkerStates(); //get objects of all markers from rigidbody List <int> idList = new List <int>(); float maxz = -1000f; int idz = -1; float maxx = -1000f; int idx = -1; for (int j = 0; j < markers.Count; j++) { OptitrackMarkerState m = markers[j]; int mID = m.Id; Vector3 pos = m.Position; idList.Add(mID); // one corner is placed further to the positive z (right) if (pos.x > maxx) { maxx = pos.x; idx = mID; } // one marker is placed further to the negative x (toward end of table) if (pos.z > maxz) { maxz = pos.z; idz = mID; } } idList.Remove(idz); idList.Remove(idx); //store marker IDs corner1ID = idList[0]; //by elimination we get the 3rd corner corner2ID = idx; corner3ID = idz; }
private const float maxAge_sec = 0.01F; // if a measurement is older then this value, it will not be used public override void UpdateComponent() { OptitrackRigidBodyState rbState = streamingClient.GetLatestRigidBodyState(streamingID); if (rbState == null || rbState.DeliveryTimestamp.AgeSeconds > maxAge_sec) { status = Status.Present; positionConfidence = 0; rotationConfidence = 0; gameObject.SetActive(false); return; } status = Status.Tracking; transform.position = trackerTransform.TransformPoint(rbState.Pose.Position); transform.rotation = trackerTransform.rotation * rbState.Pose.Orientation; positionConfidence = 1F; rotationConfidence = 0.99F; gameObject.SetActive(true); }
/// <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>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); } return(rbState); }
void Update() { if (!useOptitrack) { this.transform.localPosition = new Vector3(0, 0, 0); if (!m_initialPoseSet) { m_initialPoseSet = true; m_fusion.setCurrentOrientation(this.transform.localRotation); } return; } OptitrackRigidBodyState rbState = StreamingClient.GetLatestRigidBodyState(RigidBodyId); if (rbState != null && rbState.DeliveryTimestamp.AgeSeconds < 1.0f) { Debug.Log("new position", this); // Update position. this.transform.localPosition = rbState.Pose.Position; // Update drift correction. if (m_lastTimestamp.AgeSeconds != rbState.DeliveryTimestamp.AgeSeconds) { // If we're coming here the first time: reset to optical pose. Otherwise // we're using the optical orientation for drift correction. if (!m_initialPoseSet) { m_initialPoseSet = true; m_fusion.setCurrentYaw(rbState.Pose.Orientation); } else if (enableDriftCorrection) { m_fusion.setCurrentYawSoft(rbState.Pose.Orientation); } m_lastTimestamp = rbState.DeliveryTimestamp; } } }
void UpdatePose() { OptitrackRigidBodyState rbState = StreamingClient.GetLatestRigidBodyState(RigidBodyId); if (rbState != null) { if (rbState.DeliveryTimestamp.AgeSeconds > 0.05) { if (updated) { Debug.Log("no data from optitrack"); } updated = false; } else { this.transform.localPosition = rbState.Pose.Position; this.transform.localRotation = rbState.Pose.Orientation; updated = true; } } }
/// <summary> /// Draws marker visualizations in the editor viewport for any selected OptitrackRigidBody component. /// </summary> void OnSceneGUI() { OptitrackRigidBody rb = target as OptitrackRigidBody; if (!rb || rb.StreamingClient == null) { return; } rb.StreamingClient._EnterFrameDataUpdateLock(); try { OptitrackRigidBodyState rbState = rb.StreamingClient.GetLatestRigidBodyState(rb.RigidBodyId); if (rbState != null && rbState.Markers != null) { for (int iMarker = 0; iMarker < rbState.Markers.Count; ++iMarker) { OptitrackMarkerState marker = rbState.Markers[iMarker]; Vector3 markerPos = marker.Position; if (rb.transform.parent) { markerPos = rb.transform.parent.TransformPoint(markerPos); } Matrix4x4 markerTransform = Matrix4x4.TRS(markerPos, Quaternion.identity, new Vector3(marker.Size, marker.Size, marker.Size)); Graphics.DrawMesh(m_markerMesh, markerTransform, m_markerMaterial, 0, camera: null, submeshIndex: 0, properties: null, castShadows: false, receiveShadows: false); } } } finally { rb.StreamingClient._ExitFrameDataUpdateLock(); } }
/// <summary> /// Event handler for NatNet frame delivery. Updates our simplified state representations. /// NOTE: This executes in the context of the NatNetLib network service thread! /// </summary> /// <remarks> /// Because the <see cref="sFrameOfMocapData"/> type is expensive to marshal, we instead utilize the /// <see cref="NatNetClient.NativeFrameReceivedEventArgs.NativeFramePointer"/>, treating it as as opaque, and /// passing it to some helper "accessor" functions to retrieve the subset of data we care about, using only /// blittable types which do not cause any garbage to be allocated. /// </remarks> /// <param name="sender"></param> /// <param name="eventArgs"></param> private void OnNatNetFrameReceived(object sender, NatNetClient.NativeFrameReceivedEventArgs eventArgs) { // In the event of contention, drop the frame being delivered and return immediately. // We don't want to stall NatNetLib's internal network service thread. if (!Monitor.TryEnter(m_frameDataUpdateLock)) { return; } try { // Update health markers. m_receivedFrameSinceConnect = true; Interlocked.Exchange(ref m_lastFrameDeliveryTimestamp.m_ticks, OptitrackHiResTimer.Now().m_ticks); // Process received frame. IntPtr pFrame = eventArgs.NativeFramePointer; NatNetError result = NatNetError.NatNetError_OK; // Update rigid bodies. Int32 frameRbCount; result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetRigidBodyCount(pFrame, out frameRbCount); NatNetException.ThrowIfNotOK(result, "NatNet_Frame_GetRigidBodyCount failed."); for (int rbIdx = 0; rbIdx < frameRbCount; ++rbIdx) { sRigidBodyData rbData = new sRigidBodyData(); result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetRigidBody(pFrame, rbIdx, out rbData); NatNetException.ThrowIfNotOK(result, "NatNet_Frame_GetRigidBody failed."); bool bTrackedThisFrame = (rbData.Params & 0x01) != 0; if (bTrackedThisFrame == false) { continue; } // Ensure we have a state corresponding to this rigid body ID. OptitrackRigidBodyState rbState = GetOrCreateRigidBodyState(rbData.Id); rbState.DeliveryTimestamp = OptitrackHiResTimer.Now(); // Flip coordinate handedness from right to left by inverting X and W. rbState.Pose.Position = new Vector3(-rbData.X, rbData.Y, rbData.Z); rbState.Pose.Orientation = new Quaternion(-rbData.QX, rbData.QY, rbData.QZ, -rbData.QW); } // Update skeletons. Int32 frameSkeletonCount; result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetSkeletonCount(pFrame, out frameSkeletonCount); NatNetException.ThrowIfNotOK(result, "NatNet_Frame_GetSkeletonCount failed."); for (int skelIdx = 0; skelIdx < frameSkeletonCount; ++skelIdx) { Int32 skeletonId; result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_Skeleton_GetId(pFrame, skelIdx, out skeletonId); NatNetException.ThrowIfNotOK(result, "NatNet_Frame_Skeleton_GetId failed."); // Ensure we have a state corresponding to this skeleton ID. OptitrackSkeletonState skelState = GetOrCreateSkeletonState(skeletonId); // Enumerate this skeleton's bone rigid bodies. Int32 skelRbCount; result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_Skeleton_GetRigidBodyCount(pFrame, skelIdx, out skelRbCount); NatNetException.ThrowIfNotOK(result, "NatNet_Frame_Skeleton_GetRigidBodyCount failed."); for (int boneIdx = 0; boneIdx < skelRbCount; ++boneIdx) { sRigidBodyData boneData = new sRigidBodyData(); result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_Skeleton_GetRigidBody(pFrame, skelIdx, boneIdx, out boneData); NatNetException.ThrowIfNotOK(result, "NatNet_Frame_Skeleton_GetRigidBody failed."); // In the context of frame data (unlike in the definition data), this ID value is a // packed composite of both the asset/entity (skeleton) ID and member (bone) ID. Int32 boneSkelId, boneId; NaturalPoint.NatNetLib.NativeMethods.NatNet_DecodeID(boneData.Id, out boneSkelId, out boneId); // TODO: Could pre-populate this map when the definitions are retrieved. // Should never allocate after the first frame, at least. if (skelState.BonePoses.ContainsKey(boneId) == false) { skelState.BonePoses[boneId] = new OptitrackPose(); } // Flip coordinate handedness from right to left by inverting X and W. skelState.BonePoses[boneId].Position = new Vector3(-boneData.X, boneData.Y, boneData.Z); skelState.BonePoses[boneId].Orientation = new Quaternion(-boneData.QX, boneData.QY, boneData.QZ, -boneData.QW); } } // Update markers Int32 MarkerCount; result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetLabeledMarkerCount(pFrame, out MarkerCount); NatNetException.ThrowIfNotOK(result, "NatNet_Frame_GetSkeletonCount failed."); m_latestMarkerStates.Clear(); for (int markerIdx = 0; markerIdx < MarkerCount; ++markerIdx) { sMarker marker = new sMarker(); result = NaturalPoint.NatNetLib.NativeMethods.NatNet_Frame_GetLabeledMarker(pFrame, markerIdx, out marker); NatNetException.ThrowIfNotOK(result, "NatNet_Frame_GetLabeledMarker failed."); // Flip coordinate handedness Vector3 markerPos = new Vector3(-marker.X, marker.Y, marker.Z); OptitrackMarkerState markerState = GetOrCreateMarkerState(marker.Id); markerState.Position = markerPos; markerState.Size = marker.Size; markerState.Labeled = (marker.Params & 0x10) == 0; markerState.Id = marker.Id; } } catch (Exception ex) { Debug.LogError(GetType().FullName + ": OnNatNetFrameReceived encountered an exception.", this); Debug.LogException(ex, this); } finally { Monitor.Exit(m_frameDataUpdateLock); } }