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
}