//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++;
}
}
