void nui_SkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e)
{
SkeletonFrame allSkeletons = e.SkeletonFrame;
//get the first tracked skeleton
SkeletonData skeleton = (from s in allSkeletons.Skeletons
where s.TrackingState == SkeletonTrackingState.Tracked
select s).FirstOrDefault();
if (skeleton != null)
{
this._skel = skeleton;
Joint head = skeleton.Joints[JointID.Head];
HumanSkeleton hs = new HumanSkeleton(skeleton.Joints);
NaoSkeleton ns = new NaoSkeleton(hs);
UpdateTextBoxes(ns, hs);
controller.update(ns);
SetLineAngle(RightArmTopDown, Util.radToDeg(hs.RightShoulderYaw));
SetLineAngle(LeftArmTopDown, Util.radToDeg(hs.LeftShoulderYaw));
SetLineAngle(RightArmFront, Util.radToDeg(-hs.RightShoulderPitch) + 90);
SetLineAngle(LeftArmFront, Util.radToDeg(hs.LeftShoulderPitch) - 90);
SetLineAngle(RightArm, Util.radToDeg(-hs.RightShoulderRoll) + 90);
SetLineAngle(LeftArm, Util.radToDeg(hs.LeftShoulderRoll) - 90);
}
}
private void UpdateTextBoxes(NaoSkeleton ns, HumanSkeleton hs)
{
NaoElbowRoll.Text = "Left: " + ns.LeftElbow.getRoll() + "(NaoRoll) - " + hs.LeftElbowYaw + "(HumanYaw) || Right: " + ns.RightElbow.getRoll() + "(NaoRoll) - " + hs.RightElbowYaw + "(HumanYaw)";
NaoElbowYaw.Text = "Left: " + ns.LeftElbow.getYaw() + "(NaoYaw) - " + hs.LeftShoulderRoll + "(HumanRoll) || Right: " + ns.RightElbow.getYaw() + "(NaoYaw) - " + hs.RightShoulderRoll + "(HumanRoll)";
NaoShoulderRoll.Text = "Left: " + ns.LeftShoulder.Roll + "(NaoRoll) - " + hs.LeftShoulderPitch + "(HumanPitch) || Right: " + ns.RightShoulder.Roll + "(NaoRoll) - " + hs.RightShoulderPitch + "(HumanPitch)";
NaoShoulderPitch.Text = "Left: " + ns.LeftShoulder.Pitch + "(NaoPitch) - " + hs.LeftShoulderYaw + "(HumanYaw) || Right: " + ns.RightShoulder.Pitch + "(NaoPitch) - " + hs.RightShoulderYaw + "(HumanYaw)";
}
public NaoSkeleton(HumanSkeleton skeleton)
{
this.LeftShoulder = new NaoShoulder(skeleton.LeftShoulderYaw, skeleton.LeftShoulderPitch, true);
this.RightShoulder = new NaoShoulder(skeleton.RightShoulderYaw, skeleton.RightShoulderPitch, false);
this.LeftWrist = new NaoWrist(0.0);
this.RightWrist = new NaoWrist(0.0);
this.LeftElbow = new NaoElbow(skeleton.LeftShoulderRoll, skeleton.LeftElbowYaw, true);
this.RightElbow = new NaoElbow(skeleton.RightShoulderRoll, skeleton.RightElbowYaw, false);
this.LeftHand = new NaoHand(true);
this.RightHand = new NaoHand(true);
}
