private unsafe List <Point> GetSpinePoints(Body activeBody, KinectBuffer depthBuffer, KinectBuffer bodyIndexBuffer)
{
var joints = activeBody.Joints;
if (!BackTrackerHelper.isBodyTracked(joints))
{
return(null);
}
// calculateRotationAngle(leftShoulder, rightShoulder, out double tan, out double cos);
var width = depthFrameDescription.Width;
var height = depthFrameDescription.Height;
var bodyIndexFrameData = bodyIndexBuffer.UnderlyingBuffer;
byte * frameData = (byte *)bodyIndexFrameData;
ushort *depthFrameData = (ushort *)depthBuffer.UnderlyingBuffer;
// preparation work
List <Point> spinePoints = new List <Point>(height);
BackArea backArea = new BackArea(kinectSensor, joints);
int lastPointY = -1;
Point point = new Point();
var depthSpacePoint = new DepthSpacePoint();
var point3D = new CameraSpacePoint();
if (debugData)
{
writetext.WriteLine("############## FRAME START ##############");
}
List <Point> rowPoints = new List <Point>();
var size = bodyIndexBuffer.Size;
for (int i = 0; i < (int)size; ++i)
{
// the BodyColor array has been sized to match
// BodyFrameSource.BodyCount
// check if point belong to a person
if (frameData[i] < BodyColor.Length)
{
point.X = i % width;
point.Y = i / width;
depthSpacePoint.X = i % width;
depthSpacePoint.Y = i / width;
// check if point is on the back
var pointIsOnBack = backArea.isPointInSpineArea(point);
if (pointIsOnBack)
{
this.bodyIndexPixels[i] = 0xFFFFFFFF;
}
else
{
this.bodyIndexPixels[i] = BodyColor[frameData[i]];
}
if (!pointIsOnBack)
{
continue;
}
// fill the current row of points (pixels that have the same Y)
point3D = kinectSensor.CoordinateMapper.MapDepthPointToCameraSpace(depthSpacePoint, depthFrameData[i]);
rowPoints.Add(new Point(point3D.Z, point3D.Y));
if (debugData)
{
writetext.WriteLine($"{point3D.Z}, {point3D.Y}");
}
if (i / width != lastPointY && rowPoints.Count != 0)
{
// if the current pixel doesn't belong to the same row
// process the current row and calculate the spine point from it
rowPoints.Sort(pointCompare);
spinePoints.Add(BackTrackerHelper.calculateSpinePoint(rowPoints));
rowPoints.Clear();
if (debugData)
{
writetext.WriteLine("-100, -100");
}
}
lastPointY = i / width;
}
else
{
this.bodyIndexPixels[i] = 0x00000000;
}
}
if (debugData)
{
writetext.WriteLine("************** FRAME END **************");
}
// process the last line of pixels
if (rowPoints.Count != 0)
{
rowPoints.Sort(pointCompare);
spinePoints.Add(BackTrackerHelper.calculateSpinePoint(rowPoints));
rowPoints.Clear();
}
return(spinePoints);
}
private void drawSpinePoints(List <Point> spinePoints)
{
if (spinePoints == null)
{
return;
}
Array.Clear(spinePixels, 0, spinePixels.Length);
var width = depthFrameDescription.Width;
var height = depthFrameDescription.Height;
// calculate max and mix points
// of the spine
int maxHeightPoint = 0, minHeightPoint = 0;
int maxZPoint = 0, minZPoint = 0;
for (int i = 0; i < spinePoints.Count; i++)
{
if (spinePoints[minHeightPoint].Y > spinePoints[i].Y)
{
minHeightPoint = i;
}
if (spinePoints[maxHeightPoint].Y < spinePoints[i].Y)
{
maxHeightPoint = i;
}
if (spinePoints[minZPoint].X > spinePoints[i].X)
{
minZPoint = i;
}
if (spinePoints[maxZPoint].X < spinePoints[i].X)
{
maxZPoint = i;
}
}
double roundFactor = 0.3;
// round them, otherwise the picture is to shaky
// because max and min value are slightly different from frame to frame
double maxHeight = BackTrackerHelper.upperBound(spinePoints[maxHeightPoint].Y, roundFactor);
double minHeight = BackTrackerHelper.lowerBound(spinePoints[minHeightPoint].Y, roundFactor);
double maxZ = BackTrackerHelper.upperBound(spinePoints[maxZPoint].X, roundFactor);
double minZ = BackTrackerHelper.lowerBound(spinePoints[minZPoint].X, roundFactor);
double conversionRateY = height / (maxHeight - minHeight);
double conversionRateZ = width / (maxZ - minZ);
double conversionRate = Math.Min(conversionRateY, conversionRateZ);
// convert coordinates of a spine to pixels on the screen
for (int i = 0; i < spinePoints.Count; i++)
{
int pictureY = height - 1 - (int)(conversionRate * (spinePoints[i].Y - minHeight));
int pictureX = (int)(conversionRate * (spinePoints[i].X - minZ));
this.spinePixels[pictureY * width + pictureX] = BodyColor[0];
}
this.spineBitmap.WritePixels(
new Int32Rect(0, 0, this.spineBitmap.PixelWidth, this.spineBitmap.PixelHeight),
this.spinePixels,
this.spineBitmap.PixelWidth * (int)BytesPerPixel,
0);
}
