public object MarshalNativeToManaged(IntPtr pNativeData)
{
// Here, we call C++/CLI code
FrameOfData frame = Marshaler.MarshalFrame(pNativeData);
return(frame);
}
IEnumerable <Vector3> GetUnlabledMarkers()
{
FrameOfData msg = dataBuffer[dataBufferHead];
foreach (Vector3 marker in msg.other_markers)
{
yield return(ToUnity(marker));
}
}
IEnumerable <Tuple <int, Vector3> > GetMarkers()
{
FrameOfData msg = dataBuffer[dataBufferHead];
foreach (LabeledMarker marker in msg.labeled_markers)
{
yield return(Tuple.Create(marker.id, ToUnity(marker.position)));
}
}
public List<Vector3> GetUnlabledMarkers()
{
FrameOfData msg = dataBuffer[dataBufferHead];
var markers = new List<Vector3>(msg.other_markers.Count);
foreach (Vector3 marker in msg.other_markers)
markers.Add(ToUnity(marker));
return markers;
}
public List<(int label, Vector3 position)> GetMarkers()
{
FrameOfData msg = dataBuffer[dataBufferHead];
var markers = new List<(int, Vector3)>(msg.labeled_markers.Count);
foreach (LabeledMarker marker in msg.labeled_markers)
markers.Add((marker.id, ToUnity(marker.position)));
return markers;
}
public List<Vector3> GetFrameMarkers()
{
FrameOfData msg = dataBuffer[dataBufferHead];
var markers = new List<Vector3>(msg.rigid_bodies.Count * 3);
foreach (RigidBody rb in msg.rigid_bodies)
foreach (Vector3 marker in rb.markers)
markers.Add(ToUnity(marker));
return markers;
}
IEnumerable <Vector3> GetFrameMarkers()
{
FrameOfData msg = dataBuffer[dataBufferHead];
foreach (RigidBody rb in msg.rigid_bodies)
{
foreach (Vector3 marker in rb.markers)
{
yield return(ToUnity(marker));
}
}
}
private void _unpack_frameofdata(byte[] payload, ref int offset, int[] version, out FrameOfData fod)
{
int frameno;
int nsets;
_unpack_head <int>(payload, ref offset, out frameno);
_unpack_head <int>(payload, ref offset, out nsets);
Dictionary <string, ArrayList> sets = new Dictionary <string, ArrayList>();
string setname;
ArrayList markers;
ArrayList rbodies;
ArrayList skels;
ArrayList lmarkers;
ArrayList forceplates;
for (int i = 0; i < nsets; i++)
{
_unpack_cstring(payload, ref offset, MAX_NAMELENGTH, out setname);
_unpack_markers(payload, ref offset, out markers);
if (sets.ContainsKey(setname))
{
sets[setname] = markers;
}
else
{
sets.Add(setname, markers);
}
}
_unpack_markers(payload, ref offset, out markers);
_unpack_rigid_bodies(payload, ref offset, version, out rbodies);
_unpack_skeletons(payload, ref offset, version, out skels);
_unpack_labeled_markers(payload, ref offset, version, out lmarkers);
_unpack_force_plates(payload, ref offset, version, out forceplates);
float latency;
uint timecode;
uint timecode_sub;
double timestamp = 0;
float timestampf = 0;
short param = 0;
bool is_recording = false;
bool tracked_models_changed = false;
if (_version_is_at_least(version, 2, 7))
{
_unpack_head <float>(payload, ref offset, out latency);
_unpack_head <uint>(payload, ref offset, out timecode);
_unpack_head <uint>(payload, ref offset, out timecode_sub);
_unpack_head <double>(payload, ref offset, out timestamp);
_unpack_head <short>(payload, ref offset, out param);
is_recording = (param & 0x01) == 1;
tracked_models_changed = (param & 0x02) == 2;
}
else if (_version_is_at_least(version, 2, 6))
{
_unpack_head <float>(payload, ref offset, out latency);
_unpack_head <uint>(payload, ref offset, out timecode);
_unpack_head <uint>(payload, ref offset, out timecode_sub);
_unpack_head <float>(payload, ref offset, out timestampf);
_unpack_head <short>(payload, ref offset, out param);
timestamp = timestampf;
is_recording = (param & 0x01) == 1;
tracked_models_changed = (param & 0x02) == 2;
}
else
{
_unpack_head <float>(payload, ref offset, out latency);
_unpack_head <uint>(payload, ref offset, out timecode);
_unpack_head <uint>(payload, ref offset, out timecode_sub);
}
int eod;
_unpack_head <int>(payload, ref offset, out eod);
if (eod != 0)
{
Debug.Log("End-of-data marker is not 0.");
}
fod = new FrameOfData();
fod.frameno = frameno;
fod.sets = sets;
fod.other_markers = markers;
fod.rigid_bodies = rbodies;
fod.skeletons = skels;
fod.labeled_markers = lmarkers;
fod.latency = latency;
fod.timecode = new uint[2];
fod.timecode[0] = timecode;
fod.timecode[1] = timecode_sub;
fod.timestamp = timestamp;
fod.is_recording = is_recording;
fod.tracked_models_changed = tracked_models_changed;
}
public void Update()
{
lock (syncLock){
if (dataBufferHead != -1) // skip if no data comes in
{
FrameOfData msg = dataBuffer[dataBufferHead];
/* zach added */
bDataReceived = true;
msgData = dataBuffer[dataBufferHead];
if (createDebugObjects)
{
// sets... as in marker sets. this is the marker data
foreach (KeyValuePair <string, ArrayList> element in msg.sets)
{
GameObject mSet = null;
string setName = string.Concat(element.Key, "_set");
if (!gameObjectDictionary.TryGetValue(setName, out mSet))
{
mSet = GameObject.Find(setName);
if (mSet == null)
{
mSet = new GameObject(setName);
mSet.transform.parent = transform;
}
gameObjectDictionary [setName] = mSet;
}
Dictionary <int, Transform> transformDict = null;
if (!markerSetIDtoTransfrom.TryGetValue(element.Key, out transformDict))
{
transformDict = new Dictionary <int, Transform> ();
markerSetIDtoTransfrom [element.Key] = transformDict;
}
Dictionary <int, string> nameDict = null;
if (markerSetIDtoName.TryGetValue(element.Key, out nameDict))
{
int i = 0;
foreach (Vector3 marker in element.Value)
{
string mkName = "";
if (nameDict.TryGetValue(i, out mkName))
{
Transform mkTransform = null;
if (!transformDict.TryGetValue(i, out mkTransform))
{
mkTransform = mSet.transform.Find(mkName);
if (mkTransform == null)
{
GameObject mk = new GameObject(mkName);
mk.transform.parent = mSet.transform;
GameObject debugObject = GameObject.CreatePrimitive(PrimitiveType.Sphere);
debugObject.transform.localScale *= 0.02f;
debugObject.transform.parent = mk.transform;
debugObject.name = "debug";
debugObjects.Add(debugObject);
mkTransform = mk.transform;
}
transformDict [i] = mkTransform;
}
mkTransform.localPosition = convertRightHandedToLeftHandedPosition(marker);
}
i++;
}
}
}
// here's the rigid body data
for (int i = 0; i < msg.rigid_bodies.Count; i++)
{
RigidBody rbody = (RigidBody)msg.rigid_bodies [i];
GameObject rb = null;
string rbName = "";
if (rigidBodyIDtoName.TryGetValue(rbody.id, out rbName))
{
if (!gameObjectDictionary.TryGetValue(rbName, out rb))
{
rb = GameObject.Find(rbName);
if (rb == null)
{
rb = new GameObject(rbName);
rb.transform.parent = transform;
GameObject debugObject = GameObject.CreatePrimitive(PrimitiveType.Cube);
debugObject.transform.localScale *= 0.1f;
debugObject.transform.parent = rb.transform;
debugObject.name = "debug";
debugObjects.Add(debugObject);
}
gameObjectDictionary [rbName] = rb;
}
rb.transform.localPosition = convertRightHandedToLeftHandedPosition(rbody.position);
rb.transform.localRotation = convertRightHandedToLeftHandedRotation(rbody.rotation);
//rb.tag = rbody.tracking_valid? "tracked" : "untracked";
}
}
for (int i = 0; i < msg.skeletons.Count; i++)
{
Skeleton skeleton = (Skeleton)msg.skeletons [i];
GameObject sk = null;
string skName = "";
if (skeletonIDtoName.TryGetValue(skeleton.id, out skName))
{
if (!gameObjectDictionary.TryGetValue(skName, out sk))
{
sk = GameObject.Find(skName);
if (sk == null)
{
sk = new GameObject(skName);
sk.transform.parent = transform;
sk.transform.localPosition = Vector3.zero;
sk.transform.localRotation = Quaternion.identity;
}
gameObjectDictionary [skName] = sk;
}
}
for (int j = 0; j < skeleton.rigid_bodies.Count; j++)
{
RigidBody rbody = (RigidBody)skeleton.rigid_bodies [j];
int skeletonID = HighWord(rbody.id);
int boneID = LowWord(rbody.id);
Dictionary <int, string> skeletonDict = null;
string boneName = "";
GameObject bone = null;
if (boneIDtoName.TryGetValue(skeletonID, out skeletonDict) && skeletonDict.TryGetValue(boneID, out boneName))
{
if (!gameObjectDictionary.TryGetValue(boneName, out bone))
{
bone = GameObject.Find(boneName);
if (bone == null)
{
bone = new GameObject(boneName);
bone.transform.parent = sk.transform;
GameObject debugObject = GameObject.CreatePrimitive(PrimitiveType.Cube);
debugObject.transform.localScale *= 0.1f;
debugObject.transform.parent = bone.transform;
debugObject.name = "debug";
debugObjects.Add(debugObject);
}
gameObjectDictionary [boneName] = bone;
}
bone.transform.localPosition = convertRightHandedToLeftHandedPosition(rbody.position);
bone.transform.localRotation = convertRightHandedToLeftHandedRotation(rbody.rotation);
//bone.tag = rbody.tracking_valid? "tracked" : "untracked";
}
}
}
}
}
}
//update model definition once a period
modelDefTimer -= Time.deltaTime;
if (modelDefTimer <= 0)
{
modelDefUpdated = false;
modelDefTimer = updateModelDefTime;
}
//show debug objects
foreach (GameObject debugObject in debugObjects)
{
debugObject.SetActive(showDebugObjects);
}
}
void Update()
{
lock (syncLock){
if (dataBufferHead != -1) // skip if no data comes in
{
FrameOfData msg = dataBuffer[dataBufferHead];
//create markers that are not in rigidbodies/skeletons/markersets.
{
GameObject mSet = null;
string setPrefix = "other";
string setName = string.Concat(setPrefix, "_set");
if (!gameObjectDictionary.TryGetValue(setName, out mSet))
{
mSet = GameObject.Find(setName);
if (mSet == null)
{
mSet = new GameObject(setName);
mSet.transform.parent = transform;
}
gameObjectDictionary[setName] = mSet;
}
Dictionary <int, Transform> transformDict = null;
if (!markerSetIDtoTransfrom.TryGetValue(setPrefix, out transformDict))
{
transformDict = new Dictionary <int, Transform>();
markerSetIDtoTransfrom[setPrefix] = transformDict;
}
// clean the tracking flag
foreach (Transform mkTransform in transformDict.Values)
{
mkTransform.tag = "untracked";
}
foreach (LabeledMarker lmk in msg.labeled_markers)
{
int setID = HighWord(lmk.id);
int mkID = LowWord(lmk.id);
if (setID != 0)
{
continue; //skip markers that are already included in rigidbodies/skeletons/marker sets.
}
string mkName = string.Concat(setPrefix, "_", mkID.ToString());
Transform mkTransform = null;
if (!transformDict.TryGetValue(mkID, out mkTransform))
{
mkTransform = mSet.transform.Find(mkName);
if (mkTransform == null)
{
GameObject mk = new GameObject(mkName);
mk.transform.parent = mSet.transform;
GameObject debugObject = GameObject.CreatePrimitive(PrimitiveType.Sphere);
debugObject.transform.localScale *= 0.02f;
debugObject.transform.parent = mk.transform;
debugObject.name = "debug";
debugObjects.Add(debugObject);
mkTransform = mk.transform;
}
transformDict[mkID] = mkTransform;
}
mkTransform.tag = "tracked";
mkTransform.localPosition = convertToLeftHandPosition(lmk.position);
}
List <int> idToDelete = new List <int>();
foreach (KeyValuePair <int, Transform> element in transformDict)
{
Transform mk = element.Value;
if (mk.CompareTag("untracked"))
{
Transform debugTransform = mk.FindChild("debug");
if (debugTransform != null)
{
debugObjects.Remove(debugTransform.gameObject);
}
Destroy(element.Value.gameObject);
idToDelete.Add(element.Key);
}
}
foreach (int i in idToDelete)
{
transformDict.Remove(i);
}
}
//create markers that are in rigidbodies/skeletons/markersets
foreach (KeyValuePair <string, ArrayList> element in msg.sets)
{
GameObject mSet = null;
string setName = string.Concat(element.Key, "_set");
if (!gameObjectDictionary.TryGetValue(setName, out mSet))
{
mSet = GameObject.Find(setName);
if (mSet == null)
{
mSet = new GameObject(setName);
mSet.transform.parent = transform;
}
gameObjectDictionary[setName] = mSet;
}
Dictionary <int, Transform> transformDict = null;
if (!markerSetIDtoTransfrom.TryGetValue(element.Key, out transformDict))
{
transformDict = new Dictionary <int, Transform>();
markerSetIDtoTransfrom[element.Key] = transformDict;
}
Dictionary <int, string> nameDict = null;
if (markerSetIDtoName.TryGetValue(element.Key, out nameDict))
{
int i = 0;
foreach (Vector3 marker in element.Value)
{
string mkName = "";
if (nameDict.TryGetValue(i, out mkName))
{
Transform mkTransform = null;
if (!transformDict.TryGetValue(i, out mkTransform))
{
mkTransform = mSet.transform.Find(mkName);
if (mkTransform == null)
{
GameObject mk = new GameObject(mkName);
mk.transform.parent = mSet.transform;
GameObject debugObject = GameObject.CreatePrimitive(PrimitiveType.Sphere);
debugObject.transform.localScale *= 0.02f;
debugObject.transform.parent = mk.transform;
debugObject.name = "debug";
debugObjects.Add(debugObject);
mkTransform = mk.transform;
}
transformDict[i] = mkTransform;
}
mkTransform.localPosition = convertToLeftHandPosition(marker);
}
i++;
}
}
}
//create rigidbodies
for (int i = 0; i < msg.rigid_bodies.Count; i++)
{
RigidBody rbody = (RigidBody)msg.rigid_bodies[i];
GameObject rb = null;
string rbName = "";
if (rigidBodyIDtoName.TryGetValue(rbody.id, out rbName))
{
if (!gameObjectDictionary.TryGetValue(rbName, out rb))
{
rb = GameObject.Find(rbName);
if (rb == null)
{
rb = new GameObject(rbName);
rb.transform.parent = transform;
GameObject debugObject = GameObject.CreatePrimitive(PrimitiveType.Cube);
debugObject.transform.localScale *= 0.1f;
debugObject.transform.parent = rb.transform;
debugObject.name = "debug";
debugObjects.Add(debugObject);
}
gameObjectDictionary[rbName] = rb;
}
rb.transform.localPosition = convertToLeftHandPosition(rbody.position);
rb.transform.localRotation = convertToLeftHandRotation(rbody.rotation);
rb.tag = rbody.tracking_valid? "tracked" : "untracked";
}
}
//create skeletons
for (int i = 0; i < msg.skeletons.Count; i++)
{
Skeleton skeleton = (Skeleton)msg.skeletons[i];
GameObject sk = null;
string skName = "";
if (skeletonIDtoName.TryGetValue(skeleton.id, out skName))
{
if (!gameObjectDictionary.TryGetValue(skName, out sk))
{
sk = GameObject.Find(skName);
if (sk == null)
{
sk = new GameObject(skName);
sk.transform.parent = transform;
sk.transform.localPosition = Vector3.zero;
sk.transform.localRotation = Quaternion.identity;
}
gameObjectDictionary[skName] = sk;
}
}
for (int j = 0; j < skeleton.rigid_bodies.Count; j++)
{
RigidBody rbody = (RigidBody)skeleton.rigid_bodies[j];
int skeletonID = HighWord(rbody.id);
int boneID = LowWord(rbody.id);
Dictionary <int, string> skeletonDict = null;
string boneName = "";
GameObject bone = null;
if (boneIDtoName.TryGetValue(skeletonID, out skeletonDict) && skeletonDict.TryGetValue(boneID, out boneName))
{
if (!gameObjectDictionary.TryGetValue(boneName, out bone))
{
bone = GameObject.Find(boneName);
if (bone == null)
{
bone = new GameObject(boneName);
bone.transform.parent = sk.transform;
GameObject debugObject = GameObject.CreatePrimitive(PrimitiveType.Cube);
debugObject.transform.localScale *= 0.1f;
debugObject.transform.parent = bone.transform;
debugObject.name = "debug";
debugObjects.Add(debugObject);
}
gameObjectDictionary[boneName] = bone;
}
bone.transform.localPosition = convertToLeftHandPosition(rbody.position);
bone.transform.localRotation = convertToLeftHandRotation(rbody.rotation);
bone.tag = rbody.tracking_valid? "tracked" : "untracked";
}
}
}
}
}
//update model definition once a period
modelDefTimer -= Time.deltaTime;
if (modelDefTimer <= 0)
{
modelDefUpdated = false;
modelDefTimer = updateModelDefTime;
}
//show debug objects
foreach (GameObject debugObject in debugObjects)
{
debugObject.SetActive(showDebugObject);
}
}
