public void ManageHijackedBodies() { var writeOutput = false; #region handle body Hi jacking //handle when a body walks in front of another body and takes the initial bodies body //when this occurs the initial body may get it's body back or the initial body will eventually get a new body var hijackedBodies = _trackedBodies.Where(tb => tb.Key.DetectBodyHijack(tb.Value.Joints[BodyPositionJoint].Position) && !_trackedBodiesHijacked.Any(tbHijacked => tbHijacked.CurrentPlayerTrackingId == tb.Key.CurrentPlayerTrackingId) && !_trackedBodiesMissing.Any(tbMissing => tbMissing.CurrentPlayerTrackingId == tb.Key.CurrentPlayerTrackingId)); if (hijackedBodies != null && hijackedBodies.Count() > 0) { foreach (var hijackedBody in hijackedBodies) { if (!_trackedBodiesHijacked.Contains(hijackedBody.Key)) { var newHijackedBody = new TrackedBody(this) { BodyPosition = new CameraSpacePoint() { X = hijackedBody.Key.BodyPosition.X, Y = hijackedBody.Key.BodyPosition.Y, Z = hijackedBody.Key.BodyPosition.Z }, }; foreach (var trackedPlayerId in hijackedBody.Key.PlayerTrackingIds) { newHijackedBody.PlayerTrackingIds.Add(trackedPlayerId); } newHijackedBody.TimeWentMissing = DateTime.Now; _trackedBodiesHijacked.Add(newHijackedBody); } } } CleanupHijackedBodies(); #endregion handle body Hi jacking }
public void ManageMissingBodies(Body[] bodies) { bool writeOutput = false; #region handle missing Bodies //A Missing Body Candidate is initially identified by a TrackedBody that has a .Key.CurrentPlayerTrackingId that does not match the body.Tracking Id //we could also use IsTracked flag (body.IsTracked and Key.CurrentPlayerTrackingId != 0) //once we have a candidate missing body we need to check if it is missing because it disappaearred while in the viewing area or if it just gradually- walked out of the viewing area var missingTrackedBodies = (from trackedBodies in _trackedBodies where !bodies.Any(b => b.TrackingId == trackedBodies.Key.CurrentPlayerTrackingId) && !_trackedBodiesMissing.Any(tbMissing => tbMissing.CurrentPlayerTrackingId == trackedBodies.Key.CurrentPlayerTrackingId) && !_trackedBodiesHijacked.Any(tbHijacked => tbHijacked.CurrentPlayerTrackingId == trackedBodies.Key.CurrentPlayerTrackingId) select trackedBodies); foreach (var missingTrackedBody in missingTrackedBodies) { //check if the body was within the viewing area before being lost, if it was assume it's missing due to someone walking in front of it if (missingTrackedBody.Key.BodyPosition.X > MissingBodyLeftEdgeBoundary && missingTrackedBody.Key.BodyPosition.X < MissingBodyRightEdgeBoundary && missingTrackedBody.Key.BodyPosition.Z < MissingBodyBackDepthBoundary) { #region debug output #if DEBUG { if (writeOutput) { System.Diagnostics.Debug.Print("MISSING FromTrackedBody - CorrelatedPlayerId:{0}, CurrentPlayerTrackingId{1}, TrackedBodyX:{2}, TrackedBodyY:{3}, TrackedBodyZ{4}" , missingTrackedBody.Key.CorrelationPlayerId , missingTrackedBody.Key.CurrentPlayerTrackingId , missingTrackedBody.Key.BodyPosition.X , missingTrackedBody.Key.BodyPosition.Y , missingTrackedBody.Key.BodyPosition.Z); foreach (var trackingId in missingTrackedBody.Key.PlayerTrackingIds) { System.Diagnostics.Debug.Print("MISSING FromTrackedBody - PlayerTrackingIds for CorrelatedPlayerId: {0} - {1}", missingTrackedBody.Key.CorrelationPlayerId, trackingId); } } } #endif #endregion debug output //create missing body and add to missing body collection var newMissingBody = new TrackedBody(this) { BodyPosition = new CameraSpacePoint() { X = missingTrackedBody.Key.BodyPosition.X, Y = missingTrackedBody.Key.BodyPosition.Y, Z = missingTrackedBody.Key.BodyPosition.Z }, }; foreach (var trackedPlayerId in missingTrackedBody.Key.PlayerTrackingIds) { newMissingBody.PlayerTrackingIds.Add(trackedPlayerId); } newMissingBody.TimeWentMissing = DateTime.Now; _trackedBodiesMissing.Add(newMissingBody); #region debug output #if DEBUG { if (writeOutput) { System.Diagnostics.Debug.Print("MissingBody Added - CorrelatedPlayerID:{0}, CurrentPlayerId{1}, BodyPosition.X{2}, BodyPosition.Y{3}, BodyPosition.Z{4}, TimeWentMissing{5}" , newMissingBody.CorrelationPlayerId , newMissingBody.CurrentPlayerTrackingId , newMissingBody.BodyPosition.X , newMissingBody.BodyPosition.Y , newMissingBody.BodyPosition.Z , newMissingBody.TimeWentMissing.ToString()); foreach (var trackingId in newMissingBody.PlayerTrackingIds) { System.Diagnostics.Debug.Print("MissingBody Added - PlayerTrackingIds for CorrelatedPlayerId: {0} - {1}", newMissingBody.CorrelationPlayerId, trackingId); } } } #endif #endregion end debug output } //sync up the TrackedBody associated with the missing body with it's new body missingTrackedBody.Key.PlayerTrackingIds.Clear(); missingTrackedBody.Key.PlayerTrackingIds.Add(missingTrackedBody.Value.TrackingId); missingTrackedBody.Key.BodyPosition = missingTrackedBody.Value.Joints[BodyPositionJoint].Position; } #endregion handle missing bodies #region clean up Missing Bodies CleanupMissingBodies(); #endregion clean up Missing Bodies }
/// <summary> /// Manages Body Tracking Id by detecting when a body moves is front of an existing body and the existing body loses it's body /// </summary> /// <param name="bodies">The Kinect bodies collection</param> public void TrackBodies(Body[] bodies) { bool writeOutput = false; //if our body count hasn't changed just return null //we're doing this for performance //this could fail if someone quickly runs in front of a existing body or if an existing player leaves quickly and a new player enters quickly at the same time if (bodies.Where(b => b.IsTracked).Count() == TrackedKinectBodyCount) { CleanupMissingBodies(); CleanupHijackedBodies(); return; } #region debug output #if DEBUG { if (writeOutput) { System.Diagnostics.Debug.Print("-------------------- Tracking Bodies ---------------------- "); var trackedBodies = bodies.Where(b => b.IsTracked); System.Diagnostics.Debug.Print("trackedBodies Count:{0}", trackedBodies.Count()); var ghostBodies = bodies.Where(b => b.Joints.Count(joint => joint.Key == JointType.Head && joint.Value.TrackingState == TrackingState.Tracked || joint.Key == JointType.Neck && joint.Value.TrackingState == TrackingState.Tracked || joint.Key == JointType.ShoulderLeft && joint.Value.TrackingState == TrackingState.Tracked || joint.Key == JointType.ShoulderRight && joint.Value.TrackingState == TrackingState.Tracked || joint.Key == JointType.ElbowLeft && joint.Value.TrackingState == TrackingState.Tracked || joint.Key == JointType.ElbowRight && joint.Value.TrackingState == TrackingState.Tracked || joint.Key == JointType.WristLeft && joint.Value.TrackingState == TrackingState.Tracked || joint.Key == JointType.WristRight && joint.Value.TrackingState == TrackingState.Tracked) >= RequiredJointsTrackedCount); System.Diagnostics.Debug.Print("ghostBodies Count:{0}", ghostBodies.Count()); } if (writeOutput && _trackedBodiesMissing != null && _trackedBodiesMissing.Count() > 0) { foreach (var missingBody in _trackedBodiesMissing) { System.Diagnostics.Debug.Print("MissingBody Output - CorrelatedPlayerId:{0}, CurrentPlayerTrackingId:{1}, BodyPosition.X:{2}, BodyPosition.Y:{3}, BodyPosition.Z{4}, TimeWentMissing{5}", missingBody.CorrelationPlayerId , missingBody.CurrentPlayerTrackingId , missingBody.BodyPosition.X , missingBody.BodyPosition.Y , missingBody.BodyPosition.Z , missingBody.TimeWentMissing.ToString()); foreach (var trackingId in missingBody.PlayerTrackingIds) { System.Diagnostics.Debug.Print("MissingBody Output PlayerTrackingIds for CorrelatedPlayerID: {0} - {1}", missingBody.CorrelationPlayerId, trackingId); } } } else if (writeOutput) { System.Diagnostics.Debug.Print("MissingBody Output - there are no missing bodies "); } if (writeOutput && bodies.Any(b => b.IsTracked) && _trackedBodies != null) { foreach (var body in bodies.Where(b => b.IsTracked)) { System.Diagnostics.Debug.Print("Body - IsTracked:{0}, TrackingId:{1}, X:{2}, Y:{3}, Z{4}", body.IsTracked, body.TrackingId, body.Joints[BodyPositionJoint].Position.X, body.Joints[BodyPositionJoint].Position.Y, body.Joints[BodyPositionJoint].Position.Z); } foreach (var tb in _trackedBodies) { System.Diagnostics.Debug.Print("TrackedBody - BodyIsTracked:{0}, BodyTrackingId:{1}, TrackedBodyCorrelatedPlayerId:{2}, TrackedBodyCurrentPlayerTrackingId:{3}, BodyX:{4}, BodyY:{5}, BodyZ{6}, TrackedBodyX:{7}, TrackedBodyY:{8}, TrackedBodyZ{9}" , tb.Value.IsTracked , tb.Value.TrackingId , tb.Key.CorrelationPlayerId , tb.Key.CurrentPlayerTrackingId , tb.Value.Joints[BodyPositionJoint].Position.X , tb.Value.Joints[BodyPositionJoint].Position.Y , tb.Value.Joints[BodyPositionJoint].Position.Z , tb.Key.BodyPosition.X , tb.Key.BodyPosition.Y , tb.Key.BodyPosition.Z); foreach (var trackingId in tb.Key.PlayerTrackingIds) { System.Diagnostics.Debug.Print("PlayerTrackingIds for CorrelatedPlayerID: {0} - {1}", tb.Key.CorrelationPlayerId, trackingId); } } } } #endif //DEBUG #endregion debug output if (bodies != null && bodies.Length > 0) { //first time in, just add bodies to TrackedBodies collection if (_trackedBodies.Count() == 0) { foreach (var body in bodies) { var trackedBody = new TrackedBody(this) { BodyPosition = body.Joints[BodyPositionJoint].Position, }; trackedBody.PlayerTrackingIds.Add(body.TrackingId); _trackedBodies.Add(trackedBody, body); } } else { //scenarios 1 - Missing Body - A second player body slowly passes in front of an existing player, //in this scenario, the first body losses it's tracking and get's a new body assigned //scenario 2 - Hijacked Body - A second body quickly passes in front of an existing body and takes the body of the first player #region debug output #if DEBUG { if (writeOutput && TrackedKinectBodyCount != bodies.Where(b => b.IsTracked).Count()) { System.Diagnostics.Debug.Print("BodyCount changed from {0} to {1}", TrackedKinectBodyCount, bodies.Where(b => b.IsTracked).Count()); } } #endif #endregion debug output ManageMissingBodies(bodies); ManageHijackedBodies(); ManageNewBodies(); TrackedKinectBodyCount = bodies.Where(b => b.IsTracked).Count(); _trackedBodiesCount = _trackedBodies.Where(tb => tb.Key.CurrentPlayerTrackingId != 0).Count(); } } }
/// <summary> /// Manages Body Tracking Id by detecting when a body moves is front of an existing body and the existing body loses it's body /// </summary> /// <param name="bodies">The Kinect bodies collection</param> public void TrackBodies(Body[] bodies) { bool writeOutput = false; //if our body count hasn't changed just return null //we're doing this for performance //this could fail if someone quickly runs in front of a existing body or if an existing player leaves quickly and a new player enters quickly at the same time if (bodies.Where(b => b.IsTracked).Count() == TrackedKinectBodyCount) { CleanupMissingBodies(); CleanupHijackedBodies(); return; } #region debug output #if DEBUG { if (writeOutput) { System.Diagnostics.Debug.Print("-------------------- Tracking Bodies ---------------------- "); var trackedBodies = bodies.Where(b => b.IsTracked); System.Diagnostics.Debug.Print("trackedBodies Count:{0}", trackedBodies.Count()); var ghostBodies = bodies.Where(b => b.Joints.Count(joint => joint.Key == JointType.Head && joint.Value.TrackingState == TrackingState.Tracked || joint.Key == JointType.Neck && joint.Value.TrackingState == TrackingState.Tracked || joint.Key == JointType.ShoulderLeft && joint.Value.TrackingState == TrackingState.Tracked || joint.Key == JointType.ShoulderRight && joint.Value.TrackingState == TrackingState.Tracked || joint.Key == JointType.ElbowLeft && joint.Value.TrackingState == TrackingState.Tracked || joint.Key == JointType.ElbowRight && joint.Value.TrackingState == TrackingState.Tracked || joint.Key == JointType.WristLeft && joint.Value.TrackingState == TrackingState.Tracked || joint.Key == JointType.WristRight && joint.Value.TrackingState == TrackingState.Tracked) >= RequiredJointsTrackedCount); System.Diagnostics.Debug.Print("ghostBodies Count:{0}", ghostBodies.Count()); } if (writeOutput && _trackedBodiesMissing != null && _trackedBodiesMissing.Count() > 0) { foreach (var missingBody in _trackedBodiesMissing) { System.Diagnostics.Debug.Print("MissingBody Output - CorrelatedPlayerId:{0}, CurrentPlayerTrackingId:{1}, BodyPosition.X:{2}, BodyPosition.Y:{3}, BodyPosition.Z{4}, TimeWentMissing{5}", missingBody.CorrelationPlayerId , missingBody.CurrentPlayerTrackingId , missingBody.BodyPosition.X , missingBody.BodyPosition.Y , missingBody.BodyPosition.Z , missingBody.TimeWentMissing.ToString()); foreach (var trackingId in missingBody.PlayerTrackingIds) System.Diagnostics.Debug.Print("MissingBody Output PlayerTrackingIds for CorrelatedPlayerID: {0} - {1}", missingBody.CorrelationPlayerId, trackingId); } } else if (writeOutput) System.Diagnostics.Debug.Print("MissingBody Output - there are no missing bodies "); if (writeOutput && bodies.Any(b => b.IsTracked) && _trackedBodies != null) { foreach (var body in bodies.Where(b => b.IsTracked)) System.Diagnostics.Debug.Print("Body - IsTracked:{0}, TrackingId:{1}, X:{2}, Y:{3}, Z{4}", body.IsTracked, body.TrackingId, body.Joints[BodyPositionJoint].Position.X, body.Joints[BodyPositionJoint].Position.Y, body.Joints[BodyPositionJoint].Position.Z); foreach (var tb in _trackedBodies) { System.Diagnostics.Debug.Print("TrackedBody - BodyIsTracked:{0}, BodyTrackingId:{1}, TrackedBodyCorrelatedPlayerId:{2}, TrackedBodyCurrentPlayerTrackingId:{3}, BodyX:{4}, BodyY:{5}, BodyZ{6}, TrackedBodyX:{7}, TrackedBodyY:{8}, TrackedBodyZ{9}" , tb.Value.IsTracked , tb.Value.TrackingId , tb.Key.CorrelationPlayerId , tb.Key.CurrentPlayerTrackingId , tb.Value.Joints[BodyPositionJoint].Position.X , tb.Value.Joints[BodyPositionJoint].Position.Y , tb.Value.Joints[BodyPositionJoint].Position.Z , tb.Key.BodyPosition.X , tb.Key.BodyPosition.Y , tb.Key.BodyPosition.Z); foreach (var trackingId in tb.Key.PlayerTrackingIds) System.Diagnostics.Debug.Print("PlayerTrackingIds for CorrelatedPlayerID: {0} - {1}", tb.Key.CorrelationPlayerId, trackingId); } } } #endif //DEBUG #endregion debug output if (bodies != null && bodies.Length > 0) { //first time in, just add bodies to TrackedBodies collection if (_trackedBodies.Count() == 0) { foreach (var body in bodies) { var trackedBody = new TrackedBody(this) { BodyPosition = body.Joints[BodyPositionJoint].Position, }; trackedBody.PlayerTrackingIds.Add(body.TrackingId); _trackedBodies.Add(trackedBody, body); } } else { //scenarios 1 - Missing Body - A second player body slowly passes in front of an existing player, //in this scenario, the first body losses it's tracking and get's a new body assigned //scenario 2 - Hijacked Body - A second body quickly passes in front of an existing body and takes the body of the first player #region debug output #if DEBUG { if (writeOutput && TrackedKinectBodyCount != bodies.Where(b => b.IsTracked).Count()) System.Diagnostics.Debug.Print("BodyCount changed from {0} to {1}", TrackedKinectBodyCount, bodies.Where(b => b.IsTracked).Count()); } #endif #endregion debug output ManageMissingBodies(bodies); ManageHijackedBodies(); ManageNewBodies(); TrackedKinectBodyCount = bodies.Where(b => b.IsTracked).Count(); _trackedBodiesCount = _trackedBodies.Where(tb => tb.Key.CurrentPlayerTrackingId != 0).Count(); } } }
public void ManageMissingBodies(Body[] bodies) { bool writeOutput = false; #region handle missing Bodies //A Missing Body Candidate is initially identified by a TrackedBody that has a .Key.CurrentPlayerTrackingId that does not match the body.Tracking Id //we could also use IsTracked flag (body.IsTracked and Key.CurrentPlayerTrackingId != 0) //once we have a candidate missing body we need to check if it is missing because it disappaearred while in the viewing area or if it just gradually- walked out of the viewing area var missingTrackedBodies = (from trackedBodies in _trackedBodies where !bodies.Any(b => b.TrackingId == trackedBodies.Key.CurrentPlayerTrackingId) && !_trackedBodiesMissing.Any(tbMissing => tbMissing.CurrentPlayerTrackingId == trackedBodies.Key.CurrentPlayerTrackingId) && !_trackedBodiesHijacked.Any(tbHijacked => tbHijacked.CurrentPlayerTrackingId == trackedBodies.Key.CurrentPlayerTrackingId) select trackedBodies); foreach (var missingTrackedBody in missingTrackedBodies) { //check if the body was within the viewing area before being lost, if it was assume it's missing due to someone walking in front of it if (missingTrackedBody.Key.BodyPosition.X > MissingBodyLeftEdgeBoundary && missingTrackedBody.Key.BodyPosition.X < MissingBodyRightEdgeBoundary && missingTrackedBody.Key.BodyPosition.Z < MissingBodyBackDepthBoundary) { #region debug output #if DEBUG { if (writeOutput) { System.Diagnostics.Debug.Print("MISSING FromTrackedBody - CorrelatedPlayerId:{0}, CurrentPlayerTrackingId{1}, TrackedBodyX:{2}, TrackedBodyY:{3}, TrackedBodyZ{4}" , missingTrackedBody.Key.CorrelationPlayerId , missingTrackedBody.Key.CurrentPlayerTrackingId , missingTrackedBody.Key.BodyPosition.X , missingTrackedBody.Key.BodyPosition.Y , missingTrackedBody.Key.BodyPosition.Z); foreach (var trackingId in missingTrackedBody.Key.PlayerTrackingIds) System.Diagnostics.Debug.Print("MISSING FromTrackedBody - PlayerTrackingIds for CorrelatedPlayerId: {0} - {1}", missingTrackedBody.Key.CorrelationPlayerId, trackingId); } } #endif #endregion debug output //create missing body and add to missing body collection var newMissingBody = new TrackedBody(this) { BodyPosition = new CameraSpacePoint() { X = missingTrackedBody.Key.BodyPosition.X, Y = missingTrackedBody.Key.BodyPosition.Y, Z = missingTrackedBody.Key.BodyPosition.Z }, }; foreach (var trackedPlayerId in missingTrackedBody.Key.PlayerTrackingIds) { newMissingBody.PlayerTrackingIds.Add(trackedPlayerId); } newMissingBody.TimeWentMissing = DateTime.Now; _trackedBodiesMissing.Add(newMissingBody); #region debug output #if DEBUG { if (writeOutput) { System.Diagnostics.Debug.Print("MissingBody Added - CorrelatedPlayerID:{0}, CurrentPlayerId{1}, BodyPosition.X{2}, BodyPosition.Y{3}, BodyPosition.Z{4}, TimeWentMissing{5}" , newMissingBody.CorrelationPlayerId , newMissingBody.CurrentPlayerTrackingId , newMissingBody.BodyPosition.X , newMissingBody.BodyPosition.Y , newMissingBody.BodyPosition.Z , newMissingBody.TimeWentMissing.ToString()); foreach (var trackingId in newMissingBody.PlayerTrackingIds) System.Diagnostics.Debug.Print("MissingBody Added - PlayerTrackingIds for CorrelatedPlayerId: {0} - {1}", newMissingBody.CorrelationPlayerId, trackingId); } } #endif #endregion end debug output } //sync up the TrackedBody associated with the missing body with it's new body missingTrackedBody.Key.PlayerTrackingIds.Clear(); missingTrackedBody.Key.PlayerTrackingIds.Add(missingTrackedBody.Value.TrackingId); missingTrackedBody.Key.BodyPosition = missingTrackedBody.Value.Joints[BodyPositionJoint].Position; } #endregion handle missing bodies #region clean up Missing Bodies #region debug output //#if DEBUG // { // if (writeOutput) // { // System.Diagnostics.Debug.Print("TrackedMissingBody Count before Removing expired missing bodies {0}", TrackedBodiesMissing.Count()); // System.Diagnostics.Debug.Print("TrackedMissingBodies Count to be removed:{0}", TrackedBodiesMissing.Where(tb => tb.TimeWentMissing.Value.AddSeconds(+_missingBodyExpiredTimeLimit) < DateTime.Now).Count()); // foreach (var trackedMissingBody in TrackedBodiesMissing.Where(tb => tb.TimeWentMissing.Value.AddSeconds(+_missingBodyExpiredTimeLimit) < DateTime.Now)) // System.Diagnostics.Debug.Print("MissingBody to be Removed - Missing Body CorrelatedPlayerId:{0}, CurrentPlayerId:{1}", trackedMissingBody.CorrelationPlayerId, trackedMissingBody.CurrentPlayerTrackingId); // } // } //#endif #endregion debug output CleanupMissingBodies(); #region debug output //#if DEBUG // { // if (writeOutput) // { // System.Diagnostics.Debug.Print("TrackedMissingBody Count after Removing expired missing bodies {0}", TrackedBodiesMissing.Count()); // if (TrackedBodiesCount != TrackedBodies.Where(tb => tb.Key.CurrentPlayerTrackingId != 0).Count()) // System.Diagnostics.Debug.Print("TrackedBodyCount changed from {0} to {1}", TrackedBodiesCount, TrackedBodies.Where(tb => tb.Key.CurrentPlayerTrackingId != 0).Count()); // } // } //#endif #endregion debug output #endregion clean up Missing Bodies }
public void ManageHijackedBodies() { var writeOutput = false; #region handle body Hi jacking //handle when a body walks in front of another body and takes the initial bodies body //when this occurs the initial body may get it's body back or the initial body will eventually get a new body var hijackedBodies = _trackedBodies.Where(tb => tb.Key.DetectBodyHijack(tb.Value.Joints[BodyPositionJoint].Position) && !_trackedBodiesHijacked.Any(tbHijacked => tbHijacked.CurrentPlayerTrackingId == tb.Key.CurrentPlayerTrackingId) && !_trackedBodiesMissing.Any(tbMissing => tbMissing.CurrentPlayerTrackingId == tb.Key.CurrentPlayerTrackingId)); if (hijackedBodies != null && hijackedBodies.Count() > 0) { if (writeOutput) System.Diagnostics.Debug.Print("Body Hijack Detected for BodyTrackingId:{0}", hijackedBodies.First().Value.TrackingId); foreach (var hijackedBody in hijackedBodies) { if (!_trackedBodiesHijacked.Contains(hijackedBody.Key)) { var newHijackedBody = new TrackedBody(this) { BodyPosition = new CameraSpacePoint() { X = hijackedBody.Key.BodyPosition.X, Y = hijackedBody.Key.BodyPosition.Y, Z = hijackedBody.Key.BodyPosition.Z }, }; foreach (var trackedPlayerId in hijackedBody.Key.PlayerTrackingIds) { newHijackedBody.PlayerTrackingIds.Add(trackedPlayerId); } newHijackedBody.TimeWentMissing = DateTime.Now; _trackedBodiesHijacked.Add(newHijackedBody); } } } CleanupHijackedBodies(); #endregion handle body Hi jacking }