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