private static Vector3D[] CalculateDisplacementHands(Body[] bodies)
{
float displacementFirstHandX = 0;
float displacementFirstHandY = 0;
float displacementFirstHandZ = 0;
float displacementSecondHandX = 0;
float displacementSecondHandY = 0;
float displacementSecondHandZ = 0;
for (int i = 1; i < bodies.Length; i++)
{
if (bodies[i].Hands[0] != null && bodies[i - 1].Hands[0] != null && !float.IsNaN(bodies[i].Hands[0].AvgCenterPointLastFrames.X) && !float.IsNaN(bodies[i - 1].Hands[0].AvgCenterPointLastFrames.X))
{
displacementFirstHandX = Math.Abs(bodies[i].Hands[0].AvgCenterPointLastFrames.X - bodies[i - 1].Hands[0].AvgCenterPointLastFrames.X);
displacementFirstHandY = Math.Abs(bodies[i].Hands[0].AvgCenterPointLastFrames.Y - bodies[i - 1].Hands[0].AvgCenterPointLastFrames.Y);
displacementFirstHandZ = Math.Abs(bodies[i].Hands[0].AvgCenterPointLastFrames.Z - bodies[i - 1].Hands[0].AvgCenterPointLastFrames.Z);
}
if (bodies[i].Hands[1] != null && bodies[i - 1].Hands[1] != null && !float.IsNaN(bodies[i].Hands[1].AvgCenterPointLastFrames.X) && !float.IsNaN(bodies[i - 1].Hands[1].AvgCenterPointLastFrames.X))
{
displacementSecondHandX = Math.Abs(bodies[i].Hands[1].AvgCenterPointLastFrames.X - bodies[i - 1].Hands[1].AvgCenterPointLastFrames.X);
displacementSecondHandY = Math.Abs(bodies[i].Hands[1].AvgCenterPointLastFrames.Y - bodies[i - 1].Hands[1].AvgCenterPointLastFrames.Y);
displacementSecondHandZ = Math.Abs(bodies[i].Hands[1].AvgCenterPointLastFrames.Z - bodies[i - 1].Hands[1].AvgCenterPointLastFrames.Z);
}
}
var displacementFirstHand = new Vector3D(displacementFirstHandX,displacementFirstHandY,displacementFirstHandZ);
var displacementSecondHand = new Vector3D(displacementSecondHandX,displacementSecondHandY,displacementSecondHandZ);
return new[]{displacementFirstHand,displacementSecondHand};
}
/// <summary>
/// Draw bodyelements in detection frame for debugging purposes.
/// </summary>
public static void Draw(Body body)
{
if (ShowHandCenterPoint)
{
Hand firstHand = body.Hands[0];
if (firstHand != null)
{
GraphicsUtils.DrawPoint(firstHand.CenterPoint);
}
Hand secondHand = body.Hands[1];
if (secondHand != null)
{
GraphicsUtils.DrawPoint(secondHand.CenterPoint);
}
}
if (ShowHandAvgPoint)
{
Hand firstHand = body.Hands[0];
if (firstHand != null && !float.IsNaN(firstHand.AvgCenterPointLastFrames.Z))
{
GraphicsUtils.DrawPoint(firstHand.AvgCenterPointLastFrames);
}
Hand secondHand = body.Hands[1];
if (secondHand != null && !float.IsNaN(secondHand.AvgCenterPointLastFrames.Z))
{
GraphicsUtils.DrawPoint(secondHand.AvgCenterPointLastFrames);
}
}
if (ShowTorsoCenterPoint)
{
if (body.Torso != null && !float.IsNaN(body.Torso.CenterPoint.Z))
{
GraphicsUtils.DrawPoint(body.Torso.CenterPoint);
}
}
if (ShowTorsoAvgCenterPoint)
{
if (body.Torso != null && !float.IsNaN(body.Torso.AvgCenterPoint.Z))
{
GraphicsUtils.DrawPoint(body.Torso.AvgCenterPoint);
}
}
if (ShowHeadCenterPoint)
{
GraphicsUtils.DrawPoint(body.Head.CenterPoint);
}
}
/// <summary>
/// Attributes the body object with the calculated mechanical energy-use since last frame.
/// </summary>
public static void CalculateBodyMechanicalEnergy(Body body)
{
bool handTracking = false;
List<Body> lastBodies = BodyUtils.GetBodiesWithIdFromHistory(body.Id);
if (lastBodies.Count < 1)
{
body.EnergyLevel = 0;
return;
}
Body[] lastTwoBodies = new Body[2];
lastTwoBodies[0] = lastBodies[lastBodies.Count - 1];
lastTwoBodies[1] = body;
double totalPotential = 0;
double totalKinetic = 0;
Vector3D headDisplacement = CalculateDisplacementHead(lastTwoBodies);
totalPotential += CalculatePotentialEnergyChange(headDisplacement, HeadMass);
totalKinetic += CalculateKineticEnergyChange(headDisplacement, HeadMass, lastTwoBodies);
Vector3D torsoDisplacement = CalculateDisplacementTorso(lastTwoBodies);
totalPotential += CalculatePotentialEnergyChange(torsoDisplacement, TorsoMass);
totalKinetic += CalculateKineticEnergyChange(torsoDisplacement, TorsoMass, lastTwoBodies);
if (handTracking)
{
Vector3D[] handDisplacement = CalculateDisplacementHands(lastTwoBodies);
totalPotential += CalculatePotentialEnergyChange(handDisplacement[0], ForeArmMass);
totalKinetic += CalculateKineticEnergyChange(handDisplacement[0], ForeArmMass, lastTwoBodies);
totalPotential += CalculatePotentialEnergyChange(handDisplacement[1], ForeArmMass);
totalKinetic += CalculateKineticEnergyChange(handDisplacement[1], ForeArmMass, lastTwoBodies);
}
body.EnergyLevel = totalKinetic + totalPotential;
}
/// <summary>
/// Uses Dijkstra to find the shortest geodesic path from the top of the head to the rest of the body. The body parts
/// torso and hands are classified as the points from the geodesic map that fall into bins with a certain distance from
/// the top of the head.
/// </summary>
/// <returns>Returns a dictionary with the heads as keys and labels as values. If head isn't identified, the label is -1</returns>
public static void AddBodyRegions(Dictionary<int, Dictionary<int, float>> geodesicGraph, Head head, Body body)
{
var startIndex = head.HighestPointIndex;
if (!geodesicGraph.ContainsKey(startIndex))
{
startIndex = GeodesicUtils.GetClosestValidNeighbourInGeodesicGraph(startIndex);
}
if (startIndex == -1)
{
return;
}
var minDistances = Dijkstra.Perform(startIndex, geodesicGraph);
var handPoints = new List<int>();
var torsoPoints = new List<int>();
foreach (var minDistance in minDistances)
{
if (minDistance.Value < 1.1f)
{
// Include body in current heatcanvas
GlobVar.HeatCanvas[minDistance.Key] = body.Id;
}
if (minDistance.Value < 0.2f)
{
if (Logger.ShowHeadRegionPixels)
{
GlobVar.GraphicsCanvas[minDistance.Key] = 255;
}
torsoPoints.Add(minDistance.Key);
}
else if (minDistance.Value < 0.6f && minDistance.Value > 0.2f)
{
if (Logger.ShowTorsoPixels)
{
GlobVar.GraphicsCanvas[minDistance.Key] = 200;
}
torsoPoints.Add(minDistance.Key);
}
else if (minDistance.Value < 1.2f && minDistance.Value > 0.9f)
{
if (Logger.ShowHandPixels)
{
GlobVar.GraphicsCanvas[minDistance.Key] = 150;
}
handPoints.Add(minDistance.Key);
}
}
if (torsoPoints.Count > Thresholds.TorsoPointMinCount)
{
body.AddTorso(torsoPoints);
}
if (handPoints.Count > Thresholds.HandPointMinCount)
{
body.AddHands(handPoints);
}
}
private List<Body> CreateBodies(TimeSpan frameRelativeTime, List<Head> validatedHeads)
{
_stopwatch.Restart();
Dictionary<Head, int> identifiedHeadsWithLabels = BodyUtils.IdentifyHeads(validatedHeads);
var bodies = new List<Body>();
foreach (var identifiedHead in identifiedHeadsWithLabels)
{
Stopwatch stopwatchBody = new Stopwatch();
stopwatchBody.Start();
Body body;
int bodyId = identifiedHead.Value;
Head head = identifiedHead.Key;
if (bodyId == -1)
{
GlobVar.MaxBodyCount++;
int newBodyId = GlobVar.MaxBodyCount;
body = new Body(newBodyId, frameRelativeTime);
body.AddHead(head);
bodies.Add(body);
BodyUtils.AddBodyRegions(GeodesicUtils.GeodesicGraph, head, body);
}
else
{
// Attribute head with average centerpoint in from the last
// frames.
head.AvgCenterPoint = BodyUtils.GetAverageHeadLocationLastFrames(bodyId);
if (Logger.ShowHeadAvgCenterPoint)
{
GraphicsUtils.DrawPoint(head.AvgCenterPoint);
}
body = new Body(bodyId, frameRelativeTime);
body.AddHead(head);
bodies.Add(body);
BodyUtils.AddBodyRegions(GeodesicUtils.GeodesicGraph, head, body);
}
Logger.Draw(body);
EnergyUtils.CalculateBodyMechanicalEnergy(body);
}
if (Logger.LogTimeCreateBodies)
{
_stopwatch.Stop();
Console.WriteLine("CreateBodies: {0}", _stopwatch.ElapsedMilliseconds);
}
return bodies;
}
private static double CalculateKineticEnergyChange(Vector3D displacement, double mass, Body[] bodies)
{
if (double.IsNaN(displacement.Z))
{
return 0;
}
TimeSpan timeStampStart = bodies[0].TimeStamp;
TimeSpan timeStampStop = bodies[1].TimeStamp;
double deltaTime = timeStampStop.Subtract(timeStampStart).TotalSeconds;
if (deltaTime == 0)
{
return 0;
}
double velocityX = displacement.X/deltaTime;
double velocityY = displacement.Y/deltaTime;
double kineticEnergyX = 0.5*mass*velocityX*velocityX;
double kineticEnergyY = 0.5*mass*velocityY*velocityY;
return kineticEnergyX + kineticEnergyY;
}
private static Vector3D CalculateDisplacementTorso(Body[] bodies)
{
float displacementX = 0;
float displacementY = 0;
float displacementZ = 0;
for (int i = 1; i < bodies.Length; i++)
{
if (bodies[i].Torso != null && bodies[i-1].Torso != null && !float.IsNaN(bodies[i].Torso.AvgCenterPoint.X) && !float.IsNaN(bodies[i - 1].Torso.AvgCenterPoint.X))
{
displacementX += Math.Abs(bodies[i].Torso.AvgCenterPoint.X - bodies[i - 1].Torso.AvgCenterPoint.X);
displacementY += Math.Abs(bodies[i].Torso.AvgCenterPoint.Y - bodies[i - 1].Torso.AvgCenterPoint.Y);
displacementZ += Math.Abs(bodies[i].Torso.AvgCenterPoint.Z - bodies[i - 1].Torso.AvgCenterPoint.Z);
}
}
return new Vector3D(displacementX, displacementY, displacementZ);
}
private static Vector3D CalculateDisplacementHead(Body[] bodies)
{
float displacementX = 0;
float displacementY = 0;
float displacementZ = 0;
displacementX += Math.Abs(bodies[0].Head.AvgCenterPoint.X - bodies[1].Head.AvgCenterPoint.X);
displacementY += Math.Abs(bodies[0].Head.AvgCenterPoint.Y - bodies[1].Head.AvgCenterPoint.Y);
displacementZ += Math.Abs(bodies[0].Head.AvgCenterPoint.Z - bodies[1].Head.AvgCenterPoint.Z);
return new Vector3D(displacementX, displacementY, displacementZ);
}