public float GetValue(string jointName) { // TODO: Исправить на другое значение if (Joints == null || Joints.Count == 0) { return(0); } try { return(Joints.Where(x => x.Name == jointName).FirstOrDefault().Value); } catch { return(0); } }
private void SensorSkeletonFrameReady(object sender, SkeletonFrameReadyEventArgs e) { var skeletons = new Skeleton[0]; using (var skeletonFrame = e.OpenSkeletonFrame()) { if (skeletonFrame != null) { skeletons = new Skeleton[skeletonFrame.SkeletonArrayLength]; skeletonFrame.CopySkeletonDataTo(skeletons); } } foreach (var skeleton in skeletons) { Joints = skeleton.Joints; IsSkeletonTracked = skeleton.TrackingState == SkeletonTrackingState.Tracked; SkeletonTracked?.Invoke(IsSkeletonTracked, new EventArgs()); var trackedJoints = Joints .Where(x => x.TrackingState == JointTrackingState.Tracked) .Select(x => $"{x.JointType.ToString()} X:{x.Position.X.ToString("F2")} Y:{x.Position.Y.ToString("F2")} Z:{x.Position.Z.ToString("F2")}") .ToList(); if (Joints[JointType.HandLeft].Position.Y < Joints[JointType.HipLeft].Position.Y && Joints[JointType.HandRight].Position.Y < Joints[JointType.HipRight].Position.Y || !Joints.Any(x => x.Position.Z > 0)) { return; } if (Joints[JointType.HandRight].Position.Y > Joints[JointType.Spine].Position.Y && Joints[JointType.HandLeft].Position.Y > Joints[JointType.Spine].Position.Y && Joints[JointType.ElbowLeft].Position.Y < Joints[JointType.Spine].Position.Y && Joints[JointType.ElbowRight].Position.Y < Joints[JointType.Spine].Position.Y) { S = true; } if (Joints[JointType.HandRight].Position.Y > Joints[JointType.ShoulderCenter].Position.Y && Joints[JointType.HandLeft].Position.Y > Joints[JointType.ShoulderCenter].Position.Y && Joints[JointType.ElbowLeft].Position.Y < Joints[JointType.ShoulderCenter].Position.Y && Joints[JointType.ElbowRight].Position.Y < Joints[JointType.ShoulderCenter].Position.Y) { W = true; } if (Joints[JointType.HandRight].Position.Y > Joints[JointType.ShoulderCenter].Position.Y && Joints[JointType.ElbowRight].Position.Y > Joints[JointType.ShoulderCenter].Position.Y && Joints[JointType.HandRight].Position.Y > Joints[JointType.Head].Position.Y && Joints[JointType.HandLeft].Position.Y < Joints[JointType.Spine].Position.Y) { Up = true; } if (Joints[JointType.HandLeft].Position.Y > Joints[JointType.ShoulderCenter].Position.Y && Joints[JointType.ElbowLeft].Position.Y > Joints[JointType.ShoulderCenter].Position.Y && Joints[JointType.HandLeft].Position.Y > Joints[JointType.Head].Position.Y && Joints[JointType.HandRight].Position.Y < Joints[JointType.Spine].Position.Y) { Down = true; } if (Joints[JointType.HandLeft].Position.X > Joints[JointType.HipCenter].Position.X) { Right = true; } if (Joints[JointType.HandRight].Position.X < Joints[JointType.HipCenter].Position.X) { Left = true; } if (Joints[JointType.HandLeft].Position.Y > Joints[JointType.Spine].Position.Y && Joints[JointType.ElbowLeft].Position.Y > Joints[JointType.Spine].Position.Y && Joints[JointType.HandLeft].Position.Y < Joints[JointType.Head].Position.Y && Joints[JointType.ElbowLeft].Position.Y < Joints[JointType.Head].Position.Y && Joints[JointType.HandRight].Position.Y < Joints[JointType.Spine].Position.Y) { A = true; } } }
/// <summary> /// Determines the lengths and references. /// </summary> internal void DetermineLengthsAndReferences() { numJoints = Joints.Count; #region Define Initial Link Angle var fixedJoints = Joints.Where(j => j.FixedWithRespectTo(this)). OrderBy(j => (j.IsGround) ? 0 : 1). ThenBy(j => (j.Link2 != null) ? 0 : 1).ThenBy(j => j.XInitial).ToList(); ReferenceJoint1 = fixedJoints[0]; /* the linkAngle is defined from "the joint with the lowest initial x value * that is fixed to this link" to "the joint with the highest initial x value * that is fixed to this link. If the link has only one fixed joint (and * it will necessarily have one due to the addition of a joint added in * Simulator.addReferencePivotsToSlideOnlyLinks()) or is ground, then the * initial angle is zero. */ if (fixedJoints.Count == 2 && !IsGround) { AngleInitial = Constants.Angle(ReferenceJoint1, fixedJoints[1]); } else { AngleInitial = 0.0; } Angle = AngleNumerical = AngleLast = AngleInitial; foreach (var j in Joints.Where(j => j.SlidingWithRespectTo(this))) { j.OffsetSlideAngle -= AngleInitial; } #endregion #region Joint-to-Joint Dictionaries lengths = new Dictionary <int, double>(); distanceToSlideLine = new Dictionary <int, double>(); angleFromBlockToJoint = new Dictionary <int, double>(); for (var i = 0; i < Joints.Count - 1; i++) { for (var j = i + 1; j < Joints.Count; j++) { var iJoint = Joints[i]; var jJoint = Joints[j]; if (iJoint.SlidingWithRespectTo(this) && jJoint.SlidingWithRespectTo(this)) { var key = numJoints * i + j; angleFromBlockToJoint.Add(key, 0.0); distanceToSlideLine.Add(key, 0.0); key = numJoints * j + i; angleFromBlockToJoint.Add(key, 0.0); distanceToSlideLine.Add(key, 0.0); } else if (iJoint.SlidingWithRespectTo(this) && !jJoint.SlidingWithRespectTo(this)) { AddSlideDictionaryEntry(iJoint, jJoint, i, j); AddBlockAngleDictionaryEntry(iJoint, jJoint, i, j); if (jJoint.TypeOfJoint == JointType.P) { AddBlockAngleDictionaryEntry(jJoint, iJoint, j, i); AddSlideDictionaryEntry(jJoint, iJoint, j, i); } } else if (!iJoint.SlidingWithRespectTo(this) && jJoint.SlidingWithRespectTo(this)) { AddSlideDictionaryEntry(jJoint, iJoint, j, i); AddBlockAngleDictionaryEntry(jJoint, iJoint, j, i); if (iJoint.TypeOfJoint == JointType.P) { AddBlockAngleDictionaryEntry(iJoint, jJoint, i, j); AddSlideDictionaryEntry(iJoint, jJoint, i, j); } } else // if (!iJoint.SlidingWithRespectTo(this) && !jJoint.SlidingWithRespectTo(this)) { lengths.Add(numJoints * i + j, Constants.Distance(iJoint.XInitial, iJoint.YInitial, jJoint.XInitial, jJoint.YInitial)); if (iJoint.TypeOfJoint == JointType.P) { AddBlockAngleDictionaryEntry(iJoint, jJoint, i, j); AddSlideDictionaryEntry(iJoint, jJoint, i, j); } if (jJoint.TypeOfJoint == JointType.P) { AddBlockAngleDictionaryEntry(jJoint, iJoint, j, i); AddSlideDictionaryEntry(jJoint, iJoint, j, i); } } } } #endregion }