private Node AddChildNode(Node parentNode, Joint childJoint)
{
Node newChildNode = new Node();
newChildNode.Name = GetNodeIdName(childJoint.JointType);
parentNode.Children.Add(newChildNode);
return (newChildNode);
}
/// <summary>
/// Draw a line between two joints in a canvas
/// </summary>
/// <param name="canvas">the canvas</param>
/// <param name="jstart">the joint where to start the line</param>
/// <param name="jend">the joint where to end the line</param>
/// <param name="vskeleton">the visual skeleton</param>
private static void DrawLineBetween(Canvas canvas, Joint jstart, Joint jend, VisualSkeleton vskeleton)
{
if (vskeleton.positionMap.ContainsKey(jstart) && vskeleton.positionMap.ContainsKey(jend))
{
Line l = new Line();
l.X1 = vskeleton.positionMap[jstart].X;
l.Y1 = vskeleton.positionMap[jstart].Y;
l.X2 = vskeleton.positionMap[jend].X;
l.Y2 = vskeleton.positionMap[jend].Y;
if (jstart.TrackingState == JointTrackingState.Inferred &&
jend.TrackingState == JointTrackingState.Inferred)
{
l.Stroke = Brushes.Yellow;
l.StrokeThickness = 3;
}
else if (jstart.TrackingState == JointTrackingState.Tracked &&
jend.TrackingState == JointTrackingState.Tracked)
{
l.Stroke = Brushes.Green;
l.StrokeThickness = 3;
}
else if (jstart.TrackingState == JointTrackingState.NotTracked ||
jend.TrackingState == JointTrackingState.NotTracked)
{
l.Stroke = Brushes.Transparent;
l.StrokeThickness = 0;
}
canvas.Children.Add(l);
}
}
public void JointTrackingStateIsSerialised()
{
var j = new Joint {TrackingState = JointTrackingState.Tracked};
var json = j.ToJson();
Assert.IsTrue(json.Contains("Tracked"));
}
public void actualizarPostura(Skeleton esqueleto)
{
manoDerAct = esqueleto.Joints[JointType.WristRight];
manoIzqAct = esqueleto.Joints[JointType.WristLeft];
codoDerAct = esqueleto.Joints[JointType.ElbowRight];
codoIzqAct = esqueleto.Joints[JointType.ElbowLeft];
}
/// <summary>
/// ジョイント間の線を引く
/// </summary>
/// <param name="skeleton"></param>
/// <param name="joint"></param>
/// <returns></returns>
private void DrawJointLine( Skeleton skeleton, Joint joint )
{
// ジョイントと関連づいている一方のジョイントを取得する
var start = skeleton.BoneOrientations[joint.JointType].StartJoint;
var end = skeleton.BoneOrientations[joint.JointType].EndJoint;
var startJoint = skeleton.Joints[start];
var endJoint = skeleton.Joints[end];
// どちらかが追跡状態でない場合には描画しない
if ( (startJoint.TrackingState == JointTrackingState.NotTracked) ||
(endJoint.TrackingState == JointTrackingState.NotTracked) ) {
return;
}
// 3次元座標を距離カメラの2次元座標に変換する
var startPoint = SkeletonToPiont( skeleton.Joints[start].Position );
var endPoint = SkeletonToPiont( skeleton.Joints[end].Position );
if ( !IsDrawablePoint( startPoint ) || !IsDrawablePoint( endPoint ) ) {
return;
}
// ジョイント間の線を引く
canvas.Children.Add( new Line()
{
X1 = startPoint.X,
Y1 = startPoint.Y,
X2 = endPoint.X,
Y2 = endPoint.Y,
Stroke = new SolidColorBrush( Colors.Red ),
} );
}
public void SetPosition(Joint joint)
{
if (!menuOpened)
{
if (Global.initPos)
{
//in Global check if z moving forward by some amount then increase 0.3 and 0.4 so hand moves less
Joint scaledJoint = joint.ScaleTo((int)Global.windowWidth, (int)Global.windowHeight, (float)0.25, (float)0.25);
cursorX = cursorX + 519;
cursorX = (double)Math.Max(0, Math.Min(scaledJoint.Position.X, Global.windowWidth - hand.Width));
cursorY = (double)Math.Max(0, Math.Min(scaledJoint.Position.Y, Global.windowHeight - hand.Height));
Canvas.SetLeft(this, cursorX);
Canvas.SetTop(this, cursorY);
}
else
{
Canvas.SetLeft(this, Global.windowWidth - hand.Width);
Canvas.SetTop(this, Global.windowHeight - hand.Height);
}
}
else
{
radialMenu.setCursorPosition(this, joint);
Canvas.SetLeft(this, cursorX);
Canvas.SetTop(this, cursorY);
}
}
/// <summary>
/// Gets the joint distance.
/// </summary>
/// <param name="firstJoint">The first joint.</param>
/// <param name="secondJoint">The second joint.</param>
/// <returns>retunr the distance</returns>
public static float GetJointDistance(Joint firstJoint, Joint secondJoint)
{
float distanceX = firstJoint.Position.X - secondJoint.Position.X;
float distanceY = firstJoint.Position.Y - secondJoint.Position.Y;
float distanceZ = firstJoint.Position.Z - secondJoint.Position.Z;
return (float)Math.Sqrt(Math.Pow(distanceX, 2) + Math.Pow(distanceY, 2) + Math.Pow(distanceZ, 2));
}
//--------------------------------------------------------------------------------------
// Implementation of a Holt Double Exponential Smoothing filter. The double exponential
// smooths the curve and predicts. There is also noise jitter removal. And maximum
// prediction bounds. The paramaters are commented in the init function.
//--------------------------------------------------------------------------------------
public void UpdateFilter(Body pBody)
{
if (pBody == null)
{
return;
}
// Check for divide by zero. Use an epsilon of a 10th of a millimeter
m_fJitterRadius = Math.Max(0.0001f, m_fJitterRadius);
TRANSFORM_SMOOTH_PARAMETERS SmoothingParams;
for (JointType jt = JointType.SpineBase; jt <= JointType.ThumbRight; jt++)
{
SmoothingParams.fSmoothing = m_fSmoothing;
SmoothingParams.fCorrection = m_fCorrection;
SmoothingParams.fPrediction = m_fPrediction;
SmoothingParams.fJitterRadius = m_fJitterRadius;
SmoothingParams.fMaxDeviationRadius = m_fMaxDeviationRadius;
// If inferred, we smooth a bit more by using a bigger jitter radius
Microsoft.Kinect.Joint joint = pBody.Joints[jt];
if (joint.TrackingState == TrackingState.Inferred)
{
SmoothingParams.fJitterRadius *= 2.0f;
SmoothingParams.fMaxDeviationRadius *= 2.0f;
}
UpdateJoint(pBody, jt, SmoothingParams);
}
}
/// <summary>
/// Get most matched action with given joint queue.
/// Calculating match by multiplying similarity and count of points checked.
/// </summary>
/// <param name="joint">Array of joint positions, smaller index means former position</param>
/// <param name="leaseCnt">Least amount of positions to be checked</param>
/// <param name="similarity">Similarity of matched action</param>
/// <returns>Most matched action type</returns>
public static ActionType MatchedAction(Joint[] joint, int leaseCnt, out float similarity)
{
float maxSimi = 0.0f;
ActionType maxType = ActionType.NONE;
//for (int i = leaseCnt; i < joint.Length; ++i)
//{
for (int j = 0; j < 8; ++j)
{
ActionType type = (ActionType)j;
float simi = Similarity(joint, leaseCnt, type);
if (simi > maxSimi)
{
maxSimi = simi;
maxType = type;
}
}
//}
similarity = maxSimi;
if (maxSimi > 0.25f)
{
return maxType;
}
else
{
return ActionType.NONE;
}
}
/// <summary>
/// Deserialize a skeleton
/// </summary>
/// <param name="reader">Binary reader</param>
/// <returns>Deserialized skeleton</returns>
private kinect.Skeleton DeserializeSkeleton(BinaryReader reader)
{
kinect.Skeleton skeleton = new kinect.Skeleton();
skeleton.ClippedEdges = (kinect.FrameEdges)reader.ReadInt32();
kinect.SkeletonPoint point = new kinect.SkeletonPoint();
point.X = reader.ReadSingle();
point.Y = reader.ReadSingle();
point.Z = reader.ReadSingle();
skeleton.Position = point;
skeleton.TrackingId = reader.ReadInt32();
skeleton.TrackingState = (kinect.SkeletonTrackingState)reader.ReadInt32();
int jointsCount = reader.ReadInt32();
for (int index = 0; index < jointsCount; index++)
{
kinect.JointType jointType = (kinect.JointType)reader.ReadInt32();
kinect.Joint joint = skeleton.Joints[jointType];
joint.TrackingState = (kinect.JointTrackingState)reader.ReadInt32();
point.X = reader.ReadSingle();
point.Y = reader.ReadSingle();
point.Z = reader.ReadSingle();
joint.Position = point;
skeleton.Joints[joint.JointType] = joint;
}
return(skeleton);
}
double CalculateAngle2D(Joint shoulder, Joint hip, Joint knee)
{
double angle = 0;
double value = CalcDiff();
value = 0.3 * value;
double shrhY = shoulder.Position.Y - (hip.Position.Y-value);
double shrhZ = shoulder.Position.Z - hip.Position.Z;
double hsl = vectorNorm( shrhY, shrhZ);
double unrhY = knee.Position.Y - (hip.Position.Y-value);
double unrhZ = knee.Position.Z - hip.Position.Z;
double hul = vectorNorm( unrhY, unrhZ);
double mhshu = shrhY * unrhY + shrhZ * unrhZ;
double x = mhshu / (hul * hsl);
if (x != Double.NaN)
{
if (-1 <= x && x <= 1)
{
double angleRad = Math.Acos(x);
angle = angleRad * (180.0 / 3.1416);
}
else
angle = 0;
}
else
angle = 0;
return angle;
}
/// <summary>
/// Calculates the angle between the provided joints in the YZ plane.
/// </summary>
/// <param name="joint1">The first joint.</param>
/// <param name="joint2">The second joint.</param>
/// <returns>The angle (in degrees) between the joints.</returns>
public static double CalculateAngleYZ(Joint joint1, Joint joint2)
{
double deltaY = joint2.Position.Y - joint1.Position.Y;
double deltaZ = joint2.Position.Z - joint1.Position.Z;
return Units.RadiansToDegrees(Math.Atan2(deltaZ, deltaY));
}
public static Joint alignJoints(Joint aligningJoint, Joint startingJoint, Joint endingJoint)
{
Vector3 pointToBeTranslated = new Vector3(startingJoint.Position.X, startingJoint.Position.Y, startingJoint.Position.Z);
Vector3 origin = new Vector3(aligningJoint.Position.X, aligningJoint.Position.Y, aligningJoint.Position.Z);
Vector3 differentialVector = pointToBeTranslated - origin;
//get the distance between the alingning joint and starting joint. We need to maintain this distance when we rotate the joint
double distance = Math.Sqrt(startingJoint.Position.X * aligningJoint.Position.X + startingJoint.Position.Y * aligningJoint.Position.Y + startingJoint.Position.Z * aligningJoint.Position.Z);
//get current vectors
Vector3 startingVector = new Vector3(startingJoint.Position.X - aligningJoint.Position.X, startingJoint.Position.Y - aligningJoint.Position.Y, startingJoint.Position.Z - aligningJoint.Position.Z);
Vector3 expectedVector = new Vector3(endingJoint.Position.X - aligningJoint.Position.X, endingJoint.Position.Y - aligningJoint.Position.Y, endingJoint.Position.Z - aligningJoint.Position.Z);
// Normalize vectors so we can use Dot product correctly
startingVector.Normalize();
expectedVector.Normalize();
float dotAngle = (float)Math.Acos(Vector3.Dot(startingVector, expectedVector));
if (float.IsNaN(dotAngle))
{
dotAngle = 0;
}
Vector3 rotatedVect = Vector3.Transform(differentialVector, Matrix.CreateFromAxisAngle(startingVector, dotAngle));
rotatedVect += origin;
SkeletonPoint newSkeletonPoint = new SkeletonPoint();
newSkeletonPoint.X = rotatedVect.X;
newSkeletonPoint.Y = rotatedVect.Y;
newSkeletonPoint.Z = rotatedVect.Z;
startingJoint.Position = newSkeletonPoint;
return startingJoint;
}
public void SetPosition(Joint joint)
{
Joint scaledJoint = joint.ScaleTo(1270 - 88, 750 - 38, 0.5f, 0.5f);
Canvas.SetLeft(this, scaledJoint.Position.X);
Canvas.SetTop(this, scaledJoint.Position.Y);
}
private ColorImagePoint ConverterCoordenadasArticulacao(Joint articulacao, double larguraCanvas, double alturaCanvas)
{
ColorImagePoint posicaoArticulacao = Kinect.CoordinateMapper.MapSkeletonPointToColorPoint(articulacao.Position, Kinect.ColorStream.Format);
posicaoArticulacao.X = (int)(posicaoArticulacao.X * larguraCanvas) / Kinect.ColorStream.FrameWidth;
posicaoArticulacao.Y = (int)(posicaoArticulacao.Y * alturaCanvas) / Kinect.ColorStream.FrameHeight;
return posicaoArticulacao;
}
double findAngle(Joint hombroIzquierdo, Joint hombroDerecho)
{
if (hombroDerecho.TrackingState == JointTrackingState.NotTracked && hombroIzquierdo.TrackingState == JointTrackingState.NotTracked)
{
return 0;
}
if (hombroDerecho.TrackingState == JointTrackingState.NotTracked ||
hombroDerecho.TrackingState == JointTrackingState.Inferred)
{
return 90;
}
else if (hombroIzquierdo.TrackingState == JointTrackingState.NotTracked ||
hombroIzquierdo.TrackingState == JointTrackingState.Inferred)
{
return -90;
}
else
{
return Math.Atan2(
hombroDerecho.Position.Z - hombroIzquierdo.Position.Z,
hombroDerecho.Position.X - hombroIzquierdo.Position.X) * 180.0 / Math.PI;
}
}
/// <summary>
/// Draws the bone.
/// </summary>
/// <param name="trackedJoint1">The tracked joint1.</param>
/// <param name="trackedJoint2">The tracked joint2.</param>
///
void drawBone(Joint trackedJoint1, Joint trackedJoint2)
{
Line bone = new Line();
bone.Stroke = Brushes.Red;
bone.StrokeThickness = 3;
Point joint1 = this.ScalePosition(trackedJoint1.Position);
bone.X1 = joint1.X;
bone.Y1 = joint1.Y;
Point mappedPoint1 = this.ScalePosition(trackedJoint1.Position);
Rectangle r = new Rectangle(); r.Height = 10; r.Width = 10;
r.Fill = Brushes.Red;
Canvas.SetLeft(r, mappedPoint1.X - 2);
Canvas.SetTop(r, mappedPoint1.Y - 2);
this.caller.myCanvas.Children.Add(r);
Point joint2 = this.ScalePosition(trackedJoint2.Position);
bone.X2 = joint2.X;
bone.Y2 = joint2.Y;
Point mappedPoint2 = this.ScalePosition(trackedJoint2.Position);
if (LeafJoint(trackedJoint2))
{
Rectangle r1 = new Rectangle(); r1.Height = 10; r1.Width = 10;
r1.Fill = Brushes.Red;
Canvas.SetLeft(r1, mappedPoint2.X - 2);
Canvas.SetTop(r1, mappedPoint2.Y - 2);
this.caller.myCanvas.Children.Add(r1);
Point mappedPoint = this.ScalePosition(trackedJoint2.Position);
}
this.caller.myCanvas.Children.Add(bone);
}
/// <summary>
/// Calculates the angle between the provided joints in the XY plane.
/// </summary>
/// <param name="joint1">The first joint.</param>
/// <param name="joint2">The second joint.</param>
/// <returns>The angle (in degrees) between the joints.</returns>
public static double CalculateAngleXY(Joint joint1, Joint joint2)
{
double deltaX = joint2.Position.X - joint1.Position.X;
double deltaY = joint2.Position.Y - joint1.Position.Y;
return Units.RadiansToDegrees(Math.Atan2(deltaY, deltaX));
}
private void DrawBone(Joint jointFrom, Joint jointTo, Pen aPen, DrawingContext aContext)
{
if (jointFrom.TrackingState == JointTrackingState.NotTracked ||
jointTo.TrackingState == JointTrackingState.NotTracked)
{
return;
}
if (jointFrom.TrackingState == JointTrackingState.Inferred ||
jointTo.TrackingState == JointTrackingState.Inferred)
{
ColorImagePoint p1 = mySensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointFrom.Position, ColorImageFormat.RgbResolution640x480Fps30);
ColorImagePoint p2 = mySensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointTo.Position, ColorImageFormat.RgbResolution640x480Fps30);
//Thin line
aPen.DashStyle = DashStyles.Dash;
aContext.DrawLine(aPen, new Point(p1.X, p1.Y), new Point(p2.X, p2.Y));
}
if (jointFrom.TrackingState == JointTrackingState.Tracked ||
jointTo.TrackingState == JointTrackingState.Tracked)
{
ColorImagePoint p1 = mySensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointFrom.Position, ColorImageFormat.RgbResolution640x480Fps30);
ColorImagePoint p2 = mySensor.CoordinateMapper.MapSkeletonPointToColorPoint(jointTo.Position, ColorImageFormat.RgbResolution640x480Fps30);
//Thick line
aPen.DashStyle = DashStyles.Solid;
aContext.DrawLine(aPen, new Point(p1.X, p1.Y), new Point(p2.X, p2.Y));
}
}
/// <summary>
/// Returns the length of the segment defined by the specified joints.
/// </summary>
/// <param name="p1">The first joint (start of the segment).</param>
/// <param name="p2">The second joint (end of the segment).</param>
/// <returns>The length of the segment in meters.</returns>
public static double Distance(Joint p1, Joint p2)
{
return Math.Sqrt(
Math.Pow(p1.Position.X - p2.Position.X, 2) +
Math.Pow(p1.Position.Y - p2.Position.Y, 2) +
Math.Pow(p1.Position.Z - p2.Position.Z, 2));
}
const float POSITIVEFARDISTANCE = 0.35f; //Distancia extra que debe alejarse la mano para considerar que esta en la zona positiva lejana
#endregion Fields
#region Methods
/*
* Función: Get1AxleValue
* Descripción: Función que devuelve el valor de movimiento en 1 eje
* Autor: Christian Vargas
* Fecha de creación: 19/08/15
* Fecha de modificación: --/--/--
* Entradas: mainHand (Joint, nodo de la mano principal del usuario), secondaryHand (Joint, nodo de la mano secundaria del usuario), mainShoulder (Joint, nodo del hombro principal del usuario), head (Joint, nodo de la cabeza el usuario)
* Salidas: (Tuple<bool, int>, valor que indica si el movimiento continua y el valor del eje)
*/
public static Tuple<bool, int> Get1AxleValue(Joint mainHand, Joint secondaryHand, Joint mainShoulder, Joint head)
{
int xMovement = 0;
bool moving = true;
if (secondaryHand.Position.Y >= head.Position.Y)
{
moving = false;
}
else
{
if (mainHand.Position.X >= mainShoulder.Position.X + POSITIVEDISTANCE + POSITIVEFARDISTANCE)
{
xMovement = 2;
}
else if (mainHand.Position.X >= mainShoulder.Position.X + POSITIVEDISTANCE)
{
xMovement = 1;
}
else if (mainHand.Position.X + NEGATIVEDISTANCE + NEGATIVEFARDISTANCE < mainShoulder.Position.X)
{
xMovement = -2;
}
else if (mainHand.Position.X + NEGATIVEDISTANCE < mainShoulder.Position.X)
{
xMovement = -1;
}
}
return Tuple.Create(moving, xMovement);
}
public void Test90DegreeAngle()
{
Joint vertex = new Joint();
Joint[] otherJoints = new Joint[2];
SkeletonPoint sp = new SkeletonPoint();
sp.X = 0.0F;
sp.Y = 0.0F;
sp.Z = 0.0F;
vertex.Position = sp;
Joint joint1 = new Joint();
SkeletonPoint point = new SkeletonPoint();
point.X = 0.0F;
point.Y = 1.0F;
point.Z = 0.0F;
joint1.Position = point;
Joint joint2 = new Joint();
SkeletonPoint point2 = new SkeletonPoint();
point2.X = 1.0F;
point2.Y = 0.0F;
point2.Z = 0.0F;
joint2.Position = point2;
otherJoints[0] = joint1;
otherJoints[1] = joint2;
Assert.AreEqual(90, JointAnalyzer.findAngle(vertex,otherJoints), "Angle should be 90 degrees");
}
void drawABone(Joint _a, Joint _b)
{
ColorSpacePoint pointA = mySensor.CoordinateMapper.MapCameraPointToColorSpace(_a.Position);
ColorSpacePoint pointB = mySensor.CoordinateMapper.MapCameraPointToColorSpace(_b.Position);
myBitmap.DrawLine((int)pointA.X - 1, (int)pointA.Y - 1, (int)pointB.X - 1, (int)pointB.Y - 1, Color.FromRgb(200, 0, 200));
myBitmap.DrawLine((int)pointA.X, (int)pointA.Y, (int)pointB.X, (int)pointB.Y, Color.FromRgb(200, 0, 200));
myBitmap.DrawLine((int)pointA.X+1, (int)pointA.Y+1, (int)pointB.X+1, (int)pointB.Y+1, Color.FromRgb(200, 0, 200));
}
// Transform the skeleton joints positions before drawing them
private Matrix getJointTransform(Joint joint)
{
// TODO: Return a world matrix to use for the joint.
//return Matrix.Identity;
// ANSWER
return Matrix.Scaling(joint.JointType == JointType.Head ? 0.2f : 0.04f) * Matrix.Translation(joint.Position.X, joint.Position.Y, -joint.Position.Z);
}
private static Vector4 CriarVetorEntreDoisPontos(Joint articulacao1, Joint articulacao2)
{
Vector4 vetorResultante = new Vector4();
vetorResultante.X = Convert.ToSingle(Math.Sqrt(Math.Pow( articulacao1.Position.X - articulacao2.Position.X, 2)));
vetorResultante.Y = Convert.ToSingle(Math.Sqrt(Math.Pow( articulacao1.Position.Y - articulacao2.Position.Y, 2)));
vetorResultante.Z = Convert.ToSingle(Math.Sqrt(Math.Pow( articulacao1.Position.Z - articulacao2.Position.Z, 2)));
return vetorResultante;
}
public bool SamePosition(Joint newFoot)
{
if (PositionMathX(newFoot) < calibrationX && PositionMathZ(newFoot) < calibrationZ)
{
return true;
}
return false;
}
private static Point GetJointPointHand(KinectSensor kinectDevice, Joint joint, Size containerSize, Point offset)
{
DepthImagePoint point = kinectDevice.MapSkeletonPointToDepth(joint.Position, kinectDevice.DepthStream.Format);
point.X = (int)((point.X * containerSize.Width / kinectDevice.DepthStream.FrameWidth) - offset.X);
point.Y = (int)((point.Y * containerSize.Height / kinectDevice.DepthStream.FrameHeight) - offset.Y);
return new Point(point.X, point.Y);
}
public void BuildJointMessageTest()
{
Joint joint = new Joint();
joint.JointType = JointType.HandLeft;
var msg = OscSerializer.BuildJointMessage(joint);
Assert.AreEqual("/kinect/joint/HandLeft", msg.Address);
}
private ScaledJoint CreateScaledJoint(Joint joint)
{
return new ScaledJoint()
{
JointType = joint.JointType,
LocationScreenPercent = ScaleSkeletonPoint(joint.Position)
};
}
public float jointDistance(Joint first, Joint second)
{
float dx = first.Position.X - second.Position.X;
float dy = first.Position.Y - second.Position.Y;
float dz = first.Position.Z - second.Position.Z;
return (float)Math.Sqrt((dx * dx) + (dy * dy) + (dz * dz));
}
public static OscMessage BuildVectorMessage(Joint joint1, Joint joint2)
{
var address = String.Format(Constants.OscVectorAddr, joint1.JointType, joint2.JointType);
var xDist = joint2.Position.X - joint1.Position.X;
var yDist = joint2.Position.Y - joint1.Position.Y;
var zDist = joint2.Position.Z - joint1.Position.Z;
return new OscMessage(address, xDist , yDist , zDist );
}
// Metod för att räkna ut avståndet mellan två joints.
public float GetJointDistance(Joint rightJoint, Joint leftJoint)
{
float xDistance = rightJoint.Position.X - leftJoint.Position.X;
float yDistance = rightJoint.Position.Y - leftJoint.Position.Y;
float zDistance = rightJoint.Position.Z - leftJoint.Position.Z;
return (float)Math.Sqrt(Math.Pow(xDistance, 2) + Math.Pow(yDistance, 2) + Math.Pow(zDistance, 2));
}
void UpdateJoint(Body body, JointType jt, TRANSFORM_SMOOTH_PARAMETERS smoothingParams)
{
CameraSpacePoint vPrevRawPosition;
CameraSpacePoint vPrevFilteredPosition;
CameraSpacePoint vPrevTrend;
CameraSpacePoint vRawPosition;
CameraSpacePoint vFilteredPosition;
CameraSpacePoint vPredictedPosition;
CameraSpacePoint vDiff;
CameraSpacePoint vTrend;
float fDiff;
bool bJointIsValid;
Microsoft.Kinect.Joint joint = body.Joints[jt];
vRawPosition = joint.Position;
vPrevFilteredPosition = m_pHistory[(int)jt].m_vFilteredPosition;
vPrevTrend = m_pHistory[(int)jt].m_vTrend;
vPrevRawPosition = m_pHistory[(int)jt].m_vRawPosition;
bJointIsValid = JointPositionIsValid(vRawPosition);
// If joint is invalid, reset the filter
if (!bJointIsValid)
{
m_pHistory[(int)jt].m_dwFrameCount = 0;
}
// Initial start values
if (m_pHistory[(int)jt].m_dwFrameCount == 0)
{
vFilteredPosition = vRawPosition;
vTrend = CSVectorZero();
m_pHistory[(int)jt].m_dwFrameCount++;
}
else if (m_pHistory[(int)jt].m_dwFrameCount == 1)
{
vFilteredPosition = CSVectorScale(CSVectorAdd(vRawPosition, vPrevRawPosition), 0.5f);
vDiff = CSVectorSubtract(vFilteredPosition, vPrevFilteredPosition);
vTrend = CSVectorAdd(CSVectorScale(vDiff, smoothingParams.fCorrection), CSVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection));
m_pHistory[(int)jt].m_dwFrameCount++;
}
else
{
// First apply jitter filter
vDiff = CSVectorSubtract(vRawPosition, vPrevFilteredPosition);
fDiff = CSVectorLength(vDiff);
if (fDiff <= smoothingParams.fJitterRadius)
{
vFilteredPosition = CSVectorAdd(CSVectorScale(vRawPosition, fDiff / smoothingParams.fJitterRadius),
CSVectorScale(vPrevFilteredPosition, 1.0f - fDiff / smoothingParams.fJitterRadius));
}
else
{
vFilteredPosition = vRawPosition;
}
// Now the double exponential smoothing filter
vFilteredPosition = CSVectorAdd(CSVectorScale(vFilteredPosition, 1.0f - smoothingParams.fSmoothing),
CSVectorScale(CSVectorAdd(vPrevFilteredPosition, vPrevTrend), smoothingParams.fSmoothing));
vDiff = CSVectorSubtract(vFilteredPosition, vPrevFilteredPosition);
vTrend = CSVectorAdd(CSVectorScale(vDiff, smoothingParams.fCorrection), CSVectorScale(vPrevTrend, 1.0f - smoothingParams.fCorrection));
}
// Predict into the future to reduce latency
vPredictedPosition = CSVectorAdd(vFilteredPosition, CSVectorScale(vTrend, smoothingParams.fPrediction));
// Check that we are not too far away from raw data
vDiff = CSVectorSubtract(vPredictedPosition, vRawPosition);
fDiff = CSVectorLength(vDiff);
if (fDiff > smoothingParams.fMaxDeviationRadius)
{
vPredictedPosition = CSVectorAdd(CSVectorScale(vPredictedPosition, smoothingParams.fMaxDeviationRadius / fDiff),
CSVectorScale(vRawPosition, 1.0f - smoothingParams.fMaxDeviationRadius / fDiff));
}
// Save the data from this frame
m_pHistory[(int)jt].m_vRawPosition = vRawPosition;
m_pHistory[(int)jt].m_vFilteredPosition = vFilteredPosition;
m_pHistory[(int)jt].m_vTrend = vTrend;
// Output the data
m_pFilteredJoints[(int)jt] = vPredictedPosition;
}
void OnUpdate(object sender, AllFramesReadyEventArgs e)
{
if (Application.Current.MainWindow == null || MainWindow.Instance == null)
{
return;
}
MainWindow.Instance.NuiRuntime_VideoFrameReady(this, e);
Nui.Skeleton skeleton = null;
PreviousCursorPosition = CursorPosition;
if (e == null)
{
CursorPosition = Mouse.GetPosition(Application.Current.MainWindow);
}
else
{
using (SkeletonFrame skeletonFrameData = e.OpenSkeletonFrame())
{
if (skeletonFrameData != null)
{
Skeleton[] allSkeletons = new Skeleton[skeletonFrameData.SkeletonArrayLength];
skeletonFrameData.CopySkeletonDataTo(allSkeletons);
foreach (Skeleton sd in allSkeletons)
{
if (sd.TrackingState == Nui.SkeletonTrackingState.Tracked)
{
skeleton = sd;
break;
}
}
if (skeleton == null)
{
return;
}
var nuiv = skeleton.Joints[JointType.HandRight].ScaleTo((int)ActualWidth, (int)ActualHeight, 0.50f, 0.30f).Position;
CursorPosition = new Point(nuiv.X, nuiv.Y);
}
}
}
if (!Passive)
{
UpdateElementOver();
}
if (MainWindow.Instance.sensor == null)
{
if (_isPainting)
{
if (Mouse.LeftButton == MouseButtonState.Released)
{
MainWindow.Instance.StopPainting();
}
}
else
{
if (Mouse.LeftButton == MouseButtonState.Pressed)
{
MainWindow.Instance.StartPainting();
}
}
_isPainting = Mouse.LeftButton == MouseButtonState.Pressed;
}
else
{
if (skeleton == null)
{
return;
}
Nui.Joint lh = skeleton.Joints[Nui.JointType.HandLeft];
Nui.Joint ls = skeleton.Joints[Nui.JointType.ElbowLeft];
bool isup = lh.Position.Y > ls.Position.Y;
/* 무조건 그리게*/
// isup = true;
if (isup != _isPainting)
{
_isPainting = isup;
if (_isPainting)
{
MainWindow.Instance.StartPainting();
}
else
{
MainWindow.Instance.StopPainting();
}
}
}
}
void OnUpdate(object sender, AllFramesReadyEventArgs e)
{
// KINECT Add code to get joint data and smooth it
if (Application.Current.MainWindow == null || Container.Instance == null)
{
return;
}
Nui.Skeleton skeleton = null;
PreviousCursorPosition = CursorPosition;
if (e == null)
{
CursorPosition = Mouse.GetPosition(Application.Current.MainWindow);
}
else
{
using (SkeletonFrame skeletonFrameData = e.OpenSkeletonFrame())
{
if (skeletonFrameData != null)
{
Skeleton[] allSkeletons = new Skeleton[skeletonFrameData.SkeletonArrayLength];
skeletonFrameData.CopySkeletonDataTo(allSkeletons);
foreach (Skeleton sd in allSkeletons)
{
if (sd.TrackingState == Nui.SkeletonTrackingState.Tracked)
{
skeleton = sd;
break;
}
}
if (skeleton == null)
{
return;
}
var nuiv = skeleton.Joints[JointType.HandRight].ScaleTo((int)ActualWidth, (int)ActualHeight, 0.50f, 0.30f).Position;
CursorPosition = new Point(nuiv.X, nuiv.Y);
}
}
}
// Update which graphical element the cursor is currently over
if (!Passive)
{
UpdateElementOver();
}
if (Container.Instance.sensor == null)
{
// For mouse, see if the right mouse button is down.
if (_isPainting)
{
if (Mouse.LeftButton == MouseButtonState.Released)
{
MainWindow.Instance.StopPainting();
}
}
else
{
if (Mouse.LeftButton == MouseButtonState.Pressed)
{
MainWindow.Instance.StartPainting();
}
}
_isPainting = Mouse.LeftButton == MouseButtonState.Pressed;
}
else
{
if (skeleton == null)
{
return;
}
// To begin painting w/ Kinect, raise your left hand above your shoulder.
// To stop painting, lower it.
Nui.Joint lh = skeleton.Joints[Nui.JointType.HandLeft];
Nui.Joint ls = skeleton.Joints[Nui.JointType.ShoulderLeft];
bool isup = lh.Position.Y > ls.Position.Y;
if (isup != _isPainting)
{
_isPainting = isup;
if (_isPainting)
{
MainWindow.Instance.StartPainting();
}
else
{
MainWindow.Instance.StopPainting();
}
}
}
}
