internal Finger(DepthPointEx point, CoordinateMapper coordinateMapper)
{
ushort depth = (ushort)point.Z;
DepthPoint = new DepthSpacePoint
{
X = point.X,
Y = point.Y
};
ColorPoint = coordinateMapper.MapDepthPointToColorSpace(DepthPoint, (ushort)point.Z);
CameraPoint = coordinateMapper.MapDepthPointToCameraSpace(DepthPoint, (ushort)point.Z);
}
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);
}
}
}
public Boulder(DepthSpacePoint depPoint, ushort dep, Point ptOnCanvas, double widthOnCanvas, double heightOnCanvas, Canvas canvas, CoordinateMapper coMapper)
: this(coMapper.MapDepthPointToCameraSpace(depPoint, dep), ptOnCanvas, widthOnCanvas, heightOnCanvas, canvas)
{
}
void _faceReader_FrameArrived(FaceFrameReader sender, FaceFrameArrivedEventArgs args)
{
using (var faceFrame = args.FrameReference.AcquireFrame())
{
if (faceFrame != null)
{
using (var bodyFrame = faceFrame.BodyFrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
using (var depthFrame = faceFrame.DepthFrameReference.AcquireFrame())
{
if (depthFrame != null)
{
depthFrame.CopyFrameDataToArray(this.depthFrameData);
}
}
//get hands and check for touching nose and/or covering mouth
Body[] bodies = new Body[bodyFrame.BodyCount];
bodyFrame.GetAndRefreshBodyData(bodies);
foreach (var body in bodies)
{
if (body.IsTracked)
{
//get nose and mouth positions
var noseLocation = faceFrame.FaceFrameResult.FacePointsInInfraredSpace[FacePointType.Nose];
var mouthLeftLocation = faceFrame.FaceFrameResult.FacePointsInInfraredSpace[FacePointType.MouthCornerLeft];
var mouthRightLocation = faceFrame.FaceFrameResult.FacePointsInInfraredSpace[FacePointType.MouthCornerRight];
//get the depthSpacePoint from the noseLocation
// calculate index into depth array
int noseDepthIndex = ((int)noseLocation.Y * _depthWidth) + (int)noseLocation.X;
int mouthLeftIndex = ((int)mouthLeftLocation.Y * _depthWidth) + (int)mouthLeftLocation.X;
int mouthRightIndex = ((int)mouthRightLocation.Y * _depthWidth) + (int)mouthRightLocation.X;
var depthSpaceNosePoint = new DepthSpacePoint()
{
X = (float)noseLocation.X, Y = (float)noseLocation.Y
};
var depthSpaceMouthLeftPoint = new DepthSpacePoint()
{
X = (float)mouthLeftLocation.X, Y = (float)mouthLeftLocation.Y
};
var depthSpaceMouthRightPoint = new DepthSpacePoint()
{
X = (float)mouthRightLocation.X, Y = (float)mouthRightLocation.Y
};
var noseCamera = _coordinateMapper.MapDepthPointToCameraSpace(depthSpaceNosePoint, depthFrameData[noseDepthIndex]);
var mouthLeftCamera = _coordinateMapper.MapDepthPointToCameraSpace(depthSpaceMouthLeftPoint, depthFrameData[mouthLeftIndex]);
var mouthRightCamera = _coordinateMapper.MapDepthPointToCameraSpace(depthSpaceMouthRightPoint, depthFrameData[mouthRightIndex]);
var leftHandLocation = body.Joints[JointType.HandTipLeft].Position;
var rightHandLocation = body.Joints[JointType.HandTipRight].Position;
if (noseCamera.X != 0 || noseCamera.Y != 0)
{
var isLeftHandTouchingNose = (Math.Abs((leftHandLocation.X * leftHandLocation.X - noseCamera.X * noseCamera.X)) <= 0.01f) && (Math.Abs((leftHandLocation.Y * leftHandLocation.Y - noseCamera.Y * noseCamera.Y)) <= 0.01f) && (Math.Abs((leftHandLocation.Z * leftHandLocation.Z - noseCamera.Z * noseCamera.Z)) <= 0.010f);
var isRightHandTouchingNose = (Math.Abs((rightHandLocation.X * rightHandLocation.X - noseCamera.X * noseCamera.X)) <= 0.01f) && (Math.Abs((rightHandLocation.Y * rightHandLocation.Y - noseCamera.Y * noseCamera.Y)) <= 0.01f) && (Math.Abs((rightHandLocation.Z * rightHandLocation.Z - noseCamera.Z * noseCamera.Z)) <= 0.010f);
if (isLeftHandTouchingNose || isRightHandTouchingNose)
{
TouchNoseToleranceCount++;
if (TouchNoseToleranceCount >= 1)
{
OnNoseTouchArrived(new NoseTouchArrivedEventArgs()
{
Confidence = .90f
});
TouchNoseToleranceCount = 0;
}
}
}
if (mouthLeftCamera.X != 0 || mouthLeftCamera.Y != 0 || mouthRightCamera.X != 0 || mouthRightCamera.Y != 0)
{
var isLeftHandTouchingMouthLeft = (Math.Abs((leftHandLocation.X * leftHandLocation.X - mouthLeftCamera.X * mouthLeftCamera.X)) <= 0.01f) && (Math.Abs((leftHandLocation.Y * leftHandLocation.Y - mouthLeftCamera.Y * mouthLeftCamera.Y)) <= 0.01f) && (Math.Abs((leftHandLocation.Z * leftHandLocation.Z - mouthLeftCamera.Z * mouthLeftCamera.Z)) <= 0.010f);
var isRightHandTouchingMouthLeft = (Math.Abs((rightHandLocation.X * rightHandLocation.X - mouthLeftCamera.X * mouthLeftCamera.X)) <= 0.01f) && (Math.Abs((rightHandLocation.Y * rightHandLocation.Y - mouthLeftCamera.Y * mouthLeftCamera.Y)) <= 0.01f) && (Math.Abs((rightHandLocation.Z * rightHandLocation.Z - mouthLeftCamera.Z * mouthLeftCamera.Z)) <= 0.010f);
var isLeftHandTouchingMouthRight = (Math.Abs((leftHandLocation.X * leftHandLocation.X - mouthRightCamera.X * mouthRightCamera.X)) <= 0.01f) && (Math.Abs((leftHandLocation.Y * leftHandLocation.Y - mouthRightCamera.Y * mouthRightCamera.Y)) <= 0.01f) && (Math.Abs((leftHandLocation.Z * leftHandLocation.Z - mouthRightCamera.Z * mouthRightCamera.Z)) <= 0.010f);
var isRightHandTouchingMouthRight = (Math.Abs((rightHandLocation.X * rightHandLocation.X - mouthRightCamera.X * mouthRightCamera.X)) <= 0.01f) && (Math.Abs((rightHandLocation.Y * rightHandLocation.Y - mouthRightCamera.Y * mouthRightCamera.Y)) <= 0.01f) && (Math.Abs((rightHandLocation.Z * rightHandLocation.Z - mouthRightCamera.Z * mouthRightCamera.Z)) <= 0.010f);
if (isLeftHandTouchingMouthRight || isRightHandTouchingMouthRight || isLeftHandTouchingMouthLeft || isRightHandTouchingMouthLeft)
{
MouthCoverToleranceCount++;
if (MouthCoverToleranceCount >= 1)
{
OnMouthCoverArrived(new MouthCoverArrivedEventArgs()
{
Confidence = .90f
});
MouthCoverToleranceCount = 0;
}
}
}
}
}
}
}
}
}
}
public RockViewModel(DepthSpacePoint depPoint, ushort dep,
double widthOnCanvas, double heightOnCanvas, Canvas canvas, CoordinateMapper coorMap) :
this(coorMap.MapDepthPointToCameraSpace(depPoint, dep), new Size(widthOnCanvas, heightOnCanvas), canvas, coorMap)
{
}
