private IEnumerable<Joint> PredictSkeleton(LearnerPredictedFeatureSet learnedFeatures)
{
var skel = SkeletonExtensions.ExampleSkeleton;
var vectors = skel.ToDictionary(x => x.ID, x => x.Position.ToVector3D());
var torsoPlaneNormal = PlaneNormal(vectors[JointID.ShoulderCenter], vectors[JointID.HipLeft], vectors[JointID.HipRight]).NormalizeVector();
var leftLeg = new PredictedLeg(false, learnedFeatures.PredictedLeftLegAngle, torsoPlaneNormal,
vectors[JointID.HipLeft], vectors[JointID.HipRight], vectors[JointID.KneeLeft], vectors[JointID.AnkleLeft], vectors[JointID.FootLeft]);
var rightLeg = new PredictedLeg(true, learnedFeatures.PredictedRightLegAngle, torsoPlaneNormal,
vectors[JointID.HipRight], vectors[JointID.HipLeft], vectors[JointID.KneeRight], vectors[JointID.AnkleRight], vectors[JointID.FootRight]);
var offset = Math.Min(rightLeg.Foot.Y, leftLeg.Foot.Y) - vectors[JointID.FootRight].Y;
var shoulderRotation = Matrix3D.Identity;
shoulderRotation.RotateAt(new Quaternion(new Vector3D(0, 1, 0), learnedFeatures.PredictedPersonHeading.ToDegrees()),
(Point3D)vectors[JointID.Spine]);
for (int i = 0; i < (int)JointID.Count; i++)
{
Joint joint = skel[i];
var vec = joint.Position;
Vector3D newVec = new Vector3D();
switch (skel[i].ID)
{
case JointID.KneeRight: { newVec = rightLeg.Knee; break; }
case JointID.AnkleRight: { newVec = rightLeg.Ankle; break; }
case JointID.FootRight: { newVec = rightLeg.Foot; break; }
case JointID.KneeLeft: { newVec = leftLeg.Knee; break; }
case JointID.AnkleLeft: { newVec = leftLeg.Ankle; break; }
case JointID.FootLeft: { newVec = leftLeg.Foot; break; }
default: { newVec = vec.ToVector3D(); break; }
}
newVec = (Vector3D)(((Point3D)newVec) * shoulderRotation);
newVec.Y -= offset;
joint.Position = newVec.ToNuiVector();
skel[i] = joint;
}
return skel;
}