public unsafe void Update(ushort[] depthData, byte[] bodyIndexData, Body body)
{
double handLength = 0.0;
double barLength = 0.0;
double barHeight = 0.0;
double angle = 0.0;
Joint shoulderLeft = body.Joints[JointType.ShoulderLeft];
Joint shoulderRight = body.Joints[JointType.ShoulderRight];
Joint chest = body.Joints[JointType.SpineShoulder];
Joint waist = body.Joints[JointType.SpineBase];
Joint elbowLeft = body.Joints[JointType.ElbowLeft];
Joint elbowRight = body.Joints[JointType.ElbowRight];
Joint handLeft = body.Joints[JointType.HandLeft];
Joint handRight = body.Joints[JointType.HandRight];
Joint footLeft = body.Joints[JointType.FootLeft];
Joint footRight = body.Joints[JointType.FootRight];
if (waist.TrackingState == TrackingState.NotTracked)
{
return;
}
if (waist.Position.Z < 2.0f || waist.Position.Z > 4.5f)
{
return;
}
if (shoulderLeft.TrackingState != TrackingState.NotTracked && shoulderRight.TrackingState != TrackingState.NotTracked &&
elbowLeft.TrackingState != TrackingState.NotTracked && elbowRight.TrackingState != TrackingState.NotTracked &&
handLeft.TrackingState != TrackingState.NotTracked && handRight.TrackingState != TrackingState.NotTracked)
{
handLength =
shoulderLeft.Position.Length(shoulderRight.Position) +
shoulderLeft.Position.Length(elbowLeft.Position) +
shoulderRight.Position.Length(elbowRight.Position) +
elbowLeft.Position.Length(handLeft.Position) +
elbowRight.Position.Length(handRight.Position);
}
CoordinateMapper.MapColorFrameToDepthSpace(depthData, _depthPoints);
fixed(DepthSpacePoint *colorMappedToDepthPointsPointer = _depthPoints)
{
int minimumX = int.MaxValue;
int maximumX = int.MinValue;
int minimumY = int.MaxValue;
int maximumY = int.MinValue;
ushort minimumDistance = 0;
ushort maximumdistance = 0;
for (int colorIndex = 0; colorIndex < _depthPoints.Length; ++colorIndex)
{
float colorMappedToDepthX = colorMappedToDepthPointsPointer[colorIndex].X;
float colorMappedToDepthY = colorMappedToDepthPointsPointer[colorIndex].Y;
if (!float.IsNegativeInfinity(colorMappedToDepthX) &&
!float.IsNegativeInfinity(colorMappedToDepthY))
{
int depthX = (int)(colorMappedToDepthX + 0.5f);
int depthY = (int)(colorMappedToDepthY + 0.5f);
if ((depthX >= 0) && (depthX < DepthWidth) && (depthY >= 0) && (depthY < DepthHeight))
{
int depthIndex = (depthY * DepthWidth) + depthX;
ushort depth = depthData[depthIndex];
if (bodyIndexData[depthIndex] != 0xff)
{
if (depthX < minimumX)
{
minimumX = depthX;
minimumY = depthY;
minimumDistance = depth;
}
if (depthX > maximumX)
{
maximumX = depthX;
maximumY = depthY;
maximumdistance = depth;
}
continue;
}
}
}
}
DepthSpacePoint depthMinimum = new DepthSpacePoint
{
X = minimumX,
Y = minimumY
};
DepthSpacePoint depthMaximum = new DepthSpacePoint
{
X = maximumX,
Y = maximumY
};
CameraSpacePoint cameraMinimum = CoordinateMapper.MapDepthPointToCameraSpace(depthMinimum, minimumDistance);
CameraSpacePoint cameraMaximum = CoordinateMapper.MapDepthPointToCameraSpace(depthMaximum, maximumdistance);
ColorSpacePoint colorMinimum = CoordinateMapper.MapDepthPointToColorSpace(depthMinimum, minimumDistance);
ColorSpacePoint colorMaximum = CoordinateMapper.MapDepthPointToColorSpace(depthMaximum, maximumdistance);
CameraSpacePoint cameraTrail = new CameraSpacePoint
{
X = (cameraMinimum.X + cameraMaximum.X) / 2f,
Y = (cameraMinimum.Y + cameraMaximum.Y) / 2f,
Z = (cameraMinimum.Z + cameraMaximum.Z) / 2f
};
ColorSpacePoint colorTrail = new ColorSpacePoint
{
X = (colorMinimum.X + colorMaximum.X) / 2f,
Y = (colorMinimum.Y + colorMaximum.Y) / 2f
};
DepthSpacePoint depthTrail = new DepthSpacePoint
{
X = (depthMinimum.X + depthMaximum.X) / 2f,
Y = (depthMinimum.Y + depthMaximum.Y) / 2f
};
CameraSpacePoint feet = new CameraSpacePoint
{
X = (footLeft.Position.X + footRight.Position.X) / 2f,
Y = (footLeft.Position.Y + footRight.Position.Y) / 2f,
Z = (footLeft.Position.Z + footRight.Position.Z) / 2f
};
CameraSpacePoint projection = new CameraSpacePoint
{
X = cameraTrail.X,
Y = feet.Y,
Z = cameraTrail.Z
};
barLength = cameraMinimum.Length(cameraMaximum);
barHeight = cameraTrail.Length(projection);
angle = cameraMinimum.Angle(cameraMaximum, new CameraSpacePoint {
X = cameraMaximum.X, Y = cameraMinimum.Y, Z = (cameraMaximum.Z + cameraMinimum.Z) / 2f
});
if (angle > 180.0)
{
angle = 360.0 - angle;
}
if (cameraMinimum.Y < cameraMaximum.Y)
{
angle = -angle;
}
if (barLength > handLength)
{
BarDetectionResult result = new BarDetectionResult
{
Minimum = new MultiPoint
{
CameraPoint = cameraMinimum,
ColorPoint = colorMinimum,
DepthPoint = depthMinimum
},
Maximum = new MultiPoint
{
CameraPoint = cameraMaximum,
ColorPoint = colorMaximum,
DepthPoint = depthMaximum
},
Trail = new MultiPoint
{
CameraPoint = cameraTrail,
ColorPoint = colorTrail,
DepthPoint = depthTrail
},
BarHeight = barHeight + HEIGHT_DIFFERENCE,
BarLength = barLength,
Angle = angle
};
BarDetected?.Invoke(this, result);
}
}
}