public static int GetHighestValidPointIndexInRectangle(int bIndex, int cIndex)
{
Point pB = GetPoint(bIndex);
Point pC = GetPoint(cIndex);
int rectangleWidth = pB.X - pC.X;
CameraSpacePoint highestPoint = new CameraSpacePoint()
{
X = float.PositiveInfinity,
Y = float.PositiveInfinity,
Z = float.PositiveInfinity
};
int highestPointIndex = -1;
for (int i = 0; i < rectangleWidth; i++)
{
for (int j = 0; j < rectangleWidth; j++)
{
int currentIndex = GetIndex(pC.X + j, pB.Y + i);
if (BoundaryCheck(currentIndex))
{
CameraSpacePoint currentPoint = GlobVar.SubtractedFilteredPointCloud[currentIndex];
if (currentPoint.Z < highestPoint.Z && !float.IsInfinity(currentPoint.X) && !float.IsInfinity(currentPoint.Y))
{
highestPoint = currentPoint;
highestPointIndex = currentIndex;
}
}
}
}
return highestPointIndex;
}
public static float GetEuclideanDistance(CameraSpacePoint a, CameraSpacePoint b)
{
float distance =
(float)Math.Sqrt((a.X - b.X) * (a.X - b.X) + (a.Y - b.Y) * (a.Y - b.Y) + (a.Z - b.Z) * (a.Z - b.Z));
return float.IsNaN(distance) ? float.MaxValue : distance;
}
/// <summary>
/// Normalizes the depths in the input frame into a 0-255 byte gray-value array.
/// </summary>
public static byte[] CalculateIntensityFromCameraSpacePoints(CameraSpacePoint[] cameraSpacePoint)
{
var intensityMap = new byte[cameraSpacePoint.Length];
for (int i = 0; i < cameraSpacePoint.Length; i++)
{
float depth = cameraSpacePoint[i].Z;
if (depth != 0)
{
float currentMax = depth - GlobVar.MinSensingDepth;
const float currentDepthRange = GlobVar.MaxSensingDepth - GlobVar.MinSensingDepth;
if (depth < GlobVar.MaxSensingDepth && depth > GlobVar.MinSensingDepth)
{
intensityMap[i] = (byte)(255 - (255 * currentMax / currentDepthRange));
}
else
{
intensityMap[i] = (byte)0;
}
}
else
{
intensityMap[i] = (byte)0;
}
}
return intensityMap;
}
private static float[] CreateIntegralImage(CameraSpacePoint[] cameraSpacePoints)
{
float[] integralImage = new float[GlobVar.ScaledFrameLength];
for (var i = 0; i < GlobVar.ScaledFrameHeight; i++)
{
for (var j = 0; j < GlobVar.ScaledFrameWidth; j++)
{
int indexCurrent = i * GlobVar.ScaledFrameWidth + j;
int indexUpLeft = (i - 1) * GlobVar.ScaledFrameWidth + j - 1;
int indexUp = (i - 1) * GlobVar.ScaledFrameWidth + j;
int indexLeft = i * GlobVar.ScaledFrameWidth + j - 1;
if (i==0 && j==0)
{
integralImage[indexCurrent] = cameraSpacePoints[indexCurrent].Z;
}
else if (i==0)
{
integralImage[indexCurrent] = cameraSpacePoints[indexCurrent].Z + integralImage[indexLeft];
}
else if (j == 0)
{
integralImage[indexCurrent] = cameraSpacePoints[indexCurrent].Z + integralImage[indexUp];
}
else
{
integralImage[indexCurrent] = cameraSpacePoints[indexCurrent].Z + integralImage[indexUp] + integralImage[indexLeft] - integralImage[indexUpLeft];
}
}
}
return integralImage;
}
/// <summary>
/// Returns the length of the segment defined by the specified points.
/// </summary>
/// <param name="p1">The first point (start of the segment).</param>
/// <param name="p2">The second point (end of the segment).</param>
/// <returns>The length of the segment in meters.</returns>
public static double Distance(CameraSpacePoint p1, CameraSpacePoint 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 bool checPeriodicMovements(Body newBody)
{
time = DateTime.Now- startTime;
if(time.TotalMilliseconds>=4000)
{
result = checkAnalisys();
resetValues();
startTime = DateTime.Now;
}
else
{
hipNew = newBody.Joints[JointType.HipRight].Position;
if (BodyFramePreAnalysis.bodyOld.Count > 0)
{
if(Math.Abs(hipNew.X-hipOld.X)>0.009
|| Math.Abs(hipNew.Y - hipOld.Y) > 0.009 ||
Math.Abs(hipNew.Z - hipOld.Z) > 0.009)
{
hipOld = BodyFramePreAnalysis.bodyOld[JointType.HipRight].Position;
storeValues();
}
}
}
return result;
}
/// <summary>
/// Calculates the angle between the specified points.
/// </summary>
/// <param name="center">The center of the angle.</param>
/// <param name="start">The start of the angle.</param>
/// <param name="end">The end of the angle.</param>
/// <returns>The angle, in degrees.</returns>
public static double Angle(this CameraSpacePoint center, CameraSpacePoint start, CameraSpacePoint end)
{
Vector3D first = start.ToVector3() - center.ToVector3();
Vector3D second = end.ToVector3() - center.ToVector3();
return Vector3D.AngleBetween(first, second);
}
public static List<Point3D> CameraSpacePointsToPoint3Ds(CameraSpacePoint[] cameraSpacePoints)
{
if (cameraSpacePoints == null)
{
return null;
}
if (cameraSpacePoints.Length == 0)
{
return null;
}
List<Point3D> point3Ds = new List<Point3D>();
foreach (CameraSpacePoint point in cameraSpacePoints)
{
if (GeometryHelper.IsValidPoint(point))
{
point3Ds.Add(new Point3D
{
X = point.X,
Y = point.Y,
Z = point.Z
});
}
}
return point3Ds;
}
/// <summary>
/// Calculates the angle between the specified points around the specified axis.
/// </summary>
/// <param name="center">The center of the angle.</param>
/// <param name="start">The start of the angle.</param>
/// <param name="end">The end of the angle.</param>
/// <param name="axis">The axis around which the angle is calculated.</param>
/// <returns>The angle, in degrees.</returns>
public static double Angle(this CameraSpacePoint center, CameraSpacePoint start, CameraSpacePoint end, Axis axis)
{
switch (axis)
{
case Axis.X:
start.X = 0f;
center.X = 0f;
end.X = 0f;
break;
case Axis.Y:
start.Y = 0f;
center.Y = 0f;
end.Y = 0f;
break;
case Axis.Z:
start.Z = 0f;
center.Z = 0f;
end.Z = 0f;
break;
}
Vector3D first = start.ToVector3() - center.ToVector3();
Vector3D second = end.ToVector3() - center.ToVector3();
return Vector3D.AngleBetween(first, second);
}
public CameraSpacePoint Filter(CameraSpacePoint point)
{
if (this.lastTrend == null || this.lastOutput == null)
{
this.lastTrend = point;
this.lastOutput = point;
return point;
}
var newTrend = new CameraSpacePoint
{
X = gamma * (point.X - lastOutput.Value.X) + (1 - gamma) * lastTrend.Value.X,
Y = gamma * (point.Y - lastOutput.Value.Y) + (1 - gamma) * lastTrend.Value.Y,
Z = gamma * (point.Z - lastOutput.Value.Z) + (1 - gamma) * lastTrend.Value.Z
};
var newOutput = new CameraSpacePoint
{
X = alpha * point.X + (1 - alpha) * (lastOutput.Value.X + lastTrend.Value.X),
Y = alpha * point.Y + (1 - alpha) * (lastOutput.Value.Y + lastTrend.Value.Y),
Z = alpha * point.Z + (1 - alpha) * (lastOutput.Value.Z + lastTrend.Value.Z)
};
this.lastTrend = newTrend;
this.lastOutput = newOutput;
return newOutput;
}
// Update is called once per frame
void Update ()
{
float deltaTime = Time.deltaTime;
mousePosX = CrossPlatformInputManager.GetAxis ("Mouse X");
translateVector.Set (-deltaTime*50*mousePosX, 0, 0);
if (Input.GetMouseButtonDown (0))
mouseClicked = true;
else if (Input.GetMouseButtonUp (0))
mouseClicked = false;
if (mouseClicked) {
transform.Translate(translateVector);
}
//Kinect
if (bodyManager == null) {
return;
}
bodies = bodyManager.GetData ();
if (bodies == null) {
return;
}
foreach (var body in bodies) {
if (body == null){
continue;
}
if (body.IsTracked)
{
Debug.Log(body.HandLeftState.ToString());
if (body.HandLeftState == HandState.Closed){
if(grab == false){
lastPos = body.Joints[TrackedJoint].Position;
lastObjPos = gameObject.transform.position;
}
grab = true;
var pos = body.Joints[TrackedJoint].Position;
Debug.Log(pos.X-lastPos.X);
transform.position = new Vector3(lastObjPos.x + (pos.X - lastPos.X) * multiplier, 1, -10);
//translateVector.Set (deltaTime*50*pos.X-lastPos.X , 0, 0);
//transform.Translate(translateVector);
}
else if (body.HandLeftState == HandState.Open){
grab = false;
}
}
}
}
private void SetValues(CameraSpacePoint hand)
{
KinectX = hand.X;
KinectY = hand.Y;
ScreenX = (MaxValue / 2) + (KinectX * MaxValue);
ScreenY = (MaxValue / 2) - (KinectY * MaxValue);
}
public void CheckActiveWorkspace(CameraSpacePoint[] handPositions)
{
if (handPositions.Length > 0)
{
CheckActiveWorkspace(Converter.CameraSpacePointsToPoint3Ds(handPositions).ToArray());
}
}
private static CameraSpacePoint CopyPosition(CameraSpacePoint position)
{
CameraSpacePoint result = new CameraSpacePoint();
result.X = position.X;
result.Y = position.Y;
result.Z = position.Z;
return result;
}
/// <summary>
/// Create a new <c>CustomJoint</c> object based on the supplied
/// <c>JointType</c> value.
/// </summary>
public CustomJoint(JointType type)
{
_jointType = type;
_position = new CameraSpacePoint();
_depthPosition = new DepthSpacePoint();
_colorPosition = new ColorSpacePoint();
_trackingState = TrackingState.NotTracked;
}
private static double DistanceBetweenPoints(CameraSpacePoint point1, CameraSpacePoint point2)
{
double dx = Math.Abs(point2.X - point1.X);
double dy = Math.Abs(point2.Y - point1.Y);
double dz = Math.Abs(point2.Z - point1.Z);
return Math.Sqrt(dx * dx + dy * dy + dz * dz);
}
private static CameraSpacePoint SubstractPoints(CameraSpacePoint position1, CameraSpacePoint position2)
{
CameraSpacePoint result = new CameraSpacePoint();
result.X = position1.X - position2.X;
result.Y = position1.Y - position2.Y;
result.Z = position1.Z - position2.Z;
return result;
}
/* Depricated */
public static CameraSpacePoint getEinsvektor(CameraSpacePoint point)
{
var e = (float)Math.Sqrt(Math.Pow(point.X, 2) + Math.Pow(point.Y, 2) + Math.Pow(point.Z, 2));
point.X = point.X / e;
point.Y = point.Y / e;
point.Z = point.Z / e;
return point;
}
public static Point3D CameraSpacePointToPoint3D(CameraSpacePoint cameraSpacePoint)
{
return new Point3D
{
X = cameraSpacePoint.X,
Y = cameraSpacePoint.Y,
Z = cameraSpacePoint.Z
};
}
public void AddFrame(CameraSpacePoint[] newFrame)
{
_temporalFrames.Add(_frameCounter,newFrame);
_frameCounter++;
if (_frameCounter == _maxTemporalFrames)
{
CalculateTemporalMedianImage();
}
}
/// <summary>
/// Copies camera space points from face to the gpu
/// </summary>
/// <param name="context"></param>
/// <param name="points"></param>
public void Copy(DeviceContext context, CameraSpacePoint[] points)
{
if (points.Length == 0)
return;
fixed (CameraSpacePoint* cptr = &points[0])
{
this.buffer.Upload(context, new IntPtr(cptr), points.Length * 12);
}
}
CameraSpacePoint CSVectorSubtract(CameraSpacePoint p1, CameraSpacePoint p2)
{
CameraSpacePoint diff = new CameraSpacePoint();
diff.X = p1.X - p2.X;
diff.Y = p1.Y - p2.Y;
diff.Z = p1.Z - p2.Z;
return diff;
}
public Head(int highestPointIndex)
{
HighestPointIndex = highestPointIndex;
AvgCenterPoint = new CameraSpacePoint()
{
X = float.NaN,
Y = float.NaN,
Z = float.NaN
};
}
public Point MapCameraSpacePoint(CameraSpacePoint point)
{
double canvasWidth = canvas.ActualWidth;
double canvasHeight = canvas.ActualHeight;
var colourSpacePosition = mapper.MapCameraPointToColorSpace(point);
return (new Point()
{
X = (colourSpacePosition.X / colourFrameSize.Width) * canvasWidth,
Y = (colourSpacePosition.Y / colourFrameSize.Height) * canvasHeight
});
}
public static Joint[] ValidRandomJointsSpinePosition(CameraSpacePoint cp)
{
Joint[] result = new Joint[Microsoft.Kinect.Body.JointCount];
JointType[] jt = (JointType[])Enum.GetValues(typeof(JointType));
for (int i = 0; i < result.Length; i++)
{
result[i].JointType = jt[i];
if (jt[i] == JointType.SpineBase)
{
result[i].Position = cp;
}
}
return result;
}
public static CameraSpacePoint[] CreateEmptyPointCloud()
{
CameraSpacePoint[] emptyPointCloud = new CameraSpacePoint[GlobVar.ScaledFrameLength];
float maxDepth = GlobVar.MaxSensingDepth;
for (int i = 0; i < emptyPointCloud.Length; i++)
{
emptyPointCloud[i] = new CameraSpacePoint()
{
X = float.PositiveInfinity,
Y = float.PositiveInfinity,
Z = maxDepth
};
}
return emptyPointCloud;
}
public static double Process(CameraSpacePoint md1, CameraSpacePoint md2, CameraSpacePoint md3, bool twoD, bool supplementary = false, bool relative = false)
{
double rx1 = md1.X - md2.X;
double ry1 = md1.Y - md2.Y;
double rz1 = md1.Z - md2.Z;
double rx2 = md3.X - md2.X;
double ry2 = md3.Y - md2.Y;
double rz2 = md3.Z - md2.Z;
double angle = double.NaN;
if (twoD)
angle = GetAngleBetweenPoints(rx1, ry1, rx2, ry2);
else
angle = GetAngleBetweenPoints(rx1, ry1, rz1, rx2, ry2, rz2);
double ret = supplementary ? 180.0 - angle : angle;
if (twoD) ret = -ret;
return ret;
}
public void Update(CameraSpacePoint currentPoint)
{
wasReset = false;
anchorPoint.X = CumulativeAverage(anchorPoint.X, currentPoint.X, this.count);
anchorPoint.Y = CumulativeAverage(anchorPoint.Y, currentPoint.Y, this.count);
anchorPoint.Z = CumulativeAverage(anchorPoint.Z, currentPoint.Z, this.count);
++count;
if (CSVectorLength(CSVectorSubtract(anchorPoint, currentPoint)) > resetThreshold)
{
Reset(currentPoint);
wasReset = true;
}
}
public static CameraSpacePoint GetAccumulatedAvarage(this CameraSpacePoint lastAnchor, CameraSpacePoint newPosition, ref Int32 count)
{
var result = new CameraSpacePoint();
// TODO ! RORU find out poblem of this filtering
//result.X = (lastAnchor.X * count) / (count + 1) + newPosition.X * (1 / (count + 1));
//result.Y = (lastAnchor.Y * count) / (count + 1) + newPosition.Y * (1 / (count + 1));
//result.Z = (lastAnchor.Z * count) / (count + 1) + newPosition.Z * (1 / (count + 1));
result.X = (lastAnchor.X + newPosition.X) / 2;
result.Y = (lastAnchor.Y + newPosition.Y) / 2;
result.Z = (lastAnchor.Z + newPosition.Z) / 2;
++count;
return result;
}
/* Depricated */
public static CameraSpacePoint getNormalVector(CameraSpacePoint p1, CameraSpacePoint p2, CameraSpacePoint p3)
{
var a = new CameraSpacePoint();
a.X = p1.X - p2.X;
a.Y = p1.Y - p2.Y;
a.Z = p1.Z - p2.Z;
var b = new CameraSpacePoint();
b.X = p1.X - p3.X;
b.Y = p1.Y - p3.Y;
b.Z = p1.Z - p3.Z;
var n = new CameraSpacePoint();
n.X = a.Y * b.Z - a.Z * b.Y;
n.Y = a.X * b.Z - a.Z * b.X;
n.Z = a.X * b.Y - a.Y * b.X;
return n;
}
/// <summary>
/// 获取两个三维的点组成的向量,
/// 如向量AB
/// </summary>
/// <param name="A"></param>
/// <param name="B"></param>
/// <returns></returns>
public static CameraSpacePoint GetVector(CameraSpacePoint A, CameraSpacePoint B)
{
return(VectorHelp.GetSubtract(B, A));
}
public Point projectToScreen(CameraSpacePoint point)
{
Vector3D convertedPoint = new Vector3D(point.X, point.Y, point.Z);
return(mapCameraScreenPointToPixelCoord(mapCameraSpacePointToScreenPoint(mapKinectSpacePointToCameraSpace(convertedPoint))));
}
internal void RecoverCalibrationFromSensor(Microsoft.Kinect.KinectSensor kinectSensor)
{
var stopWatch = new System.Diagnostics.Stopwatch();
stopWatch.Start();
var objectPoints1 = new List <Vector <double> >();
var colorPoints1 = new List <System.Drawing.PointF>();
var depthPoints1 = new List <System.Drawing.PointF>();
int n = 0;
for (float x = -2f; x < 2f; x += 0.2f)
{
for (float y = -2f; y < 2f; y += 0.2f)
{
for (float z = 0.4f; z < 4.5f; z += 0.4f)
{
var kinectCameraPoint = new CameraSpacePoint();
kinectCameraPoint.X = x;
kinectCameraPoint.Y = y;
kinectCameraPoint.Z = z;
// use SDK's projection
// adjust Y to make RH coordinate system that is a projection of Kinect 3D points
var kinectColorPoint = kinectSensor.CoordinateMapper.MapCameraPointToColorSpace(kinectCameraPoint);
kinectColorPoint.Y = colorImageHeight - kinectColorPoint.Y;
var kinectDepthPoint = kinectSensor.CoordinateMapper.MapCameraPointToDepthSpace(kinectCameraPoint);
kinectDepthPoint.Y = depthImageHeight - kinectDepthPoint.Y;
if ((kinectColorPoint.X >= 0) && (kinectColorPoint.X < colorImageWidth) &&
(kinectColorPoint.Y >= 0) && (kinectColorPoint.Y < colorImageHeight) &&
(kinectDepthPoint.X >= 0) && (kinectDepthPoint.X < depthImageWidth) &&
(kinectDepthPoint.Y >= 0) && (kinectDepthPoint.Y < depthImageHeight))
{
n++;
var objectPoint = Vector <double> .Build.Dense(3);
objectPoint[0] = kinectCameraPoint.X;
objectPoint[1] = kinectCameraPoint.Y;
objectPoint[2] = kinectCameraPoint.Z;
objectPoints1.Add(objectPoint);
var colorPoint = new System.Drawing.PointF();
colorPoint.X = kinectColorPoint.X;
colorPoint.Y = kinectColorPoint.Y;
colorPoints1.Add(colorPoint);
//Console.WriteLine(objectPoint[0] + "\t" + objectPoint[1] + "\t" + colorPoint.X + "\t" + colorPoint.Y);
var depthPoint = new System.Drawing.PointF();
depthPoint.X = kinectDepthPoint.X;
depthPoint.Y = kinectDepthPoint.Y;
depthPoints1.Add(depthPoint);
}
}
}
}
this.colorCameraMatrix[0, 0] = 1000; //fx
this.colorCameraMatrix[1, 1] = 1000; //fy
this.colorCameraMatrix[0, 2] = colorImageWidth / 2; //cx
this.colorCameraMatrix[1, 2] = colorImageHeight / 2; //cy
this.colorCameraMatrix[2, 2] = 1;
var rotation = Vector <double> .Build.Dense(3);
var translation = Vector <double> .Build.Dense(3);
var colorError = CalibrateColorCamera(objectPoints1, colorPoints1, colorCameraMatrix, colorLensDistortion, rotation, translation, this.silent);
var rotationMatrix = AxisAngleToMatrix(rotation);
this.depthToColorTransform = Matrix <double> .Build.DenseIdentity(4, 4);
for (int i = 0; i < 3; i++)
{
this.depthToColorTransform[i, 3] = translation[i];
for (int j = 0; j < 3; j++)
{
this.depthToColorTransform[i, j] = rotationMatrix[i, j];
}
}
this.depthCameraMatrix[0, 0] = 360; //fx
this.depthCameraMatrix[1, 1] = 360; //fy
this.depthCameraMatrix[0, 2] = depthImageWidth / 2.0; //cx
this.depthCameraMatrix[1, 2] = depthImageHeight / 2.0; //cy
this.depthCameraMatrix[2, 2] = 1;
var depthError = CalibrateDepthCamera(objectPoints1, depthPoints1, depthCameraMatrix, depthLensDistortion, silent);
// check projections
double depthProjectionError = 0;
double colorProjectionError = 0;
var testObjectPoint4 = Vector <double> .Build.Dense(4);
for (int i = 0; i < n; i++)
{
var testObjectPoint = objectPoints1[i];
var testDepthPoint = depthPoints1[i];
var testColorPoint = colorPoints1[i];
// "camera space" == depth camera space
// depth camera projection
double depthU, depthV;
Project(depthCameraMatrix, depthLensDistortion, testObjectPoint[0], testObjectPoint[1], testObjectPoint[2], out depthU, out depthV);
double dx = testDepthPoint.X - depthU;
double dy = testDepthPoint.Y - depthV;
depthProjectionError += (dx * dx) + (dy * dy);
// color camera projection
testObjectPoint4[0] = testObjectPoint[0];
testObjectPoint4[1] = testObjectPoint[1];
testObjectPoint4[2] = testObjectPoint[2];
testObjectPoint4[3] = 1;
var color = depthToColorTransform * testObjectPoint4;
color *= (1.0 / color[3]); // not necessary for this transform
double colorU, colorV;
Project(colorCameraMatrix, colorLensDistortion, color[0], color[1], color[2], out colorU, out colorV);
dx = testColorPoint.X - colorU;
dy = testColorPoint.Y - colorV;
colorProjectionError += (dx * dx) + (dy * dy);
}
depthProjectionError /= n;
colorProjectionError /= n;
stopWatch.Stop();
if (!this.silent)
{
Console.WriteLine("FakeCalibration :");
Console.WriteLine("n = " + n);
Console.WriteLine("color error = " + colorError);
Console.WriteLine("depth error = " + depthError);
Console.WriteLine("depth reprojection error = " + depthProjectionError);
Console.WriteLine("color reprojection error = " + colorProjectionError);
Console.WriteLine("depth camera matrix = \n" + depthCameraMatrix);
Console.WriteLine("depth lens distortion = \n" + depthLensDistortion);
Console.WriteLine("color camera matrix = \n" + colorCameraMatrix);
Console.WriteLine("color lens distortion = \n" + colorLensDistortion);
Console.WriteLine(stopWatch.ElapsedMilliseconds + " ms");
Console.WriteLine("________________________________________________________");
}
}
/// <summary>
/// Handles the body frame data arriving from the sensor
/// </summary>
/// <param name="sender">object sending the event</param>
/// <param name="e">event arguments</param>
private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
bool dataReceived = false;
using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
if (this.bodies == null)
{
this.bodies = new Body[bodyFrame.BodyCount];
}
// The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
// As long as those body objects are not disposed and not set to null in the array,
// those body objects will be re-used.
bodyFrame.GetAndRefreshBodyData(this.bodies);
dataReceived = true;
}
}
if (dataReceived)
{
using (DrawingContext dc = this.drawingGroup.Open())
{
// Draw a transparent background to set the render size
//dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
dc.DrawRectangle(Brushes.Black, null, this.displayRect);
//Count Bodies Observed
bodies_currently_observed = 0;
//Check for alerts
global_contamination_alert = Contamination_Alert();
global_proximity_alert = Proximity_Alert();
//foreach (Body body in this.bodies)
for (int i = 0; i < this.bodies.Length; i++)
{
//Change pen color to red if that pedestrian is currently causing an alert
Pen drawPen = new Pen(Brushes.White, 6);
if (proximity_alert_offenders.Contains(i) || contamination_alert_offenders.Contains(i))
{
drawPen = new Pen(Brushes.Red, 6);
}
this.DrawFaceFrameResults(i, dc);
if (bodies[i].IsTracked)
{
//Increment Body Count
bodies_currently_observed++;
// draw face frame results
this.DrawClippedEdges(bodies[i], dc);
IReadOnlyDictionary <JointType, Joint> joints = bodies[i].Joints;
// convert the joint points to depth (display) space
Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();
foreach (JointType jointType in joints.Keys)
{
// sometimes the depth(Z) of an inferred joint may show as negative
// clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
CameraSpacePoint position = joints[jointType].Position;
if (position.Z < 0)
{
position.Z = InferredZPositionClamp;
}
DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
}
this.DrawBody(joints, jointPoints, dc, drawPen);
}
}
//Play Audio Alerts
//this.PropertyChanged(this, new PropertyChangedEventArgs("ImageSource"));
//Face-Touch Contamination Alert
if (global_contamination_alert)
{
if (!contamination_alarm_currently_playing && !proximity_alarm_currently_playing)
{
contamination_alarm_currently_playing = true;
alarm_1.PlayLooping();
this.PropertyChanged(this, new PropertyChangedEventArgs("ImageSource"));
}
}
else
{
if (contamination_alarm_currently_playing)
{
alarm_1.Stop();
contamination_alarm_currently_playing = false;
this.PropertyChanged(this, new PropertyChangedEventArgs("ImageSource"));
}
}
//Ped - Ped Proximity Alert - SHOULD BE THE MASTER ALARM IF THIS ONE IS ACTIVE THEN A FACE ALARM WON'T MATTER
if (global_proximity_alert)
{
if (!proximity_alarm_currently_playing)
{
contamination_alarm_currently_playing = false;
alarm_1.Stop();
proximity_alarm_currently_playing = true;
alarm_2.PlayLooping();
this.PropertyChanged(this, new PropertyChangedEventArgs("ImageSource"));
}
}
else
{
if (proximity_alarm_currently_playing)
{
alarm_2.Stop();
proximity_alarm_currently_playing = false;
this.PropertyChanged(this, new PropertyChangedEventArgs("ImageSource"));
}
}
//Display the Number of bodies tracked currently
dc.DrawText(
new FormattedText(
("Number of Bodies Detected = " + bodies_currently_observed + "\nProximity Alert: " + global_proximity_alert + "\nFace Touch Alert: " + global_contamination_alert),
CultureInfo.GetCultureInfo("en-us"),
FlowDirection.LeftToRight,
new Typeface("Georgia"),
DrawTextFontSize,
Brushes.White),
new Point(displayWidth / 2, displayHeight - 50)
);
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(this.displayRect);//new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
}
}
}
public void Read_BodyFrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
bool dataReceived = false;
using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
if (this.bodies == null)
{
this.bodies = new Body[bodyFrame.BodyCount];
}
bodyFrame.GetAndRefreshBodyData(this.bodies);
dataReceived = true;
}
}
if (dataReceived)
{
using (DrawingContext dc = this.drawingGroup.Open())
{
dc.DrawRectangle(Brushes.Transparent, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
//Transparent
int penIndex = 0;
foreach (Body body in this.bodies)
{
Pen drawPen = this.bodyColors[penIndex++];
if (body.IsTracked)
{
this.DrawClippedEdges(body, dc);
IReadOnlyDictionary <JointType, Joint> joints = body.Joints;
Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();
foreach (JointType jointType in joints.Keys)
{
CameraSpacePoint position = joints[jointType].Position;
if (position.Z < 0)
{
position.Z = InferredZPositionClamp;
}
DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
}
this.DrawBody(joints, jointPoints, dc, drawPen);
HandMotion_Tracking.DrawHand_Right(jointPoints[JointType.HandRight], dc);
//HandMotion_Tracking.DrawHand_left(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
}
}
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
}
}
}
private double DistBetPoints(CameraSpacePoint P1, CameraSpacePoint P2)
{
return(Math.Sqrt(((P2.X - P1.X) * (P2.X - P1.X)) + ((P2.Y - P1.Y) * (P2.Y - P1.Y)) + ((P2.Z - P1.Z) * (P2.Z - P1.Z))));
}
public static Vector3 CSPtoVector3(CameraSpacePoint a_csp)
{
return(new Vector3(a_csp.X, a_csp.Y, a_csp.Z));
}
/// <summary>
/// Handles the body frame data arriving from the sensor
/// </summary>
/// <param name="sender">object sending the event</param>
/// <param name="e">event arguments</param>
//TODO ほんとにこれでよい？
private void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
bool dataReceived = false;
using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame().BodyFrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
if (this.bodies == null)
{
this.bodies = new Body[bodyFrame.BodyCount];
}
// The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
// As long as those body objects are not disposed and not set to null in the array,
// those body objects will be re-used.
bodyFrame.GetAndRefreshBodyData(this.bodies);
dataReceived = true;
}
}
using (ColorFrame colorFrame = e.FrameReference.AcquireFrame().ColorFrameReference.AcquireFrame())
{
if (colorFrame != null)
{
if (this.bodyColors == null)
{
}
}
}
using (DrawingContext dc = this.drawingGroup.Open())
{
if (dataReceived)
{
// Draw a transparent background to set the render size
dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
int penIndex = 0;
foreach (Body body in this.bodies)
{
Pen drawPen = this.bodyColors[penIndex++];
if (body.IsTracked)
{
if (!bodyDrawDictionary.ContainsKey(body))
{
List <List <Point> > newPointListList = new List <List <Point> >();
newPointListList.Add(new List <Point>());
bodyDrawDictionary.Add(body, newPointListList);
}
List <List <Point> > pointListList = bodyDrawDictionary[body];
if (pointListList.Count > 10)
{
pointListList.RemoveAt(0);
}
this.DrawClippedEdges(body, dc);
IReadOnlyDictionary <JointType, Joint> joints = body.Joints;
// convert the joint points to depth (display) space
Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();
foreach (JointType jointType in joints.Keys)
{
// sometimes the depth(Z) of an inferred joint may show as negative
// clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
CameraSpacePoint position = joints[jointType].Position;
if (position.Z < 0)
{
position.Z = InferredZPositionClamp;
}
DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
}
this.DrawBody(joints, jointPoints, dc, drawPen);
this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
// クリア（全消去）する
//if ( body.HandLeftState == HandState.Closed )
//{
// pointListList = new List<List<Point>>();
// pointListList.Add(new List<Point>());
//}
List <Point> lastPointList = pointListList[pointListList.Count - 1];
// 長過ぎる線は先端から消していく
if (lastPointList.Count > 100)
{
lastPointList.RemoveAt(0);
}
// 条件を満たしたとき pointListに追加する
if (lastPointList.Count > 0 && body.HandRightState == HandState.Closed)
{
Point tmpPoint = jointPoints[JointType.HandRight];
Point lastPoint = lastPointList[lastPointList.Count - 1];
if (Math.Abs(tmpPoint.X - lastPoint.X) > 10 ||
Math.Abs(tmpPoint.Y - lastPoint.Y) > 10)
{
lastPointList.Add(tmpPoint);
}
}
HandState prevHandState;
if (prevHandStateDictionary.ContainsKey(body))
{
prevHandState = prevHandStateDictionary[body];
}
else
{
prevHandState = HandState.Unknown;
}
if (body.HandRightState != HandState.NotTracked &&
body.HandRightState != HandState.Unknown &&
prevHandState != body.HandRightState)
{
if (body.HandRightState == HandState.Closed)
{
//閉じた手に変わった瞬間
isRightHandClosedDictionary[body] = false;
String str = "";
str += " " + DateTime.Now.ToString();
str += " Start";
Console.WriteLine(str);
controlState = ControlState.Start;
startPoint = jointPoints[JointType.HandRight];
lastPointList.Add(startPoint);
}
else if (prevHandState == HandState.Closed)
{
//閉じた手だったのがそれ以外に変わった瞬間
isRightHandClosedDictionary[body] = true;
String str = "";
str += body.ToString();
str += " " + DateTime.Now.ToString();
str += " End";
str += " tmpHandState:" + prevHandState;
str += " body.HandRightState: " + body.HandRightState;
Console.WriteLine(str);
controlState = ControlState.None;
endPoint = jointPoints[JointType.HandRight];
lastPointList.Add(endPoint);
pointListList.Add(new List <Point>());
}
}
if (body.HandRightState != HandState.Unknown &&
body.HandRightState != HandState.NotTracked)
{
prevHandStateDictionary[body] = body.HandRightState;
}
if (body.HandLeftState == HandState.Lasso)
{
Point currentPoint = jointPoints[JointType.HandLeft];
double cX = currentPoint.X;
double cY = currentPoint.Y;
for (int j = 0; j < pointListList.Count; j++)
{
List <Point> pointList = pointListList[j];
for (int i = 0; i < pointList.Count - 1; i++)
{
Point prevPoint = pointList[i];
Point nextPoint = pointList[i + 1];
double dX = nextPoint.X - prevPoint.X;
double dY = nextPoint.Y - prevPoint.Y;
double d = Math.Abs(dY / dX * cX - cY);
if (d < 100)
{
pointListList.RemoveAt(j);
break;
}
}
}
}
}
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
}
foreach (KeyValuePair <Body, List <List <Point> > > pair in bodyDrawDictionary)
{
List <List <Point> > pointListList = pair.Value;
/* 線の描画 */
for (int j = 0; j < pointListList.Count; j++)
{
List <Point> pointList = pointListList[j];
for (int i = 0; i < pointList.Count - 1; i++)
{
Point lineStartPoint = pointList[i];
Point lineEndPoint = pointList[i + 1];
dc.DrawLine(pen, lineStartPoint, lineEndPoint);
}
}
}
}
}
/// <summary>
/// Returns the Euclidean distante betweeen two 3D vectors p1 - p2.
/// </summary>
/// <param name="p1">First 3D vector</param>
/// <param name="p2">Second 3D vector</param>
/// <returns></returns>
public double EuclideanDistance3d(CameraSpacePoint p1, CameraSpacePoint 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)));
}
private void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
MultiSourceFrame frameReference = e.FrameReference.AcquireFrame();
using (ColorFrame colorFrame = frameReference.ColorFrameReference.AcquireFrame())
{
if (colorFrame != null)
{
mainWindow.cameraImage.Source = colorFrame.ToBitmap();
}
}
bool dataReceived = false;
using (BodyFrame bodyFrame = frameReference.BodyFrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
mainWindow.bodyCanvas.Children.Clear();
if (this.bodies == null)
{
this.bodies = new Body[bodyFrame.BodyCount];
}
bodyFrame.GetAndRefreshBodyData(bodies);
dataReceived = true;
}
}
if (dataReceived)
{
int bodyColor = 0;
if (this.bodies != null)
{
mainWindow.client.Bodies = this.bodies;
int maxBodies = this.kinectSensor.BodyFrameSource.BodyCount;
int trackedBodies = 0;
for (int i = 0; i < maxBodies; i++)
{
if (bodies[i] != null)
{
if (bodies[i].IsTracked)
{
trackedBodies++;
IReadOnlyDictionary <JointType, Joint> joints = bodies[i].Joints;
Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();
foreach (JointType jointType in joints.Keys)
{
CameraSpacePoint position = joints[jointType].Position;
if (position.Z < 0)
{
position.Z = InferredZPosition;
}
ColorSpacePoint colorSpacePoint = this.coordinateMapper.MapCameraPointToColorSpace(position);
double x = float.IsInfinity(colorSpacePoint.X) ? 0 : colorSpacePoint.X / COLOR_SPACE_MAX_WIDTH * mainWindow.bodyCanvas.ActualWidth;
double y = float.IsInfinity(colorSpacePoint.Y) ? 0 : colorSpacePoint.Y / COLOR_SPACE_MAX_HEIGHT * mainWindow.bodyCanvas.ActualHeight;
jointPoints[jointType] = new Point(x, y);
}
mainWindow.bodyCanvas.DrawSkeleton(joints, jointPoints, color[2 * bodyColor], color[2 * bodyColor + 1]);
}
}
bodyColor++;
}
changeNumberOfTrackedPeople(trackedBodies);
}
}
}
/// <summary>
/// Handles the body frame data arriving from the sensor
/// </summary>
/// <param name="sender">object sending the event</param>
/// <param name="e">event arguments</param>
private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
bool dataReceived = false;
using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
if (this.bodies == null)
{
this.bodies = new Body[bodyFrame.BodyCount];
}
// The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
// As long as those body objects are not disposed and not set to null in the array,
// those body objects will be re-used.
bodyFrame.GetAndRefreshBodyData(this.bodies);
dataReceived = true;
}
}
if (dataReceived)
{
using (DrawingContext dc = this.drawingGroup.Open())
{
// Draw a transparent background to set the render size
dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
int penIndex = 0;
foreach (Body body in this.bodies)
{
Pen drawPen = this.bodyColors[penIndex++];
if (body.IsTracked)
{
this.DrawClippedEdges(body, dc);
IReadOnlyDictionary <JointType, Joint> joints = body.Joints;
// convert the joint points to depth (display) space
Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();
foreach (JointType jointType in joints.Keys)
{
// sometimes the depth(Z) of an inferred joint may show as negative
// clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
CameraSpacePoint position = joints[jointType].Position;
if (position.Z < 0)
{
position.Z = InferredZPositionClamp;
}
DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
}
Joint kneeR = body.Joints[JointType.KneeRight];
Joint ankleR = body.Joints[JointType.AnkleRight];
Joint hipR = body.Joints[JointType.HipRight];
Joint spine = body.Joints[JointType.SpineBase];
Joint kneeL = body.Joints[JointType.KneeLeft];
int myangle = (int)angle(kneeL, spine, kneeR);
//Console.WriteLine(angle(hipR, ankleR, kneeR));
int rAngle = (int)angle(ankleR, kneeR, hipR);
this.DrawBody(joints, jointPoints, dc, drawPen);
this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
if (recording)
{
x.Add(myangle);
kneeAngle.Add(rAngle);
y.Add(count++);
//Console.WriteLine(myangle);
file.WriteLine(myangle);
file2.WriteLine(rAngle);
if (count % 10 == 0)
{
drawgraph(y, x, plotter);
drawgraph(y, kneeAngle, plotter2);
}
}
}
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
}
}
}
// Handles the body frame data arriving from the sensor
private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
bool dataReceived = false;
using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame()) {
if (bodyFrame != null)
{
if (this.bodies == null)
{
this.bodies = new Body[bodyFrame.BodyCount];
}
// The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
// As long as those body objects are not disposed and not set to null in the array,
// those body objects will be re-used.
bodyFrame.GetAndRefreshBodyData(this.bodies);
dataReceived = true;
}
}
if (dataReceived)
{
using (DrawingContext dc = this.drawingGroup.Open()) {
/*******************************************************/
// Dibuja la batería
this.DrawDrums(dc);
/*******************************************************/
// Draw a transparent background to set the render size
dc.DrawRectangle(Brushes.Transparent, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
int penIndex = 0;
//Mantiene la cuenta de cuerpos detectados para solo mostrar el máximo establecido
int bodiesCount = 0;
foreach (Body body in this.bodies)
{
Pen drawPen = this.bodyColors[penIndex++];
if (body.IsTracked)
{
IReadOnlyDictionary <JointType, Joint> joints = body.Joints;
// convert the joint points to depth (display) space
Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();
foreach (JointType jointType in joints.Keys)
{
// sometimes the depth(Z) of an inferred joint may show as negative
// clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
CameraSpacePoint position = joints[jointType].Position;
if (position.Z < 0)
{
position.Z = InferredZPositionClamp;
}
DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
}
this.DrawBody(joints, jointPoints, dc, drawPen);
this.DrawCircle(body.HandLeftState, jointPoints[JointType.HandTipLeft], dc);
this.DrawCircle(body.HandRightState, jointPoints[JointType.HandTipRight], dc);
this.DrawCircle(body.HandLeftState, jointPoints[JointType.FootLeft], dc);
this.DrawCircle(body.HandRightState, jointPoints[JointType.FootRight], dc);
/*************************************************************************************************************/
//Comprueba si ha tocado un tambor
OnDrumHit(jointPoints[JointType.HandTipLeft], jointPoints[JointType.HandTipRight], jointPoints[JointType.FootLeft], jointPoints[JointType.FootRight]);
//Obtiene el ID del body actual
ulong trackingId = body.TrackingId;
// Si el TrackingId de un Body cambia, actualiza su correspondiente GestureDetector con el nuevo valor
if (trackingId != this.gestureDetectorList[bodiesCount].TrackingId)
{
this.gestureDetectorList[bodiesCount].TrackingId = trackingId;
// Si el Body actual esta siendo detectado, inicia su GestureDetector
// Si no está siendo detectado, pausa el GestureDetector para ahorrar recursos
this.gestureDetectorList[bodiesCount].IsPaused = trackingId == 0;
}
//Incrementa los cuerpos detectados;
bodiesCount++;
//Si ya se ha pintado el máximo de bodies, termina de pintarlos
if (bodiesCount == maxBodies)
{
break;
}
/*************************************************************************************************************/
}
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
}
}
}
public double vectMag(CameraSpacePoint point)
{
return(Math.Sqrt(Math.Pow((float)(point.X), 2) + Math.Pow((float)(point.Y), 2) + Math.Pow((float)(point.Z), 2)));
}
//Update position of the raised hand
public void Update()
{
if (trackingId != 0)
{
UserId = trackingId;
}
if (!isPaused)
{
//Get hand, elbow position of left and right hand with tracking ID from kinect camera
#region GetPosition
CameraSpacePoint leftHandPos = UserManager.bodies[UserManager.Users[trackingId]].Joints[JointType.HandLeft].Position;
CameraSpacePoint rightHandPos = UserManager.bodies[UserManager.Users[trackingId]].Joints[JointType.HandRight].Position;
CameraSpacePoint leftEl = UserManager.bodies[UserManager.Users[trackingId]].Joints[JointType.ElbowLeft].Position;
CameraSpacePoint rightEl = UserManager.bodies[UserManager.Users[trackingId]].Joints[JointType.ElbowRight].Position;
#endregion
//=================================================================================
//Check left hand rise
#region CheckLeftHand
if (CheckHandRaise(leftHandPos, leftEl))
{
if (countLeft)
{
stopwatchLeft.Start();
countLeft = false;
}
long leftElapsed = stopwatchLeft.ElapsedMilliseconds;
if (leftElapsed > RaiseTime)
{
OnHandRaise(JointType.HandLeft, UserId);
ResetCountLeft();
}
}
else
{
ResetCountLeft();
}
#endregion
//=================================================================================
//Check right hand rise
#region CheckRightHand
if (CheckHandRaise(rightHandPos, rightEl))
{
if (countRight)
{
stopwatchRight.Start();
countRight = false;
}
long rightElapsed = stopwatchRight.ElapsedMilliseconds;
if (rightElapsed > RaiseTime)
{
OnHandRaise(JointType.HandRight, UserId);
ResetCountRight();
}
}
else
{
ResetCountRight();
}
#endregion
//=================================================================================
}
}
/// <summary>
/// Handles the body frame data arriving from the sensor
/// </summary>
/// <param name="sender">object sending the event</param>
/// <param name="e">event arguments</param>
private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
bool dataReceived = true;
using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
{
if (ranOunce == false)
{
textBox.Text = username;
using (DataService.Service1Client s = new DataService.Service1Client())
{
retrievedData = s.GetBodyData(username, sessionname).ToArray();
}
ranOunce = true;
}
if (bodyFrame != null)
{
if (this.bodies == null)
{
this.bodies = new Body[bodyFrame.BodyCount];
}
// The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
// As long as those body objects are not disposed and not set to null in the array,
// those body objects will be re-used.
bodyFrame.GetAndRefreshBodyData(this.bodies);
dataReceived = true;
}
}
if (dataReceived)
{
using (DrawingContext dc = this.drawingGroup.Open())
{
// Draw a transparent background to set the render size
dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
int penIndex = 0;
Pen drawPen = this.bodyColors[penIndex++];
if (true)
{
this.DrawClippedEdges(this.bodies[0], dc);
IReadOnlyDictionary <JointType, Joint> joints = this.bodies[0].Joints;
// convert the joint points to depth (display) space
Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();
foreach (JointType jointType in joints.Keys)
{
// sometimes the depth(Z) of an inferred joint may show as negative
// clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
CameraSpacePoint position = joints[jointType].Position;
// if (sendMe)
// {
// using (XboxWCFService.Service1Client s = new XboxWCFService.Service1Client())
//{
// textBox.Text = s.GetData(Convert.ToInt32(position.Z));
// sendMe = false;
// }
//}
if (position.Z < 0)
{
position.Z = InferredZPositionClamp;
}
DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
}
joints = Newtonsoft.Json.JsonConvert.DeserializeObject <IReadOnlyDictionary <JointType, Joint> >(retrievedData[replayCount].Joints);
jointPoints = Newtonsoft.Json.JsonConvert.DeserializeObject <Dictionary <JointType, Point> >(retrievedData[replayCount].JointPoints);
if (replayCount < retrievedData.Length - 1)
{
replayCount++;
}
this.DrawBody(joints, jointPoints, dc, drawPen);
this.DrawHand(this.bodies[0].HandLeftState, jointPoints[JointType.HandLeft], dc);
this.DrawHand(this.bodies[0].HandRightState, jointPoints[JointType.HandRight], dc);
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
}
}
}
// multi frame reader event handler
private void ReaderMultiFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
// Get a reference to the multi-frame
var reference = e.FrameReference.AcquireFrame();
// depth
using (DepthFrame depthFrame = reference.DepthFrameReference.AcquireFrame())
{
string label_coords_blob = "";//label_blobsの文字列
if (depthFrame != null)
{
FrameDescription depthFrameDescription = depthFrame.FrameDescription;
int width = depthFrameDescription.Width;
int height = depthFrameDescription.Height;
var depthData = new ushort[width * height];
depthFrame.CopyFrameDataToArray(depthData);
this.depthBitmap.WritePixels(depthRect, depthData, width * (int)depthFrameDescription.BytesPerPixel, 0);
// depthData -> camera space data
CameraSpacePoint[] cs_points = new CameraSpacePoint[width * height];
_sensor.CoordinateMapper.MapDepthFrameToCameraSpace(depthData, cs_points);
// calculate camera space coordinates of each marker(blob) // plan: 以下のループは別functionにする
float[,] coord_blobs_center = new float[list_arr_index.Count(), 3]; // blob中心座標の2次元配列
//label_sample.Content = list_arr_index.Count().ToString();
int i_blob = 0; // blobのindex
foreach (int[] arr_index in list_arr_index)
{
// 各blobのcamera space pointからx, y, z座標を取り出して配列 -> 平均
float[] coord_blob_center = new float[3];//blob (反射マーカー)の中心座標を入れる
// select camera space points corresponding each blob
CameraSpacePoint[] cs_points_blob = new CameraSpacePoint[arr_index.Length];// camera space配列宣言
// x,y,z座標のlist
List <float> list_x_cs_points_blob = new List <float>();
List <float> list_y_cs_points_blob = new List <float>();
List <float> list_z_cs_points_blob = new List <float>();
// x,y,z座標の平均
float x_coord_cs_points_blob = 0;
float y_coord_cs_points_blob = 0;
float z_coord_cs_points_blob = 0;
// listの初期化. 念のため
list_x_cs_points_blob.Clear();
list_y_cs_points_blob.Clear();
list_z_cs_points_blob.Clear();
// for loop
int i_points_blob = 0; // blob内のcs_pointsのindex
//int i_coord_blob = 0; // blob内の座標のindex
foreach (int i_point in arr_index)
{
// arr_index: blobのcamera space pointsに対応するindexes
// cs_points_blobをまとめる
cs_points_blob[i_points_blob] = cs_points[i_point];
i_points_blob += 1;
// x,y,z座標のlistを求める: infinityを外す
if (!Double.IsInfinity(cs_points[i_point].X))
{
list_x_cs_points_blob.Add(cs_points[i_point].X);
list_y_cs_points_blob.Add(cs_points[i_point].Y);
list_z_cs_points_blob.Add(cs_points[i_point].Z);
// 座標の足し算
x_coord_cs_points_blob += cs_points[i_point].X;
y_coord_cs_points_blob += cs_points[i_point].Y;
z_coord_cs_points_blob += cs_points[i_point].Z;
}
}
// listを配列に変換
float[] arr_x_cs_points_blob = list_x_cs_points_blob.ToArray();
float[] arr_y_cs_points_blob = list_y_cs_points_blob.ToArray();
float[] arr_z_cs_points_blob = list_z_cs_points_blob.ToArray();
// cs_points_blobからblobの中心座標を求める ////////////////////
// infの割合を求める
float ratio_valid_points_blob = (float)arr_x_cs_points_blob.Length /
(float)arr_index.Length;// blobの内infinityでなかったpointの割合
// infの割合が1割以以上だったら中心座標の計算
if (ratio_valid_points_blob > 0.0)
{
// 足し算したものを数で割る
x_coord_cs_points_blob = x_coord_cs_points_blob / (float)arr_x_cs_points_blob.Count();
y_coord_cs_points_blob = y_coord_cs_points_blob / (float)arr_y_cs_points_blob.Count(); // 分母はどれも同じ
z_coord_cs_points_blob = z_coord_cs_points_blob / (float)arr_z_cs_points_blob.Count(); // 分母はどれも同じ
}
else
{
x_coord_cs_points_blob = 0;
y_coord_cs_points_blob = 0;
z_coord_cs_points_blob = 0;
}
coord_blob_center = new float[]
{
x_coord_cs_points_blob,
y_coord_cs_points_blob,
z_coord_cs_points_blob
};
// 座標coord_blob_centerを二次元配列にまとめる+ label_coordsのstringを生成
for (int i_xyz = 0; i_xyz < 3; i_xyz++)
{
coord_blobs_center[i_blob, i_xyz] = coord_blob_center[i_xyz];
}
label_coords_blob +=
string.Format("X: {0:+000.0;-000.0;+ 0.0}, ", coord_blob_center[0] * 100) +
string.Format("Y: {0:+000.0;-000.0;+ 0.0}, ", coord_blob_center[1] * 100) +
string.Format("Z: {0:+000.0;-000.0;+ 0.0}\n", coord_blob_center[2] * 100);
i_blob += 1;
}
// coord_blobs_centerを画面に出力
label_coords.Content = label_coords_blob;
}
}
// IR
using (InfraredFrame infraredFrame = reference.InfraredFrameReference.AcquireFrame())
{
if (infraredFrame != null)
{
FrameDescription infraredFrameDescription = infraredFrame.FrameDescription;
int width = infraredFrameDescription.Width;
int height = infraredFrameDescription.Height;
//ushort[] infraredData = new ushort[width * height];
// http://www.naturalsoftware.jp/entry/2014/07/25/020750
var infraredData = new ushort[width * height]; // ushort array
infraredFrame.CopyFrameDataToArray(infraredData);
this.infraredBitmap.Lock();
this.infraredBitmap.WritePixels(infraredRect, infraredData, width * (int)infraredFrameDescription.BytesPerPixel, 0);
//depthImage.WritePixels(depthRect, depthBuffer, depthStride, 0);// template
this.infraredBitmap.Unlock();
ColorImage.Source = this.infraredBitmap;
// OpenCV: Count blobs and
CountBlobs(this.infraredBitmap);
}
}
}
private void UpdateFacePoints()
{
if (_faceModel == null)
{
return;
}
var vertices = _faceModel.CalculateVerticesForAlignment(_faceAlignment);
if (vertices.Count > 0)
{
if (_points.Count == 0)
{
for (int index = 0; index < vertices.Count; index++)
{
Ellipse ellipse = new Ellipse
{
Width = 2.0,
Height = 2.0,
Fill = new SolidColorBrush(Colors.Blue)
};
_points.Add(ellipse);
}
foreach (Ellipse ellipse in _points)
{
canvas.Children.Add(ellipse);
}
}
for (int index = 0; index < vertices.Count; index++)
{
CameraSpacePoint vertice = vertices[index];
DepthSpacePoint point = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(vertice);
//inner eyebrow check
if (index == 803 || index == 346)
{
DepthSpacePoint innerbrowl = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(vertices[803]);
DepthSpacePoint innerbrowr = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(vertices[346]);
double dx = innerbrowl.X - innerbrowr.X;
double dy = innerbrowl.Y - innerbrowr.Y;
double dist = dx * dx + dy * dy;
// Debug.WriteLine("eyebrowdistance " + Math.Round(Math.Sqrt( dist), 1));
}
//eyelid check
if (index == 866 || index == 868)
{
DepthSpacePoint eyelid1 = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(vertices[866]);
DepthSpacePoint eyelid2 = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(vertices[868]);
double dx = eyelid1.X - eyelid2.X;
double dy = eyelid1.Y - eyelid2.Y;
double dist = dx * dx + dy * dy;
// Debug.WriteLine("eyelidopen " + Math.Round(Math.Sqrt(dist), 1));
}
//nose wrinkler
if (index == 673 || index == 24)
{
DepthSpacePoint nose1 = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(vertices[11]);
DepthSpacePoint nose2 = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(vertices[1]);
double dx = nose1.X - nose2.X;
double dy = nose1.Y - nose2.Y;
double dist = dx * dx + dy * dy;
// Debug.WriteLine("side nose distance " + Math.Round(Math.Sqrt(dist), 1));
}
//lip level raise
if (index == 309 || index == 761)
{
DepthSpacePoint nose1 = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(vertices[309]);
DepthSpacePoint nose2 = _sensor.CoordinateMapper.MapCameraPointToDepthSpace(vertices[761]);
double dx = nose1.X - nose2.X;
double dy = nose1.Y - nose2.Y;
double dist = dx * dx + dy * dy;
// Debug.WriteLine("lip level raise " + Math.Round(Math.Sqrt(dist), 1));
}
if (float.IsInfinity(point.X) || float.IsInfinity(point.Y))
{
return;
}
Ellipse ellipse = _points[index];
Canvas.SetLeft(ellipse, point.X);
Canvas.SetTop(ellipse, point.Y);
}
}
}
public static BitmapSource SliceDepthImageWithRectWithoutPlane(this DepthFrame image, Floor floor, CoordinateMapper coordinateMapper, float planePos,
int min = 20, int max = 1000, int left = 0, int top = 0, int right = 512, int bottom = 424)
{
ushort[] _depthData = new ushort[512 * 424];
image.CopyFrameDataToArray(_depthData);
CameraSpacePoint[] depthMappedToCameraPoints = new CameraSpacePoint[512 * 424];
coordinateMapper.MapDepthFrameToCameraSpace(
_depthData,
depthMappedToCameraPoints);
CameraSpacePoint s;
double dist;
var tmFrameDescription = image.FrameDescription;
int width = tmFrameDescription.Width; //image.Width;
int height = tmFrameDescription.Height; // image.Height;
//var depthFrame = image.Image.Bits;
//short[] rawDepthData = new short[tmFrameDescription.LengthInPixels]; //new short[image.PixelDataLength];
ushort[] rawDepthData = new ushort[tmFrameDescription.LengthInPixels];
//image.CopyPixelDataTo(rawDepthData);
image.CopyFrameDataToArray(rawDepthData);
var pixels = new byte[height * width * 4];
const int BlueIndex = 0;
const int GreenIndex = 1;
const int RedIndex = 2;
int w, h = 0;
for (int depthIndex = 0, colorIndex = 0;
depthIndex < rawDepthData.Length && colorIndex < pixels.Length;
depthIndex++, colorIndex += 4)
{
h = depthIndex / 512;
w = depthIndex % 512;
if (w < left || right < w || h < top || bottom < h)
{
continue;
}
s = depthMappedToCameraPoints[depthIndex];
dist = floor.DistanceFrom(s);
//if (dist > -0.2f)
if (dist > planePos)
{
// Calculate the distance represented by the two depth bytes
//int depth = rawDepthData[depthIndex] >> DepthImageFrame.PlayerIndexBitmaskWidth;
//int depth = rawDepthData[depthIndex] >> image.DepthMinReliableDistance;
int depth = rawDepthData[depthIndex];
// Map the distance to an intesity that can be represented in RGB
var intensity = CalculateIntensityFromDistance(depth);
if (depth > min && depth < max)
{
// Apply the intensity to the color channels
pixels[colorIndex + BlueIndex] = intensity; //blue
pixels[colorIndex + GreenIndex] = intensity; //green
pixels[colorIndex + RedIndex] = intensity; //red
}
}
}
return(BitmapSource.Create(width, height, 96, 96, PixelFormats.Bgr32, null, pixels, width * 4));
}
void bfReader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
if (this.bodies == null)
{
this.bodies = new Body[bodyFrame.BodyCount];
}
// The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
// As long as those body objects are not disposed and not set to null in the array,
// those body objects will be re-used.
bodyFrame.GetAndRefreshBodyData(this.bodies);
}
}
if (this.bodies != null && this.bodies.Length > 0)
{
foreach (Body body in this.bodies)
{
if (body.IsTracked)
{
lblLefthand.Text = "Left Hand : " + GetHandStateString(body.HandLeftState);
lblRighthand.Text = "Right Hand : " + GetHandStateString(body.HandRightState);
//IReadOnlyDictionary<JointType, Joint> joints = body.Joints;
//// convert the joint points to depth (display) space
//Dictionary<JointType, Point> jointPoints = new Dictionary<JointType, Point>();
//foreach (JointType jointType in joints.Keys)
//{
// // sometimes the depth(Z) of an inferred joint may show as negative
// // clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
// CameraSpacePoint position = joints[jointType].Position;
// if (position.Z < 0)
// {
// position.Z = InferredZPositionClamp;
// }
// DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
// jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
//}
Joint rightThumb = body.Joints[JointType.HandTipRight];
Joint rightWrist = body.Joints[JointType.WristRight];
Joint rightShoulder = body.Joints[JointType.ShoulderRight];
Joint rightTb = body.Joints[JointType.ThumbRight];
Joint leftTip = body.Joints[JointType.HandTipLeft];
Joint leftShoulder = body.Joints[JointType.ShoulderLeft];
CameraSpacePoint cspt = rightThumb.Position;
CameraSpacePoint csptW = rightWrist.Position;
CameraSpacePoint csptS = rightShoulder.Position;
CameraSpacePoint csptT = rightTb.Position;
CameraSpacePoint csptLTip = leftTip.Position;
CameraSpacePoint csptLShoulder = leftShoulder.Position;
string rightThumbPosition = "\n Right Thumb : ";
if (rightThumb != null && rightThumb.TrackingState != TrackingState.NotTracked)
{
rightThumbPosition += " tracked/infered.. ";
if (cspt.Z < 0)
{
cspt.Z = 0.1f; //to avoid mapping errors as it may return -ve value for Z some times
}
DepthSpacePoint dspt = mapper.MapCameraPointToDepthSpace(cspt);
DepthSpacePoint dsptW = mapper.MapCameraPointToDepthSpace(csptW);
DepthSpacePoint dsptS = mapper.MapCameraPointToDepthSpace(csptS);
DepthSpacePoint dsptT = mapper.MapCameraPointToDepthSpace(csptT);
DepthSpacePoint dsptLTip = mapper.MapCameraPointToDepthSpace(csptLTip);
DepthSpacePoint dsptLShoulder = mapper.MapCameraPointToDepthSpace(csptLShoulder);
rightThumbPosition += "Depth space -- X : " + dspt.X + " Y : " + dspt.Y + "\n";
// rightThumbPosition += "Depth spaceW -- X : " + dsptW.X + " Y : " + dsptW.Y + "\n";
//srightThumbPosition += "Depth spaceS -- Y : " + dsptS.Y ;
lock (this)
{
if (DoAutoHelp && !IsProcessing && !CanRecord)
{
IsProcessing = true;
CheckAutoHelpCoordinates(dspt, dsptLTip);
IsProcessing = false;
}
}
switch (body.HandLeftState)
{
case HandState.Open:
break;
case HandState.Closed:
break;
case HandState.Lasso:
lock (this)
{
if (CanRecord && !IsProcessing && dsptLShoulder.Y > dsptLTip.Y)
{
IsProcessing = true;
RecordCoordinates(dspt, dsptW, dsptS, dsptT);
IsProcessing = false;
}
}
break;
}
}
else
{
rightThumbPosition += "NOT tracked.. ";
}
lblRighthand.Text += rightThumbPosition;
lblStatus.Text = "Bodies are being tracked...";
switch (body.HandRightState)
{
case HandState.Open:
break;
case HandState.Closed:
break;
case HandState.Lasso:
break;
}
}
}
}
else
{
lblStatus.Text = "No bodies to track..";
}
}
public float dotProduct(CameraSpacePoint point1, CameraSpacePoint point2)
{
return(point1.X * point2.X + point1.Y * point2.Y + point1.Z * point2.Z);
}
/// <summary>
/// Handles the body frame data arriving from the sensor
/// </summary>
/// <param name="sender">object sending the event</param>
/// <param name="e">event arguments</param>
private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
bool dataReceived = false;
using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
if (this.bodies == null)
{
this.bodies = new Body[bodyFrame.BodyCount];
}
// The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
// As long as those body objects are not disposed and not set to null in the array,
// those body objects will be re-used.
bodyFrame.GetAndRefreshBodyData(this.bodies);
dataReceived = true;
}
}
if (dataReceived)
{
using (DrawingContext dc = this.drawingGroup.Open())
{
// Draw a transparent background to set the render size
dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, 1920, 1080));
int penIndex = 0;
foreach (Body body in this.bodies)
{
Pen drawPen = this.bodyColors[penIndex++];
if (body.IsTracked)
{
//this.DrawClippedEdges(body, dc);
IReadOnlyDictionary <JointType, Joint> joints = body.Joints;
// convert the joint points to depth (display) space
Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();
foreach (JointType jointType in joints.Keys)
{
// sometimes the depth(Z) of an inferred joint may show as negative
// clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
CameraSpacePoint position = joints[jointType].Position;
if (position.Z < 0)
{
position.Z = InferredZPositionClamp;
}
DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
jointPoints[jointType] = screenMapper.projectToScreen(position);
}
this.DrawBody(joints, jointPoints, dc, drawPen);
//this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
//this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
BitmapImage head = new BitmapImage(new Uri(@"C:/skeleton-head.png"));
//this.DrawImage(jointPoints[JointType.Head], jointPoints[JointType.Neck], head, dc); //draw head
}
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, 1920, 1080));
}
}
}
//public float AngleBetweenTwoVectors(Vector3 b1, Vector3 b2)
//{
// AngleBetweenTwoVectors(Vector3.Normalize(b1), Vector3.Normlize(b2).ToString());
// float dotProduct = 0.0f;
// dotProduct = Vector3.Dot(b1, b2);
// double Out = Math.Acos(dotProduct);
// return (float)Math.Acos(dotProduct);
//}
public double AngleThreePoints(CameraSpacePoint point1, CameraSpacePoint point2, CameraSpacePoint point3)
{
CameraSpacePoint vect1 = subVector(point2, point3);
CameraSpacePoint vect2 = subVector(point1, point2);
double angle = Math.Round((180 - (Math.Acos((dotProduct(vect1, vect2)) / ((vectMag(vect1) * vectMag(vect2)))) * (180 / Math.PI))));
return(angle);
}
public static IPoint3D Point3DFromCameraSpacePoint(CameraSpacePoint csp)
{
return(new Vector3D(csp.X, csp.Y, csp.Z));
}
/// <summary>
/// Draws a body
/// </summary>
/// <param name="joints">joints to draw</param>
/// <param name="jointPoints">translated positions of joints to draw</param>
/// <param name="drawingContext">drawing context to draw to</param>
/// <param name="drawingPen">specifies color to draw a specific body</param>
private void DrawBody(IReadOnlyDictionary <JointType, Joint> joints, IDictionary <JointType, Point> jointPoints, DrawingContext drawingContext, Pen drawingPen)
{
CameraSpacePoint point1 = new CameraSpacePoint();
CameraSpacePoint point2 = new CameraSpacePoint();
CameraSpacePoint point3 = new CameraSpacePoint();
Boolean point1init = false;
Boolean point2init = false;
Boolean point3init = false;
double angle = 0;
double check = 0;
int i = 0;
// Draw the bones
Thread t = new Thread(new ThreadStart(ThreadProc));
t.Start();
foreach (var bone in this.bones)
{
//joints[16 or 17 or 18].position -> returns x, y, z coordinates
this.DrawBone(joints, jointPoints, bone.Item1, bone.Item2, drawingContext, drawingPen);
//Debug.WriteLine("Joint " + bone.Item1 + " : " + joints[bone.Item1].Position.X);
if (bone.Item1 == JointType.KneeRight)
{
point2 = joints[bone.Item1].Position;
point1 = joints[bone.Item2].Position;
point1init = true;
point2init = true;
}
if (bone.Item1 == JointType.HipRight)
{
point3 = joints[bone.Item1].Position;
point3init = true;
}
if (point1init && point2init && point3init)
{
angle = AngleThreePoints(point1, point2, point3);
//check array for 2 length
//if not, only push
//if so, first subtract numbers and check
//then delete first element
//then push
//if subtract is less than 0 counter++;
angleOld = angleNew;
angleNew = angle;
//Debug.WriteLine("ANGLE: " + angle);
//Debug.WriteLine("STATE: " + state);
if (angle < 90)
{
check = angleOld - angleNew;
//Debug.WriteLine(check + "a" );
//Debug.WriteLine(angles[0] + "angle 0");
//Debug.WriteLine(angles[1] + "angle 1");
if (!state)
{
counter++;
state = true;
Debug.WriteLine(counter);
//Debug.WriteLine("");
//Debug.WriteLine("COUNT: " + counter);
//Debug.WriteLine("old angle: " + angleOld);
//Debug.WriteLine("new angle: " + angleNew);
//Debug.WriteLine("angle: " + angle);
//Debug.WriteLine("state: " + state);
//Debug.WriteLine("check: " + check);
if (t.IsAlive)
{
t.Abort();
}
t = new Thread(new ThreadStart(ThreadProc));
t.Start();
}
}
else if (angle > 95)
{
state = false;
//Debug.WriteLine(" RESET");
}
point1init = false;
point2init = false;
point3init = false;
}
}
// Draw the joints
foreach (JointType jointType in joints.Keys)
{
Brush drawBrush = null;
TrackingState trackingState = joints[jointType].TrackingState;
if (trackingState == TrackingState.Tracked)
{
drawBrush = this.trackedJointBrush;
}
else if (trackingState == TrackingState.Inferred)
{
drawBrush = this.inferredJointBrush;
}
if (drawBrush != null)
{
drawingContext.DrawEllipse(drawBrush, null, jointPoints[jointType], JointThickness, JointThickness);
}
}
}
/// <summary>
/// Handles the body frame data arriving from the sensor
/// </summary>
/// <param name="sender">object sending the event</param>
/// <param name="e">event arguments</param>
private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
bool dataReceived = false;
using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
if (this.bodies == null)
{
this.bodies = new Body[bodyFrame.BodyCount];
}
// The first time GetAndRefreshBodyData is called, Kinect will allocate each Body in the array.
// As long as those body objects are not disposed and not set to null in the array,
// those body objects will be re-used.
bodyFrame.GetAndRefreshBodyData(this.bodies);
dataReceived = true;
}
}
//if there are bodies:
if (dataReceived)
{
//recieve boxes
using (DrawingContext dc = this.drawingGroup.Open())
{
// Draw a transparent background to set the render size
dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
int penIndex = 0;
foreach (Body body in this.bodies)
{
Pen drawPen = this.bodyColors[penIndex++];
if (body.IsTracked)
{
this.DrawClippedEdges(body, dc);
IReadOnlyDictionary <JointType, Joint> joints = body.Joints;
// convert the joint points to depth (display) space
Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();
foreach (JointType jointType in joints.Keys)
{
// sometimes the depth(Z) of an inferred joint may show as negative
// clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
CameraSpacePoint position = joints[jointType].Position;
if (position.Z < 0)
{
position.Z = InferredZPositionClamp;
}
DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
}
this.DrawBody(joints, jointPoints, dc, drawPen);
///hand pos!!
Point leftHandPos = jointPoints[JointType.HandLeft];
Point rightHandPos = jointPoints[JointType.HandRight];
HandState leftHand = body.HandLeftState;
HandState rightHand = body.HandRightState;
this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
// add boxes
SolidColorBrush color = Brushes.Red;
//draw test box
foreach (var label in this.Labels)
{
DrawBox(label.Box[0], label.Box[1], label.Box[2], label.Box[3], dc, color);
if (HandInBox(label.Box, leftHandPos, leftHand, rightHandPos, rightHand))
{
color = Brushes.Green;
Debug.WriteLine(label.Name);
}
else
{
color = Brushes.Red;
}
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
}
}
}
}
}
private double AccelBetPoints(CameraSpacePoint P1, CameraSpacePoint P2, CameraSpacePoint P3)
{
return((SpeedBetPoints(P2, P3) - SpeedBetPoints(P1, P2)) / 0.0333333333);
}
float CSVectorLength(CameraSpacePoint p)
{
return(Convert.ToSingle(Math.Sqrt(p.X * p.X + p.Y * p.Y + p.Z * p.Z)));
}
private double SpeedBetPoints(CameraSpacePoint P1, CameraSpacePoint P2)
{
return(DistBetPoints(P1, P2) / 0.0333333333);
}
public HandsState(Body body)
{
CameraSpacePoint handLeft = body.Joints[JointType.HandLeft].Position;
CameraSpacePoint handRight = body.Joints[JointType.HandRight].Position;
wristLeft = body.Joints[JointType.WristLeft].Position;
wristRight = body.Joints[JointType.WristRight].Position;
CameraSpacePoint spineBase = body.Joints[JointType.SpineBase].Position;
input = new HandInput();
input.isLeftGrip = (body.HandLeftState == HandState.Closed);
input.isRightGrip = (body.HandRightState == HandState.Closed);
//select wrist
if (prime_hand)
{
primeHandy = (int)(wristRight.Y * 100);
isRight = true;
}
else
{
primeHandy = (int)(wristLeft.Y * 100);
isRight = false;
}
//set left hand position
float leftDepth = spineBase.Z - handLeft.Z;
input._LPressExtent = (leftDepth - TOUCH_REGION) / (EMBED_REGION - TOUCH_REGION);
if (leftDepth > EMBED_REGION)
{
LeftHandPosition = HandPositionZ.EMBED;
}
else if (leftDepth > TOUCH_REGION)
{
LeftHandPosition = HandPositionZ.TOUCH;
}
else
{
LeftHandPosition = HandPositionZ.UNKNOW;
}
//set right hand position
float rightDepth = spineBase.Z - handRight.Z;
input._RPressExtent = (rightDepth - TOUCH_REGION) / (EMBED_REGION - TOUCH_REGION);
if (rightDepth > EMBED_REGION)
{
RightHandPosition = HandPositionZ.EMBED;
}
else if (rightDepth > TOUCH_REGION)
{
RightHandPosition = HandPositionZ.TOUCH;
}
else
{
RightHandPosition = HandPositionZ.UNKNOW;
}
//set left hand state
LeftHandState = body.HandLeftState;
//set right hand state
RightHandState = body.HandRightState;
//no hand
if (LeftHandPosition == HandPositionZ.UNKNOW && RightHandPosition == HandPositionZ.UNKNOW)
{
operation = Operation.no_operation;
input._isWhich = 0;
}
//single hand
else if (LeftHandPosition == HandPositionZ.UNKNOW || RightHandPosition == HandPositionZ.UNKNOW)
{
//left hand operate
if (LeftHandPosition != HandPositionZ.UNKNOW)
{
SelectHandPosition = LeftHandPosition;
SelectHandState = LeftHandState;
input._isWhich = 1;
isRight = false;
}
//right hand operate
else
{
SelectHandPosition = RightHandPosition;
SelectHandState = RightHandState;
input._isWhich = 2;
isRight = true;
}
//single hand touch region
if (SelectHandPosition == HandPositionZ.TOUCH)
{
if (SelectHandState == HandState.Closed)
{
if (mouse_click_region)
{
operation = Operation.left_down;
}
else
{
operation = Operation.right_down;
}
}
else
{
operation = Operation.move;
}
}
//single hand embed region
else
{
if (SelectHandState == HandState.Closed)
{
if (mouse_click_region)
{
operation = Operation.right_down;
}
else
{
operation = Operation.left_down;
}
}
else
{
operation = Operation.move;
}
}
}
else
{
//two hand closed will operate wheel
input._isWhich = 3;
if (LeftHandState == HandState.Closed && RightHandState == HandState.Closed)
{
if (middle_button_and_wheel)
{
operation = Operation.wheel;
}
else
{
operation = Operation.middle_down;
}
}
//one hand closed
else if (LeftHandState == HandState.Closed || RightHandState == HandState.Closed)
{
if (middle_button_and_wheel)
{
operation = Operation.middle_down;
}
else
{
operation = Operation.wheel;
}
}
else
{
operation = Operation.move;
}
}
}
private void MultiSourceFrameReader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
bool dataReceived = false;
MultiSourceFrame multiSourceFrame = e.FrameReference.AcquireFrame();
using (ColorFrame colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame())
{
if (colorFrame != null)
{
using (KinectBuffer colorBuffer = colorFrame.LockRawImageBuffer())
{
colorBitmap.Lock();
if ((colorFrameDescription.Width == colorBitmap.PixelWidth) && (colorFrameDescription.Height == colorBitmap.PixelHeight))
{
colorFrame.CopyConvertedFrameDataToIntPtr(
colorBitmap.BackBuffer,
(uint)(colorFrameDescription.Width * colorFrameDescription.Height * 4),
ColorImageFormat.Bgra);
colorBitmap.AddDirtyRect(new Int32Rect(0, 0, colorBitmap.PixelWidth, colorBitmap.PixelHeight));
}
colorBitmap.Unlock();
}
}
}
using (BodyFrame bodyFrame = multiSourceFrame.BodyFrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
if (bodies == null)
{
bodies = new Body[bodyFrame.BodyCount];
}
bodyFrame.GetAndRefreshBodyData(bodies);
dataReceived = true;
}
}
if (dataReceived)
{
canvas.Children.Clear();
foreach (Body body in bodies.Where(b => b.IsTracked))
{
int colorIndex = 0;
foreach (var joint in body.Joints)
{
SolidColorBrush colorBrush = bodyColors[colorIndex++];
Dictionary <JointType, Point> jointColorPoints = new Dictionary <JointType, Point>();
CameraSpacePoint position = joint.Value.Position;
if (position.Z < 0)
{
position.Z = 0.1f;
}
ColorSpacePoint colorSpacePoint = coordinateMapper.MapCameraPointToColorSpace(position);
jointColorPoints[joint.Key] = new Point(colorSpacePoint.X, colorSpacePoint.Y);
if (joint.Value.TrackingState == TrackingState.Tracked)
{
DrawJoint(new Point(colorSpacePoint.X, colorSpacePoint.Y), new SolidColorBrush(Colors.Purple));
}
if (joint.Value.TrackingState == TrackingState.Inferred)
{
DrawJoint(new Point(colorSpacePoint.X, colorSpacePoint.Y), new SolidColorBrush(Colors.LightGray));
}
foreach (var bone in bones)
{
DrawBone(body.Joints, jointColorPoints, bone.Item1, bone.Item2, colorBrush);
}
DrawClippedEdges(body);
DrawHandStates(body.HandRightState, jointColorPoints[JointType.HandRight]);
DrawHandStates(body.HandLeftState, jointColorPoints[JointType.HandLeft]);
}
}
}
}
