void HandleOnBodyFrameDataReceived(KinectBodiesReceiver.BodyFrameData bodyFrame)
{
List <ulong> userCullList = new List <ulong> ();
foreach (var id in userTable.Keys)
{
//if frame is from same Kinect as body data
if (userTable [id].data.kinectId == bodyFrame.kinectId)
{
userCullList.Add(id);
}
else
{
//todo: deal with bodies from other Kinects
}
}
//TODO redo order of operations to avoid unnecessary math on untracked things
Matrix4x4 matrix = Matrix4x4.identity;
if (bodyReceiver.kinectInfoTable.ContainsKey(bodyFrame.kinectId))
{
matrix = bodyReceiver.kinectInfoTable [bodyFrame.kinectId].matrix;
}
foreach (var bodyData in bodyFrame.bodyData)
{
//skip filtered data
if (!bodyData.valid)
{
//if (bodyFrame.kinectId == 0x04)
//Debug.Log (bodyData.trackingId + " Not Valid!");
continue;
}
Vector3 spinePos = matrix.MultiplyPoint3x4(bodyData.position);
Vector3 spineMidPos = matrix.MultiplyPoint3x4(bodyData.spineMidPosition);
Vector3 spineShoulderPos = matrix.MultiplyPoint3x4(bodyData.spineShoulderPosition);
Vector3 headPos = matrix.MultiplyPoint3x4(bodyData.headPosition);
Vector3 handLeftPos = matrix.MultiplyPoint3x4(bodyData.handLeftPosition);
Vector3 handRightPos = matrix.MultiplyPoint3x4(bodyData.handRightPosition);
// Vector3 ShoulderLeftPos = matrix.MultiplyPoint3x4(bodyData.shoulderLeftPosition);
// Vector3 ShoulderRightPos = matrix.MultiplyPoint3x4(bodyData.shoulderRightPosition);
// Vector3 elbowLeftPosition = matrix.MultiplyPoint3x4(bodyData.elbowLeftPosition);
// Vector3 elbowRightPosition = matrix.MultiplyPoint3x4(bodyData.elbowRightPosition);
// Vector3 handTipLeftPosition = matrix.MultiplyPoint3x4(bodyData.handTipLeftPosition);
// Vector3 handTipRightPosition = matrix.MultiplyPoint3x4(bodyData.handTipRightPosition);
if (bounds.center == Vector3.zero)
{
bounds.center = spinePos;
}
bounds.Encapsulate(spinePos);
if (userTable.ContainsKey(bodyData.trackingId))
{
if (bodyData.trackingState != TrackingState.NotTracked)
{
userCullList.Remove(bodyData.trackingId);
}
else
{
continue;
}
if (bodyData.kinectId != userTable [bodyData.trackingId].data.kinectId)
{
Debug.Log("WTH THIS SHOULDNT HAPPEN");
}
userTable [bodyData.trackingId].data = bodyData;
}
else
{
if (bodyData.trackingState == TrackingState.NotTracked)
{
continue;
}
////************************ TODO: assign avatar to each body in user table corresponding to its station
/*using UnityEngine;
* using System.Collections;
* using System.Collections.Generic;
* using Kinect = Windows.Kinect;
*
* public class HandsManager : MonoBehaviour
* {
* public BodySourceManager BodyManager;
* public GameObject HandObject;
* private Dictionary<ulong, GameObject[]> _Bodies = new Dictionary<ulong, GameObject[]>();
* private Kinect.JointType[] UpperLimbsJointTypes;
* //public Kinect.ColorSpacePoint SpacePoint;
* // Use this for initialization
*
* void Start()
* {
* UpperLimbsJointTypes = new Kinect.JointType[8] { Kinect.JointType.HandRight, Kinect.JointType.ElbowRight, Kinect.JointType.SpineMid,Kinect.JointType.HandLeft, Kinect.JointType.ElbowLeft, Kinect.JointType.SpineMid,Kinect.JointType.HandTipRight, Kinect.JointType.HandTipLeft};
* }
*
* // Update is called once per frame
* void Update()
* {
* if (BodyManager == null)
* {
* return;
* }
*
* Kinect.Body[] data = BodyManager.GetData();
* if (data == null)
* {
* return;
* }
* List<ulong> trackedIds = new List<ulong>();
* foreach (var body in data)
* {
* if (body == null)
* {
* continue;
* }
*
* if (body.IsTracked)
* {
* trackedIds.Add(body.TrackingId);
* }
* }
* List<ulong> knownIds = new List<ulong>(_Bodies.Keys);
*
* // First delete untracked bodies
* foreach (ulong trackingId in knownIds)
* {
* if (!trackedIds.Contains(trackingId))
* {
* for (int i = 0; i < _Bodies[trackingId].Length; i++) { Destroy(_Bodies[trackingId][i]); }
* _Bodies.Remove(trackingId);
* }
* }
* foreach (var body in data)
* {
* if (body == null)
* {
* continue;
* }
*
* if (body.IsTracked)
* {
* if (!_Bodies.ContainsKey(body.TrackingId))
* {
* _Bodies[body.TrackingId] = CreateBodyObject(body.TrackingId);
* }
*
* MoveBothHands(body, _Bodies[body.TrackingId]);
* }
* }
* }
*
* private void MoveBothHands(Kinect.Body body, GameObject[] upperLimbs)
* {
* Kinect.CameraSpacePoint cameraspace;
* Vector2 vectorPoint;
* for (int i = 0; i < UpperLimbsJointTypes.Length; i++)
* {
* cameraspace = body.Joints[UpperLimbsJointTypes[i]].Position;
* // if (i == 1 || i == 4) { upperLimbs[i].transform.rotation = new Quaternion(body.JointOrientations[UpperLimbsJointTypes[i - 1]].Orientation.X, body.JointOrientations[UpperLimbsJointTypes[i - 1]].Orientation.Y, body.JointOrientations[UpperLimbsJointTypes[i - 1]].Orientation.Z, body.JointOrientations[UpperLimbsJointTypes[i - 1]].Orientation.W); }
* try
* {
* vectorPoint = Vectorize(cameraspace);
* try
* {
* upperLimbs[i].transform.position = new Vector3(vectorPoint.x, vectorPoint.y, 0);
* }
* catch { print("positioning trouble");
* }
* }
* catch { print("Missing coordinate mapper");
* }
*
* }
*
* }
*
* public CoordinateMapperManager CoordinateMapper;
*
* private Kinect.ColorSpacePoint ColorPointize(Kinect.CameraSpacePoint cameraspace)
* {
* return CoordinateMapper.MapSpaceToCoorPoints(cameraspace);
* }
*
* private static Vector2 Vectorize(Kinect.CameraSpacePoint cameraspace)
* {
* Vector2 cameraVector = new Vector2(cameraspace.X, cameraspace.Y);
* return cameraVector;
* }
*
* private GameObject[] CreateBodyObject(ulong p)
* {
* GameObject[] UpperLimbs = new GameObject[8];
* var bothHands = Instantiate(HandObject);
* bothHands.SetActive(true);
* foreach (Transform child in bothHands.transform) { child.gameObject.layer = 9; foreach (Transform grandchild in child) { grandchild.gameObject.layer = 9; foreach (Transform greatgrandchild in grandchild) { greatgrandchild.gameObject.layer = 9; foreach (Transform greatgreatgrandchild in greatgrandchild) { greatgreatgrandchild.gameObject.layer = 9; foreach (Transform lastgrandchild in greatgreatgrandchild) lastgrandchild.gameObject.layer = 9; } } } }
* bothHands.name = "BothHands" + p;
*
* try { UpperLimbs[(int)UpperLimbParts.RightHandTip] = bothHands.transform.FindChild("RightHandTip").gameObject; }
* catch { print("Missing Right HAnd"); }
* try { UpperLimbs[(int)UpperLimbParts.RightHand] = bothHands.transform.FindChild("RightHand").gameObject; }
* catch { print("Missing Right HAnd"); }
* try { UpperLimbs[(int)UpperLimbParts.RightElbow] = bothHands.transform.FindChild("RightElbow").gameObject; }
* catch { print("Missing rightelbow"); }
* try { UpperLimbs[(int)UpperLimbParts.RightShoulder] = bothHands.transform.FindChild("RightTorso").gameObject; }
* catch { print("missing right shoulder"); }
* try { UpperLimbs[(int)UpperLimbParts.LeftHandTip] = bothHands.transform.FindChild("LeftHandTip").gameObject; }
* catch { print("Missing Right HAnd"); }
* try {UpperLimbs[(int)UpperLimbParts.LeftHand] = bothHands.transform.FindChild("LeftHand").gameObject;}
* catch { print("missing lieft hand"); }
* try {UpperLimbs[(int)UpperLimbParts.LeftElbow] = bothHands.transform.FindChild("LeftElbow").gameObject;}
* catch { print("missiding left elbow"); }
* try { UpperLimbs[(int)UpperLimbParts.LeftShoulder] = bothHands.transform.FindChild("LeftTorso").gameObject; }
* catch { print("LeftShoulder");}
* return UpperLimbs;
* }
*
* enum UpperLimbParts : int
* {
* RightHand = 0,
* RightElbow = 1,
* RightShoulder = 2,
* LeftHand = 3,
* LeftElbow = 4,
* LeftShoulder = 5,
* RightHandTip = 6,
* LeftHandTip = 7
* }
*
* }
*/
//add new
GameObject go = (GameObject)Instantiate(bodyPrefab, spinePos, Quaternion.identity);
go.GetComponent <Renderer> ().material.color = new Color(Random.value, Random.value, Random.value, 1);
if (handPrefab != null)
{
GameObject hl = (GameObject)Instantiate(handPrefab, handLeftPos, Quaternion.identity);
GameObject hr = (GameObject)Instantiate(handPrefab, handRightPos, Quaternion.identity);
GameObject hd = (GameObject)Instantiate(headPrefab, headPos, Quaternion.identity);
hl.name += "[Left]" + "[" + bodyFrame.kinectId + "]";
hr.name += "[Right]" + "[" + bodyFrame.kinectId + "]";
hd.name += "[Head]" + "[" + bodyFrame.kinectId + "]";
hr.transform.parent = go.transform;
hl.transform.parent = go.transform;
hd.transform.parent = go.transform;
//Debug.Log ("Adding Tracker: " + bodyData.trackingId + " Valid: " + bodyData.valid + " " + bodyData.kinectId);
userTable.Add(bodyData.trackingId, new Body(go.transform, hl.transform, hr.transform, hd.transform, bodyData));
}
else
{
userTable.Add(bodyData.trackingId, new Body(go.transform, null, null, null, bodyData));
}
go.BroadcastMessage("SetBody", userTable [bodyData.trackingId], SendMessageOptions.DontRequireReceiver);
userCullList.Remove(bodyData.trackingId);
}
//update existing
Body body = userTable [bodyData.trackingId];
body.data.position = spinePos;
body.data.spineMidPosition = spineMidPos;
body.data.spineShoulderPosition = spineShoulderPos;
// body.data.elbowLeftPosition = elbowLeftPosition;
body.data.headPosition = headPos;
body.data.handLeftPosition = handLeftPos;
body.data.handRightPosition = handRightPos;
body.data.modifiedByMatrix = true;
body.transform.position = spinePos;
if (body.handLeftTransform != null)
{
body.handLeftTransform.position = handLeftPos;
}
if (body.handRightTransform != null)
{
body.handRightTransform.position = handRightPos;
}
if (body.headTransform != null)
{
body.headTransform.position = headPos;
}
}
foreach (ulong id in userCullList)
{
//if (bodyFrame.kinectId == 0x04)
//Debug.Log (id);
if (userTable.ContainsKey(id))
{
//Debug.Log ("Destroying: " + id + " From " + userTable [id].data.kinectId);
Destroy(userTable [id].transform.gameObject);
if (userTable [id].handLeftTransform != null)
{
Destroy(userTable [id].handLeftTransform.gameObject);
Destroy(userTable [id].handRightTransform.gameObject);
Destroy(userTable [id].headTransform.gameObject);
}
userTable.Remove(id);
}
}
}
/// <summary>
/// Handle incoming data from any kinect, only care if it matches configured Kinect ID
/// </summary>
/// <param name="bodyFrame"></param>
void OnBodyFrameDataReceived(KinectBodiesReceiver.BodyFrameData bodyFrame)
{
// make sure this is only from the kinect assigned to this station
if (bodyFrame.kinectId != kinectId)
{
return;
}
Matrix4x4 matrix = Matrix4x4.identity;
if (KinectBodiesReceiver.Instance.kinectInfoTable.ContainsKey(bodyFrame.kinectId))
{
matrix = KinectBodiesReceiver.Instance.kinectInfoTable[bodyFrame.kinectId].matrix;
}
//check to see if bodies are in playfield
// if (LastTimeBodiesInPlayfield + .3f < Time.time)
// {
if (bodyFrame.bodyData.Length > 0)
{
foreach (var bodyData in bodyFrame.bodyData)
{
//skip filtered data
if (!bodyData.valid)
{
ThisStationListener.PlayerIn(false);
continue;
}
ThisStationListener.PlayerIn(true);
}
}
else
{
ThisStationListener.PlayerIn(false);
}
// }
foreach (var bodyData in bodyFrame.bodyData)
{
//skip filtered data
if (!bodyData.valid)
{
continue;
}
// LastTimeBodiesInPlayfield = Time.time; // DS added
spinePos = matrix.MultiplyPoint3x4(bodyData.position);
spineMidPos = matrix.MultiplyPoint3x4(bodyData.spineMidPosition);
spineShoulderPos = matrix.MultiplyPoint3x4(bodyData.spineShoulderPosition);
// headPos = matrix.MultiplyPoint3x4(bodyData.headPosition);
handLeftPos = matrix.MultiplyPoint3x4(bodyData.handLeftPosition);
handRightPos = matrix.MultiplyPoint3x4(bodyData.handRightPosition);
handLeftDelta = handLeftPos - handLeftPrevious;
handRightDelta = handRightPos - handRightPrevious;
//if(kinectId == 0x04)
//Debug.Log(handRightDelta);
handLeftPrevious = handLeftPos;
handRightPrevious = handRightPos;
}
}
