//private static int cnt = 0; internal static Position3D GetPositionOfEndOfStickRight(Position3D hand_pos, Position3D wrist_pos) { /* Position3D stick_pos = new Position3D(); double angleY = System.Math.Atan((hand_pos.Y - wrist_pos.Y) / (hand_pos.Z - wrist_pos.Z)); double angleX = System.Math.Atan((hand_pos.X - wrist_pos.X) / (hand_pos.Z - wrist_pos.Z)); stick_pos.Z = hand_pos.Z - System.Math.Cos(angleX - Math.PI/6); stick_pos.X = hand_pos.X - System.Math.Sin(angleX - Math.PI/6); stick_pos.Y = hand_pos.Y - System.Math.Sin(angleY); */ Position3D stick_pos = new Position3D(); double angleY = System.Math.Atan((hand_pos.Y - wrist_pos.Y) / (wrist_pos.Z - hand_pos.Z)); double angleX = System.Math.Atan((wrist_pos.Z - hand_pos.Z) / (hand_pos.X - wrist_pos.X)); if (angleX < 0) angleX += Math.PI; stick_pos.Z = hand_pos.Z - length * System.Math.Sin(angleX + angle); stick_pos.X = hand_pos.X + length * System.Math.Cos(angleX + angle); stick_pos.Y = hand_pos.Y + length * System.Math.Sin(angleY); /* if (cnt > 150) { System.Console.WriteLine("wrist" + wrist_pos); System.Console.WriteLine("hand" + hand_pos); System.Console.WriteLine("stick" + stick_pos + "\n"); cnt = 0; } cnt++; */ return stick_pos; }
public bool eval(Position3D prePos, Position3D pos) { if (prePos.Y > pos.Y + 0.01) { return true; } else { return false; } }
public void Trail(Position3D newPos) { if (this.tracks.Count >= this.tracks.Capacity) { this.tracks.RemoveAt(0); } if (action.eval(curPos, newPos)) { cnt++; if (cnt > 1) { hasAction = true; } } else { cnt = 0; hasAction = false; } this.tracks.Add(newPos); curPos = newPos; }
public PositionRatio(Position3D p) : this(p.X, p.Z) { }
public CircleTarget(string id, Position3D pos, double radius) { this.Pos = pos; this.Id = id; this.radius = radius; }
private void SensorSkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e) { Skeleton[] skeletons = new Skeleton[0]; using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame()) { if (skeletonFrame != null) { skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength]; skeletonFrame.CopySkeletonDataTo(skeletons); } } Position3D right_hand_pos = null; Position3D right_wrist_pos = null; Position3D left_hand_pos = null; Position3D left_wrist_pos = null; foreach (Skeleton skeleton in skeletons) { if (skeleton.TrackingState == SkeletonTrackingState.Tracked) { //余裕があれば書き変えたい foreach (Joint joint in skeleton.Joints) { if (joint.JointType == JointType.HandRight) { right_hand_pos = new Position3D(joint.Position.X, joint.Position.Y, joint.Position.Z); } else if (joint.JointType == JointType.WristRight) { right_wrist_pos = new Position3D(joint.Position.X, joint.Position.Y, joint.Position.Z); } else if (joint.JointType == JointType.HandLeft) { left_hand_pos = new Position3D(joint.Position.X, joint.Position.Y, joint.Position.Z); } else if (joint.JointType == JointType.WristLeft) { left_wrist_pos = new Position3D(joint.Position.X, joint.Position.Y, joint.Position.Z); } } } } Position3D right_stick; Position3D left_stick; if (right_hand_pos != null && right_wrist_pos != null) { right_stick = Stick.GetPositionOfEndOfStickRight(right_hand_pos, right_wrist_pos); tracksRight.Trail(right_stick); Drumset.DetectResult result = drumset.Detect(tracksRight); if (result != null) { System.Console.WriteLine(result); } if (cnt1 > 3) { PositionRatio r1 = new PositionRatio(right_hand_pos); PositionRatio r2 = new PositionRatio(right_stick); //System.Console.WriteLine(r1.ToJSON() + "\n" + r2.ToJSON()); sendMessage("stick", r1.ToJSON() + "&" + r2.ToJSON() + "&R"); cnt1 = 0; } cnt1++; } if (left_hand_pos != null && left_wrist_pos != null) { left_stick = Stick.GetPositionOfEndOfStickLeft(left_hand_pos, left_wrist_pos); tracksLeft.Trail(left_stick); Drumset.DetectResult result = drumset.Detect(tracksLeft); if (result != null) { System.Console.WriteLine(result); } if (cnt2 > 3) { PositionRatio l1 = new PositionRatio(left_hand_pos); PositionRatio l2 = new PositionRatio(left_stick); //System.Consol2 =riteLine(r1.ToJSON() + "\n" + r2.ToJSON()); sendMessage("stick", l1.ToJSON() + "&" + l2.ToJSON() + "&L"); cnt2 = 0; } cnt2++; } }