private void UpdateBodyTrack()
{
var handler = this.BodyTrackUpdate;
if (handler == null)
{
return;
}
var others = _bodyTrackingService.GetOtherBodies(this._bodies);
var bodyTrack = new float[others.Count() + 1];
bodyTrack[0] = (_bodyTrackingService.ActiveBody != null) ? _coordinateMapper.MapCameraPointToDepthSpace(_bodyTrackingService.ActiveBody.Joints[JointType.Neck].Position).X : 0;
for (int i = 0; i < others.Count(); i++)
{
bodyTrack[i + 1] = _coordinateMapper.MapCameraPointToDepthSpace(others[i].Joints[JointType.Neck].Position).X;
}
handler(this, new BodyTrackEventArgs()
{
BodyTrack = bodyTrack
});
}
internal static void DrawBone(DrawingContext drawingContext, System.Windows.Media.Brush brush, JointType startJoint, JointType endJoint, IReadOnlyDictionary <JointType, Joint> joints, System.Windows.Rect rect, CoordinateMapper coordinateMapper, bool useDepthSpace = true, double line = 0.8F)
{
if (joints[startJoint].TrackingState != TrackingState.Tracked &&
joints[endJoint].TrackingState != TrackingState.Tracked)
{
return;
}
System.Windows.Point startPoint;
System.Windows.Point endPoint;
if (useDepthSpace)
{
startPoint = coordinateMapper.MapCameraPointToDepthSpace(joints[startJoint].Position).GetPoint();
endPoint = coordinateMapper.MapCameraPointToDepthSpace(joints[endJoint].Position).GetPoint();
}
else
{
startPoint = coordinateMapper.MapCameraPointToColorSpace(joints[startJoint].Position).GetPoint();
endPoint = coordinateMapper.MapCameraPointToColorSpace(joints[endJoint].Position).GetPoint();
}
if (rect.Contains(startPoint) && rect.Contains(endPoint))
{
drawingContext.DrawLine(new System.Windows.Media.Pen(brush, line), startPoint, endPoint);
}
}
public void MapCameraPointsToDepthSpace(Vector3[] cameraPoints, Vector2[] depthPoints)
{
for (int i = 0; i < cameraPoints.Length && i < depthPoints.Length; i++)
{
var depthSpacePoint =
_coordinateMapper.MapCameraPointToDepthSpace(new CameraSpacePoint()
{
X = cameraPoints[i].X,
Y = cameraPoints[i].Y,
Z = cameraPoints[i].Z
});
depthPoints[i] = new Vector2(depthSpacePoint.X, depthSpacePoint.Y);
}
}
/// <summary>
/// Converts the specified CameraSpacePoint into a 2-D point.
/// </summary>
/// <param name="position">The CameraSpacePoint to convert.</param>
/// <param name="visualization">The type of the conversion (color, depth, or infrared).</param>
/// <param name="coordinateMapper">The CoordinateMapper to make the conversion.</param>
/// <returns>The corresponding 2-D point.</returns>
public static Point ToPoint(this CameraSpacePoint position, Visualization visualization, CoordinateMapper coordinateMapper)
{
Point point = new Point();
switch (visualization)
{
case Visualization.Color:
{
ColorSpacePoint colorPoint = coordinateMapper.MapCameraPointToColorSpace(position);
point.X = (int)(float.IsInfinity(colorPoint.X) ? 0.0 : colorPoint.X);
point.Y = (int)(float.IsInfinity(colorPoint.Y) ? 0.0 : colorPoint.Y);
}
break;
case Visualization.Depth:
case Visualization.Infrared:
{
DepthSpacePoint depthPoint = coordinateMapper.MapCameraPointToDepthSpace(position);
point.X = (int)(float.IsInfinity(depthPoint.X) ? 0.0 : depthPoint.X);
point.Y = (int)(float.IsInfinity(depthPoint.Y) ? 0.0 : depthPoint.Y);
}
break;
default:
break;
}
return(point);
}
private Point ToPoint(Joint joint)
{
double x;
double y;
switch (Mode)
{
case VisualizationMode.Color:
ColorSpacePoint colorPoint = CoordinateMapper.MapCameraPointToColorSpace(joint.Position);
x = colorPoint.X;
y = colorPoint.Y;
break;
case VisualizationMode.Depth:
case VisualizationMode.Infrared:
DepthSpacePoint depthPoint = CoordinateMapper.MapCameraPointToDepthSpace(joint.Position);
x = depthPoint.X;
y = depthPoint.Y;
break;
default:
x = joint.Position.X;
y = joint.Position.Y;
break;
}
return(new Point(x, y));
}
/// <summary>
///把相机空间里的点映射到指定的彩色或深度空间
/// </summary>
/// <param name="bodyJoints"></param>
/// <param name="isMapColor"></param>
/// <returns></returns>
public Dictionary <JointType, Joint2D> JointToJoint2Ds(IReadOnlyDictionary <JointType, Joint> bodyJoints,
bool isMapColor = true)
{
//将关节点转换为2D深度（显示）空间
Dictionary <JointType, Joint2D> joints2 = new Dictionary <JointType, Joint2D>();
foreach (KeyValuePair <JointType, Joint> pair in bodyJoints)
{
//有时，推断关节的深度（Z）可能显示为负数
CameraSpacePoint position = pair.Value.Position;
if (position.Z < 0) //深度为负值时
{
position.Z = InferredZPositionClamp;
}
// 将点从相机空间映射到颜色空间。
ColorSpacePoint colorSpacePoint = CoordinateMapper.MapCameraPointToColorSpace(position);
// 将点从相机空间映射到深度空间。
DepthSpacePoint depthSpacePoint = CoordinateMapper.MapCameraPointToDepthSpace(position);
Point point = new Point
{
X = isMapColor ? colorSpacePoint.X : depthSpacePoint.X,
Y = isMapColor ? colorSpacePoint.Y : depthSpacePoint.Y
};
joints2[pair.Key] = new Joint2D()
{
Joint2DType = pair.Key,
Position = point,
TrackingState = pair.Value.TrackingState
};
}
return(joints2);
}
public void DrawFrame(Body body, CoordinateMapper mapper,
Rect depthFrameSize)
#endif
{
if (Constants.bPaused == true)
{
this.Resize();
foreach (var jointType in interestedJointTypes)
{
var joint = body.Joints[jointType];
if (joint.TrackingState != TrackingState.NotTracked)
{
var cameraPosition = joint.Position;
DepthSpacePoint depthPosition = mapper.MapCameraPointToDepthSpace(cameraPosition);
if (!float.IsNegativeInfinity(depthPosition.X))
{
var consolePosition = MapDepthPointToConsoleSpace(depthPosition, depthFrameSize);
// Ajust index
if (iTrack > iMax)
{
iTrack = 0;
}
// Populate holders
iXY[iTrack] = consolePosition.Item1;
iXY[iTrack + 1] = consolePosition.Item2;
String strPrintout = (iTrack == 0 ? "LeftHand" : "RightHand");
int iCode = getCode(iXY);
iTrack += 2;
// draw lines - MAKES DISPLAY FLICKER TOO MUCH
// DrawLines();
// swapped jointType.ToString().Substring(0,1) for strPrintout
ConsoleEx.DrawAt(
consolePosition.Item1,
consolePosition.Item2,
strPrintout,
joint.TrackingState == TrackingState.Inferred ? ConsoleColor.Black : ConsoleColor.Black);
// draw code
ConsoleEx.DrawAt(1, 1, "Code Left = " + iXY[idxLX].ToString() + ", " + iXY[idxLY].ToString(), ConsoleColor.Black);
ConsoleEx.DrawAt(1, 2, "Code Right = " + iXY[idxRX].ToString() + ", " + iXY[idxRY].ToString(), ConsoleColor.Black);
ConsoleEx.DrawAt(1, 3, "CODE = " + iCode.ToString(), ConsoleColor.Black);
PrintStack(iCode, iXY[idxLX], iXY[idxLY], iXY[idxRX], iXY[idxRY]);
// draw border lines
// Console.BackgroundColor = ConsoleColor.DarkRed;
}
}
}
}
#if ZERO
if (isMain)
{
DumpMainBodyInfo(body);
}
#endif
}
// --- body
private void ProcessBodyData(MultiFrame multiFrame)
{
// NOTES (!!)
// we assume there is only one body
// we assume joints are always in the same order
// 12 values per joint -> trackingStatus, posX, posY, posZ, depthX, depthY, colorX, colorY, orientX, orientY, orientZ, orientW
// joint trackingstate enum: tracked = 2, nottracked = 0, inffered = 1?
Body[] bodies = multiFrame.Bodies;
Body body = null;
foreach (var b in bodies)
{
if (b.IsTracked)
{
body = b;
break;
}
}
if (body == null)
{
multiFrame.BodyData = new float[25 * 12]; // return zeros if not found
return;
}
int jointsCount = body.Joints.Count;
float[] bodyData = new float[jointsCount * 12];
int idx = 0;
foreach (var kvp in body.Joints)
{
var jointType = kvp.Key;
var joint = kvp.Value;
var jointOrientation = body.JointOrientations[jointType];
CameraSpacePoint position = joint.Position;
if (position.Z < 0)
{
position.Z = 0.1f; // according to Kinect code sample (sometimes z < 0 and the mapping return -infinity)
}
DepthSpacePoint depthSpacePoint = coordMapper.MapCameraPointToDepthSpace(position);
ColorSpacePoint colorSpacePoint = coordMapper.MapCameraPointToColorSpace(position);
bodyData[idx++] = (int)joint.TrackingState;
bodyData[idx++] = joint.Position.X;
bodyData[idx++] = joint.Position.Y;
bodyData[idx++] = joint.Position.Z;
bodyData[idx++] = depthSpacePoint.X;
bodyData[idx++] = depthSpacePoint.Y;
bodyData[idx++] = colorSpacePoint.X;
bodyData[idx++] = colorSpacePoint.Y;
bodyData[idx++] = jointOrientation.Orientation.X;
bodyData[idx++] = jointOrientation.Orientation.Y;
bodyData[idx++] = jointOrientation.Orientation.Z;
bodyData[idx++] = jointOrientation.Orientation.W;
}
multiFrame.BodyData = bodyData;
}
public void Update(Body body, CoordinateMapper coordinateMapper)
{
//this.DrawClippedEdges(body, dc);
IReadOnlyDictionary <JointType, Joint> jointsMS = body.Joints;
// convert the joint points to depth (display) space
Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();
foreach (JointType jointType in jointsMS.Keys)
{
// sometimes the depth(Z) of an inferred joint may show as negative
// clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
CameraSpacePoint position = jointsMS[jointType].Position;
if (position.Z < 0)
{
position.Z = InferredZPositionClamp;
}
//CameraSpacePoint[] myRealWorldPoints = new CameraSpacePoint[DepthMetaData.XDepthMaxKinect * DepthMetaData.YDepthMaxKinect];
//
//coordinateMapper.MapDepthFrameToCameraSpace(myDepthMetaData.FrameData, myRealWorldPoints);
DepthSpacePoint depthSpacePoint = coordinateMapper.MapCameraPointToDepthSpace(position);
if (depthSpacePoint.X < MetaDataBase.XDepthMaxKinect && depthSpacePoint.Y < MetaDataBase.XDepthMaxKinect)
{
//jointPoints[jointType] = new Point(Convert.ToInt32(depthSpacePoint.X), Convert.ToInt32(depthSpacePoint.Y));
Joints[jointType] = new Vector3(position.X, position.Y, position.Z);
}
BonesAsLines = this.SetBodyLines(jointsMS, Joints);
//float f = Height(body);
//System.Diagnostics.Debug.WriteLine("Height: " + f.ToString());
}
}
public void UpdateBody(Body body, FrameView frameView, CoordinateMapper coordinateMapper, Visualization visualization)
{
if (!Initialized)
{
return;
}
Mirrored = frameView.MirroredView;
for (int i = 0; i < controller.JointCount(); i++)
{
Vector2 position = visualization == Visualization.Color ?
coordinateMapper.MapCameraPointToColorSpace(body.Joints[controller.GetJointType(i)].Position).ToPoint() :
coordinateMapper.MapCameraPointToDepthSpace(body.Joints[controller.GetJointType(i)].Position).ToPoint();
if (float.IsInfinity(position.x))
{
position.x = 0f;
}
if (float.IsInfinity(position.y))
{
position.y = 0f;
}
frameView.SetPositionOnFrame(ref position);
controller.SetJointPosition(i, smoothJoints ? Vector3.Lerp(controller.GetJointPosition(i), position, smoothness) : (Vector3)position);
}
UpdateLines();
}
public double[][] CameraToIR(double[][] points)
{
if (points != null)
{
CameraSpacePoint camPoint = new CameraSpacePoint();
DepthSpacePoint depthSpacePoint;
double[][] depthSpacePoints = new double[points.Length][];
double[] point;
for (int i = 0; i < points.Length; i++)
{
point = points[i];
if (point == null)
{
depthSpacePoints[i] = null;
}
else
{
camPoint.X = (float)point[0];
camPoint.Y = (float)point[1];
camPoint.Z = (float)point[2];
depthSpacePoint = mapper.MapCameraPointToDepthSpace(camPoint);
depthSpacePoints[i] = new double[] { depthSpacePoint.X,
depthSpacePoint.Y };
}
}
return(depthSpacePoints);
}
else
{
return(null);
}
}
/// <summary>
/// Maps the 3D point to its corresponding 2D point.
/// </summary>
/// <param name="position">The 3D space point.</param>
/// <returns>The X, Y coordinates of the point.</returns>
public Point GetPoint(CameraSpacePoint position)
{
Point point = new Point();
switch (Visualization)
{
case Visualization.Color:
{
ColorSpacePoint colorPoint = CoordinateMapper.MapCameraPointToColorSpace(position);
point.X = float.IsInfinity(colorPoint.X) ? 0.0 : colorPoint.X;
point.Y = float.IsInfinity(colorPoint.Y) ? 0.0 : colorPoint.Y;
}
break;
case Visualization.Depth:
case Visualization.Infrared:
{
DepthSpacePoint depthPoint = CoordinateMapper.MapCameraPointToDepthSpace(position);
point.X = float.IsInfinity(depthPoint.X) ? 0.0 : depthPoint.X;
point.Y = float.IsInfinity(depthPoint.Y) ? 0.0 : depthPoint.Y;
}
break;
default:
break;
}
return(point);
}
public static Shape DrawPoint(this Canvas canvas, Joint joint, CoordinateMapper mapper, SpaceMode mode, Brush brush)
{
Shape shapeToReturn = null;
// 0) Check whether the joint is tracked.
if (joint.TrackingState == TrackingState.NotTracked)
{
return(null);
}
SpacePointBase spPt;
switch (mode)
{
case SpaceMode.Color:
default:
// 1a) Convert Joint positions to Color space coordinates.
ColorSpacePoint colSpaceJoint = mapper.MapCameraPointToColorSpace(joint.Position);
spPt = new SpacePointBase(colSpaceJoint);
break;
case SpaceMode.Depth:
// 1b) Convert Joint positions to Depth space coordinates.
DepthSpacePoint depSpacePoint = mapper.MapCameraPointToDepthSpace(joint.Position);
spPt = new SpacePointBase(depSpacePoint);
break;
}
#region Joint Mapping Messages
if (spPt.X == float.NegativeInfinity || spPt.Y == float.NegativeInfinity)
{
//Debug.WriteLine($"Joint Mapping Error: Joint[{joint.JointType.ToString()}] ( {spPt.X} , {spPt.Y} )");
}
else if ((spPt.X < 0 || spPt.Y < 0 || spPt.X > FrameDimensions[mode].Item1 || spPt.Y > FrameDimensions[mode].Item2))
{
//Debug.WriteLine($"Joint Mapping Overflow: Joint[{joint.JointType.ToString()}] ( {spPt.X} , {spPt.Y} )");
}
#endregion
//-inf meaning Joint is not detected and no corresponding mapped space point
if (spPt.IsValid)
{
// 2) Scale the mapped coordinates to window dimensions.
spPt = spPt.ScaleTo(canvas.ActualWidth, canvas.ActualHeight, mode);
//if (joint.JointType == 0) Debug.WriteLine($"Head Position in Color Space = {spPt.X}, {spPt.Y}");
// 3) Draw the point on Canvas
shapeToReturn = spPt.DrawPoint(canvas, brush);
}
return(shapeToReturn);
}
/// <summary>
/// Updates bones, joints and hand state for a single body.
/// </summary>
/// <param name="kinectSensor">active kinect sensor</param>
/// <param name="body">body for getting joint info</param>
/// <param name="bodyIndex">index for body we are currently updating</param>
internal void UpdateBody(KinectSensor kinectSensor, Body body, int bodyIndex)
{
if (kinectSensor == null)
{
throw new ArgumentNullException("kinectSensor");
}
if (body != null)
{
IReadOnlyDictionary <JointType, Joint> joints = body.Joints;
var jointPointsInDepthSpace = new Dictionary <JointType, Point>();
var bodyInfo = this.bodyInfos[bodyIndex];
CoordinateMapper coordinateMapper = kinectSensor.CoordinateMapper;
// update all joints
foreach (var jointType in body.Joints.Keys)
{
// sometimes the depth(Z) of an inferred joint may show as negative
// clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
CameraSpacePoint position = body.Joints[jointType].Position;
if (position.Z < 0)
{
position.Z = InferredZPositionClamp;
}
// map joint position to depth space
DepthSpacePoint depthSpacePoint = coordinateMapper.MapCameraPointToDepthSpace(position);
jointPointsInDepthSpace[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
// modify the joint's visibility and location
this.UpdateJoint(bodyInfo.JointPoints[jointType], joints[jointType], jointPointsInDepthSpace[jointType]);
// modify hand ellipse colors based on hand states
// modity hand ellipse sizes based on tracking confidences
if (jointType == JointType.HandRight)
{
this.UpdateHand(bodyInfo.HandRightEllipse, body.HandRightState, body.HandRightConfidence, jointPointsInDepthSpace[jointType]);
}
if (jointType == JointType.HandLeft)
{
this.UpdateHand(bodyInfo.HandLeftEllipse, body.HandLeftState, body.HandLeftConfidence, jointPointsInDepthSpace[jointType]);
}
}
// update all bones
foreach (var bone in bodyInfo.Bones)
{
this.UpdateBone(
bodyInfo.BoneLines[bone],
joints[bone.Item1],
joints[bone.Item2],
jointPointsInDepthSpace[bone.Item1],
jointPointsInDepthSpace[bone.Item2]);
}
}
}
private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
bool dataReceived = false;
using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
if (bodies == null)
{
bodies = new Body[bodyFrame.BodyCount];
}
bodyFrame.GetAndRefreshBodyData(bodies);
dataReceived = true;
frame++;
}
}
if (dataReceived)
{
foreach (Body body in bodies)
{
IReadOnlyDictionary <JointType, Joint> joints = body.Joints;
Dictionary <JointType, Array> jointPoints = new Dictionary <JointType, Array>();
foreach (JointType jointType in joints.Keys)
{
CameraSpacePoint position = joints[jointType].Position;
if (position.Z < 0)
{
position.Z = InferredZPositionClamp;
}
DepthSpacePoint depthSpacePoint = coordinateMapper.MapCameraPointToDepthSpace(position);
jointPoints[jointType] = new float[] { depthSpacePoint.X, depthSpacePoint.Y };
}
if (IsConnected)
{
var bodyJson = JsonConvert.SerializeObject(body);
var projectionMappedPointsJson = "";
if (body.IsTracked)
{
projectionMappedPointsJson = JsonConvert.SerializeObject(jointPoints);
Invoke("OnBody", bodyJson, projectionMappedPointsJson);
}
}
}
var trackedBodies = bodies.Where(b => b.IsTracked);
if (trackedBodies.Count() > 0)
{
var trackedBodyTrackingIdsJson = JsonConvert.SerializeObject(trackedBodies.Select(b => b.TrackingId));
Invoke("OnBodies", trackedBodyTrackingIdsJson, frame, UserName);
}
}
}
private void DrawJoint(Joint joint, double radius, SolidColorBrush fill, double borderWidth, SolidColorBrush border)
{
//if (joint.TrackingState != TrackingState.Tracked) return;
Point spacePoint1 = new Point();
if (Visualization == Visualization.Color)
{
//Map the CameraPoint to ColorSpace so they match //Mapeia do "CameraPoint" para o "ColorSpace"
ColorSpacePoint colorPoint = _coordinateMapper.MapCameraPointToColorSpace(joint.Position);
spacePoint1.X = colorPoint.X;
spacePoint1.Y = colorPoint.Y;
}
else if (Visualization == Visualization.Depth || Visualization == Visualization.Infrared)
{
//Map the CameraPoint to ColorSpace so they match //Mapeia do "CameraPoint" para o "ColorSpace"
DepthSpacePoint depthPoint = _coordinateMapper.MapCameraPointToDepthSpace(joint.Position);
spacePoint1.X = depthPoint.X;
spacePoint1.Y = depthPoint.Y;
}
//Create the UI element based on the parameters //Cria os elementos do Canvas baseado nos parâmetros
Ellipse el = new Ellipse();
el.Fill = fill;
el.Stroke = border;
el.StrokeThickness = borderWidth;
el.Width = el.Height = radius;
//Add the Ellipse to the canvas //Adiciona a "Ellipse" no canvas
Skeleton.Children.Add(el);
//Avoid exceptions based on bad tracking //Evita exceções com base no mal rastreamento
if (float.IsInfinity((float)(spacePoint1.X)) || float.IsInfinity((float)(spacePoint1.Y)))
{
return;
}
//Alinha a "Ellipse" no canvas
Canvas.SetLeft(el, (spacePoint1.X - el.Width / 2));
Canvas.SetTop(el, (spacePoint1.Y - el.Height / 2));
}
/// <summary>
/// Convert camera space point to depth space
/// </summary>
/// <param name="coordinateMapper"></param>
/// <param name="cameraSpacePoint"></param>
/// <returns></returns>
public static DepthSpacePoint CameraSpacePointToDepthSpace(CoordinateMapper coordinateMapper, CameraSpacePoint cameraSpacePoint)
{
DepthSpacePoint depthPoint;
if (cameraSpacePoint.Z < 0)
{
CameraSpacePoint inferredPoint = new CameraSpacePoint();
inferredPoint.X = cameraSpacePoint.X;
inferredPoint.Y = cameraSpacePoint.Y;
inferredPoint.Z = InferredZPositionClamp;
depthPoint = coordinateMapper.MapCameraPointToDepthSpace(inferredPoint);
}
else
{
depthPoint = coordinateMapper.MapCameraPointToDepthSpace(cameraSpacePoint);
}
return(depthPoint);
}
public void DrawBone(Joint first, Joint second, Brush brush, double thickness)
{
if (first.TrackingState == TrackingState.NotTracked || second.TrackingState == TrackingState.NotTracked)
{
return;
}
Point firstPoint = new Point(_ratioX, _ratioY);
Point secondPoint = new Point(_ratioX, _ratioY);
switch (FrameType)
{
case VisualizationMode.Color:
{
ColorSpacePoint colorFirstPoint = CoordinateMapper.MapCameraPointToColorSpace(first.Position);
firstPoint.X *= float.IsInfinity(colorFirstPoint.X) ? 0.0 : colorFirstPoint.X;
firstPoint.Y *= float.IsInfinity(colorFirstPoint.Y) ? 0.0 : colorFirstPoint.Y;
ColorSpacePoint colorSecondPoint = CoordinateMapper.MapCameraPointToColorSpace(second.Position);
secondPoint.X *= float.IsInfinity(colorSecondPoint.X) ? 0.0 : colorSecondPoint.X;
secondPoint.Y *= float.IsInfinity(colorSecondPoint.Y) ? 0.0 : colorSecondPoint.Y;
}
break;
case VisualizationMode.Depth:
case VisualizationMode.Infrared:
{
DepthSpacePoint depthFirstPoint = CoordinateMapper.MapCameraPointToDepthSpace(first.Position);
firstPoint.X *= float.IsInfinity(depthFirstPoint.X) ? 0.0 : depthFirstPoint.X;
firstPoint.Y *= float.IsInfinity(depthFirstPoint.Y) ? 0.0 : depthFirstPoint.Y;
DepthSpacePoint depthSecondPoint = CoordinateMapper.MapCameraPointToDepthSpace(second.Position);
secondPoint.X *= float.IsInfinity(depthSecondPoint.X) ? 0.0 : depthSecondPoint.X;
secondPoint.Y *= float.IsInfinity(depthSecondPoint.Y) ? 0.0 : depthSecondPoint.Y;
}
break;
default:
break;
}
Line line = new Line
{
Tag = TAG,
X1 = firstPoint.X,
Y1 = firstPoint.Y,
X2 = secondPoint.X,
Y2 = secondPoint.Y,
StrokeThickness = thickness,
Stroke = brush
};
canvas.Children.Add(line);
}
public static Shape DrawLine(this Canvas canvas, Joint first, Joint second, CoordinateMapper mapper, SpaceMode mode,
double thickness, Brush brush)
{
Shape lineToReturn = null;
if (first.TrackingState == TrackingState.NotTracked || second.TrackingState == TrackingState.NotTracked)
{
return(null);
}
SpacePointBase myFirstPoint;
SpacePointBase mySecondPoint;
switch (mode)
{
case SpaceMode.Color:
default:
myFirstPoint = new SpacePointBase(mapper.MapCameraPointToColorSpace(first.Position));
mySecondPoint = new SpacePointBase(mapper.MapCameraPointToColorSpace(second.Position));
break;
case SpaceMode.Depth:
myFirstPoint = new SpacePointBase(mapper.MapCameraPointToDepthSpace(first.Position));
mySecondPoint = new SpacePointBase(mapper.MapCameraPointToDepthSpace(second.Position));
break;
}
//Both points that the line joins must be mapped correctly
if (myFirstPoint.IsValid && mySecondPoint.IsValid)
{
myFirstPoint = myFirstPoint.ScaleTo(canvas.ActualWidth, canvas.ActualHeight, mode);
mySecondPoint = mySecondPoint.ScaleTo(canvas.ActualWidth, canvas.ActualHeight, mode);
//call static DrawLine from class SpacePointBae
lineToReturn = SpacePointBase.DrawLine(canvas, myFirstPoint, mySecondPoint, thickness, brush);
}
return(lineToReturn);
}
public static void DrawLine(this Canvas canvas, Joint first, Joint second, CoordinateMapper mapper, SpaceMode mode)
{
if (first.TrackingState == TrackingState.NotTracked || second.TrackingState == TrackingState.NotTracked)
{
return;
}
SpacePointBase myFirstPoint;
SpacePointBase mySecondPoint;
switch (mode)
{
case SpaceMode.Color:
default:
myFirstPoint = new SpacePointBase(mapper.MapCameraPointToColorSpace(first.Position));
mySecondPoint = new SpacePointBase(mapper.MapCameraPointToColorSpace(second.Position));
break;
case SpaceMode.Depth:
myFirstPoint = new SpacePointBase(mapper.MapCameraPointToDepthSpace(first.Position));
mySecondPoint = new SpacePointBase(mapper.MapCameraPointToDepthSpace(second.Position));
break;
}
//Both points that the line joins must be mapped correctly
if (
(!float.IsNegativeInfinity(myFirstPoint.X) &&
!float.IsNegativeInfinity(myFirstPoint.Y)) ||
(!float.IsNegativeInfinity(mySecondPoint.X) &&
!float.IsNegativeInfinity(mySecondPoint.Y))
)
{
myFirstPoint = myFirstPoint.ScaleTo(canvas.ActualWidth, canvas.ActualHeight, mode);
mySecondPoint = mySecondPoint.ScaleTo(canvas.ActualWidth, canvas.ActualHeight, mode);
//call static DrawLine from class SpacePointBae
SpacePointBase.DrawLine(canvas, myFirstPoint, mySecondPoint);
}
}
private Point Scale(Joint joint, CoordinateMapper mapper)
{
Point point = new Point();
ColorSpacePoint colorPoint = mapper.MapCameraPointToColorSpace(joint.Position);
DepthSpacePoint depthSpacePoint = mapper.MapCameraPointToDepthSpace(joint.Position);
point.X = float.IsInfinity(colorPoint.X) ? 0.0 : colorPoint.X;
point.Y = float.IsInfinity(colorPoint.Y) ? 0.0 : colorPoint.Y;
return(point);
}
/// <summary>
/// Update body data for each body that is tracked.
/// </summary>
/// <param name="body">body for getting joint info</param>
/// <param name="bodyIndex">index for body we are currently updating</param>
internal void UpdateBody(Body body, int bodyIndex)
{
IReadOnlyDictionary <JointType, Joint> joints = body.Joints;
var jointPointsInDepthSpace = new Dictionary <JointType, Point>();
var bodyInfo = this.bodyInfos[bodyIndex];
// update all joints
foreach (var jointType in body.Joints.Keys)
{
if (jointType.Equals(JointType.Head) || jointType.Equals(JointType.Neck) || jointType.Equals(JointType.SpineShoulder) || jointType.Equals(JointType.SpineMid) ||
jointType.Equals(JointType.SpineMid) || jointType.Equals(JointType.SpineBase) || jointType.Equals(JointType.ShoulderRight) || jointType.Equals(JointType.ShoulderLeft) ||
jointType.Equals(JointType.ElbowRight) || jointType.Equals(JointType.WristRight) || jointType.Equals(JointType.HandRight) || jointType.Equals(JointType.HandTipRight) ||
jointType.Equals(JointType.ThumbRight) || jointType.Equals(JointType.ElbowLeft) || jointType.Equals(JointType.WristLeft) || jointType.Equals(JointType.HandLeft) ||
jointType.Equals(JointType.HandTipLeft) || jointType.Equals(JointType.ThumbLeft))
{
// sometimes the depth(Z) of an inferred joint may show as negative
// clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
CameraSpacePoint position = body.Joints[jointType].Position;
if (position.Z < 0)
{
position.Z = InferredZPositionClamp;
}
// map joint position to depth space
DepthSpacePoint depthSpacePoint = coordinateMapper.MapCameraPointToDepthSpace(position);
jointPointsInDepthSpace[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
// modify the joint's visibility and location
this.UpdateJoint(bodyInfo.JointPoints[jointType], joints[jointType], jointPointsInDepthSpace[jointType]);
// modify hand ellipse colors based on hand states
// modity hand ellipse sizes based on tracking confidences
if (jointType == JointType.HandRight)
{
this.UpdateHand(bodyInfo.HandRightEllipse, body.HandRightState, body.HandRightConfidence, jointPointsInDepthSpace[jointType]);
}
if (jointType == JointType.HandLeft)
{
this.UpdateHand(bodyInfo.HandLeftEllipse, body.HandLeftState, body.HandLeftConfidence, jointPointsInDepthSpace[jointType]);
}
}
}
// update all bones
foreach (var bone in bodyInfo.Bones)
{
this.UpdateBone(bodyInfo.BoneLines[bone], joints[bone.Item1], joints[bone.Item2],
jointPointsInDepthSpace[bone.Item1],
jointPointsInDepthSpace[bone.Item2]);
}
}
/// <summary>
/// Handles the body frame data arriving from the sensor
/// </summary>
/// <param name="e">event arguments</param>
public void Reader_FrameArrived(BodyFrameArrivedEventArgs e)
{
BodyFrame frame = e.FrameReference.AcquireFrame();
if (frame == null)
{
return;
}
frame.GetAndRefreshBodyData(bodies);
frame.Dispose();
using (DrawingContext drawingContext = drawingGroup.Open())
{
// Draw a transparent background to set the render size
drawingContext.DrawRectangle(Brushes.Transparent, null, new Rect(0.0, 0.0, RenderWidth, RenderHeight));
foreach (Body body in bodies)
{
if (body.IsTracked)
{
DrawClippedEdges(body, drawingContext);
IReadOnlyDictionary <JointType, Joint> joints = body.Joints;
// Convert the joint points to depth (display) space
Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();
foreach (JointType jointType in joints.Keys)
{
DepthSpacePoint depthSpacePoint = coordinateMapper.MapCameraPointToDepthSpace(joints[jointType].Position);
jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
}
DrawBody(joints, jointPoints, drawingContext);
DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], drawingContext);
DrawHand(body.HandRightState, jointPoints[JointType.HandRight], drawingContext);
FormattedText text = new FormattedText(KinectManager.Instance.GetSimpleID(body.TrackingId).ToString(),
System.Globalization.CultureInfo.CurrentCulture,
FlowDirection.LeftToRight,
new Typeface("Calibri"),
50,
Brushes.Chartreuse);
drawingContext.DrawText(text, new System.Windows.Point(jointPoints[JointType.Head].x - 20, jointPoints[JointType.Head].y - 80));
}
}
drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, RenderWidth, RenderHeight));
}
}
public static string Serialize(this List <Body> skeletons, CoordinateMapper mapper, Mode mode)
{
JSONSkeletonCollection jsonSkeletons = new JSONSkeletonCollection {
Skeletons = new List <JSONSkeleton>()
};
foreach (Body skeleton in skeletons)
{
JSONSkeleton jsonSkeleton = new JSONSkeleton();
if (skeleton.IsTracked)
{
jsonSkeleton.command = "bodyData";
jsonSkeleton.trackingID = skeleton.TrackingId.ToString();
jsonSkeleton.Joints = new List <JSONJoint>();
jsonSkeleton.HandLeftState = skeleton.HandLeftState;
jsonSkeleton.HandRightState = skeleton.HandRightState;
foreach (var joint in skeleton.Joints)
{
Point point = new Point();
switch (mode)
{
case Mode.Color:
ColorSpacePoint colorPoint = mapper.MapCameraPointToColorSpace(joint.Value.Position);
point.X = colorPoint.X;
point.Y = colorPoint.Y;
break;
case Mode.Depth:
DepthSpacePoint depthPoint = mapper.MapCameraPointToDepthSpace(joint.Value.Position);
point.X = depthPoint.X;
point.Y = depthPoint.Y;
break;
default:
break;
}
jsonSkeleton.Joints.Add(new JSONJoint
{
Name = joint.Key.ToString().ToLower(),
X = joint.Value.Position.X,
Y = joint.Value.Position.Y,
mappedX = point.X,
mappedY = point.Y,
Z = joint.Value.Position.Z
});
}
jsonSkeletons.Skeletons.Add(jsonSkeleton);
}
}
return(JsonConvert.SerializeObject(jsonSkeletons));
}
/// <summary>
/// Converts the specified 3D Camera point into its equivalent 2D Depth point.
/// </summary>
/// <param name="point3D">The point in the 3D Camera space.</param>
/// <returns>The equivalent point in the 2D Depth space.</returns>
public static Point ToPoint(this CameraSpacePoint point3D)
{
Point point = new Point();
DepthSpacePoint point2D = _mapper.MapCameraPointToDepthSpace(point3D);
if (!float.IsInfinity(point2D.X) && !float.IsInfinity(point2D.Y))
{
point.X = point2D.X;
point.Y = point2D.Y;
}
return(point);
}
/// <summary>
/// Project the camera space point using the given sensor coordinate mapper.
/// </summary>
/// <param name="inPoint"></param>
/// <param name="coordinateMapper"></param>
/// <returns></returns>
public Point ProjectCameraPoint(CameraSpacePoint inPoint, CoordinateMapper coordinateMapper)
{
if (_useDepthMapper)
{
DepthSpacePoint depthPoint = coordinateMapper.MapCameraPointToDepthSpace(inPoint);
return(new Point(depthPoint.X, depthPoint.Y));
}
else
{
ColorSpacePoint colorPoint = coordinateMapper.MapCameraPointToColorSpace(inPoint);
return(new Point(colorPoint.X, colorPoint.Y));
}
}
/* Update the body array with new information from the sensor
* Should be called whenever a new BodyFrameArrivedEvent occurs */
public void UpdateBodyFrame(Body[] bodies)
{
if (bodies != null)
{
using (DrawingContext dc = drawingGroup.Open())
{
// Draw a transparent background to set the render size
dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, displayWidth, displayHeight));
int penIndex = 0;
foreach (Body body in bodies)
{
Pen drawPen = bodyColors[penIndex++];
if (body.IsTracked)
{
DrawClippedEdges(body, dc);
IReadOnlyDictionary <JointType, Joint> joints = body.Joints;
// convert the joint points to depth (display) space
Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();
foreach (JointType jointType in joints.Keys)
{
// ometimes the depth(Z) of an inferred joint may show as negative
// Clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
CameraSpacePoint position = joints[jointType].Position;
if (position.Z < 0)
{
position.Z = InferredZPositionClamp;
}
DepthSpacePoint depthSpacePoint = coordinateMapper.MapCameraPointToDepthSpace(position);
jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
}
DrawBody(joints, jointPoints, dc, drawPen);
DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
}
}
// Prevent drawing outside of our render area
drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, displayWidth, displayHeight));
}
}
}
/// <summary>
/// Maps 3D coordinates to the 2D space of a depth map
/// </summary>
/// <param name="position">3D position</param>
/// <returns>2D point</returns>
public static DepthSpacePoint ConvertBodyToDepthCoordinate(CameraSpacePoint position)
{
if (position.Z < 0)
{
position.Z = 0;
}
if (CoordinateMapper != null)
{
return(CoordinateMapper.MapCameraPointToDepthSpace(position));
}
else
{
throw new Exception("Initialize the CoordinateMapper before calling this helper function!");
}
}
public unsafe void Update(Body body)
{
Joint jointHandLeft = body.Joints[JointType.HandLeft];
Joint jointHandRight = body.Joints[JointType.HandRight];
DepthSpacePoint depthPointHandLeft = CoordinateMapper.MapCameraPointToDepthSpace(jointHandLeft.Position);
DepthSpacePoint depthPointHandRight = CoordinateMapper.MapCameraPointToDepthSpace(jointHandRight.Position);
handLeftX = (int)depthPointHandLeft.X;
handLeftY = (int)depthPointHandLeft.Y;
handRightX = (int)depthPointHandRight.X;
handRightY = (int)depthPointHandRight.Y;
hand_right_depth = this.calculate_depth_value(handRightX, handRightY);
hand_left_depth = this.calculate_depth_value(handLeftX, handLeftY);
}
private static BodyFrameData.Body CreateBody(Body input, CoordinateMapper coordinateMapper)
{
var output = new BodyFrameData.Body()
{
TrackingId = input.TrackingId,
IsTracked = input.IsTracked,
IsRestricted = input.IsRestricted,
ClippedEdges = (Schemas.RecorderMessages.FrameEdges)(int)input.ClippedEdges,
HandLeft = new BodyFrameData.Hand()
{
Confidence = (Schemas.RecorderMessages.TrackingConfidence)(int)input.HandLeftConfidence,
State = (Schemas.RecorderMessages.HandState)(int)input.HandLeftState,
},
HandRight = new BodyFrameData.Hand()
{
Confidence = (Schemas.RecorderMessages.TrackingConfidence)(int)input.HandRightConfidence,
State = (Schemas.RecorderMessages.HandState)(int)input.HandRightState,
},
Joints = new Dictionary<Schemas.RecorderMessages.JointType, BodyFrameData.Joint>(),
};
var jointTypes = Enum.GetValues(typeof(JointType));
foreach (JointType type in jointTypes)
{
var joint = input.Joints[type];
var position2d = coordinateMapper.MapCameraPointToDepthSpace(joint.Position);
var position3d = joint.Position;
var orientation = input.JointOrientations[type].Orientation;
output.Joints.Add((Schemas.RecorderMessages.JointType)(int)type, new BodyFrameData.Joint()
{
Type = (Schemas.RecorderMessages.JointType)(int)type,
State = (Schemas.RecorderMessages.TrackingState)(int)joint.TrackingState,
Position2D = new Point(position2d.X, position2d.Y),
Position3D = new Vector3D(position3d.X, position3d.Y, position3d.Z),
Rotation = new Quaternion(orientation.X, orientation.Y, orientation.Z, orientation.W),
});
}
return output;
}
public void DrawJoint(Joint joint, Brush brush, double radius)
{
if (joint.TrackingState == TrackingState.NotTracked)
{
return;
}
Point point = new Point(_ratioX, _ratioY);
switch (FrameType)
{
case VisualizationMode.Color:
{
ColorSpacePoint colorPoint = CoordinateMapper.MapCameraPointToColorSpace(joint.Position);
point.X *= float.IsInfinity(colorPoint.X) ? 0.0 : colorPoint.X;
point.Y *= float.IsInfinity(colorPoint.Y) ? 0.0 : colorPoint.Y;
}
break;
case VisualizationMode.Depth:
case VisualizationMode.Infrared:
{
DepthSpacePoint depthPoint = CoordinateMapper.MapCameraPointToDepthSpace(joint.Position);
point.X *= float.IsInfinity(depthPoint.X) ? 0.0 : depthPoint.X;
point.Y *= float.IsInfinity(depthPoint.Y) ? 0.0 : depthPoint.Y;
}
break;
default:
break;
}
Ellipse ellipse = new Ellipse
{
Tag = TAG,
Width = radius,
Height = radius,
Fill = brush
};
Canvas.SetLeft(ellipse, point.X - ellipse.Width / 2);
Canvas.SetTop(ellipse, point.Y - ellipse.Height / 2);
canvas.Children.Add(ellipse);
}
/// <summary>
/// Project the camera space point using the given sensor coordinate mapper.
/// </summary>
/// <param name="inPoint"></param>
/// <param name="coordinateMapper"></param>
/// <returns></returns>
public Point ProjectCameraPoint(CameraSpacePoint inPoint, CoordinateMapper coordinateMapper)
{
if (_useDepthMapper)
{
DepthSpacePoint depthPoint = coordinateMapper.MapCameraPointToDepthSpace(inPoint);
return new Point(depthPoint.X, depthPoint.Y);
}
else
{
ColorSpacePoint colorPoint = coordinateMapper.MapCameraPointToColorSpace(inPoint);
return new Point(colorPoint.X, colorPoint.Y);
}
}
private void ShowJoint( CoordinateMapper mapper, Joint joint )
{
var point = mapper.MapCameraPointToDepthSpace( joint.Position );
var ellipse = new Ellipse()
{
Fill = Brushes.Red,
Width = 5,
Height = 5,
};
Canvas.SetLeft( ellipse, point.X );
Canvas.SetTop( ellipse, point.Y );
CanvasBody.Children.Add( ellipse );
}
public void DrawBodies(Body[] bodies, CoordinateMapper coordinateMapper, DisplayMode displayMode, int? targetIndex)
{
// Draw a transparent background to set the render size
this.imageSource.DrawRectangle(0, 0, this.displayWidth, this.displayHeight, Color.FromArgb(255, 128, 128, 128));
int count = 0;
bool trackedBody = false;
foreach (Body body in bodies)
{
if (body.IsTracked)
{
this.DrawClippedEdges(body);
IReadOnlyDictionary<JointType, Joint> joints = body.Joints;
// convert the joint points to depth (display) space
Dictionary<JointType, Point> jointPoints = new Dictionary<JointType, Point>();
foreach (JointType jointType in joints.Keys)
{
// sometimes the depth(Z) of an inferred joint may show as negative
// clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
CameraSpacePoint position = joints[jointType].Position;
if (position.Z < 0)
{
position.Z = InferredZPositionClamp;
}
DepthSpacePoint depthSpacePoint = coordinateMapper.MapCameraPointToDepthSpace(position);
//convert joint space to color space so that we can draw skeletons on top of color feed
if (displayMode == DisplayMode.COLOR) {
jointPoints[jointType] = new Point((depthSpacePoint.X / this.jointDisplayWidth) * this.displayWidth, (depthSpacePoint.Y / this.jointDisplayHeight) * this.displayHeight);
}
else
{
jointPoints[jointType] = new Point(depthSpacePoint.X / this.jointDisplayWidth,depthSpacePoint.Y / this.jointDisplayHeight);
}
//check if this is the skeleton that has been targeted by system
//if it is we'll draw a big red circle on the skelton chest
if (targetIndex != null && targetIndex == count)
{
if (jointType == JointType.Head)
{
var joint = jointPoints[jointType];
if (!hasValidTracking)
{
voiceSynth.SpeakAsync("Target Aquired!");
hasValidTracking = true;
}
trackedBody = true;
if (displayMode == DisplayMode.COLOR)
{
this.imageSource.FillEllipse((int)joint.X, (int)joint.Y, (int)joint.X + 50, (int)joint.Y + 50, Color.FromArgb(128, 255, 0, 0));
}
else if(displayMode == DisplayMode.INFRARED)
{
//we can't draw on a grayscale 16bit bitmap. So TODO on implementing the drawing here
//var pgra32Version = BitmapFactory.ConvertToPbgra32Format(this.infraredSource);
//pgra32Version.FillEllipse((int)joint.X, (int)joint.Y, (int)joint.X + 50, (int)joint.Y + 50, Colors.Black);
}
}
}
}
this.DrawBody(joints, jointPoints, this.trackedJointColor);
this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft]);
this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight]);
count++;
}
}
if (!trackedBody)
{
hasValidTracking = false;
}
// prevent drawing outside of our render area
//this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
}
/// <summary>
/// Converts the specified CameraSpacePoint into a 2-D point.
/// </summary>
/// <param name="position">The CameraSpacePoint to convert.</param>
/// <param name="visualization">The type of the conversion (color, depth, or infrared).</param>
/// <param name="coordinateMapper">The CoordinateMapper to make the conversion.</param>
/// <returns>The corresponding 2-D point.</returns>
public static Point ToPoint(this CameraSpacePoint position, Visualization visualization, CoordinateMapper coordinateMapper)
{
Point point = new Point();
switch (visualization)
{
case Visualization.Color:
{
ColorSpacePoint colorPoint = coordinateMapper.MapCameraPointToColorSpace(position);
point.X = float.IsInfinity(colorPoint.X) ? 0.0 : colorPoint.X;
point.Y = float.IsInfinity(colorPoint.Y) ? 0.0 : colorPoint.Y;
}
break;
case Visualization.Depth:
case Visualization.Infrared:
{
DepthSpacePoint depthPoint = coordinateMapper.MapCameraPointToDepthSpace(position);
point.X = float.IsInfinity(depthPoint.X) ? 0.0 : depthPoint.X;
point.Y = float.IsInfinity(depthPoint.Y) ? 0.0 : depthPoint.Y;
}
break;
default:
break;
}
return point;
}
internal static void DrawBone(DrawingContext drawingContext, System.Windows.Media.Brush brush, JointType startJoint, JointType endJoint, IReadOnlyDictionary<JointType, Joint> joints, System.Windows.Rect rect, CoordinateMapper coordinateMapper, bool useDepthSpace = true, double line = 0.8F)
{
if (joints[startJoint].TrackingState != TrackingState.Tracked
&& joints[endJoint].TrackingState != TrackingState.Tracked)
{
return;
}
System.Windows.Point startPoint;
System.Windows.Point endPoint;
if (useDepthSpace)
{
startPoint = coordinateMapper.MapCameraPointToDepthSpace(joints[startJoint].Position).GetPoint();
endPoint = coordinateMapper.MapCameraPointToDepthSpace(joints[endJoint].Position).GetPoint();
}
else
{
startPoint = coordinateMapper.MapCameraPointToColorSpace(joints[startJoint].Position).GetPoint();
endPoint = coordinateMapper.MapCameraPointToColorSpace(joints[endJoint].Position).GetPoint();
}
if (rect.Contains(startPoint) && rect.Contains(endPoint))
drawingContext.DrawLine(new System.Windows.Media.Pen(brush, line), startPoint, endPoint);
}
/// <summary>
/// For the body frame of the first tracked skeleton, get a dictionary of all jointTypes and their corresponding currentPoint
/// </summary>
/// <param name="skeletons"></param>
/// <param name="mapper"></param>
/// <returns></returns>
static Dictionary<JointType, Point> getJointPoints(List<Body> skeletons, CoordinateMapper mapper)
{
Body skeleton = skeletons[0]; //only consider the first tracked skeleton.
IReadOnlyDictionary<JointType, Joint> joints = skeleton.Joints;
Dictionary<JointType, Point> jointPoints = new Dictionary<JointType, Point>();
foreach (JointType jointType in joints.Keys)
{
// sometimes the depth(Z) of an inferred joint may show as negative
// clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
CameraSpacePoint position = joints[jointType].Position;
if (position.Z < 0)
{
position.Z = Constants.InferredZPositionClamp;
}
DepthSpacePoint depthSpacePoint = mapper.MapCameraPointToDepthSpace(position);
jointPoints[jointType] = new Point((int)depthSpacePoint.X, (int)depthSpacePoint.Y);
}
return jointPoints;
}