static void Main(string[] args)
{
JointType[] points = { JointType.ShoulderCenter, JointType.Head };
String filename = "C:\\Temp\\Winken.csv";
int pretimer = 100;
int timer = 300;
GetKinect();
Dev.Start();
Console.WriteLine(Dev.IsRunning);
Dev.ElevationAngle = 5;
Dev.SkeletonStream.Enable();
StreamWriter writeFile = new StreamWriter(new FileStream(filename, FileMode.OpenOrCreate, FileAccess.Write));
foreach (JointType jt in points)
{
writeFile.Write(jt.ToString() + ".X;" + jt.ToString() + ".Y;" + jt.ToString() + ".Z;");
}
writeFile.WriteLine();
for (int i = pretimer; i > 0; i--)
{
SkeletonFrame Frm = Dev.SkeletonStream.OpenNextFrame(100);
if (!(Frm == null))
{
Skeleton[] skeletons = new Skeleton[6];
Frm.CopySkeletonDataTo(skeletons);
Skeleton ske = skeletons.FirstOrDefault(x => x.TrackingState == SkeletonTrackingState.Tracked);
if (!(ske == null))
{
Console.ForegroundColor = ConsoleColor.Green;
Console.WriteLine("Starting in " + i);
}
else
{
Console.ForegroundColor = ConsoleColor.Red;
Console.WriteLine("Starting in " + i);
}
System.Threading.Thread.Sleep(33);
Frm.Dispose();
}
}
for (int i = timer; i > 0; i--)
{
SkeletonFrame Frm = Dev.SkeletonStream.OpenNextFrame(50);
if (!(Frm == null))
{
Skeleton[] skeletons = new Skeleton[6];
Frm.CopySkeletonDataTo(skeletons);
Skeleton ske = skeletons.FirstOrDefault(x => x.TrackingState == SkeletonTrackingState.Tracked);
if (!(ske == null))
{
foreach (JointType jt in points)
{
writeFile.Write(ske.Joints[jt].Position.X + ";" + ske.Joints[jt].Position.Y + ";" +
ske.Joints[jt].Position.Z + ";");
}
writeFile.WriteLine();
Console.ForegroundColor = ConsoleColor.Green;
Console.WriteLine("Skeleton recogniced");
}
else
{
Console.ForegroundColor = ConsoleColor.Red;
Console.WriteLine("Not recogniced!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
}
System.Threading.Thread.Sleep(33);
Frm.Dispose();
}
}
writeFile.Close();
Console.WriteLine("File saved to: " + filename);
Console.Read();
}
private void miKinect_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
Skeleton[] skeletons = new Skeleton[0];
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
}
}
if (skeletons.Length != 0)
{
string json = "{\"Skeleton\":[";
foreach (Skeleton skel in skeletons)
{
if (skel.TrackingState == SkeletonTrackingState.Tracked)
{
json += "{\"Id\":" + skel.TrackingId + ",";
foreach (Joint joint in skel.Joints)
{
switch (joint.JointType)
{
case JointType.Head:
//Cabeza
var heanp = joint.Position;
json += "\"Cabeza\":" + oSerializer.Serialize(heanp) + ",";
break;
case JointType.ShoulderCenter:
//Cuello
heanp = joint.Position;
json += "\"Cuello\":" + oSerializer.Serialize(heanp) + ",";
break;
case JointType.ShoulderLeft:
//Hombro izquierdo
heanp = joint.Position;
json += "\"HombroIzq\":" + oSerializer.Serialize(heanp) + ",";
break;
case JointType.ShoulderRight:
//Hombro derecho
heanp = joint.Position;
json += "\"HombroDer\":" + oSerializer.Serialize(heanp) + ",";
//distancia += heanp.Z;
break;
case JointType.ElbowLeft:
//Codo Izquierdo
heanp = joint.Position;
json += "\"CodoIzq\":" + oSerializer.Serialize(heanp) + ",";
break;
case JointType.ElbowRight:
//Codo derecho
heanp = joint.Position;
json += "\"CodoDer\":" + oSerializer.Serialize(heanp) + ",";
break;
case JointType.WristLeft:
//Muñeca Izquierda
heanp = joint.Position;
json += "\"MuñecaIzq\":" + oSerializer.Serialize(heanp) + ",";
break;
case JointType.WristRight:
//Muñeca Derecha
heanp = joint.Position;
json += "\"MuñecaDer\":" + oSerializer.Serialize(heanp) + ",";
break;
case JointType.HandLeft:
//Mano Izquierda
heanp = joint.Position;
json += "\"ManoIzq\":" + oSerializer.Serialize(heanp) + ",";
break;
case JointType.HandRight:
//Mano Derecha
heanp = joint.Position;
json += "\"ManoDer\":" + oSerializer.Serialize(heanp) + ",";
//distancia -= heanp.Z;
break;
case JointType.Spine:
//Columna
heanp = joint.Position;
json += "\"Columna\":" + oSerializer.Serialize(heanp) + ",";
break;
case JointType.HipCenter:
//Cadera
heanp = joint.Position;
json += "\"Cadera\":" + oSerializer.Serialize(heanp) + ",";
break;
case JointType.HipRight:
//Cadera derecha
heanp = joint.Position;
json += "\"CaderaDer\":" + oSerializer.Serialize(heanp) + ",";
break;
case JointType.HipLeft:
//Cadera izquierda
heanp = joint.Position;
json += "\"CaderaIzq\":" + oSerializer.Serialize(heanp) + ",";
break;
case JointType.KneeRight:
//Rodilla derecha
heanp = joint.Position;
json += "\"RodillaDer\":" + oSerializer.Serialize(heanp) + ",";
break;
case JointType.KneeLeft:
//Rodilla izquierda
heanp = joint.Position;
json += "\"RodillaIzq\":" + oSerializer.Serialize(heanp) + ",";
break;
case JointType.AnkleRight:
//Tobillo derecho
heanp = joint.Position;
json += "\"TobilloDer\":" + oSerializer.Serialize(heanp) + ",";
break;
case JointType.AnkleLeft:
//Tobillo izquierdo
heanp = joint.Position;
json += "\"TobilloIzq\":" + oSerializer.Serialize(heanp) + ",";
break;
case JointType.FootRight:
//Pie derecho
heanp = joint.Position;
json += "\"PieDer\":" + oSerializer.Serialize(heanp) + ",";
break;
case JointType.FootLeft:
//Pie izquierdo
heanp = joint.Position;
json += "\"PieIzq\":" + oSerializer.Serialize(heanp) + ",";
break;
}
}
json = json.Substring(0, json.Length - 1) + "},";
}
}
json = json.Substring(0, json.Length - 1) + "]}";
jsonSkeleton = json;
}
}
private void SensorSkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
Skeleton[] skeletons = new Skeleton[0];
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
}
}
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, RenderWidth, RenderHeight));
if (skeletons.Length != 0)
{
foreach (Skeleton skel in skeletons)
{
RenderClippedEdges(skel, dc);
if (skel.TrackingState == SkeletonTrackingState.Tracked)
{
Joint joint0 = skel.Joints[JointType.HandRight];
DepthImagePoint depthPoint = this.sensor.CoordinateMapper.MapSkeletonPointToDepthPoint(joint0.Position, DepthImageFormat.Resolution640x480Fps30);
this.DrawBonesAndJoints(skel, dc);
Angles MyAngles = new Angles();
byte[] ReadyAngles = MyAngles.GetVector(skel, depthPoint);
//RightElbow.Text = ReadyAngles[0].ToString();
//RightShoulder.Text = ReadyAngles[1].ToString();
//LeftElbow.Text = ReadyAngles[2].ToString();
// LeftShoulder.Text = ReadyAngles[3].ToString();
byte[] SequenceStart = { 255 };
//SERİ İÇİN AÇ//////////////////////////////////////////////
//if (ArduinoPort.IsOpen)
//{
// // ArduinoPort.Write(SequenceStart, 0, 1);
// // ArduinoPort.Write(ReadyAngles, 0, 4);
// ArduinoPort.Write(ReadyAngles, 0, 4);
// Console.Write("Sağ Dirsek Eklemi: " + RightElbow.Text + "\n");
// Console.Write("Sağ Omuz(Sağ-Sol) Eklemi : " + RightElbow.Text + "\n");
// Console.Write("Sağ Omuz(Yukarı-Aşağı) Eklemi : " + RightElbow.Text + "\n");
// Console.Write("Sol Dirsek Eklemi: " + RightElbow.Text + "\n");
//}
//String s = ReadyAngles[0].ToString() + "," + ReadyAngles[1].ToString() + "*" + ReadyAngles[2].ToString() + "/";
//byte[] byteTime = Encoding.ASCII.GetBytes(s);
// ns.Write(byteTime, 0, byteTime.Length);
ns.Write(ReadyAngles, 0, ReadyAngles.Length);
}
else if (skel.TrackingState == SkeletonTrackingState.PositionOnly)
{
dc.DrawEllipse(
this.centerPointBrush,
null,
this.SkeletonPointToScreen(skel.Position),
BodyCenterThickness,
BodyCenterThickness);
}
}
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, RenderWidth, RenderHeight));
}
}
private void OnAllFramesReady(object sender, AllFramesReadyEventArgs allFramesReadyEventArgs)
{
Trace.WriteLine("allframesready");
ColorImageFrame colorImageFrame = null;
DepthImageFrame depthImageFrame = null;
SkeletonFrame skeletonFrame = null;
try
{
Trace.WriteLine("try start");
colorImageFrame = allFramesReadyEventArgs.OpenColorImageFrame();
depthImageFrame = allFramesReadyEventArgs.OpenDepthImageFrame();
skeletonFrame = allFramesReadyEventArgs.OpenSkeletonFrame();
if (colorImageFrame == null || depthImageFrame == null || skeletonFrame == null)
{
Trace.WriteLine("return from frameready");
return;
}
Trace.WriteLine("wayaround null");
// Check for image format changes. The FaceTracker doesn't
// deal with that so we need to reset.
if (this.depthImageFormat != depthImageFrame.Format)
{
this.ResetFaceTracking();
this.depthImage = null;
this.depthImageFormat = depthImageFrame.Format;
Trace.WriteLine("depth image fromat no work");
}
if (this.colorImageFormat != colorImageFrame.Format)
{
this.ResetFaceTracking();
this.colorImage = null;
this.colorImageFormat = colorImageFrame.Format;
Trace.WriteLine("color image fromat no work");
}
// Create any buffers to store copies of the data we work with
if (this.depthImage == null)
{
this.depthImage = new short[depthImageFrame.PixelDataLength];
}
if (this.colorImage == null)
{
this.colorImage = new byte[colorImageFrame.PixelDataLength];
}
// Get the skeleton information
if (this.skeletonData == null || this.skeletonData.Length != skeletonFrame.SkeletonArrayLength)
{
this.skeletonData = new Skeleton[skeletonFrame.SkeletonArrayLength];
}
Trace.WriteLine("starting copying of data");
colorImageFrame.CopyPixelDataTo(this.colorImage);
depthImageFrame.CopyPixelDataTo(this.depthImage);
skeletonFrame.CopySkeletonDataTo(this.skeletonData);
Trace.WriteLine("end copying of data");
// Update the list of trackers and the trackers with the current frame information
foreach (Skeleton skeleton in this.skeletonData)
{
Trace.WriteLine("inside foreach loop");
if (skeleton.TrackingState == SkeletonTrackingState.Tracked ||
skeleton.TrackingState == SkeletonTrackingState.PositionOnly)
{
Trace.WriteLine("iftracked");
// We want keep a record of any skeleton, tracked or untracked.
if (!this.trackedSkeletons.ContainsKey(skeleton.TrackingId))
{
this.trackedSkeletons.Add(skeleton.TrackingId, new SkeletonFaceTracker());
Trace.WriteLine("add skeleton tracking");
}
// Give each tracker the upated frame.
SkeletonFaceTracker skeletonFaceTracker;
if (this.trackedSkeletons.TryGetValue(skeleton.TrackingId, out skeletonFaceTracker))
{
Trace.WriteLine("before");
skeletonFaceTracker.OnFrameReady(this.Kinect, colorImageFormat, colorImage, depthImageFormat, depthImage, skeleton, this.activityWindow);
skeletonFaceTracker.LastTrackedFrame = skeletonFrame.FrameNumber;
}
}
}
this.RemoveOldTrackers(skeletonFrame.FrameNumber);
this.InvalidateVisual();
}
finally
{
if (colorImageFrame != null)
{
colorImageFrame.Dispose();
}
if (depthImageFrame != null)
{
depthImageFrame.Dispose();
}
if (skeletonFrame != null)
{
skeletonFrame.Dispose();
}
}
}
// Event for handling skeleton stream updates.
private void SensorSkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
Skeleton[] skeletons = new Skeleton[0];
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
}
}
skeleton = null;
if (skeletons.Length != 0)
{
// Iterate through available skeletons and choose the one closest to the previous skeleton
foreach (Skeleton skel in skeletons)
{
// TODO: add in processing to only follow the closest skeleton to the previously tracked
if (skel.TrackingState == SkeletonTrackingState.Tracked)
{
skeleton = skel;
break;
}
}
}
if (skeleton != null)
{
if (skeleton.Joints[JointType.HandLeft].TrackingState == JointTrackingState.Tracked)
{
DepthImagePoint leftHandPoint = this.sensor.CoordinateMapper.MapSkeletonPointToDepthPoint(
skeleton.Joints[JointType.HandLeft].Position, DepthImageFormat.Resolution640x480Fps30);
leftHandPoint.Y -= 15; // Kinect sensor seems to set the hand a bit high. Adjusted.
Hands[(int)HandPoint.HANDS.LEFT].position_pixels_absolute = new Point3D {
X = leftHandPoint.X, Y = leftHandPoint.Y, Z = leftHandPoint.Depth
};
// Update Roll angle
Hands[(int)HandPoint.HANDS.LEFT].angle_from_elbow_degrees.X = (
180 + (180 / Math.PI) * Math.Atan2(
skeleton.Joints[JointType.HandLeft].Position.Y - skeleton.Joints[JointType.ElbowLeft].Position.Y,
skeleton.Joints[JointType.HandLeft].Position.X - skeleton.Joints[JointType.ElbowLeft].Position.X)) % 360;
// Update Pitch angle
Hands[(int)HandPoint.HANDS.LEFT].angle_from_elbow_degrees.Y = (
180 + (180 / Math.PI) * Math.Atan2(
skeleton.Joints[JointType.HandLeft].Position.Z - skeleton.Joints[JointType.ElbowLeft].Position.Z,
skeleton.Joints[JointType.HandLeft].Position.Y - skeleton.Joints[JointType.ElbowLeft].Position.Y)) % 360;
// Update Yaw angle
Hands[(int)HandPoint.HANDS.LEFT].angle_from_elbow_degrees.Z = (
180 + (180 / Math.PI) * Math.Atan2(
skeleton.Joints[JointType.HandLeft].Position.Z - skeleton.Joints[JointType.ElbowLeft].Position.Z,
skeleton.Joints[JointType.HandLeft].Position.X - skeleton.Joints[JointType.ElbowLeft].Position.X)) % 360;
}
if (skeleton.Joints[JointType.HandRight].TrackingState == JointTrackingState.Tracked)
{
DepthImagePoint rightHandPoint = this.sensor.CoordinateMapper.MapSkeletonPointToDepthPoint(
skeleton.Joints[JointType.HandRight].Position, DepthImageFormat.Resolution640x480Fps30);
rightHandPoint.Y -= 15; // Kinect sensor seems to set the hand a bit high. Adjusted.
Hands[(int)HandPoint.HANDS.RIGHT].position_pixels_absolute = new Point3D {
X = rightHandPoint.X, Y = rightHandPoint.Y, Z = rightHandPoint.Depth
};
// Update Roll angle
Hands[(int)HandPoint.HANDS.RIGHT].angle_from_elbow_degrees.X = (
180 + (180 / Math.PI) * Math.Atan2(
skeleton.Joints[JointType.HandRight].Position.Y - skeleton.Joints[JointType.ElbowRight].Position.Y,
skeleton.Joints[JointType.HandRight].Position.X - skeleton.Joints[JointType.ElbowRight].Position.X)) % 360;
// Update Pitch angle
Hands[(int)HandPoint.HANDS.LEFT].angle_from_elbow_degrees.Y = (
180 + (180 / Math.PI) * Math.Atan2(
skeleton.Joints[JointType.HandLeft].Position.Z - skeleton.Joints[JointType.ElbowLeft].Position.Z,
skeleton.Joints[JointType.HandLeft].Position.Y - skeleton.Joints[JointType.ElbowLeft].Position.Y)) % 360;
// Update Yaw angle
Hands[(int)HandPoint.HANDS.RIGHT].angle_from_elbow_degrees.Z = (
180 + (180 / Math.PI) * Math.Atan2(
skeleton.Joints[JointType.HandRight].Position.Z - skeleton.Joints[JointType.ElbowRight].Position.Z,
skeleton.Joints[JointType.HandRight].Position.X - skeleton.Joints[JointType.ElbowRight].Position.X)) % 360;
}
} // end if skeleton != null
else
{
Hands[(int)HandPoint.HANDS.LEFT].tracked = false;
Hands[(int)HandPoint.HANDS.RIGHT].tracked = false;
}
this.Invalidate();
}
void sensor_AllFramesReady(object sender, AllFramesReadyEventArgs e)
{
if ((playback || recordSet) && countdown < 90)
{
if (countdown == 0)
{
Countdown.Text = "3";
}
else if (countdown == 30)
{
Countdown.Text = "2";
}
else if (countdown == 60)
{
Countdown.Text = "1";
}
countdown++;
return;
}
else
{
Countdown.Text = "";
if (playback)
{
changeVisibility(System.Windows.Visibility.Visible);
}
}
if (playback == true)
{
using (ColorImageFrame colorFrame = e.OpenColorImageFrame())
{
if (colorFrame == null)
{
return;
}
playbackFrameNumber++;
byte[] pixels = new byte[colorFrame.PixelDataLength];
dataStream.Read(pixels, 0, colorFrame.PixelDataLength);
int stride = colorFrame.Width * 4;
masterView.Source = BitmapSource.Create(colorFrame.Width, colorFrame.Height, 96, 96, PixelFormats.Bgr32, null, pixels, stride);
if (totalCounted != 0)
{
String playerFeedback;
if (playbackFrameNumber % 30 == 0)
{
playerPercentage = Math.Round((totalCorrespondence / totalCounted) * 100);
if (playerPercentage < 20)
{
playerFeedback = "You're terrible";
}
else if (playerPercentage < 40)
{
playerFeedback = "You could do better";
}
else if (playerPercentage < 60)
{
playerFeedback = "Getting there";
}
else if (playerPercentage < 80)
{
playerFeedback = "Not bad";
}
else if (playerPercentage < 100)
{
playerFeedback = "Great job!";
}
else
{
playerFeedback = "Are you even real?";
}
/*
* else
* {
* playerFeedback = playerPercentage.ToString() + "%";
* }
*/
// update display
if (!textBlock3.Text.Equals(playerFeedback))
{
//speech.Speak(playerFeedback);
textBlock3.Text = playerFeedback;
}
textPercentage.Text = playerPercentage.ToString() + "%";
}
}
if (dataStream.Position == streamLength)
{
countdown = 0;
playback = false;
dataStream.Close();
// swap image
BitmapImage bitmap = new BitmapImage();
bitmap.BeginInit();
bitmap.UriSource = new Uri("/VirtualSifu;component/Images/play.png", UriKind.Relative);
bitmap.EndInit();
image4.Stretch = Stretch.Fill;
image4.Source = bitmap;
// undim record button
bitmap = new BitmapImage();
bitmap.BeginInit();
bitmap.UriSource = new Uri("/VirtualSifu;component/Images/record.png", UriKind.Relative);
bitmap.EndInit();
image2.Stretch = Stretch.Fill;
image2.Source = bitmap;
masterView.Source = null;
changeVisibility(System.Windows.Visibility.Hidden);
}
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
Skeleton[] data = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(data);
foreach (Skeleton skeleton in data)
{
if (skeleton.TrackingState == SkeletonTrackingState.Tracked)
{
SkeletonPoint studentPoint;
foreach (String joint in jointsTracked)
{
studentPoint = getJoint(joint, skeleton).Position;
((ArrayList)studentData.Get(joint)).Insert(playbackFrameNumber % 30, new JointData(studentPoint.X, studentPoint.Y, studentPoint.Z));
}
ArrayList dtwData = new ArrayList();
if (playbackFrameNumber != 0 && playbackFrameNumber % 30 == 0)
{
//run DTW for each joint
dtwData = runDTW3();
Console.Write((double)dtwData[0] + "\n");
colorJoint(ankleRight, (double)dtwData[0]);
colorJoint(ankleLeft, (double)dtwData[1]);
colorJoint(kneeRight, (double)dtwData[2]);
colorJoint(kneeLeft, (double)dtwData[3]);
colorJoint(hipRight, (double)dtwData[4]);
colorJoint(hipLeft, (double)dtwData[5]);
colorJoint(shoulderRight, (double)dtwData[6]);
colorJoint(shoulderLeft, (double)dtwData[7]);
colorJoint(elbowRight, (double)dtwData[8]);
colorJoint(elbowLeft, (double)dtwData[9]);
colorJoint(wristRight, (double)dtwData[10]);
colorJoint(wristLeft, (double)dtwData[11]);
//colorJoint(ellipse, random.Next(0, 4));
//Probably can do this part like Gina's
//Get a joint list that you want calculated
//perform runDTW on each individual joint
//then do your coloring/drawing for it.
}
ScalePosition(wristRight, skeleton.Joints[JointType.WristRight]);
ScalePosition(wristLeft, skeleton.Joints[JointType.WristLeft]);
ScalePosition(elbowRight, skeleton.Joints[JointType.ElbowRight]);
ScalePosition(elbowLeft, skeleton.Joints[JointType.ElbowLeft]);
ScalePosition(shoulderRight, skeleton.Joints[JointType.ShoulderRight]);
ScalePosition(shoulderLeft, skeleton.Joints[JointType.ShoulderLeft]);
ScalePosition(ankleRight, skeleton.Joints[JointType.AnkleRight]);
ScalePosition(ankleLeft, skeleton.Joints[JointType.AnkleLeft]);
ScalePosition(kneeRight, skeleton.Joints[JointType.KneeRight]);
ScalePosition(kneeLeft, skeleton.Joints[JointType.KneeLeft]);
ScalePosition(hipRight, skeleton.Joints[JointType.HipRight]);
ScalePosition(hipLeft, skeleton.Joints[JointType.HipLeft]);
GetCameraPoint(skeleton, e);
}
}
}
}
}
}
if (recordSet == true)
{
using (ColorImageFrame colorFrame = e.OpenColorImageFrame())
{
if (colorFrame == null)
{
return;
}
frameNumber++;
byte[] pixels = new byte[colorFrame.PixelDataLength];
colorFrame.CopyPixelDataTo(pixels);
dataStream.Write(pixels, 0, colorFrame.PixelDataLength);
int stride = colorFrame.Width * 4;
masterView.Source = BitmapSource.Create(colorFrame.Width, colorFrame.Height, 96, 96, PixelFormats.Bgr32, null, pixels, stride);
}
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
Console.Write(masterLeftWristData.Count);
Skeleton[] data = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(data);
foreach (Skeleton skeleton in data)
{
if (skeleton.TrackingState == SkeletonTrackingState.Tracked)
{
SkeletonPoint point = skeleton.Joints[JointType.Head].Position;
foreach (String joint in jointsTracked)
{
point = getJoint(joint, skeleton).Position;
writer.Write(joint + ": " + point.X + " " + point.Y + " " + point.Z + "\r\n");
}
writer.Write("\r\n");
//Somewhere after all this code has run, we need to finish construction of
//our FileStreamReader for Student [and master?]
}
}
}
}
}
}
//スケルトンが更新される時に呼び出される
private void skeleton_update(object sender, SkeletonFrameReadyEventArgs e)
{
KinectSensor kinect = sender as KinectSensor;
//Form fm = FindForm();
//if (fm != null) fm.SetDesktopBounds(0, 0, 1920, 1080);
//ジョイント座標の更新
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
Skeleton[] skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
foreach (Skeleton skeleton in skeletons)
{
Joint handLeft = skeleton.Joints[JointType.HandLeft];
Joint handRight = skeleton.Joints[JointType.HandRight];
Joint spine = skeleton.Joints[JointType.Spine];
if (skeleton.TrackingState == SkeletonTrackingState.Tracked &&
handLeft.TrackingState == JointTrackingState.Tracked &&
handRight.TrackingState == JointTrackingState.Tracked &&
spine.TrackingState == JointTrackingState.Tracked)
{
//swipeDetector.Add(handRight.Position, kinect);
//RGB画像の座標への変換
var coordinateMapper = KinectSensor.KinectSensors[0].CoordinateMapper;
ColorImagePoint rightHandPos = coordinateMapper.MapSkeletonPointToColorPoint(handRight.Position, KinectSensor.KinectSensors[0].ColorStream.Format);
ColorImagePoint leftHandPos = coordinateMapper.MapSkeletonPointToColorPoint(handLeft.Position, KinectSensor.KinectSensors[0].ColorStream.Format);
ColorImagePoint spinePos = coordinateMapper.MapSkeletonPointToColorPoint(spine.Position, KinectSensor.KinectSensors[0].ColorStream.Format);
int leftdiff = leftHandPos.X - spinePos.X;
int rightdiff = rightHandPos.X - spinePos.X;
int center = spinePos.X;
if (-leftdiff < paddle.MinimumWidth / 5)
{
leftdiff = -paddle.MinimumWidth / 5;
}
else if (-leftdiff > paddle.MaximumWidth / 5)
{
leftdiff = -paddle.MaximumWidth / 5;
}
if (rightdiff < paddle.MinimumWidth / 5)
{
rightdiff = paddle.MinimumWidth / 5;
}
else if (rightdiff > paddle.MaximumWidth / 5)
{
rightdiff = paddle.MaximumWidth / 5;
}
paddle.Size = new Rectangle((center + leftdiff) * Width / 640, Height * 9 / 10, (-leftdiff + rightdiff) * Width / 640, Height / 80);
}
}
}
}
//ボールをウィンドウサイズに合わせる
if (!ballLaunched)
{
ball.Speed = new Vector(0, 0);
ball.Position = new Vector(paddle.Size.X + paddle.Size.Width / 2, paddle.Size.Y - ball.Radius);
}
//ball.SetSpeed(Bounds.X / 50, Bounds.Y / 50);
ball.Radius = Height / 40;
paddle.MinimumWidth = Width / 7;
paddle.MaximumWidth = Width / 3;
//壁をウィンドウサイズに合わせる
leftWall = new Rectangle(0, 0, Width / 10, Height);
rightWall = new Rectangle(Width * 9 / 10, 0, Width / 10, Height);
//ブロックをウィンドウサイズに合わせる
blocks.Size = new Rectangle(Width / 5, Height / 20, Width / 10, Height / 20);
blocks.BlockInterval = new Vector(5, 5);
//球と壁との衝突を判定する
if (ball.Position.X + ball.Radius > rightWall.Left)
{
ball.ReverseSpeedX();
}
if (ball.Position.X - ball.Radius < leftWall.Right)
{
ball.ReverseSpeedX();
}
if (ball.Position.Y + ball.Radius <= 0)
{
ball.ReverseSpeedY();
}
//球と画面下の衝突を判定する
if (ball.Position.Y + ball.Radius >= Height && once)
{
once = false;
int score = scoreCounter * 100;
DateTime date = DateTime.Now;
string dateString = date.ToString("HH:mm");
File.AppendAllText(@"Scores.csv", score.ToString() + "A" + dateString + ",");
Dispose();
}
//球の移動
ball.Position += ball.Speed;
//全消ししたら復活する
if (blocks.DurabilityArray.All(s => s <= 0))
{
blocks.DurabilityArray = tools.ArrayRandomize(5 * 4, 1, 5);
}
//球とパドルの当たり判定
if (tools.IsCollisionBase(new Vector(paddle.Size.X, paddle.Size.Y), new Vector(paddle.Size.X + paddle.Size.Width, paddle.Size.Y), ball.Position, ball.Radius))
{
if (paddle.Size.Width < Width / 5)
{
timer.Enabled = true;
ball.Brush = new SolidBrush(Color.Purple);
ball.IsDoubleDamaged = true;
}
ball.Speed = tools.BallReflection(new Vector(paddle.Size.X, paddle.Size.Y), new Vector(paddle.Size.X + paddle.Size.Width, paddle.Size.Y), ball.Position, ball.Radius, paddle.Size, ball.Speed);
}
//球とブロックの当たり判定
for (int i = 0; i < blocks.Column; i++)
{
for (int j = 0; j < blocks.Row; j++)
{
int x = j * blocks.Column + i;
if (blocks.DurabilityArray[x] > 0)
{
int collision = tools.IsCollisionBlock(blocks.BlockPosition(j, i), ball);
if (collision == 1 || collision == 2)
{
if (ball.IsDoubleDamaged)
{
blocks.DurabilityArray[x]--; scoreCounter++;
}
blocks.DurabilityArray[x]--; scoreCounter++;
ball.ReverseSpeedY();
}
else if (collision == 3 || collision == 4)
{
if (ball.IsDoubleDamaged)
{
blocks.DurabilityArray[x]--; scoreCounter++;
}
blocks.DurabilityArray[x]--; scoreCounter++;
ball.ReverseSpeedX();
}
}
}
}
//パドルの特殊処理
if (paddle.Size.Width < Width / 5)
{
paddle.Brush = new SolidBrush(Color.LightGreen);
}
else
{
paddle.Brush = new SolidBrush(Color.DimGray);
}
//壁とパドルの当たり判定
if (leftWall.Width >= paddle.Size.X)
{
int x = paddle.Size.X;
x = leftWall.Width;
}
else if (rightWall.X <= paddle.Size.Right)
{
int y = paddle.Size.Y;
y = rightWall.X;
}
//Drawの再描画
Invalidate();
}
/// <summary>
/// Handles the AllFramesReady event of the kinectSensor control.
/// </summary>
/// <param name="sender">The source of the event.</param>
/// <param name="e">The <see cref="Microsoft.Kinect.AllFramesReadyEventArgs"/> instance containing the event data.</param>
void kinectSensor_AllFramesReady(object sender, AllFramesReadyEventArgs e)
{
// Retrieve each single frame and copy the data
using (ColorImageFrame colorImageFrame = e.OpenColorImageFrame())
{
if (colorImageFrame == null)
{
return;
}
colorImageFrame.CopyPixelDataTo(colorPixelData);
}
using (DepthImageFrame depthImageFrame = e.OpenDepthImageFrame())
{
if (depthImageFrame == null)
{
return;
}
depthImageFrame.CopyPixelDataTo(depthPixelData);
}
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame == null)
{
return;
}
skeletonFrame.CopySkeletonDataTo(skeletonData);
}
// Retrieve the first tracked skeleton if any. Otherwise, do nothing.
var skeleton = skeletonData.FirstOrDefault(s => s.TrackingState == SkeletonTrackingState.Tracked);
if (skeleton == null)
{
return;
}
// Make the faceTracker processing the data.
FaceTrackFrame faceFrame = faceTracker.Track(kinectSensor.ColorStream.Format, colorPixelData,
kinectSensor.DepthStream.Format, depthPixelData,
skeleton);
// If a face is tracked, then we can use it
if (faceFrame.TrackSuccessful)
{
// Retrieve only the Animation Units coeffs
var AUCoeff = faceFrame.GetAnimationUnitCoefficients();
// Records to list buffer if record is enabled
if (isRecord == true)
{
// Start stopwatch
stopwatch.Start();
// AU coefficients
lipRaiserBuffer.Add(AUCoeff[AnimationUnit.LipRaiser]);
jawLowerBuffer.Add(AUCoeff[AnimationUnit.JawLower]);
lipStretchBuffer.Add(AUCoeff[AnimationUnit.LipStretcher]);
browLowerBuffer.Add(AUCoeff[AnimationUnit.BrowLower]);
lipDepressBuffer.Add(AUCoeff[AnimationUnit.LipCornerDepressor]);
browRaiserBuffer.Add(AUCoeff[AnimationUnit.BrowLower]);
// Face rotation
xRotation.Add(faceFrame.Rotation.X);
yRotation.Add(faceFrame.Rotation.Y);
zRotation.Add(faceFrame.Rotation.Z);
// Get time in ms
timeBuffer.Add(stopwatch.ElapsedMilliseconds);
}
// Display on UI coefficients and rotation for user
LipRaiser.Content = AUCoeff[AnimationUnit.LipRaiser];
JawLower.Content = AUCoeff[AnimationUnit.JawLower];
LipStretch.Content = AUCoeff[AnimationUnit.LipStretcher];
BrowLower.Content = AUCoeff[AnimationUnit.BrowLower];
LipDepress.Content = AUCoeff[AnimationUnit.LipCornerDepressor];
BrowRaiser.Content = AUCoeff[AnimationUnit.BrowRaiser];
XRotation.Content = faceFrame.Rotation.X;
YRotation.Content = faceFrame.Rotation.Y;
ZRotation.Content = faceFrame.Rotation.Z;
// Animates the drawn face
var jawLowerer = AUCoeff[AnimationUnit.JawLower];
jawLowerer = jawLowerer < 0 ? 0 : jawLowerer;
MouthScaleTransform.ScaleY = jawLowerer * 5 + 0.1;
MouthScaleTransform.ScaleX = (AUCoeff[AnimationUnit.LipStretcher] + 1);
LeftBrow.Y = RightBrow.Y = (AUCoeff[AnimationUnit.BrowLower]) * 40;
RightBrowRotate.Angle = (AUCoeff[AnimationUnit.BrowRaiser] * 20);
LeftBrowRotate.Angle = -RightBrowRotate.Angle;
CanvasRotate.Angle = faceFrame.Rotation.Z;
}
}
/// <summary>
/// 프레임이 들어올때마다 이에 대한 스켈리턴 작업을 하는 이벤트 처리 메소드입니다.
/// </summary>
public void nui_AllFramesReady(object sender, AllFramesReadyEventArgs e)
{
using (SkeletonFrame sf = e.OpenSkeletonFrame())
{
if (sf == null)
{
return;
}
Skeleton[] skeletonData = new Skeleton[sf.SkeletonArrayLength];
sf.CopySkeletonDataTo(skeletonData);
using (DepthImageFrame depthImageFrame = e.OpenDepthImageFrame())
{
if (depthImageFrame != null)
{
foreach (Skeleton sd in skeletonData)
{
if (sd.TrackingState == SkeletonTrackingState.Tracked)
{
int nMax = 20;
Joint[] joints = new Joint[nMax];
for (int j = 0; j < nMax; j++)
{
joints[j] = sd.Joints[(JointType)j];
}
points = new Point[20];
for (int j = 0; j < nMax; j++)
{
DepthImagePoint depthPoint;
depthPoint = depthImageFrame.MapFromSkeletonPoint(joints[j].Position);
points[j] = new Point((int)(image1.Width * depthPoint.X / depthImageFrame.Width), (int)(image1.Height * depthPoint.Y / depthImageFrame.Height));
}
PointCollection pc0 = new PointCollection(4);/*pc()에 4개의 포인트 추가*/
pc0.Add(points[(int)JointType.HipCenter]);
pc0.Add(points[(int)JointType.Spine]);
pc0.Add(points[(int)JointType.ShoulderCenter]);
pc0.Add(points[(int)JointType.Head]);
m_poly[0].Points = pc0;
m_poly[0].Visibility = Visibility.Visible;
PointCollection pc1 = new PointCollection(5);
pc1.Add(points[(int)JointType.ShoulderCenter]);
pc1.Add(points[(int)JointType.ShoulderLeft]);
pc1.Add(points[(int)JointType.ElbowLeft]);
pc1.Add(points[(int)JointType.WristLeft]);
pc1.Add(points[(int)JointType.HandLeft]);
m_poly[1].Points = pc1;
m_poly[1].Visibility = Visibility.Visible;
PointCollection pc2 = new PointCollection(5);
pc2.Add(points[(int)JointType.ShoulderCenter]);
pc2.Add(points[(int)JointType.ShoulderRight]);
pc2.Add(points[(int)JointType.ElbowRight]);
pc2.Add(points[(int)JointType.WristRight]);
pc2.Add(points[(int)JointType.HandRight]);
m_poly[2].Points = pc2;
m_poly[2].Visibility = Visibility.Visible;
PointCollection pc3 = new PointCollection(5);
pc3.Add(points[(int)JointType.HipCenter]);
pc3.Add(points[(int)JointType.HipLeft]);
pc3.Add(points[(int)JointType.KneeLeft]);
pc3.Add(points[(int)JointType.AnkleLeft]);
pc3.Add(points[(int)JointType.FootLeft]);
m_poly[3].Points = pc3;
m_poly[3].Visibility = Visibility.Visible;
PointCollection pc4 = new PointCollection(5);
pc4.Add(points[(int)JointType.HipCenter]);
pc4.Add(points[(int)JointType.HipRight]);
pc4.Add(points[(int)JointType.KneeRight]);
pc4.Add(points[(int)JointType.AnkleRight]);
pc4.Add(points[(int)JointType.FootRight]);
m_poly[4].Points = pc4;
m_poly[4].Visibility = Visibility.Visible;
int[] inputAngles = new int[14];
inputAngles[0] = (int)GetJointAngle(points[(int)JointType.ElbowRight], points[(int)JointType.ShoulderRight]);
inputAngles[1] = (int)GetJointAngle(points[(int)JointType.WristRight], points[(int)JointType.ElbowRight]);
inputAngles[2] = (int)GetJointAngle(points[(int)JointType.ElbowLeft], points[(int)JointType.ShoulderLeft]);
inputAngles[3] = (int)GetJointAngle(points[(int)JointType.WristLeft], points[(int)JointType.ElbowLeft]);
inputAngles[4] = (int)GetJointAngle(points[(int)JointType.ShoulderRight], points[(int)JointType.ShoulderCenter]);
inputAngles[5] = (int)GetJointAngle(points[(int)JointType.ShoulderLeft], points[(int)JointType.ShoulderCenter]);
inputAngles[6] = (int)GetJointAngle(points[(int)JointType.Head], points[(int)JointType.ShoulderCenter]);
inputAngles[7] = (int)GetJointAngle(points[(int)JointType.ShoulderCenter], points[(int)JointType.Spine]);
inputAngles[8] = (int)GetJointAngle(points[(int)JointType.HipCenter], points[(int)JointType.HipRight]);
inputAngles[9] = (int)GetJointAngle(points[(int)JointType.HipCenter], points[(int)JointType.HipLeft]);
inputAngles[10] = (int)GetJointAngle(points[(int)JointType.HipRight], points[(int)JointType.KneeRight]);
inputAngles[11] = (int)GetJointAngle(points[(int)JointType.HipLeft], points[(int)JointType.KneeLeft]);
inputAngles[12] = (int)GetJointAngle(points[(int)JointType.KneeRight], points[(int)JointType.AnkleRight]);
inputAngles[13] = (int)GetJointAngle(points[(int)JointType.KneeLeft], points[(int)JointType.AnkleLeft]);
SetDiffAngle(inputAngles);
textBlocks[0].RenderTransform = new TranslateTransform(points[(int)JointType.ElbowRight].X, points[(int)JointType.ElbowRight].Y);
textBlocks[0].Text = diffAngles[0] + "";
textBlocks[1].RenderTransform = new TranslateTransform(points[(int)JointType.WristRight].X, points[(int)JointType.WristRight].Y);
textBlocks[1].Text = diffAngles[1] + "";
textBlocks[2].RenderTransform = new TranslateTransform(points[(int)JointType.ElbowLeft].X, points[(int)JointType.ElbowLeft].Y);
textBlocks[2].Text = diffAngles[2] + "";
textBlocks[3].RenderTransform = new TranslateTransform(points[(int)JointType.WristLeft].X, points[(int)JointType.WristLeft].Y);
textBlocks[3].Text = diffAngles[3] + "";
textBlocks[4].RenderTransform = new TranslateTransform(points[(int)JointType.ShoulderRight].X, points[(int)JointType.ShoulderRight].Y);
textBlocks[4].Text = diffAngles[4] + "";
textBlocks[5].RenderTransform = new TranslateTransform(points[(int)JointType.ShoulderLeft].X, points[(int)JointType.ShoulderLeft].Y);
textBlocks[5].Text = diffAngles[5] + "";
textBlocks[6].RenderTransform = new TranslateTransform(points[(int)JointType.ShoulderCenter].X, points[(int)JointType.ShoulderCenter].Y);
textBlocks[6].Text = diffAngles[6] + "";
textBlocks[7].RenderTransform = new TranslateTransform(points[(int)JointType.Spine].X, points[(int)JointType.Spine].Y);
textBlocks[7].Text = diffAngles[7] + "";
textBlocks[8].RenderTransform = new TranslateTransform(points[(int)JointType.HipRight].X, points[(int)JointType.HipRight].Y);
textBlocks[8].Text = diffAngles[8] + "";
textBlocks[9].RenderTransform = new TranslateTransform(points[(int)JointType.HipLeft].X, points[(int)JointType.HipLeft].Y);
textBlocks[9].Text = diffAngles[9] + "";
textBlocks[10].RenderTransform = new TranslateTransform(points[(int)JointType.KneeRight].X, points[(int)JointType.KneeRight].Y);
textBlocks[10].Text = diffAngles[10] + "";
textBlocks[11].RenderTransform = new TranslateTransform(points[(int)JointType.KneeLeft].X, points[(int)JointType.KneeLeft].Y);
textBlocks[11].Text = diffAngles[11] + "";
textBlocks[12].RenderTransform = new TranslateTransform(points[(int)JointType.AnkleRight].X, points[(int)JointType.AnkleRight].Y);
textBlocks[12].Text = diffAngles[12] + "";
textBlocks[13].RenderTransform = new TranslateTransform(points[(int)JointType.AnkleLeft].X, points[(int)JointType.AnkleLeft].Y);
textBlocks[13].Text = diffAngles[13] + "";
textBlocks[14].RenderTransform = new TranslateTransform(10, 10);
textBlocks[14].Height = 100;
textBlocks[14].Width = 500;
if (isCorrect())
{
// if (flag == false)
// speechSynthesizer.Speak("Correct Posture");
textBlocks[14].Foreground = Brushes.Green;
textBlocks[14].Text = "올바른 자세입니다.";
flag = true;
}
else
{
// if (flag == true)
// speechSynthesizer.Speak("Incorrect Posture");
textBlocks[14].Foreground = Brushes.Red;
textBlocks[14].Text = "잘못된 자세입니다.";
flag = false;
}
}
}
}
}
}
}
private void _Kinect_AllFramesReady(object sender, AllFramesReadyEventArgs e)
{
// Have we already been "shut down" by the user of this viewer?
if (this.Kinect == null)
{
return;
}
bool haveSkeletonData = false;
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
if (skeletonCanvases == null)
{
CreateListOfSkeletonCanvases();
}
if ((skeletonData == null) || (skeletonData.Length != skeletonFrame.SkeletonArrayLength))
{
skeletonData = new Skeleton[skeletonFrame.SkeletonArrayLength];
}
skeletonFrame.CopySkeletonDataTo(skeletonData);
haveSkeletonData = true;
}
}
if (haveSkeletonData)
{
using (DepthImageFrame depthImageFrame = e.OpenDepthImageFrame())
{
if (depthImageFrame != null)
{
int trackedSkeletons = 0;
foreach (Skeleton skeleton in skeletonData)
{
KinectSkeleton skeletonCanvas = skeletonCanvases[trackedSkeletons++];
switch (skeleton.TrackingState)
{
case SkeletonTrackingState.Tracked:
skeletonCanvas.ShowCenter = false;
if (skeletonCanvases != null)
{
skeletonCanvas.Visibility = ShowJoints || ShowBones
? Visibility.Visible
: Visibility.Hidden;
if (ShowBones)
{
adjustLocationOfBones(skeleton, skeletonCanvas, depthImageFrame);
}
if (ShowJoints)
{
adjustLocationOfJoints(skeleton, skeletonCanvas, depthImageFrame);
}
}
break;
case SkeletonTrackingState.PositionOnly:
skeletonCanvas.ShowCenter = true;
skeletonCanvas.Visibility = ShowCenter ? Visibility.Visible : Visibility.Hidden;
skeletonCanvas.CenterPoint = getPosition2DLocation(skeleton.Position, skeletonCanvas, depthImageFrame);
break;
default:
//when people leave the scene, hide their skeletons
skeletonCanvas.Visibility = Visibility.Hidden;
break;
}
}
//when people leave the scene, hide their skeleton.
switch (trackedSkeletons)
{
case 0:
skeletonCanvas1.Visibility = System.Windows.Visibility.Hidden;
skeletonCanvas2.Visibility = System.Windows.Visibility.Hidden;
break;
case 1:
skeletonCanvas2.Visibility = System.Windows.Visibility.Hidden;
break;
}
if (ImageType == ImageType.Depth)
{
ChooseTrackedSkeletons(skeletonData);
}
}
}
}
}
/// <summary>
/// Event handler for Kinect sensor's SkeletonFrameReady event
/// </summary>
/// <param name="sender">object sending the event</param>
/// <param name="e">event arguments</param>
private void SensorSkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
Skeleton[] skeletons = new Skeleton[0];
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
}
}
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, RenderWidth, RenderHeight));
if (skeletons.Length != 0)
{
foreach (Skeleton skel in skeletons)
{
//lets do something here
hipXR = skel.Joints[JointType.HipRight].Position.X;
hipYR = skel.Joints[JointType.HipRight].Position.Y;
hipZR = skel.Joints[JointType.HipRight].Position.Z;
handXR = skel.Joints[JointType.HandRight].Position.X;
handYR = skel.Joints[JointType.HandRight].Position.Y;
handZR = skel.Joints[JointType.HandRight].Position.Z;
shXR = skel.Joints[JointType.ShoulderRight].Position.X;
shYR = skel.Joints[JointType.ShoulderRight].Position.Y;
shZR = skel.Joints[JointType.ShoulderRight].Position.Z;
hipXL = skel.Joints[JointType.HipLeft].Position.X;
hipYL = skel.Joints[JointType.HipLeft].Position.Y;
hipZL = skel.Joints[JointType.HipLeft].Position.Z;
handXL = skel.Joints[JointType.HandLeft].Position.X;
handYL = skel.Joints[JointType.HandLeft].Position.Y;
handZL = skel.Joints[JointType.HandLeft].Position.Z;
shXL = skel.Joints[JointType.ShoulderLeft].Position.X;
shYL = skel.Joints[JointType.ShoulderLeft].Position.Y;
shZL = skel.Joints[JointType.ShoulderLeft].Position.Z;
///////////////////////////////////
// servo4();
// servo5();//bool "true" to write to socket
servo3(true);
servo2(true);
double smallDistance = Math.Abs(hipXL - hipXR);
if (Math.Abs(handYL - shYL) > smallDistance)
{
servo5(true);
}
if (Math.Abs(handYR - shYR) > smallDistance)
{
servo4(true);
}
if (skel.Joints[JointType.FootRight].Position.Y > skel.Joints[JointType.KneeLeft].Position.Y)
{
returnRobotHome();
this.Close();
}
////////////////end user code//////////////////////////
RenderClippedEdges(skel, dc);
if (skel.TrackingState == SkeletonTrackingState.Tracked)
{
this.DrawBonesAndJoints(skel, dc);
}
else if (skel.TrackingState == SkeletonTrackingState.PositionOnly)
{
dc.DrawEllipse(
this.centerPointBrush,
null,
this.SkeletonPointToScreen(skel.Position),
BodyCenterThickness,
BodyCenterThickness);
}
}
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, RenderWidth, RenderHeight));
}
}
/// <summary>
/// Event handler for Kinect sensor's SkeletonFrameReady event
/// </summary>
/// <param name="sender">object sending the event</param>
/// <param name="e">event arguments</param>
private void SensorSkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
Skeleton[] skeletons = new Skeleton[0];
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
}
}
if (skeletons.Length != 0)
{
foreach (Skeleton skel in skeletons)
{
if (skel.TrackingState == SkeletonTrackingState.Tracked)
{
DateTime curTime = DateTime.Now;
this.DrawBonesAndJoints(skel, this.MainViewPort);
if (this.lastTouch.AddSeconds(0.4).CompareTo(curTime) < 0 && this.hasTouched)
{
this.hasTouched = false;
this.ball.flyUp();
}
// if the ball is not touched
if (!this.hasTouched)
{
this.DrawSphere3D(this.Point3DChangeView(this.ball.Center), 0.5, Brushes.Red, this.MainViewPort);
}
else
{
this.DrawSphere3D(this.Point3DChangeView(this.ball.Center), 0.5, Brushes.Green, this.MainViewPort);
}
if (this.lastCombo.AddSeconds(1.5).CompareTo(curTime) < 0 &&
this.txtBlkCombo.Visibility == Visibility.Visible)
{
this.txtBlkCombo.Visibility = Visibility.Hidden;
}
if (this.CheckBallTouch(skel) && !this.hasTouched)
{
this.hasTouched = true;
this.scores += 1;
// if it is within one second
if (this.lastTouch.AddSeconds(1.5).CompareTo(curTime) > 0)
{
this.txtBlkCombo.Visibility = Visibility.Visible;
this.scores += 1;
this.lastCombo = curTime;
}
this.lastTouch = curTime;
this.DataLogWindow.Text = "Scores: " + this.scores.ToString();
}
}
}
}
}
// [Add new GestureTriggers here]
// Raised whenever new skeleton data arrives from the Kinect sensor.
// Updates person tracking and performs gesture recognition.
private void SensorSkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
Skeleton[] skeletons = new Skeleton[0];
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
}
}
if (skeletons.Length != 0)
{
bool isTrackingSameSkeleton = false;
int skeletonIdClosestToCenter = 0;
float shortestDistanceFromCenter = 0;
// First, check if this skeleton frame contains the skeleton that we were already tracking.
foreach (Skeleton skel in skeletons)
{
if (skel.TrackingState == SkeletonTrackingState.Tracked)
{
if (skel.TrackingId == trackedSkeletonId)
{
isTrackingSameSkeleton = true;
break;
}
else if (skeletonIdClosestToCenter == 0 || Math.Abs(skel.Joints[JointType.ShoulderCenter].Position.X) < shortestDistanceFromCenter)
{
skeletonIdClosestToCenter = skel.TrackingId;
shortestDistanceFromCenter = Math.Abs(skel.Joints[JointType.ShoulderCenter].Position.X);
}
}
}
// If the skeleton frame does not contain the one we were tracking, then track the skeleton that is closest to the center of the image (x-axis).
if (!isTrackingSameSkeleton)
{
trackedSkeletonId = skeletonIdClosestToCenter;
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "SensorSkeletonFrameReady() now tracking skeleton ID " + trackedSkeletonId.ToString() + ".");
}
// Update person tracking and check gestures from the tracked skeleton.
foreach (Skeleton skel in skeletons)
{
// Only pay attention to the tracked skeleton.
if (skel.TrackingId == trackedSkeletonId)
{
// Only pay attention to the skeleton if it is within the right and left edges of the view.
// This should help prevent detecting false gestures, due to the skeleton being scrambled as it leaves the viewing area.
if ((skel.ClippedEdges & FrameEdges.Left) == 0 && (skel.ClippedEdges & FrameEdges.Right) == 0)
{
personTracking.UpdatePosition(skel.Joints[JointType.ShoulderCenter].Position);
if (gestureRecognitionEnabled)
{
// Check gesture recognition conditions.
//leftHandGesture.Evaluate(sequenceProcessor, skel.Joints[JointType.HandLeft].Position.Y > skel.Joints[JointType.ElbowLeft].Position.Y, "Left Hand Raised");
//rightHandGesture.Evaluate(sequenceProcessor, skel.Joints[JointType.HandRight].Position.Y > skel.Joints[JointType.ElbowRight].Position.Y, "Right Hand Raised");
RightHandAboveHead.Evaluate(sequenceProcessor, skel.Joints[JointType.HandRight].Position.Y > skel.Joints[JointType.Head].Position.Y, "Hello");
RightHandBetweenSpineAndShoulderCenter.Evaluate(sequenceProcessor, skel.Joints[JointType.HandRight].Position.Y > skel.Joints[JointType.Spine].Position.Y && skel.Joints[JointType.HandRight].Position.Y < skel.Joints[JointType.ShoulderCenter].Position.Y && skel.Joints[JointType.HandRight].Position.X < (skel.Joints[JointType.Spine].Position.X + 0.1), "PLAYMUSIC");
LeftElbowAboveLeftShoulder.Evaluate(sequenceProcessor, skel.Joints[JointType.ElbowLeft].Position.Y > skel.Joints[JointType.ShoulderLeft].Position.Y, "NeckRotate");
WalkCloseToKinect.Evaluate(sequenceProcessor, skel.Joints[JointType.Spine].Position.Z < 1.5, "ArmRaise");
// [Add new Evaluate calls here]
}
}
else
{
ErrorLogging.AddMessage(ErrorLogging.LoggingLevel.Debug, "SensorSkeletonFrameReady() skipped update due to skeleton being outside of view.");
}
break;
}
}
}
}
private void OnAllFramesReady(object sender, AllFramesReadyEventArgs allFramesReadyEventArgs)
{
ColorImageFrame colorImageFrame = null;
DepthImageFrame depthImageFrame = null;
SkeletonFrame skeletonFrame = null;
try
{
colorImageFrame = allFramesReadyEventArgs.OpenColorImageFrame();
depthImageFrame = allFramesReadyEventArgs.OpenDepthImageFrame();
skeletonFrame = allFramesReadyEventArgs.OpenSkeletonFrame();
if (colorImageFrame == null || depthImageFrame == null || skeletonFrame == null)
{
return;
}
//redimensionar o array dos esqueletos, se necessario
if (this.skeletons.Length != skeletonFrame.SkeletonArrayLength)
{
this.skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
}
skeletonFrame.CopySkeletonDataTo(this.skeletons);
if (this.colorPixelData == null || colorPixelData.Length != colorImageFrame.PixelDataLength)
{
this.colorPixelData = new byte[colorImageFrame.PixelDataLength];
}
colorImageFrame.CopyPixelDataTo(this.colorPixelData);
//detectar o numero de utilizadores e qual o mais proximo
var newNearestId = -1;
var nearestDistance2 = double.MaxValue;
int nUsers = 0;
int nTrackedUsers = 0;
foreach (var skeleton in skeletons)
{
if (skeleton.TrackingState == SkeletonTrackingState.Tracked)
{
// Find the distance squared.
var distance2 = (skeleton.Position.X * skeleton.Position.X) +
(skeleton.Position.Y * skeleton.Position.Y) +
(skeleton.Position.Z * skeleton.Position.Z);
// Is the new distance squared closer than the nearest so far?
if (distance2 < nearestDistance2)
{
newNearestId = skeleton.TrackingId;
nearestDistance2 = distance2;
}
nTrackedUsers++;
}
if (skeleton.TrackingState != SkeletonTrackingState.NotTracked)
{
nUsers++;
}
}
this.nearestId = newNearestId;
this.NumberOfDetectUsers = nUsers;
//processar a detecao de movimento
if (detectMotion && motionDetector != null)
{
if (this.depthPixelData == null || depthPixelData.Length != depthImageFrame.BytesPerPixel)
{
this.depthPixelData = new short[depthImageFrame.PixelDataLength];
}
depthImageFrame.CopyPixelDataTo(depthPixelData);
var coloredPixels = new byte[colorImageFrame.PixelDataLength];
colorImageFrame.CopyPixelDataTo(coloredPixels);
var colorBitmap = ImageProcessingAux.CreateBitmapFromPixelData(coloredPixels, colorImageFrame.Width, colorImageFrame.Height);
var playerBitmap = ImageProcessingAux.ProccessPlayerPixels(coloredPixels, depthPixelData, colorImageFrame.Format);
motionDetector.ProcessFrame(ref colorBitmap, ref playerBitmap);
}
//processar a deteccao da face
if (trackFace && faceTracker != null)
{
faceTracker.Track(skeletons, colorImageFrame, depthImageFrame, nearestId);
}
//processar gestos
if (gestureDetectorsValue.Count > 0)
{
foreach (Skeleton s in skeletons)
{
if (s.TrackingId == nearestId)
{
foreach (GestureDetector gd in gestureDetectorsValue)
{
gd.Add(s.Joints[gd.TrackedJoint].Position, kinectSensorValue);
}
}
}
}
//processar posturas
if (postureDetectorsValue.Count > 0)
{
foreach (Skeleton s in skeletons)
{
if (s.TrackingId == nearestId)
{
foreach (PostureDetector pd in postureDetectorsValue)
{
pd.Add(s, kinectSensorValue);
}
}
}
}
}
finally
{
if (colorImageFrame != null)
{
colorImageFrame.Dispose();
}
if (depthImageFrame != null)
{
depthImageFrame.Dispose();
}
if (skeletonFrame != null)
{
skeletonFrame.Dispose();
}
}
}
void skeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
canvas1.Children.Remove(drawObject);
if (shapeCondition == 1)
{
/* condition for changing shape */
drawObject = new Ellipse();
}
else if (shapeCondition == 2)
{
drawObject = new Rectangle();
}
if (drawObject != null)
{
drawObject.Height = height;
drawObject.Width = width;
drawObject.Fill = new SolidColorBrush(objectColor);
}
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame == null)
{
return;
}
skeletonFrame.CopySkeletonDataTo(totalSkeleton);
Skeleton firstSkeleton = (from trackskeleton in totalSkeleton
where trackskeleton.TrackingState == SkeletonTrackingState.Tracked
select trackskeleton).FirstOrDefault();
if (firstSkeleton == null)
{
return;
}
if (firstSkeleton.Joints[JointType.WristRight].TrackingState ==
JointTrackingState.Tracked)
{
righthandPoint = this.MapRightJointsWithUIElement(firstSkeleton);
}
if (firstSkeleton.Joints[JointType.WristLeft].TrackingState ==
JointTrackingState.Tracked)
{
lefthandPoint = this.MapLeftJointsWithUIElement(firstSkeleton);
}
if (drawObject != null)
{
/* condition for changing position */
if (positionCondition == 1)
{
drawObject.SetValue(Canvas.LeftProperty, righthandPoint.X);
drawObject.SetValue(Canvas.TopProperty, righthandPoint.Y);
}
else
{
drawObject.SetValue(Canvas.LeftProperty, lefthandPoint.X);
drawObject.SetValue(Canvas.TopProperty, lefthandPoint.Y);
}
canvas1.Children.Add(drawObject);
}
}
}
private void Sensor_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
canvas.Children.Clear();
Skeleton[] esqueletos = null;
using (SkeletonFrame framesEsqueleto = e.OpenSkeletonFrame())
{
if (framesEsqueleto != null)
{
esqueletos = new Skeleton[framesEsqueleto.SkeletonArrayLength];
framesEsqueleto.CopySkeletonDataTo(esqueletos);
}
}
if (esqueletos == null)
{
return;
}
foreach (Skeleton esqueleto in esqueletos)
{
if (esqueleto.TrackingState == SkeletonTrackingState.Tracked)
{
List <SkeletonPoint> spookyScarySkeleton = new List <SkeletonPoint>();
Joint jointCabeza = esqueleto.Joints[JointType.Head];
Joint jointMuñecaIzq = esqueleto.Joints[JointType.WristLeft];
Joint jointMuñecaDer = esqueleto.Joints[JointType.WristRight];
Joint jointManoIzq = esqueleto.Joints[JointType.HandLeft];
Joint jointManoDer = esqueleto.Joints[JointType.HandRight];
Joint jointHombroIzq = esqueleto.Joints[JointType.ShoulderLeft];
Joint jointHombroDer = esqueleto.Joints[JointType.ShoulderRight];
Joint jointHombroCentro = esqueleto.Joints[JointType.ShoulderCenter];
Joint jointCodoIzq = esqueleto.Joints[JointType.ElbowLeft];
Joint jointCodoDer = esqueleto.Joints[JointType.ElbowRight];
Joint jointSpine = esqueleto.Joints[JointType.Spine];
Joint jointCaderaCent = esqueleto.Joints[JointType.HipCenter];
Joint jointCaderaIzq = esqueleto.Joints[JointType.HipLeft];
Joint jointCaderaDer = esqueleto.Joints[JointType.HipRight];
Joint jointRodillaIzq = esqueleto.Joints[JointType.KneeLeft];
Joint jointRodillaDer = esqueleto.Joints[JointType.KneeRight];
Joint jointTobilloIzq = esqueleto.Joints[JointType.AnkleLeft];
Joint jointTobilloDer = esqueleto.Joints[JointType.AnkleRight];
Joint jointPieIzq = esqueleto.Joints[JointType.FootLeft];
Joint jointPieDer = esqueleto.Joints[JointType.FootRight];
SkeletonPoint posicionCabeza = jointCabeza.Position;
SkeletonPoint posicionMuñecaIzq = jointMuñecaIzq.Position;
SkeletonPoint posicionMuñecaDer = jointMuñecaDer.Position;
SkeletonPoint posicionManoIzq = jointManoIzq.Position;
SkeletonPoint posicionManoDer = jointManoDer.Position;
SkeletonPoint posicionHombroIzq = jointHombroIzq.Position;
SkeletonPoint posicionHombroDer = jointHombroDer.Position;
SkeletonPoint posicionHombroCentro = jointHombroCentro.Position;
SkeletonPoint posicionCodoIzq = jointCodoIzq.Position;
SkeletonPoint posicionCodoDer = jointCodoDer.Position;
SkeletonPoint posicionSpine = jointSpine.Position;
SkeletonPoint posicionCaderaCent = jointCaderaCent.Position;
SkeletonPoint posicionCaderaIzq = jointCaderaIzq.Position;
SkeletonPoint posicionCaderaDer = jointCaderaDer.Position;
SkeletonPoint posicionRodillaIzq = jointRodillaIzq.Position;
SkeletonPoint posicionRodillaDer = jointRodillaDer.Position;
SkeletonPoint posicionTobilloIzq = jointTobilloIzq.Position;
SkeletonPoint posicionTobilloDer = jointTobilloDer.Position;
SkeletonPoint posicionPieIzq = jointPieIzq.Position;
SkeletonPoint posicionPieDer = jointPieDer.Position;
spookyScarySkeleton.Add(posicionCabeza);
spookyScarySkeleton.Add(posicionMuñecaIzq);
spookyScarySkeleton.Add(posicionMuñecaDer);
spookyScarySkeleton.Add(posicionManoIzq);
spookyScarySkeleton.Add(posicionManoDer);
spookyScarySkeleton.Add(posicionHombroIzq);
spookyScarySkeleton.Add(posicionHombroDer);
spookyScarySkeleton.Add(posicionHombroCentro);
spookyScarySkeleton.Add(posicionCodoIzq);
spookyScarySkeleton.Add(posicionCodoDer);
spookyScarySkeleton.Add(posicionSpine);
spookyScarySkeleton.Add(posicionCaderaCent);
spookyScarySkeleton.Add(posicionCaderaIzq);
spookyScarySkeleton.Add(posicionCaderaDer);
spookyScarySkeleton.Add(posicionRodillaIzq);
spookyScarySkeleton.Add(posicionRodillaDer);
spookyScarySkeleton.Add(posicionTobilloIzq);
spookyScarySkeleton.Add(posicionTobilloDer);
spookyScarySkeleton.Add(posicionPieIzq);
spookyScarySkeleton.Add(posicionPieDer);
SolidColorBrush colorRojo = new SolidColorBrush(Colors.Red);
Hueso huesoMuñecaIzq = new Hueso(2, colorRojo);
Hueso huesoMuñecaDer = new Hueso(2, colorRojo);
Hueso huesoAntebrazoIzq = new Hueso(2, colorRojo);
Hueso huesoBrazoIzq = new Hueso(2, colorRojo);
Hueso huesoHombroIzq = new Hueso(2, colorRojo);
Hueso huesoCuello = new Hueso(2, colorRojo);
Hueso huesoHombroDer = new Hueso(2, colorRojo);
Hueso huesoBrazoDer = new Hueso(2, colorRojo);
Hueso huesoAntebrazoDer = new Hueso(2, colorRojo);
Hueso huesoSpine = new Hueso(2, colorRojo);
Hueso huesoCaderaCent = new Hueso(2, colorRojo);
Hueso huesoCaderaIzq = new Hueso(2, colorRojo);
Hueso huesoCaderaDer = new Hueso(2, colorRojo);
Hueso huesoMusloIzq = new Hueso(2, colorRojo);
Hueso huesoMusloDer = new Hueso(2, colorRojo);
Hueso huesoPantoIzq = new Hueso(2, colorRojo);
Hueso huesoPantoDer = new Hueso(2, colorRojo);
Hueso huesoPieIzq = new Hueso(2, colorRojo);
Hueso huesoPieDer = new Hueso(2, colorRojo);
ElipseSeteada eliCab = new ElipseSeteada(20, colorRojo);
ElipseSeteada eliManoIzq = new ElipseSeteada(10, colorRojo);
ElipseSeteada eliManoDer = new ElipseSeteada(10, colorRojo);
ElipseSeteada eliMuñecaIzq = new ElipseSeteada(10, colorRojo);
ElipseSeteada eliMuñecaDer = new ElipseSeteada(10, colorRojo);
ElipseSeteada eliHomIzq = new ElipseSeteada(10, colorRojo);
ElipseSeteada eliHomDer = new ElipseSeteada(10, colorRojo);
ElipseSeteada eliHomCent = new ElipseSeteada(10, colorRojo);
ElipseSeteada eliCodoIzq = new ElipseSeteada(10, colorRojo);
ElipseSeteada eliCodoDer = new ElipseSeteada(10, colorRojo);
ElipseSeteada eliSpine = new ElipseSeteada(10, colorRojo);
ElipseSeteada eliCaderaCent = new ElipseSeteada(10, colorRojo);
ElipseSeteada eliCaderaIzq = new ElipseSeteada(10, colorRojo);
ElipseSeteada eliCaderaDer = new ElipseSeteada(10, colorRojo);
ElipseSeteada eliRodillaIzq = new ElipseSeteada(10, colorRojo);
ElipseSeteada eliRodillaDer = new ElipseSeteada(10, colorRojo);
ElipseSeteada eliTobilloIzq = new ElipseSeteada(10, colorRojo);
ElipseSeteada eliTobilloDer = new ElipseSeteada(10, colorRojo);
ElipseSeteada eliPieIzq = new ElipseSeteada(10, colorRojo);
ElipseSeteada eliPieDer = new ElipseSeteada(10, colorRojo);
ColorImagePoint puntoCabeza = sensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointCabeza.Position, ColorImageFormat.RawBayerResolution640x480Fps30);
ColorImagePoint puntoManoIzq = sensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointManoIzq.Position, ColorImageFormat.RawBayerResolution640x480Fps30);
ColorImagePoint puntoManoDer = sensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointManoDer.Position, ColorImageFormat.RawBayerResolution640x480Fps30);
ColorImagePoint puntoMuñecaIzq = sensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointMuñecaIzq.Position, ColorImageFormat.RawBayerResolution640x480Fps30);
ColorImagePoint puntoMuñecaDer = sensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointMuñecaDer.Position, ColorImageFormat.RawBayerResolution640x480Fps30);
ColorImagePoint puntoHomIzq = sensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointHombroIzq.Position, ColorImageFormat.RawBayerResolution640x480Fps30);
ColorImagePoint puntoHomDer = sensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointHombroDer.Position, ColorImageFormat.RawBayerResolution640x480Fps30);
ColorImagePoint puntoHomCent = sensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointHombroCentro.Position, ColorImageFormat.RawBayerResolution640x480Fps30);
ColorImagePoint puntoCodoIzq = sensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointCodoIzq.Position, ColorImageFormat.RawBayerResolution640x480Fps30);
ColorImagePoint puntoCodoDer = sensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointCodoDer.Position, ColorImageFormat.RawBayerResolution640x480Fps30);
ColorImagePoint puntoSpine = sensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointSpine.Position, ColorImageFormat.RawBayerResolution640x480Fps30);
ColorImagePoint puntoCaderaCent = sensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointCaderaCent.Position, ColorImageFormat.RawBayerResolution640x480Fps30);
ColorImagePoint puntoCaderaIzq = sensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointCaderaIzq.Position, ColorImageFormat.RawBayerResolution640x480Fps30);
ColorImagePoint puntoCaderaDer = sensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointCaderaDer.Position, ColorImageFormat.RawBayerResolution640x480Fps30);
ColorImagePoint puntoRodillaIzq = sensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointRodillaIzq.Position, ColorImageFormat.RawBayerResolution640x480Fps30);
ColorImagePoint puntoRodillaDer = sensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointRodillaDer.Position, ColorImageFormat.RawBayerResolution640x480Fps30);
ColorImagePoint puntoTobilloIzq = sensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointTobilloIzq.Position, ColorImageFormat.RawBayerResolution640x480Fps30);
ColorImagePoint puntoTobilloDer = sensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointTobilloDer.Position, ColorImageFormat.RawBayerResolution640x480Fps30);
ColorImagePoint puntoPieIzq = sensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointPieIzq.Position, ColorImageFormat.RawBayerResolution640x480Fps30);
ColorImagePoint puntoPieDer = sensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointPieDer.Position, ColorImageFormat.RawBayerResolution640x480Fps30);
eliCab.setPosicion(puntoCabeza.X, puntoCabeza.Y);
eliManoIzq.setPosicion(puntoManoIzq.X, puntoManoIzq.Y);
eliManoDer.setPosicion(puntoManoDer.X, puntoManoDer.Y);
eliMuñecaIzq.setPosicion(puntoMuñecaIzq.X, puntoMuñecaIzq.Y);
eliMuñecaDer.setPosicion(puntoMuñecaDer.X, puntoMuñecaDer.Y);
eliHomIzq.setPosicion(puntoHomIzq.X, puntoHomIzq.Y);
eliHomDer.setPosicion(puntoHomDer.X, puntoHomDer.Y);
eliHomCent.setPosicion(puntoHomCent.X, puntoHomCent.Y);
eliCodoIzq.setPosicion(puntoCodoIzq.X, puntoCodoIzq.Y);
eliCodoDer.setPosicion(puntoCodoDer.X, puntoCodoDer.Y);
eliSpine.setPosicion(puntoSpine.X, puntoSpine.Y);
eliCaderaCent.setPosicion(puntoCaderaCent.X, puntoCaderaCent.Y);
eliCaderaIzq.setPosicion(puntoCaderaIzq.X, puntoCaderaIzq.Y);
eliCaderaDer.setPosicion(puntoCaderaDer.X, puntoCaderaDer.Y);
eliRodillaIzq.setPosicion(puntoRodillaIzq.X, puntoRodillaIzq.Y);
eliRodillaDer.setPosicion(puntoRodillaDer.X, puntoRodillaDer.Y);
eliTobilloIzq.setPosicion(puntoTobilloIzq.X, puntoTobilloIzq.Y);
eliTobilloDer.setPosicion(puntoTobilloDer.X, puntoTobilloDer.Y);
eliPieIzq.setPosicion(puntoPieIzq.X, puntoPieIzq.Y);
eliPieDer.setPosicion(puntoPieDer.X, puntoPieDer.Y);
huesoMuñecaIzq.setPosicionUno(puntoManoIzq.X, puntoManoIzq.Y);
huesoMuñecaIzq.setPosicionDos(puntoMuñecaIzq.X, puntoMuñecaIzq.Y);
huesoMuñecaDer.setPosicionUno(puntoManoDer.X, puntoManoDer.Y);
huesoMuñecaDer.setPosicionDos(puntoMuñecaDer.X, puntoMuñecaDer.Y);
huesoAntebrazoIzq.setPosicionUno(puntoMuñecaIzq.X, puntoMuñecaIzq.Y);
huesoAntebrazoIzq.setPosicionDos(puntoCodoIzq.X, puntoCodoIzq.Y);
huesoBrazoIzq.setPosicionUno(puntoCodoIzq.X, puntoCodoIzq.Y);
huesoBrazoIzq.setPosicionDos(puntoHomIzq.X, puntoHomIzq.Y);
huesoHombroIzq.setPosicionUno(puntoHomIzq.X, puntoHomIzq.Y);
huesoHombroIzq.setPosicionDos(puntoHomCent.X, puntoHomCent.Y);
huesoCuello.setPosicionUno(puntoHomCent.X, puntoHomCent.Y);
huesoCuello.setPosicionDos(puntoCabeza.X, puntoCabeza.Y);
huesoHombroDer.setPosicionUno(puntoHomCent.X, puntoHomCent.Y);
huesoHombroDer.setPosicionDos(puntoHomDer.X, puntoHomDer.Y);
huesoBrazoDer.setPosicionUno(puntoHomDer.X, puntoHomDer.Y);
huesoBrazoDer.setPosicionDos(puntoCodoDer.X, puntoCodoDer.Y);
huesoAntebrazoDer.setPosicionUno(puntoCodoDer.X, puntoCodoDer.Y);
huesoAntebrazoDer.setPosicionDos(puntoMuñecaDer.X, puntoMuñecaDer.Y);
huesoSpine.setPosicionUno(puntoHomCent.X, puntoHomCent.Y);
huesoSpine.setPosicionDos(puntoSpine.X, puntoSpine.Y);
huesoCaderaCent.setPosicionUno(puntoSpine.X, puntoSpine.Y);
huesoCaderaCent.setPosicionDos(puntoCaderaCent.X, puntoCaderaCent.Y);
huesoCaderaIzq.setPosicionUno(puntoCaderaCent.X, puntoCaderaCent.Y);
huesoCaderaIzq.setPosicionDos(puntoCaderaIzq.X, puntoCaderaIzq.Y);
huesoCaderaDer.setPosicionUno(puntoCaderaCent.X, puntoCaderaCent.Y);
huesoCaderaDer.setPosicionDos(puntoCaderaDer.X, puntoCaderaDer.Y);
huesoMusloIzq.setPosicionUno(puntoCaderaIzq.X, puntoCaderaIzq.Y);
huesoMusloIzq.setPosicionDos(puntoRodillaIzq.X, puntoRodillaIzq.Y);
huesoMusloDer.setPosicionUno(puntoCaderaDer.X, puntoCaderaDer.Y);
huesoMusloDer.setPosicionDos(puntoRodillaDer.X, puntoRodillaDer.Y);
huesoPantoIzq.setPosicionUno(puntoRodillaIzq.X, puntoRodillaIzq.Y);
huesoPantoIzq.setPosicionDos(puntoTobilloIzq.X, puntoTobilloIzq.Y);
huesoPantoDer.setPosicionUno(puntoRodillaDer.X, puntoRodillaDer.Y);
huesoPantoDer.setPosicionDos(puntoTobilloDer.X, puntoTobilloDer.Y);
huesoPieIzq.setPosicionUno(puntoTobilloIzq.X, puntoTobilloIzq.Y);
huesoPieIzq.setPosicionDos(puntoPieIzq.X, puntoPieIzq.Y);
huesoPieDer.setPosicionUno(puntoTobilloDer.X, puntoTobilloDer.Y);
huesoPieDer.setPosicionDos(puntoPieDer.X, puntoPieDer.Y);
canvas.Children.Add(eliCab.miElipse);
canvas.Children.Add(eliManoIzq.miElipse);
canvas.Children.Add(eliManoDer.miElipse);
canvas.Children.Add(eliMuñecaIzq.miElipse);
canvas.Children.Add(eliMuñecaDer.miElipse);
canvas.Children.Add(eliHomIzq.miElipse);
canvas.Children.Add(eliHomDer.miElipse);
canvas.Children.Add(eliHomCent.miElipse);
canvas.Children.Add(eliCodoIzq.miElipse);
canvas.Children.Add(eliCodoDer.miElipse);
canvas.Children.Add(eliSpine.miElipse);
canvas.Children.Add(eliCaderaCent.miElipse);
canvas.Children.Add(eliCaderaIzq.miElipse);
canvas.Children.Add(eliCaderaDer.miElipse);
canvas.Children.Add(eliRodillaIzq.miElipse);
canvas.Children.Add(eliRodillaDer.miElipse);
canvas.Children.Add(eliTobilloIzq.miElipse);
canvas.Children.Add(eliTobilloDer.miElipse);
canvas.Children.Add(eliPieIzq.miElipse);
canvas.Children.Add(eliPieDer.miElipse);
canvas.Children.Add(huesoMuñecaIzq.linea);
canvas.Children.Add(huesoMuñecaDer.linea);
canvas.Children.Add(huesoAntebrazoIzq.linea);
canvas.Children.Add(huesoBrazoIzq.linea);
canvas.Children.Add(huesoHombroIzq.linea);
canvas.Children.Add(huesoCuello.linea);
canvas.Children.Add(huesoHombroDer.linea);
canvas.Children.Add(huesoAntebrazoDer.linea);
canvas.Children.Add(huesoBrazoDer.linea);
canvas.Children.Add(huesoSpine.linea);
canvas.Children.Add(huesoCaderaCent.linea);
canvas.Children.Add(huesoCaderaIzq.linea);
canvas.Children.Add(huesoCaderaDer.linea);
canvas.Children.Add(huesoMusloIzq.linea);
canvas.Children.Add(huesoMusloDer.linea);
canvas.Children.Add(huesoPantoIzq.linea);
canvas.Children.Add(huesoPantoDer.linea);
canvas.Children.Add(huesoPieIzq.linea);
canvas.Children.Add(huesoPieDer.linea);
Console.WriteLine(((posicionManoDer.Y + posicionManoIzq.Y) / 2));
if (((posicionManoIzq.Y + posicionManoDer.Y) / 2) > 0.8)
{
posicion.Source = images[0];
}
if (Math.Round((posicionCabeza.Y)) < 0.1)
{
posicion.Source = images[1];
}
lbSkel.Content = string.Format(": X:{0} , Y:{1} , Z:{2}", posicionSpine.X, posicionSpine.Y, posicionSpine.Z);
//bool leftyEdge;
//bool rightyEdge;
//bool isClose;
//bool isFar;
//if (posicionSpine.Z < 1.5 )
//{
// isClose = true;
// isFar = false;
// posicion.Source = images[5];
//}
//else
//{
// isClose = false;
// isFar = true;
// posicion.Source = images[3];
//}
//if (posicionSpine.X < -0.4)
//{
// leftyEdge = true;
// posicion.Source = images[2];
//}
//else
// rightyEdge = false;
//if (posicionSpine.X > 0.4 )
//{
// rightyEdge = true;
// posicion.Source = images[4];
//}
//else
// leftyEdge = false;
}
}
}
private void KinectDevice_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
using (SkeletonFrame frame = e.OpenSkeletonFrame())
{
if (frame != null)
{
Polyline figure;
Brush userBrush;
Skeleton skeleton;
JointType[] joints;
LayoutRoot.Children.Clear();
frame.CopySkeletonDataTo(this._FrameSkeletons);
for (int i = 0; i < this._FrameSkeletons.Length; i++)
{
skeleton = this._FrameSkeletons[i];
if (skeleton.TrackingState == SkeletonTrackingState.Tracked)
{
userBrush = this._SkeletonBrushes[i % this._SkeletonBrushes.Length];
//Draws the skeleton’s head and torso
joints = new[] {
JointType.Head,
JointType.ShoulderCenter,
JointType.ShoulderLeft,
JointType.Spine,
JointType.ShoulderRight,
JointType.ShoulderCenter,
JointType.HipCenter,
JointType.HipLeft,
JointType.Spine,
JointType.HipRight,
JointType.HipCenter
};
LayoutRoot.Children.Add(CreateFigure(skeleton, userBrush, joints));
//Draws the skeleton’s left leg
joints = new[] {
JointType.HipLeft,
JointType.KneeLeft,
JointType.AnkleLeft,
JointType.FootLeft
};
LayoutRoot.Children.Add(CreateFigure(skeleton, userBrush, joints));
//Draws the skeleton’s right leg
joints = new[] {
JointType.HipRight,
JointType.KneeRight,
JointType.AnkleRight,
JointType.FootRight
};
LayoutRoot.Children.Add(CreateFigure(skeleton, userBrush, joints));
//Draws the skeleton’s left arm
joints = new[] {
JointType.ShoulderLeft,
JointType.ElbowLeft,
JointType.WristLeft,
JointType.HandLeft
};
LayoutRoot.Children.Add(CreateFigure(skeleton, userBrush, joints));
//Draws the skeleton’s right arm
joints = new[] {
JointType.ShoulderRight,
JointType.ElbowRight,
JointType.WristRight,
JointType.HandRight
};
LayoutRoot.Children.Add(CreateFigure(skeleton, userBrush, joints));
}
}
}
}
}
///<summary>
///RGBカメラ、距離カメラ、骨格のフレーム更新イベント
///</summary>
///<param name="sender"></param>
///<param name="e"></param>
void kinect_AllFramesReady(object sender, AllFramesReadyEventArgs e)
{
try
{
//Kinectのインスタンスを取得する
KinectSensor kinect = sender as KinectSensor;
if (kinect == null)
{
return;
}
//RGBカメラのフレームデータを取得する
using (ColorImageFrame colorFrame = e.OpenColorImageFrame())
{
if (colorFrame != null)
{
//RGBカメラのピクセルデータを取得する
byte[] colorPixel = new byte[colorFrame.PixelDataLength];
colorFrame.CopyPixelDataTo(colorPixel);
//ピクセルデータをビットマップに変換する
Bitmap bitmap =
new Bitmap(kinect.ColorStream.FrameWidth,
kinect.ColorStream.FrameHeight,
System.Drawing.Imaging.PixelFormat.Format32bppRgb);
Rectangle rect = new Rectangle(0, 0, bitmap.Width, bitmap.Height);
BitmapData data = bitmap.LockBits(rect, System.Drawing.Imaging.ImageLockMode.WriteOnly,
System.Drawing.Imaging.PixelFormat.Format32bppRgb);
Marshal.Copy(colorPixel, 0, data.Scan0, colorPixel.Length);
bitmap.UnlockBits(data);
pictureBoxRgb.Image = bitmap;
}
}
//スケルトンのフレームを取得する
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
const int R = 5;
Graphics g = Graphics.FromImage(pictureBoxRgb.Image);
skeletons =
new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
//トラッキングされているスケルトンのジョイントを描画する
foreach (var skeleton in skeletons)
{
//スケルトンがトラッキングされてなければ次へ
if (skeleton.TrackingState != SkeletonTrackingState.Tracked)
{
continue;
}
//ジョイントを描画する
foreach (Joint joint in skeleton.Joints)
{
//ジョイントがトラッキングされていなければ次へ
if (joint.TrackingState != JointTrackingState.Tracked)
{
continue;
}
//スケルトンの座標を、RGBカメラの座標に変換して円を書く
ColorImagePoint point =
kinect.MapSkeletonPointToColor(joint.Position, kinect.ColorStream.Format);
g.DrawEllipse(new Pen(Brushes.Red),
new Rectangle(point.X - R, point.Y - R, R * 2, R * 2));
}
}
}
//////////////////////////////////////////メソッド
Judge(kinect, skeletonFrame);
//////////////////////////////////////////////
}
}
catch (Exception ex)
{
MessageBox.Show(ex.Message);
}
}
/// <summary>
/// Event handler for Kinect sensor's SkeletonFrameReady event
/// </summary>
/// <param name="sender">object sending the event</param>
/// <param name="e">event arguments</param>
private void SensorSkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
Skeleton[] skeletons = new Skeleton[0];
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
}
}
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, RenderWidth, RenderHeight));
if (skeletons.Length != 0)
{
foreach (Skeleton skel in skeletons)
{
RenderClippedEdges(skel, dc);
if (skel.TrackingState == SkeletonTrackingState.Tracked)
{
this.DrawBonesAndJoints(skel, dc);
}
else if (skel.TrackingState == SkeletonTrackingState.PositionOnly)
{
dc.DrawEllipse(
this.centerPointBrush,
null,
this.SkeletonPointToScreen(skel.Position),
BodyCenterThickness,
BodyCenterThickness);
}
}
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, RenderWidth, RenderHeight));
}
if (skeletons.Length != 0)
{
//射擊
Thread shoot = new Thread(() => shooting(skeletons));
shoot.Start();
for (int i = 0; i < skeletons.Length; i++)
{
if ((skeletons[i].TrackingState == SkeletonTrackingState.Tracked) || (skeletons[i].TrackingState == SkeletonTrackingState.PositionOnly))
{
JointCollection joint = skeletons[i].Joints;
double Hip_distance = Math.Abs(joint[JointType.HipRight].Position.X - joint[JointType.HipLeft].Position.X);
//膝蓋間(X軸)的距離
double kneeDistance = Math.Abs(joint[JointType.KneeRight].Position.X - joint[JointType.KneeLeft].Position.X);
//屁股到膝蓋間(x,z軸)的距離
double kneeHipDistance = Math.Sqrt(Math.Pow(joint[JointType.HipLeft].Position.Z - joint[JointType.KneeLeft].Position.Z, 2) + Math.Pow(joint[JointType.HipLeft].Position.X - joint[JointType.KneeLeft].Position.X, 2));
//Console.WriteLine("Distance: " + kneeHipDistance);
//Console.WriteLine("Knee: " + kneeDistance);
if (mouse_state == 0)
{
// 模擬開車w(前)
if (joint[JointType.KneeRight].Position.Z < (joint[JointType.HipRight].Position.Z - 0.04))
{
if (shift_on == 1)
{
keybd_event(VK_shift, (byte)MapVirtualKey(VK_shift, 0), KEYEVENTF_KEYUP, (IntPtr)0);
shift_on = 0;
}
W_counter++;
if (W_counter < 12)
{
keybd_event(VK_W, (byte)MapVirtualKey(VK_W, 0), KEYEVENTF_EXTENDEDKEY, (IntPtr)0);
}
Console.WriteLine("Left amount: " + (joint[JointType.AnkleRight].Position.X - joint[JointType.KneeRight].Position.X));
Console.WriteLine("Right amount: " + (joint[JointType.KneeRight].Position.X - joint[JointType.HipRight].Position.X));
// 模擬A(向左)+(大彎)
if (joint[JointType.AnkleRight].Position.X > (joint[JointType.KneeRight].Position.X + 0.09))
{
Console.WriteLine("Left Big Turn");
keybd_event(VK_W, (byte)MapVirtualKey(VK_W, 0), KEYEVENTF_KEYUP, (IntPtr)0);
keybd_event(VK_A, (byte)MapVirtualKey(VK_A, 0), KEYEVENTF_EXTENDEDKEY, (IntPtr)0);
}
// 模擬D(向右)+(大彎)
else if (joint[JointType.KneeRight].Position.X > (joint[JointType.HipRight].Position.X + 0.1))
{
Console.WriteLine("Right Big Turn");
keybd_event(VK_W, (byte)MapVirtualKey(VK_W, 0), KEYEVENTF_KEYUP, (IntPtr)0);
keybd_event(VK_D, (byte)MapVirtualKey(VK_D, 0), KEYEVENTF_EXTENDEDKEY, (IntPtr)0);
}
// 模擬A(向左)
else if (joint[JointType.AnkleRight].Position.X > (joint[JointType.KneeRight].Position.X + 0.02))
{
Console.WriteLine("Left Small Turn");
keybd_event(VK_D, (byte)MapVirtualKey(VK_D, 0), KEYEVENTF_KEYUP, (IntPtr)0);
if (A_counter < 1)
{
keybd_event(VK_A, (byte)MapVirtualKey(VK_A, 0), KEYEVENTF_EXTENDEDKEY, (IntPtr)0);
}
else
{
keybd_event(VK_A, (byte)MapVirtualKey(VK_A, 0), KEYEVENTF_KEYUP, (IntPtr)0);
}
A_counter++;
}
// 模擬D(向右)
else if (joint[JointType.KneeRight].Position.X > (joint[JointType.HipRight].Position.X + 0.04))
{
Console.WriteLine("Right Small Turn");
keybd_event(VK_A, (byte)MapVirtualKey(VK_A, 0), KEYEVENTF_KEYUP, (IntPtr)0);
if (D_counter < 1)
{
keybd_event(VK_D, (byte)MapVirtualKey(VK_D, 0), KEYEVENTF_EXTENDEDKEY, (IntPtr)0);
}
else
{
keybd_event(VK_D, (byte)MapVirtualKey(VK_D, 0), KEYEVENTF_KEYUP, (IntPtr)0);
}
D_counter++;
}
else
{
A_counter = 0;
D_counter = 0;
keybd_event(VK_A, (byte)MapVirtualKey(VK_A, 0), KEYEVENTF_KEYUP, (IntPtr)0);
keybd_event(VK_D, (byte)MapVirtualKey(VK_D, 0), KEYEVENTF_KEYUP, (IntPtr)0);
}
}
// 模擬W(向前+跑)
else if (kneeHipDistance > 0.147)
{
keybd_event(VK_W, (byte)MapVirtualKey(VK_W, 0), KEYEVENTF_EXTENDEDKEY, (IntPtr)0);
keybd_event(VK_shift, (byte)MapVirtualKey(VK_shift, 0), KEYEVENTF_EXTENDEDKEY, (IntPtr)0);
shift_on = 1;
// 模擬滑鼠(向左滑)
if (joint[JointType.HipLeft].Position.X > (joint[JointType.KneeLeft].Position.X + 0.05))
{
int moveAmount = (int)((double)(kneeDistance - 0.227) * 1000);
//Console.WriteLine("moveLeft: " + moveAmount);
mouse_event(MOUSEEVENTF_MOVE, (-1) * moveAmount, 0, 0, 0);
}
// 模擬滑鼠(向右滑)
else if (joint[JointType.AnkleLeft].Position.X < (joint[JointType.KneeLeft].Position.X - 0.018))
{
int moveAmount = (int)((double)(0.217 - kneeDistance) * 1100);
//Console.WriteLine("moveRight: " + moveAmount);
mouse_event(MOUSEEVENTF_MOVE, moveAmount, 0, 0, 0);
}
}
else if (kneeHipDistance > 0.06)
{
if (shift_on == 1)
{
keybd_event(VK_shift, (byte)MapVirtualKey(VK_shift, 0), KEYEVENTF_KEYUP, (IntPtr)0);
shift_on = 0;
}
keybd_event(VK_W, (byte)MapVirtualKey(VK_W, 0), KEYEVENTF_EXTENDEDKEY, (IntPtr)0);
// 模擬滑鼠(向左滑)
if (joint[JointType.HipLeft].Position.X > (joint[JointType.KneeLeft].Position.X + 0.05))
{
int moveAmount = (int)((double)(kneeDistance - 0.23) * 1000);
//Console.WriteLine("moveLeft: " + moveAmount);
mouse_event(MOUSEEVENTF_MOVE, (-1) * moveAmount, 0, 0, 0);
}
// 模擬滑鼠(向右滑)
else if (joint[JointType.AnkleLeft].Position.X < (joint[JointType.KneeLeft].Position.X - 0.018))
{
int moveAmount = (int)((double)(0.212 - kneeDistance) * 1100);
//Console.WriteLine("moveRight: " + moveAmount);
mouse_event(MOUSEEVENTF_MOVE, moveAmount, 0, 0, 0);
}
}
else
{
if (shift_on == 1)
{
keybd_event(VK_shift, (byte)MapVirtualKey(VK_shift, 0), KEYEVENTF_KEYUP, (IntPtr)0);
shift_on = 0;
}
keybd_event(VK_D, (byte)MapVirtualKey(VK_D, 0), KEYEVENTF_KEYUP, (IntPtr)0);
keybd_event(VK_A, (byte)MapVirtualKey(VK_A, 0), KEYEVENTF_KEYUP, (IntPtr)0);
keybd_event(VK_W, (byte)MapVirtualKey(VK_W, 0), KEYEVENTF_KEYUP, (IntPtr)0);
W_counter = 0;
A_counter = 0;
D_counter = 0;
}
}
directionCounter();
{
/*
* // 模擬W(向前)
* if (joint[JointType.KneeLeft].Position.Z < (joint[JointType.HipLeft].Position.Z - 0.03))
* {
* keybd_event(VK_W, (byte)MapVirtualKey(VK_W, 0), KEYEVENTF_EXTENDEDKEY, (IntPtr)0);
* }
* // 模擬w(前+跑)
* else if (joint[JointType.KneeRight].Position.Z < (joint[JointType.HipRight].Position.Z - 0.03))
* {
* keybd_event(VK_W, (byte)MapVirtualKey(VK_W, 0), KEYEVENTF_EXTENDEDKEY, (IntPtr)0);
* keybd_event(VK_shift, (byte)MapVirtualKey(VK_shift, 0), KEYEVENTF_EXTENDEDKEY, (IntPtr)0);
* shift_on = 1;
* }
* else
* {
* if (shift_on == 1)
* {
* keybd_event(VK_shift, (byte)MapVirtualKey(VK_shift, 0), KEYEVENTF_KEYUP, (IntPtr)0);
* shift_on = 0;
* }
* keybd_event(VK_W, (byte)MapVirtualKey(VK_W, 0), KEYEVENTF_KEYUP, (IntPtr)0);
* }*/
/*
* // 模擬D(向右)
* if ((Hip_distance < 0.144) && (joint[JointType.Head].Position.X-0.02 > joint[JointType.ShoulderCenter].Position.X) && (D_counter <= 3))
* {
* keybd_event(VK_D, (byte)MapVirtualKey(VK_D, 0), KEYEVENTF_EXTENDEDKEY, (IntPtr)0);
* }
* else
* {
* keybd_event(VK_D, (byte)MapVirtualKey(VK_D, 0), KEYEVENTF_KEYUP, (IntPtr)0);
* D_counter = 0;
* }
* // 模擬A(向左)
* if ((Hip_distance < 0.144) && (joint[JointType.Head].Position.X+0.02 < joint[JointType.ShoulderCenter].Position.X) && (A_counter <= 3))
* {
* keybd_event(VK_A, (byte)MapVirtualKey(VK_A, 0), KEYEVENTF_EXTENDEDKEY, (IntPtr)0);
* }
* else
* {
* A_counter = 0;
* keybd_event(VK_A, (byte)MapVirtualKey(VK_A, 0), KEYEVENTF_KEYUP, (IntPtr)0);
* }*/
}
// 模擬s(後退)
if (joint[JointType.HandRight].Position.Z > joint[JointType.HipCenter].Position.Z + 0.03 && joint[JointType.HandLeft].Position.Z > joint[JointType.HipCenter].Position.Z + 0.03)
{
keybd_event(VK_S, (byte)MapVirtualKey(VK_S, 0), KEYEVENTF_EXTENDEDKEY, (IntPtr)0);
}
else
{
keybd_event(VK_S, (byte)MapVirtualKey(VK_S, 0), KEYEVENTF_KEYUP, (IntPtr)0);
}
// 模擬空白建(跳躍)
if ((joint[JointType.Spine].Position.Y > spine_coord + 0.025) && (mouse_state == 0))
{
keybd_event(VK_space, (byte)MapVirtualKey(VK_space, 0), KEYEVENTF_EXTENDEDKEY, (IntPtr)0);
}
else
{
keybd_event(VK_space, (byte)MapVirtualKey(VK_space, 0), KEYEVENTF_KEYUP, (IntPtr)0);
}
// 模擬F(左手開門)
if ((joint[JointType.HandRight].Position.Y < joint[JointType.ElbowRight].Position.Y) && (joint[JointType.HandLeft].Position.Y > joint[JointType.HipCenter].Position.Y) && (joint[JointType.HandLeft].Position.X < joint[JointType.ElbowLeft].Position.X))
{
keybd_event(VK_F, (byte)MapVirtualKey(VK_F, 0), KEYEVENTF_EXTENDEDKEY, (IntPtr)0);
}
else
{
keybd_event(VK_F, (byte)MapVirtualKey(VK_F, 0), KEYEVENTF_KEYUP, (IntPtr)0);
}
// 模擬滑鼠滾輪(選武器)
if ((joint[JointType.HandLeft].Position.Y < joint[JointType.ElbowLeft].Position.Y) && (joint[JointType.HandRight].Position.Y > joint[JointType.Spine].Position.Y) && (joint[JointType.HandRight].Position.X > joint[JointType.ElbowRight].Position.X))
{
wheel_count++;
if (wheel_count <= 1)
{
mouse_event(MOUSEEVENTF_WHEEL, 0, 0, 120, 0);
}
else if (wheel_count > 20)
{
wheel_count = 0;
}
}
else
{
wheel_count = 0;
}
//紀錄目前身體中心的Y軸
spine_coord = joint[JointType.Spine].Position.Y;
}
}
}
}
private void skeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
ClearSkeleton();
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame == null)
{
return;
}
skeletonFrame.CopySkeletonDataTo(totalSkeleton);
skeleton = (from trackskeleton in totalSkeleton
where trackskeleton.TrackingState == SkeletonTrackingState.Tracked
select trackskeleton).FirstOrDefault();
if (skeleton == null)
{
return;
}
if (skeleton != null && this.currentSkeletonID != skeleton.TrackingId)
{
this.currentSkeletonID = skeleton.TrackingId;
int totalTrackedJoints = skeleton.Joints.Where(item => item.TrackingState == JointTrackingState.Tracked).Count();
string status = "\nSkeleton ID:" + this.currentSkeletonID + ", total tracked joints: " + totalTrackedJoints;
//this.status.Text += status;
}
DrawSkeleton(skeleton);
}
// Determine if the current test object is touching the right hand, if it is make the object follow the hand
Point RightHandPoint = ScalePosition(skeleton.Joints[JointType.HandRight].Position);
if (object_id < TEST_LENGTH && testObjects[object_id].Touch(RightHandPoint))
{
testObjects[object_id].center.X = RightHandPoint.X;
testObjects[object_id].center.Y = RightHandPoint.Y;
Canvas.SetTop(testObjects[object_id].shape, testObjects[object_id].center.Y - testObjects[object_id].size / 2);
Canvas.SetLeft(testObjects[object_id].shape, testObjects[object_id].center.X - testObjects[object_id].size / 2);
//this.statusBar.Text += "grabbed!\n";
// Determine of the current test object is in the zone
if (!this.finished) //if not finished
{
for (int i = 0; i < resultZones.Length; i++)
{
// if object in drop zone
if (resultZones[i].inZone(testObjects[object_id].center))
{
// get object shape and color
Shape obj_shape = testObjects[object_id].shape;
Brush obj_color = testObjects[object_id].shape.Fill;
// check for marching based on game type
if ((gameOption == "shape" && ((i == 0 && obj_shape is System.Windows.Shapes.Ellipse) ||
(i == 1 && obj_shape is System.Windows.Shapes.Rectangle) ||
(i == 2 && obj_shape is System.Windows.Shapes.Polygon)
)) ||
(gameOption == "color" && ((i == 0 && obj_color == Brushes.Red) ||
(i == 1 && obj_color == Brushes.Green) ||
(i == 2 && obj_color == Brushes.Blue)
)) ||
(gameOption == "both" && ((i == 0 && obj_shape is System.Windows.Shapes.Ellipse && obj_color == Brushes.Red ||
(i == 1 && obj_shape is System.Windows.Shapes.Rectangle && obj_color == Brushes.Green) ||
(i == 2 && obj_shape is System.Windows.Shapes.Polygon && obj_color == Brushes.Blue)
))
))
{
// it's a match
status.Content = "That's a match!";
status.Foreground = Brushes.Green;
testObjects[object_id].center = resultZones[i].center;
// place the object into the zone
Canvas.SetTop(testObjects[object_id].shape, testObjects[object_id].center.Y - testObjects[object_id].size / 2);
Canvas.SetLeft(testObjects[object_id].shape, testObjects[object_id].center.X - testObjects[object_id].size / 2);
// draw the next object
drawNextObject();
}
else
{
// it's not a match
status.Content = "That's NOT a match! Try another bin :)";
status.Foreground = Brushes.Blue;
}
// a quick timer to clear the status
StartTimer();
}
}
}
}
}
private void SensorAllFramesReady(object sender, AllFramesReadyEventArgs e)
{
Skeleton[] skeletons = new Skeleton[0];
bool depthReceived = false;
_ultimoFrameTomado = DateTime.Now.Ticks;
using (DepthImageFrame framesDistancia = e.OpenDepthImageFrame())
{
if (framesDistancia == null)
{
return;
}
framesDistancia.CopyDepthImagePixelDataTo(this.depthPixels);
depthReceived = true;
if (datosDistancia == null)
{
datosDistancia = new short[framesDistancia.PixelDataLength];
}
if (colorImagenDistancia == null)
{
colorImagenDistancia = new byte[framesDistancia.PixelDataLength * 4];
}
framesDistancia.CopyPixelDataTo(datosDistancia);
}
using (ColorImageFrame colorFrame = e.OpenColorImageFrame())
{
if (colorFrame != null)
{
colorFrame.CopyPixelDataTo(this.colorPixels);
System.Drawing.Bitmap bmp = EmguCVHelper.ImageToBitmap(colorFrame);
videoController.InicioGrabacion = DateTime.Now.Ticks;
if (videoController.framesBmp.Count < 2000)
{
videoController.framesBmp.Add(bmp);
}
else
{
videoController.ForzarReinicioDeVideo();
}
Image <Hsv, Byte> currentFrameHSV = new Image <Hsv, byte>(bmp);
Image <Gray, Byte> grayFrame = currentFrameHSV.Convert <Gray, Byte>();
Image <Gray, Byte> imageHSVDest = currentFrameHSV.InRange(lowerLimit, upperLimit);
imageHSVDest.Erode(100);
VectorOfVectorOfPoint vectorOfPoint = EmguCVHelper.FindContours(imageHSVDest);
for (int i = 0; i < vectorOfPoint.Size; i++)
{
var contour = vectorOfPoint[i];
var area = CvInvoke.ContourArea(contour);
if (area > 100)
{
System.Drawing.Rectangle rec = CvInvoke.BoundingRectangle(contour);
Point p1 = new Point(rec.X, rec.Y);
Point p2 = new Point(rec.X + rec.Width, rec.Y + rec.Height);
ObjetoX = (p1.X + p2.X) / 2;
ObjetoY = (p1.Y + p2.Y) / 2;
if (true == depthReceived)
{
this.sensor.CoordinateMapper.MapDepthFrameToColorFrame(
DepthFormat,
this.depthPixels,
ColorFormat,
this.colorCoordinates);
int depthIndex = (int)ObjetoX + ((int)ObjetoY * this.depthWidth);
DepthImagePixel depthPixel = this.depthPixels[depthIndex];
ObjetoZ = datosDistancia[depthIndex] >> 3;
int X = (int)ObjetoX / this.colorToDepthDivisor;
int Y = (int)ObjetoY / this.colorToDepthDivisor;
}
if (ObjetoZ > 0)
{
skelObjeto = DistanceHelper.ObtenerSkelPoint((int)ObjetoX, (int)ObjetoY,
ObjetoZ, this.sensor);
flagObjeto = true;
}
}
}
colorFrame.CopyPixelDataTo(this.colorPixels);
// Write the pixel data into our bitmap
this.colorBitmap.WritePixels(
new Int32Rect(0, 0, this.colorBitmap.PixelWidth, this.colorBitmap.PixelHeight),
this.colorPixels,
this.colorBitmap.PixelWidth * sizeof(int),
0);
}
}
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
}
}
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, RenderWidth, RenderHeight));
dc.DrawImage(this.colorBitmap, new Rect(0.0, 0.0, RenderWidth, RenderHeight));
if (skeletons.Length != 0)
{
foreach (Skeleton skel in skeletons)
{
//Skeleton skel = skeletons[0];
if (skel.TrackingState == SkeletonTrackingState.Tracked)
{
this.DrawBonesAndJoints(skel, dc);
}
//Toma de mediciones de mano, hombro y codo derecho:
ManoDerecha = skel.Joints[JointType.HandRight];
//Joint munecaDer = skel.Joints[JointType.WristRight];
CodoDerecho = skel.Joints[JointType.ElbowRight];
HombroDerecho = skel.Joints[JointType.ShoulderRight];
//Dibujo un punto negro sobre el objeto detectado
Point objeto = new Point(this.ObjetoX, this.ObjetoY);
dc.DrawEllipse(Brushes.Black, new Pen(Brushes.Black, 5), objeto, 5, 5);
if ((HombroDerecho.TrackingState == JointTrackingState.Tracked) &&
(ManoDerecha.TrackingState == JointTrackingState.Tracked) &&
(CodoDerecho.TrackingState == JointTrackingState.Tracked))
{
if (flagObjeto && !flagSkeleton)
{
puntos = new Puntos(HombroDerecho, CodoDerecho, ManoDerecha, skelObjeto);
CalcularAngulosFinales(puntos);
}
//Console.WriteLine($"Mano X Y Z {handRight.Position.X} {handRight.Position.Y} {handRight.Position.Z}");
//Console.WriteLine($"Objeto X Y Z {skelObjeto.X} {skelObjeto.Y} {skelObjeto.Z}");
#region FinalizacionDelEjercicio
if (!_objetoTomado && DistanceHelper.ObtenerDistancia(ManoDerecha, skelObjeto) < 0.2)
{
_objetoTomadoTiempo = DateTime.Now.AddSeconds(1).Ticks;
_objetoTomado = true;
}
if (!_brazoMovido && _objetoTomadoTiempo < DateTime.Now.Ticks)
{
_vueltaPosicionInicialTiempo = DateTime.Now.AddSeconds(1).Ticks;
arduinoController.EnviarAngulosFromAngulosServos(new AngulosServos(ArduinoController.BRAZO_GB));
_brazoMovido = true;
}
if (_vueltaPosicionInicialTiempo < DateTime.Now.Ticks)
{
Ejercicio.FinalizoConExito = true;
Cerrar();
}
#endregion FinalizacionDelEjercicio
}
}
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, RenderWidth, RenderHeight));
}
}
public override void OnSkeletonFrameReady(
object sender, SkeletonFrameReadyEventArgs e
)
{
if (!Finished)
{
using (SkeletonFrame s = e.OpenSkeletonFrame())
{
if (s != null)
{
Skeleton[] skels = new Skeleton[s.SkeletonArrayLength];
s.CopySkeletonDataTo(skels);
foreach (Skeleton data in skels)
{
if (
data.TrackingState == SkeletonTrackingState.Tracked
)
{
Point3d leftHip =
PointFromVector(
data.Joints[JointType.HipLeft].Position, false
);
Point3d leftHand =
PointFromVector(
data.Joints[JointType.HandLeft].Position, false
);
Point3d rightHand =
PointFromVector(
data.Joints[JointType.HandRight].Position, false
);
if (
leftHand.DistanceTo(Point3d.Origin) > 0 &&
rightHand.DistanceTo(Point3d.Origin) > 0 &&
leftHand.DistanceTo(rightHand) < 0.03)
{
// Hands are less than 3cm from each other
_drawing = false;
_resizing = false;
Finished = true;
}
else
{
// Hands are within 10cm of each other vertically
// and both hands are above the waist, so we resize
// the profile radius
_resizing =
(leftHand.Z > leftHip.Z &&
rightHand.Z > leftHip.Z &&
Math.Abs(leftHand.Z - rightHand.Z) < 0.1);
// If the left hand is below the waist, we draw
_drawing = (leftHand.Z < leftHip.Z);
}
if (_resizing)
{
// If resizing, set some data to help draw
// a sphere where we're resizing
Vector3d vec = (leftHand - rightHand) / 2;
_resizeLocation = rightHand + vec;
_profRad = vec.Length;
}
if (_drawing)
{
// If we have at least one prior vertex...
if (_vertices.Count > 0)
{
// ... check whether we're a certain distance
// away from the last one before adding it (this
// smooths off the jitters of adding every point)
Point3d lastVert =
_vertices[_vertices.Count - 1];
if (
lastVert.DistanceTo(rightHand) > _profRad * 4
)
{
// Add the new vertex to our list
_vertices.Add(rightHand);
}
}
else
{
// Add the first vertex to our list
_vertices.Add(rightHand);
}
}
break;
}
}
}
}
}
}
private void SkeletonsReady(object sender, SkeletonFrameReadyEventArgs e)
{
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
int skeletonSlot = 0;
if ((this.skeletonData == null) || (this.skeletonData.Length != skeletonFrame.SkeletonArrayLength))
{
this.skeletonData = new Skeleton[skeletonFrame.SkeletonArrayLength];
}
skeletonFrame.CopySkeletonDataTo(this.skeletonData);
foreach (Skeleton skeleton in this.skeletonData)
{
if (SkeletonTrackingState.Tracked == skeleton.TrackingState)
{
Player player;
if (this.players.ContainsKey(skeletonSlot))
{
player = this.players[skeletonSlot];
}
else
{
player = new Player(skeletonSlot);
player.SetBounds(this.playerBounds);
this.players.Add(skeletonSlot, player);
}
player.LastUpdated = DateTime.Now;
// Update player's bone and joint positions
if (skeleton.Joints.Count > 0)
{
player.IsAlive = true;
// Head, hands, feet (hit testing happens in order here)
player.UpdateJointPosition(skeleton.Joints, JointType.Head);
player.UpdateJointPosition(skeleton.Joints, JointType.HandLeft);
player.UpdateJointPosition(skeleton.Joints, JointType.HandRight);
player.UpdateJointPosition(skeleton.Joints, JointType.FootLeft);
player.UpdateJointPosition(skeleton.Joints, JointType.FootRight);
// Hands and arms
player.UpdateBonePosition(skeleton.Joints, JointType.HandRight, JointType.WristRight);
player.UpdateBonePosition(skeleton.Joints, JointType.WristRight, JointType.ElbowRight);
player.UpdateBonePosition(skeleton.Joints, JointType.ElbowRight, JointType.ShoulderRight);
player.UpdateBonePosition(skeleton.Joints, JointType.HandLeft, JointType.WristLeft);
player.UpdateBonePosition(skeleton.Joints, JointType.WristLeft, JointType.ElbowLeft);
player.UpdateBonePosition(skeleton.Joints, JointType.ElbowLeft, JointType.ShoulderLeft);
// Head and Shoulders
player.UpdateBonePosition(skeleton.Joints, JointType.ShoulderCenter, JointType.Head);
player.UpdateBonePosition(skeleton.Joints, JointType.ShoulderLeft, JointType.ShoulderCenter);
player.UpdateBonePosition(skeleton.Joints, JointType.ShoulderCenter, JointType.ShoulderRight);
// Legs
player.UpdateBonePosition(skeleton.Joints, JointType.HipLeft, JointType.KneeLeft);
player.UpdateBonePosition(skeleton.Joints, JointType.KneeLeft, JointType.AnkleLeft);
player.UpdateBonePosition(skeleton.Joints, JointType.AnkleLeft, JointType.FootLeft);
player.UpdateBonePosition(skeleton.Joints, JointType.HipRight, JointType.KneeRight);
player.UpdateBonePosition(skeleton.Joints, JointType.KneeRight, JointType.AnkleRight);
player.UpdateBonePosition(skeleton.Joints, JointType.AnkleRight, JointType.FootRight);
player.UpdateBonePosition(skeleton.Joints, JointType.HipLeft, JointType.HipCenter);
player.UpdateBonePosition(skeleton.Joints, JointType.HipCenter, JointType.HipRight);
// Spine
player.UpdateBonePosition(skeleton.Joints, JointType.HipCenter, JointType.ShoulderCenter);
}
}
skeletonSlot++;
}
}
}
}
void FramesReady(object sender, AllFramesReadyEventArgs e)
{
ColorImageFrame VFrame = e.OpenColorImageFrame();
if (VFrame == null)
{
return;
}
byte[] pixelS = new byte[VFrame.PixelDataLength];
Bitmap bmap = ImageToBitmap(VFrame);
SkeletonFrame SFrame = e.OpenSkeletonFrame();
if (SFrame == null)
{
return;
}
Graphics g = Graphics.FromImage(bmap);
Skeleton[] Skeletons = new Skeleton[SFrame.SkeletonArrayLength];
SFrame.CopySkeletonDataTo(Skeletons);
foreach (Skeleton S in Skeletons)
{
//List<JointInfo> jointInfos = new List<JointInfo>();
if (S.TrackingState == SkeletonTrackingState.Tracked)
{
foreach (JointType j in Enum.GetValues(typeof(JointType)))
{
JointInfo.Update(S.Joints[j]);
//jointInfos.Add(new JointInfo(S.Joints[j]));
}
for (int i = 0; i < 20; i++)
{
if (JointInfo.allJoints[i] != null)
{
jointLabels[i].Text = JointInfo.allJoints[i].ToString();
}
else
{
jointLabels[i].Text = " ";
}
}
foreach (JointInfo j in JointInfo.allJoints)
{
if (j != null)
{
MarkAtxy(j.position, Brushes.Blue, g);
}
}
//Testing required coords
/* Debug.Print(JointInfo.HandLeft.position.y +
* " : " + JointInfo.HandRight.position.y +
* " : " + JointInfo.Head.position.y);
*/
//Check for the relevant pose
if ((JointInfo.HandRight.position.y) > JointInfo.Head.position.y && (JointInfo.HandLeft.position.y) > JointInfo.Head.position.y)
{
if (timer > 1)
{
if (alarm == false)
{
//Play Alarm on detecting hands above head
player.PlayLooping();
}
alarm = true;
Debug.Print("Alarm");
pictureBox2.BackColor = Color.Red;
}
else
{
timer += 0.01f;
pictureBox2.BackColor = Color.Orange;
}
}
else
{
if (timer <= 0)
{
player.Stop();
timer = 0;
alarm = false;
pictureBox2.BackColor = Color.Green;
}
else
{
timer -= 0.005f;
}
}
//body
DrawBone(JointType.Head, JointType.ShoulderCenter, S, g);
DrawBone(JointType.ShoulderCenter, JointType.Spine, S, g);
DrawBone(JointType.Spine, JointType.HipCenter, S, g);
//left leg
DrawBone(JointType.HipCenter, JointType.HipLeft, S, g);
DrawBone(JointType.HipLeft, JointType.KneeLeft, S, g);
DrawBone(JointType.KneeLeft, JointType.AnkleLeft, S, g);
DrawBone(JointType.AnkleLeft, JointType.FootLeft, S, g);
//Right Leg
DrawBone(JointType.HipCenter, JointType.HipRight, S, g);
DrawBone(JointType.HipRight, JointType.KneeRight, S, g);
DrawBone(JointType.KneeRight, JointType.AnkleRight, S, g);
DrawBone(JointType.AnkleRight, JointType.FootRight, S, g);
//Left Arm
DrawBone(JointType.ShoulderCenter, JointType.ShoulderLeft, S, g);
DrawBone(JointType.ShoulderLeft, JointType.ElbowLeft, S, g);
DrawBone(JointType.ElbowLeft, JointType.WristLeft, S, g);
DrawBone(JointType.WristLeft, JointType.HandLeft, S, g);
//Right Arm
DrawBone(JointType.ShoulderCenter, JointType.ShoulderRight, S, g);
DrawBone(JointType.ShoulderRight, JointType.ElbowRight, S, g);
DrawBone(JointType.ElbowRight, JointType.WristRight, S, g);
DrawBone(JointType.WristRight, JointType.HandRight, S, g);
}
}
pictureBox1.Image = bmap;
SFrame.Dispose();
VFrame.Dispose();
}
/// <summary>
/// Event handler for Kinect sensor's DepthFrameReady event
/// </summary>
/// <param name="sender">object sending the event</param>
/// <param name="e">event arguments</param>
private void SensorAllFramesReady(object sender, AllFramesReadyEventArgs e)
{
// in the middle of shutting down, so nothing to do
if (null == this.sensor)
{
return;
}
bool depthReceived = false;
bool colorReceived = false;
Skeleton[] skeletons = new Skeleton[0];
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
if (skeletons.Length != 0)
{
foreach (Skeleton skel in skeletons)
{
Joint joint0 = skel.Joints[JointType.WristLeft];
if (joint0.Position.Z != 0)
{
myJoint = joint0;
// keyVal = joint0.Position.Z;
depthPoint = this.sensor.CoordinateMapper.MapSkeletonPointToDepthPoint(joint0.Position, DepthImageFormat.Resolution640x480Fps30);
keyVal = depthPoint.Depth;
}
}
}
}
}
using (DepthImageFrame depthFrame = e.OpenDepthImageFrame())
{
if (null != depthFrame)
{
// Copy the pixel data from the image to a temporary array
depthFrame.CopyDepthImagePixelDataTo(this.depthPixels);
depthReceived = true;
}
}
using (ColorImageFrame colorFrame = e.OpenColorImageFrame())
{
if (null != colorFrame)
{
// Copy the pixel data from the image to a temporary array
colorFrame.CopyPixelDataTo(this.colorPixels);
colorReceived = true;
}
}
// do our processing outside of the using block
// so that we return resources to the kinect as soon as possible
if (true == depthReceived)
{
this.sensor.CoordinateMapper.MapDepthFrameToColorFrame(
DepthFormat,
this.depthPixels,
ColorFormat,
this.colorCoordinates);
Array.Clear(this.playerPixelData, 0, this.playerPixelData.Length);
// loop over each row and column of the depth
for (int y = 0; y < this.depthHeight; ++y)
{
for (int x = 0; x < this.depthWidth; ++x)
{
// calculate index into depth array
int depthIndex = x + (y * this.depthWidth);
DepthImagePixel depthPixel = this.depthPixels[depthIndex];
int player = depthPixel.PlayerIndex;
// if we're tracking a player for the current pixel, sets it opacity to full
//if (player > 0)
if (keyVal != 0 && depthPixel.Depth < (keyVal + 30) && depthPixel.Depth > (keyVal - 1000))
{
// retrieve the depth to color mapping for the current depth pixel
ColorImagePoint colorImagePoint = this.colorCoordinates[depthIndex];
// scale color coordinates to depth resolution
int colorInDepthX = colorImagePoint.X / this.colorToDepthDivisor;
int colorInDepthY = colorImagePoint.Y / this.colorToDepthDivisor;
// make sure the depth pixel maps to a valid point in color space
// check y > 0 and y < depthHeight to make sure we don't write outside of the array
// check x > 0 instead of >= 0 since to fill gaps we set opaque current pixel plus the one to the left
// because of how the sensor works it is more correct to do it this way than to set to the right
if (colorInDepthX > 0 && colorInDepthX < this.depthWidth && colorInDepthY >= 0 && colorInDepthY < this.depthHeight)
{
// calculate index into the player mask pixel array
int playerPixelIndex = colorInDepthX + (colorInDepthY * this.depthWidth);
// set opaque
this.playerPixelData[playerPixelIndex] = opaquePixelValue;
// compensate for depth/color not corresponding exactly by setting the pixel
// to the left to opaque as well
this.playerPixelData[playerPixelIndex - 1] = opaquePixelValue;
}
}
}
}
}
// do our processing outside of the using block
// so that we return resources to the kinect as soon as possible
if (true == colorReceived)
{
// Write the pixel data into our bitmap
this.colorBitmap.WritePixels(
new Int32Rect(0, 0, this.colorBitmap.PixelWidth, this.colorBitmap.PixelHeight),
this.colorPixels,
this.colorBitmap.PixelWidth * sizeof(int),
0);
if (this.playerOpacityMaskImage == null)
{
this.playerOpacityMaskImage = new WriteableBitmap(
this.depthWidth,
this.depthHeight,
96,
96,
PixelFormats.Bgra32,
null);
MaskedColor.OpacityMask = new ImageBrush {
ImageSource = this.playerOpacityMaskImage
};
}
this.playerOpacityMaskImage.WritePixels(
new Int32Rect(0, 1, this.depthWidth, this.depthHeight - 1),
this.playerPixelData,
this.depthWidth * ((this.playerOpacityMaskImage.Format.BitsPerPixel + 7) / 8),
0);
Image <Gray, Byte> My_Image = new Image <Gray, byte>(BitmapFromSource(this.colorBitmap));
Image <Gray, Byte> My_Mask = new Image <Gray, byte>(BitmapFromSource(this.playerOpacityMaskImage));
Image <Gray, byte> armMask = My_Mask.PyrUp();
armMask = armMask.Erode(2);
armMask = armMask.Dilate(1);
//////////////////////////////////
Image <Gray, Byte> HandG = My_Image.Copy(armMask);
Gray gray = new Gray(255);
Image <Gray, Byte> iLine = HandG.ThresholdBinaryInv(new Gray(50), gray);
armMask = armMask.Erode(4);
// CvInvoke.cvNamedWindow("gray");
//CvInvoke.cvShowImage("gray", armMask);
iLine = iLine.Copy(armMask);
System.Windows.Point myPoint = this.SkeletonPointToScreen(myJoint.Position);
HandG.ROI = new Rectangle((int)(myPoint.X - 70), (int)(myPoint.Y - 90), 140, 140);
iLine.ROI = new Rectangle((int)(myPoint.X - 70), (int)(myPoint.Y - 90), 140, 140);
iLine = iLine.Erode(1);
//iLine = iLine.Dilate(2);
//Create the window using the specific name
// CvInvoke.cvNamedWindow("line");
//CvInvoke.cvShowImage("line", iLine);
Image <Gray, Byte> resultImage = HandG.CopyBlank();
Image <Gray, Single> resultImageIN = resultImage.Convert <Gray, Single>();
Image <Gray, Byte> maskC = HandG.CopyBlank();
Image <Bgr, Byte> iAffiche = new Image <Bgr, byte>(resultImage.Width, resultImage.Height);
Double Result1 = 0;
Double Result2 = 0;
HandG = HandG.ThresholdBinary(new Gray(50), gray);
HandG = HandG.Erode(2);
LineSegment2D[] lines = iLine.HoughLinesBinary(1, Math.PI / 45, 15, 5, 15)[0];
//if (lines.Length != 0)
//{
// int a1 = lines[0].P2.Y - lines[0].P1.Y;
// int b1 = lines[0].P1.X - lines[0].P2.X;
// for (int i = 0; i < HandG.Width; i++)
// {
// for (int j = 0; j < HandG.Height; j++)
// {
// if (HandG[i, j].Intensity == gray.Intensity)
// {
// int a2 = i - lines[0].P1.X;
// int b2 = j - lines[0].P1.Y;
// if (a1 * a2 + b1 * b2 < 0)
// {
// HandG[i, j] = new Gray(0);
// }
// }
// }
// }
//}
using (var mem = new MemStorage())
{
Contour <System.Drawing.Point> contour = HandG.FindContours(
CHAIN_APPROX_METHOD.CV_CHAIN_APPROX_NONE,
RETR_TYPE.CV_RETR_LIST,
mem);
Contour <System.Drawing.Point> biggestContour = null;
while (contour != null)
{
Result1 = contour.Area;
if (Result1 > Result2)
{
Result2 = Result1;
biggestContour = contour;
}
contour = contour.HNext;
}
if (biggestContour != null)
{
// biggestContour = biggestContour.ApproxPoly(1.5);
resultImage.Draw(biggestContour, gray, 2);
maskC.Draw(biggestContour, gray, -1);
Emgu.CV.Image <Gray, byte> binaryIM = resultImage.ThresholdBinaryInv(new Gray(100), new Gray(255));
CvInvoke.cvDistTransform(binaryIM, resultImageIN, DIST_TYPE.CV_DIST_L1, 3, null, IntPtr.Zero);
CvInvoke.cvNormalize(resultImageIN, resultImageIN, 0, 1, Emgu.CV.CvEnum.NORM_TYPE.CV_MINMAX, IntPtr.Zero);
double minD = 0, maxD = 0;
System.Drawing.Point maxP = System.Drawing.Point.Empty, minP = System.Drawing.Point.Empty;
CvInvoke.cvMinMaxLoc(resultImageIN, ref minD, ref maxD, ref minP, ref maxP, maskC.Ptr);
iAffiche[0] = resultImage;
iAffiche[1] = resultImage;
iAffiche[2] = resultImage;
if (lines.Length != 0 && lines.Length < 3)
{
System.Drawing.Point lineP1 = lines[0].P1;
iAffiche.Draw(lines[0], new Bgr(0, 255, 0), 2);
int sPx = lineP1.X;
int sPy = lineP1.Y;
System.Drawing.Point startP = lineP1;
double minDist = 999999.0;
System.Drawing.Point[] bContArr = biggestContour.ToArray();
firstPoint = bContArr[0];
int nbPoints = bContArr.Length;
double startAngle = 0;
double startDist = 0;
double rInsCircle = 99999.0;
for (int i = 0; i < nbPoints; i++)
{
System.Drawing.Point v = bContArr[i];
double tempDist = ((v.X - sPx) * (v.X - sPx) + (v.Y - sPy) * (v.Y - sPy));
double tempCirc = Math.Sqrt((v.X - maxP.X) * (v.X - maxP.X) + (v.Y - maxP.Y) * (v.Y - maxP.Y));
if (tempDist < minDist)
{
minDist = tempDist;
startP = v;
}
if (tempCirc < rInsCircle)
{
rInsCircle = tempCirc;
}
}
chart1.Series.SuspendUpdates();
foreach (var series in chart1.Series)
{
series.Points.Clear();
}
List <double[]> pointList = new List <double[]>();
pointList.Clear();
startAngle = -1 * getTSAngle(maxP, startP);
startDist = getTSDist(maxP, startP);
for (int i = 0; i < (nbPoints); i++)
{
System.Drawing.Point v = bContArr[i];
tsAngle = -1 * getTSAngle(maxP, v);
tsAngle = tsAngle - startAngle;
if (tsAngle < 0)
{
tsAngle = tsAngle + 360;
}
tsAngle = tsAngle / 360;
tsDist = getTSDist(maxP, v);
tsDist = (tsDist / (rInsCircle)) - (1.9);
if (tsDist < 0)
{
tsDist = 0.0;
}
if (tsDist != 0 && i == 0)
{
System.Drawing.Point a = bContArr[i];
Array.Copy(bContArr, 1, bContArr, 0, bContArr.Length - 1);
bContArr[bContArr.Length - 1] = a;
i = -1;
}
if (i != -1)
{
chart1.Series["Series1"].Points.AddXY(tsAngle, tsDist);
double[] XY = { tsAngle, tsDist };
pointList.Add(XY);
}
}
CircleF startPoint = new CircleF(startP, 3);
iAffiche.Draw(startPoint, new Bgr(0, 0, 255), 2);
CircleF palmCir = new CircleF(maxP, 1);
iAffiche.Draw(palmCir, new Bgr(255, 255, 0), 3);
//chart1.Series.Invalidate();
///////////////////SIGNATURES
List <double[]> inGesteSignatures = new List <double[]>();
inGesteSignatures.Clear();
inGesteSignatures = getSignatures(chart1.Series["Series1"].Points);
int gNo = -1;
if ((inGesteSignatures.Count != 0) && (inGesteSignatures.Count < 7))
{
gNo = findGesture(inGesteSignatures, references);
// textBox1.Text = gNo.ToString();
if (old_gNo != -1 && gNo != -1 && gNo == old_gNo)
{
textBox1.Text = gNo.ToString();
}
old_gNo = gNo;
}
//chart1.Series.Invalidate();
// chart1.Series.ResumeUpdates();
CvInvoke.cvNamedWindow("affiche");
CvInvoke.cvShowImage("affiche", iAffiche);
}
}
}
}
}
public static int cycleCounter = 0; // counts the number of times the function is sending data
// in order to calculate the sensor sample rate
private static void kinect_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
Skeleton[] skeletons = new Skeleton[0];
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
}
}
if (skeletons.Length != 0)
{
foreach (Skeleton skel in skeletons)
{
if (skel.TrackingState == SkeletonTrackingState.Tracked)
{
SkeletonPoint leftWrist = skel.Joints[JointType.WristLeft].Position;
SkeletonPoint rightWrist = skel.Joints[JointType.WristRight].Position;
SkeletonPoint hipCenter = skel.Joints[JointType.HipCenter].Position;
int hipCenter_Z = Convert.ToInt32(hipCenter.Z * 100);
int leftWrist_Z = (hipCenter_Z - Convert.ToInt32(leftWrist.Z * 100)) < 0 ? 0 : hipCenter_Z - Convert.ToInt32(leftWrist.Z * 100);
int leftWrist_Y = (Convert.ToInt32(leftWrist.Y * 100)) < 0 ? 0 : Convert.ToInt32(leftWrist.Y * 100);
int rightWrist_Z = (hipCenter_Z - Convert.ToInt32(rightWrist.Z * 100)) < 0 ? 0 : hipCenter_Z - Convert.ToInt32(rightWrist.Z * 100);
int rightWrist_Y = (Convert.ToInt32(rightWrist.Y * 100)) < 0 ? 0 : Convert.ToInt32(rightWrist.Y * 100);
Console.WriteLine("{0} | {1} | {2} | {3} | {4} | {5}", skel.TrackingId, leftWrist_Z, leftWrist_Y, rightWrist_Z, rightWrist_Y, hipCenter_Z);
try
{
BluetoothAddress addr = BluetoothAddress.Parse("00:80:37:2e:31:20");
Guid serviceClass;
serviceClass = BluetoothService.BluetoothBase;
var ep = new BluetoothEndPoint(addr, serviceClass, 2);
var cli = new BluetoothClient();
Console.WriteLine("Trying to connect...");
cli.Connect(ep);
Console.WriteLine("just connected");
Stream peerStream = cli.GetStream();
// Send this players left and right arm data
string msg = String.Format("{0}{1}{2}{3}{4}", Convert.ToChar(skel.TrackingId), Convert.ToChar(leftWrist_Y), Convert.ToChar(leftWrist_Z), Convert.ToChar(rightWrist_Y), Convert.ToChar(rightWrist_Z));
Console.WriteLine("Sending msg");//
Byte[] to_send = System.Text.Encoding.ASCII.GetBytes(msg);
peerStream.Write(to_send, 0, to_send.Length);
peerStream.Close();
cli.Close();
cycleCounter++;
Console.WriteLine("Number of times called: " + cycleCounter);
}
catch (Exception)
{
Console.WriteLine("ERROR: Could not connect to Bluetooth Server");
}
//Thread.Sleep(100); // 10Hz update rate
}
}
}
}
/// <summary>
/// Event handler for Kinect sensor's SkeletonFrameReady event
/// </summary>
/// <param name="sender">object sending the event</param>
/// <param name="e">event arguments</param>
private void SensorSkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
Skeleton[] skeletons = new Skeleton[0];
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
}
}
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, RenderWidth, RenderHeight));
if (skeletons.Length != 0)
{
side = 0;
foreach (Skeleton skel in skeletons)
{
RenderClippedEdges(skel, dc);
if (skel.TrackingState == SkeletonTrackingState.Tracked)
{
if (this.SkeletonPointToScreen(skel.Joints[JointType.Head].Position).X > 320)
{
side = 1;
}
else
{
side = 2;
}
this.DrawBonesAndJoints(skel, dc);
}
else if (skel.TrackingState == SkeletonTrackingState.PositionOnly)
{
dc.DrawEllipse(
this.centerPointBrush,
null,
this.SkeletonPointToScreen(skel.Position),
BodyCenterThickness,
BodyCenterThickness);
}
}
//ここから主に改変
//label1.Content = side;//デバッグ用
if (side != stock) //値が変わったら(前の値と今の値で変化があったら)
{
if (stock == 0) //前の値が人が居ない状態だったら
{
if (side == 1)
{
Image2.Source = backImage2; //右からの画像に変更
label1.Content = ToRight_Text; //説明
}
else
{
Image2.Source = backImage3; //左からの画像に変更
label1.Content = ToLeft_Text; //説明
}
}
else
{
if (side == 0) //前の値が人がいる状態だったら
{
Image2.Source = backImage1; //通常画像に戻る
label1.Content = Normal_Text;
}
}
}
stock = side;//値を記録しておく
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, RenderWidth, RenderHeight));
}
}
private void KinectSensorOnAllFramesReady(object sender, AllFramesReadyEventArgs allFramesReadyEventArgs)
{
if (null == this.sensor)
{
return;
}
bool depthReceived = false;
bool colorReceived = false;
bool bodyReceived = false;
Skeleton[] skeletons = null;
List <Point> points = new List <Point>();
Point handPointColor = new Point()
{
};
Point wristPointColor = new Point()
{
};
PointXYZ handXYZ = new PointXYZ();
PointXYZ wristXYZ = new PointXYZ();
using (var colorImageFrame = allFramesReadyEventArgs.OpenColorImageFrame())
{
if (colorImageFrame == null)
{
return;
}
colorImageFrame.CopyPixelDataTo(this.colorPixels);
colorReceived = true;
}
using (SkeletonFrame skeletonFrame = allFramesReadyEventArgs.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
bodyReceived = true;
skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
}
}
int leftBound = 0;
int topBound = 0;
int rightBound = 0;
int bottomBound = 0;
if (bodyReceived)
{
foreach (var skel in skeletons)
{
if (skel.TrackingState == SkeletonTrackingState.Tracked)
{
Joint handLeft = skel.Joints[JointType.HandLeft];
Joint wristLeft = skel.Joints[JointType.WristLeft];
Joint shoulder0 = skel.Joints[JointType.ShoulderLeft];
Joint shoulder1 = skel.Joints[JointType.ShoulderRight];
var shoulder0Color = this.sensor.CoordinateMapper.MapSkeletonPointToColorPoint(shoulder0.Position, ColorImageFormat.RgbResolution640x480Fps30);
var shoulder1Color = this.sensor.CoordinateMapper.MapSkeletonPointToColorPoint(shoulder1.Position, ColorImageFormat.RgbResolution640x480Fps30);
// !!!important parameter to adapt the recognition region
// we have to tune it
float bodyScaleColor = 9.0f + 32.0f / handLeft.Position.Z;
ColorImagePoint handColor =
this.sensor.CoordinateMapper.MapSkeletonPointToColorPoint(handLeft.Position,
ColorImageFormat.RgbResolution640x480Fps30);
ColorImagePoint wristColor =
this.sensor.CoordinateMapper.MapSkeletonPointToColorPoint(wristLeft.Position,
ColorImageFormat.RgbResolution640x480Fps30);
leftBound = (handColor.X - bodyScaleColor) < 0 ? 0 : (int)(handColor.X - bodyScaleColor);
topBound = (handColor.Y - bodyScaleColor) < 0 ? 0 : (int)(handColor.Y - bodyScaleColor);
rightBound = (handColor.X + bodyScaleColor) > colorWidth ? colorWidth : (int)(handColor.X + bodyScaleColor);
bottomBound = (handColor.Y + bodyScaleColor) > colorHeight ? colorHeight : (int)(handColor.Y + bodyScaleColor);
handPointColor = new Point(handColor.X, handColor.Y);
wristPointColor = new Point(wristColor.X, wristColor.Y);
handXYZ = new PointXYZ(handLeft.Position.X, handLeft.Position.Y, handLeft.Position.Z);
wristXYZ = new PointXYZ(wristLeft.Position.X, wristLeft.Position.Y, wristLeft.Position.Z);
// quit after handling one player
// assuming only one player
break;
}
}
}
using (DepthImageFrame depthFrame = allFramesReadyEventArgs.OpenDepthImageFrame())
{
if (null != depthFrame)
{
// Copy the pixel data from the image to a temporary array
depthFrame.CopyDepthImagePixelDataTo(this.depthPixels);
depthReceived = true;
}
}
List <SkeletonPoint> candidatePoints = new List <SkeletonPoint>();
if (true == depthReceived && true == bodyReceived && true == colorReceived)
{
this.sensor.CoordinateMapper.MapDepthFrameToColorFrame(
DepthFormat,
this.depthPixels,
ColorFormat,
this.colorCoordinates);
System.Drawing.Bitmap bmp = new System.Drawing.Bitmap(colorWidth, colorHeight);
Image <Bgra, Byte> img = new Image <Bgra, byte>(colorWidth, colorHeight);
img.Bytes = this.colorPixels;
Image <Bgra, Byte> crop = null;
Image <Gray, Byte> res = null;
if (bottomBound > topBound && rightBound > leftBound)
{
crop = img.Copy(new System.Drawing.Rectangle(
leftBound, topBound, rightBound - leftBound, bottomBound - topBound));
Image <Gray, Byte> canny = crop.Convert <Gray, Byte>().Canny(50, 150);
VectorOfVectorOfPoint vvp = new VectorOfVectorOfPoint();
CvInvoke.FindContours(canny, vvp, null, RetrType.Ccomp, ChainApproxMethod.ChainApproxNone);
res = new Image <Gray, byte>(crop.Width, crop.Height);
for (int i = 0; i < vvp.Size; i++)
{
for (int j = 0; j < vvp[i].Size; j++)
{
res.Data[vvp[i][j].Y, vvp[i][j].X, 0] = 255;
}
}
}
// loop over each row and column of the depth
for (int y = 0; y < this.depthHeight; ++y)
{
for (int x = 0; x < this.depthWidth; ++x)
{
// calculate index into depth array
int depthIndex = x + (y * this.depthWidth);
DepthImagePixel depthPixel = this.depthPixels[depthIndex];
int player = depthPixel.PlayerIndex;
// assuming one player
if (player > 0 && depthPixel.IsKnownDepth)
{
// retrieve the depth to color mapping for the current depth pixel
ColorImagePoint colorImagePoint = this.colorCoordinates[depthIndex];
if (colorImagePoint.X >= leftBound && colorImagePoint.X < rightBound &&
colorImagePoint.Y >= topBound && colorImagePoint.Y < bottomBound)
{
SkeletonPoint p = this.sensor.CoordinateMapper.MapDepthPointToSkeletonPoint(
DepthFormat,
new DepthImagePoint()
{
X = x, Y = y, Depth = depthPixel.Depth
});
candidatePoints.Add(p);
}
}
}
}
PointXYZ v1 = new PointXYZ();
PointXYZ v2 = new PointXYZ();
PointXYZ v3 = new PointXYZ();
int nPoint = candidatePoints.Count;
const int min_point_size = 100;
if (nPoint > min_point_size)
{
PointXYZ[] pointCloud = new PointXYZ[nPoint];
for (int i = 0; i < nPoint; i++)
{
pointCloud[i] = new PointXYZ(candidatePoints[i].X, candidatePoints[i].Y, candidatePoints[i].Z);
}
int pointCloudLength = pointCloud.Length;
ExternalAPI.infer_palmpose(ref v1, ref v2, ref v3, pointCloud, ref pointCloudLength,
ref handXYZ, ref wristXYZ);
SkeletonPoint[] skelPoints = new SkeletonPoint[pointCloudLength];
ColorImagePoint[] newColorPoints = new ColorImagePoint[pointCloudLength];
for (int i = 0; i < pointCloudLength; i++)
{
skelPoints[i].X = pointCloud[i].x;
skelPoints[i].Y = pointCloud[i].y;
skelPoints[i].Z = pointCloud[i].z;
}
for (int i = 0; i < pointCloudLength; i++)
{
ColorImagePoint colorP = this.sensor.CoordinateMapper.MapSkeletonPointToColorPoint(skelPoints[i], ColorFormat);
newColorPoints[i] = colorP;
}
foreach (var cp in newColorPoints)
{
{
int index = cp.X + cp.Y * this.colorWidth;
this.colorPixels[index * 4 + 0] = 0;
this.colorPixels[index * 4 + 1] = 255;
this.colorPixels[index * 4 + 2] = 255;
this.colorPixels[index * 4 + 3] = 255;
}
}
}
if (res != null)
{
//Image<Bgra, Byte> resColor = res.Convert<Bgra, Byte>();
//int x = leftBound;
//int y = topBound;
//int w = rightBound - leftBound;
//int h = bottomBound - topBound;
//int offset = x + y * colorWidth;
//int ii = offset * 4;
//for (int i = 0; i < h; i++)
//{
// for (int j = 0; j < w; j++)
// {
// this.colorPixels[ii++] = resColor.Data[i, j, 0];
// this.colorPixels[ii++] = resColor.Data[i, j, 1];
// this.colorPixels[ii++] = resColor.Data[i, j, 2];
// this.colorPixels[ii++] = 255;
// }
// ii += (colorWidth - w) * 4;
//}
}
this.colorBitmap.WritePixels(new Int32Rect(0, 0, this.colorWidth, this.colorHeight),
this.colorPixels,
this.colorBitmap.PixelWidth * sizeof(int), 0);
using (DrawingContext dc = this.drawingGroup.Open())
{
dc.DrawImage(this.colorBitmap, new Rect(0, 0, this.colorWidth, this.colorHeight));
// if draw hands
if (bodyReceived)
{
SkeletonPoint origin = new SkeletonPoint();
origin.X = handXYZ.x;
origin.Y = handXYZ.y;
origin.Z = handXYZ.z;
ColorImagePoint originDepth = this.sensor.CoordinateMapper.MapSkeletonPointToColorPoint(origin, ColorFormat);
const float len = 0.2f;
SkeletonPoint end1 = new SkeletonPoint();
end1.X = origin.X + len * v1.x;
end1.Y = origin.Y + len * v1.y;
end1.Z = origin.Z + len * v1.z;
ColorImagePoint end1Depth = this.sensor.CoordinateMapper.MapSkeletonPointToColorPoint(end1, ColorFormat);
SkeletonPoint end2 = new SkeletonPoint();
end2.X = origin.X + len * v2.x;
end2.Y = origin.Y + len * v2.y;
end2.Z = origin.Z + len * v2.z;
ColorImagePoint end2Depth = this.sensor.CoordinateMapper.MapSkeletonPointToColorPoint(end2, ColorFormat);
SkeletonPoint end3 = new SkeletonPoint();
end3.X = origin.X + len * v3.x;
end3.Y = origin.Y + len * v3.y;
end3.Z = origin.Z + len * v3.z;
ColorImagePoint end3Depth = this.sensor.CoordinateMapper.MapSkeletonPointToColorPoint(end3, ColorFormat);
dc.DrawLine(penX, new Point(originDepth.X, originDepth.Y), new Point(end1Depth.X, end1Depth.Y));
dc.DrawLine(penY, new Point(originDepth.X, originDepth.Y), new Point(end2Depth.X, end2Depth.Y));
dc.DrawLine(penZ, new Point(originDepth.X, originDepth.Y), new Point(end3Depth.X, end3Depth.Y));
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, this.colorWidth, this.colorHeight));
}
}
}
/// <summary>
/// 当kinect准备好一帧骨架数据之后，触发这个函数
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void skeleton_ready(object sender, SkeletonFrameReadyEventArgs e)
{
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame()) // 获取骨架帧
{
if (skeletonFrame != null) // 如果这个骨架帧非空
{
// 把骨架帧中的数据，复制到 skeletonData 变量中
skeletonData = new Skeleton[myKinect.SkeletonStream.FrameSkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletonData);
// 摄像头中可能出现两人个，只取其中一个
Skeleton skeleton = (from s in skeletonData
where s.TrackingState == SkeletonTrackingState.Tracked
select s).FirstOrDefault();
if (skeleton != null) // 如果骨架帧非空
{
// 遍历这个骨架中的所有的关节结点
foreach (Joint joint in skeleton.Joints)
{
byte[] buffer = new byte[50];
switch (joint.JointType) // 分析这个关节点的类型
{
case JointType.Head:
setPointPosition(headPoint, joint);
break;
case JointType.ShoulderLeft:
setPointPosition(shoulderleftPoint, joint);
break;
case JointType.ShoulderRight:
setPointPosition(shoulderrightPoint, joint);
break;
case JointType.ShoulderCenter:
setPointPosition(shouldercenterPoint, joint);
break;
case JointType.ElbowRight:
setPointPosition(elbowrightPoint, joint);
break;
case JointType.ElbowLeft:
setPointPosition(elbowleftPoint, joint);
break;
case JointType.WristRight:
setPointPosition(wristrightPoint, joint);
break;
case JointType.WristLeft:
setPointPosition(wristleftPoint, joint);
break;
case JointType.HandLeft:
setPointPosition(handleftPoint, joint);
break;
case JointType.HandRight:
setPointPosition(handrightPoint, joint);
break;
case JointType.Spine:
setPointPosition(spinePoint, joint);
break;
case JointType.HipCenter:
setPointPosition(hipcenterPoint, joint);
break;
case JointType.HipLeft:
setPointPosition(hipleftPoint, joint);
break;
case JointType.HipRight:
setPointPosition(hiprightPoint, joint);
break;
case JointType.KneeLeft:
setPointPosition(kneeleftPoint, joint);
break;
case JointType.KneeRight:
setPointPosition(kneerightPoint, joint);
break;
case JointType.AnkleLeft:
setPointPosition(ankleleftPoint, joint);
break;
case JointType.AnkleRight:
setPointPosition(anklerightPoint, joint);
break;
case JointType.FootLeft:
setPointPosition(footleftPoint, joint);
break;
case JointType.FootRight:
setPointPosition(footrightPoint, joint);
break;
}
} //外if
} //using
} //类的
}
}
// Skeleton Stream Actions
private void Sensor_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
// Initialise skeleton array to store available skeletons (maximum 6)
Skeleton[] skeletons = new Skeleton[0];
// Store skeletons from available skeleton frame to the array
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame == null)
{
return;
}
skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
}
// Choose the first/nearest skeleton as the main skeleton
Skeleton skel = (from trackskeleton in skeletons
where trackskeleton.TrackingState == SkeletonTrackingState.Tracked
select trackskeleton).FirstOrDefault();
// Code to go ahead if any skeletons are available
if (skel == null)
{
return;
}
//Draw Skeleton
using (DrawingContext dc = drawingGroup.Open())
{
// Draw a transparent background to set the render size
dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, RenderWidth, RenderHeight));
RenderClippedEdges(skel, dc);
if (skel.TrackingState == SkeletonTrackingState.Tracked)
{
DrawBonesAndJoints(skel, dc);
}
else if (skel.TrackingState == SkeletonTrackingState.PositionOnly)
{
dc.DrawEllipse(
this.centerPointBrush,
null,
SkeletonPointToScreen(skel.Position),
BodyCenterThickness,
BodyCenterThickness);
}
// prevent drawing outside of our render area
this.drawingGroup.ClipGeometry = new RectangleGeometry(new Rect(0.0, 0.0, RenderWidth, RenderHeight));
}
// If skeletons are being tracked in real time in synchronisation, process angles
if (skel.Joints[JointType.ShoulderRight].TrackingState == JointTrackingState.Tracked &&
skel.Joints[JointType.ElbowRight].TrackingState == JointTrackingState.Tracked &&
skel.Joints[JointType.WristRight].TrackingState == JointTrackingState.Tracked &&
skel.Joints[JointType.ShoulderLeft].TrackingState == JointTrackingState.Tracked &&
skel.Joints[JointType.ElbowLeft].TrackingState == JointTrackingState.Tracked &&
skel.Joints[JointType.WristLeft].TrackingState == JointTrackingState.Tracked &&
skel.Joints[JointType.HipRight].TrackingState == JointTrackingState.Tracked &&
skel.Joints[JointType.KneeRight].TrackingState == JointTrackingState.Tracked &&
skel.Joints[JointType.HipLeft].TrackingState == JointTrackingState.Tracked &&
skel.Joints[JointType.KneeLeft].TrackingState == JointTrackingState.Tracked)
{
// Print angles on the WPF application's labels
int[] ang = PrintAngles(skel);
ls1.Content = ang[0];
ls2.Content = ang[1];
le.Content = ang[2];
rs1.Content = ang[3];
rs2.Content = ang[4];
re.Content = ang[5];
lh.Content = ang[6];
rh.Content = ang[7];
lk.Content = ang[8];
rk.Content = ang[9];
// Print angles on the serial port
string toSend = "";
for (int i = 0; i < 6; i++)
{
toSend += (ang[i] + ":");
}
Console.WriteLine(toSend);
port.WriteLine(toSend);
Thread.Sleep(500);
}
}