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