public Point MapJointToDepth(KinectBase.Joint joint, bool undoTransform)
{
//TODO: Update this so it takes a joint array instead of a single joint (this is supposed to be more efficient for the Kinect 2)
Point mappedPoint = new Point(0, 0);
Point3D transformedPosition = joint.Position;
if (undoTransform)
{
Matrix3D inverseTransform = skeletonTransformation;
inverseTransform.Invert();
transformedPosition = inverseTransform.Transform(transformedPosition);
}
//Setup the Kinect v2 objects to do the transformation
CameraSpacePoint[] skelPoints = new CameraSpacePoint[1];
skelPoints[0] = new CameraSpacePoint();
skelPoints[0].X = (float)transformedPosition.X;
skelPoints[0].Y = (float)transformedPosition.Y;
skelPoints[0].Z = (float)transformedPosition.Z;
DepthSpacePoint[] points = new DepthSpacePoint[1];
kinect.CoordinateMapper.MapCameraPointsToDepthSpace(skelPoints, points);
//Convert back to the base object type
mappedPoint.X = points[0].X;
mappedPoint.Y = points[0].Y;
return mappedPoint;
}