public IList<DepthPointEx> Filter(IList<DepthPointEx> points)
{
IList<DepthPointEx> result = new List<DepthPointEx>();
if (points.Count > 0)
{
var point = new DepthPointEx(points.First());
result.Add(point);
foreach (var currentSourcePoint in points.Skip(1))
{
if (!PointsAreClose(currentSourcePoint, point))
{
point = new DepthPointEx(currentSourcePoint);
result.Add(point);
}
}
if (result.Count > 1)
{
CheckFirstAndLastPoint(result);
}
}
return result;
}
public IList <DepthPointEx> Filter(IList <DepthPointEx> points)
{
IList <DepthPointEx> result = new List <DepthPointEx>();
if (points.Count > 0)
{
var point = new DepthPointEx(points.First());
result.Add(point);
foreach (var currentSourcePoint in points.Skip(1))
{
if (!PointsAreClose(currentSourcePoint, point))
{
point = new DepthPointEx(currentSourcePoint);
result.Add(point);
}
}
if (result.Count > 1)
{
CheckFirstAndLastPoint(result);
}
}
return(result);
}
public DepthPointEx(DepthPointEx point)
: this()
{
X = point.X;
Y = point.Y;
Z = point.Z;
}
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);
}
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);
}
private DepthPointEx GetMinimumPoint(DepthPointEx p1, DepthPointEx p2)
{
if (p1.Y < p2.Y)
{
return(p1);
}
else if (p1.Y == p2.Y)
{
if (p1.X < p2.X)
{
return(p1);
}
}
return(p2);
}
public static int FindIndexOfNearestPoint(DepthPointEx target, IList<DepthPointEx> points)
{
int index = 0;
int resultIndex = -1;
double minDist = double.MaxValue;
foreach (DepthPointEx p in points)
{
var distance = Distance(p.X, p.Y, target.X, target.Y);
if (distance < minDist)
{
resultIndex = index;
minDist = distance;
}
index++;
}
return resultIndex;
}
public static int FindIndexOfNearestPoint(DepthPointEx target, IList <DepthPointEx> points)
{
int index = 0;
int resultIndex = -1;
double minDist = double.MaxValue;
foreach (DepthPointEx p in points)
{
var distance = Distance(p.X, p.Y, target.X, target.Y);
if (distance < minDist)
{
resultIndex = index;
minDist = distance;
}
index++;
}
return(resultIndex);
}
public static double Distance(DepthPointEx p1, DepthPointEx p2)
{
return Math.Sqrt(Math.Pow(p1.X - p2.X, 2) + Math.Pow(p1.Y - p2.Y, 2) + Math.Pow(p1.Z - p2.Z, 2));
}
public static DepthPointEx Center(DepthPointEx p1, DepthPointEx p2)
{
return new DepthPointEx((p1.X + p2.X) / 2, (p1.Y + p2.Y) / 2, (p1.Z + p2.Z) / 2);
}
public static double Angle(DepthPointEx center, DepthPointEx start, DepthPointEx end)
{
return Angle(center.X, center.Y, start.X, start.Y, end.X, end.Y);
}
public static DepthPointEx Center(DepthPointEx p1, DepthPointEx p2)
{
return(new DepthPointEx((p1.X + p2.X) / 2, (p1.Y + p2.Y) / 2, (p1.Z + p2.Z) / 2));
}
private bool PointsAreClose(DepthPointEx sourcePoint, DepthPointEx destPoint)
{
return(DepthPointEx.Distance(sourcePoint, destPoint) < MINIMUM_DISTANCE);
}
private bool PointsAreClose(DepthPointEx sourcePoint, DepthPointEx destPoint)
{
return DepthPointEx.Distance(sourcePoint, destPoint) < MINIMUM_DISTANCE;
}
private Hand GetHand(ulong trackingID, HandState state, List<DepthPointEx> contour, double angle, float wristX, float wristY, bool hand, float handRightX, float handRightY)
{
IList<DepthPointEx> convexHull = _grahamScan.ConvexHull(contour);
IList<DepthPointEx> filtered = _lineThinner.Filter(convexHull);
if (angle > -90.0 && angle < 30.0)
{
// Hand "up".
fingers = filtered.Where(p => p.Y < wristY).Take(11).ToList();
}
else if (angle >= 30.0 && angle < 90.0)
{
fingers = filtered.Where(p => p.X > wristX).Take(11).ToList();
//Console.WriteLine(aux.X);
}
else if (angle >= 90.0 && angle < 180.0)
{
fingers = filtered.Where(p => p.Y > wristY).Take(11).ToList();
}
else
{
fingers = filtered.Where(p => p.X < wristX).Take(11).ToList();
}
// List<string> listForPython = null;
//try
// {
// PythonInstance receiveHand = new PythonInstance(@"
//class PyClass:
// def __init__(self):
// pass
// def receiveHand(self,list):
// print list[0]");
// receiveHand.CallMethod("receiveHand", listForPython);
// }
// catch (Exception e) { }
try
{
// Hand is a flag for right hand
if(hand) {
//so considera uma mao propria para gravacao a que consiga captar 5 pontos
if (fingers.Count == 5)
{
this.rightFingers = null;
this.rightFingers = this.fingers;
DepthPointEx centerRightHand = new DepthPointEx();
centerRightHand.X = handRightX;
centerRightHand.Y = handRightY;
this.rightFingers.Add(centerRightHand);
int tam = this.rightFingers.Count - 1;
Console.WriteLine(tam);
//normalizacao, setando todos os valores com origem no centro da mao
//Calculon das falanges, simulacao leap motion
for (int i = 0; i < tam; i++)
{
DepthPointEx middleEachFinger = new DepthPointEx();
//delimita o dedao
if (i == 3)
{
middleEachFinger.X = ((this.rightFingers[i].X + handRightX) / (float)2.0);
middleEachFinger.Y = ((this.rightFingers[i].Y + handRightY) / (float)1.9);
this.rightFingers.Add(middleEachFinger);
}
else if(i==4)
{
middleEachFinger.X = ((this.rightFingers[i].X + handRightX) / (float)2.02);
middleEachFinger.Y = ((this.rightFingers[i].Y + handRightY) / (float)2.0);
this.rightFingers.Add(middleEachFinger);
}
else if(i==0)
{
middleEachFinger.X = ((this.rightFingers[i].X + handRightX) / (float)2.0);
middleEachFinger.Y = ((this.rightFingers[i].Y + handRightY) / (float)2.0);
this.rightFingers.Add(middleEachFinger);
}
else
{
middleEachFinger.X = ((this.rightFingers[i].X + handRightX) / (float)1.98);
middleEachFinger.Y = ((this.rightFingers[i].Y + handRightY) / (float)2.0);
this.rightFingers.Add(middleEachFinger);
}
}
}
}
}
catch (Exception e) { }
if (contour.Count > 0 && fingers.Count > 0)
{
return new Hand(trackingID, state, contour, fingers, CoordinateMapper);
}
return null;
}
public static double Angle(DepthPointEx center, DepthPointEx start, DepthPointEx end)
{
return(Angle(center.X, center.Y, start.X, start.Y, end.X, end.Y));
}
public static DepthPointEx FindNearestPoint(DepthPointEx target, IEnumerable <DepthPointEx> points)
{
var pointList = points.ToList();
return(pointList[FindIndexOfNearestPoint(target, pointList)]);
}
/// <summary>
/// Updates the finger-detection engine with the new data.
/// </summary>
/// <param name="data">A pointer to an array of depth data.</param>
/// <param name="body">The body to search for hands and fingers.</param>
public unsafe void Update(ushort *data, Body body)
{
if (data == null || body == null)
{
return;
}
if (DepthWidth == 0)
{
DepthWidth = DEFAULT_DEPTH_WIDTH;
}
if (DepthHeight == 0)
{
DepthHeight = DEFAULT_DEPTH_HEIGHT;
}
if (_handPixelsLeft == null)
{
_handPixelsLeft = new byte[DepthWidth * DepthHeight];
}
if (_handPixelsRight == null)
{
_handPixelsRight = new byte[DepthWidth * DepthHeight];
}
//Hand handLeft = null;
Hand handRight = null;
//Joint jointHandLeft = body.Joints[JointType.HandLeft];
Joint jointHandRight = body.Joints[JointType.HandRight];
//Joint jointWristLeft = body.Joints[JointType.WristLeft];
Joint jointWristRight = body.Joints[JointType.WristRight];
//Joint jointTipLeft = body.Joints[JointType.HandTipLeft];
Joint jointTipRight = body.Joints[JointType.HandTipRight];
//Joint jointThumbLeft = body.Joints[JointType.ThumbLeft];
Joint jointThumbRight = body.Joints[JointType.ThumbRight];
// DepthSpacePoint depthPointHandLeft = CoordinateMapper.MapCameraPointToDepthSpace(jointHandLeft.Position);
// DepthSpacePoint depthPointWristLeft = CoordinateMapper.MapCameraPointToDepthSpace(jointWristLeft.Position);
//DepthSpacePoint depthPointTipLeft = CoordinateMapper.MapCameraPointToDepthSpace(jointTipLeft.Position);
//DepthSpacePoint depthPointThumbLeft = CoordinateMapper.MapCameraPointToDepthSpace(jointThumbLeft.Position);
DepthSpacePoint depthPointHandRight = CoordinateMapper.MapCameraPointToDepthSpace(jointHandRight.Position);
DepthSpacePoint depthPointWristRight = CoordinateMapper.MapCameraPointToDepthSpace(jointWristRight.Position);
DepthSpacePoint depthPointTipRight = CoordinateMapper.MapCameraPointToDepthSpace(jointTipRight.Position);
DepthSpacePoint depthPointThumbRight = CoordinateMapper.MapCameraPointToDepthSpace(jointThumbRight.Position);
//float handLeftX = depthPointHandLeft.X;
//float handLeftY = depthPointHandLeft.Y;
//float wristLeftX = depthPointWristLeft.X;
//float wristLeftY = depthPointWristLeft.Y;
//float tipLeftX = depthPointTipLeft.X;
//float tipLeftY = depthPointTipLeft.Y;
//float thumbLeftX = depthPointThumbLeft.X;
//float thumbLeftY = depthPointThumbLeft.Y;
float handRightX = depthPointHandRight.X;
float handRightY = depthPointHandRight.Y;
float wristRightX = depthPointWristRight.X;
float wristRightY = depthPointWristRight.Y;
float tipRightX = depthPointTipRight.X;
float tipRightY = depthPointTipRight.Y;
float thumbRightX = depthPointThumbRight.X;
float thumbRightY = depthPointThumbRight.Y;
// bool searchForLeftHand = DetectLeftHand && !float.IsInfinity(handLeftX) && !float.IsInfinity(handLeftY) && !float.IsInfinity(wristLeftX) && !float.IsInfinity(wristLeftY) && !float.IsInfinity(tipLeftX) && !float.IsInfinity(tipLeftY) && !float.IsInfinity(thumbLeftX) && !float.IsInfinity(thumbLeftY);
bool searchForRightHand = DetectRightHand && !float.IsInfinity(handRightX) && !float.IsInfinity(handRightY) && !float.IsInfinity(wristRightX) && !float.IsInfinity(wristRightY) && !float.IsInfinity(tipRightX) && !float.IsInfinity(tipRightY) && !float.IsInfinity(thumbRightX) && !float.IsInfinity(thumbRightY);
this.fingers = new List <DepthPointEx>();
//if (searchForLeftHand || searchForRightHand)
if (searchForRightHand)
{
//double distanceLeft = searchForLeftHand ? CalculateDistance(handLeftX, handLeftY, tipLeftX, tipLeftY, thumbLeftX, thumbLeftY) : 0.0;
double distanceRight = searchForRightHand ? CalculateDistance(handRightX, handRightY, tipRightX, tipRightY, thumbRightX, thumbRightY) : 0.0;
//double angleLeft = searchForLeftHand ? DepthPointEx.Angle(wristLeftX, wristLeftY, wristLeftX, 0, handLeftX, handLeftY) : 0.0;
double angleRight = searchForRightHand ? DepthPointEx.Angle(wristRightX, wristRightY, wristRightX, 0, handRightX, handRightY) : 0.0;
//int minLeftX = searchForLeftHand ? (int)(handLeftX - distanceLeft) : 0;
//int minLeftY = searchForLeftHand ? (int)(handLeftY - distanceLeft) : 0;
//int maxLeftX = searchForLeftHand ? (int)(handLeftX + distanceLeft) : 0;
//int maxLeftY = searchForLeftHand ? (int)(handLeftY + distanceLeft) : 0;
int minRightX = searchForRightHand ? (int)(handRightX - distanceRight) : 0;
int minRightY = searchForRightHand ? (int)(handRightY - distanceRight) : 0;
int maxRightX = searchForRightHand ? (int)(handRightX + distanceRight) : 0;
int maxRightY = searchForRightHand ? (int)(handRightY + distanceRight) : 0;
//float depthLeft = jointHandLeft.Position.Z * 1000; // m to mm
float depthRight = jointHandRight.Position.Z * 1000;
for (int i = 0; i < DepthWidth * DepthHeight; ++i)
{
ushort depth = data[i];
int depthX = i % DepthWidth;
int depthY = i / DepthWidth;
bool isInBounds = depth >= MIN_DEPTH && depth <= MAX_DEPTH;
//bool conditionLeft = depth >= depthLeft - DEPTH_THRESHOLD &&
// depth <= depthLeft + DEPTH_THRESHOLD &&
// depthX >= minLeftX && depthX <= maxLeftX &&
// depthY >= minLeftY && depthY <= maxLeftY;
bool conditionRight = depth >= depthRight - DEPTH_THRESHOLD &&
depth <= depthRight + DEPTH_THRESHOLD &&
depthX >= minRightX && depthX <= maxRightX &&
depthY >= minRightY && depthY <= maxRightY;
//_handPixelsLeft[i] = (byte)(isInBounds && searchForLeftHand && conditionLeft ? 255 : 0);
_handPixelsRight[i] = (byte)(isInBounds && searchForRightHand && conditionRight ? 255 : 0);
}
List <DepthPointEx> contourLeft = new List <DepthPointEx>();
List <DepthPointEx> contourRight = new List <DepthPointEx>();
for (int i = 0; i < DepthWidth * DepthHeight; ++i)
{
ushort depth = data[i];
int depthX = i % DepthWidth;
int depthY = i / DepthWidth;
//if (searchForLeftHand)
//{
// if (_handPixelsLeft[i] != 0)
// {
// byte top = i - DepthWidth >= 0 ? _handPixelsLeft[i - DepthWidth] : (byte)0;
// byte bottom = i + DepthWidth < _handPixelsLeft.Length ? _handPixelsLeft[i + DepthWidth] : (byte)0;
// byte left = i - 1 >= 0 ? _handPixelsLeft[i - 1] : (byte)0;
// byte right = i + 1 < _handPixelsLeft.Length ? _handPixelsLeft[i + 1] : (byte)0;
// bool isInContour = top == 0 || bottom == 0 || left == 0 || right == 0;
// if (isInContour)
// {
// contourLeft.Add(new DepthPointEx { X = depthX, Y = depthY, Z = depth });
// }
// }
//}
if (searchForRightHand)
{
if (_handPixelsRight[i] != 0)
{
byte top = i - DepthWidth >= 0 ? _handPixelsRight[i - DepthWidth] : (byte)0;
byte bottom = i + DepthWidth < _handPixelsRight.Length ? _handPixelsRight[i + DepthWidth] : (byte)0;
byte left = i - 1 >= 0 ? _handPixelsRight[i - 1] : (byte)0;
byte right = i + 1 < _handPixelsRight.Length ? _handPixelsRight[i + 1] : (byte)0;
bool isInContour = top == 0 || bottom == 0 || left == 0 || right == 0;
if (isInContour)
{
contourRight.Add(new DepthPointEx {
X = depthX, Y = depthY, Z = depth
});
}
}
}
}
//if (searchForLeftHand)
//{
// handLeft = GetHand(body.TrackingId, body.HandLeftState, contourLeft, angleLeft, wristLeftX, wristLeftY, false, handLeftX, handLeftY);
//}
if (searchForRightHand)
{
handRight = GetHand(body.TrackingId, body.HandRightState, contourRight, angleRight, wristRightX, wristRightY, true, handRightX, handRightY);
}
}
//if (handLeft != null || handRight != null)
if (handRight != null)
{
HandCollection hands = new HandCollection
{
TrackingId = body.TrackingId,
//HandLeft = handLeft,
HandRight = handRight
};
if (HandsDetected != null)
{
HandsDetected(this, hands);
}
}
}
private Hand GetHand(ulong trackingID, HandState state, List <DepthPointEx> contour, double angle, float wristX, float wristY, bool hand, float handRightX, float handRightY)
{
IList <DepthPointEx> convexHull = _grahamScan.ConvexHull(contour);
IList <DepthPointEx> filtered = _lineThinner.Filter(convexHull);
if (angle > -90.0 && angle < 30.0)
{
// Hand "up".
fingers = filtered.Where(p => p.Y < wristY).Take(11).ToList();
}
else if (angle >= 30.0 && angle < 90.0)
{
fingers = filtered.Where(p => p.X > wristX).Take(11).ToList();
//Console.WriteLine(aux.X);
}
else if (angle >= 90.0 && angle < 180.0)
{
fingers = filtered.Where(p => p.Y > wristY).Take(11).ToList();
}
else
{
fingers = filtered.Where(p => p.X < wristX).Take(11).ToList();
}
// List<string> listForPython = null;
//try
// {
// PythonInstance receiveHand = new PythonInstance(@"
//class PyClass:
// def __init__(self):
// pass
// def receiveHand(self,list):
// print list[0]");
// receiveHand.CallMethod("receiveHand", listForPython);
// }
// catch (Exception e) { }
try
{
// Hand is a flag for right hand
if (hand)
{
//so considera uma mao propria para gravacao a que consiga captar 5 pontos
if (fingers.Count == 5)
{
this.rightFingers = null;
this.rightFingers = this.fingers;
DepthPointEx centerRightHand = new DepthPointEx();
centerRightHand.X = handRightX;
centerRightHand.Y = handRightY;
this.rightFingers.Add(centerRightHand);
int tam = this.rightFingers.Count - 1;
Console.WriteLine(tam);
//normalizacao, setando todos os valores com origem no centro da mao
//Calculon das falanges, simulacao leap motion
for (int i = 0; i < tam; i++)
{
DepthPointEx middleEachFinger = new DepthPointEx();
//delimita o dedao
if (i == 3)
{
middleEachFinger.X = ((this.rightFingers[i].X + handRightX) / (float)2.0);
middleEachFinger.Y = ((this.rightFingers[i].Y + handRightY) / (float)1.9);
this.rightFingers.Add(middleEachFinger);
}
else if (i == 4)
{
middleEachFinger.X = ((this.rightFingers[i].X + handRightX) / (float)2.02);
middleEachFinger.Y = ((this.rightFingers[i].Y + handRightY) / (float)2.0);
this.rightFingers.Add(middleEachFinger);
}
else if (i == 0)
{
middleEachFinger.X = ((this.rightFingers[i].X + handRightX) / (float)2.0);
middleEachFinger.Y = ((this.rightFingers[i].Y + handRightY) / (float)2.0);
this.rightFingers.Add(middleEachFinger);
}
else
{
middleEachFinger.X = ((this.rightFingers[i].X + handRightX) / (float)1.98);
middleEachFinger.Y = ((this.rightFingers[i].Y + handRightY) / (float)2.0);
this.rightFingers.Add(middleEachFinger);
}
}
}
}
}
catch (Exception e) { }
if (contour.Count > 0 && fingers.Count > 0)
{
return(new Hand(trackingID, state, contour, fingers, CoordinateMapper));
}
return(null);
}
public DepthPointEx(DepthPointEx point) : this()
{
X = point.X;
Y = point.Y;
Z = point.Z;
}
public static DepthPointEx FindNearestPoint(DepthPointEx target, IEnumerable<DepthPointEx> points)
{
var pointList = points.ToList();
return pointList[FindIndexOfNearestPoint(target, pointList)];
}
public static double Distance(DepthPointEx p1, DepthPointEx p2)
{
return(Math.Sqrt(Math.Pow(p1.X - p2.X, 2) + Math.Pow(p1.Y - p2.Y, 2) + Math.Pow(p1.Z - p2.Z, 2)));
}