List <Microsoft.Research.Kinect.Nui.Vector> convertRealDepth()
{
List <Microsoft.Research.Kinect.Nui.Vector> ret = new List <Microsoft.Research.Kinect.Nui.Vector>();
Console.WriteLine("Pts: {0}", depthPixels.Length);
for (int i = 0; i < depthPixels.Length; i += 2)
{
int depthPixel = depthPixels[i / 2];
// The x and y positions can be calculated using modulus
// division from the array index
int x = (i / 2) % depthWidth;
int y = (i / 2) / depthWidth;
// The x and y we pass into DepthImageToSkeleton() need to
// be normalised (between 0 and 1), so we divide by the
// width and height of the depth image, respectively
// As we're using UseDepth (not UseDepthAndPlayerIndex) in
// the depth sensor settings, we also need to shift the
// depth pixel by 3 bits
Microsoft.Research.Kinect.Nui.Vector v = _kinectRuntime.SkeletonEngine.DepthImageToSkeleton(
((float)x) / ((float)depthWidth),
((float)y) / ((float)depthHeight),
(short)(depthPixel << 3)
);
if (v.Z > 0)
{
ret.Add(v);
}
}
return(ret);
}
public virtual void Add(Vector position, SkeletonEngine engine)
{
Entry newEntry = new Entry {
Position = position.ToVector3(), Time = DateTime.Now
};
Entries.Add(newEntry);
if (displayCanvas != null)
{
newEntry.DisplayEllipse = new Ellipse
{
Width = 4,
Height = 4,
HorizontalAlignment = HorizontalAlignment.Left,
VerticalAlignment = VerticalAlignment.Top,
StrokeThickness = 2.0,
Stroke = new SolidColorBrush(displayColor),
StrokeLineJoin = PenLineJoin.Round
};
float x, y;
engine.SkeletonToDepthImage(position, out x, out y);
x = (float)(x * displayCanvas.ActualWidth);
y = (float)(y * displayCanvas.ActualHeight);
Canvas.SetLeft(newEntry.DisplayEllipse, x - newEntry.DisplayEllipse.Width / 2);
Canvas.SetTop(newEntry.DisplayEllipse, y - newEntry.DisplayEllipse.Height / 2);
displayCanvas.Children.Add(newEntry.DisplayEllipse);
}
if (Entries.Count > WindowSize)
{
Entry entryToRemove = Entries[0];
if (displayCanvas != null)
{
displayCanvas.Children.Remove(entryToRemove.DisplayEllipse);
}
Entries.Remove(entryToRemove);
}
LookForGesture();
}
//This method is used to position the ellipses on the canvas
//according to correct movements of the tracked joints.
//IMPORTANT NOTE: Code for vector scaling was imported from the Coding4Fun Kinect Toolkit
//available here: http://c4fkinect.codeplex.com/
//I only used this part to avoid adding an extra reference.
private void SetEllipsePosition(Ellipse ellipse, Joint joint)
{
Microsoft.Research.Kinect.Nui.Vector vector = new Microsoft.Research.Kinect.Nui.Vector();
vector.X = ScaleVector(640, joint.Position.X);
vector.Y = ScaleVector(480, -joint.Position.Y);
vector.Z = joint.Position.Z;
Joint updatedJoint = new Joint();
updatedJoint.ID = joint.ID;
updatedJoint.TrackingState = JointTrackingState.Tracked;
updatedJoint.Position = vector;
Canvas.SetLeft(ellipse, updatedJoint.Position.X);
Canvas.SetTop(ellipse, updatedJoint.Position.Y);
}
private Point getDisplayPosition(Microsoft.Research.Kinect.Nui.Vector mainPoint)
{
float depthX, depthY;
nui.SkeletonEngine.SkeletonToDepthImage(mainPoint, out depthX, out depthY);
// Convert to 320, 240 space
depthX = depthX * 320; // MICHI: WATCH OUT HERE!!!! this are the depth X/Y coordinates first in the 0-1 interval
depthY = depthY * 240;
int colorX, colorY;
ImageViewArea iv = new ImageViewArea();
// Only ImageResolution.Resolution640x480 is supported at this point
nui.NuiCamera.GetColorPixelCoordinatesFromDepthPixel(ImageResolution.Resolution640x480, iv, (int)depthX, (int)depthY, (short)0, out colorX, out colorY);
// Map back to skeletonCanvas.Width & skeletonCanvas.Height
return(new Point((int)(skeletonCanvas.Width * colorX / 640.0), (int)(skeletonCanvas.Height * colorY / 480)));
}
public virtual void Add(Vector position, SkeletonEngine engine)
{
Entry newEntry = new Entry {Position = position.ToVector3(), Time = DateTime.Now};
Entries.Add(newEntry);
if (displayCanvas != null)
{
newEntry.DisplayEllipse = new Ellipse
{
Width = 4,
Height = 4,
HorizontalAlignment = HorizontalAlignment.Left,
VerticalAlignment = VerticalAlignment.Top,
StrokeThickness = 2.0,
Stroke = new SolidColorBrush(displayColor),
StrokeLineJoin = PenLineJoin.Round
};
float x, y;
engine.SkeletonToDepthImage(position, out x, out y);
x = (float)(x * displayCanvas.ActualWidth);
y = (float)(y * displayCanvas.ActualHeight);
Canvas.SetLeft(newEntry.DisplayEllipse, x - newEntry.DisplayEllipse.Width / 2);
Canvas.SetTop(newEntry.DisplayEllipse, y - newEntry.DisplayEllipse.Height / 2);
displayCanvas.Children.Add(newEntry.DisplayEllipse);
}
if (Entries.Count > WindowSize)
{
Entry entryToRemove = Entries[0];
if (displayCanvas != null)
{
displayCanvas.Children.Remove(entryToRemove.DisplayEllipse);
}
Entries.Remove(entryToRemove);
}
LookForGesture();
}
public static Gesture guessGuesture(Microsoft.Research.Kinect.Nui.Vector v, Joint source, float xThreshold, float yThreshold, float zThreshold)
{
//double max = Math.Max(Math.Abs(v.X), Math.Max(Math.Abs(v.Y), Math.Abs(v.Z)));
//if (max < 0.17)
//{
// return new Gesture(DateTime.Now, 0, GestureID.Invalid, source);
//}
//if (max == Math.Abs(v.X) && max > xThreshold
// && ((previousGuesture == GestureID.SwipeLeft || previousGuesture == GestureID.SwipeRight || previousGuesture == GestureID.Invalid)
// || DateTime.Now.Subtract(timer).TotalSeconds > 0.1))
//{
// timer = DateTime.Now;
// return (v.X > 0.0) ? new Gesture(DateTime.Now, max, GestureID.SwipeRight, source) :
// new Gesture(DateTime.Now, max, GestureID.SwipeLeft, source);
//}
//if (DateTime.Now.Subtract(timer).TotalSeconds > 0.1)
//{
// if (max == Math.Abs(v.Y) && max > yThreshold)
// {
// timer = DateTime.Now;
// return (v.Y > 0.0) ? new Gesture(DateTime.Now, max, GestureID.SwipeUp, source) :
// new Gesture(DateTime.Now, max, GestureID.SwipeDown, source);
// }
// else if (max == Math.Abs(v.Z) && max > zThreshold)
// {
// timer = DateTime.Now;
// return (v.Z > 0.0) ? new Gesture(DateTime.Now, max, GestureID.Pull, source) :
// new Gesture(DateTime.Now, max, GestureID.Push, source);
// }
//}
if (DateTime.Now.Subtract(timer).TotalSeconds > 0.1)
{
if (Math.Abs(v.Z) > zThreshold)
{
timer = DateTime.Now;
return(new Gesture(DateTime.Now, Math.Abs(v.Z), v.Z > 0 ? GestureID.Pull : GestureID.Push, source));
}
}
return(new Gesture(DateTime.Now, 0, GestureID.Invalid, source));
}
//這個方法會傳入代表某個點的控制項，還有該點相對於Kinect擷取到的座標
private void SetFrameworkElementPosition(FrameworkElement frameworkElement, Joint joint)
{
Microsoft.Research.Kinect.Nui.Vector vector = new Microsoft.Research.Kinect.Nui.Vector();
//下面的兩行為透過呼叫ScaleVector方法，將Kinect的座標系換算回螢幕座標系
vector.X = ScaleVector(640, joint.Position.X);
//別忘了，Y座標軸往上是負的，所以這邊要變負數才行
vector.Y = ScaleVector(480, -joint.Position.Y);
vector.Z = joint.Position.Z;
//宣告一個新的Joint物件來存放重新計算過的座標
Joint updatedJoint = new Joint();
updatedJoint.ID = joint.ID;
updatedJoint.TrackingState = JointTrackingState.Tracked;
updatedJoint.Position = vector;
//重新設定控制項的座標位置，後面我自己加上了加回控制項大小的程式碼，以減少誤差
Canvas.SetLeft(frameworkElement, updatedJoint.Position.X + frameworkElement.ActualWidth);
Canvas.SetTop(frameworkElement, updatedJoint.Position.Y + frameworkElement.ActualHeight);
}
public static Gesture guessGuesture(Microsoft.Research.Kinect.Nui.Vector v, Joint source, float xThreshold, float yThreshold, float zThreshold)
{
double magnitude = Math.Sqrt(v.X * v.X + v.Y * v.Y + v.Z * v.Z);
double max = Math.Max(Math.Abs(v.X), Math.Max(Math.Abs(v.Y), Math.Abs(v.Z)));
if (max < 0.17f)
{
return(new Gesture(DateTime.Now, 0, GestureID.Invalid, source));
}
if (Math.Abs(v.Z) * 2 >= magnitude && v.Z > zThreshold)
{
timer = DateTime.Now;
return(new Gesture(DateTime.Now, Math.Abs(v.Z), v.Z > 0 ? GestureID.Pull : GestureID.Push, source));
}
if (max == Math.Abs(v.X) && max > xThreshold &&
((previousGuesture == GestureID.SwipeLeft || previousGuesture == GestureID.SwipeRight || previousGuesture == GestureID.Invalid) ||
DateTime.Now.Subtract(timer).TotalSeconds > 0.1))
{
timer = DateTime.Now;
return((v.X > 0.0) ? new Gesture(DateTime.Now, max, GestureID.SwipeRight, source) :
new Gesture(DateTime.Now, max, GestureID.SwipeLeft, source));
}
if (DateTime.Now.Subtract(timer).TotalSeconds > 0.1)
{
if (max == Math.Abs(v.Y) && max > yThreshold && Math.Abs(v.Y) * 1.5 >= magnitude)
{
timer = DateTime.Now;
return((v.Y > 0.0) ? new Gesture(DateTime.Now, max, GestureID.SwipeUp, source) :
new Gesture(DateTime.Now, max, GestureID.SwipeDown, source));
}
else if (max == Math.Abs(v.Z) && max > zThreshold)
{
timer = DateTime.Now;
return((v.Z > 0.0) ? new Gesture(DateTime.Now, max, GestureID.Pull, source) :
new Gesture(DateTime.Now, max, GestureID.Push, source));
}
}
return(new Gesture(DateTime.Now, 0, GestureID.Invalid, source));
}
private void SmoothenBody(Body currentBody)
{
if (_smoothBody == null)
{
_smoothBody = currentBody;
}
else
{
Vector head = _smoothBody.Head;
head.X = (float)(_smoothBody.Head.X + _dampeningFactor * (currentBody.Head.X - _smoothBody.Head.X));
head.Y = (float)(_smoothBody.Head.Y + _dampeningFactor * (currentBody.Head.Y - _smoothBody.Head.Y));
_smoothBody.Head = head;
Vector left = _smoothBody.LeftHand;
left.X = (float)(_smoothBody.LeftHand.X + _dampeningFactor * (currentBody.LeftHand.X - _smoothBody.LeftHand.X));
left.Y = (float)(_smoothBody.LeftHand.Y + _dampeningFactor * (currentBody.LeftHand.Y - _smoothBody.LeftHand.Y));
_smoothBody.LeftHand = left;
Vector right = _smoothBody.RightHand;
right.X = (float)(_smoothBody.RightHand.X + _dampeningFactor * (currentBody.RightHand.X - _smoothBody.RightHand.X));
right.Y = (float)(_smoothBody.RightHand.Y + _dampeningFactor * (currentBody.RightHand.Y - _smoothBody.RightHand.Y));
_smoothBody.RightHand = right;
}
}
public static Gesture guessGuesture(Microsoft.Research.Kinect.Nui.Vector v, Joint source, float xThreshold, float yThreshold, float zThreshold)
{
double max = Math.Max(Math.Abs(v.X), Math.Max(Math.Abs(v.Y), Math.Abs(v.Z)));
if (max < 0.0)
{
return(new Gesture(DateTime.Now, 0, GestureID.Invalid, source));
}
else if (Math.Sqrt(v.X * v.X + v.Y * v.Y) <= 0.031)// Math.Abs(v.Z - ((v.X + v.Y) )) <= 0.1 )
{
return(new Gesture(DateTime.Now, max, GestureID.Still, source));
}
else if ((Math.Abs(v.Z) > 2 * Math.Abs(v.X)) && (Math.Abs(v.Z) > 2 * Math.Abs(v.Y)))
{
return(new Gesture(DateTime.Now, max, (v.Z > 0) ? GestureID.Pull : GestureID.Push, source));
}
else
{
return(new Gesture(DateTime.Now, max, GestureID.Planar, source));
}
}
public static Vector2 ToVector2(Microsoft.Research.Kinect.Nui.Vector p, Vector2 scale)
{
var half = scale / 2.0f;
return((new Vector2(p.X, p.Y) * half * new Vector2(1, -1)) + half);
}
static void RuntimeSkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
var skeleton = e.SkeletonFrame.Skeletons
.Where(s => s.TrackingState == SkeletonTrackingState.Tracked)
.FirstOrDefault();
if (skeleton != null)
{
Status = KinectStatus.TrackingUser;
leftHandJoint = skeleton.Joints[JointID.HandLeft];
LeftHand = leftHandJoint.Position;
rightHandJoint = skeleton.Joints[JointID.HandRight];
RightHand = rightHandJoint.Position;
Torso = skeleton.Joints[JointID.Spine].Position;
}
else
{
Status = KinectStatus.WaitingToAcquire;
}
}
private void Window_Loaded(object sender, EventArgs e)
{
nui = new Runtime();
try
{
nui.Initialize(RuntimeOptions.UseDepthAndPlayerIndex | RuntimeOptions.UseSkeletalTracking | RuntimeOptions.UseColor);
}
catch (InvalidOperationException)
{
System.Windows.MessageBox.Show("Runtime initialization failed. Please make sure Kinect device is plugged in.");
return;
}
try
{
nui.VideoStream.Open(ImageStreamType.Video, 2, ImageResolution.Resolution640x480, ImageType.Color);
nui.DepthStream.Open(ImageStreamType.Depth, 2, ImageResolution.Resolution320x240, ImageType.DepthAndPlayerIndex);
}
catch (InvalidOperationException)
{
System.Windows.MessageBox.Show("Failed to open stream. Please make sure to specify a supported image type and resolution.");
return;
}
lastTime = DateTime.Now;
nui.SkeletonFrameReady += new EventHandler<SkeletonFrameReadyEventArgs>(nui_SkeletonFrameReady);
nui.VideoFrameReady += new EventHandler<ImageFrameReadyEventArgs>(nui_ColorFrameReady);
SkeletonDataAlternative.JointPositionConverter = new JointPositionConverter(delegate(Joint joint)
{
float depthX, depthY;
nui.SkeletonEngine.SkeletonToDepthImage(joint.Position, out depthX, out depthY);
ImageViewArea iv = new ImageViewArea();
int colorX, colorY;
nui.NuiCamera.GetColorPixelCoordinatesFromDepthPixel(ImageResolution.Resolution640x480, iv, (int)(depthX * 320), (int)(depthY * 240), (short)0, out colorX, out colorY);
Microsoft.Research.Kinect.Nui.Vector converted = new Microsoft.Research.Kinect.Nui.Vector();
converted.X = (float)colorX / 640.0f;
converted.Y = (float)colorY / 480.0f;
converted.Z = joint.Position.Z;
converted.W = joint.Position.W;
joint.Position = converted;
return joint;
});
}
void nui_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
SkeletonFrame allSkeletons = e.SkeletonFrame;
//get the first tracked skeleton
SkeletonData skeleton = (from s in allSkeletons.Skeletons
where s.TrackingState == SkeletonTrackingState.Tracked
select s).FirstOrDefault();
skeletonReady = true;
playerCenter = skeleton.Position;
if (Math.Abs(playerCenter.X - skellyX) > 0.05)
{
string dataString = playerCenter.X.ToString();
Byte[] sendBytes = Encoding.ASCII.GetBytes(dataString);
myClient.Send(sendBytes, sendBytes.Length); //virtual ball pos sent to server
skellyX = playerCenter.X;
}
//textBlock2.Text ="W: " + e.SkeletonFrame.FloorClipPlane.W.ToString() + "\tX: " + e.SkeletonFrame.FloorClipPlane.X.ToString() + "\tY: " + e.SkeletonFrame.FloorClipPlane.X.ToString() + "\tZ: " + e.SkeletonFrame.FloorClipPlane.X.ToString();
//textBlock2.Text = "X: " + skeleton.Joints[JointID.HandLeft].Position.X.ToString() + "\tY: " + skeleton.Joints[JointID.HandLeft].Position.Y.ToString() + "\tZ: " + skeleton.Joints[JointID.HandLeft].Position.Z.ToString();
}
// skeleton handler
void nui_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
SkeletonFrame skeletonFrame = e.SkeletonFrame;
lastSkeletonTime = DateTime.Now;
uint absoluteTime = (uint)(lastSkeletonTime.Ticks / TimeSpan.TicksPerMillisecond); // 32-bit uint
log.Text = "";
skeleton.Children.Clear();
byte skeletonNum = 0;
using (MemoryStream skeletonStream = new MemoryStream())
{
using (BinaryWriter skeletonWriter = new BinaryWriter(skeletonStream))
{
foreach (SkeletonData data in skeletonFrame.Skeletons)
{
if (SkeletonTrackingState.Tracked == data.TrackingState)
{
// skeleton id write to buffer
skeletonWriter.Write(KinectConst.MS_DATA);
skeletonWriter.Write(KinectConst.OUT_SKELETON);
skeletonWriter.Write(KinectConst.SKELETON_BUFFER_LENGTH);
skeletonWriter.Write(data.TrackingID);
// Draw bones
Brush brush = brushes[skeletonNum % brushes.Length];
skeleton.Children.Add(getBodySegment(data.Joints, brush, JointID.HipCenter, JointID.Spine, JointID.ShoulderCenter, JointID.Head));
skeleton.Children.Add(getBodySegment(data.Joints, brush, JointID.ShoulderCenter, JointID.ShoulderLeft, JointID.ElbowLeft, JointID.WristLeft, JointID.HandLeft));
skeleton.Children.Add(getBodySegment(data.Joints, brush, JointID.ShoulderCenter, JointID.ShoulderRight, JointID.ElbowRight, JointID.WristRight, JointID.HandRight));
skeleton.Children.Add(getBodySegment(data.Joints, brush, JointID.HipCenter, JointID.HipLeft, JointID.KneeLeft, JointID.AnkleLeft, JointID.FootLeft));
skeleton.Children.Add(getBodySegment(data.Joints, brush, JointID.HipCenter, JointID.HipRight, JointID.KneeRight, JointID.AnkleRight, JointID.FootRight));
// Draw and write to buffer joints
foreach (Joint joint in data.Joints)
{
Point jointPos = getDisplayPosition(joint);
Ellipse jointPoint = new Ellipse();
jointPoint.Width = 6;
jointPoint.Height = 6;
jointPoint.HorizontalAlignment = HorizontalAlignment.Center;
jointPoint.VerticalAlignment = VerticalAlignment.Center;
jointPoint.Fill = jointColors[joint.ID];
jointPoint.Margin = new Thickness(jointPos.X - 3, jointPos.Y - 3, 0, 0);
skeleton.Children.Add(jointPoint);
// write to buffer
skeletonWriter.Write(joint.Position.X);
skeletonWriter.Write(joint.Position.Y);
skeletonWriter.Write(joint.Position.Z);
}
skeletonWriter.Write(absoluteTime);
skeletonNum++;
Microsoft.Research.Kinect.Nui.Vector position = data.Joints[JointID.ShoulderCenter].Position;
log.AppendText("skeleton " + data.TrackingID + "\t(" + position.X.ToString("000.00") + "\t" + position.Y.ToString("000.00") + "\t" + position.Z.ToString("000.00") + ")\n");
position = data.Joints[JointID.HandLeft].Position;
log.AppendText("leftHand\t\t(" + position.X.ToString("000.00") + "\t" + position.Y.ToString("000.00") + "\t" + position.Z.ToString("000.00") + ")\n");
position = data.Joints[JointID.HandRight].Position;
log.AppendText("rightHand\t(" + position.X.ToString("000.00") + "\t" + position.Y.ToString("000.00") + "\t" + position.Z.ToString("000.00") + ")\n");
if (sre == null)
{
log.AppendText("Speech Recognition Platform x86 is not instalted!\n");
}
}
} // for each skeleton
log.AppendText("skeletons : " + skeletonNum + "\n");
if (lastRecognizedCommand != "")
{
log.AppendText("cmd : " + lastRecognizedCommand + "\t" + lastRecognizedAngle + "\t" + lastRecognizedConfidence + "\n");
}
log.AppendText("clients : " + ClientConnection.Count() + "\n");
if (skeletonNum > 0 && server != null)
{
server.Send(skeletonStream.ToArray());
}
}
}
}
void nui_DepthFrameReady(object sender, ImageFrameReadyEventArgs e)
{
int i16=0,x,y;
long dx = 0, dy = 0;
double bx=0, by=0, bz=0;
short depth;
int count = 0;
Microsoft.Research.Kinect.Nui.Vector point;
DateTime cur = DateTime.Now;
frameSpan = cur.Subtract(lastTime);
//textBox1.Text += cur.Subtract(lastTime).ToString() + '\n';
//work on virtual ball
if (virtualBall)
{
xDisplace = xDisplace + (xVel * frameSpan.Milliseconds)/1000;
yDisplace = yDisplace + (yVel * frameSpan.Milliseconds)/1000;
zDisplace = zDisplace + (zVel * frameSpan.Milliseconds)/1000;
//send to server here!!
string dataString = (xDisplace+0.3f).ToString() + " " + (yDisplace-0.1f).ToString() + " " + zDisplace.ToString();
Byte[] sendBytes = Encoding.ASCII.GetBytes(dataString);
myClient.Send(sendBytes, sendBytes.Length); //virtual ball pos sent to server
virtualBallsSent++;
textBlock10.Text = "Virtual balls sent: " + virtualBallsSent.ToString();
textBlock9.Text = xDisplace.ToString() + " " + yDisplace.ToString() + " " + zDisplace.ToString();
if(yDisplace < -1.5 || Math.Abs(xDisplace) > 2 || zDisplace > 8) //out of play bounds
virtualBall = false;
return;
}
//process current depth frame
PlanarImage depthData = e.ImageFrame.Image;
for (y=0;y<depthData.Height;y++)
{
for(x=0;x<depthData.Width;x++)
{
if ((depthData.Bits[i16] & 0x07) != 0)
{
depthData.Bits[i16 + 1] = (byte)0;
depthData.Bits[i16] = (byte)0;
i16 += 2;
continue;
}
depth = (short)((depthData.Bits[i16+1]<<8) | depthData.Bits[i16]);
point = nui.SkeletonEngine.DepthImageToSkeleton(((float)x) / ((float)depthData.Width), ((float)y) / ((float)depthData.Height),depth);
if (point.Z < 1 || point.Z > 3.8 || point.Y < -1 || point.Y > 1.2)
{
//not ball
depthData.Bits[i16 + 1] = (byte)0;
depthData.Bits[i16] = (byte)0;
}
else
{
//ball
count++;
dx += x;
dy += y;
bx += point.X;
by += point.Y;
bz += point.Z;
depthData.Bits[i16 + 1] = (byte)60;
depthData.Bits[i16] = (byte)60;
}
i16 += 2;
}
}
if (count > 80 && count < 4000)
{
//ball detected in frame
ballframes++;
dx /= count;
dy /= count;
bx /= count;
by /= count;
bz /= count;
i16 = (int)dy * depthData.Width * depthData.BytesPerPixel + (int)dx * depthData.BytesPerPixel;
depthData.Bits[i16] = (byte)255;
depthData.Bits[i16 + 1] = (byte)255;
textBlock1.Text = "X: " + bx.ToString();
textBlock2.Text = "Y: " + by.ToString();
textBlock3.Text = "Z: " + bz.ToString();
textBlock4.Text = "FrameNumber: " + e.ImageFrame.FrameNumber.ToString();
textBlock5.Text = "Ball Frames: " + ballframes.ToString();
//textBox1.Text += "FrameNumber: " + e.ImageFrame.FrameNumber.ToString() + " X: " + bx.ToString() + " Y: " + by.ToString() + " Z: " + bz.ToString() + " T: " + frameSpan.ToString() + '\n';
lastTime = cur;
Microsoft.Research.Kinect.Nui.Vector tempPoint = new Microsoft.Research.Kinect.Nui.Vector();
tempPoint.X = (float)bx;
tempPoint.Y = (float)by;
tempPoint.Z = (float)bz;
if (listSize == 0) //first in sequence
{
pointList.Add(tempPoint);
numList.Add(e.ImageFrame.FrameNumber);
timeList.Add(frameSpan);
listSize++;
}
else //not first
{
if ((e.ImageFrame.FrameNumber != (numList[listSize - 1] + 2)) || ((tempPoint.Z - 0.05f) < pointList[listSize - 1].Z)) //end of sequence
{
if (listSize > 2)
{
textBox1.Text += "Last " + listSize.ToString() + " records are forward movement\n";
forwardCount++;
textBlock7.Text = forwardCount.ToString();
Microsoft.Research.Kinect.Nui.Vector sPoint = pointList[listSize - 3];
Microsoft.Research.Kinect.Nui.Vector ePoint = pointList[listSize - 1];
//do tracking schtuff here
xDisplace = ePoint.X - sPoint.X;
yDisplace = ePoint.Y - sPoint.Y;
zDisplace = ePoint.Z - sPoint.Z;
TimeSpan displaceTime = timeList[listSize - 1] + timeList[listSize - 2];
xVel = (xDisplace / displaceTime.Milliseconds) * 1000;
yVel = (yDisplace / displaceTime.Milliseconds) * 1000;
zVel = (zDisplace / displaceTime.Milliseconds) * 1000;
virtualBall = true;
}
pointList.Clear();
numList.Clear();
timeList.Clear();
listSize = 0;
}
else //still moving forward!
{
//float[] fltArr = {tempPoint.X,tempPoint.Y,tempPoint.Z};
//Byte[] sendBytes = ToByteArray(fltArr);
string dataString = (xDisplace + 0.3f).ToString() + " " + (yDisplace - 0.1f).ToString() + " " + zDisplace.ToString();
Byte[] sendBytes = Encoding.ASCII.GetBytes(dataString);
myClient.Send(sendBytes, sendBytes.Length); //real ball pos sent to server
realBallsSent++;
textBlock8.Text = "Real Balls Sent: " + realBallsSent.ToString();
pointList.Add(tempPoint);
numList.Add(e.ImageFrame.FrameNumber);
timeList.Add(frameSpan);
listSize++;
}
}
}
else //not a ball detected in this frame
{
if (listSize == 0) //first in sequence
{
}
else //not first
{
if (listSize > 2)
{
textBox1.Text += "Last " + listSize.ToString() + " records are forward movement\n";
forwardCount++;
textBlock7.Text = forwardCount.ToString();
Microsoft.Research.Kinect.Nui.Vector sPoint = pointList[listSize - 3];
Microsoft.Research.Kinect.Nui.Vector ePoint = pointList[listSize - 1];
//do tracking schtuff here
xDisplace = ePoint.X - sPoint.X;
yDisplace = ePoint.Y - sPoint.Y;
zDisplace = ePoint.Z - sPoint.Z;
TimeSpan displaceTime = timeList[listSize - 1] + timeList[listSize - 2];
xVel = (xDisplace / displaceTime.Milliseconds)*1000;
yVel = (yDisplace / displaceTime.Milliseconds)*1000;
zVel = (zDisplace / displaceTime.Milliseconds)*1000;
virtualBall = true;
}
pointList.Clear();
numList.Clear();
timeList.Clear();
listSize = 0;
}
}
++totalFrames;
//timeList.Add(cur.Subtract(lastTime));
/*
if (cur.Subtract(lastTime) > TimeSpan.FromSeconds(1))
{
int frameDiff = totalFrames - lastFrames;
lastFrames = totalFrames;
lastTime = cur;
textBlock6.Text = "FPS: " + frameDiff.ToString();
}*/
//Use Coding4Fun extension method on ImageFrame class for Depth
image2.Source = BitmapSource.Create(depthData.Width, depthData.Height, 96, 96, PixelFormats.Gray16, null, depthData.Bits, depthData.Width * depthData.BytesPerPixel);
}
public virtual void Add(Vector position, SkeletonEngine engine)
{
currentPosition = new InputData(position.ToVector3(), 0);
LookForGesture();
}
void nui_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
SkeletonFrame skeletonFrame = e.SkeletonFrame;
int iSkeleton = 0;
Brush[] brushes = new Brush[6];
brushes[0] = new SolidColorBrush(Color.FromRgb(255, 0, 0));
brushes[1] = new SolidColorBrush(Color.FromRgb(0, 255, 0));
brushes[2] = new SolidColorBrush(Color.FromRgb(64, 255, 255));
brushes[3] = new SolidColorBrush(Color.FromRgb(255, 255, 64));
brushes[4] = new SolidColorBrush(Color.FromRgb(255, 64, 255));
brushes[5] = new SolidColorBrush(Color.FromRgb(128, 128, 255));
skeleton.Children.Clear();
foreach (SkeletonData data in skeletonFrame.Skeletons)
{
if (SkeletonTrackingState.Tracked == data.TrackingState)
{
// Draw bones
Brush brush = brushes[iSkeleton % brushes.Length];
skeleton.Children.Add(getBodySegment(data.Joints, brush, JointID.HipCenter, JointID.Spine, JointID.ShoulderCenter, JointID.Head));
skeleton.Children.Add(getBodySegment(data.Joints, brush, JointID.ShoulderCenter, JointID.ShoulderLeft, JointID.ElbowLeft, JointID.WristLeft, JointID.HandLeft));
skeleton.Children.Add(getBodySegment(data.Joints, brush, JointID.ShoulderCenter, JointID.ShoulderRight, JointID.ElbowRight, JointID.WristRight, JointID.HandRight));
skeleton.Children.Add(getBodySegment(data.Joints, brush, JointID.HipCenter, JointID.HipLeft, JointID.KneeLeft, JointID.AnkleLeft, JointID.FootLeft));
skeleton.Children.Add(getBodySegment(data.Joints, brush, JointID.HipCenter, JointID.HipRight, JointID.KneeRight, JointID.AnkleRight, JointID.FootRight));
JointsCollection jc = data.Joints;
StringBuilder sb = new StringBuilder();
StringBuilder up = new StringBuilder();
StringBuilder lo = new StringBuilder();
int k = 0;
foreach (JointID jid in Enum.GetValues(typeof(JointID)))
{
if (jid != JointID.Count)
{
k++;
Microsoft.Research.Kinect.Nui.Vector v = jc[jid].Position;
if (k <= 4)
{
sb.Append(string.Format("{0} = {1}, {2}, {3}\n", jid, v.X, v.Y, v.Z));
}
else if (k <= 12)
{
up.Append(string.Format("{0} = {1}, {2}, {3}\n", jid, v.X, v.Y, v.Z));
if (k == 8)//HandLeft
{
HandLY = v.Y;
up.Append(string.Format("\n"));
}
if (k == 9)//ShoulderRight
{
ShouRX = v.X;
ShouRY = v.Y;
ShouRZ = v.Z;
}
if (k == 10)//ElbowRight
{
ElRX = v.X;
ElRY = v.Y;
ElRZ = v.Z;
}
if (k == 11)//WristRight
{
WrRX = v.X;
WrRY = v.Y;
WrRZ = v.Z;
}
if (k == 12)//HandRight
{
HandRX = v.X;
HandRY = v.Y;
HandRZ = v.Z;
}
}
else if (k <= 20)
{
lo.Append(string.Format("{0} = {1}, {2}, {3}\n", jid, v.X, v.Y, v.Z));
if (k == 13)//HipLeft
{
HipLY = v.Y;
}
if (k == 16)
{
lo.Append(string.Format("\n"));
}
}
}
}
middle.Text = sb.ToString();
upper.Text = up.ToString();
lower.Text = lo.ToString();
//Arduino Control 1st Rotation Angle
double angROT1 = Math.Atan((HandRX - ShouRX) / (HandRY - ShouRY));
if (angROT1 < 0)
{
angROT1 = 0.5 * Math.PI - angROT1;
}
arduino.analogWrite(3, Convert.ToInt32(Math.Floor(angROT1 * 180 / Math.PI)));
//Arduino Control 2nd Rotation Angle (Shoulder)
double angROT2 = Math.Atan((ShouRZ - ElRZ) / Math.Sqrt(Math.Pow((ElRY - ShouRY), 2) + Math.Pow((ElRX - ShouRX), 2)));
if (angROT2 < 0)
{
angROT2 += Math.PI;
}
if (ElRX < ShouRX)
{
angROT2 = Math.PI - angROT2;
}
int degRot2 = Convert.ToInt32(Math.Floor(angROT2 * 180 / Math.PI));
//Constrained by mechanism
if (degRot2 < 25)
{
degRot2 = 25;
}
else if (degRot2 > 155)
{
degRot2 = 155;
}
arduino.analogWrite(5, degRot2);
//Arduino Control 3rd Rotation Angle (Elbo)
double ax = HandRX - ElRX;
double ay = HandRY - ElRY;
double az = HandRZ - ElRZ;
double bx = ElRX - ShouRX;
double by = ElRY - ShouRY;
double bz = ElRZ - ShouRZ;
double angROT3 = Math.Acos((ax * bx + ay * by + az * bz) / Math.Sqrt(ax * ax + ay * ay + az * az) / Math.Sqrt(bx * bx + by * by + bz * bz));
arduino.analogWrite(10, Convert.ToInt32(Math.Floor(angROT3 * 180 / Math.PI)));
//Arduino Control 4th Rotation Angle (Grab)
if (HandLY > HipLY)
{
arduino.analogWrite(9, 30);
}
else
{
arduino.analogWrite(9, 110);
}
// Draw joints
foreach (Joint joint in data.Joints)
{
Point jointPos = getDisplayPosition(joint);
Line jointLine = new Line();
jointLine.X1 = jointPos.X - 3;
jointLine.X2 = jointLine.X1 + 6;
jointLine.Y1 = jointLine.Y2 = jointPos.Y;
jointLine.Stroke = jointColors[joint.ID];
jointLine.StrokeThickness = 6;
skeleton.Children.Add(jointLine);
}
}
iSkeleton++;
} // for each skeleton
}
/// <summary>
/// This method is used to position the ellipses on the
/// canvas according to correct movements of the tracked
/// joints.
/// IMPORTANT NOTE: Code for vector scaling was imported from
/// the Coding4Fun Kinect Toolkit available here:
/// http://c4fkinect.codeplex.com/
/// I only used this part to avoid adding an extra reference.
/// </summary>
/// <param name="ellipse"></param>
/// <param name="joint"></param>
private void SetEllipsePosition(Ellipse ellipse, Joint joint)
{
Microsoft.Research.Kinect.Nui.Vector vector = new Microsoft.Research.Kinect.Nui.Vector();
vector.X = ScaleVector(640, joint.Position.X);
vector.Y = ScaleVector(480, -joint.Position.Y);
vector.Z = joint.Position.Z;
Joint updatedJoint = new Joint();
updatedJoint.ID = joint.ID;
updatedJoint.TrackingState = JointTrackingState.Tracked;
updatedJoint.Position = vector;
Canvas.SetLeft(ellipse, updatedJoint.Position.X);
Canvas.SetTop(ellipse, updatedJoint.Position.Y);
}
public static Vector3 ToVector3(Microsoft.Research.Kinect.Nui.Vector p, Vector3 scale)
{
var half = scale / 2.0f;
return((new Vector3(p.X, p.Y, p.Z) * half * new Vector3(1, -1, 1)) + half);
}
public Gesture track(SkeletonData trackSkeleton, Joint trackJoint, double elevationAngle)
{
//Guesture guesture = null;
double theta = elevationAngle * Math.PI / 180;
if (positions.Count < trackSkeleton.Joints.Count)
{
foreach (Joint j in trackSkeleton.Joints)
{
if (!positions.ContainsKey(j))
{
Microsoft.Research.Kinect.Nui.Vector newPosition
= new Microsoft.Research.Kinect.Nui.Vector();
newPosition.W = j.Position.W;
newPosition.X = j.Position.X;
newPosition.Y = (float)(j.Position.Y * Math.Cos(theta) + j.Position.Z * Math.Sin(theta));
newPosition.Z = (float)(j.Position.Z * Math.Cos(theta) - j.Position.Y * Math.Sin(theta));
RotationalArray <Microsoft.Research.Kinect.Nui.Vector> jointPositions =
new RotationalArray <Microsoft.Research.Kinect.Nui.Vector>(interval, 0);
jointPositions.Add(newPosition);
positions.Add(j, jointPositions);
}
}
}
else
{
foreach (Joint j in trackSkeleton.Joints)
{
for (int i = 0; i < positions.Keys.Count; i++)
{
Joint k = positions.Keys.ElementAt(i);
if (j.ID == k.ID)
{
Microsoft.Research.Kinect.Nui.Vector newPosition
= new Microsoft.Research.Kinect.Nui.Vector();
newPosition.W = j.Position.W;
newPosition.X = j.Position.X;
newPosition.Y = (float)(j.Position.Y * Math.Cos(theta) + j.Position.Z * Math.Sin(theta));
newPosition.Z = (float)(j.Position.Z * Math.Cos(theta) - j.Position.Y * Math.Sin(theta));
positions[k].Add(newPosition);
if (k.ID == trackJoint.ID && positions[k].GetFirst().X != 0)
{
Microsoft.Research.Kinect.Nui.Vector velocityVector = new Microsoft.Research.Kinect.Nui.Vector();
velocityVector.X = positions[k].GetLast().X - positions[k].GetFirst().X;
velocityVector.Y = positions[k].GetLast().Y - positions[k].GetFirst().Y;
velocityVector.Z = positions[k].GetLast().Z - positions[k].GetFirst().Z;
velocityVector.W = positions[k].GetLast().W - positions[k].GetFirst().W;
guesture = guessGuesture(velocityVector, trackJoint, xThreshold, yThreshold, zThreshold);
if (guesture.id == previousGuesture)
{
lockout = true;
}
else
{
lockout = false;
previousGuesture = guesture.id;
}
}
break;
}
}
}
}
if (!lockout)
{
return(guesture);
}
else
{
return(new Gesture(DateTime.Now, 0, GestureID.Invalid, trackJoint));
}
}
public bool IsActive(Microsoft.Research.Kinect.Nui.Vector CurrentCoords, float ReferencePlaneZ)
{
Position = CurrentCoords;
if ((NearEnd < (ReferencePlaneZ - CurrentCoords.Z)) && ((ReferencePlaneZ - CurrentCoords.Z) < FarEnd))
{
if (ControlDisabled)
return false;
if (!ActiveFlag)
Init = CurrentCoords;
ActiveFlag = true;
return true;
}
else
{
if (!ControlDisabled && ActiveFlag)
CompleteGesture();
ActiveFlag = false;
ControlDisabled = false;
return false;
}
}