private static void RenderClippedEdges(Skeleton skeleton, DrawingContext drawingContext)
{
if (skeleton.ClippedEdges.HasFlag(FrameEdges.Bottom))
{
drawingContext.DrawRectangle(
Brushes.Red,
null,
new Rect(0, RenderHeight - ClipBoundsThickness, RenderWidth, ClipBoundsThickness));
}
if (skeleton.ClippedEdges.HasFlag(FrameEdges.Top))
{
drawingContext.DrawRectangle(
Brushes.Red,
null,
new Rect(0, 0, RenderWidth, ClipBoundsThickness));
}
if (skeleton.ClippedEdges.HasFlag(FrameEdges.Left))
{
drawingContext.DrawRectangle(
Brushes.Red,
null,
new Rect(0, 0, ClipBoundsThickness, RenderHeight));
}
if (skeleton.ClippedEdges.HasFlag(FrameEdges.Right))
{
drawingContext.DrawRectangle(
Brushes.Red,
null,
new Rect(RenderWidth - ClipBoundsThickness, 0, ClipBoundsThickness, RenderHeight));
}
}
protected override bool PosicaoValida(Microsoft.Kinect.Skeleton esqueletoUsuario)
{
const double ANGULO_ESPERADO = 25;
double margemErroPosicao = 0.30;
double margemErroAngulo = 10;
Joint quadrilEsquerdo = esqueletoUsuario.Joints[JointType.HipLeft];
Joint ombroEsquerdo = esqueletoUsuario.Joints[JointType.ShoulderLeft];
Joint maoEsquerda = esqueletoUsuario.Joints[JointType.HandLeft];
Joint cotoveloEsquerdo = esqueletoUsuario.Joints[JointType.ElbowLeft];
double resultadoAngulo = Util.CalcularProdutoEscalar(quadrilEsquerdo, ombroEsquerdo, maoEsquerda);
bool anguloCorreto = Util.CompararComMargemErro(margemErroAngulo, resultadoAngulo, ANGULO_ESPERADO);
bool maoEsquerdaDistanciaCorreta = Util.CompararComMargemErro(margemErroPosicao, maoEsquerda.Position.Z, quadrilEsquerdo.Position.Z);
bool maoEsquerdaAposCotovelo = maoEsquerda.Position.X < cotoveloEsquerdo.Position.X;
bool maoEsquerdaAbaixoCotovelo = maoEsquerda.Position.Y < cotoveloEsquerdo.Position.Y;
return(anguloCorreto &&
maoEsquerdaDistanciaCorreta &&
maoEsquerdaAposCotovelo &&
maoEsquerdaAbaixoCotovelo);
}
/// <summary>
/// Dequeues from the queue if available and accepting
/// </summary>
/// <param name="skeleton">Skeletal array data from the SkeletonFrame</param>
public void Add(Skeleton[] skeleton, long timeStamp)
{
// Is there any space left?
if (free.Count == 0)
{
for (int i = 0; i < maxInstances; ++i)
{
// enqueue again so long as no one else is using this
if (!this.universe[i].InUse)
{
free.Enqueue(this.universe[i]);
}
}
}
if (free.Count > 0)
{
SkeletonStamp s = free.Dequeue();
s.TimeStamp = timeStamp;
s.SkeletonData = skeleton;
s.InUse = false;
s.IsActive = true;
//Debug.WriteLine("Adding {0}", free.Count);
}
else
{
Debug.WriteLine("Out of skeletons! Consider increasing the max instances on initialization.");
}
}
/// <summary>
/// Updates all gestures.
/// </summary>
/// <param name="data">The skeleton data.</param>
public void UpdateAllGestures(Skeleton data)
{
foreach (Gesture gesture in this.gestures)
{
gesture.UpdateGesture(data);
}
}
protected override bool IsGestureValid(Skeleton skeleton)
{
SkeletonPoint newLeft = skeleton.Joints[JointType.HandRight].Position;
SkeletonPoint newRight = skeleton.Joints[JointType.HandRight].Position;
if ((originLeft != newLeft) && (originRight != newRight)) return false;
return true;
}
/// <summary>
/// fill the queues gathering the hand positions
/// </summary>
/// <param name="skeleton">the actual skeleton</param>
private void fillHandQueues(Microsoft.Kinect.Skeleton skeleton)
{
Joint handJoint;
Queue <Point3D> q;
foreach (HandType handType in Enum.GetValues(typeof(HandType)))
{
//Only LEFT and RIGHT
if (handType != HandType.NULL)
{
handJoint = KinectHelper.Instance.GetHandJoint(handType, skeleton);
if (handType == HandType.LEFT)
{
q = this.leftHandQueue;
q.Enqueue(new Point3D(handJoint.Position.X, handJoint.Position.Y, handJoint.Position.Z));
if (q.Count > this.movementQueueCount)
{
q.Dequeue();
}
handDetector.leftHandQueue = q;
}
else
{
q = this.rightHandQueue;
q.Enqueue(new Point3D(handJoint.Position.X, handJoint.Position.Y, handJoint.Position.Z));
if (q.Count > this.movementQueueCount)
{
q.Dequeue();
}
handDetector.rightHandQueue = q;
}
}
}
}
public void ResetAll(Skeleton skeleton)
{
foreach (var state in _gesturestate)
{
state.Reset();
}
}
static void Sensor_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
using (var frame = e.OpenSkeletonFrame())
{
if (frame != null)
{
Microsoft.Kinect.Skeleton[] skeletons = new Microsoft.Kinect.Skeleton[frame.SkeletonArrayLength];
frame.CopySkeletonDataTo(skeletons);
if (skeletons.Length > 0)
{
var user = skeletons.Where(u => u.TrackingState == SkeletonTrackingState.Tracked).FirstOrDefault();
if (user != null)
{
if (!_skeleton)
{
Console.WriteLine("Skeleton detected");
}
_skeleton = true;
_mainGesture.Update(user);
}
else
{
if (_skeleton)
{
Console.WriteLine("No Skeleton detected");
}
_skeleton = false;
}
}
}
}
}
/// <summary>
/// Checks if the given skeleton's tracking data matches
/// the gesture represented by this frame.
/// </summary>
/// <param name="skeleton">Skeleton to analize</param>
/// <returns>
/// Success if the gesture was correct, Waiting if the
/// gesture was not quite right, but still possible, Fail otherwise.
/// </returns>
public KinectGestureResult ProcessFrame(Skeleton skeleton)
{
// Checks if right hand right of the right shoulder.
if (skeleton.Joints[JointType.HandRight].Position.X > skeleton.Joints[JointType.ShoulderRight].Position.X)
{
// Checks if right hand is above the right elbow.
if (skeleton.Joints[JointType.HandRight].Position.Y > skeleton.Joints[JointType.ElbowRight].Position.Y)
{
// Checks if the right hand is above the head.
if (skeleton.Joints[JointType.HandRight].Position.Y > skeleton.Joints[JointType.Head].Position.Y)
{
// The third part of the gesture was completed.
return KinectGestureResult.Success;
}
else
{
// Will pause recognition and try later.
return KinectGestureResult.Waiting;
}
}
// The standard result is failure.
return KinectGestureResult.Fail;
}
// The standard result is failure.
return KinectGestureResult.Fail;
}
public override Enums.GesturePartResult checkGesture(Microsoft.Kinect.Skeleton skeleton)
{
if (handType != HandType.NULL)
{
if (handType == HandType.LEFT)
{
if (skeleton.Joints[JointType.HandLeft].Position.X - skeleton.Joints[JointType.HipLeft].Position.X < -0.1)
{
return(Enums.GesturePartResult.SUCCESS);
}
else
{
return(Enums.GesturePartResult.FAIL);
}
}
else if (handType == HandType.RIGHT)
{
if (skeleton.Joints[JointType.HandRight].Position.X - skeleton.Joints[JointType.HipLeft].Position.X < -0.1)
{
return(Enums.GesturePartResult.PAUSING);
}
else
{
return(Enums.GesturePartResult.FAIL);
}
}
}
return(GesturePartResult.FAIL);
}
static void Timer_Elapsed(object sender, ElapsedEventArgs e)
{
var passive = 0;
var active = 0;
foreach(var k in KinectSensor.KinectSensors)
{
using(var frame = k.SkeletonStream.OpenNextFrame(50))
{
if(frame != null)
{
var s = new Skeleton[frame.SkeletonArrayLength];
frame.CopySkeletonDataTo(s);
foreach (var skeleton in s)
{
if (skeleton.TrackingState == SkeletonTrackingState.Tracked)
active++;
if (skeleton.TrackingState == SkeletonTrackingState.PositionOnly)
passive++;
}
}
}
Console.WriteLine(logic.Process(active, passive));
}
}
public GesturePartResult CheckGesture(Skeleton skeleton)
{
double LeftA = Math.Sqrt((Math.Pow(skeleton.Joints[JointType.KneeLeft].Position.Z - skeleton.Joints[JointType.FootLeft].Position.Z, 2) + Math.Pow(skeleton.Joints[JointType.KneeLeft].Position.Y - skeleton.Joints[JointType.FootLeft].Position.Y, 2)));
double LeftB = Math.Sqrt((Math.Pow(skeleton.Joints[JointType.HipRight].Position.Z - skeleton.Joints[JointType.KneeLeft].Position.Z, 2) + Math.Pow(skeleton.Joints[JointType.HipLeft].Position.Y - skeleton.Joints[JointType.KneeLeft].Position.Y, 2)));
double LeftC = skeleton.Joints[JointType.HipLeft].Position.Y - skeleton.Joints[JointType.FootLeft].Position.Y;
double RightA = Math.Sqrt((Math.Pow(skeleton.Joints[JointType.KneeRight].Position.Z - skeleton.Joints[JointType.FootRight].Position.Z, 2) + Math.Pow(skeleton.Joints[JointType.KneeRight].Position.Y - skeleton.Joints[JointType.FootRight].Position.Y, 2)));
double RightB = Math.Sqrt((Math.Pow(skeleton.Joints[JointType.HipRight].Position.Z - skeleton.Joints[JointType.KneeRight].Position.Z, 2) + Math.Pow(skeleton.Joints[JointType.HipRight].Position.Y - skeleton.Joints[JointType.KneeRight].Position.Y, 2)));
double RightC = skeleton.Joints[JointType.HipRight].Position.Y - skeleton.Joints[JointType.FootRight].Position.Y;
double LeftAngle = Math.Acos((Math.Pow(LeftA, 2) + Math.Pow(LeftB, 2) - Math.Pow(LeftC, 2)) / (2 * LeftA * LeftB));
double RightAngle = Math.Acos((Math.Pow(RightA, 2) + Math.Pow(RightB, 2) - Math.Pow(RightC, 2)) / (2 * RightA * RightB));
LeftAngle = (LeftAngle * 180) / Math.PI;
RightAngle = (RightAngle * 180) / Math.PI;
if (LeftAngle > 160 && RightAngle > 160)
{
NewHipCenterAverage = (skeleton.Joints[JointType.HipLeft].Position.Y + skeleton.Joints[JointType.HipCenter].Position.Y + skeleton.Joints[JointType.HipRight].Position.Y) / 3;
if(NewHipCenterAverage - BendSegment1.HipCenterAverage < 0.04)
{
return GesturePartResult.Succeed;
}
return GesturePartResult.Fail;
}
else
{
return GesturePartResult.Pausing;
}
}
public GesturePartResult CheckGesture(Skeleton skeleton)
{
if (skeleton != null)
{
// Right and Left Hand in front of Shoulders
if (skeleton.Joints[JointType.HandLeft].Position.Z < skeleton.Joints[JointType.ElbowLeft].Position.Z && skeleton.Joints[JointType.HandRight].Position.Z < skeleton.Joints[JointType.ElbowRight].Position.Z)
{
//Debug.WriteLine("Zoom 0 - Right hand in front of right shoudler - PASS");
// Hands between shoulder and hip
if (skeleton.Joints[JointType.HandRight].Position.Y < skeleton.Joints[JointType.ShoulderCenter].Position.Y && skeleton.Joints[JointType.HandRight].Position.Y > skeleton.Joints[JointType.HipCenter].Position.Y &&
skeleton.Joints[JointType.HandLeft].Position.Y < skeleton.Joints[JointType.ShoulderCenter].Position.Y && skeleton.Joints[JointType.HandLeft].Position.Y > skeleton.Joints[JointType.HipCenter].Position.Y)
{
// Hands between shoulders
if (skeleton.Joints[JointType.HandRight].Position.X < skeleton.Joints[JointType.ShoulderRight].Position.X && skeleton.Joints[JointType.HandRight].Position.X > skeleton.Joints[JointType.ShoulderLeft].Position.X &&
skeleton.Joints[JointType.HandLeft].Position.X > skeleton.Joints[JointType.ShoulderLeft].Position.X && skeleton.Joints[JointType.HandLeft].Position.X < skeleton.Joints[JointType.ShoulderRight].Position.X)
{
return GesturePartResult.Succeed;
}
return GesturePartResult.Pausing;
}
return GesturePartResult.Fail;
}
}
return GesturePartResult.Fail;
}
public Boolean rightArmUp(Skeleton skeleton)
{
if (skeleton == null)
{
return false;
}
Joint elbowRight = skeleton.Joints[JointType.ElbowRight];
Joint shoulderRight = skeleton.Joints[JointType.ShoulderRight];
Joint handRight = skeleton.Joints[JointType.HandRight];
double elbowRightX = elbowRight.Position.X;
double shoulderRightX = shoulderRight.Position.X;
double handRightX = handRight.Position.X;
double deltaX = Math.Max(Math.Abs(elbowRightX - shoulderRightX), Math.Abs(elbowRightX - handRightX));
//Console.WriteLine("deltaX: " + deltaX);
if (deltaX < 0.075)
{
return true;
}
else
{
return false;
}
}
public void AppendSkeleton(Skeleton skeleton)
{
SkeletonPoint head = skeleton.Joints[JointType.Head].Position;
SkeletonPoint shoulderCenter = skeleton.Joints[JointType.ShoulderCenter].Position;
SkeletonPoint shoulderLeft = skeleton.Joints[JointType.ShoulderLeft].Position;
SkeletonPoint shoulderRight = skeleton.Joints[JointType.ShoulderRight].Position;
SkeletonPoint elbowLeft = skeleton.Joints[JointType.ElbowLeft].Position;
SkeletonPoint wristLeft = skeleton.Joints[JointType.WristLeft].Position;
SkeletonPoint handLeft = skeleton.Joints[JointType.HandLeft].Position;
SkeletonPoint elbowRight = skeleton.Joints[JointType.ElbowRight].Position;
SkeletonPoint wristRight = skeleton.Joints[JointType.WristRight].Position;
SkeletonPoint handRight = skeleton.Joints[JointType.HandRight].Position;
StringBuilder sb = new StringBuilder();
sb.Append(DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss")+", ");
sb.Append(head.X.ToString() + "," + head.Y.ToString() + "," + head.Z.ToString() + ",");
sb.Append(shoulderCenter.X + "," + shoulderCenter.Y + "," + shoulderCenter.Z + ",");
sb.Append(shoulderLeft.X + "," + shoulderLeft.Y + "," + shoulderLeft.Z + ",");
sb.Append(shoulderRight.X + "," + shoulderRight.Y + "," + shoulderRight.Z + ",");
sb.Append(elbowLeft.X + "," + elbowLeft.Y + "," + elbowLeft.Z + ",");
sb.Append(wristLeft.X + "," + wristLeft.Y + "," + wristLeft.Z + ",");
sb.Append(handLeft.X + "," + handLeft.Y + "," + handLeft.Z + ",");
sb.Append(elbowRight.X + "," + elbowRight.Y + "," + elbowRight.Z + ",");
sb.Append(wristRight.X + "," + wristRight.Y + "," + wristRight.Z + ",");
sb.Append(handRight.X + "," + handRight.Y + "," + handRight.Z);
AppendSkeletonString(sb.ToString());
}
/// <summary>
/// Checks if the given skeleton's tracking data matches
/// the gesture represented by this frame.
/// </summary>
/// <param name="skeleton">Skeleton to analize</param>
/// <returns>
/// Success if the gesture was correct, Waiting if the
/// gesture was not quite right, but still possible, Fail otherwise.
/// </returns>
public KinectGestureResult ProcessFrame(Skeleton skeleton)
{
// Checks if right hand is down.
if (skeleton.Joints[JointType.HandRight].Position.Y < skeleton.Joints[JointType.ElbowRight].Position.Y)
{
// Checks if the left hand is right of the left elbow
if (skeleton.Joints[JointType.HandLeft].Position.X > skeleton.Joints[JointType.ElbowLeft].Position.X)
{
// Checks if the right hand is above the right elbow
if (skeleton.Joints[JointType.HandLeft].Position.Y > skeleton.Joints[JointType.ElbowLeft].Position.Y)
{
// The fourth part of the gesture was completed.
return KinectGestureResult.Success;
}
else
{
// Gesture recognition will pause.
return KinectGestureResult.Waiting;
}
}
}
// The standard result is failure.
return KinectGestureResult.Fail;
}
public KinectHelper()
{
Skeletons=new Skeleton[0];
NearestId = -1;
InitializeNui();
KinectSensor.KinectSensors.StatusChanged += (s, ee) =>
{
switch (ee.Status)
{
case KinectStatus.Connected:
if (nui == null)
{
InitializeNui();
}
break;
default:
if (ee.Sensor == nui)
{
UninitializeNui();
}
break;
}
};
}
void kinect_AllFramesReady( object sender, AllFramesReadyEventArgs e )
{
using ( var colorFrame = e.OpenColorImageFrame() ) {
if ( colorFrame != null ) {
var pixel = new byte[colorFrame.PixelDataLength];
colorFrame.CopyPixelDataTo( pixel );
ImageRgb.Source = BitmapSource.Create( colorFrame.Width, colorFrame.Height, 96, 96,
PixelFormats.Bgr32, null, pixel, colorFrame.Width * 4 );
}
}
using ( var depthFrame = e.OpenDepthImageFrame() ) {
if ( depthFrame != null ) {
// Depth情報を入れる
// GetRawPixelData()はインタラクションライブラリ内で実装された拡張メソッド
stream.ProcessDepth( depthFrame.GetRawPixelData(), depthFrame.Timestamp );
}
}
using ( var skeletonFrame = e.OpenSkeletonFrame() ) {
if ( skeletonFrame != null ) {
var skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo( skeletons );
// スケルトン情報を入れる
stream.ProcessSkeleton( skeletons, kinect.AccelerometerGetCurrentReading(), skeletonFrame.Timestamp );
}
}
}
protected override void onUpdate(GameTime gameTime)
{
this.skeleton = this.kinect.skeleton;
if (this.skeleton != null)
{
if (this.posture != null)
{
this.posture.joints = Posture.castSkeletonToJoints(this.skeleton);
}
else
{
this.posture = new Posture(this.skeleton);
}
}
// IMPORTANT:
// FIX: ARREGLAR ESTO PARA QUE CALCULE LOS ERRORES Y LA DIFICULTAD LA DE DESDE UNA CAPA MAS ARRIBA...
if (postureToCompare != null && this.posture != null)
{
this.posture.compareTo(this.postureToCompare, ref accuracy, 0.07f, 0.058f);
}
else
if (this.kinect.skeleton != null)
{
accuracy = new double[20] {
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0
}
}
;
}
/// <summary>
/// Prueft die history, ob die jeweilige Bewegung ausgelöst wird oder nicht.
/// </summary>
/// <param name="history">The history.</param>
/// <returns></returns>
public ErkennerStatus Pruefe(Skeleton[] history)
{
var headY = history.Select(x => x.Joints[JointType.Head].Position.Y);
var leftFootY = history.Select(x => x.Joints[JointType.FootLeft].Position.Y);
bool unten = (headY.Max() - headY.First() > 0.15) && (leftFootY.First() - leftFootY.Min()) < 0.02;
bool oben = (headY.First() - headY.Min() > 0.1) && (leftFootY.Max()-leftFootY.First()) < 0.02;
if (Blocked)
{
if (BlockStopwatch.ElapsedMilliseconds > 700)
{
Blocked = false;
BlockStopwatch = null;
}
}
if (!Blocked)
{
if (_geduckt && oben)
{
_geduckt = false;
MotionFunctions.SendAction(MotionFunctions.DownUp());
return ErkennerStatus.NichtAktiv;
}
if (!_geduckt && unten)
{
_geduckt = true;
MotionFunctions.SendAction(MotionFunctions.DownDown());
return ErkennerStatus.Aktiv;
}
}
return _geduckt ? ErkennerStatus.Aktiv : ErkennerStatus.NichtAktiv;
}
void sensor_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame == null)
{
return;
}
Skeleton[] totalSkeleton = new Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(totalSkeleton);
// 如果不用 LINQ表达式应该是怎么写？
Skeleton firstSkeleton = (from trackskeleton in totalSkeleton
where trackskeleton.TrackingState == SkeletonTrackingState.
Tracked
select trackskeleton).FirstOrDefault();
if (firstSkeleton == null)
{
return;
}
if (firstSkeleton.Joints[JointType.HandRight].TrackingState == JointTrackingState.Tracked)
{
this.MapJointsWithUIElement(firstSkeleton);
}
}
}
/// <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);
}
public virtual bool CheckForGesture(Skeleton skeleton)
{
if (this.IsRecognitionStarted == false)
{
if (this.ValidateGestureStartCondition(skeleton))
{
this.IsRecognitionStarted = true;
this.CurrentFrameCount = 0;
}
}
else
{
if (this.CurrentFrameCount == this.MaximumNumberOfFrameToProcess)
{
this.IsRecognitionStarted = false;
if (ValidateBaseCondition(skeleton) && ValidateGestureEndCondition(skeleton))
{
return true;
}
}
this.CurrentFrameCount++;
if (!IsGestureValid(skeleton) && !ValidateBaseCondition(skeleton))
{
this.IsRecognitionStarted = false;
}
}
return false;
}
/// <summary>
/// Checks the gesture.
/// </summary>
/// <param name="skeleton">The skeleton.</param>
/// <returns>GesturePartResult based on if the gesture part has been completed</returns>
public GesturePartResult CheckGesture(Skeleton skeleton)
{
// //left hand in front of left Shoulder
if (skeleton.Joints[JointType.HandLeft].Position.Z < skeleton.Joints[JointType.ElbowLeft].Position.Z && skeleton.Joints[JointType.HandRight].Position.Y < skeleton.Joints[JointType.HipCenter].Position.Y)
{
// Debug.WriteLine("GesturePart 1 - left hand in front of left Shoulder - PASS");
// /left hand below shoulder height but above hip height
if (skeleton.Joints[JointType.HandLeft].Position.Y < skeleton.Joints[JointType.Head].Position.Y && skeleton.Joints[JointType.HandLeft].Position.Y > skeleton.Joints[JointType.HipCenter].Position.Y)
{
// Debug.WriteLine("GesturePart 1 - left hand below shoulder height but above hip height - PASS");
// //left hand left of left Shoulder
if (skeleton.Joints[JointType.HandLeft].Position.X < skeleton.Joints[JointType.ShoulderRight].Position.X && skeleton.Joints[JointType.HandLeft].Position.X > skeleton.Joints[JointType.ShoulderLeft].Position.X)
{
// Debug.WriteLine("GesturePart 1 - left hand left of left Shoulder - PASS");
return GesturePartResult.Suceed;
}
// Debug.WriteLine("GesturePart 1 - left hand left of left Shoulder - UNDETERMINED");
return GesturePartResult.Pausing;
}
// Debug.WriteLine("GesturePart 1 - left hand below shoulder height but above hip height - FAIL");
return GesturePartResult.Fail;
}
// Debug.WriteLine("GesturePart 1 - left hand in front of left Shoulder - FAIL");
return GesturePartResult.Fail;
}
/// <summary>
/// Checks the gesture.
/// </summary>
/// <param name="skeleton">The skeleton.</param>
/// <returns>GesturePartResult based on if the gesture part has been completed</returns>
public GesturePartResult CheckGesture(Skeleton skeleton)
{
if (skeleton.Joints[JointType.HandLeft].Position.Y > skeleton.Joints[JointType.ShoulderCenter].Position.Y)
{
// //Right hand in front of right shoulder
if (skeleton.Joints[JointType.HandRight].Position.Z < skeleton.Joints[JointType.ShoulderRight].Position.Z)
{
// right hand above head
if (skeleton.Joints[JointType.HandRight].Position.Y > skeleton.Joints[JointType.Head].Position.Y)
{
// right hand right of right shoulder
if (skeleton.Joints[JointType.HandRight].Position.X > skeleton.Joints[JointType.HipLeft].Position.X)
{
return GesturePartResult.Succeed;
}
return GesturePartResult.Pausing;
}
// Debug.WriteLine("GesturePart 2 - right hand below shoulder height but above hip height - FAIL");
return GesturePartResult.Fail;
}
// Debug.WriteLine("GesturePart 2 - Right hand in front of right Shoulder - FAIL");
return GesturePartResult.Fail;
}
return GesturePartResult.Fail;
}
/// <summary>
/// launch the update on all gestures relying on skeleton datas
/// </summary>
/// <param name="skel">the skeleton datas</param>
/// <param name="gesture_context">the context when the gesture is triggered</param>
public void UpdateAllGestures(Skeleton skel, ContextGesture gesture_context)
{
foreach (BodyGesture bg in this.gestures)
{
bg.updateGesture(skel, gesture_context);
}
}
private void TrackGesture(Skeleton skeleton, ref GestureTracker tracker, long timeStamp)
{
Joint leftHand = skeleton.Joints[JointType.HandLeft];
Joint rightHand = skeleton.Joints[JointType.HandRight];
if (leftHand.TrackingState != JointTrackingState.NotTracked && rightHand.TrackingState != JointTrackingState.NotTracked)
{
if (tracker.State == GestureState.InProcess && tracker.TimeStamp + _TIMEOUT <= timeStamp)
//响应超时
tracker.UpdateState(GestureState.Failure, timeStamp);
else
{
if (tracker.State == GestureState.InProcess)
{
if (Math.Abs(leftHand.Position.X - rightHand.Position.X) >= UPPER_THRESHOLD || Math.Abs(leftHand.Position.Y - rightHand.Position.Y) >= UPPER_THRESHOLD)
{
tracker.UpdateState(GestureState.Success, timeStamp);
if (GestureDetected != null)
GestureDetected(this, new EventArgs());
}
}
else
{
if (Math.Abs(leftHand.Position.Y - rightHand.Position.Y) < LOWER_THRESHOLD && Math.Abs(leftHand.Position.X - rightHand.Position.X) <= LOWER_THRESHOLD)
tracker.UpdatePosition(timeStamp);
else
tracker.Reset();
}
}
}
else
tracker.Reset();
}
// Detect high fives between pairs of skeletons.
private void detectHighFives(Skeleton[] skeletons)
{
// Loop over every pair of skeletons.
for (int i = 0; i < skeletons.Length; i++)
{
for (int j = i + 1; j < skeletons.Length; j++)
{
var skeleton1 = skeletons[i];
var skeleton2 = skeletons[j];
var id1 = skeleton1.TrackingId;
var id2 = skeleton2.TrackingId;
// Generate a key for the pair.
Tuple<int, int> key;
if (id1 < id2)
key = new Tuple<int, int>(id1, id2);
else
key = new Tuple<int, int>(id2, id1);
// Add the pair to the dictionary if it hasn't been seen before.
if (!hasHighFived.ContainsKey(key))
hasHighFived.Add(key, false);
// Check if the skeletons are high fiving at the moment.
bool currentHighFiveStatus = isHighFiving(skeleton1, skeleton2, hasHighFived[key]);
// Invoke the highFive method when a new high five has been detected.
if (!hasHighFived[key] && currentHighFiveStatus)
highFive();
// Save the current high five status.
hasHighFived[key] = currentHighFiveStatus;
}
}
}
public override Enums.GesturePartResult checkGesture(Skeleton skeleton)
{
if (handType != HandType.NULL)
{
//hand is moving backward
if (handType == HandType.RIGHT)
{
if (skeleton.Joints[JointType.ShoulderRight].Position.Z - skeleton.Joints[JointType.HandRight].Position.Z > Helper.armExtendedDistance * 0.84)
{
return Enums.GesturePartResult.SUCCESS;
}
else
{
return Enums.GesturePartResult.FAIL;
}
}
else if (handType == HandType.LEFT)
{
if (skeleton.Joints[JointType.ShoulderLeft].Position.Z - skeleton.Joints[JointType.HandLeft].Position.Z > Helper.armExtendedDistance * 0.84)
{
return Enums.GesturePartResult.SUCCESS;
}
else
{
return Enums.GesturePartResult.FAIL;
}
}
}
return GesturePartResult.FAIL;
}
protected override bool PosicaoValida(Skeleton esqueletoUsuario)
{
Joint centroOmbros = esqueletoUsuario.Joints[JointType.ShoulderCenter];
Joint maoDireita = esqueletoUsuario.Joints[JointType.HandRight];
Joint cotoveloDireito = esqueletoUsuario.Joints[JointType.ElbowRight];
Joint maoEsquerda = esqueletoUsuario.Joints[JointType.HandLeft];
Joint cotoveloEsquerdo = esqueletoUsuario.Joints[JointType.ElbowLeft];
double margemErro = 0.30;
bool maoDireitaAlturaCorreta = Util.CompararComMargemErro(margemErro, maoDireita.Position.Y, centroOmbros.Position.Y);
bool maoDireitaDistanciaCorreta = Util.CompararComMargemErro(margemErro, maoDireita.Position.Z, centroOmbros.Position.Z);
bool maoDireitaAposCotovelo = maoDireita.Position.X > cotoveloDireito.Position.X;
bool cotoveloDireitoAlturaCorreta = Util.CompararComMargemErro(margemErro, cotoveloDireito.Position.Y, centroOmbros.Position.Y);
bool cotoveloEsquerdoAlturaCorreta = Util.CompararComMargemErro(margemErro, cotoveloEsquerdo.Position.Y, centroOmbros.Position.Y);
bool maoEsquerdaAlturaCorreta = Util.CompararComMargemErro(margemErro, maoEsquerda.Position.Y, centroOmbros.Position.Y);
bool maoEsquerdaDistanciaCorreta = Util.CompararComMargemErro(margemErro, maoEsquerda.Position.Z, centroOmbros.Position.Z);
bool maoEsquerdaAposCotovelo = maoEsquerda.Position.X < cotoveloEsquerdo.Position.X;
return maoDireitaAlturaCorreta &&
maoDireitaDistanciaCorreta &&
maoDireitaAposCotovelo &&
cotoveloDireitoAlturaCorreta &&
maoEsquerdaAlturaCorreta &&
maoEsquerdaDistanciaCorreta &&
maoEsquerdaAposCotovelo &&
cotoveloEsquerdoAlturaCorreta;
}
public skelData(Skeleton skel)
{
sx = skel.Position.X;
sy = skel.Position.Y;
sz = skel.Position.Z;
spread = 0; //not coded yet!!
}
public GesturePartResult CheckGesture(Skeleton skeleton, GesturePartResult previousResult)
{
if (skeleton == null)
{
return GesturePartResult.Fail;
}
// right hand is raised but not swiping yet
if (skeleton.Joints[JointType.HandRight].Position.Y > skeleton.Joints[JointType.ElbowRight].Position.Y)
{
// right hand is to the right of the elbow
if (skeleton.Joints[JointType.HandRight].Position.X > skeleton.Joints[JointType.ElbowRight].Position.X)
{
framecount = 0;
return GesturePartResult.Succeed;
}
if (previousResult == GesturePartResult.Succeed)
{
framecount++;
if (framecount > 30)
{
framecount = 0;
return GesturePartResult.Fail;
}
return GesturePartResult.Pausing;
}
}
// hand dropped
return GesturePartResult.Fail;
}
public GesturePartResult CheckGesture(Skeleton skeleton)
{
double LeftA = Math.Sqrt((Math.Pow(skeleton.Joints[JointType.KneeLeft].Position.Z - skeleton.Joints[JointType.FootLeft].Position.Z, 2) + Math.Pow(skeleton.Joints[JointType.KneeLeft].Position.Y - skeleton.Joints[JointType.FootLeft].Position.Y, 2)));
double LeftB = Math.Sqrt((Math.Pow(skeleton.Joints[JointType.HipRight].Position.Z - skeleton.Joints[JointType.KneeLeft].Position.Z, 2) + Math.Pow(skeleton.Joints[JointType.HipLeft].Position.Y - skeleton.Joints[JointType.KneeLeft].Position.Y, 2)));
double LeftC = skeleton.Joints[JointType.HipLeft].Position.Y - skeleton.Joints[JointType.FootLeft].Position.Y;
double RightA = Math.Sqrt((Math.Pow(skeleton.Joints[JointType.KneeRight].Position.Z - skeleton.Joints[JointType.FootRight].Position.Z, 2) + Math.Pow(skeleton.Joints[JointType.KneeRight].Position.Y - skeleton.Joints[JointType.FootRight].Position.Y, 2)));
double RightB = Math.Sqrt((Math.Pow(skeleton.Joints[JointType.HipRight].Position.Z - skeleton.Joints[JointType.KneeRight].Position.Z, 2) + Math.Pow(skeleton.Joints[JointType.HipRight].Position.Y - skeleton.Joints[JointType.KneeRight].Position.Y, 2)));
double RightC = skeleton.Joints[JointType.HipRight].Position.Y - skeleton.Joints[JointType.FootRight].Position.Y;
double LeftAngle = Math.Acos((Math.Pow(LeftA, 2) + Math.Pow(LeftB, 2) - Math.Pow(LeftC, 2)) / (2 * LeftA * LeftB));
double RightAngle = Math.Acos((Math.Pow(RightA, 2) + Math.Pow(RightB, 2) - Math.Pow(RightC, 2)) / (2 * RightA * RightB));
LeftAngle = (LeftAngle * 180) / Math.PI;
RightAngle = (RightAngle * 180) / Math.PI;
if ((LeftAngle < 145 && RightAngle < 145))
{
return GesturePartResult.Succeed;
}
else if (LeftAngle < 170 && RightAngle < 170)
{
return GesturePartResult.Pausing;
}
return GesturePartResult.Fail;
}
/// <summary>
/// Checks if the given skeleton's tracking data matches
/// the gesture represented by this frame.
/// </summary>
/// <param name="skeleton">Skeleton to analize</param>
/// <returns>
/// Success if the gesture was correct, Waiting if the
/// gesture was not quite right, but still possible, Fail otherwise.
/// </returns>
public KinectGestureResult ProcessFrame(Skeleton skeleton)
{
// Checks if right hand is down.
if (skeleton.Joints[JointType.HandRight].Position.Y < skeleton.Joints[JointType.ElbowRight].Position.Y)
{
// Checks if left hand is below the left shoulder.
if (skeleton.Joints[JointType.HandLeft].Position.Y < skeleton.Joints[JointType.ShoulderLeft].Position.Y)
{
// Checks if left hand is above the hip
if (skeleton.Joints[JointType.HandLeft].Position.Y > skeleton.Joints[JointType.HipLeft].Position.Y)
{
// Checks if the left hand is at the right of the neck.
if (skeleton.Joints[JointType.HandLeft].Position.X > skeleton.Joints[JointType.Spine].Position.X)
{
// The third part of the gesture was completed.
return KinectGestureResult.Success;
}
// Gesture was not completed, but it's still possible to achieve.
else
{
// Will pause recognition and try later.
return KinectGestureResult.Waiting;
}
}
// The standard result is failure.
return KinectGestureResult.Fail;
}
}
// The standard result is failure.
return KinectGestureResult.Fail;
}
/// <summary>
/// Checks the gesture.
/// </summary>
/// <param name="skeleton">The skeleton.</param>
/// <returns>GesturePartResult based on if the gesture part has been completed</returns>
public GesturePartResult CheckGesture(Skeleton skeleton)
{
// Right and Left Hand in front of Shoulders
if (skeleton.Joints[JointType.HandLeft].Position.Z < skeleton.Joints[JointType.ElbowLeft].Position.Z && skeleton.Joints[JointType.HandRight].Position.Z < skeleton.Joints[JointType.ElbowRight].Position.Z)
{
// Hands between shoulder and hip
if (skeleton.Joints[JointType.HandRight].Position.Y < skeleton.Joints[JointType.ShoulderCenter].Position.Y && skeleton.Joints[JointType.HandRight].Position.Y > skeleton.Joints[JointType.HipCenter].Position.Y &&
skeleton.Joints[JointType.HandLeft].Position.Y < skeleton.Joints[JointType.ShoulderCenter].Position.Y && skeleton.Joints[JointType.HandLeft].Position.Y > skeleton.Joints[JointType.HipCenter].Position.Y)
{
// Hands between shoulders
if (skeleton.Joints[JointType.HandRight].Position.X < skeleton.Joints[JointType.ShoulderRight].Position.X && skeleton.Joints[JointType.HandRight].Position.X > skeleton.Joints[JointType.ShoulderLeft].Position.X &&
skeleton.Joints[JointType.HandLeft].Position.X > skeleton.Joints[JointType.ShoulderLeft].Position.X && skeleton.Joints[JointType.HandLeft].Position.X < skeleton.Joints[JointType.ShoulderRight].Position.X)
{
// Hands very close
if (skeleton.Joints[JointType.HandRight].Position.X - skeleton.Joints[JointType.HandLeft].Position.X < 0)
{
return GesturePartResult.Suceed;
}
return GesturePartResult.Pausing;
}
return GesturePartResult.Fail;
}
return GesturePartResult.Fail;
}
return GesturePartResult.Fail;
}
//Checks to see if the two joints are in either of the two states. Unless the beginning relationship has
//been satisifed, it will not check the ending relationship.
public bool Evaluate(Skeleton skeleton, int xScale, int yScale)
{
var sjoint1 = skeleton.Joints[_component.Joint1].ScaleTo(xScale, yScale);
var sjoint2 = skeleton.Joints[_component.Joint2].ScaleTo(xScale, yScale);
if (!BeginningRelationshipSatisfied)
{
var goodtogo = CompareJointRelationship(sjoint1, sjoint2, _component.BeginningRelationship);
if (goodtogo)
{
_beginningRelationshipSatisfied = true;
}
else
{
return false;
}
}
if (!EndingRelationshipSatisfied)
{
var goodtogo = CompareJointRelationship(sjoint1, sjoint2, _component.EndingRelationship);
if (goodtogo)
{
return _endingRelationshipSatisfied = true;
}
return false;
}
return true;
}
public void CheckNoSkeleton()
{
Skeleton[] skeletons = new Microsoft.Kinect.Skeleton[2];
skeletons[0] = new Skeleton();
skeletons[1] = new Skeleton();
Skeleton skeleton = KinectService.GetPrimarySkeleton(skeletons);
Assert.IsNull(skeleton);
}
protected override bool TestPosture(Microsoft.Kinect.Skeleton s)
{
if (s.Joints[JointType.HandRight].Position.Y < s.Joints[JointType.Head].Position.Y &&
s.Joints[JointType.HandRight].Position.X < s.Joints[JointType.ShoulderRight].Position.X)
{
return(true);
}
return(false);
}
/// <summary>
/// default constructor
/// </summary>
/// <param name="skeleton">the skeleton</param>
/// <param name="color">the color of the skeleton on the screen</param>
/// <param name="ellipseSize">the size of the ellipses representing the joints</param>
public VisualSkeleton(Microsoft.Kinect.Skeleton skeleton, Brush color, int ellipseSize = 10)
{
this.skeleton = skeleton;
this.color = color;
if (skeleton != null)
{
CreateVisualSkeleton();
}
}
public override Enums.GesturePartResult checkGesture(Microsoft.Kinect.Skeleton skeleton)
{
if (handType != HandType.NULL)
{
//hand joint is bellow of the wrist joint for about 0.03meters
if (handType == HandType.RIGHT)
{
if (skeleton.Joints[JointType.HandRight].Position.Y > skeleton.Joints[JointType.HipRight].Position.Y)
{
if (skeleton.Joints[JointType.HandRight].Position.Y - skeleton.Joints[JointType.ShoulderRight].Position.Y < 0.15)
{
return(Enums.GesturePartResult.SUCCESS);
}
return(Enums.GesturePartResult.PAUSING);
}
else if (skeleton.Joints[JointType.HandRight].Position.Y - skeleton.Joints[JointType.ShoulderRight].Position.Y < 0.15)
{
if (skeleton.Joints[JointType.HandRight].Position.Y > skeleton.Joints[JointType.HipRight].Position.Y)
{
return(Enums.GesturePartResult.SUCCESS);
}
return(Enums.GesturePartResult.PAUSING);
}
else
{
return(Enums.GesturePartResult.FAIL);
}
}
else if (handType == HandType.LEFT)
{
if (skeleton.Joints[JointType.HandLeft].Position.Y > skeleton.Joints[JointType.HipLeft].Position.Y)
{
if (skeleton.Joints[JointType.HandLeft].Position.Y - skeleton.Joints[JointType.ShoulderLeft].Position.Y < 0.15)
{
return(Enums.GesturePartResult.SUCCESS);
}
return(Enums.GesturePartResult.PAUSING);
}
else if (skeleton.Joints[JointType.HandLeft].Position.Y - skeleton.Joints[JointType.ShoulderLeft].Position.Y < 0.15)
{
if (skeleton.Joints[JointType.HandLeft].Position.Y > skeleton.Joints[JointType.HipLeft].Position.Y)
{
return(Enums.GesturePartResult.SUCCESS);
}
return(Enums.GesturePartResult.PAUSING);
}
else
{
return(Enums.GesturePartResult.FAIL);
}
}
}
return(GesturePartResult.FAIL);
}
/// <summary>
/// Update the sliding window of histograms.
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
public void handleNewSkeleton(object sender, Microsoft.Kinect.Skeleton e)
{
while (skeletons.Count >= WindowSize)
{
skeletons.RemoveAt(skeletons.Count - 1);
}
skeletons.Insert(0, e);
if (skeletons.Count == WindowSize)
{
hsp.histBatch = histogrammer.processSkeletons(skeletons);
}
}
// Called when Kinect has new skeleton data.
private void OnSkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs eventArgs)
{
// Get the new skeleton data from Kinect.
using (var skeletonFrame = eventArgs.OpenSkeletonFrame())
{
if (skeletonFrame == null)
{
return;
}
if (_kinectSkeletons == null || _kinectSkeletons.Length != skeletonFrame.SkeletonArrayLength)
{
_kinectSkeletons = new KinectSkeleton[skeletonFrame.SkeletonArrayLength];
}
skeletonFrame.CopySkeletonDataTo(_kinectSkeletons);
}
// Get the two tracked skeletons.
KinectSkeleton skeletonDataA = null;
KinectSkeleton skeletonDataB = null;
foreach (var skeleton in _kinectSkeletons)
{
if (skeleton.TrackingState == SkeletonTrackingState.Tracked)
{
if (skeletonDataA == null)
{
skeletonDataA = skeleton;
}
else
{
skeletonDataB = skeleton;
}
}
}
// Make sure that each player uses the same skeleton as last time.
// Swap skeleton data if necessary.
if (skeletonDataA != null && skeletonDataA.TrackingId == _trackingIdB ||
skeletonDataB != null && skeletonDataB.TrackingId == _trackingIdA)
{
MathHelper.Swap(ref skeletonDataA, ref skeletonDataB);
}
// Update tracking IDs.
_trackingIdA = (skeletonDataA != null) ? skeletonDataA.TrackingId : 0;
_trackingIdB = (skeletonDataB != null) ? skeletonDataB.TrackingId : 0;
// Update the SkeletonPose from the Kinect skeleton data.
UpdateKinectSkeletonPose(skeletonDataA, SkeletonPoseA);
UpdateKinectSkeletonPose(skeletonDataB, SkeletonPoseB);
}
/// <summary>
/// Event handler for Kinect sensor's SkeletonFrameReady event
/// </summary>
/// <param name="sender">object sending the event</param>
/// <param name="e">event arguments</param>
private void SensorSkeletonFrameReady(object sender, kinect.SkeletonFrameReadyEventArgs e)
{
kinect.Skeleton[] skeletons = new kinect.Skeleton[0];
using (kinect.SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
skeletons = new kinect.Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
}
}
processSkeletons(skeletons);
}
public override Enums.GesturePartResult checkGesture(Microsoft.Kinect.Skeleton skeleton)
{
//hands above shoulders
if (skeleton.Joints[JointType.HandLeft].Position.Y > skeleton.Joints[JointType.ShoulderLeft].Position.Y &&
skeleton.Joints[JointType.HandRight].Position.Y > skeleton.Joints[JointType.ShoulderRight].Position.Y)
{
//hands inside
if (skeleton.Joints[JointType.HandLeft].Position.X > skeleton.Joints[JointType.ElbowLeft].Position.X &&
skeleton.Joints[JointType.HandRight].Position.X < skeleton.Joints[JointType.ElbowRight].Position.X)
{
return(GesturePartResult.SUCCESS);
}
return(GesturePartResult.PAUSING);
}
return(GesturePartResult.FAIL);
}
public override Enums.GesturePartResult checkGesture(Microsoft.Kinect.Skeleton skeleton)
{
//hands between shoulders hand hips
if (skeleton.Joints[JointType.HandLeft].Position.Y <skeleton.Joints[JointType.ShoulderLeft].Position.Y &&
skeleton.Joints[JointType.HandLeft].Position.Y> skeleton.Joints[JointType.HipLeft].Position.Y &&
skeleton.Joints[JointType.HandRight].Position.Y <skeleton.Joints[JointType.ShoulderRight].Position.Y &&
skeleton.Joints[JointType.HandRight].Position.Y> skeleton.Joints[JointType.HipRight].Position.Y)
{
//hands ext
if (skeleton.Joints[JointType.HandLeft].Position.X - skeleton.Joints[JointType.HipLeft].Position.X <-0.20 &&
skeleton.Joints[JointType.HandRight].Position.X - skeleton.Joints[JointType.HipRight].Position.X> 0.20)
{
return(GesturePartResult.SUCCESS);
}
return(GesturePartResult.PAUSING);
}
return(GesturePartResult.FAIL);
}
public override Enums.GesturePartResult checkGesture(Microsoft.Kinect.Skeleton skeleton)
{
if (handType != HandType.NULL)
{
if (handType == HandType.LEFT)
{
//right hand close to the left shoulder Y level and left handclose to the hip Y level
if (Math.Abs(skeleton.Joints[JointType.HandRight].Position.Y - skeleton.Joints[JointType.ShoulderLeft].Position.Y) < 0.25 &&
Math.Abs(skeleton.Joints[JointType.HandLeft].Position.Y - skeleton.Joints[JointType.HipCenter].Position.Y) < 0.10)
{
//right hand left to the left shoulder
if (skeleton.Joints[JointType.HandRight].Position.X - skeleton.Joints[JointType.ShoulderLeft].Position.X < -0.2 &&
skeleton.Joints[JointType.HandLeft].Position.X - skeleton.Joints[JointType.HipLeft].Position.X < -0.42)
{
return(GesturePartResult.SUCCESS);
}
//pausing till next frame
return(GesturePartResult.PAUSING);
}
//hand dropper - fail
return(GesturePartResult.FAIL);
}
else if (handType == HandType.RIGHT)
{
//right hand close to the right shoulder Y level and left handclose to the hip Y level
if (Math.Abs(skeleton.Joints[JointType.HandRight].Position.Y - skeleton.Joints[JointType.ShoulderRight].Position.Y) < 0.25 &&
Math.Abs(skeleton.Joints[JointType.HandLeft].Position.Y - skeleton.Joints[JointType.HipCenter].Position.Y) < 0.10)
{
//right hand right to the right shoulder and left hand right to the right hip
if (skeleton.Joints[JointType.HandRight].Position.X - skeleton.Joints[JointType.ShoulderRight].Position.X > 0.42 &&
skeleton.Joints[JointType.HandLeft].Position.X - skeleton.Joints[JointType.HipRight].Position.X > 0.5)
{
return(GesturePartResult.SUCCESS);
}
//pausing till next frame
return(GesturePartResult.PAUSING);
}
//hand dropper - fail
return(GesturePartResult.FAIL);
}
}
return(GesturePartResult.FAIL);
}
private QuaternionF GetBoneOrientation(KinectSkeleton skeletonData, SkeletonPose skeletonPose, int boneIndex)
{
// Get first child bone. (Kinect stores the bone orientations in the child joints.)
int childIndex = -1;
if (skeletonPose.Skeleton.GetNumberOfChildren(boneIndex) > 0)
{
childIndex = skeletonPose.Skeleton.GetChild(boneIndex, 0);
}
if (childIndex == -1)
{
return(QuaternionF.Identity);
}
var q = skeletonData.BoneOrientations[(JointType)childIndex].AbsoluteRotation.Quaternion;
// Convert to DigitalRune quaternion and mirror z axis.
return(new QuaternionF(-q.W, q.X, q.Y, -q.Z));
}
protected override bool TestDynamique(Microsoft.Kinect.Skeleton s)
{
/*
* if (CurrentNbOfFrames % 4 != 0) return true;
*
* nouvellePositionMainDroiteX = s.Joints[JointType.HandRight].Position.X;
* nouvellePositionMainDroiteY = s.Joints[JointType.HandRight].Position.Y;
*
* if(nouvellePositionMainDroiteX > positionMainDroiteX &&
* nouvellePositionMainDroiteY > positionMainDroiteY)
* {
* positionMainDroiteX = nouvellePositionMainDroiteX;
* positionMainDroiteY = nouvellePositionMainDroiteY;
* return true;
* }
*
* return false;
*/
return(true);
}
private void KinectSensor_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
Microsoft.Kinect.Skeleton[] skeletons = new Microsoft.Kinect.Skeleton[0];
using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
{
if (skeletonFrame != null)
{
skeletons = new Microsoft.Kinect.Skeleton[skeletonFrame.SkeletonArrayLength];
skeletonFrame.CopySkeletonDataTo(skeletons);
Skeletons.Clear();
foreach (Microsoft.Kinect.Skeleton skeleton in skeletons)
{
Skeletons.Add(new Skeleton(skeleton));
}
}
}
OnSkeletonFrameReady();
}
/// <summary>
/// Serialize a skeleton
/// </summary>
/// <param name="skeleton">Skeleton to serialize</param>
/// <param name="writer">Binary writer</param>
private void SerializeSkeleton(kinect.Skeleton skeleton, BinaryWriter writer)
{
writer.Write((int)skeleton.ClippedEdges);
writer.Write(skeleton.Position.X);
writer.Write(skeleton.Position.Y);
writer.Write(skeleton.Position.Z);
writer.Write(skeleton.TrackingId);
writer.Write((int)skeleton.TrackingState);
writer.Write(skeleton.Joints.Count);
foreach (kinect.Joint joint in skeleton.Joints)
{
writer.Write((int)joint.JointType);
writer.Write((int)joint.TrackingState);
writer.Write(joint.Position.X);
writer.Write(joint.Position.Y);
writer.Write(joint.Position.Z);
}
}
public override Enums.GesturePartResult checkGesture(Microsoft.Kinect.Skeleton skeleton)
{
if (handType != HandType.NULL)
{
if (handType == HandType.RIGHT)
{
if (skeleton.Joints[JointType.ElbowRight].Position.Y > skeleton.Joints[JointType.HipRight].Position.Y &&
skeleton.Joints[JointType.ElbowRight].Position.Y - skeleton.Joints[JointType.ShoulderRight].Position.Y < 0.15)
{
if (skeleton.Joints[JointType.HandRight].Position.Y - skeleton.Joints[JointType.ShoulderRight].Position.Y > 0.35)
{
return(Enums.GesturePartResult.SUCCESS);
}
return(Enums.GesturePartResult.PAUSING);
}
else
{
return(Enums.GesturePartResult.FAIL);
}
}
else if (handType == HandType.LEFT)
{
if (skeleton.Joints[JointType.ElbowLeft].Position.Y > skeleton.Joints[JointType.HipLeft].Position.Y &&
skeleton.Joints[JointType.ElbowLeft].Position.Y - skeleton.Joints[JointType.ShoulderLeft].Position.Y < 0.15)
{
if (skeleton.Joints[JointType.HandLeft].Position.Y - skeleton.Joints[JointType.ShoulderLeft].Position.Y > 0.25)
{
return(Enums.GesturePartResult.SUCCESS);
}
return(Enums.GesturePartResult.PAUSING);
}
else
{
return(Enums.GesturePartResult.FAIL);
}
}
}
return(GesturePartResult.FAIL);
}
private void UpdateKinectSkeletonPose(KinectSkeleton skeletonData, SkeletonPose skeletonPose)
{
if (skeletonData == null)
{
return;
}
// Update the skeleton pose using the data from Kinect.
for (int i = 0; i < skeletonPose.Skeleton.NumberOfBones; i++)
{
var joint = (JointType)i;
if (skeletonData.Joints[joint].TrackingState != JointTrackingState.NotTracked)
{
// The joint position in "Kinect space".
SkeletonPoint kinectPosition = skeletonData.Joints[joint].Position;
// Convert Kinect joint position to a Vector3F.
// z is negated because in XNA the camera forward vectors is -z, but the Kinect
// forward vector is +z.
Vector3F position = new Vector3F(kinectPosition.X, kinectPosition.Y, -kinectPosition.Z);
// Apply scale and offset.
position = position * Scale + Offset;
var orientation = QuaternionF.Identity;
// Optional:
// The newer Kinect SDKs also compute bone orientations. We do not need these values
// because the current samples use only the joint positions to derive bone rotations.
//if (joint != JointType.HipCenter) // Motion retargeting seems to work better if the hip center bone is not rotated.
//{
// orientation = GetBoneOrientation(skeletonData, skeletonPose, i);
//}
skeletonPose.SetBonePoseAbsolute(i, new SrtTransform(orientation, position));
}
}
}
protected override bool TestConditionsInit(Microsoft.Kinect.Skeleton s)
{
positionMainDroiteX = s.Joints[JointType.HandRight].Position.X;
positionMainDroiteY = s.Joints[JointType.HandRight].Position.Y;
return(initPosture.TestPosture(s));
}
public Skeleton(Microsoft.Kinect.Skeleton skeleton)
{
_skeleton = skeleton;
}
protected override bool TestConditionsFin(Microsoft.Kinect.Skeleton s)
{
return(endingPosture.TestPosture(s));
}
/// <summary>
/// Processes a skeleton into joystick data using the default FIRST gestures.
/// </summary>
/// <param name="joy">Vector of Joysticks to put the result in</param>
/// <param name="skeleton">The skeleton to process</param>
public void ProcessGestures(Joystick[] joy, Microsoft.Kinect.Skeleton skeleton)
{
// Check edge cases
if (joy == null || joy.Length < 2 || joy[0] == null || joy[1] == null)
{
return;
}
sbyte[] leftAxis = new sbyte[6];
sbyte[] rightAxis = new sbyte[6];
sbyte[] nullAxis = new sbyte[6];
bool dataWithinExpectedRange;
ushort buttons = 0;
double leftAngle = RadToDeg(AngleXY(skeleton.Joints[JointType.ShoulderLeft].Position,
skeleton.Joints[JointType.WristLeft].Position,
true));
double rightAngle = RadToDeg(AngleXY(skeleton.Joints[JointType.ShoulderRight].Position,
skeleton.Joints[JointType.WristRight].Position));
dataWithinExpectedRange = leftAngle <ARM_MAX_ANGLE && leftAngle> ARM_MIN_ANGLE &&
rightAngle <ARM_MAX_ANGLE && rightAngle> ARM_MIN_ANGLE;
double leftYAxis = CoerceToRange(leftAngle,
-70,
70,
-127,
128);
double rightYAxis = CoerceToRange(rightAngle,
-70,
70,
-127,
128);
dataWithinExpectedRange = dataWithinExpectedRange &&
InSameZPlane(skeleton.Joints[JointType.ShoulderLeft].Position,
skeleton.Joints[JointType.WristLeft].Position,
Z_PLANE_TOLERANCE) &&
InSameZPlane(skeleton.Joints[JointType.ShoulderRight].Position,
skeleton.Joints[JointType.WristRight].Position,
Z_PLANE_TOLERANCE);
// Head buttons
double headAngle = RadToDeg(AngleXY(skeleton.Joints[JointType.ShoulderCenter].Position,
skeleton.Joints[JointType.Head].Position));
if (IsHeadRight(headAngle))
{
buttons |= (ushort)Joystick.Buttons.Btn1;
}
if (IsHeadLeft(headAngle))
{
buttons |= (ushort)Joystick.Buttons.Btn2;
}
// Right Leg XY Button
double rightLegAngle = RadToDeg(AngleXY(skeleton.Joints[JointType.HipRight].Position,
skeleton.Joints[JointType.AnkleRight].Position));
if (IsLegOut(rightLegAngle))
{
buttons |= (ushort)Joystick.Buttons.Btn3;
}
// Left Leg XY Button
double leftLegAngle = RadToDeg(AngleXY(skeleton.Joints[JointType.HipLeft].Position,
skeleton.Joints[JointType.AnkleLeft].Position,
true));
if (IsLegOut(leftLegAngle))
{
buttons |= (ushort)Joystick.Buttons.Btn4;
}
// Right Leg YZ Buttons
double rightLegYZ = RadToDeg(AngleYZ(skeleton.Joints[JointType.HipRight].Position,
skeleton.Joints[JointType.AnkleRight].Position));
if (IsLegForward(rightLegYZ))
{
buttons |= (ushort)Joystick.Buttons.Btn5;
}
if (IsLegBackward(rightLegYZ))
{
buttons |= (ushort)Joystick.Buttons.Btn6;
}
// Left Leg YZ Buttons
double leftLegYZ = RadToDeg(AngleYZ(skeleton.Joints[JointType.HipLeft].Position,
skeleton.Joints[JointType.AnkleLeft].Position));
if (IsLegForward(leftLegYZ))
{
buttons |= (ushort)Joystick.Buttons.Btn7;
}
if (IsLegBackward(leftLegYZ))
{
buttons |= (ushort)Joystick.Buttons.Btn8;
}
if (dataWithinExpectedRange)
{
// Invert joystick axis to match a real joystick
// (pushing away creates negative values)
leftAxis[(uint)Joystick.Axis.Y] = (sbyte)-leftYAxis;
rightAxis[(uint)Joystick.Axis.Y] = (sbyte)-rightYAxis;
//Use "Button 9" as Kinect control enabled signal
buttons |= (ushort)Joystick.Buttons.Btn9;
joy[0].Set(leftAxis, buttons);
joy[1].Set(rightAxis, buttons);
}
else
{
joy[0].Set(nullAxis, 0);
joy[1].Set(nullAxis, 0);
}
}
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();
}
}
}
}
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();
}
}
}
}
/// <summary>
/// Get the highest hand of the Kinect User
/// </summary>
/// <param name="skeleton">the skeleton of the user</param>
/// <returns>the highest hand of the user</returns>
private HandType GetHighestHand(Microsoft.Kinect.Skeleton skeleton)
{
return((skeleton.Joints[JointType.HandLeft].Position.Y > skeleton.Joints[JointType.HandRight].Position.Y) ? HandType.LEFT : HandType.RIGHT);
}
private void processSkeletons(kinect.Skeleton[] skeletons)
{
int iSkeleton = 0;
int iSkeletonsTracked = 0;
double panAngle = double.NaN; // degrees from forward; right positive
double tiltAngle = double.NaN;
double targetX = double.NaN; // meters
double targetY = double.NaN;
double targetZ = double.NaN;
kinect.Skeleton trackedSkeleton = (from s in skeletons
where s.TrackingState == kinect.SkeletonTrackingState.Tracked
select s).FirstOrDefault();
using (DrawingContext dc = this.drawingGroup.Open())
{
// Draw a transparent background to set the render size
dc.DrawRectangle(Brushes.Black, null, new Rect(0.0, 0.0, RenderWidth, RenderHeight));
if (skeletons.Length != 0)
{
foreach (kinect.Skeleton skel in skeletons)
{
if (kinect.SkeletonTrackingState.Tracked == skel.TrackingState)
{
//RenderClippedEdges(skel, dc);
//this.DrawBonesAndJoints(skel, dc);
//if (gameOnCheckBox.IsChecked.GetValueOrDefault()
// && skel.Joints[kinect.JointType.Head].TrackingState == kinect.JointTrackingState.Tracked
// && skel.Joints[kinect.JointType.HandLeft].TrackingState == kinect.JointTrackingState.Tracked
// && skel.Joints[kinect.JointType.HandRight].TrackingState == kinect.JointTrackingState.Tracked
// && skel.Joints[kinect.JointType.HandLeft].Position.Y > skel.Joints[kinect.JointType.Head].Position.Y
// && skel.Joints[kinect.JointType.HandRight].Position.Y > skel.Joints[kinect.JointType.Head].Position.Y)
//{
// if (!handsUp && (DateTime.Now - lastHandsUp).TotalSeconds > 1.0d)
// {
// speak();
// lastHandsUp = DateTime.Now;
// handsUp = true;
// shootBoth();
// }
// else
// {
// handsUp = false;
// }
//}
// http://social.msdn.microsoft.com/Forums/en-AU/kinectsdk/thread/d821df8d-39ca-44e3-81e7-c907d94acfca - data.UserIndex is always 254
//bool targetedUser = skel.UserIndex == 1;
bool targetedUser = skel == trackedSkeleton;
if (targetedUser)
{
targetX = skel.Position.X; // meters
targetY = skel.Position.Y;
targetZ = skel.Position.Z;
// can set Pan or Tilt to NaN:
panAngle = -Math.Atan2(targetX, targetZ) * 180.0d / Math.PI;
tiltAngle = Math.Atan2(targetY, targetZ) * 180.0d / Math.PI;
}
iSkeletonsTracked++;
}
iSkeleton++;
} // for each skeleton
}
}
if (iSkeletonsTracked > 0)
{
if (!inServoControl)
{
inServoControl = true;
if (!double.IsNaN(panAngle) && !double.IsNaN(tiltAngle))
{
TrajectoryPoint currentPoint = new TrajectoryPoint()
{
X = targetX,
Y = targetY,
Z = targetZ,
panAngle = panAngle,
tiltAngle = tiltAngle
};
double arrowSpeedMSec = 25.0d;
double timeToTargetS = targetZ / arrowSpeedMSec; // +0.01d; // +0.1d; // full shot cycle takes 0.2 sec; arrow will leave the barrel at about 0.1s. High value will lead to a lot of jittering.
TrajectoryPoint futurePoint = predictor.predict(currentPoint, new TimeSpan((long)(timeToTargetS * TimeSpan.TicksPerSecond)));
//double deltaX = futurePoint.X - currentPoint.X;
//double deltaY = futurePoint.Y - currentPoint.Y;
if (gameOnCheckBox.IsChecked.GetValueOrDefault())
{
this.Dispatcher.Invoke(new UpdateLabelDelegate(updatePmValuesLabel), string.Format("X: {0:0.000}\r\nY: {1:0.000}\r\nZ: {2:0.000}\r\nPan: {3:0}\r\nTilt: {4:0}", targetX, targetY, targetZ, panAngle, tiltAngle));
//pololuMaestroConnector.setPanTilt(futurePoint.panAngle, futurePoint.tiltAngle, futurePoint.panAngle, futurePoint.tiltAngle, 0.0d);
// see C:\Projects\Robotics\src\TrackRoamer\TrackRoamerBehaviors\Strategy\StrategyPersonFollowing.cs : 233
double targetPanRelativeToHead = futurePoint.panAngle;
double targetPanRelativeToRobot = measuredKinectPanDegrees + targetPanRelativeToHead;
//Tracer.Trace("================== currentPanKinect=" + _state.currentPanKinect + " targetJoint.Pan=" + targetJoint.Pan + " targetPanRelativeToRobot=" + targetPanRelativeToRobot);
// guns rotate (pan) with Kinect, but tilt independently of Kinect. They are calibrated when Kinect tilt = 0
//targetPan = targetPanRelativeToHead;
//targetTilt = futurePoint.tiltAngle + kinectTiltActualDegrees;
double kinectTurnEstimate = targetPanRelativeToRobot - measuredKinectPanDegrees;
double smallMovementsAngleTreshold = 10.0d;
bool shouldTurnKinect = Math.Abs(kinectTurnEstimate) > smallMovementsAngleTreshold; // don't follow small movements
//kinectPanDesiredDegrees = shouldTurnKinect ? (double?)targetPanRelativeToRobot : null; // will be processed in computeHeadTurn() when head turn measurement comes.
if (shouldTurnKinect)
{
double kinectPanDesiredDegrees = targetPanRelativeToRobot; // will be processed in computeHeadTurn() when head turn measurement comes.
panKinectTargetPosScrollBar.Value = _panTiltAlignment.mksPanKinect(kinectPanDesiredDegrees);
}
}
}
inServoControl = false;
}
}
else if (gameOnCheckBox.IsChecked.GetValueOrDefault())
{
// lost skeletons, center the head:
panKinectTargetPosScrollBar.Value = 1500.0d;
}
}