public Body Select(BodyFrame frame)
{
if (frame == null || frame.BodyCount == 0)
{
this.trackingId = 0;
return null;
}
var bodies = new Body[frame.BodyCount];
frame.GetAndRefreshBodyData(bodies);
var trackedBody = bodies.FirstOrDefault(b =>
b.TrackingId == this.trackingId &&
b.Joints[JointType.HandLeft].Position.Y > b.Joints[JointType.HipLeft].Position.Y + 0.05);
if (trackedBody != null)
{
return trackedBody;
}
var activeBody = bodies.FirstOrDefault(b =>
b.IsTracked &&
b.HandLeftState == HandState.Open &&
b.Joints[JointType.HandLeft].Position.Y > b.Joints[JointType.ShoulderLeft].Position.Y);
if (activeBody != null)
{
this.trackingId = activeBody.TrackingId;
return activeBody;
}
this.trackingId = 0;
return null;
}
private void RegisterGesture(BodyFrame bodyFrame)
{
bool dataReceived = false;
Body[] bodies = null;
if (bodyFrame != null)
{
if (bodies == null)
{
// Creates an array of 6 bodies, which is the max number of bodies that Kinect can track simultaneously
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(bodies);
dataReceived = true;
}
if (dataReceived)
{
// We may have lost/acquired bodies, so update the corresponding gesture detectors
if (bodies != null)
{
// Loop through all bodies to see if any of the gesture detectors need to be updated
for (int i = 0; i < bodyFrame.BodyCount; ++i)
{
Body body = bodies[i];
ulong trackingId = body.TrackingId;
// If the current body TrackingId changed, update the corresponding gesture detector with the new value
if (trackingId != this.gestureDetectorList[i].TrackingId)
{
this.gestureDetectorList[i].TrackingId = trackingId;
// If the current body is tracked, unpause its detector to get VisualGestureBuilderFrameArrived events
// If the current body is not tracked, pause its detector so we don't waste resources trying to get invalid gesture results
this.gestureDetectorList[i].IsPaused = trackingId == 0;
}
}
}
}
}
private static Body FindClosestBody(BodyFrame bodyFrame) // Finds the closest body from the sensor if any
{
Body result = null;
double closestBodyDistance = double.MaxValue;
Body[] bodies = new Body[bodyFrame.BodyCount];
bodyFrame.GetAndRefreshBodyData(bodies);
foreach (var body in bodies)
{
if (body.IsTracked)
{
var currentLocation = body.Joints[JointType.SpineBase].Position;
var currentDistance = VectorLength(currentLocation);
if (result == null || currentDistance < closestBodyDistance)
{
result = body;
closestBodyDistance = currentDistance;
}
}
}
return result;
}
private void Reader_MultiSourceFrameArrived(MultiSourceFrameReader sender, MultiSourceFrameArrivedEventArgs e)
{
MultiSourceFrame multiSourceFrame = e.FrameReference.AcquireFrame();
// If the Frame has expired by the time we process this event, return.
if (multiSourceFrame == null)
{
return;
}
DepthFrame depthFrame = null;
ColorFrame colorFrame = null;
InfraredFrame infraredFrame = null;
BodyIndexFrame bodyIndexFrame = null;
BodyFrame bodyFrame = null;
//windows.storage.streams
IBuffer depthFrameDataBuffer = null;
IBuffer bodyIndexFrameData = null;
IBufferByteAccess bodyIndexByteAccess = null; // Com interface for unsafe byte manipulation
switch (currentDisplayFrameType)
{
case DisplayFrameType.Infrared:
using (infraredFrame = multiSourceFrame.InfraredFrameReference.AcquireFrame()) {
ShowInfraredFrame(infraredFrame);
}
break;
case DisplayFrameType.Color:
using (colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame()) {
ShowColorFrame(colorFrame);
}
break;
case DisplayFrameType.Depth:
using (depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame()) {
ShowDepthFrame(depthFrame);
}
break;
case DisplayFrameType.BodyMask:
// Put in a try catch to utilise finally() and clean up frames
try {
depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame();
bodyIndexFrame = multiSourceFrame.BodyIndexFrameReference.AcquireFrame();
colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame();
if ((depthFrame == null) || (colorFrame == null) || (bodyIndexFrame == null))
{
return;
}
// Access the depth frame data directly via LockImageBuffer to avoid making a copy
depthFrameDataBuffer = depthFrame.LockImageBuffer();
this.coordinateMapper.MapColorFrameToDepthSpaceUsingIBuffer(depthFrameDataBuffer, this.colorMappedToDepthPoints);
// Process Color
colorFrame.CopyConvertedFrameDataToBuffer(this.bitmap.PixelBuffer, ColorImageFormat.Bgra);
// Access the body index frame data directly via LockImageBuffer to avoid making a copy
bodyIndexFrameData = bodyIndexFrame.LockImageBuffer();
ShowMappedBodyFrame(depthFrame.FrameDescription.Width, depthFrame.FrameDescription.Height, bodyIndexFrameData, bodyIndexByteAccess);
} finally {
if (depthFrame != null)
{
depthFrame.Dispose();
}
if (colorFrame != null)
{
colorFrame.Dispose();
}
if (bodyIndexFrame != null)
{
bodyIndexFrame.Dispose();
}
if (depthFrameDataBuffer != null)
{
// We must force a release of the IBuffer in order toensure that we have dropped all references to it.
System.Runtime.InteropServices.Marshal.ReleaseComObject(depthFrameDataBuffer);
}
if (bodyIndexFrameData != null)
{
System.Runtime.InteropServices.Marshal.ReleaseComObject(bodyIndexFrameData);
}
if (bodyIndexByteAccess != null)
{
System.Runtime.InteropServices.Marshal.ReleaseComObject(bodyIndexByteAccess);
}
}
break;
case DisplayFrameType.BodyJoints:
using (bodyFrame = multiSourceFrame.BodyFrameReference.AcquireFrame()) {
ShowBodyJoints(bodyFrame);
}
break;
default:
break;
}
}
private void ShowBody( BodyFrame bodyFrame )
{
frameCount++;
if ( sw.ElapsedMilliseconds >= 1000 ) {
scratch.AddSensorValue( "FrameCount", frameCount.ToString() );
sw.Restart();
frameCount = 0;
}
var mapper = kinect.CoordinateMapper;
CanvasBody.Children.Clear();
bodyFrame.GetAndRefreshBodyData( bodies );
var trackedBody = bodies.FirstOrDefault( b => b.IsTracked );
if ( trackedBody != null ) {
var body = trackedBody;
foreach ( var jointType in body.Joints.Keys ) {
var joint = body.Joints[jointType];
if ( joint.TrackingState != TrackingState.NotTracked ) {
ShowJoint( mapper, joint );
int scale = 200;
AddSensorValue( jointType, "X", (int)(joint.Position.X * scale) );
AddSensorValue( jointType, "Y", (int)(joint.Position.Y * scale) );
}
}
if ( body.HandLeftConfidence == TrackingConfidence.High ) {
AddSensorValue( "HandLeftState", body.HandLeftState.ToString() );
}
else {
AddSensorValue( "HandLeftState", HandState.Unknown.ToString() );
}
if ( body.HandRightConfidence == TrackingConfidence.High ) {
AddSensorValue( "HandRightState", body.HandRightState.ToString() );
}
else {
AddSensorValue( "HandRightState", HandState.Unknown.ToString() );
}
}
else {
foreach ( JointType jointType in Enum.GetValues(typeof(JointType) ) ) {
AddSensorValue( jointType, "X", 0 );
AddSensorValue( jointType, "Y", 0 );
}
AddSensorValue( "HandLeftState", HandState.Unknown.ToString() );
AddSensorValue( "HandRightState", HandState.Unknown.ToString() );
}
scratch.UpdateSensor();
}
/// <summary>
/// Find if there is a body tracked with the given trackingId
/// </summary>
/// <param name="bodyFrame">A body frame</param>
/// <param name="trackingId">The tracking Id</param>
/// <returns>The body object, null of none</returns>
private static Body FindBodyWithTrackingId(BodyFrame bodyFrame, ulong trackingId)
{
Body result = null;
Body[] bodies = new Body[bodyFrame.BodyCount];
bodyFrame.GetAndRefreshBodyData(bodies);
foreach (var body in bodies)
{
if (body.IsTracked)
{
if (body.TrackingId == trackingId)
{
result = body;
break;
}
}
}
return result;
}
private void ProcessFrames(DepthFrame depthFrame, ColorFrame colorFrame, BodyIndexFrame bodyIndexFrame, BodyFrame bodyFrame)
{
FrameDescription depthFrameDescription = depthFrame.FrameDescription;
FrameDescription colorFrameDescription = colorFrame.FrameDescription;
FrameDescription bodyIndexFrameDescription = bodyIndexFrame.FrameDescription;
int bodyIndexWidth = bodyIndexFrameDescription.Width;
int bodyIndexHeight = bodyIndexFrameDescription.Height;
// The ImageModel object is used to transfer Kinect data into the DataFlow rotunies.
ImageModel imageModel = new ImageModel()
{
DepthWidth = depthFrameDescription.Width,
DepthHeight = depthFrameDescription.Height,
ColorWidth = colorFrameDescription.Width,
ColorHeight = colorFrameDescription.Height,
ShowTrails = _vm.LeaveTrails,
PersonFill = _vm.PersonFill,
MaxDistance = _vm.BackgroundDistance
};
imageModel.ColorFrameData = new byte[imageModel.ColorWidth * imageModel.ColorHeight * this.bytesPerPixel];
imageModel.DisplayPixels = new byte[_PreviousFrameDisplayPixels.Length];
imageModel.BodyIndexFrameData = new byte[imageModel.DepthWidth * imageModel.DepthHeight];
imageModel.ColorPoints = new ColorSpacePoint[imageModel.DepthWidth * imageModel.DepthHeight];
imageModel.BytesPerPixel = bytesPerPixel;
imageModel.Bodies = new Body[this.kinectSensor.BodyFrameSource.BodyCount];
bodyFrame.GetAndRefreshBodyData(imageModel.Bodies);
imageModel.DepthData = new ushort[imageModel.DepthWidth * imageModel.DepthHeight];
depthFrame.CopyFrameDataToArray(imageModel.DepthData);
depthFrame.CopyFrameDataToArray(this.DepthFrameData);
if (colorFrame.RawColorImageFormat == ColorImageFormat.Bgra)
{
colorFrame.CopyRawFrameDataToArray(imageModel.ColorFrameData);
}
else
{
colorFrame.CopyConvertedFrameDataToArray(imageModel.ColorFrameData, ColorImageFormat.Bgra);
}
imageModel.PixelFormat = PixelFormats.Bgra32;
_ColorBitmap.WritePixels(new Int32Rect(0, 0, imageModel.ColorWidth, imageModel.ColorHeight),
imageModel.ColorFrameData,
imageModel.ColorWidth * imageModel.BytesPerPixel,
0);
//RenderTargetBitmap renderBitmap = new RenderTargetBitmap((int)CompositeImage.ActualWidth, (int)CompositeImage.ActualHeight, 96.0, 96.0, PixelFormats.Pbgra32);
//DrawingVisual dv = new DrawingVisual();
//VisualBrush brush = new VisualBrush(CompositeImage);
//foreach(Body body in _bodies)
//{
// if (body.IsTracked)
// {
// Joint joint = body.Joints[JointType.HandRight];
// using (DrawingContext dc = dv.RenderOpen())
// {
// dc.DrawRectangle(brush, null, new Rect(new Point(), new Size(CompositeImage.ActualWidth, CompositeImage.ActualHeight)));
// ImageBrush brush2 = new ImageBrush(_pointerBitmap);
// brush2.Opacity = 1.0;
// dc.DrawRectangle(brush2, null, new Rect(new Point(0, CompositeImage.ActualHeight - _Overlay.Height), new Size(_pointerBitmap.Width, _pointerBitmap.Height)));
// }
// }
//}
//ConvertIRDataToByte();
ImagePreview.Source = _ColorBitmap;
bodyIndexFrame.CopyFrameDataToArray(imageModel.BodyIndexFrameData);
this.coordinateMapper.MapDepthFrameToColorSpace(DepthFrameData, imageModel.ColorPoints);
if (_vm.LeaveTrails)
{
Array.Copy(this._PreviousFrameDisplayPixels, imageModel.DisplayPixels, this._PreviousFrameDisplayPixels.Length);
}
try
{
//Send the imageModel to the DataFlow transformer
_ImageTransformer.Post(imageModel);
}
catch (Exception ex)
{
#if DEBUG
Console.WriteLine(ex);
#endif
}
}
private void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
var reference = e.FrameReference;
MultiSourceFrame multiSourceFrame = null;
ColorFrame colorFrame = null;
DepthFrame depthFrame = null;
BodyFrame bodyFrame = null;
BodyIndexFrame bodyIndexFrame = null;
try
{
using (_frameCounter.Increment())
{
multiSourceFrame = reference.AcquireFrame();
if (multiSourceFrame == null)
{
return;
}
colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame();
depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame();
bodyFrame = multiSourceFrame.BodyFrameReference.AcquireFrame();
bodyIndexFrame = multiSourceFrame.BodyIndexFrameReference.AcquireFrame();
if (colorFrame == null | depthFrame == null | bodyFrame == null | bodyIndexFrame == null)
{
return;
}
var colorDesc = colorFrame.FrameDescription;
int colorWidth = colorDesc.Width;
int colorHeight = colorDesc.Height;
if (_colorFrameData == null)
{
int size = colorDesc.Width * colorDesc.Height;
_colorFrameData = new byte[size * bytesPerPixel];
_displayFrame = new byte[size * bytesPerPixel];
}
var depthDesc = depthFrame.FrameDescription;
uint depthSize = depthDesc.LengthInPixels;
_depthFrameData = new ushort[depthSize];
_colorSpacePoints = new ColorSpacePoint[depthSize];
FrameDescription bodyIndexFrameDescription = bodyIndexFrame.FrameDescription;
int bodyIndexWidth = bodyIndexFrameDescription.Width;
int bodyIndexHeight = bodyIndexFrameDescription.Height;
if ((bodyIndexWidth * bodyIndexHeight) == bodyIndexFrameData.Length)
{
bodyIndexFrame.CopyFrameDataToArray(bodyIndexFrameData);
}
Array.Clear(_displayFrame, 0, _displayFrame.Length);
colorFrame.CopyConvertedFrameDataToArray(_colorFrameData, ColorImageFormat.Bgra);
depthFrame.CopyFrameDataToArray(_depthFrameData);
kinectSensor.CoordinateMapper.MapDepthFrameToColorSpace(_depthFrameData, _colorSpacePoints);
kinectSensor.CoordinateMapper.MapDepthFrameToCameraSpace(_depthFrameData, _cameraPoints);
for (int depthIndex = 0; depthIndex < _depthFrameData.Length; ++depthIndex)
{
byte player = bodyIndexFrameData[depthIndex];
bool?c = OnlyPlayersMenuItem.IsChecked;
bool val = c != null ? (bool)c : false;
if (!val || player != 0xff)
{
ColorSpacePoint point = _colorSpacePoints[depthIndex];
CameraSpacePoint p = this._cameraPoints[depthIndex];
int colorX = (int)Math.Floor(point.X + 0.5);
int colorY = (int)Math.Floor(point.Y + 0.5);
int colorImageIndex = ((colorWidth * colorY) + colorX) * bytesPerPixel;
if ((colorX >= 0) && (colorX < colorWidth) && (colorY >= 0) && (colorY < colorHeight))
{
if (p.Z > 0)
{
_displayFrame[colorImageIndex] = _colorFrameData[colorImageIndex]; // b
_displayFrame[colorImageIndex + 1] = _colorFrameData[colorImageIndex + 1]; // g
_displayFrame[colorImageIndex + 2] = _colorFrameData[colorImageIndex + 2]; // r
_displayFrame[colorImageIndex + 3] = _colorFrameData[colorImageIndex + 3]; // a
}
}
}
}
colorBitmap.WritePixels(
new Int32Rect(0, 0, colorDesc.Width, colorDesc.Height),
_displayFrame,
//_colorFrameData,
colorDesc.Width * bytesPerPixel,
0);
if (calibratingSurface)
{
if (_pointsToDepth.Count > 0)
{
foreach (Point p in _pointsToDepth)
{
int depthIndex = Convert.ToInt32(p.Y) * depthDesc.Width + Convert.ToInt32(p.X);
try
{
CameraSpacePoint cameraPoint = _cameraPoints[depthIndex];
if (!(Double.IsInfinity(cameraPoint.X)) && !(Double.IsInfinity(cameraPoint.Y)) && !(Double.IsInfinity(cameraPoint.Z) && cameraPoint.Z > 0))
{
Console.WriteLine("" + p.X + " " + p.Y + " ---> " + cameraPoint.X + " " + cameraPoint.Y + " " + cameraPoint.Z);
_calibrationPoints.Add(cameraPoint);
drawEllipse(p.X, p.Y);
}
}
catch { }
}
_pointsToDepth = new List <Point>();
}
if (false && _calibrationPoints.Count == 3)
{
canvas.Children.Clear();
CameraSpacePoint a = VectorTools.subPoint(_calibrationPoints[0], _calibrationPoints[1]);
CameraSpacePoint b = VectorTools.subPoint(_calibrationPoints[2], _calibrationPoints[1]);
CameraSpacePoint up = VectorTools.cross(a, b);
CameraSpacePoint c1 = VectorTools.cross(b, up);
CameraSpacePoint c2 = VectorTools.mult(c1, -1f);
CameraSpacePoint c;
if (VectorTools.distance(_calibrationPoints[2], VectorTools.addPoint(_calibrationPoints[1], c1)) < VectorTools.distance(_calibrationPoints[2], VectorTools.addPoint(_calibrationPoints[1], c2)))
{
c = VectorTools.mult(VectorTools.normalize(c1), 9.0f / 16.0f * VectorTools.norm(a) /*norm(b)*/);
}
else
{
c = VectorTools.mult(VectorTools.normalize(c2), 9.0f / 16.0f * VectorTools.norm(a) /*norm(b)*/);
}
CameraSpacePoint BL = _calibrationPoints[0];
CameraSpacePoint BR = _calibrationPoints[1];
CameraSpacePoint TR = VectorTools.addPoint(BR, c);
CameraSpacePoint TL = VectorTools.addPoint(BL, c);
VectorTools.DebugPoint(BL);
VectorTools.DebugPoint(BR);
VectorTools.DebugPoint(TR);
VectorTools.DebugPoint(TL);
//_drawSurface(coordinateMapper.MapCameraPointToColorSpace(BL),
// coordinateMapper.MapCameraPointToColorSpace(BR),
// coordinateMapper.MapCameraPointToColorSpace(TR),
// coordinateMapper.MapCameraPointToColorSpace(TL));
_calibrationPoints.Clear();
calibratingSurface = false;
}
}
}
}
catch (Exception ex)
{
Console.WriteLine(ex.Message);
Console.WriteLine(ex.StackTrace);
Console.WriteLine(ex.Source);
}
finally
{
if (colorFrame != null)
{
colorFrame.Dispose();
}
if (depthFrame != null)
{
depthFrame.Dispose();
}
if (bodyFrame != null)
{
bodyFrame.Dispose();
}
if (bodyIndexFrame != null)
{
bodyIndexFrame.Dispose();
}
}
}
/// <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 (kinect_start != 0)
{
foreach (Body body in bodies)
{
if (body.IsTracked == true)
{
DateTime datenow = DateTime.Now;
int hour = datenow.Hour;
int minute = datenow.Minute;
int second = datenow.Second;
int millisecond = datenow.Millisecond;
int timestamp = hour * 3600 * 1000 + minute * 60 * 1000 + second * 1000 + millisecond;
// sw1.WriteLine(skelet.Joints[JointType.ShoulderRight].Position.X + " " + skelet.Joints[JointType.ShoulderRight].Position.Y + " " + skelet.Joints[JointType.ShoulderRight].Position.Z + " " + skelet.Joints[JointType.ShoulderRight].TrackingState + " " + timestamp);
// sw1.WriteLine(skelet.Joints[JointType.ElbowRight].Position.X + " " + skelet.Joints[JointType.ElbowRight].Position.Y + " " + skelet.Joints[JointType.ElbowRight].Position.Z + " " + skelet.Joints[JointType.ElbowRight].TrackingState + " " + timestamp);
sw1.WriteLine(body.Joints[JointType.WristRight].Position.X + " " + body.Joints[JointType.WristRight].Position.Y + " " + body.Joints[JointType.WristRight].Position.Z + " " + body.Joints[JointType.WristRight].TrackingState + " " + timestamp);
// sw1.WriteLine(skelet.Joints[JointType.ShoulderLeft].Position.X + " " + skelet.Joints[JointType.ShoulderLeft].Position.Y + " " + skelet.Joints[JointType.ShoulderLeft].Position.Z + " " + skelet.Joints[JointType.ShoulderLeft].TrackingState + " " + timestamp);
// sw1.WriteLine(skelet.Joints[JointType.ElbowLeft].Position.X + " " + skelet.Joints[JointType.ElbowLeft].Position.Y + " " + skelet.Joints[JointType.ElbowLeft].Position.Z + " " + skelet.Joints[JointType.ElbowLeft].TrackingState + " " + timestamp);
sw1.WriteLine(body.Joints[JointType.WristLeft].Position.X + " " + body.Joints[JointType.WristLeft].Position.Y + " " + body.Joints[JointType.WristLeft].Position.Z + " " + body.Joints[JointType.WristLeft].TrackingState + " " + timestamp);
// sw1.WriteLine(skelet.Joints[JointType.HipRight].Position.X + " " + skelet.Joints[JointType.HipRight].Position.Y + " " + skelet.Joints[JointType.HipRight].Position.Z + " " + skelet.Joints[JointType.HipRight].TrackingState + " " + timestamp);
// sw1.WriteLine(skelet.Joints[JointType.KneeRight].Position.X + " " + skelet.Joints[JointType.KneeRight].Position.Y + " " + skelet.Joints[JointType.KneeRight].Position.Z + " " + skelet.Joints[JointType.KneeRight].TrackingState + " " + timestamp);
//sw1.WriteLine(skelet.Joints[JointType.FootRight].Position.X + " " + skelet.Joints[JointType.FootRight].Position.Y + " " + skelet.Joints[JointType.FootRight].Position.Z + " " + skelet.Joints[JointType.FootRight].TrackingState + " " + timestamp);
//sw1.WriteLine(skelet.Joints[JointType.HipLeft].Position.X + " " + skelet.Joints[JointType.HipLeft].Position.Y + " " + skelet.Joints[JointType.HipLeft].Position.Z + " " + skelet.Joints[JointType.HipLeft].TrackingState + " " + timestamp);
// sw1.WriteLine(skelet.Joints[JointType.KneeLeft].Position.X + " " + skelet.Joints[JointType.KneeLeft].Position.Y + " " + skelet.Joints[JointType.KneeLeft].Position.Z + " " + skelet.Joints[JointType.KneeLeft].TrackingState + " " + timestamp);
//sw1.WriteLine(skelet.Joints[JointType.FootLeft].Position.X + " " + skelet.Joints[JointType.FootLeft].Position.Y + " " + skelet.Joints[JointType.FootLeft].Position.Z + " " + skelet.Joints[JointType.FootLeft].TrackingState + " " + timestamp);
}
}
}
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);
}
this.DrawBody(joints, jointPoints, dc, drawPen);
this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
this.DrawHand(body.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));
}
}
}
public static PhysicsJoint CreateJoint(GameObject parentBody, GameObject childBody, BasicJointInfo.BasicJointType jointType)
{
var bodyPBounds = parentBody.GetComponent <MeshRenderer>().bounds;
var bodyCBounds = childBody.GetComponent <MeshRenderer>().bounds;
var pointConPWorld = bodyPBounds.ClosestPoint(bodyCBounds.center);
var pointPonCWorld = bodyCBounds.ClosestPoint(bodyPBounds.center);
var bodyPTransform = new RigidTransform(parentBody.transform.rotation, parentBody.transform.position); // was torso
var bodyCTransform = new RigidTransform(childBody.transform.rotation, childBody.transform.position); // was head
PhysicsJoint jointData = default;
switch (jointType)
{
case BasicJointInfo.BasicJointType.BallAndSocket:
{
var pivotP = math.transform(math.inverse(bodyPTransform), pointConPWorld);
var pivotC = math.transform(math.inverse(bodyCTransform), pointConPWorld);
jointData = PhysicsJoint.CreateBallAndSocket(pivotP, pivotC);
}
break;
case BasicJointInfo.BasicJointType.Distance:
{
var pivotP = math.transform(math.inverse(bodyPTransform), pointConPWorld);
var pivotC = math.transform(math.inverse(bodyCTransform), pointPonCWorld);
var range = new FloatRange(0, math.distance(pointConPWorld, pointPonCWorld));
jointData = PhysicsJoint.CreateLimitedDistance(pivotP, pivotC, range);
}
break;
case BasicJointInfo.BasicJointType.Hinge:
{
var commonPivotPointWorld = math.lerp(pointConPWorld, pointPonCWorld, 0.5f);
// assume a vertical hinge joint
var axisP = math.rotate(math.inverse(bodyPTransform.rot), math.up());
var axisC = math.rotate(math.inverse(bodyCTransform.rot), math.up());
float3 perpendicularAxisA, perpendicularAxisB;
Math.CalculatePerpendicularNormalized(axisP, out perpendicularAxisA, out _);
Math.CalculatePerpendicularNormalized(axisC, out perpendicularAxisB, out _);
var pivotP = math.transform(math.inverse(bodyPTransform), commonPivotPointWorld);
var pivotC = math.transform(math.inverse(bodyCTransform), commonPivotPointWorld);
var jointFrameP = new BodyFrame {
Axis = axisP, PerpendicularAxis = perpendicularAxisA, Position = pivotP
};
var jointFrameC = new BodyFrame {
Axis = axisC, PerpendicularAxis = perpendicularAxisB, Position = pivotC
};
var range = new FloatRange(math.radians(-90), math.radians(90.0f));
jointData = PhysicsJoint.CreateLimitedHinge(jointFrameP, jointFrameC, range);
}
break;
default:
break;
}
return(jointData);
}
private void Process(BodyFrame bodyFrame)
{
double currentTime = bodyFrame.RelativeTime.TotalMilliseconds;
// Selects the first body that is tracked and use that for our calculations
Body body = System.Linq.Enumerable.FirstOrDefault(this.bodies, bod => bod.IsTracked);
if (body != null && body.IsTracked)
{
currentBody = body;
drawer.Draw();
// add all joint positions to the records
jointRecords.AddRecordForEachJoint(currentTime);
// Main sound mapping logic
if (!timeSignature.isEstablished)
{
// First, establish the time signature (with specific gestures)
timeSignature.CheckForBeats();
}
else
{
// Then, do general sound-movement mappings
SoundCreator.Create();
}
// update last hand state
lastHandLeftState = currentBody.HandLeftState;
lastHandRightState = currentBody.HandRightState;
}
}
public void OnNewFrame(BodyFrame frame)
{
throw new NotImplementedException();
}
void DoThingsWithBodyFrame(BodyFrame frame)
{
canvas.Children.Clear();
bodies = new Body[frame.BodyFrameSource.BodyCount];
frame.GetAndRefreshBodyData(bodies);
foreach (Body body in bodies)
{
if (body != null)
{
//Console.WriteLine("Body not null");
//canvas.DrawBody(body);
if (body.IsTracked)
{
Joint[] joints = {
body.Joints[JointType.ThumbLeft],
body.Joints[JointType.ThumbRight],
body.Joints[JointType.HandLeft],
body.Joints[JointType.HandRight]
};
foreach (Joint joint in joints)
{
CameraSpacePoint jointPosition = joint.Position;
Point point = new Point();
if (joint.TrackingState == TrackingState.Tracked)
{
ColorSpacePoint colorPoint = sensor.CoordinateMapper.MapCameraPointToColorSpace(jointPosition);
point.X = float.IsInfinity(colorPoint.X) ? 0 : colorPoint.X;
point.Y = float.IsInfinity(colorPoint.Y) ? 0 : colorPoint.Y;
}
Ellipse ellipse = new Ellipse
{
Fill = Brushes.IndianRed,
Width = 15,
Height = 15
};
Canvas.SetLeft(ellipse, point.X - ellipse.Width / 2);
Canvas.SetTop(ellipse, point.Y - ellipse.Height / 2);
canvas.Children.Add(ellipse);
}
}
#region oldTest
//if (body.IsTracked)
//{
// Joint handRight = body.Joints[JointType.HandRight];
// CameraSpacePoint camHandRight = handRight.Position;
// ColorSpacePoint hrPoint = sensor.CoordinateMapper.MapCameraPointToColorSpace(camHandRight);
// Joint thumbRight = body.Joints[JointType.ThumbRight];
// CameraSpacePoint camThumbRight = thumbRight.Position;
// ColorSpacePoint trPoint = sensor.CoordinateMapper.MapCameraPointToColorSpace(camThumbRight);
// Joint handLeft = body.Joints[JointType.HandLeft];
// CameraSpacePoint camHandLeft = handLeft.Position;
// ColorSpacePoint hlPoint = sensor.CoordinateMapper.MapCameraPointToColorSpace(camHandLeft);
// Joint thumbLeft = body.Joints[JointType.ThumbLeft];
// CameraSpacePoint camThumbLeft = thumbLeft.Position;
// ColorSpacePoint tlPoint = sensor.CoordinateMapper.MapCameraPointToColorSpace(camThumbLeft);
// if (handLeft.TrackingState != TrackingState.Tracked) return;
// if (handRight.TrackingState != TrackingState.Tracked) return;
// if (thumbLeft.TrackingState != TrackingState.Tracked) return;
// if (thumbRight.TrackingState != TrackingState.Tracked) return;
// canvas.DrawPoint(hrPoint, 0);
// canvas.DrawPoint(trPoint, 1);
// canvas.DrawPoint(hlPoint, 0);
// canvas.DrawPoint(tlPoint, 1);
//}
#endregion
}
}
}
// Polls for new skeleton data
public static bool PollSkeleton(ref BodyFrame bodyFrame, Int64 lastFrameTime)
{
bool newSkeleton = false;
int hr = KinectWrapper.GetBodyFrameData(ref bodyFrame, true, true);
if(hr == 0 && (bodyFrame.liRelativeTime > lastFrameTime))
{
newSkeleton = true;
}
return newSkeleton;
}
public static extern int GetBodyFrameData(ref BodyFrame pBodyFrame, bool bGetOrientations, bool bGetHandStates);
/// <summary>
/// Update to get a new frame.
/// This code is similar to the code in the Kinect SDK samples.
/// </summary>
private static void Update()
{
if (!isConnected)
{
return;
}
dataAvailable.WaitOne();
MultiSourceFrame multiSourceFrame = null;
DepthFrame depthFrame = null;
InfraredFrame irFrame = null;
BodyFrame bodyFrame = null;
lock (updateLock)
{
try
{
if (frameReference != null)
{
multiSourceFrame = frameReference.AcquireFrame();
if (multiSourceFrame != null)
{
DepthFrameReference depthFrameReference = multiSourceFrame.DepthFrameReference;
InfraredFrameReference irFrameReference = multiSourceFrame.InfraredFrameReference;
BodyFrameReference bodyFrameReference = multiSourceFrame.BodyFrameReference;
depthFrame = depthFrameReference.AcquireFrame();
irFrame = irFrameReference.AcquireFrame();
if ((depthFrame != null) && (irFrame != null))
{
FrameDescription depthFrameDescription = depthFrame.FrameDescription;
FrameDescription irFrameDescription = irFrame.FrameDescription;
int depthWidth = depthFrameDescription.Width;
int depthHeight = depthFrameDescription.Height;
int irWidth = irFrameDescription.Width;
int irHeight = irFrameDescription.Height;
// verify data and write the new registered frame data to the display bitmap
if (((depthWidth * depthHeight) == depthFrameData.Length) &&
((irWidth * irHeight) == irFrameData.Length))
{
depthFrame.CopyFrameDataToArray(depthFrameData);
irFrame.CopyFrameDataToArray(irFrameData);
}
if (bodyFrameReference != null)
{
bodyFrame = bodyFrameReference.AcquireFrame();
if (bodyFrame != null)
{
if (bodies == null || bodies.Length < bodyFrame.BodyCount)
{
bodies = new Body[bodyFrame.BodyCount];
}
using (bodyFrame)
{
bodyFrame.GetAndRefreshBodyData(bodies);
}
}
}
}
}
}
}
catch (Exception)
{
// ignore if the frame is no longer available
}
finally
{
if (depthFrame != null)
{
depthFrame.Dispose();
depthFrame = null;
}
if (irFrame != null)
{
irFrame.Dispose();
irFrame = null;
}
if (bodyFrame != null)
{
bodyFrame.Dispose();
bodyFrame = null;
}
if (multiSourceFrame != null)
{
multiSourceFrame = null;
}
}
}
}
/// <summary>
/// Kinect数据帧事件处理函数
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void Reader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
if (IsOpen == true)
{
if (conn.IsConnect == true) //待socket连接完成再进行数据处理
{
using (BodyFrame b = e.FrameReference.AcquireFrame()) //获取body数据帧，using会在括号结束后自动销毁申请的数据结构
{
try
{
body = new Body[b.BodyCount]; //新建body数组
b.GetAndRefreshBodyData(body); //从body数据帧里获取body对象放入body数组
}
catch (Exception)
{
//Console.WriteLine(ie.Message);在此计数，累加到一定次数后初始化传感器
if (FrameNullCounter++ == 10)
{
FrameNullCounter = 0;
sensor.Close();
sensor.Open();
}
return;
}
}
foreach (var item in body) //遍历每个body
{
if (item.IsTracked) //body处于被追踪状态时处理
{
if (item.JointOrientations[JointType.Head].Orientation.Z < 2) //Z轴距离大于2不处理
{
int[] position = new int[Constants.POSITION_LENTH]; //新建PWM数组
if (TrackID == 9) //第一次进入时trackID = 9，其他时候都不等于9
{
TrackID = item.TrackingId;
}
if (TrackID == item.TrackingId) //只有当TrackID和当前ID一致时进行处理
{
#region 关节角度计算
Stopwatch sw = new Stopwatch(); sw.Start();
int c = 0;
foreach (var j in item.Joints) //遍历所有关节结构体
{
joints[c++] = ToAngle.filter(j.Value); //过滤
}
//Console.WriteLine("State:{0} x:{1} y:{2}", item.LeanTrackingState, item.Lean.X, item.Lean.Y);
position[(int)angle.servos.ShoulderRight] = ToAngle.ToPWMshoulder(joints[(int)JointType.ShoulderRight], joints[(int)JointType.ElbowRight], joints[(int)JointType.HandRight]);
position[(int)angle.servos.ShoulderLeft] = ToAngle.ToPWMshoulder(joints[(int)JointType.ShoulderLeft], joints[(int)JointType.ElbowLeft], joints[(int)JointType.HandLeft]);
position[(int)angle.servos.ElbowRight] = ToAngle.ToPWM_elbow(joints[(int)JointType.ShoulderRight], joints[(int)JointType.ElbowRight]);
position[(int)angle.servos.ElbowLeft] = ToAngle.ToPWM_elbow(joints[(int)JointType.ShoulderLeft], joints[(int)JointType.ElbowLeft]);
position[(int)angle.servos.HandRight] = ToAngle.ToPWM_hand(joints[(int)JointType.ShoulderRight], joints[(int)JointType.ElbowRight], joints[(int)JointType.HandRight]);
position[(int)angle.servos.HandLeft] = ToAngle.ToPWM_hand(joints[(int)JointType.ShoulderLeft], joints[(int)JointType.ElbowLeft], joints[(int)JointType.HandLeft]);
position[(int)angle.servos.HipRight] = ToAngle.ToPWM_hip(joints[(int)JointType.HipRight], joints[(int)JointType.KneeRight]);
position[(int)angle.servos.HipLeft] = ToAngle.ToPWM_hip(joints[(int)JointType.HipLeft], joints[(int)JointType.KneeLeft]);
position[(int)angle.servos.ThighRight] = ToAngle.ToPWM_thigh(joints[(int)JointType.HipRight], joints[(int)JointType.KneeRight]);
position[(int)angle.servos.ThighLeft] = ToAngle.ToPWM_thigh(joints[(int)JointType.HipLeft], joints[(int)JointType.KneeLeft]);
//position[(int)angle.servos.KneeRight] = ToAngle.ToPWM_knee( joints[(int)JointType.HipRight], joints[(int)JointType.KneeRight], joints[(int)JointType.AnkleRight]);
//position[(int)angle.servos.KneeLeft] = ToAngle.ToPWM_knee( joints[(int)JointType.HipLeft], joints[(int)JointType.KneeLeft], joints[(int)JointType.AnkleLeft]);
//Console.WriteLine("{0}: {1}",angle.servos.ThighLeft.ToString() ,position[(int)angle.servos.ThighLeft]);
//position[(int)angle.servos.AnkleRight] = ToAngle.ToPWM_ankle( joints[(int)JointType.AnkleRight]);
//position[(int)angle.servos.AnkleLeft] = ToAngle.ToPWM_ankle( joints[(int)JointType.AnkleLeft]);
#endregion
#region 舵机PMW异常为0，数据帧丢弃
//Console.WriteLine("舵机PMW异常为0，数据帧丢弃");
for (int i = 0; i < Constants.POSITION_LENTH; i++)
{
switch (position[i])
{
case 0:
if (i != (int)angle.servos.FootLeft && i != (int)angle.servos.FootRight && i != (int)angle.servos.Head && i != Constants.POSITION_LENTH - 1 && i != (int)angle.servos.AnkleRight && i != (int)angle.servos.AnkleLeft && i != (int)angle.servos.KneeLeft && i != (int)angle.servos.KneeRight)
{
return; //非踝部脚部和头部舵机 值为0，计算异常，数据帧丢弃
}
break;
case Constants.INVALID_JOINT_VALUE:
return; //有舵机值异常，数据帧丢弃
default:
break;
}
}
#endregion
//Console.WriteLine("算术平均滤波");
#region 算术平均滤波
//开始执行PWM算术平均滤波
if (speed++ < Constants.frame_count) //先判断，再自增
{
positions.Add(position); //把当前舵机值添加至列表
return; //返回
}
else//采集处理了超过frame_count帧数据
{
speed = 0; //复位计数器
positions.Add(position); //添加当前帧至列表
position = filter_position(positions); //执行算术平均滤波
positions.Clear(); //清除列表
}
#endregion
difference = true;
int diff = 0;
#region 若计算的PWM值变化小于10，返回不发送
//Console.WriteLine("若计算的PWM值变化小于10，返回不发送");
if (IsFirst)//如果是第一次进入
{
//Console.WriteLine("first in");
IsFirst = false;//设置第一次进入标志位false
difference = false;
}
else
{
for (int i = 0; i < position.Length; i++)
{
switch (i)
{
case (int)angle.servos.HandLeft: //只对手部及大腿舵机进行变化值过滤
case (int)angle.servos.HandRight:
case (int)angle.servos.ElbowLeft:
case (int)angle.servos.ElbowRight:
case (int)angle.servos.ShoulderLeft:
case (int)angle.servos.ShoulderRight:
case (int)angle.servos.ThighLeft:
case (int)angle.servos.ThighRight:
diff = Math.Abs(position_pre[i] - position[i]); //计算本帧舵机PWM值和上帧的差值
if (diff > 5) //若有舵机变化值大于5，则置位difference标志
{
difference = false;
}
//Console.WriteLine("diff:" + diff);
else if (diff > 100) //变化值大于40说明计算异常，数据帧丢弃
{
//ToAngle.PrintPosition(position_pre);
Console.WriteLine(position_pre[i] + " " + position[i]);
Console.WriteLine("here is >" + ((angle.servos)i).ToString());
return;
}
break;
default: //非手部及大腿舵机不进行过滤
break;
}
}
}
ToAngle.CopyArray(ref position_pre, ref position);
if (difference == true)//无变化
{
//Console.WriteLine("here is <");
return;//返回
}
#endregion
#region 姿态检测
//Console.WriteLine("姿态检测");
IsOpen = false;
ToAngle.PoseDetect(position, item.HandRightState);
#endregion
#region PWM数据帧发送和显示
//Console.WriteLine("PWM数据帧发送和显示");
if (ToAngle.IsSquat == true) //如果是下蹲
{
ToAngle.InitBottomLeg(ref position); //则初始化至下蹲
}
else
{
ToAngle.InitLeg(ref position);//否则初始化至站立
}
string PWM = ToAngle.toPWM(position);
if (PWM == null)
{
return;
}
try
{
if (TimeOut)
{
conn.Send("sb" + PWM + "\r\n");
}
//Console.WriteLine("PWM:" + PWM.Length);
}
catch (Exception ex)
{
MessageBox.Show(ex.ToString());
}
this.Dispatcher.Invoke(new Action(() => //在与 Dispatcher 关联的线程上同步执行指定的委托
{
/* 在 WPF 中，只有创建 DispatcherObject 的线程才能访问该对象。例如，一个从主 UI 线程派生的后台线程不能更新在该 UI 线程上创建的 Button 的内容。为了使该后台线程能够访问 Button 的 Content 属性，该后台线程必须将此工作委托给与该 UI 线程关联的 Dispatcher。它使用Invoke 或 BeginInvoke完成。Invoke是同步，BeginInvoke 是异步。该操作将按指定的 DispatcherPriority 添加到 Dispatcher 的事件队列中。
* Invoke 是同步操作；因此，直到回调返回之后才会将控制权返回给调用对象。 */
//String p;
//TextBox_data.AppendText(PWM.Substring(0, 2));
//p = System.Text.RegularExpressions.Regex.Replace(PWM.Substring(2, 51), @".{3}","$0 ");
//TextBox_data.AppendText(p + "\n");
TextBox_data.AppendText(PWM + "\n"); //textbox追加PWM
scroller_dataShow.ScrollToEnd(); //滚动至尾部
}));
#endregion
//Console.WriteLine(sw.Elapsed.TotalMilliseconds);
}
}
}
}
//IsOpen = true;
}
}
}
public void processBodyFrame(BodyFrame frame)
{
frame.GetAndRefreshBodyData(bodies);
foreach (var body in Bodies)
{
if (!eManager.users.ContainsKey(body.TrackingId))
{
eManager.users[body.TrackingId] = new MyHuman(body);
eManager.holdTime[body.TrackingId] = 0;
}
// Multithreading maybe
drawer.currentCanvasName = "body";
drawer.drawSkeleton(body);
}
}
/// <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;
int bodyIndex = 0;
OscBundle oscBundle = new OscBundle(DateTime.Now);
foreach (Body body in this.bodies)
{
Pen drawPen = this.bodyColors[penIndex++];
if (body.IsTracked)
{
bodyIndex++;
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;
}
// add all joints data to osc bundle
Object[] args = new Object[10];
args[0] = bodyIndex;
args[1] = (int)jointType;
args[2] = position.X;
args[3] = position.Y;
args[4] = position.Z;
args[5] = body.JointOrientations[jointType].Orientation.X;
args[6] = body.JointOrientations[jointType].Orientation.Y;
args[7] = body.JointOrientations[jointType].Orientation.Z;
args[8] = body.JointOrientations[jointType].Orientation.W;
args[9] = (int)joints[jointType].TrackingState;
// JointType: https://msdn.microsoft.com/en-us/library/microsoft.kinect.jointtype.aspx
foreach (OscConnection oscConnection in oscConnectionList)
{
if (oscConnection.OscUdpWriter != null)
{
string oscAddress = "/" + kinectSensor.UniqueKinectId;
if (oscConnection.Identifier != null)
{
if (oscConnection.Identifier != "")
{
oscAddress = "/" + oscConnection.Identifier;
}
}
OscElement oscElement = new OscElement(oscAddress, args);
oscConnection.OscUdpWriter.Send(oscElement);
oscBundle.AddElement(oscElement);
}
}
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);
}
}
// send osc bundle
// if (oscUdpWriter != null)
// oscUdpWriter.SendBundle(oscBundle);
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
}
}
}
private void ShowInfraredFrame(InfraredFrame infraredFrame, BodyFrame bodyFrame)
{
bool infraredFrameProcessed = false;
if (infraredFrame != null)
{
FrameDescription infraredFrameDescription = infraredFrame.FrameDescription;
// verify data and write the new infrared frame data to the display bitmap
if (((infraredFrameDescription.Width * infraredFrameDescription.Height)
== this.infraredFrameData.Length) &&
(infraredFrameDescription.Width == this.bitmap.PixelWidth) &&
(infraredFrameDescription.Height == this.bitmap.PixelHeight))
{
//Debug.WriteLine("Width is " + infraredFrameDescription.Width);
//Debug.WriteLine("Height is " + infraredFrameDescription.Height);
infraredWidth = infraredFrameDescription.Width;
infraredHeight = infraredFrameDescription.Height;
// Copy the pixel data from the image to a temporary array
infraredFrame.CopyFrameDataToArray(this.infraredFrameData);
infraredFrameProcessed = true;
}
}
if (bodyFrame != null && bodyFrame.BodyCount > 0)
{
myBodies = new Body[this.kinectSensor.BodyFrameSource.BodyCount];
bodyFrame.GetAndRefreshBodyData(myBodies);
}
// we got a frame, convert and render
if (infraredFrameProcessed)
{
this.ConvertInfraredDataToPixels(myBodies);
this.RenderPixelArray(this.infraredPixels);
}
}
void msfr_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
try
{
MultiSourceFrame msf = e.FrameReference.AcquireFrame();
if (msf != null)
{
using (BodyFrame bodyFrame = msf.BodyFrameReference.AcquireFrame())
using (ColorFrame colorFrame = msf.ColorFrameReference.AcquireFrame())
using (DepthFrame irFrame = msf.DepthFrameReference.AcquireFrame())
{
if (bodyFrame != null && colorFrame != null && irFrame != null)
{
bodies = new Body[bodyFrame.BodyFrameSource.BodyCount];
FrameDescription framedesc = colorFrame.FrameDescription;
using (KinectBuffer colorBuffer = colorFrame.LockRawImageBuffer())
{
colorBitmap.Lock();
if ((framedesc.Width == colorBitmap.PixelWidth) && (framedesc.Height == colorBitmap.PixelHeight))
{
colorFrame.CopyConvertedFrameDataToIntPtr(colorBitmap.BackBuffer, (uint)(framedesc.Width * framedesc.Height * 4), ColorImageFormat.Bgra);
colorBitmap.AddDirtyRect(new Int32Rect(0, 0, colorBitmap.PixelWidth, colorBitmap.PixelHeight));
}
colorBitmap.Unlock();
bodyFrame.GetAndRefreshBodyData(bodies);
canvas.Children.Clear();
foreach (var body in bodies)
{
if (body != null)
{
if (body.IsTracked)
{
//DrawJoint(body);
foreach (var item in body.Joints)
{
JointType jointType = item.Key;
Joint joint = item.Value;
if (flag)
{
if (joint.TrackingState == TrackingState.Tracked)
{
CameraSpacePoint camersp = joint.Position;
ColorSpacePoint csp = this.kinectRegion.KinectSensor.CoordinateMapper.MapCameraPointToColorSpace(camersp);
//this part draws a line between the waistline
if (joint.JointType == JointType.HipLeft)
{
Point point1 = new Point(csp.X - 140, csp.Y);
foreach (Joint joint2 in body.Joints.Values)
{
if (joint2.JointType == JointType.HipRight)
{
CameraSpacePoint camers = joint2.Position;
ColorSpacePoint cs = this.kinectRegion.KinectSensor.CoordinateMapper.MapCameraPointToColorSpace(camers);
Point point2 = new Point(cs.X + 140, cs.Y);
DrawLine(point1, point2);
DrawTextWidth("Your waist is large about " + BodyWidth.Width(body) + " centimeters", cs, 410, 70);
DrawPoint(point1, point2);
}
}
}
//this part draws a line between the head and the foot for the height
if (joint.JointType == JointType.Head)
{
Point point3 = new Point(csp.X + 500, csp.Y);
foreach (Joint joint3 in body.Joints.Values)
{
if (joint3.JointType == JointType.FootLeft)
{
if ((joint3.TrackingState != TrackingState.Inferred))
{
CameraSpacePoint camer = joint3.Position;
ColorSpacePoint c = this.kinectRegion.KinectSensor.CoordinateMapper.MapCameraPointToColorSpace(camer);
Point point4 = new Point(csp.X + 500, c.Y);
if (IsPointForDrawing(point3))
{
if (IsPointForDrawing(point4))
{
DrawLine(point3, point4);
DrawTextHeight("Your height is about " + BodyHeight.Height(body) + " meters", csp, 320, 70);
DrawPoint(point3, point4);
}
}
}
}
}
}
}
}
//this part puts some buttons on screen
if (joint.JointType == JointType.SpineMid)
{
SetTables();
DepthSpacePoint space2 = this.kinectRegion.KinectSensor.CoordinateMapper.MapCameraPointToDepthSpace(joint.Position);
ColorSpacePoint space = this.kinectRegion.KinectSensor.CoordinateMapper.MapCameraPointToColorSpace(joint.Position);
Point center = new Point(space.X - 75, space.Y);
foreach (Joint joints in body.Joints.Values)
{
if (joints.JointType == JointType.ShoulderLeft)
{
DepthSpacePoint depth_point = this.kinectRegion.KinectSensor.CoordinateMapper.MapCameraPointToDepthSpace(joints.Position);
ColorSpacePoint color_point = this.kinectRegion.KinectSensor.CoordinateMapper.MapCameraPointToColorSpace(joints.Position);
float distance = (float)Math.Round((joints.Position.Z), 2);
ShowMessageAboutDistance(distance, color_point);
Point[] verticies = new Point[MAX];
//DrawText("Your distance from the sensor is " + distance, color_point);
//TODO probabilmente è da cambiare la variabile all'interno
for (int i = 0; i < bodyFrame.BodyCount; i++)
{
if (distance < 1.5)
{
verticies = Polygon.CalculateVertices(buttons_array[i].Count, 300, 18, center);
ReplaceButtons(100, 100, center, buttons_array[i], verticies);
}
if ((distance < 2.5) && (distance > 1.5))
{
verticies = Polygon.CalculateVertices(buttons_array[i].Count, 250, 18, center);
ReplaceButtons(80, 85, center, buttons_array[i], verticies);
}
if ((distance < 3.5) && (distance > 2.5))
{
verticies = Polygon.CalculateVertices(buttons_array[i].Count, 200, 18, center);
ReplaceButtons(60, 70, center, buttons_array[i], verticies);
}
}
}
}
}
}
}
}
}
}
}
}
}
}
finally
{
//if (msfr != null) msfr.Dispose();
}
}
private void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
if (!(KinectStreamerConfig.ProvideBodyData || KinectStreamerConfig.ProvideColorData || KinectStreamerConfig.ProvideDepthData))
{
return;
}
depthFrame = null;
colorFrame = null;
bodyFrame = null;
multiSourceFrame = e.FrameReference.AcquireFrame();
// If the Frame has expired by the time we process this event, return.
if (multiSourceFrame == null)
{
return;
}
// We use a try/finally to ensure that we clean up before we exit the function.
// This includes calling Dispose on any Frame objects that we may have and unlocking the bitmap back buffer.
try
{
depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame();
colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame();
bodyFrame = multiSourceFrame.BodyFrameReference.AcquireFrame();
// If any frame has expired by the time we process this event, return.
// The "finally" statement will Dispose any that are not null.
if ((depthFrame == null) || (colorFrame == null) || (bodyFrame == null))
{
return;
}
// Process color stream if needed
if (KinectStreamerConfig.ProvideColorData)
{
ProcessColorData();
}
// Process depth frame if needed
if (KinectStreamerConfig.ProvideDepthData)
{
ProcessDepthData();
}
// Process body data if needed
if (KinectStreamerConfig.ProvideBodyData)
{
ProcessBodyData();
}
}
finally
{
if (depthFrame != null)
{
depthFrame.Dispose();
}
if (colorFrame != null)
{
colorFrame.Dispose();
}
if (bodyFrame != null)
{
bodyFrame.Dispose();
}
}
}
public void RecordFrame(BodyFrame frame)
{
if (!_isStarted)
throw new InvalidOperationException("Cannot record frames unless the KinectRecorder is started.");
if (frame != null)
{
var replayFrame = new RPBodyFrame(frame);
if (MapDepthPositions)
{
replayFrame.MapDepthPositions();
}
if (MapColorPositions)
{
replayFrame.MapColorPositions();
}
// _recordQueue.Enqueue(replayFrame);
System.Diagnostics.Debug.WriteLine("+++ Enqueued Body Frame ({0})", _recordQueue.Count);
}
else
{
System.Diagnostics.Debug.WriteLine("!!! FRAME SKIPPED (Body in KinectRecorder)");
}
}
private void BodyFrameReader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
// effector start at (-81.7, -0.6, -19.3)
// Shoulder->Elbow->Wrist->Hand->HandTip
// Wrist->Thumb
bool dataReceived = false;
using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
if (bodies == null)
{
bodies = new Body[bodyFrame.BodyCount];
}
bodyFrame.GetAndRefreshBodyData(bodies);
dataReceived = true;
}
}
if (dataReceived)
{
var body = bodies.Where(b => b.IsTracked).FirstOrDefault();
if (body != null)
{
var joints = body.Joints;
var shoulder = joints[JointType.ShoulderRight];
var elbow = joints[JointType.ElbowRight];
var wrist = joints[JointType.WristRight];
var hand = joints[JointType.HandRight];
var handTip = joints[JointType.HandTipRight];
var thumb = joints[JointType.ThumbRight];
shoulderPosition = new Vector3(shoulder.Position.X, shoulder.Position.Y, shoulder.Position.Z);
elbowPositon = new Vector3(elbow.Position.X, elbow.Position.Y, elbow.Position.Z);
handPosition = new Vector3(hand.Position.X, hand.Position.Y, hand.Position.Z);
var pe = ProjectPointOnLine(shoulderPosition, handPosition, elbowPositon);
float dist = Vector3.Distance(pe, elbowPositon);
float length = Vector3.Distance(shoulderPosition, handPosition);
armCalibrationRate = dist / length;
if (!armCalibrated && dist / length < 0.01)
{
armCalibrated = true;
armLength = length;
}
if (armCalibrated)
{
var diff = handPosition - shoulderPosition;
var tar = diff / armLength * ur5length;
var t = tar.z;
tar.z = tar.x;
tar.x = t;
target.transform.position = tar;
}
}
}
}
private void RenderBodyFrame(BodyFrame bodyFrame)
{
bool dataReceived = false;
Body[] bodies = null;
if (bodyFrame != null)
{
if (bodies == null)
{
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(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, Instance._displayWidth, Instance._displayHeight));
int penIndex = 0;
foreach (Body body in 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, body.JointOrientations);
if (Instance.DrawOrientationVectors == true)
{
this.DrawLocalCoordinates(body.JointOrientations, joints, jointPoints, dc);
}
}
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, Instance._displayWidth, Instance._displayHeight));
}
}
}
/// <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())
{
// 画一个透明背景设置渲染的大小（绘制矩形，黑色背景）
dc.DrawRectangle(Brushes.DarkGray, null, new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
//虚拟按钮绘制
//dc.DrawRectangle(null, new Pen(Brushes.Red, 4), new Rect(50, 120, 50, 100));//左
//dc.DrawRectangle(null, new Pen(Brushes.Red, 4), new Rect(240, 120, 50, 100));//右
//dc.DrawRectangle(null, new Pen(Brushes.Red, 4), new Rect(120, 50, 100, 50));//前
//dc.DrawRectangle(null, new Pen(Brushes.Red, 4), new Rect(120, 240, 100, 50));//后
//dc.DrawRectangle(null, new Pen(Brushes.Red, 4), new Rect(120, 120, 100, 40));//上
//dc.DrawRectangle(null, new Pen(Brushes.Red, 4), new Rect(120, 180, 100, 40));//下
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;
// 关节点转换为深度(显示)空间：X Y坐标
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);
//////////user//////////////
//显示坐标
Lhand_PostionX.Content = (jointPoints[JointType.HandLeft].X).ToString("#0.00");
Lhand_PostionY.Content = (jointPoints[JointType.HandLeft].Y).ToString("#0.00");
Rhand_PostionX.Content = (jointPoints[JointType.HandRight].X).ToString("#0.00");
Rhand_PostionY.Content = (jointPoints[JointType.HandRight].Y).ToString("#0.00");
if (StartControl && joints[JointType.HandLeft].TrackingState == TrackingState.Tracked)
{
Canvas.SetTop(_controlHand, jointPoints[JointType.HandLeft].Y - 30);
Canvas.SetLeft(_controlHand, jointPoints[JointType.HandLeft].X - 30);
double deltaY = 25;
if (body.HandLeftState == HandState.Lasso && body.HandRightState == HandState.Lasso)
{
ControlMode = false;
}
if (body.HandLeftState == HandState.Closed && body.HandRightState == HandState.Closed && Math.Abs(jointPoints[JointType.HandLeft].Y - jointPoints[JointType.HandRight].Y) < deltaY)
{
ControlMode = true;
}
if (ControlMode)
{
if (AppStateIsDrawing == true)
{
AppStateIsDrawing = false;
AppStateIsButton = true;
BeginStoryboard((Storyboard)FindResource("Area_Show"));
}
this.PositonControl(joints, jointPoints);//进入按键控制
}
else
{
if (AppStateIsButton == true)
{
AppStateIsButton = false;
AppStateIsDrawing = true;
BeginStoryboard((Storyboard)FindResource("Area_Dispear"));//按钮消失
drawPoints.Clear();
}
this.DrawControl(body, joints, jointPoints, dc);//进入画笔控制
}
}
else if (joints[JointType.HandLeft].TrackingState != TrackingState.Tracked) //
{
_controlHand.Opacity = 0;
//mainTimer.Stop();
}
//huatu
if (!ControlMode)
{
Pen tempPen = new Pen(Brushes.Red, 2);
foreach (List <System.Drawing.Point> lp in drawPoints)
{
for (int i = 0; i < lp.Count - 1; i++)
{
Point tpt = new Point(lp[i].X, lp[i].Y);
Point tpt2 = new Point(lp[i + 1].X, lp[i + 1].Y);
dc.DrawLine(tempPen, tpt, tpt2);
}
}
}
}
}
// 防止图像出界
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
}
}
}
private void RefreshBodyData(BodyFrame frame){
cache.Clear();
foreach (var body in (IList<Body>) frame.RawData) {
if (!body.IsTracked) { continue; }
NBody nbody = frame.Find(body.TrackingId);
if (nbody == null) {
nbody = new NBody(body.TrackingId, frame.Width, frame.Height);
}
cache.Add(nbody);
RefreshBodyData(body, nbody);
}
lock (frame.Bodies) {
frame.Bodies.Clear();
frame.Bodies.AddRange(cache);
}
}
/// <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)
{
txt_kinect_info.Content = "Tracking: " + this.scanType.ToString() + "\n";
if (this.drawingGroup != null && this.bodyColors != null)
{
using (DrawingContext dc = this.drawingGroup.Open())
{
// 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;
foreach (Body body in this.bodies)
{
Pen drawPen = this.bodyColors[penIndex++];
if (body.IsTracked)//body.istrack
{
this.DrawClippedEdges(body, dc);
IReadOnlyDictionary <JointType, Joint> joints = body.Joints;
Dictionary <JointType, Point> jointPoints = new Dictionary <JointType, Point>();
CameraSpacePoint r_hip = new CameraSpacePoint();
CameraSpacePoint r_shoulder = new CameraSpacePoint();
CameraSpacePoint r_elbow = new CameraSpacePoint();
CameraSpacePoint r_wrist = new CameraSpacePoint();
CameraSpacePoint l_hip = new CameraSpacePoint();
CameraSpacePoint l_shoulder = new CameraSpacePoint();
CameraSpacePoint l_elbow = new CameraSpacePoint();
CameraSpacePoint l_wrist = new CameraSpacePoint();
CameraSpacePoint neck = new CameraSpacePoint();
CameraSpacePoint up = new CameraSpacePoint();
CameraSpacePoint spine_base = new CameraSpacePoint();
CameraSpacePoint spine_mid = new CameraSpacePoint();
CameraSpacePoint head = new CameraSpacePoint();
foreach (JointType jointType in joints.Keys)
{
CameraSpacePoint position = joints[jointType].Position;
float x = position.X;
switch (jointType)
{
case JointType.ShoulderRight:
r_shoulder.X = position.X;
r_shoulder.Y = position.Y;
break;
case JointType.HipRight:
r_hip.X = position.X;
r_hip.Y = position.Y;
//this is for trunk
up.X = position.X;
up.Y = position.Y + 0.1f;
break;
case JointType.ElbowRight:
r_elbow.X = position.X;
r_elbow.Y = position.Y;
break;
case JointType.WristRight:
r_wrist.X = position.X;
r_wrist.Y = position.Y;
break;
case JointType.ShoulderLeft:
l_shoulder.X = position.X;
l_shoulder.Y = position.Y;
break;
case JointType.HipLeft:
l_hip.X = position.X;
l_hip.Y = position.Y;
//this is for trunk
up.X = position.X;
up.Y = position.Y + 0.1f;
break;
case JointType.ElbowLeft:
l_elbow.X = position.X;
l_elbow.Y = position.Y;
break;
case JointType.WristLeft:
l_wrist.X = position.X;
l_wrist.Y = position.Y;
break;
case JointType.Neck:
neck.X = position.X;
neck.Y = position.Y;
break;
case JointType.SpineBase:
spine_base.X = position.X;
spine_base.Y = position.Y;
break;
case JointType.Head:
head.X = position.X;
head.Y = position.Y;
break;
case JointType.SpineMid:
spine_mid.X = position.X;
spine_mid.Y = position.Y;
break;
}
if (position.Z < 0)
{
position.Z = InferredZPositionClamp;
}
var depthSpacePoint = this.coordinateMapper.MapCameraPointToColorSpace(position);
jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
JointType parent = Helpers.GetOrigintJoint(jointType);
if (this.typeOfEvaluation == "right" && parent != JointType.Head && r_hip != null && r_shoulder != null && r_elbow != null && r_wrist != null && neck != null)
{
#region Right Rula
double r_hip_shoulder = Helpers.Angle(r_shoulder, r_hip);
double r_shoulder_elbow = Helpers.Angle(r_shoulder, r_elbow, 90);
double r_elbow_wrist = Helpers.Angle(r_elbow, r_wrist, 90);
double upper_arm_angle = Helpers.Inverse(r_shoulder_elbow - 180);
double lower_arm_angle = Helpers.Inverse((r_elbow_wrist - 180)) + upper_arm_angle;
double up_spinebase = Helpers.Angle(spine_mid, up);
double neck_spine_base = Helpers.Angle(neck, spine_mid);
//double trunk_angle = (Angle(up, r_hip) + Angle(r_hip, neck)) - 270;
double trunk_angle = neck_spine_base - 90;
double neck_head = Helpers.Angle(head, neck);
double neck_angle = (neck_head - trunk_angle) - 90 - 20;
#region Scores
score_upper_armk = Helpers.GetRulaUpperArmScore(upper_arm_angle);
score_lower_armk = Helpers.GetRulaLowerArmScore(lower_arm_angle);
score_trunk = Helpers.GetRulaTrunkScore(trunk_angle);
score_neck = Helpers.GetRulaNeckScore(neck_angle);
#endregion
//txt_kinect_info.Content += "UPPER ARM:" + upper_arm_angle.ToString("00.0") + "\n";
//txt_kinect_info.Content += "UPPER ARM SCORE:" + score_upper_armk.ToString("0") + "\n";
//txt_kinect_info.Content += "LOWER ARM:" + lower_arm_angle.ToString("00.0") + "\n";
//txt_kinect_info.Content += "LOWER ARM SCORE:" + score_lower_armk.ToString("0") + "\n";
//txt_kinect_info.Content += "Trunk Angle:" + trunk_angle.ToString("00.0") + "\n";
//txt_kinect_info.Content += "Trunk Score:" + score_trunk.ToString("00.0") + "\n";
//txt_kinect_info.Content += "Neck Angle:" + neck_angle.ToString("00.0") + "\n";
//txt_kinect_info.Content += "Neck Score:" + score_neck.ToString("00.0") + "\n";
#endregion
}
else if (this.typeOfEvaluation == "left" && parent != JointType.Head && l_hip != null && l_shoulder != null && l_elbow != null && l_wrist != null && neck != null)
{
double l_hip_shoulder = Helpers.Angle(l_shoulder, l_hip);
double l_shoulder_elbow = Helpers.Angle(l_shoulder, l_elbow, 90);
double l_elbow_wrist = Helpers.Angle(l_elbow, l_wrist, 90);
double upper_arm_angle = Helpers.Inverse(l_shoulder_elbow - 180);
double lower_arm_angle = Helpers.Inverse((l_elbow_wrist - 180)) + upper_arm_angle;
double up_spinebase = Helpers.Angle(spine_mid, up);
double neck_spine_base = Helpers.Angle(neck, spine_mid);
//double trunk_angle = (Angle(up, r_hip) + Angle(r_hip, neck)) - 270;
double trunk_angle = neck_spine_base - 90;
double neck_head = Helpers.Angle(head, neck);
double neck_angle = (neck_head - trunk_angle) - 90 - 20;
#region Scores
score_upper_armk = Helpers.GetRulaUpperArmScore(upper_arm_angle);
score_lower_armk = Helpers.GetRulaLowerArmScore(lower_arm_angle);
score_trunk = Helpers.GetRulaTrunkScore(trunk_angle);
score_neck = Helpers.GetRulaNeckScore(neck_angle);
#endregion
//txt_kinect_info.Content += "UPPER ARM:" + upper_arm_angle.ToString("00.0") + "\n";
//txt_kinect_info.Content += "UPPER ARM SCORE:" + score_upper_armk.ToString("0") + "\n";
//txt_kinect_info.Content += "LOWER ARM:" + lower_arm_angle.ToString("00.0") + "\n";
//txt_kinect_info.Content += "LOWER ARM SCORE:" + score_lower_armk.ToString("0") + "\n";
//txt_kinect_info.Content += "Trunk Angle:" + trunk_angle.ToString("00.0") + "\n";
//txt_kinect_info.Content += "Trunk Score:" + score_trunk.ToString("00.0") + "\n";
//txt_kinect_info.Content += "Neck Angle:" + neck_angle.ToString("00.0") + "\n";
//txt_kinect_info.Content += "Neck Score:" + score_neck.ToString("00.0") + "\n";
}
}
this.DrawBody(joints, jointPoints, dc, drawPen);
//this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
//this.DrawHand(body.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));
}
}
}
}
private void ProcessFrames(ColorFrame colorFrame, DepthFrame depthFrame, BodyIndexFrame bodyIndexFrame, BodyFrame bodyFrame, byte [] psBytes0, byte [] psBytes1)
{
// create multiframe to process
long ticksCopyData = DateTime.Now.Ticks;
MultiFrame multiFrame = new MultiFrame();
multiFrame.FrameNb = Interlocked.Increment(ref frameNb);
// color
long ticksCreateColorData = DateTime.Now.Ticks;
byte[] colorData = new byte[colorByteSize];
Utils.UpdateTimer("CreateColorData", ticksCreateColorData);
long ticksCopyColorData = DateTime.Now.Ticks;
colorFrame.CopyConvertedFrameDataToArray(colorData, ColorImageFormat.Bgra);
Utils.UpdateTimer("CopyColorData", ticksCopyColorData);
// depth
long ticksCreateDepthData = DateTime.Now.Ticks;
ushort[] depthData = new ushort[depthPixelSize];
depthFrame.CopyFrameDataToArray(depthData);
Utils.UpdateTimer("CreateDepthData", ticksCreateDepthData);
// body index
long ticksCreateBodyIndexData = DateTime.Now.Ticks;
byte[] bodyIndexData = new byte[depthPixelSize];
bodyIndexFrame.CopyFrameDataToArray(bodyIndexData);
Utils.UpdateTimer("CreateBodyIndexData", ticksCreateBodyIndexData);
// bodies
long ticksCreateBodiesData = DateTime.Now.Ticks;
Body[] bodies = new Body[bodyFrame.BodyCount];
bodyFrame.GetAndRefreshBodyData(bodies);
Utils.UpdateTimer("CreateBodiesData", ticksCreateBodiesData);
// ps3eye
byte[] psBytes = null;
if (psBytes0 != null && psBytes1 != null)
{
long ticksCreatePS3EyeData = DateTime.Now.Ticks;
psBytes = new byte[psByteSize * 2];
Utils.UpdateTimer("CreatePS3EyeData", ticksCreatePS3EyeData);
long ticksCopyPS3EyeData = DateTime.Now.Ticks;
CopyPS3EyeDataMirror(psBytes, psBytes0, psBytes1);
Utils.UpdateTimer("CopyPS3EyeData", ticksCopyPS3EyeData);
}
// multiFrame
long ticksMultiFrame = DateTime.Now.Ticks;
multiFrame.DepthData = depthData;
multiFrame.ColorData = colorData;
multiFrame.BodyIndexData = bodyIndexData;
multiFrame.Bodies = bodies;
multiFrame.PS3EyeData = psBytes;
multiFrame.HasKinectData = true;
multiFrame.HasPS3EyeData = psBytes != null ? true : false;
Utils.UpdateTimer("MultiFrame", ticksMultiFrame);
long ticksEnqueue = DateTime.Now.Ticks;
ProcessingManager.Instance.EnqueueMultiFrame(multiFrame);
Utils.UpdateTimer("Enqueue", ticksEnqueue);
Utils.UpdateTimer("CopyFramesData", ticksCopyData);
// display timers & queues
Context.GUI.DisplayPerformance();
}
private void ProcessBodyFrame(BodyFrame bodyFrame)
{
if (null != bodyFrame)
{
if (this.m_currentTrackedBody != null)
{
this.m_currentTrackedBody = FindBodyWithTrackingId(bodyFrame, this.m_CurrentTrackingId);
if (this.m_currentTrackedBody != null)
{
return;
}
}
Body selectedBody = FindClosestBody(bodyFrame);
if (selectedBody == null)
{
return;
}
this.m_currentTrackedBody = selectedBody;
this.m_CurrentTrackingId = selectedBody.TrackingId;
//SetupFace
InitializeFace();
}
}
private void bfReader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
using (BodyFrame bdframe = e.FrameReference.AcquireFrame())
{
if (null != bdframe)
{
Bodies = new Body[bdframe.BodyCount];
bdframe.GetAndRefreshBodyData(Bodies);
Body bodyselected = selectTracker(Bodies);
if (bodyselected != null)
{
bdframe.BodyFrameSource.OverrideHandTracking(bodyselected.TrackingId);
n++;
if (bodyselected.HandRightConfidence == TrackingConfidence.High && bodyselected.HandRightState == HandState.Closed)
{
startpos = bodyselected.Joints[JointType.HandRight].Position;
priorpos = startpos;
ControlState = 1;
}
if (ControlState == 1)
{
Currentpos = bodyselected.Joints[JointType.HandRight].Position;
float dirflag = Currentpos.Y - startpos.Y;//正反转方向
float addOrsub = Currentpos.X - priorpos.X;
if (Math.Abs(addOrsub) > CtlWidth)
{
if (addOrsub > CtlWidth)
{
if (dirflag > 0)
{
addone = true;
subone = false;
}
else
{
addone = false;
subone = false;
}
}
else
{
if (dirflag > 0)
{
addone = false;
subone = true;
}
else
{
addone = true;
subone = false;
}
}
}
}
}
}
}
}
/// <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)
{
if (!File.Exists("Data.csv"))
{
using (StreamWriter writer = new StreamWriter("Data.csv", true, Encoding.UTF8))
{
writer.Write(string.Join(",", CsvData.Fields()) + "\n");
}
}
using (DrawingContext dc = this.drawingGroup.Open())
{
var names = new List <string>();
this.timeText = DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss.fff");
this.StatusText = (this.kinectSensor.IsAvailable ? Properties.Resources.RunningStatusText
: Properties.Resources.SensorNotAvailableStatusText) + " " + this.timeText;
names.Add(this.timeText);
// Draw a transparent background to set the render size
dc.DrawImage(this.colorBitmap, 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;
names.Add(position.X + "," + position.Y + "," + position.Z);
if (position.Z < 0)
{
position.Z = InferredZPositionClamp;
}
ColorSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToColorSpace(position);
jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
names.Add(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);
}
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
if (names.Count > 1)
{
using (StreamWriter writer = new StreamWriter("Data.csv", true, Encoding.UTF8))
{
writer.Write(string.Join(",", names) + "\n");
}
}
}
}
}
/// <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);
}
using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
{
// System.Console.WriteLine(bodyFrame.FloorClipPlane.W + " " + bodyFrame.FloorClipPlane.X + " " + bodyFrame.FloorClipPlane.Y + " " + bodyFrame.FloorClipPlane.Z);
CameraSpacePoint position = joints[JointType.SpineBase].Position;
//JUSTTHERE
// System.Console.WriteLine(position.X);
// System.Console.WriteLine(position.Z);
if (position.Z != 0)
{
if (position.X / position.Z > 0.6)
{
if (position.Z >= 1.15)
{
flag = 1;
System.Console.WriteLine("head and turn right" + position.X / position.Z);
}
if (position.Z >= 0.85 && position.Z < 1.15)
{
flag = 2;
System.Console.WriteLine("stop" + position.X / position.Z);
}
if (position.Z < 0.85)
{
flag = 3;
System.Console.WriteLine("back" + position.X / position.Z);
}
}
if ((position.X / position.Z > 0.5) && (position.X / position.Z <= 0.6))
{
if (position.Z >= 1.15)
{
flag = 4;
System.Console.WriteLine("head and turn right" + position.X / position.Z);
}
if (position.Z >= 0.85 && position.Z < 1.15)
{
flag = 2;
System.Console.WriteLine("stop" + position.X / position.Z);
}
if (position.Z < 0.85)
{
flag = 3;
System.Console.WriteLine("back" + position.X / position.Z);
}
}
if ((position.X / position.Z > 0.4) && (position.X / position.Z <= 0.5))
{
if (position.Z >= 1.15)
{
flag = 5;
System.Console.WriteLine("head and turn right" + position.X / position.Z);
}
if (position.Z >= 0.85 && position.Z < 1.15)
{
flag = 2;
System.Console.WriteLine("stop" + position.X / position.Z);
}
if (position.Z < 0.85)
{
flag = 3;
System.Console.WriteLine("back" + position.X / position.Z);
}
}
if ((position.X / position.Z > 0.3) && (position.X / position.Z <= 0.4))
{
if (position.Z >= 1.15)
{
flag = 6;
System.Console.WriteLine("ahead and turn right" + position.X / position.Z);
}
if (position.Z >= 0.85 && position.Z < 1.15)
{
flag = 2;
System.Console.WriteLine("stop" + position.X / position.Z);
}
if (position.Z < 0.85)
{
flag = 3;
System.Console.WriteLine("back" + position.X / position.Z);
}
}
if ((position.X / position.Z > 0.2) && (position.X / position.Z <= 0.3))
{
if (position.Z >= 1.15)
{
flag = 7;
System.Console.WriteLine("ahead and turn right" + position.X / position.Z);
}
if (position.Z >= 0.85 && position.Z < 1.15)
{
flag = 2;
System.Console.WriteLine("stop" + position.X / position.Z);
}
if (position.Z < 0.85)
{
flag = 3;
System.Console.WriteLine("back" + position.X / position.Z);
}
}
if ((position.X / position.Z > -0.2) && (position.X / position.Z <= 0.2))
{
if (position.Z >= 1.15)
{
flag = 8;
System.Console.WriteLine("ahead" + position.X / position.Z);
}
if (position.Z >= 0.85 && position.Z < 1.15)
{
flag = 2;
System.Console.WriteLine("stop");
}
if (position.Z < 0.85)
{
flag = 3;
System.Console.WriteLine("back");
}
}
if ((position.X / position.Z > -0.3) && (position.X / position.Z <= -0.2))
{
if (position.Z >= 1.15)
{
flag = 9;
System.Console.WriteLine("ahead and turn left" + position.X / position.Z);
}
if (position.Z >= 0.85 && position.Z < 1.15)
{
flag = 2;
System.Console.WriteLine("stop");
}
if (position.Z < 0.85)
{
flag = 3;
System.Console.WriteLine("back");
}
}
if ((position.X / position.Z >= -0.4) && (position.X / position.Z <= -0.3))
{
if (position.Z >= 1.15)
{
flag = 10;
System.Console.WriteLine("ahead and turn left" + position.X / position.Z);
}
if (position.Z >= 0.85 && position.Z < 1.15)
{
flag = 2;
System.Console.WriteLine("stop");
}
if (position.Z < 0.85)
{
flag = 3;
System.Console.WriteLine("back");
}
}
if ((position.X / position.Z >= -0.5) && (position.X / position.Z <= -0.4))
{
if (position.Z >= 1.15)
{
flag = 11;
System.Console.WriteLine("ahead and turn left" + position.X / position.Z);
}
if (position.Z >= 0.85 && position.Z < 1.15)
{
flag = 2;
System.Console.WriteLine("stop");
}
if (position.Z < 0.85)
{
flag = 3;
System.Console.WriteLine("back");
}
}
if ((position.X / position.Z >= -0.6) && (position.X / position.Z <= -0.5))
{
if (position.Z >= 1.15)
{
flag = 12;
System.Console.WriteLine("ahead" + position.X / position.Z);
}
if (position.Z >= 0.85 && position.Z < 1.15)
{
flag = 2;
System.Console.WriteLine("stop");
}
if (position.Z < 0.85)
{
flag = 3;
System.Console.WriteLine("back");
}
}
if (position.X / position.Z < -0.6)
{
if (position.Z >= 1.15)
{
flag = 13;
System.Console.WriteLine("ahead and turn left" + position.X / position.Z);
}
if (position.Z >= 0.85 && position.Z < 1.15)
{
flag = 2;
System.Console.WriteLine("stop");
}
if (position.Z < 0.85)
{
flag = 3;
System.Console.WriteLine("back");
}
}
}
}
this.DrawBody(joints, jointPoints, dc, drawPen);
this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
this.DrawHand(body.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));
}
}
}
// BodyFrameReader event handler
void bodyFrameReader_FrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
{
if (bodyFrame == null)
{
return;
}
if (bodies == null)
{
bodies = new Body[bodyFrame.BodyCount];
}
// Updates body data
bodyFrame.GetAndRefreshBodyData(bodies);
foreach (var body in bodies)
{
if (body.IsTracked)
{
var joints = body.Joints;
// Sets up tracking for left and right hands
var handRight = joints[JointType.HandRight];
var handLeft = joints[JointType.HandLeft];
if (handRight.TrackingState == TrackingState.Tracked && handLeft.TrackingState == TrackingState.Tracked)
{
// Updates the onscreen labels with current data
handLx.Content = handLeft.Position.X;
handLy.Content = handLeft.Position.Y;
handLz.Content = handLeft.Position.Z;
handRx.Content = handRight.Position.X;
handRy.Content = handRight.Position.Y;
handRz.Content = handRight.Position.Z;
//pitch increases as right hand moves up, volume increases as left hand moves up
if (handRight.Position.Y <= 1.5)
{
currentPitch = handLeft.Position.Y / (1 / 6) * -1 + pitchReference;
}
else if (handRight.Position.Y > 1.5)
{
currentPitch = handLeft.Position.Y / (1 / 6) + pitchReference;
}
currentPitch = pitchReference + handRight.Position.Y;
osc.Frequency = freqReference * Math.Pow(freqRatio, currentPitch - pitchReference);
osc.Amplitude = handLeft.Position.Y / .05 * 2716;
//frequency output on screen in label
freqLabel.Content = osc.Frequency;
//plays note only if kinect has sensed a body
if (waveOut != null)
{
waveOut.Play();
}
using (var canvas = bodyHighlight.Open())
{
canvas.DrawRectangle(Brushes.Transparent, null, new Rect(0, 0, Width, Height));
// Place a circle around the user's left hand
canvas.DrawEllipse(Brushes.Red, null, new Point(handLeft.Position.X, handLeft.Position.Y), 20, 20);
// Place a circle around the user's right hand
canvas.DrawEllipse(Brushes.Blue, null, new Point(handRight.Position.X, handRight.Position.Y), 20, 20);
// Pushes bodyHighlight to the window
BodyOverlay.Source = new DrawingImage(bodyHighlight);
}
}
}
}
}
}
/// <summary>
/// Kinect が複数種類のフレームを取得したとき実行されるメソッド(イベントハンドラ)。
/// </summary>
/// <param name="sender">
/// イベントを通知したオブジェクト。ここでは Kinect になる。
/// </param>
/// <param name="e">
/// イベントの発生時に渡されるデータ。
/// </param>
void MultiSourceFrameReader_MultiSourceFrameArrived
(object sender, MultiSourceFrameArrivedEventArgs e)
{
MultiSourceFrame frames = this.multiSourceFrameReader.AcquireLatestFrame();
if (frames == null)
{
return;
}
ColorFrame colorFrame = frames.ColorFrameReference.AcquireFrame();
if (colorFrame == null)
{
return;
}
BodyFrame bodyFrame = frames.BodyFrameReference.AcquireFrame();
if (bodyFrame == null)
{
colorFrame.Dispose();
return;
}
//キャンバスをクリアして更新する。
this.canvas.Background
= new ImageBrush(GetBitmapSource(colorFrame, colorFrameDescription));
this.canvas.Children.Clear();
Body[] bodies = new Body[bodyFrame.BodyCount];
bodyFrame.GetAndRefreshBodyData(bodies);
foreach (Body body in bodies)
{
if (body.IsTracked == false)
{
continue;
}
//描く関節を可視化する。
IReadOnlyDictionary <JointType, Joint> joints = body.Joints;
foreach (KeyValuePair <JointType, Joint> joint in joints)
//foreach (var joint in joints) // こちらでも可。
{
if (joint.Value.TrackingState == TrackingState.Tracked)
{
DrawJointEllipseInColorSpace(joint.Value, 10, Colors.Aqua);
}
else if (joint.Value.TrackingState == TrackingState.Inferred)
{
DrawJointEllipseInColorSpace(joint.Value, 10, Colors.Yellow);
}
}
//左手の状態を可視化する。
switch (body.HandLeftState)
{
//閉じてる(グー)。
case HandState.Closed:
{
DrawJointEllipseInColorSpace(joints[JointType.HandLeft], 20, Colors.Blue);
break;
}
//チョキ(実際には精度の都合上、指一本でも反応するが)
case HandState.Lasso:
{
DrawJointEllipseInColorSpace(joints[JointType.HandLeft], 20, Colors.Green);
break;
}
//開いている(パー)。
case HandState.Open:
{
DrawJointEllipseInColorSpace(joints[JointType.HandLeft], 20, Colors.Red);
break;
}
}
}
colorFrame.Dispose();
bodyFrame.Dispose();
}
public void Dispose()
{
ColorFrame?.Dispose();
BodyFrame?.Dispose();
DepthFrame?.Dispose();
}
/// <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);
}
this.DrawBody(joints, jointPoints, dc, drawPen);
this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
soundState.addPoint(JointType.HandLeft, jointPoints[JointType.HandLeft]);
soundState.addPoint(JointType.HandRight, jointPoints[JointType.HandRight]);
//System.Console.WriteLine("left hand x:{0} right hand x: {1}, diff: {2}",
// jointPoints[JointType.HandLeft].X,
// jointPoints[JointType.HandRight].X,
// jointPoints[JointType.HandRight].X - jointPoints[JointType.HandLeft].X);
// play sound dongdong
if (jointPoints[JointType.HandRight].X - jointPoints[JointType.HandLeft].X <SOUND_TRIGGER_DISTANCE
&&
jointPoints[JointType.HandRight].Y> jointPoints[JointType.Head].Y
&&
jointPoints[JointType.HandLeft].Y > jointPoints[JointType.Head].Y
&&
soundState.is_applauding()
)
{
Play_Sound(soundPlayer_1);
//if (Play_Sound_DongDongCi() == true)
//{
// System.Console.WriteLine("Play Sound true");
//}
}
if (jointPoints[JointType.HandRight].X - jointPoints[JointType.HandLeft].X < SOUND_TRIGGER_DISTANCE
&&
jointPoints[JointType.HandRight].Y < jointPoints[JointType.Head].Y
&&
jointPoints[JointType.HandLeft].Y < jointPoints[JointType.Head].Y
&&
soundState.is_applauding()
)
{
Play_Sound(soundPlayer_2);
}
}
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
}
}
}
/// <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)
{
CameraSpacePoint position = joints[jointType].Position;
CameraSpacePoint head = joints[JointType.Head].Position;
CameraSpacePoint HandLeft = joints[JointType.HandLeft].Position;
CameraSpacePoint HandRight = joints[JointType.HandRight].Position;
CameraSpacePoint SpineBase = joints[JointType.SpineBase].Position;
CameraSpacePoint SpineMid = joints[JointType.SpineMid].Position;
CameraSpacePoint Shoulder = joints[JointType.SpineShoulder].Position;
CameraSpacePoint ElbowLeft = joints[JointType.ElbowLeft].Position;
CameraSpacePoint ElbowRight = joints[JointType.ElbowRight].Position;
CameraSpacePoint KneeLeft = joints[JointType.KneeLeft].Position;
CameraSpacePoint KneeRight = joints[JointType.KneeRight].Position;
CameraSpacePoint FootLeft = joints[JointType.FootLeft].Position;
// sometimes the depth(Z) of an inferred joint may show as negative
// clamp down to 0.1f to prevent coordinatemapper from returning (-Infinity, -Infinity)
if (position.Z < 0)
{
position.Z = InferredZPositionClamp;
}
DepthSpacePoint depthSpacePoint = this.coordinateMapper.MapCameraPointToDepthSpace(position);
jointPoints[jointType] = new Point(depthSpacePoint.X, depthSpacePoint.Y);
p_head = Math.Round(head.Y, 2);
p_lhand = Math.Round(HandLeft.Y, 2);
p_rhand = Math.Round(HandLeft.Y, 2);
p_base_spine = Math.Round(SpineBase.Y, 2);
p_mid_base = Math.Round(SpineMid.Y, 2);
p_shoulder = Math.Round(Shoulder.Y);
p_lelbow = Math.Round(ElbowLeft.Y, 2);
p_relbow = Math.Round(ElbowRight.Y, 2);
p_lknee = Math.Round(KneeLeft.Y, 2);
p_rknee = Math.Round(KneeRight.Y, 2);
p_lfoot = Math.Round(FootLeft.Y, 2);
x_head = Math.Round(head.X, 2);
x_lknee = Math.Round(head.X, 2);
x_lfoot = Math.Round(KneeLeft.X, 2);
x_rfoot = Math.Round(FootLeft.X, 2);
start_lhand = p_lhand;
start_rhand = p_rhand;
}
this.DrawBody(joints, jointPoints, dc, drawPen);
this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
distance_root_knee = p_base_spine - p_lknee; //dystans miedzy kolanem a poczatkiem kregoslopa
double distance_root_hand = p_lhand - p_lknee; //dystand lewe kolano lewa reka
while (jumping_jack_done && distance_root_hand < 0.2)
{
counter_jumping_jack++;
jumping_jack_done = false;
JumpinJacksOutput.Text = counter_jumping_jack.ToString();
break;
}
while (distance_root_hand < 0.2 && !under_root)
{
under_root = true;
under_return_root = false;
}
while (p_lhand > p_shoulder && p_rhand > p_shoulder && under_root && !shoulder_height)
{
shoulder_height = true;
System.Console.WriteLine("Rece ponad barkami" + distance_root_hand);
}
while (p_lhand > p_head && p_rhand > p_head && shoulder_height && !pac)
{
pac = true;
System.Console.WriteLine("Rece nad glowa");
}
while (p_head < p_rhand && p_head < p_lhand && pac && !under_return_root)
{
under_return_root = true;
System.Console.WriteLine("Rece pod rootem -> returns");
}
while (under_return_root && !jumping_jack_done)
{
under_root = false;
shoulder_height = false;
pac = false;
under_return_root = false;
jumping_jack_done = true;
}
//skretosklony================================================================================
distance_foot = x_rfoot - x_lfoot;
double rece_ramiona = p_lhand - p_shoulder;
while (skret_done && p_lhand < p_mid_base && p_lhand > p_lknee)
{
counter_skret_done++;
skret_done = false;
skretOut.Text = counter_skret_done.ToString();
break;
System.Console.WriteLine("skretosklon");
}
while (rece_ramiona < 0.2 && !position_skret)
{
position_skret = true;
return_position_skret = false;
System.Console.WriteLine("aaaaaaaa");
}
while ((p_lhand < p_lknee && !half_skret) || (p_rhand < p_lknee && !half_skret) && position_skret)
{
half_skret = true;
System.Console.WriteLine("bbbbbb2");
}
while ((p_lhand > p_head || p_rhand > p_head) && !hand_on_ground)
{
hand_on_ground = true;
System.Console.WriteLine("ccccccccc");
}
rece_ramiona = p_lhand - p_shoulder;
while (p_head > p_mid_base && hand_on_ground && !return_position_skret && rece_ramiona < 0.2)
{
return_position_skret = true;
position_skret = false;
System.Console.WriteLine("ddddddd");
}
while (!position_skret && return_position_skret)
{
position_skret = false;
return_position_skret = false;
hand_on_ground = false;
half_skret = false;
System.Console.WriteLine("eeeee");
skret_done = true;
}
//=======================================================================================================
//////////////////////////////////////
//150 na zapisywanie
// sklony ====================================================================
distance_foot = x_rfoot - x_lfoot;
while (slope_done && p_head < p_lhand)
{
counter_slope_done += 1;
sklonyOut.Text = counter_slope_done.ToString();
slope_done = false;
System.Console.WriteLine("skretosklon");
break;
}
while (p_lhand - p_shoulder < 0.2 && !position_slope)
{
position_slope = true;
return_position_slope = false;
System.Console.WriteLine("111111111111111");
}
while (p_lhand < p_lknee && !half_slope)
{
half_slope = true;
System.Console.WriteLine("222222222");
}
while (p_rhand - p_lfoot < 0.2 && p_lhand - p_lfoot < 0.2 && !hands_on_ground && half_slope)
{
hands_on_ground = true;
System.Console.WriteLine("333333");
}
while (p_head > p_mid_base && hands_on_ground && !return_position_slope)
{
return_position_slope = true;
position_slope = false;
System.Console.WriteLine("44444444");
}
while (!position_slope && return_position_slope)
{
position_slope = false;
return_position_slope = false;
hands_on_ground = false;
half_slope = false;
System.Console.WriteLine("5555555");
slope_done = true;
}
////////////////////
}
}
double root_move = p_base_spine - p_lknee;// dystans miedzy poczatkiem kregoslupa a kolanem
while (p_lelbow > p_mid_base && sit_up_done)
{
counter_sit_up++;
SitUps.Text = counter_sit_up.ToString();
sit_up_done = false;
}
if (p_lhand == p_head && p_rhand == p_head && root_move > 0.2 && root_goes_down == false)
{
hands_on_head = true;
}
if (root_move < 0.1)
{
root_goes_down = true;//przykucykuc
}
if (root_move > 0.2 && root_goes_down == true)
{
sit_up_done = true;
root_goes_down = false;
}
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
//===========================sklonyOut,brzuszkiOut====================================================
}
}
}
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(body.HandRightState, 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));
}
}
}
/// <summary>
/// Handles the body frame data arriving from the sensor and updates the associated gesture detector object for each body
/// </summary>
/// <param name="sender">object sending the event</param>
/// <param name="e">event arguments</param>
private void Reader_BodyFrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
bool dataReceived = false;
using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
if (this.bodies == null)
{
// creates an array of 6 bodies, which is the max number of bodies that Kinect can track simultaneously
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[this.activeBodyIndex].IsTracked)
{
// we lost tracking of the active body, so update to the first tracked body in the array
int bodyIndex = this.GetActiveBodyIndex();
if (bodyIndex >= 0)
{
this.activeBodyIndex = bodyIndex;
}
}
dataReceived = true;
}
}
if (dataReceived)
{
// visualize the new body data
this.kinectBodyView.UpdateBodyFrame(this.bodies);
sagittalAngle.Text = kinectBodyView.kinectFeedback.sagittalAngleTxt;
flexAngle.Text = kinectBodyView.kinectFeedback.flexAngleTxt;
zeroBool.Text = kinectBodyView.kinectFeedback.isZero;
thirtyBool.Text = kinectBodyView.kinectFeedback.isThirty;
isFullFlex.Text = kinectBodyView.kinectFeedback.isFlex;
// we may have lost/acquired bodies, so update the corresponding gesture detectors
if (this.bodies != null)
{
// loop through all bodies to see if any of the gesture detectors need to be updated
//int maxBodies = this.kinectSensor.BodyFrameSource.BodyCount;
int maxBodies = 1;
for (int i = 0; i < maxBodies; ++i)
{
Body activeBody = this.bodies[this.activeBodyIndex];
ulong trackingId = activeBody.TrackingId;
// if the current body TrackingId changed, update the corresponding gesture detector with the new value
if (activeBody.TrackingId != this.gestureDetectorList[i].TrackingId)
{
this.gestureDetectorList[i].TrackingId = trackingId;
// if the current body is tracked, unpause its detector to get VisualGestureBuilderFrameArrived events
// if the current body is not tracked, pause its detector so we don't waste resources trying to get invalid gesture results
this.gestureDetectorList[i].IsPaused = trackingId == 0;
}
}
}
}
}
private void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
int depthWidth = 0;
int depthHeight = 0;
DepthFrame depthFrame = null;
ColorFrame colorFrame = null;
BodyIndexFrame bodyIndexFrame = null;
BodyFrame bodyFrame = null;
bool isBitmapLocked = false;
try
{
MultiSourceFrame multiSourceFrame = e.FrameReference.AcquireFrame();
depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame();
colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame();
bodyIndexFrame = multiSourceFrame.BodyIndexFrameReference.AcquireFrame();
bodyFrame = multiSourceFrame.BodyFrameReference.AcquireFrame();
if ((depthFrame == null) || (colorFrame == null) || (bodyIndexFrame == null) || (bodyFrame == null))
{
return;
}
if (this.bodies == null)
{
this.bodies = new Body[bodyFrame.BodyCount];
}
bodyFrame.GetAndRefreshBodyData(this.bodies);
bodyFrame.Dispose();
bodyFrame = null;
bool isBodyTracked = false;
foreach (Body body in this.bodies)
{
if (body.IsTracked)
{
isBodyTracked = true;
continue;
}
}
hasTrackedBodies = isBodyTracked;
if (hasTrackedBodies && !isCapturing)
{
BeginCountdown();
}
FrameDescription depthFrameDescription = depthFrame.FrameDescription;
depthWidth = depthFrameDescription.Width;
depthHeight = depthFrameDescription.Height;
using (KinectBuffer depthFrameData = depthFrame.LockImageBuffer())
{
this.coordinateMapper.MapColorFrameToDepthSpaceUsingIntPtr(
depthFrameData.UnderlyingBuffer,
depthFrameData.Size,
this.colorMappedToDepthPoints);
}
depthFrame.Dispose();
depthFrame = null;
this.liveBitmap.Lock();
isBitmapLocked = true;
colorFrame.CopyConvertedFrameDataToIntPtr(this.liveBitmap.BackBuffer, this.bitmapBackBufferSize, ColorImageFormat.Bgra);
colorFrame.Dispose();
colorFrame = null;
using (KinectBuffer bodyIndexData = bodyIndexFrame.LockImageBuffer())
{
unsafe
{
byte *bodyIndexDataPointer = (byte *)bodyIndexData.UnderlyingBuffer;
int colorMappedToDepthPointCount = this.colorMappedToDepthPoints.Length;
fixed(DepthSpacePoint *colorMappedToDepthPointsPointer = this.colorMappedToDepthPoints)
{
uint *bitmapPixelsPointer = (uint *)this.liveBitmap.BackBuffer;
for (int colorIndex = 0; colorIndex < colorMappedToDepthPointCount; ++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;
if (bodyIndexDataPointer[depthIndex] != 0xff)
{
continue;
}
}
}
bitmapPixelsPointer[colorIndex] = 0;
}
}
this.liveBitmap.AddDirtyRect(new Int32Rect(0, 0, this.liveBitmap.PixelWidth, this.liveBitmap.PixelHeight));
}
}
}
finally
{
if (isBitmapLocked)
{
this.liveBitmap.Unlock();
}
if (depthFrame != null)
{
depthFrame.Dispose();
}
if (colorFrame != null)
{
colorFrame.Dispose();
}
if (bodyIndexFrame != null)
{
bodyIndexFrame.Dispose();
}
if (bodyFrame != null)
{
bodyFrame.Dispose();
}
}
}
/// <summary>
/// Handles the body frame data arriving from the sensor and updates the associated gesture detector object for each body
/// </summary>
/// <param name="sender">object sending the event</param>
/// <param name="e">event arguments</param>
private void Reader_BodyFrameArrived(object sender, BodyFrameArrivedEventArgs e)
{
bool dataReceived = false;
using (BodyFrame bodyFrame = e.FrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
if (this.bodies == null)
{
// creates an array of 6 bodies, which is the max number of bodies that Kinect can track simultaneously
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);
foreach (var body in bodies)
{
if (body != null)
{
if (body.IsTracked)
{
Joint handRight = body.Joints[JointType.HandRight];
CameraSpacePoint skeletonPoint = handRight.Position;
DepthSpacePoint depthPoint = this.kinectSensor.CoordinateMapper.MapCameraPointToDepthSpace(skeletonPoint);
/*
* this.Dispatcher.Invoke(() =>
* {
* cursor.Flip(handRight);
* cursor.Update(depthPoint);
* });*/
}
}
}
dataReceived = true;
}
else
{
Console.WriteLine("bodyFrame is null");
}
}
if (dataReceived)
{
// visualize the new body data
this.kinectBodyView.UpdateBodyFrame(this.bodies);
// we may have lost/acquired bodies, so update the corresponding gesture detectors
if (this.bodies != null)
{
// loop through all bodies to see if any of the gesture detectors need to be updated
int maxBodies = 1; // this.kinectSensor.BodyFrameSource.BodyCount;
for (int i = 0; i < maxBodies; ++i)
{
Body body = this.bodies[i];
ulong trackingId = body.TrackingId;
// if the current body TrackingId changed, update the corresponding gesture detector with the new value
if (trackingId != this.gestureDetectorList[i].TrackingId)
{
this.gestureDetectorList[i].TrackingId = trackingId;
// if the current body is tracked, unpause its detector to get VisualGestureBuilderFrameArrived events
// if the current body is not tracked, pause its detector so we don't waste resources trying to get invalid gesture results
this.gestureDetectorList[i].IsPaused = trackingId == 0;
}
}
}
}
}
private void OnFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
double utcTime = (DateTime.UtcNow - new DateTime(1970, 1, 1)).TotalSeconds;
MultiSourceFrame multiSourceFrame = e.FrameReference.AcquireFrame();
using (ColorFrame colorFrame = multiSourceFrame.ColorFrameReference.AcquireFrame())
{
if (colorFrame != null)
{
colorFrame.CopyConvertedFrameDataToArray(this.colorArray, ColorImageFormat.Bgra);
System.Buffer.BlockCopy(this.colorArray, 0, this.byteColorArray, 0, (this.kinect.ColorFrameSource.FrameDescription.Height * this.kinect.ColorFrameSource.FrameDescription.Width * BYTES_PER_COLOR_PIXEL));
System.Buffer.BlockCopy(BitConverter.GetBytes(utcTime), 0, this.byteColorArray, (this.kinect.ColorFrameSource.FrameDescription.Height * this.kinect.ColorFrameSource.FrameDescription.Width * BYTES_PER_COLOR_PIXEL), sizeof(double));
this.colorConnector.Broadcast(this.byteColorArray);
}
}
using (DepthFrame depthFrame = multiSourceFrame.DepthFrameReference.AcquireFrame())
{
if (depthFrame != null)
{
depthFrame.CopyFrameDataToArray(this.depthArray);
System.Buffer.BlockCopy(this.depthArray, 0, this.byteDepthArray, 0, this.kinect.DepthFrameSource.FrameDescription.Height * this.kinect.DepthFrameSource.FrameDescription.Width * BYTES_PER_DEPTH_PIXEL);
System.Buffer.BlockCopy(BitConverter.GetBytes(utcTime), 0, this.byteDepthArray, this.kinect.DepthFrameSource.FrameDescription.Height * this.kinect.DepthFrameSource.FrameDescription.Width * BYTES_PER_DEPTH_PIXEL, sizeof(double));
this.depthConnector.Broadcast(this.byteDepthArray);
}
}
using (InfraredFrame irFrame = multiSourceFrame.InfraredFrameReference.AcquireFrame())
{
if (irFrame != null)
{
irFrame.CopyFrameDataToArray(this.irArray);
System.Buffer.BlockCopy(this.irArray, 0, this.byteIRArray, 0, this.kinect.InfraredFrameSource.FrameDescription.Height * this.kinect.InfraredFrameSource.FrameDescription.Width * BYTES_PER_IR_PIXEL);
System.Buffer.BlockCopy(BitConverter.GetBytes(utcTime), 0, this.byteIRArray, this.kinect.InfraredFrameSource.FrameDescription.Height * this.kinect.InfraredFrameSource.FrameDescription.Width * BYTES_PER_IR_PIXEL, sizeof(double));
this.irConnector.Broadcast(this.byteIRArray);
}
}
using (BodyFrame bodyFrame = multiSourceFrame.BodyFrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
bodyFrame.GetAndRefreshBodyData(this.bodyArray);
/*//Console.WriteLine("New bodies");
* foreach (Body b in this.bodyArray)
* {
* //Console.WriteLine(b.TrackingId);
* //Console.WriteLine(b.IsTracked);
* }*/
for (int i = 0; i < this.bodyArray.Length; i++)
{
this.filter.UpdateFilter(this.bodyArray[i]);
}
string jsonString = JsonConvert.SerializeObject(this.bodyArray);
int diff = 40000 - jsonString.Length;
/*for (int i = 0; i < diff;i++ )
* {
* jsonString += " ";
* }*/
byte[] bodyByteArray = new byte[jsonString.Length * sizeof(char) + sizeof(double) + 4]; //modification start
int sizeofString = jsonString.Length * sizeof(char) + sizeof(double);
byte[] sizeByte = BitConverter.GetBytes(sizeofString);
System.Buffer.BlockCopy(sizeByte, 0, bodyByteArray, 0, 4);
System.Buffer.BlockCopy(jsonString.ToCharArray(), 0, bodyByteArray, 4, jsonString.Length * sizeof(char));
System.Buffer.BlockCopy(BitConverter.GetBytes(utcTime), 0, bodyByteArray, 4 + jsonString.Length * sizeof(char), sizeof(double));
this.bodyConnector.Broadcast(bodyByteArray);
}
}
}
public static extern int GetBodyFrameData(ref BodyFrame pBodyFrame, bool bGetOrientations, bool bGetHandStates);
/// <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)
{
//The following function is commented out to reduce load on the machine
//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);
}
// Added parameter: StreamWriter sw
this.DrawBody(joints, jointPoints, dc, drawPen);
///this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
///this.DrawHand(body.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));
}
}
}
/// <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)
{
try
{
using (DrawingContext dc = this.drawingGroup.Open())
{
// 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;
var body = bodies[0];
foreach (Body thebody in this.bodies)
{
Pen drawPen = this.bodyColors[penIndex++];
if (thebody.IsTracked)
{
this.DrawBody(thebody, dc, drawPen);
if (body.Joints[JointType.SpineBase].Position.Z == 0)
{
body = thebody;
}
else if (body.Joints[JointType.SpineBase].Position.Z > thebody.Joints[JointType.SpineBase].Position.Z)
{
body = thebody;
}
}
}
Pen drawPenInside = new Pen(Brushes.LightBlue, 11);
Pen drawPenOutside = new Pen(Brushes.DarkBlue, 11);
this.DrawClippedEdges(body, dc);
if (outside)
{
this.DrawBody(body, dc, drawPenOutside);
}
else
{
this.DrawBody(body, dc, drawPenInside);
#region PPT control
CameraSpacePoint head = getCameraSpacePoint(body, JointType.Head);
CameraSpacePoint handL = getCameraSpacePoint(body, JointType.HandLeft);
CameraSpacePoint handR = getCameraSpacePoint(body, JointType.HandRight);
CameraSpacePoint spineBase = getCameraSpacePoint(body, JointType.SpineBase);
CameraSpacePoint shoulderR = getCameraSpacePoint(body, JointType.ShoulderRight);
CameraSpacePoint shoulderL = getCameraSpacePoint(body, JointType.ShoulderLeft);
if (hiding)
{
state.Content = "";
state.Background = Brushes.Transparent;
}
else
{
state.Content = mode[State];
state.Background = Brushes.Black;
}
if (handL.Y < head.Y)
{
State = LOCKED;
}
if (State == LOCKED)
{
}
else if (spineBase.Z - handL.Z > LIFT && spineBase.Z - handR.Z < LIFT && handR.X - spineBase.X > 150)
{
exit();
}
else if (spineBase.Z - handL.Z > LIFT && spineBase.Z - handR.Z > LIFT)
{
if (body.HandRightState == HandState.Open && body.HandLeftState == HandState.Open)
{
State = TOESC;
}
if (body.HandRightState == HandState.Open && body.HandLeftState == HandState.Closed)
{
State = SELECTING; itab = 0;
}
if (body.HandRightState == HandState.Closed && body.HandLeftState == HandState.Open)
{
State = TOPLAY;
}
if (body.HandRightState == HandState.Closed && body.HandLeftState == HandState.Closed)
{
State = TOSTART;
}
if (body.HandRightState == HandState.Open && body.HandLeftState == HandState.Lasso)
{
State = TOHIDE;
}
}
switch (State)
{
case LOCKED:
if (handR.Y < head.Y)
{
State = CONTROLLING;
}
break;
case TOHIDE:
if (spineBase.Z - handL.Z < LIFT && spineBase.Z - handR.Z < LIFT)
{
hiding = !hiding;
State = CONTROLLING;
}
break;
case TOSTART:
if (spineBase.Z - handL.Z < LIFT && spineBase.Z - handR.Z < LIFT)
{
SendKeys.SendWait("{F5}");
State = CONTROLLING;
}
break;
case TOPLAY:
if (spineBase.Z - handL.Z < LIFT && spineBase.Z - handR.Z < LIFT)
{
SendKeys.SendWait("{Tab}");
SendKeys.SendWait("{Enter}");
State = LOCKED;
}
break;
case TOESC:
if (spineBase.Z - handL.Z < LIFT && spineBase.Z - handR.Z < LIFT)
{
SendKeys.SendWait("{Esc}");
State = LOCKED;
}
break;
case CONTROLLING:
if (spineBase.Z - handL.Z > LIFT && spineBase.Z - handR.Z < LIFT) // if left hand lift forward only
{
if (handL.X < shoulderL.X)
{
prevpage = true;
}
else
{
if (prevpage)
{
SendKeys.SendWait("{Left}");
prevpage = false;
}
}
}
else if (spineBase.Z - handR.Z > LIFT && spineBase.Z - handL.Z < LIFT) // if right hand lift forward only
{
if (handR.X > shoulderR.X)
{
nextpage = true;
}
else
{
if (nextpage)
{
SendKeys.SendWait("{Right}");
nextpage = false;
}
}
}
break;
case SELECTING:
if (itab != 0)
{
state.Content += "(" + itab.ToString() + ")";
}
if (spineBase.Z - handL.Z < LIFT && spineBase.Z - handR.Z < LIFT)
{
SendKeys.SendWait("{Enter}");
State = CONTROLLING;
}
else if (spineBase.Z - handL.Z > LIFT && spineBase.Z - handR.Z < LIFT)
{
if (handL.X < shoulderL.X)
{
prevpage = true;
}
else
{
if (prevpage)
{
SendKeys.SendWait("+{Tab}");
itab--;
prevpage = false;
}
}
}
else if (spineBase.Z - handR.Z > LIFT && spineBase.Z - handL.Z < LIFT)
{
if (handR.X > shoulderR.X)
{
nextpage = true;
}
else
{
if (nextpage)
{
SendKeys.SendWait("{Tab}");
itab++;
nextpage = false;
}
}
}
break;
default: break;
}
#endregion
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
}
}
catch { return; }
}
}
// Reads in the bodyFrame (if it contains bodies)
private void GetBodyJoints(BodyFrame bodyFrame)
{
//Makes sure the bodyFrame contains bodies, and if so, puts the data in bodies
if(bodyFrame != null)
{
if (this.bodies == null)
{
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;
}
}
/* Analisi dei dati provenienti dal sensore, identificazione utente e calibrazione area di gioco
* area di gioco identificata a partire dalla posizione iniziale dell'utente
*/
void reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
MultiSourceFrame frame_reference = e.FrameReference.AcquireFrame();
/*Prelevo frame RGB*/
using (ColorFrame frame = frame_reference.ColorFrameReference.AcquireFrame())
{
if (frame != null)
{
this._currentCFrame = frame.ToBitmap();
}
}
/*Elaborazione informazioni body Frame*/
using (BodyFrame bodyFrame = frame_reference.BodyFrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
if (bodies == null)
{
/* Alloco un vettore di dimensioni pari al numero di persone identificate
* all'avvio del sensore
*/
this.bodies = new Body[bodyFrame.BodyCount];
}
/* Aggiorno le informazioni sui corpi identificati dalla kinect */
bodyFrame.GetAndRefreshBodyData(this.bodies);
#region Player Lost
/* Caso di Player Perso
* Potrebbe capitare che il player esca dall'area tracciata dalla kinect, in tal caso va notificato all'utente
* di rientrare nell'area di gioco ed alzare il braccio destro o sinistro per essere reintrodotto nel sistema.
* Il caso di player perso è identificato dalla variabile _playerLost==True e _playerIsRecognizer==True;
* */
//Verifica se tra i corpi tracciati dal sensore è presente il player, verifica effettuata tramite TrackingID
if (this._playerIsRecognized)
{
this._playerLost = true;
foreach (Body body in this.bodies)
{
if (body.IsTracked && body.TrackingId == this._playerIdentifier)
{
this._playerLost = false;
break;
}
}
}
//se l'utente è uscito dal campo visivo della kinect
if (this._playerLost)
{
//Se è stato già lanciato l'evento effettuo analisi per identificare di nuovo il player
if (this._playerLost_Launched)
{
foreach (Body body in this.bodies)
{
if (body.IsTracked)
{
//Se utente alza una delle due mani sopra la spalla viene identificato come Player
if (body.Joints[JointType.HandRight].Position.Y > body.Joints[JointType.Head].Position.Y ||
body.Joints[JointType.HandLeft].Position.Y > body.Joints[JointType.Head].Position.Y)
{
this._playerIdentifier = body.TrackingId;
this._playerLost_Launched = false;
this.OnPlayerLocked();
}
}
}
}
//Se l'evento di player perso non è stato lanciato, viene lanciato e termina funzione
else
{
this.OnPlayerLost();
}
//Terminate le operazioni di Player Perso termino notifico aggiornamento dati e ritorno
this.OnDataUpdated();
return;
}
#endregion
//Player non identificato
if (!this._playerIsRecognized)
{
#region PlayerIdentification
foreach (Body body in this.bodies)
{
if (body.IsTracked)
{
//Se utente alza entrambe le mani all'altezza delle spalle
if (body.Joints[JointType.HandRight].Position.Y > body.Joints[JointType.ElbowRight].Position.Y &&
body.Joints[JointType.HandLeft].Position.Y > body.Joints[JointType.ElbowLeft].Position.Y)
{
this._currentPlayerPosition.X = body.Joints[_trackedJoint].Position.X;
this._currentPlayerPosition.Y = body.Joints[_trackedJoint].Position.Y;
this._currentPlayerPosition.Z = body.Joints[_trackedJoint].Position.Z;
//Salvo la posizione iniziale dell'utente, utile in fase di calibrazione
this._startingPlayerPosition = this._currentPlayerPosition;
//Converto coordinate sensore in coordinate RGB
this._playerIdColorPoint = kinect.CoordinateMapper.MapCameraPointToColorSpace(body.Joints[_trackedJoint].Position);
this._playerIdColorPoint3D.X = _playerIdColorPoint.X;
this._playerIdColorPoint3D.Y = _playerIdColorPoint.Y;
//Il body è identificato come player, salvo il trackingID
this._playerIdentifier = body.TrackingId;
this._playerIsRecognized = true;
//Notifico cambio di stato del player
this.OnPlayerLocked();
//Esco dal ciclo
break;
}
}
}
#endregion PlayerIdentification
}
//Player identificato
else
{
#region Movement Analysis
foreach (Body body in bodies)
{
//Se il corpo è tracciato ed è quello del player allora memorizzo la posizione
if (body.IsTracked && body.TrackingId == this._playerIdentifier)
{
this._currentPlayerPosition.X = body.Joints[_trackedJoint].Position.X;
this._currentPlayerPosition.Y = body.Joints[_trackedJoint].Position.Y;
this._currentPlayerPosition.Z = body.Joints[_trackedJoint].Position.Z;
this._playerIdColorPoint = kinect.CoordinateMapper.MapCameraPointToColorSpace(body.Joints[_trackedJoint].Position);
this._playerIdColorPoint3D.X = _playerIdColorPoint.X;
this._playerIdColorPoint3D.Y = _playerIdColorPoint.Y;
//Conversione coordinata X
this._convertedPlayerPosition.X = this._currentPlayerPosition.X - this._startingPlayerPosition.X;
this._convertedPlayerPosition.Z = this._currentPlayerPosition.Z - this._startingPlayerPosition.Z;
/*
* Normalizzo l'altezza del player a 2 metri, l'abbassamento è proporzionale
* questo scalamento risolve il problema dell'altezza del sensore kinect, che influenza l'altezza
* del personaggio nel mondo virtuale.
*/
//this._convertedPlayerPosition.Y = (float)Math.Round(((this._currentPlayerPosition.Y * 1.8) / this._startingPlayerPosition.Y), 2, MidpointRounding.AwayFromZero);
this._convertedPlayerPosition.Y = 2f + this._currentPlayerPosition.Y - this._startingPlayerPosition.Y;
//PUNTO PER INSERIRE EVENTUALI CONTROLLI DA PARTE DELL'UTENTE
//Esco dal ciclo
break;
}
}
#endregion
}
//Lancio Evento dati aggiornati
this.OnDataUpdated();
}
}
}
private void InitFrames(DepthImageFrame depthFrame, ColorImageFrame colorFrame, SkeletonFrame skeletonFrame) {
if (init) { return; } init = true;
// Color Frame
Color = new ColorFrame();
Color.Width = colorFrame.Width;
Color.Height = colorFrame.Height;
Color.Pixels = new byte[colorFrame.PixelDataLength];
Color.Stamp = new Timestamp();
Color.Fps = FPS;
AddOnManager.GetInstance().InitFrame(Name, Color);
Log(Color.ToString());
ColorFormat = colorFrame.Format;
// Depth Frame
Depth = new DepthFrame();
Depth.Width = depthFrame.Width;
Depth.Height = depthFrame.Height;
Depth.Pixelss = new short[depthFrame.PixelDataLength];
Depth.Stamp = new Timestamp();
AddOnManager.GetInstance().InitFrame(Name, Depth);
Log(Depth.ToString());
var dueTime = TimeSpan.FromMilliseconds(200);
var interval = TimeSpan.FromMilliseconds(ConfigManager.GetInstance().Find("kinect_v1.motion.ms", 100));
Task = new MotionTask(dueTime, interval);
Task.Device = "";
Task.AddFrame(Depth);
Task.Start();
// Skeleton Frame
Skeletons = new BodyFrame();
Skeletons.Width = colorFrame.Width;
Skeletons.Height = colorFrame.Height;
Skeletons.RawData = new Skeleton[6];
Skeletons.Bodies = new List<NBody>(6);
Skeletons.Stamp = new Timestamp();
AddOnManager.GetInstance().InitFrame(Name, Skeletons);
Log(Skeletons.ToString());
}
private void RefreshBodyData(BodyFrame frame) {
cache.Clear();
foreach (var skeleton in (IList<Skeleton>)frame.RawData) {
if (skeleton.TrackingState != SkeletonTrackingState.Tracked) { continue; }
NBody nbody = frame.Find(Convert.ToUInt64(skeleton.TrackingId));
if (nbody == null) {
nbody = new NBody(Convert.ToUInt64(skeleton.TrackingId), frame.Width, frame.Height);
}
cache.Add(nbody);
RefreshBodyData(skeleton, nbody);
}
lock (frame.Bodies) {
frame.Bodies.Clear();
frame.Bodies.AddRange(cache);
}
}
private void updateBodies(BodyFrame frame)
{
if (this.bodies == null)
{
this.bodies = new Body[frame.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.
frame.GetAndRefreshBodyData(this.bodies);
}
private void processBodyFrame(BodyFrame bodyFrame, bool showSkeleton, bool showHandStates)
{
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);
players[0] = null;
players[1] = null;
Body[] tempBodies = new Body[2];
int playerCount = 0;
foreach (Body body in this.bodies)
{
if (body.IsTracked && playerCount < 2)
{
tempBodies[playerCount++] = body;
}
}
if (playerCount == 2)
{
CameraSpacePoint player0Pos = tempBodies[0].Joints[JointType.SpineMid].Position;
CameraSpacePoint player1Pos = tempBodies[1].Joints[JointType.SpineMid].Position;
if (player0Pos.X < player1Pos.X)
{
players[0] = new Player(tempBodies[0]);
players[1] = new Player(tempBodies[1]);
}
else
{
players[1] = new Player(tempBodies[0]);
players[0] = new Player(tempBodies[1]);
}
}
using (DrawingContext dc = this.drawingGroup.Open())
{
// 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;
foreach (Player player in players)
{
if (player == null) continue;
Body body = player.body;
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>();
Dictionary<JointType, float> jointPointDepths = new Dictionary<JointType, float>();
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;
}
ColorSpacePoint colorSpacePoint = this.coordinateMapper.MapCameraPointToColorSpace(position);
jointPoints[jointType] = new Point(colorSpacePoint.X, colorSpacePoint.Y);
jointPointDepths[jointType] = position.Z;
}
// Maps the actual joint points to the player
player.jointPoints = jointPoints;
player.jointPointDepths = jointPointDepths;
if (showSkeleton)
{
this.DrawBody(joints, jointPoints, dc, drawPen);
}
if (showHandStates)
{
this.DrawHand(body.HandLeftState, jointPoints[JointType.HandLeft], dc);
this.DrawHand(body.HandRightState, jointPoints[JointType.HandRight], dc);
}
// THIS IS WHERE THE UPDATES GO. CAN'T MOVE IT BECAUSE OF THE GC OF DRAWING CONTEXT
/*
this.targetCircleL.update(
body.HandLeftState,
body.HandRightState,
jointPoints[JointType.HandLeft],
jointPoints[JointType.HandRight],
jointPointDepths[JointType.HandLeft],
jointPointDepths[JointType.HandLeft]
);
this.targetCircleL.draw(dc);
*/
//basketballManager.update(player[0], player[1]);
//basketballManager.draw(dc);
//processBasketballManager(players[0], players[1], dc);
}
}
if (players[0] != null && players[1] != null)
processBasketballManager(players[0], players[1], dc);
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.displayWidth, this.displayHeight));
}
bodyFrame.Dispose();
}
private void StartBodyStream() {
// Init infr buffer
BodyFrame frame = Body = new BodyFrame();
frame.Width = Color.Width;
frame.Height = Color.Height;
frame.RawData = new Body[6]; // FIXME
frame.Bodies = new List<NBody>(6);
frame.Stamp = new Timestamp();
AddOnManager.GetInstance().InitFrame(Name, frame);
Log(frame.ToString());
// Start Watch
BodyWatch = new StopwatchAvg();
}
private void Reader_MultiSourceFrameArrived(object sender, MultiSourceFrameArrivedEventArgs e)
{
WriteableBitmap depthWBitmap = null;
WriteableBitmap colorWBitmap = null;
Bitmap colorBitmap = null;
Bitmap depthBitmap = null;
MultiSourceFrame frame = e.FrameReference.AcquireFrame();
if (frame != null)
{
using (DepthFrame depthFrame = frame.DepthFrameReference.AcquireFrame())
{
if (depthFrame != null)
{
_frameParser.ParseToBitmaps(depthFrame, out depthBitmap, out depthWBitmap);
if (_selectedCamera == ECameraType.Depth)
{
if (_selectedCamera == ECameraType.Depth)
{
bodyViewer.UpdateFrameCounter();
bodyViewer.KinectImage = depthWBitmap;
}
}
}
}
if (_selectedCamera == ECameraType.Color)
{
using (ColorFrame colorFrame = frame.ColorFrameReference.AcquireFrame())
{
if (colorFrame != null)
{
_frameParser.ParseToBitmaps(colorFrame, out colorBitmap, out colorWBitmap);
if (_selectedCamera == ECameraType.Color)
{
bodyViewer.UpdateFrameCounter();
bodyViewer.KinectImage = colorWBitmap;
}
}
}
}
using (BodyFrame bodyFrame = frame.BodyFrameReference.AcquireFrame())
{
if (bodyFrame != null)
{
BodyFrameWrapper bodyFrameWrapper = new BodyFrameWrapper(bodyFrame);
if (bodyFrameWrapper.TrackedBodies.Any())
{
bodyViewer.RenderBodies(bodyFrameWrapper.TrackedBodies, _selectedCamera);
if (_recordingStarted)
{
_recordedBodyFrames.Add(bodyFrameWrapper);
if (depthWBitmap != null)
{
_depthVideoWriter.WriteVideoFrame(depthBitmap);
//writing colorframes does work but should be deactivated by default
//_colorVideoWriter.WriteVideoFrame(colorBitmap);
}
else
{
Logger.Instance.LogMessage("DepthVideoFrame missing at: " + _recordedBodyFrames.Count.ToString());
}
}
}
}
else
{
bodyViewer.DeleteUntrackedBodies(null);
}
}
}
}
void BodyFrameReady(BodyFrame frame)
{
frame.GetAndRefreshBodyData(m_bodies);
// now update the player-index-to-body mapping
for (int i = 0; i < PlayerCount; i++) {
UpdatePlayerMapping(i);
}
for (int i = 0; i < PlayerCount; i++) {
if (m_eventSinkArray[i] != null)
{
// Update the bodies with the latest pose state. This may fire events and state machine actions.
m_holofunkBodies[i].Update(this,
m_eventSinkArray[i].OnLeftHand,
m_eventSinkArray[i].OnLeftArm,
m_eventSinkArray[i].OnRightHand,
m_eventSinkArray[i].OnRightArm);
}
}
if (m_bodyFrameUpdateAction != null) {
m_bodyFrameUpdateAction(this);
}
}
// Update is called once per frame
void Update()
{
// process bodies
bool newBodyData = false;
using (BodyFrame bodyFrame = this.bodyFrameReader.AcquireLatestFrame())
{
if (bodyFrame != null)
{
bodyFrame.GetAndRefreshBodyData(this.bodies);
newBodyData = true;
}
}
if (newBodyData)
{
// update gesture detectors with the correct tracking id
minposition_z = 8;
for (int bodyIndex = 0; bodyIndex < this.bodyCount; bodyIndex++)
{
var body = this.bodies[bodyIndex];
if (body != null)
{
//var trackingId = this.bodies[choosen].TrackingId;
var trackingId = body.TrackingId;
//print("trackingID is " + trackingId);
if (body.IsTracked)
{
//print("bodyindex is " + bodyIndex);
//print("trackingidold! " + head_Zindex[bodyIndex, 1] + " BODYINDEX " + bodyIndex);
}
//print("min" + minposition_z);
//print("true or false : " + this.gestureDetectorList[bodyIndex].IsPaused);
if (trackingId != this.gestureDetectorList[bodyIndex].TrackingId)
{
print("different!!! so please change");
this.gestureDetectorList[bodyIndex].TrackingId = trackingId;
// if the current body is tracked, unpause its detector to get VisualGestureBuilderFrameArrived events
// if the current body is not tracked, pause its detector so we don't waste resources trying to get invalid gesture results
this.gestureDetectorList[bodyIndex].IsPaused = (trackingId == 0);
//this.gestureDetectorList[bodyIndex].OnGestureDetected += CreateOnGestureHandler(bodyIndex);
}
// if the current body TrackingId changed, update the corresponding gesture detector with the new value
if (trackingId == 0)
{
// this bodyIndex isn't tracked
head_Zindex[bodyIndex, 0] = bodyIndex;
head_Zindex[bodyIndex, 1] = 0;
bodyindex_id[bodyIndex, 0] = bodyIndex;
bodyindex_id[bodyIndex, 1] = 0;
}
else
//already tracked!!
{
print("tracked " + bodyIndex);
CameraSpacePoint position_head = body.Joints[JointType.Head].Position;
head_Zindex[bodyIndex, 0] = bodyIndex;
head_Zindex[bodyIndex, 1] = position_head.Z;
bodyindex_id[bodyIndex, 0] = bodyIndex;
bodyindex_id[bodyIndex, 1] = (float)trackingId;
}
}
}
for (int bodyIndex = 0; bodyIndex < this.bodyCount; bodyIndex++)
{
if (minposition_z > head_Zindex[bodyIndex, 1] && head_Zindex[bodyIndex, 1] != 0)
{
choosen = bodyIndex;
minposition_z = head_Zindex[bodyIndex, 1];
}
}
for (int bodyIndex = 0; bodyIndex < this.bodyCount; bodyIndex++)
{
if (bodyIndex != choosen)
{
this.gestureDetectorList[bodyIndex].IsPaused = true;
}
else
{
this.gestureDetectorList[bodyIndex].IsPaused = false;
}
}
//print("choosen us + " + choosen);
if (this.bodies[choosen].IsTracked)
{
CameraSpacePoint right_hand = this.bodies[choosen].Joints[JointType.HandRight].Position;
CameraSpacePoint left_hand = this.bodies[choosen].Joints[JointType.HandLeft].Position;
double x1 = Math.Round(right_hand.X, 3);
double y1 = Math.Round(right_hand.Y, 3);
double z1 = Math.Round(right_hand.Z, 3);
//print("choosen is " + choosen);
//print("right hand position: X" + x1 + " y" + y1);
G2scripts.positionX = x1;
G2scripts.positionY = y1;
G2scripts.positionZ = z1;
if (z1 <= 1.8 && z1 > 1.6)
{
if (y1 <= -0.65 && y1 > -0.83)
{
if (x1 <= -0.9 && x1 > -1.2)
{
print("A");
G2scripts.ChoosenBall = 'A';
//G2scriptsscript.ChoosenBall = 'C';
// G2scripts.positionX = x1;
G2scripts.A = true;
G2scripts.B = false;
G2scripts.C = false;
G2scripts.D = false;
G2scripts.E = false;
}
else if (x1 <= -0.43 && x1 > -0.56)
{
print("B");
G2scripts.ChoosenBall = 'B';
//G2scriptsscript.ChoosenBall = 'C';
// G2scripts.positionX = x1;
G2scripts.A = false;
G2scripts.B = true;
G2scripts.C = false;
G2scripts.D = false;
G2scripts.E = false;
}
else if (x1 <= 0.17 && x1 > -0.1)
{
print("C");
G2scripts.ChoosenBall = 'C';
//G2scriptsscript.ChoosenBall = 'C';
// G2scripts.positionX = x1;
G2scripts.A = false;
G2scripts.B = false;
G2scripts.C = true;
G2scripts.D = false;
G2scripts.E = false;
}
else if (x1 > 0.53 && x1 <= 0.75)
{
print("D");
G2scripts.ChoosenBall = 'D';
// G2scripts.positionX = x1;
G2scripts.A = false;
G2scripts.B = false;
G2scripts.C = false;
G2scripts.D = true;
G2scripts.E = false;
}
else if (x1 <= 1.25 && x1 > 1.10)
{
print("E");
G2scripts.ChoosenBall = 'E';
//G2scripts.positionX = x1;
G2scripts.A = false;
G2scripts.B = false;
G2scripts.C = false;
G2scripts.D = false;
G2scripts.E = true;
}
else
{
G2scripts.A = false;
G2scripts.B = false;
G2scripts.C = false;
G2scripts.D = false;
G2scripts.E = false;
print("NON");
}
}
}
else
{
G2scripts.A = false;
G2scripts.B = false;
G2scripts.C = false;
G2scripts.D = false;
G2scripts.E = false;
print("NON");
}
}
}
/*
* if (trackingId != this.gestureDetectorList[bodyIndex].TrackingId)
* {
* GestureTextGameObject.text = "none";
* //this.bodyText[bodyIndex] = "none";
* this.gestureDetectorList[bodyIndex].TrackingId = trackingId;
*
* // if the current body is tracked, unpause its detector to get VisualGestureBuilderFrameArrived events
* // if the current body is not tracked, pause its detector so we don't waste resources trying to get invalid gesture results
* this.gestureDetectorList[bodyIndex].IsPaused = (trackingId == 0);
* this.gestureDetectorList[bodyIndex].OnGestureDetected += CreateOnGestureHandler(bodyIndex);
* }
* }
*/
/*
*
* if(body.IsTracked)
* {
* CameraSpacePoint position_head = body.Joints[JointType.Head].Position;
* print("tracked" + bodyIndex);
* //print("gesturedetector.body is " + gestureDetectorList[bodyIndex].TrackingId);
* //print("position_head" + position_head.Z);
* }
*
*/
}
private void ShowBodyJoints(BodyFrame bodyFrame)
{
Body[] bodies = new Body[this.kinectSensor.BodyFrameSource.BodyCount];
bool dataReceived = false;
if (bodyFrame != null)
{
bodyFrame.GetAndRefreshBodyData(bodies);
dataReceived = true;
}
if (dataReceived)
{
this.bodiesManager.UpdateBodiesAndEdges(bodies);
}
}
private void RegisterGesture(BodyFrame bodyFrame)
{
bool dataReceived = false;
Body[] bodies = null;
if (bodyFrame != null)
{
if (bodies == null)
{
bodies = new Body[bodyFrame.BodyCount];
}
bodyFrame.GetAndRefreshBodyData(bodies);
dataReceived = true;
}
if (dataReceived)
{
if (bodies != null)
{
for (int i = 0; i < bodyFrame.BodyCount; i++)
{
Body body = bodies[i];
ulong trackingId = body.TrackingId;
if (trackingId != gestureDetectorList[i].TrackingId)
{
gestureDetectorList[i].TrackingId = trackingId;
gestureDetectorList[i].IsPaused = trackingId == 0;
}
}
}
}
}