//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);
}
//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;
}
/// <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 = markerEntry.Value.Position,
Labeled = markerEntry.Value.Labeled,
Size = markerEntry.Value.Size,
Id = markerEntry.Value.Id
};
markerStates.Add(newMarkerState);
}
}
return(markerStates);
}
/// <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>
/// 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);
}
}
void calibrateTable()
{
numCalSamples++;
//Get marker positions
OptitrackRigidBodyState rbState = StreamingClient.GetLatestRigidBodyState(RigidBodyId); //access rigidbody
List <OptitrackMarkerState> markerStates = new List <OptitrackMarkerState>(); //initialize list of marker objects from rigidbody
markerStates = StreamingClient.GetLatestMarkerStates(); //get objects of all markers from rigidbody
//loop through all markers
for (int idx = 0; idx < markerStates.Count; idx++)
{
OptitrackMarkerState marker = markerStates[idx];
int mID = marker.Id;
if (mID == corner1ID)
{
C1.Add(marker.Position);
}
if (mID == corner2ID)
{
C2.Add(marker.Position);
}
if (mID == corner3ID)
{
C3.Add(marker.Position);
}
}
//stop calibratiing after 100 samples and print results
if (numCalSamples > 100)
{
Vector3 avgC1 = Experiment.AverageVec(C1);
Vector3 avgC2 = Experiment.AverageVec(C2);
Vector3 avgC3 = Experiment.AverageVec(C3);
//derive C2 based on equalateral right triangle
float length = (avgC2 - avgC1).magnitude;
Vector3 direction = Quaternion.Euler(0, -90f, 0) * (avgC2 - avgC1).normalized;
Vector3 derivedC3 = avgC1 + length * direction;
//Unity Corner Positions
PlayerPrefs.SetFloat("Ucorner1x", Experiment.corner1.position.x);
PlayerPrefs.SetFloat("Ucorner1y", Experiment.corner1.position.y);
PlayerPrefs.SetFloat("Ucorner1z", Experiment.corner1.position.z);
PlayerPrefs.SetFloat("Ucorner2x", Experiment.corner2.position.x);
PlayerPrefs.SetFloat("Ucorner2y", Experiment.corner2.position.y);
PlayerPrefs.SetFloat("Ucorner2z", Experiment.corner2.position.z);
PlayerPrefs.SetFloat("Ucorner3x", Experiment.corner3.position.x);
PlayerPrefs.SetFloat("Ucorner3y", Experiment.corner3.position.y);
PlayerPrefs.SetFloat("Ucorner3z", Experiment.corner3.position.z);
//Optitrack Corner Positions
PlayerPrefs.SetFloat("Ocorner1x", avgC1.x);
PlayerPrefs.SetFloat("Ocorner1y", avgC1.y);
PlayerPrefs.SetFloat("Ocorner1z", avgC1.z);
PlayerPrefs.SetFloat("Ocorner2x", avgC2.x);
PlayerPrefs.SetFloat("Ocorner2y", avgC2.y);
PlayerPrefs.SetFloat("Ocorner2z", avgC2.z);
PlayerPrefs.SetFloat("Ocorner3x", derivedC3.x);
PlayerPrefs.SetFloat("Ocorner3y", derivedC3.y);
PlayerPrefs.SetFloat("Ocorner3z", derivedC3.z);
//Cue ball starting position in unity
PlayerPrefs.SetFloat("Ocuex", avgC3.x);
PlayerPrefs.SetFloat("Ocuey", avgC3.y);
PlayerPrefs.SetFloat("Ocuez", avgC3.z);
//Experiment.optiPocketPoints = new float[,] { { avgC1.x, avgC2.x, avgC3.x }, { avgC1.z, avgC2.z, avgC3.z }, { 1f, 1f, 1f } };
if (test)
{
Debug.Log("corner1ID = " + corner1ID);
Debug.Log("corner1 position = " + avgC1.ToString("f4"));
Debug.Log("corner2ID = " + corner2ID);
Debug.Log("corner2 position = " + avgC2.ToString("f4"));
Debug.Log("derived C3 " + derivedC3.ToString("f4"));
Debug.Log("corner3ID = " + corner3ID);
Debug.Log("corner3 position = " + avgC3.ToString("f4"));
Debug.Log("Unity positions");
Debug.Log("corner1" + Experiment.corner1.position.ToString("f4"));
Debug.Log("corner2" + Experiment.corner2.position.ToString("f4"));
Debug.Log("corner3" + Experiment.corner3.position.ToString("f4"));
float optiPocketDist = (avgC1 - avgC2).magnitude;
float unityPocketDist = (Experiment.corner1.position - Experiment.corner2.position).magnitude;
PlayerPrefs.SetFloat("optiToUnity", unityPocketDist / optiPocketDist);
test = false;
}
}
}
/*
* Method to update the position of the cue rigidbody every by reading the data from the Optitrack client
*/
void UpdatePose()
{
#region Cue State Variables
OptitrackRigidBodyState rbState = StreamingClient.GetLatestRigidBodyState(RigidBodyId); //access rigidbody
Vector3 OfrontPos = new Vector3(); //optitrack front position
Vector3 ObackPos = new Vector3(); //optitrack back position
List <OptitrackMarkerState> markerStates = new List <OptitrackMarkerState>(); //initialize list of marker objects from rigidbody
List <Vector3> markerPositions = new List <Vector3>(); //list of all cue marker positions
#endregion
if (rbState != null)
{
//Get marker positions
markerStates = StreamingClient.GetLatestMarkerStates(); //get objects of all markers from rigidbody
#region Check if valid Optitrack markers
//if not all markers are tracked, make cue stick invisible
if (markerStates.Count < 4)
{
Experiment.cuestickMesh.enabled = false;
//Debug.Log(markerStates.Count);
cuePos = prevPos;
return;
}
Experiment.cuestickMesh.enabled = true;
#endregion
#region Access & store marker poitions
//Loop through markers
for (int idx = 0; idx < markerStates.Count; idx++)
{
OptitrackMarkerState marker = markerStates[idx];
int markerID = marker.Id;
Vector3 pos = marker.Position;
markerPositions.Add(marker.Position);
}
#endregion
#region Derive Unity cue position
IEnumerable <Vector3> sorted = markerPositions.OrderBy(v => v.z); //sort marker positions by z position
OfrontPos = (sorted.ElementAt(3) + sorted.ElementAt(2)) / 2;
Vector3 dif = OfrontPos - sorted.ElementAt(1);
ObackPos = sorted.ElementAt(0) + dif;
//Translate optitrack positions to unity using helper methods ( [Xo, Zo, 1] * M = [Xu, Zu, 1])
float[,] frontMatrix = new float[, ] {
{ OfrontPos.x }, { OfrontPos.z }, { 1 }
};
float[,] backMatrix = new float[, ] {
{ ObackPos.x }, { ObackPos.z }, { 1 }
};
float[,] frontU = Experiment.MultiplyMatrix(Experiment.M, frontMatrix);
float[,] backU = Experiment.MultiplyMatrix(Experiment.M, backMatrix);
Vector3 backPos = new Vector3(backU[0, 0], ObackPos.y * Experiment.yratio, backU[1, 0]);
Vector3 frontPos = new Vector3(frontU[0, 0], OfrontPos.y * Experiment.yratio, frontU[1, 0]);
//cue position is front position plus projected vector
Vector3 direction = (frontPos - backPos).normalized;
cuePos = frontPos + (direction * PlayerPrefs.GetFloat("tip_marker1") * PlayerPrefs.GetFloat("cmToUnity"));
//limit cuetip height to table surface height
if (cuePos.y < Experiment.corner1.position.y)
{
cuePos = new Vector3(cuePos.x, Experiment.corner1.position.y, cuePos.z);
}
#endregion
#region Calculate cue velocity
//calculate velocity
cueVelocity = (cuePos - prevPos) / Time.fixedDeltaTime;
if (cueVelocity.magnitude > 3)
{
cueVelocity = 3f * cueVelocity.normalized;
}
VelocityList.Add(cueVelocity);
//avgVelocity = AverageVelocity(VelocityList, Experiment.numVelocitiesAverage);
prevPos = cuePos;
#endregion
//move cue rigidbody to updated position
Experiment.cueFront.transform.position = cuePos;
//get forward direction by lookng at forwrd position from actual optitrack position (could also do with transformed positions)
Experiment.cueFront.transform.rotation = Quaternion.LookRotation(frontPos - backPos) * Quaternion.Euler(0f, 0f, 0f);
}
}
