void Plot(JointType centerID, JointType baseID, JointCollection joints)
{
float centerX;
float centerY;
GetCoordinates(centerID, joints, out centerX, out centerY);
float baseX;
float baseY;
GetCoordinates(baseID, joints, out baseX, out baseY);
double diameter = Math.Abs(baseY - centerY);
Ellipse ellipse = new Ellipse
{
Width = diameter,
Height = diameter,
HorizontalAlignment = HorizontalAlignment.Left,
VerticalAlignment = VerticalAlignment.Top,
StrokeThickness = 4.0,
Stroke = new SolidColorBrush(Colors.Green),
StrokeLineJoin = PenLineJoin.Round
};
Canvas.SetLeft(ellipse, centerX - ellipse.Width / 2);
Canvas.SetTop(ellipse, centerY - ellipse.Height / 2);
rootCanvas.Children.Add(ellipse);
}
public void printJointCollection(JointCollection jointCollection)
{
Console.WriteLine("Elbow right: <" + jointCollection[JointType.ElbowRight].Position.X + ", " + jointCollection[JointType.ElbowRight].Position.Y + ">");
Console.WriteLine("Elbow left: <" + jointCollection[JointType.ElbowLeft].Position.X + ", " + jointCollection[JointType.ElbowLeft].Position.Y + ">");
Console.WriteLine("Knee right: <" + jointCollection[JointType.KneeRight].Position.X + ", " + jointCollection[JointType.KneeRight].Position.Y + ">");
Console.WriteLine("Knee left: <" + jointCollection[JointType.KneeLeft].Position.X + ", " + jointCollection[JointType.KneeLeft].Position.Y + ">");
}
private void MyDataFrameReady(object sender, Xtr3D.Net.ExtremeMotion.Data.DataFrameReadyEventArgs e)
{
using (var dataFrame = e.OpenFrame() as DataFrame)
{
if (dataFrame != null)
{
currFrameID = dataFrame.FrameKey.FrameNumberKey;
Debug.Log("Skeleton frame: " + currFrameID + ", state: " + dataFrame.Skeletons[0].TrackingState + ", proximity: " + dataFrame.Skeletons[0].Proximity.SkeletonProximity);
if (currFrameID <= lastFrameID && currFrameID != 1) //currFrameId=1 on reset/resume!!!
{
return;
}
lastFrameID = currFrameID;
//update frameRateCalc, we need to call this every frame as we are calculating avarage fps in the last x frames.
frameRateCalc.UpdateAvgFps();
JointCollection skl = dataFrame.Skeletons[0].Joints;
//use a copy of the Joints data structure as the dataFrame values can change.
typesToJoints[JointType.ShoulderCenter] = skl.ShoulderCenter;
typesToJoints[JointType.Spine] = skl.Spine;
typesToJoints[JointType.HipCenter] = skl.HipCenter;
typesToJoints[JointType.ShoulderLeft] = skl.ShoulderLeft;
typesToJoints[JointType.ShoulderRight] = skl.ShoulderRight;
typesToJoints[JointType.ElbowLeft] = skl.ElbowLeft;
typesToJoints[JointType.ElbowRight] = skl.ElbowRight;
typesToJoints[JointType.HandRight] = skl.HandRight;
typesToJoints[JointType.HandLeft] = skl.HandLeft;
typesToJoints[JointType.Head] = skl.Head;
}
}
}
void Trace(JointType sourceID, JointType destinationID, JointCollection joints)
{
float sourceX;
float sourceY;
GetCoordinates(sourceID, joints, out sourceX, out sourceY);
float destinationX;
float destinationY;
GetCoordinates(destinationID, joints, out destinationX, out destinationY);
Line line = new Line
{
X1 = sourceX,
Y1 = sourceY,
X2 = destinationX,
Y2 = destinationY,
HorizontalAlignment = HorizontalAlignment.Left,
VerticalAlignment = VerticalAlignment.Top,
StrokeThickness = 4.0,
Stroke = new SolidColorBrush(Colors.Green),
StrokeLineJoin = PenLineJoin.Round
};
rootCanvas.Children.Add(line);
}
private void handleLeftMove(JointCollection joints, long timeStamp)
{
float dx = joints[JointType.HandLeft].Position.X - previousPositionL.X;
long dt = timeStamp - previousTimeStamp;
float speed = 1000 * dx / dt;
//if (speed > 2.5) //swap left
//{
// CallKeyEvent(new KeyInputEventArgs(KeyStatut.DOWN, VirtualKeyCode.VK_Q));
// CallKeyEvent(new KeyInputEventArgs(KeyStatut.UP, VirtualKeyCode.VK_Q));
// Thread.Sleep(600);
// return;
//}
//Rester appuyé sur la gauche
if (Math.Floor(speed) == 0 &&
joints[JointType.ElbowLeft].Position.Y > joints[JointType.ShoulderCenter].Position.Y)
{
if (!this.leftPressed)
{
CallKeyEvent(new KeyInputEventArgs(KeyStatut.DOWN, VirtualKeyCode.VK_Q));
//Thread.Sleep(600);
this.leftPressed = true;
}
}
else if (this.leftPressed)
{
CallKeyEvent(new KeyInputEventArgs(KeyStatut.UP, VirtualKeyCode.VK_Q));
this.leftPressed = false;
}
}
/// <summary>
/// Draws a body
/// </summary>
/// <param name="joints">joints to draw</param>
/// <param name="jointPoints">translated positions of joints to draw</param>
/// <param name="drawingContext">drawing context to draw to</param>
/// <param name="drawingPen">specifies color to draw a specific body</param>
private void DrawBody(JointCollection joints, DrawingContext drawingContext, Pen drawingPen)
{
// Draw the bones
foreach (var bone in this.bones)
{
this.DrawBone(joints, bone.Item1, bone.Item2, drawingContext, drawingPen);
}
// Draw the joints
foreach (Joint joint in joints)
{
Brush drawBrush = null;
JointTrackingState trackingState = joint.TrackingState;
if (trackingState == JointTrackingState.Tracked)
{
drawBrush = this.trackedJointBrush;
}
else if (trackingState == JointTrackingState.Inferred)
{
drawBrush = this.inferredJointBrush;
}
if (drawBrush != null)
{
drawingContext.DrawEllipse(drawBrush, null, this.SkeletonPointToScreen(joint.Position), JointThickness, JointThickness);
}
}
}
private void handleSwap(JointCollection jointCollection, long timeStamp)
{
Joint rightHand = jointCollection[JointType.HandRight];
float d2 = rightHand.Position.X - this.previousPositionR.X;
long dt = timeStamp - this.previousTimeStamp;
float speed2 = 1000 * d2 / dt;
if (speed2 < -2.5) //swap right menu
{
CallKeyEvent(new KeyInputEventArgs(KeyStatut.DOWN, VirtualKeyCode.VK_D));
CallKeyEvent(new KeyInputEventArgs(KeyStatut.UP, VirtualKeyCode.VK_D));
this.atBegin = false;
}
if (speed2 > 2.5) //swap right menu
{
CallKeyEvent(new KeyInputEventArgs(KeyStatut.DOWN, VirtualKeyCode.VK_Q));
CallKeyEvent(new KeyInputEventArgs(KeyStatut.UP, VirtualKeyCode.VK_Q));
if (this.atBegin)
{
CallStateEvent(new StateChangeEventArgs(new MenuTabState()));
}
this.atBegin = true;
}
}
public JointCollection(JointCollection other) : this(EfficioRuntimePINVOKE.new_JointCollection__SWIG_1(JointCollection.getCPtr(other)), true)
{
if (EfficioRuntimePINVOKE.SWIGPendingException.Pending)
{
throw EfficioRuntimePINVOKE.SWIGPendingException.Retrieve();
}
}
public JointCollectionEnumerator(JointCollection collection)
{
collectionRef = collection;
currentIndex = -1;
currentObject = null;
currentSize = collectionRef.Count;
}
private void handleUpDown(JointCollection jointCollection, long timeStamp)
{
Joint rightHand = jointCollection[JointType.HandRight];
Joint shoulderRight = jointCollection[JointType.ShoulderRight];
Joint hipRight = jointCollection[JointType.HipRight];
if (rightHand.Position.X > hipRight.Position.X)
{
if (rightHand.Position.Y > shoulderRight.Position.Y) //main positionnée en haut
{
if (timeStamp - this.previousTimeStampHand >= 1000) //toutes les 1000ms
{
this.previousTimeStampHand = timeStamp;
CallKeyEvent(new KeyInputEventArgs(KeyStatut.DOWN, VirtualKeyCode.VK_Z));
CallKeyEvent(new KeyInputEventArgs(KeyStatut.UP, VirtualKeyCode.VK_Z));
}
}
else if (rightHand.Position.Y < hipRight.Position.Y)//main positionnée en bas
{
if (timeStamp - this.previousTimeStampHand >= 1000)
{
this.previousTimeStampHand = timeStamp;
CallKeyEvent(new KeyInputEventArgs(KeyStatut.DOWN, VirtualKeyCode.VK_S));
CallKeyEvent(new KeyInputEventArgs(KeyStatut.UP, VirtualKeyCode.VK_S));
}
}
}
}
void initObjects(GraphicsDevice graphicsDevice)
{
getFrames = 60;
JointStates = new JointCollection[getFrames];
this.graphicsDevice = graphicsDevice;
//ColorImage = new Texture2D(graphicsDevice, kinect.ColorStream.FrameWidth, kinect.ColorStream.FrameHeight);
}
private void handleUpDown(JointCollection jointCollection, long timeStamp)
{
Joint leftHand = jointCollection[JointType.HandLeft];
Joint shoulderLeft = jointCollection[JointType.ShoulderLeft];
Joint hipLeft = jointCollection[JointType.HipLeft];
if (leftHand.Position.X < hipLeft.Position.X)
{
if (leftHand.Position.Y > shoulderLeft.Position.Y) //main positionnée en haut
{
if (timeStamp - this.previousTimeStampHand >= 1000) //toutes les 1000ms
{
//this._remote.message("up");
this.previousTimeStampHand = timeStamp;
CallKeyEvent(new KeyInputEventArgs(KeyStatut.DOWN, VirtualKeyCode.VK_Z));
CallKeyEvent(new KeyInputEventArgs(KeyStatut.UP, VirtualKeyCode.VK_Z));
}
}
else if (leftHand.Position.Y < hipLeft.Position.Y)//main positionnée en bas
{
if (timeStamp - this.previousTimeStampHand >= 1000)
{
//this._remote.message("down");
this.previousTimeStampHand = timeStamp;
CallKeyEvent(new KeyInputEventArgs(KeyStatut.DOWN, VirtualKeyCode.VK_S));
CallKeyEvent(new KeyInputEventArgs(KeyStatut.UP, VirtualKeyCode.VK_S));
}
}
}
else
{
handleSwap(jointCollection, timeStamp);
}
}
public static bool LeftArmUp(JointCollection currentJoints)
{
Vector3 leftHandPos = new Vector3(2.0f * currentJoints[JointType.HandLeft].Position.X + 0.5f, -2.0f * currentJoints[JointType.HandLeft].Position.Y + 0.5f, currentJoints[JointType.HandLeft].Position.Z);
Vector3 leftElbowPos = new Vector3(2.0f * currentJoints[JointType.ElbowLeft].Position.X + 0.5f, -2.0f * currentJoints[JointType.ElbowLeft].Position.Y + 0.5f, currentJoints[JointType.ElbowLeft].Position.Z);
Vector3 leftShoulderPos = new Vector3(2.0f * currentJoints[JointType.ShoulderLeft].Position.X + 0.5f, -2.0f * currentJoints[JointType.ShoulderLeft].Position.Y + 0.5f, currentJoints[JointType.ShoulderLeft].Position.Z);
return (leftHandPos.Y < leftElbowPos.Y) && (leftHandPos.Y < leftShoulderPos.Y);
}
public void DrawTrackedSkeletonJoints(JointCollection jointCollection)
{
// Render Head and Shoulders
DrawBone(jointCollection[JointType.Head], jointCollection[JointType.ShoulderCenter]);
DrawBone(jointCollection[JointType.ShoulderCenter], jointCollection[JointType.ShoulderLeft]);
DrawBone(jointCollection[JointType.ShoulderCenter], jointCollection[JointType.ShoulderRight]);
// Render Left Arm
DrawBone(jointCollection[JointType.ShoulderLeft], jointCollection[JointType.ElbowLeft]);
DrawBone(jointCollection[JointType.ElbowLeft], jointCollection[JointType.WristLeft]);
DrawBone(jointCollection[JointType.WristLeft], jointCollection[JointType.HandLeft]);
// Render Right Arm
DrawBone(jointCollection[JointType.ShoulderRight], jointCollection[JointType.ElbowRight]);
DrawBone(jointCollection[JointType.ElbowRight], jointCollection[JointType.WristRight]);
DrawBone(jointCollection[JointType.WristRight], jointCollection[JointType.HandRight]);
//Render Spine
DrawBone(jointCollection[JointType.ShoulderCenter], jointCollection[JointType.Spine]);
DrawBone(jointCollection[JointType.Spine], jointCollection[JointType.HipCenter]);
//Render Left Leg
DrawBone(jointCollection[JointType.HipCenter], jointCollection[JointType.HipLeft]);
DrawBone(jointCollection[JointType.HipLeft], jointCollection[JointType.KneeLeft]);
DrawBone(jointCollection[JointType.KneeLeft], jointCollection[JointType.AnkleLeft]);
DrawBone(jointCollection[JointType.AnkleLeft], jointCollection[JointType.FootLeft]);
//Render Right Leg
DrawBone(jointCollection[JointType.HipCenter], jointCollection[JointType.HipRight]);
DrawBone(jointCollection[JointType.HipRight], jointCollection[JointType.KneeRight]);
DrawBone(jointCollection[JointType.KneeRight], jointCollection[JointType.AnkleRight]);
DrawBone(jointCollection[JointType.AnkleRight], jointCollection[JointType.FootRight]);
}
void Plot(JointType centerID, JointType baseID, JointCollection joints)
{
float centerX;
float centerY;
GetCoordinates(centerID, joints, out centerX, out centerY);
float baseX;
float baseY;
GetCoordinates(baseID, joints, out baseX, out baseY);
double diameter = Math.Abs(baseY - centerY);
Ellipse ellipse = new Ellipse
{
Width = diameter,
Height = diameter,
HorizontalAlignment = HorizontalAlignment.Left,
VerticalAlignment = VerticalAlignment.Top,
StrokeThickness = 4.0,
Stroke = new SolidColorBrush(Colors.Green),
StrokeLineJoin = PenLineJoin.Round
};
Canvas.SetLeft(ellipse, centerX - ellipse.Width / 2);
Canvas.SetTop(ellipse, centerY - ellipse.Height / 2);
rootCanvas.Children.Add(ellipse);
}
private void DrawTrackedSkeletonJoints(Image <Bgr, Byte> img, JointCollection jointCollection, int depthOrColor)
{
// Render Head and Shoulders
DrawBone(img, jointCollection[JointType.Head], jointCollection[JointType.ShoulderCenter], depthOrColor);
DrawBone(img, jointCollection[JointType.ShoulderCenter], jointCollection[JointType.ShoulderLeft], depthOrColor);
DrawBone(img, jointCollection[JointType.ShoulderCenter], jointCollection[JointType.ShoulderRight], depthOrColor);
// Render Left Arm
DrawBone(img, jointCollection[JointType.ShoulderLeft], jointCollection[JointType.ElbowLeft], depthOrColor);
DrawBone(img, jointCollection[JointType.ElbowLeft], jointCollection[JointType.WristLeft], depthOrColor);
DrawBone(img, jointCollection[JointType.WristLeft], jointCollection[JointType.HandLeft], depthOrColor);
// Render Right Arm
DrawBone(img, jointCollection[JointType.ShoulderRight], jointCollection[JointType.ElbowRight], depthOrColor);
DrawBone(img, jointCollection[JointType.ElbowRight], jointCollection[JointType.WristRight], depthOrColor);
DrawBone(img, jointCollection[JointType.WristRight], jointCollection[JointType.HandRight], depthOrColor);
// Render other bones...
DrawBone(img, jointCollection[JointType.ShoulderCenter], jointCollection[JointType.Spine], depthOrColor);
DrawBone(img, jointCollection[JointType.Spine], jointCollection[JointType.HipRight], depthOrColor);
DrawBone(img, jointCollection[JointType.KneeRight], jointCollection[JointType.HipRight], depthOrColor);
DrawBone(img, jointCollection[JointType.KneeRight], jointCollection[JointType.AnkleRight], depthOrColor);
DrawBone(img, jointCollection[JointType.FootRight], jointCollection[JointType.AnkleRight], depthOrColor);
DrawBone(img, jointCollection[JointType.Spine], jointCollection[JointType.HipLeft], depthOrColor);
DrawBone(img, jointCollection[JointType.KneeLeft], jointCollection[JointType.HipLeft], depthOrColor);
DrawBone(img, jointCollection[JointType.KneeLeft], jointCollection[JointType.AnkleLeft], depthOrColor);
DrawBone(img, jointCollection[JointType.FootLeft], jointCollection[JointType.AnkleLeft], depthOrColor);
}
private void drawBone(JointCollection p_joints,
JointType p_start,
JointType p_end,
KinectManager p_kinect,
SpriteBatch p_sprite_batch)
{
// Draw a single joint-bone-joint...
Vector2 start = this.skeletonToPoint(p_joints[p_start].Position, p_kinect);
Vector2 end = this.skeletonToPoint(p_joints[p_end].Position, p_kinect);
Vector2 difference = end - start;
Vector2 scale = new Vector2(1.0f, difference.Length() / this.bone_texture.Height);
float angle = (float)Math.Atan2(difference.Y, difference.X) - MathHelper.PiOver2;
// Set colour(s):
Color colour = Color.LightGreen;
if (p_joints[p_start].TrackingState != JointTrackingState.Tracked ||
p_joints[p_end].TrackingState != JointTrackingState.Tracked)
{
colour = Color.Gray;
}
p_sprite_batch.Draw(this.bone_texture,
start,
null,
colour,
angle,
this.bone_origin,
scale,
SpriteEffects.None,
1.0f);
}
public static bool RightArmDown(JointCollection currentJoints)
{
Vector3 rightHandPos = new Vector3(2.0f * currentJoints[JointType.HandRight].Position.X + 0.5f, -2.0f * currentJoints[JointType.HandRight].Position.Y + 0.5f, currentJoints[JointType.HandRight].Position.Z);
Vector3 rightElbowPos = new Vector3(2.0f * currentJoints[JointType.ElbowRight].Position.X + 0.5f, -2.0f * currentJoints[JointType.ElbowRight].Position.Y + 0.5f, currentJoints[JointType.ElbowRight].Position.Z);
Vector3 rightShoulderPos = new Vector3(2.0f * currentJoints[JointType.ShoulderRight].Position.X + 0.5f, -2.0f * currentJoints[JointType.ShoulderRight].Position.Y + 0.5f, currentJoints[JointType.ShoulderRight].Position.Z);
return (rightHandPos.Y > rightElbowPos.Y) && (rightElbowPos.Y > rightShoulderPos.Y);
}
private static bool hareket2(JointCollection joints)
{
return
(joints[JointType.HandRight].Position.X > joints[JointType.Head].Position.X
&& degerlerYakin(0.1f, joints[JointType.HandRight].Position.X, joints[JointType.ElbowRight].Position.X)
&& joints[JointType.ElbowRight].Position.X > joints[JointType.ShoulderRight].Position.X
&& joints[JointType.ElbowRight].Position.X > joints[JointType.Head].Position.X
&& joints[JointType.HandRight].Position.Y > joints[JointType.ElbowRight].Position.Y
&& joints[JointType.ElbowRight].Position.Y > joints[JointType.ShoulderRight].Position.Y
&& joints[JointType.Head].Position.Y > joints[JointType.ElbowRight].Position.Y
&& joints[JointType.HandLeft].Position.X <joints[JointType.Head].Position.X
&& degerlerYakin(0.1f, joints[JointType.HandLeft].Position.X, joints[JointType.ElbowLeft].Position.X)
&& joints[JointType.ElbowLeft].Position.X <joints[JointType.ShoulderLeft].Position.X
&& joints[JointType.ElbowLeft].Position.X <joints[JointType.Head].Position.X
&& joints[JointType.HandLeft].Position.Y> joints[JointType.ElbowLeft].Position.Y
&& joints[JointType.ElbowLeft].Position.Y> joints[JointType.ShoulderLeft].Position.Y
&& joints[JointType.Head].Position.Y> joints[JointType.ElbowRight].Position.Y);
}
private Gestures processGesture(JointCollection joints)
{
Joint leftHand = joints[JointType.HandLeft];
Joint leftElbow = joints[JointType.ElbowLeft];
Joint rightHand = joints[JointType.HandRight];
Joint rightElbow = joints[JointType.ElbowRight];
Joint shoulderCenter = joints[JointType.ShoulderCenter];
Joint shoulderLeft = joints[JointType.ShoulderLeft];
Joint shoulderRight = joints[JointType.ShoulderRight];
if (leftHand.Position.X > rightHand.Position.X &&
leftElbow.Position.X < rightElbow.Position.X &&
leftElbow.Position.Y < rightHand.Position.Y &&
leftHand.Position.Y > rightElbow.Position.Y)
{
return(Gestures.X);
}
else if (leftHand.Position.Y > shoulderCenter.Position.Y &&
rightHand.Position.Y > shoulderCenter.Position.Y &&
leftHand.Position.X < rightHand.Position.X &&
leftElbow.Position.X < shoulderLeft.Position.X &&
rightElbow.Position.X > shoulderRight.Position.X)
{
return(Gestures.O);
}
else
{
return(Gestures.NONE);
}
}
/*
* There is not parent child direct access method in the kinect API,
* so we have to do it manually
* See hierarchy : https://msdn.microsoft.com/en-us/library/hh973073.aspx
*/
private void CreateNodeHierarchy(JointCollection jointCollection, Scene mainScene)
{
Node root = mainScene.RootNode = new Node();
Node hipcenter = AddChildNode(root, jointCollection[JointType.HipCenter]);
Node spine = AddChildNode(hipcenter, jointCollection[JointType.Spine]);
Node shouldcenter = AddChildNode(spine, jointCollection[JointType.ShoulderCenter]);
//ShouldCenter children build
AddChildNode(shouldcenter, jointCollection[JointType.Head]);
Node shoulderleft = AddChildNode(shouldcenter, jointCollection[JointType.ShoulderLeft]);
Node elbowleft = AddChildNode(shoulderleft, jointCollection[JointType.ElbowLeft]);
Node wristleft = AddChildNode(elbowleft, jointCollection[JointType.WristLeft]);
AddChildNode(wristleft, jointCollection[JointType.HandLeft]);
Node shoulderright = AddChildNode(shouldcenter, jointCollection[JointType.ShoulderRight]);
Node elbowright = AddChildNode(shoulderright, jointCollection[JointType.ElbowRight]);
Node wristright = AddChildNode(elbowright, jointCollection[JointType.WristRight]);
AddChildNode(wristright, jointCollection[JointType.HandRight]);
//HipCenter children build
Node hipleft = AddChildNode(hipcenter, jointCollection[JointType.HipLeft]);
Node kneeleft = AddChildNode(hipleft, jointCollection[JointType.KneeLeft]);
Node ankleleft = AddChildNode(kneeleft, jointCollection[JointType.AnkleLeft]);
AddChildNode(ankleleft, jointCollection[JointType.FootLeft]);
Node hipright = AddChildNode(hipcenter, jointCollection[JointType.HipRight]);
Node kneeright = AddChildNode(hipright, jointCollection[JointType.KneeRight]);
Node ankleright = AddChildNode(kneeright, jointCollection[JointType.AnkleRight]);
AddChildNode(ankleright, jointCollection[JointType.FootRight]);
}
private void AppendTestString(Skeleton skel)
{
JointCollection joint = skel.Joints;
WriteJointToString(joint[JointType.Head]);
WriteJointToString(joint[JointType.ShoulderCenter]);
WriteJointToString(joint[JointType.ShoulderRight]);
WriteJointToString(joint[JointType.ShoulderLeft]);
WriteJointToString(joint[JointType.ElbowRight]);
WriteJointToString(joint[JointType.ElbowLeft]);
WriteJointToString(joint[JointType.WristRight]);
WriteJointToString(joint[JointType.WristLeft]);
WriteJointToString(joint[JointType.HandRight]);
WriteJointToString(joint[JointType.HandLeft]);
WriteJointToString(joint[JointType.Spine]);
WriteJointToString(joint[JointType.HipCenter]);
WriteJointToString(joint[JointType.HipRight]);
WriteJointToString(joint[JointType.HipLeft]);
WriteJointToString(joint[JointType.KneeRight]);
WriteJointToString(joint[JointType.KneeLeft]);
WriteJointToString(joint[JointType.AnkleRight]);
WriteJointToString(joint[JointType.AnkleLeft]);
WriteJointToString(joint[JointType.FootRight]);
WriteJointToString(joint[JointType.FootLeft]);
}
private void handleSword(JointCollection joints, long timeStamp)
{
Joint rightHand = joints[JointType.HandRight];
Joint shoulderRight = joints[JointType.ShoulderRight];
if (rightHand.Position.Y > shoulderRight.Position.Y)
{
if (!this.handRaised)
{
this.handRaised = true;
this.timeStampHand = timeStamp;
}
}
else if (this.handRaised)
{
this._remote.mouseLeftDown();
if (timeStamp - this.timeStampHand > 800)
{
Thread.Sleep(2000);
}
else
{
Thread.Sleep(100);
}
this._remote.mouseLeftUp();
this.handRaised = false;
}
}
private void calculFootPosition(JointCollection joints)
{
Joint footLeft = joints[JointType.FootLeft];
Joint footRight = joints[JointType.FootRight];
Console.WriteLine((footLeft.Position.Z - footRight.Position.Z).ToString());
if (footLeft.Position.Z - footRight.Position.Z > 0.2)
{
remoteOperation.press_key_down(VirtualKeyCode.UP);
keysPress.Add(VirtualKeyCode.UP);
}
else
if (footRight.Position.Z - footLeft.Position.Z > 0.2)
{
remoteOperation.press_key_down(VirtualKeyCode.DOWN);
keysPress.Add(VirtualKeyCode.DOWN);
}
else
{
remoteOperation.press_key_up(VirtualKeyCode.UP);
remoteOperation.press_key_up(VirtualKeyCode.DOWN);
keysPress.Remove(VirtualKeyCode.UP);
keysPress.Remove(VirtualKeyCode.DOWN);
}
}
/// <summary>
/// Calculates the angle between the segments of the body defined by the specified joints.
/// </summary>
/// <param name="joints"></param>
/// <param name="joint1"></param>
/// <param name="joint2">Must be between joint1 and joint3</param>
/// <param name="joint3"></param>
/// <returns>The angle in degrees between the specified body segmeents.</returns>
public double GetBodySegmentAngle(JointCollection joints)
{
Joint joint1 = joints[_JointId1.JointType];
Joint joint2 = joints[_JointId2.JointType];
Joint joint3 = joints[_JointId3.JointType];
Vector3 vectorJoint1ToJoint2 = new Vector3(joint1.Position.X - joint2.Position.X, joint1.Position.Y - joint2.Position.Y, 0);
Vector3 vectorJoint2ToJoint3 = new Vector3(joint2.Position.X - joint3.Position.X, joint2.Position.Y - joint3.Position.Y, 0);
vectorJoint1ToJoint2.Normalize();
vectorJoint2ToJoint3.Normalize();
Vector3 crossProduct = Vector3.Cross(vectorJoint1ToJoint2, vectorJoint2ToJoint3);
double crossProductLength = crossProduct.Z;
double dotProduct = Vector3.Dot(vectorJoint1ToJoint2, vectorJoint2ToJoint3);
double segmentAngle = Math.Atan2(crossProductLength, dotProduct);
// Convert the result to degrees.
double degrees = segmentAngle * (180 / Math.PI);
// Add the angular offset. Use modulo 360 to convert the value calculated above to a range
// from 0 to 360.
degrees = (degrees + _RotationOffset) % 360;
// Calculate whether the coordinates should be reversed to account for different sides
if (_ReverseCoordinates)
{
degrees = CalculateReverseCoordinates(degrees);
}
return(degrees);
}
public void Paint(IPainter painter, DrawingContext dc, JointCollection joints)//change dc to draw on screen
{
Joint rightHand = joints.Where(joint => joint.JointType == JointType.HandRight).FirstOrDefault();
Joint leftHand = joints.Where(joint => joint.JointType == JointType.HandLeft).FirstOrDefault();
Joint head = joints.Where(joint => joint.JointType == JointType.Head).FirstOrDefault();
if (rightHand.Position.Y > head.Position.Y)
{
rightHandFalg = true;
leftHandFalg = false;
}
if (leftHand.Position.Y > head.Position.Y)
{
leftHandFalg = true;
rightHandFalg = false;
}
if (rightHandFalg)
{
DrawInBmp(painter.SkeletonPointToScreen(rightHand.Position), DrawThickness, trackedJointBrush);
}
if (leftHandFalg)
{
DrawInBmp(painter.SkeletonPointToScreen(leftHand.Position), ClearThickness, backgroundBrush);
}
dc.DrawImage(bmp, new System.Windows.Rect(0, 0, RenderWidth, RenderHeight));
if (leftHandFalg)
{
dc.DrawRectangle(CleanerBrush, null, new Rect(leftHand.Position.X - ClearThickness, leftHand.Position.Y - ClearThickness / 2, ClearThickness * 2, ClearThickness));
}
}
/// <summary>
/// Calculates the angle between the segments of the body defined by the specified joints.
/// </summary>
/// <param name="joints"></param>
/// <param name="joint1"></param>
/// <param name="joint2">Must be between joint1 and joint3</param>
/// <param name="joint3"></param>
/// <returns>The angle in degrees between the specified body segmeents.</returns>
public double GetBodySegmentAngle(JointCollection joints)
{
Joint joint1 = joints[_JointId1.JointType];
Joint joint2 = joints[_JointId2.JointType];
Joint joint3 = joints[_JointId3.JointType];
Vector3 vectorJoint1ToJoint2 = new Vector3(joint1.Position.X - joint2.Position.X, joint1.Position.Y - joint2.Position.Y, 0);
Vector3 vectorJoint2ToJoint3 = new Vector3(joint2.Position.X - joint3.Position.X, joint2.Position.Y - joint3.Position.Y, 0);
vectorJoint1ToJoint2.Normalize();
vectorJoint2ToJoint3.Normalize();
Vector3 crossProduct = Vector3.Cross(vectorJoint1ToJoint2, vectorJoint2ToJoint3);
double crossProductLength = crossProduct.Z;
double dotProduct = Vector3.Dot(vectorJoint1ToJoint2, vectorJoint2ToJoint3);
double segmentAngle = Math.Atan2(crossProductLength, dotProduct);
// Convert the result to degrees.
double degrees = segmentAngle * (180 / Math.PI);
// Add the angular offset. Use modulo 360 to convert the value calculated above to a range
// from 0 to 360.
degrees = (degrees + _RotationOffset) % 360;
// Calculate whether the coordinates should be reversed to account for different sides
if (_ReverseCoordinates)
{
degrees = CalculateReverseCoordinates(degrees);
}
return degrees;
}
private void adjustBoneRendering(JointCollection joints, KinectSkeleton skeletonCanvas, DepthImageFrame depthFrame, int canvasChildIndex, params JointType[] ids)
{
PointCollection points = new PointCollection(ids.Length);
for (int i = 0; i < ids.Length; ++i)
{
if (joints[ids[i]].TrackingState != JointTrackingState.NotTracked)
{
points.Add(getJoint2DLocation(joints[ids[i]], skeletonCanvas, depthFrame));
}
}
Polyline polyline = (Polyline)skeletonCanvas.Children[canvasChildIndex];
polyline.Points = points;
switch (joints[ids[ids.Length - 1]].TrackingState)
{
case JointTrackingState.Tracked:
polyline.Stroke = Brushes.Green;
break;
case JointTrackingState.Inferred:
polyline.Stroke = Brushes.Yellow;
break;
case JointTrackingState.NotTracked:
//no rendering
break;
}
}
public override bool ShouldHandle(JointCollection joints)
{
if (RoboManagerInstance.LeftForeArmStatus == ArmStatus.ArmUp) { return false; }
var leftForearmAngle = GetAngle(joints[JointType.ShoulderLeft].AsVector3D(), joints[JointType.WristLeft].AsVector3D(), joints[JointType.ElbowLeft].AsVector3D());
return leftForearmAngle <= Angles.FORE_ARM_UP;
}
void updateJointStates(JointCollection current)
{
for (int i = getFrames - 1; i > 0; i--)
{
JointStates[i] = JointStates[i - 1];
}
JointStates[0] = current;
}
static public double calculateAngleFoot(JointCollection jointcollection)
{
Vector Vector1 = new Vector(jointcollection[JointType.AnkleLeft].Position.X, jointcollection[JointType.AnkleLeft].Position.Z);
Vector Vector2 = new Vector(jointcollection[JointType.AnkleLeft].Position.X, jointcollection[JointType.AnkleLeft].Position.Z - 0.5);
Vector Vector3 = new Vector(jointcollection[JointType.FootLeft].Position.X, jointcollection[JointType.FootLeft].Position.Z);
return(anglebetween2Dvectors(Vector3 - Vector1, Vector2 - Vector1));
}
internal Skeleton(SkeletonTrackingState trackingState, int trackingID, int enrollmentIndex, SkeletonPoint position, JointCollection joints)
{
TrackingState = trackingState;
TrackingID = trackingID;
EnrollmentIndex = enrollmentIndex;
Position = position;
Joints = joints;
}
public Polyline GetBodySegment(JointCollection joints, Brush brush, params JointType[] ids)
{
var points = new PointCollection(ids.Length);
foreach (var t in ids)
points.Add(GetDisplayPosition(joints[t]));
var polyline = new Polyline { Points = points, Stroke = brush, StrokeThickness = 6 };
return polyline;
}
public void SetRange(int index, JointCollection values)
{
EfficioRuntimePINVOKE.JointCollection_SetRange(swigCPtr, index, JointCollection.getCPtr(values));
if (EfficioRuntimePINVOKE.SWIGPendingException.Pending)
{
throw EfficioRuntimePINVOKE.SWIGPendingException.Retrieve();
}
}
static public double calculateAngleBack(JointCollection jointcollection)
{
Vector Vector1 = new Vector(jointcollection[JointType.HipCenter].Position.X, jointcollection[JointType.HipCenter].Position.Y);
Vector Vector2 = new Vector(jointcollection[JointType.HipCenter].Position.X, jointcollection[JointType.HipCenter].Position.Y + 0.5);
Vector Vector3 = new Vector(jointcollection[JointType.ShoulderCenter].Position.X, jointcollection[JointType.ShoulderCenter].Position.Y);
return(anglebetween2Dvectors(Vector3 - Vector1, Vector2 - Vector1));
}
public override bool ShouldHandle(JointCollection joints)
{
if (RoboManagerInstance.RightForeArmStatus == ArmStatus.ArmMiddle) { return false; }
var rightForearmAngle = GetAngle(joints[JointType.ShoulderRight].AsVector3D(), joints[JointType.WristRight].AsVector3D(), joints[JointType.ElbowRight].AsVector3D());
return rightForearmAngle > Angles.FORE_ARM_UP && rightForearmAngle < Angles.FORE_ARM_DOWN;
}
public static bool EgyptianTest(JointCollection currentJoints)
{
Vector3 rightHandPos = new Vector3(2.0f * currentJoints[JointType.HandRight].Position.X + 0.5f, -2.0f * currentJoints[JointType.HandRight].Position.Y + 0.5f, currentJoints[JointType.HandRight].Position.Z);
Vector3 rightElbowPos = new Vector3(2.0f * currentJoints[JointType.ElbowRight].Position.X + 0.5f, -2.0f * currentJoints[JointType.ElbowRight].Position.Y + 0.5f, currentJoints[JointType.ElbowRight].Position.Z);
Vector3 leftHandPos = new Vector3(2.0f * currentJoints[JointType.HandLeft].Position.X + 0.5f, -2.0f * currentJoints[JointType.HandLeft].Position.Y + 0.5f, currentJoints[JointType.HandLeft].Position.Z);
Vector3 leftElbowPos = new Vector3(2.0f * currentJoints[JointType.ElbowLeft].Position.X + 0.5f, -2.0f * currentJoints[JointType.ElbowLeft].Position.Y + 0.5f, currentJoints[JointType.ElbowLeft].Position.Z);
return rightHandPos.Y > rightElbowPos.Y && leftHandPos.Y > leftElbowPos.Y;
}
public void SetJoints(JointCollection joints)
{
foreach (Joint joint in joints)
{
SetJoint(joint);
}
this.timestamp = DateTime.Now;
Console.WriteLine(timestamp);
}
private void setBoneConstructs()
{
boneverts = new VertexPositionColor[skeleton[0].Joints.Count + 1];
JointCollection jlist = skeleton[0].Joints;
boneverts[0] = constructVert(JointType.Head);
boneverts[1] = constructVert(JointType.ShoulderCenter);
boneverts[2] = constructVert(JointType.ShoulderRight);
boneverts[3] = constructVert(JointType.ElbowRight);
boneverts[4] = constructVert(JointType.WristRight);
boneverts[5] = constructVert(JointType.HandRight);
boneverts[6] = constructVert(JointType.ShoulderLeft);
boneverts[7] = constructVert(JointType.ElbowLeft);
boneverts[8] = constructVert(JointType.WristLeft);
boneverts[9] = constructVert(JointType.HandLeft);
boneInds = new short[18 + 2];
for (short i = 0; i < 5; i++)
{
boneInds[i * 2] = i;
boneInds[i * 2 + 1] = (short)(i + 1);
}
boneInds[10] = 1;
boneInds[11] = 6;
for (short i = 6; i < 9; i++)
{
boneInds[i * 2] = i;
boneInds[i * 2 + 1] = (short)(i + 1);
}
//The right hand ray
//boneRay = ToWorldSpace(skeleton[0].Joints[JointType.HandRight].Position);
//Vector3 sh = ToWorldSpace(skeleton[0].Joints[JointType.ElbowRight].Position) - ToWorldSpace(skeleton[0].Joints[JointType.HandRight].Position);
//sh.Normalize();
//float distance = innerWallBounds - ToWorldSpace(skeleton[0].Joints[JointType.HandRight].Position).Z;
//boneRay -= (boneNormal * distance);
/** New Line Position Calculation performed here! **/
Vector3 planeOrigin = new Vector3(0, 0, innerWallBounds);
Vector3 planeNormal = new Vector3(0, 0, outerWallBounds - innerWallBounds);
planeNormal.Normalize();
Vector3 sElbow = ToWorldSpace(skeleton[0].Joints[JointType.ElbowRight].Position);
Vector3 sDirection = ToWorldSpace(skeleton[0].Joints[JointType.HandRight].Position) - sElbow;
sDirection.Normalize();
float distance = Vector3.Dot(planeNormal, (planeOrigin - sElbow)) / Vector3.Dot(planeNormal, sDirection);
boneRay = distance * sDirection + sElbow;
boneverts[10] = new VertexPositionColor(boneRay, Color.Red);
boneInds[18] = 3;
boneInds[19] = 10;
}
public void UpdateBonePosition(JointCollection joints, JointType j1, JointType j2)
{
var seg = new Segment(
(joints[j1].Position.X * _playerScale) + _playerCenter.X,
_playerCenter.Y - (joints[j1].Position.Y * _playerScale),
(joints[j2].Position.X * _playerScale) + _playerCenter.X,
_playerCenter.Y - (joints[j2].Position.Y * _playerScale))
{ Radius = Math.Max(3.0, _playerBounds.Height * BoneSize) / 2 };
UpdateSegmentPosition(j1, j2, seg);
}
public JointCollection DistalJoints(Joint joint)
{
JointCollection ret = new JointCollection(EfficioRuntimePINVOKE.Joint_DistalJoints(swigCPtr, Joint.getCPtr(joint)), true);
if (EfficioRuntimePINVOKE.SWIGPendingException.Pending)
{
throw EfficioRuntimePINVOKE.SWIGPendingException.Retrieve();
}
return(ret);
}
/// <summary>
/// Resets the player specific settings of the gesture manager.
/// </summary>
/// <param name="skeleton">The player's skeletal representation.</param>
public void ResetPlayerSettings(Skeleton skeleton)
{
if (skeleton != null)
{
this.skeletonJoints = skeleton.Joints;
this.standardSpineY = this.skeletonJoints[JointType.Spine].Position.Y;
this.PlayerBodySize = this.CalculateBodySize();
this.SetGesturesToPlayer();
}
}
public static bool Resume(JointCollection currentJoints)
{
Vector3 handPos;
if (InputState.DominantSide == DominantSide.Right)
handPos = new Vector3(2.0f * currentJoints[JointType.HandLeft].Position.X + 0.5f, -2.0f * currentJoints[JointType.HandLeft].Position.Y + 0.5f, 2.0f * currentJoints[JointType.HandLeft].Position.Z + 0.5f);
else
handPos = new Vector3(2.0f * currentJoints[JointType.HandRight].Position.X + 0.5f, -2.0f * currentJoints[JointType.HandRight].Position.Y + 0.5f, 2.0f * currentJoints[JointType.HandRight].Position.Z + 0.5f);
Vector3 headPos = new Vector3(2.0f * currentJoints[JointType.Head].Position.X + 0.5f, -2.0f * currentJoints[JointType.Head].Position.Y + 0.5f, 2.0f * currentJoints[JointType.HandLeft].Position.Z + 0.5f);
return handPos.Y < headPos.Y;
}
private void MyDataFrameReady(object sender, DataFrameReadyEventArgs e)
{
using (var dataFrame = e.OpenFrame() as DataFrame)
{
if (dataFrame != null)
{
state = dataFrame.Skeletons[0].TrackingState;
mySkeleton = dataFrame.Skeletons[0].Joints;
}
}
}
public override bool ShouldHandle(JointCollection joints)
{
if (RoboManagerInstance.LeftArmStatus == ArmStatus.ArmMiddle) { return false; }
var shoulderCenter = joints[JointType.ShoulderCenter].AsVector3D();
var leftArmAngle = GetAngle(joints[JointType.ElbowLeft].AsVector3D() - shoulderCenter,
joints[JointType.Spine].AsVector3D() - shoulderCenter,
shoulderCenter);
return leftArmAngle > Angles.ARM_IN && leftArmAngle < Angles.ARM_OUT;
}
public override bool ShouldHandle(JointCollection joints)
{
if (RoboManagerInstance.RightArmStatus == ArmStatus.ArmDown) { return false; }
var shoulderCenter = joints[JointType.ShoulderCenter].AsVector3D();
var rightArmAngle = GetAngle(joints[JointType.ElbowRight].AsVector3D() - shoulderCenter,
joints[JointType.Spine].AsVector3D() - shoulderCenter,
shoulderCenter);
return rightArmAngle <= Angles.ARM_IN;
}
public static bool Pause(JointCollection currentJoints)
{
Vector3 handPos;
if (InputState.DominantSide == DominantSide.Right)
handPos = new Vector3(2.0f * currentJoints[JointType.HandLeft].Position.X + 0.5f, -2.0f * currentJoints[JointType.HandLeft].Position.Y + 0.5f, 2.0f * currentJoints[JointType.HandLeft].Position.Z + 0.5f);
else
handPos = new Vector3(2.0f * currentJoints[JointType.HandRight].Position.X + 0.5f, -2.0f * currentJoints[JointType.HandRight].Position.Y + 0.5f, 2.0f * currentJoints[JointType.HandRight].Position.Z + 0.5f);
Vector3 spinePos = new Vector3(2.0f * currentJoints[JointType.Spine].Position.X + 0.5f, -2.0f * currentJoints[JointType.Spine].Position.Y + 0.5f, 2.0f * currentJoints[JointType.Spine].Position.Z + 0.5f);
Vector3 hipLeftPos = new Vector3(2.0f * currentJoints[JointType.HipLeft].Position.X + 0.5f, -2.0f * currentJoints[JointType.HipLeft].Position.Y + 0.5f, 2.0f * currentJoints[JointType.HipLeft].Position.Z + 0.5f);
Vector3 hipRightPos = new Vector3(2.0f * currentJoints[JointType.HipRight].Position.X + 0.5f, -2.0f * currentJoints[JointType.HipRight].Position.Y + 0.5f, 2.0f * currentJoints[JointType.HipRight].Position.Z + 0.5f);
Vector3 headPos = new Vector3(2.0f * currentJoints[JointType.Head].Position.X + 0.5f, -2.0f * currentJoints[JointType.Head].Position.Y + 0.5f, 2.0f * currentJoints[JointType.Head].Position.Z + 0.5f);
return handPos.X > hipLeftPos.X && handPos.X < hipRightPos.X && handPos.Y > headPos.Y;
}
void Trace(JointType sourceID, JointType destinationID, JointCollection joints)
{
float sourceX;
float sourceY;
GetCoordinates(sourceID, joints, out sourceX, out sourceY);
float destinationX;
float destinationY;
GetCoordinates(destinationID, joints, out destinationX, out destinationY);
Line line = new Line
{
X1 = sourceX,
Y1 = sourceY,
X2 = destinationX,
Y2 = destinationY,
HorizontalAlignment = HorizontalAlignment.Left,
VerticalAlignment = VerticalAlignment.Top,
StrokeThickness = 4.0,
Stroke = new SolidColorBrush(Colors.Green),
StrokeLineJoin = PenLineJoin.Round
};
rootCanvas.Children.Add(line);
}
private KinectDataPoint getDataPointRelativeToBody(JointCollection joints,
JointType joint, CoordinateMapper coordinateMapper)
{
ColorImagePoint colorPoint = coordinateMapper.MapSkeletonPointToColorPoint(
joints[joint].Position, ColorImageFormat.RgbResolution640x480Fps30);
return new KinectDataPoint(colorPoint, joints[joint].Position, joints[JointType.ShoulderCenter].Position);
}
public void UpdateJointPosition(JointCollection joints, JointType j)
{
var seg = new Segment(
(joints[j].Position.X * _playerScale) + _playerCenter.X,
_playerCenter.Y - (joints[j].Position.Y * _playerScale))
{ Radius = _playerBounds.Height * ((j == JointType.Head) ? HeadSize : HandSize) / 2 };
UpdateSegmentPosition(j, j, seg);
}
/// <summary>
/// Works out how to draw a line ('bone') for sent Joints
/// </summary>
/// <param name="joints">Kinect NUI Joints</param>
/// <param name="brush">The brush we'll use to colour the joints</param>
/// <param name="ids">The JointsIDs we're interested in</param>
/// <returns>A line or lines</returns>
private Polyline GetBodySegment(JointCollection joints, Brush brush, params JointType[] ids)
{
var points = new PointCollection(ids.Length);
foreach (JointType t in ids)
{
points.Add(GetDisplayPosition(joints[t]));
}
var polyline = new Polyline();
polyline.Points = points;
polyline.Stroke = brush;
polyline.StrokeThickness = 5;
return polyline;
}
/// <summary>
/// This method draws a bone.
/// </summary>
/// <param name="joints">The joint data.</param>
/// <param name="startJoint">The starting joint.</param>
/// <param name="endJoint">The ending joint.</param>
private void DrawBone(JointCollection joints, JointType startJoint, JointType endJoint)
{
Vector2 start = this.mapMethod(joints[startJoint].Position);
Vector2 end = this.mapMethod(joints[endJoint].Position);
Vector2 diff = end - start;
Vector2 scale = new Vector2(1.0f, diff.Length() / this.boneTexture.Height);
float angle = (float)Math.Atan2(diff.Y, diff.X) - MathHelper.PiOver2;
Color color = Color.LightGreen;
if (joints[startJoint].TrackingState != JointTrackingState.Tracked ||
joints[endJoint].TrackingState != JointTrackingState.Tracked)
{
color = Color.Gray;
}
this.SharedSpriteBatch.Draw(this.boneTexture, start, null, color, angle, this.boneOrigin, scale, SpriteEffects.None, 1.0f);
}
public KinectJointsCollection(JointCollection joints)
{
_joints = joints;
}
private void GetBodySegment(
JointCollection joints, List<LineSegment3d> segs,
params JointType[] ids
)
{
// Get the sequence of segments connecting the
// locations of the joints passed in
Point3d start, end;
start = PointFromVector(joints[ids[0]].Position);
for (int i = 1; i < ids.Length; ++i)
{
end = PointFromVector(joints[ids[i]].Position);
segs.Add(new LineSegment3d(start, end));
start = end;
}
}
private Polyline GetBodySegment(JointCollection joints, Brush brush, params JointType[] ids)
{
PointCollection points = new PointCollection(ids.Length);
for (int i = 0; i < ids.Length; ++i)
{
points.Add(GetDisplayPosition(joints[ids[i]]));
}
Polyline polyline = new Polyline();
polyline.Points = points;
polyline.Stroke = brush;
polyline.StrokeThickness = 2;
return polyline;
}
/// <summary>
/// Helper used to get the world translation for the root.
/// </summary>
/// <param name="joints">Nui Joint collection.</param>
/// <param name="seatedMode">Boolean true if seated mode.</param>
/// <returns>Returns a Matrix containing the translation.</returns>
private Matrix GetModelWorldTranslation(JointCollection joints, bool seatedMode)
{
SkeletonPoint transRootVec3 = new SkeletonPoint();
if (seatedMode && joints[JointType.ShoulderCenter].TrackingState != JointTrackingState.NotTracked)
{
transRootVec3 = joints[JointType.ShoulderCenter].Position;
}
else
{
if (joints[JointType.HipCenter].TrackingState != JointTrackingState.NotTracked)
{
transRootVec3 = joints[JointType.HipCenter].Position;
}
else if (joints[JointType.ShoulderCenter].TrackingState != JointTrackingState.NotTracked)
{
// finally try shoulder center if this is tracked while hip center is not
transRootVec3 = joints[JointType.ShoulderCenter].Position;
}
}
#region Calculate Real Length
float modelZvalue = 0.0f;
{
if (joints[JointType.HipCenter].TrackingState != JointTrackingState.NotTracked
&& joints[JointType.ShoulderCenter].TrackingState != JointTrackingState.NotTracked
&& joints[JointType.Head].TrackingState != JointTrackingState.NotTracked)
{
SkeletonPoint hipCenterPoint = joints[JointType.HipCenter].Position, shoulderCenterPoint = joints[JointType.ShoulderCenter].Position;
SkeletonPoint headPoint = joints[JointType.Head].Position;
hipCenterPoint.Z = headPoint.Z;
shoulderCenterPoint.Z = headPoint.Z;
DepthImagePoint hipCenterDepthPoint = Chooser.Sensor.MapSkeletonPointToDepth(hipCenterPoint, Chooser.Sensor.DepthStream.Format),
shoulderCenterDepthPoint = Chooser.Sensor.MapSkeletonPointToDepth(shoulderCenterPoint, Chooser.Sensor.DepthStream.Format);
float dLengthDepthHipShoulder = Vector2.Distance(new Vector2(hipCenterDepthPoint.X,hipCenterDepthPoint.Y), new Vector2(shoulderCenterDepthPoint.X,shoulderCenterDepthPoint.Y));
float dLengthHipShoulder = Vector3.Distance(new Vector3(hipCenterPoint.X, hipCenterPoint.Y, hipCenterPoint.Z), new Vector3(shoulderCenterPoint.X, shoulderCenterPoint.Y, shoulderCenterPoint.Z));
float dRealZvalue = (hipCenterPoint.Z + shoulderCenterPoint.Z) / 2;
Vector3 modelEndVec3 = new Vector3();
for (int i=1;i<5;i++)
{
modelEndVec3 += boneTransforms[i].Translation;
}
float modelLengthHipShoulder = Vector3.Distance(new Vector3(0,0,0), modelEndVec3);
modelZvalue = dRealZvalue / dLengthHipShoulder * modelLengthHipShoulder;
}
}
#endregion
Vector3 certainVector = KinectHelper.SkeletonPointToVector3(transRootVec3);
DepthImagePoint deptImagePoint = Chooser.Sensor.MapSkeletonPointToDepth(transRootVec3, Chooser.Sensor.DepthStream.Format);
//
// (x,y) in kinect world space is equal ratio
// to the virtual world for the camera space
//
// Vector3 translationVector = new Vector3(deptImagePoint.X, deptImagePoint.Y, 50);
//return Matrix.CreateTranslation(translationVector);
Vector3 test = new Vector3();
if (Chooser.Sensor.DepthStream.Format == DepthImageFormat.Resolution640x480Fps30)
{
//modelZvalue
float viewportWidth = 2 * (float)Math.Tan(28.5f / 180.0f * Math.PI) * modelZvalue,
viewportHeight = 2 * (float)Math.Tan(21.5f / 180.0f * Math.PI) * modelZvalue,
ratioX = deptImagePoint.X / 640.0f,
ratioY = deptImagePoint.Y / 480.0f,
posX = viewportWidth / 2 - viewportWidth * ratioX,
posY = viewportHeight * ratioY - viewportHeight / 2;
test.X = posX;
test.Y = posY;
}
test.Z = -modelZvalue;
return Matrix.CreateTranslation(test);
}
// Draw all the bones and joints of a Tracked Skeleton
private void DrawSkeletonBonesAndJoints(DrawingContext dc, JointCollection joints)
{
// Render Head and Shoulders
DrawBone(dc, joints[JointType.Head], joints[JointType.ShoulderCenter]);
DrawBone(dc, joints[JointType.ShoulderCenter], joints[JointType.ShoulderLeft]);
DrawBone(dc, joints[JointType.ShoulderCenter], joints[JointType.ShoulderRight]);
// Render Left Arm
DrawBone(dc, joints[JointType.ShoulderLeft], joints[JointType.ElbowLeft]);
DrawBone(dc, joints[JointType.ElbowLeft], joints[JointType.WristLeft]);
DrawBone(dc, joints[JointType.WristLeft], joints[JointType.HandLeft]);
// Render Right Arm
DrawBone(dc, joints[JointType.ShoulderRight], joints[JointType.ElbowRight]);
DrawBone(dc, joints[JointType.ElbowRight], joints[JointType.WristRight]);
DrawBone(dc, joints[JointType.WristRight], joints[JointType.HandRight]);
// Render Body and Hips
DrawBone(dc, joints[JointType.ShoulderCenter], joints[JointType.Spine]);
DrawBone(dc, joints[JointType.Spine], joints[JointType.HipCenter]);
DrawBone(dc, joints[JointType.HipCenter], joints[JointType.HipLeft]);
DrawBone(dc, joints[JointType.HipCenter], joints[JointType.HipRight]);
// Render Left Leg
DrawBone(dc, joints[JointType.HipLeft], joints[JointType.KneeLeft]);
DrawBone(dc, joints[JointType.KneeLeft], joints[JointType.AnkleLeft]);
DrawBone(dc, joints[JointType.AnkleLeft], joints[JointType.FootLeft]);
// Render Right Leg
DrawBone(dc, joints[JointType.HipRight], joints[JointType.KneeRight]);
DrawBone(dc, joints[JointType.KneeRight], joints[JointType.AnkleRight]);
DrawBone(dc, joints[JointType.AnkleRight], joints[JointType.FootRight]);
// Render Joints
foreach (Joint joint in joints)
{
if (joint.TrackingState == JointTrackingState.Tracked)
dc.DrawEllipse(this.trackedJointBrush, null, this.SkeletonPointToScreen(joint.Position), JointThickness, JointThickness);
else if (joint.TrackingState == JointTrackingState.Inferred)
dc.DrawEllipse(this.inferredJointBrush, null, this.SkeletonPointToScreen(joint.Position), JointThickness, JointThickness);
}
}
private void TrackedSkeletonJoints(JointCollection jointCollection)
{
if(hand == "r")
TrackBone(jointCollection[JointType.WristRight], jointCollection[JointType.HandRight]);
else if(hand == "l")
TrackBone(jointCollection[JointType.WristLeft], jointCollection[JointType.HandLeft]);
}
/// <summary>
/// Helper used to get the world translation for the root.
/// </summary>
/// <param name="joints">Nui Joint collection.</param>
/// <param name="seatedMode">Boolean true if seated mode.</param>
/// <returns>Returns a Matrix containing the translation.</returns>
private Matrix GetModelWorldTranslation(JointCollection joints, bool seatedMode)
{
Vector3 transVec = Vector3.Zero;
if (seatedMode && joints[JointType.ShoulderCenter].TrackingState != JointTrackingState.NotTracked)
{
transVec = KinectHelper.SkeletonPointToVector3(joints[JointType.ShoulderCenter].Position);
}
else
{
if (joints[JointType.HipCenter].TrackingState != JointTrackingState.NotTracked)
{
transVec = KinectHelper.SkeletonPointToVector3(joints[JointType.HipCenter].Position);
}
else if (joints[JointType.ShoulderCenter].TrackingState != JointTrackingState.NotTracked)
{
// finally try shoulder center if this is tracked while hip center is not
transVec = KinectHelper.SkeletonPointToVector3(joints[JointType.ShoulderCenter].Position);
}
}
if (this.fixAvatarHipCenterDrawHeight)
{
transVec.Y = this.avatarHipCenterDrawHeight;
}
// Here we scale the translation, as the "Dude" avatar mesh is defined in centimeters, and the Kinect skeleton joint positions in meters.
return Matrix.CreateTranslation(transVec * SkeletonTranslationScaleFactor);
}
public override bool ShouldHandle(JointCollection joints)
{
return false;
}
