// Given a skeleton, this function stores all the rotations and the current time
// to the skeletonData object, so we can export that later.
private void storeSkeletonData(Skeleton skeleton)
{
// Get the current time, so all our actions are synchronized in the data
long time = stopwatch.ElapsedMilliseconds;
// Get the bone orientations
BoneOrientationCollection skeletonRotations = skeleton.BoneOrientations;
// From the joint orientations, get easy names for all of them
// so we can store these quaternion rotations easily and sensically
Vector4 HipCenterRotation = skeletonRotations[JointType.HipCenter].HierarchicalRotation.Quaternion;
Vector4 SpineRotation = skeletonRotations[JointType.Spine].HierarchicalRotation.Quaternion;
Vector4 ShoulderCenterRotation = skeletonRotations[JointType.ShoulderCenter].HierarchicalRotation.Quaternion;
Vector4 HeadRotation = skeletonRotations[JointType.Head].HierarchicalRotation.Quaternion;
Vector4 ShoulderLeftRotation = skeletonRotations[JointType.ShoulderLeft].HierarchicalRotation.Quaternion;
Vector4 ElbowLeftRotation = skeletonRotations[JointType.ElbowLeft].HierarchicalRotation.Quaternion;
Vector4 WristLeftRotation = skeletonRotations[JointType.WristLeft].HierarchicalRotation.Quaternion;
Vector4 HandLeftRotation = skeletonRotations[JointType.HandLeft].HierarchicalRotation.Quaternion;
Vector4 ShoulderRightRotation = skeletonRotations[JointType.ShoulderRight].HierarchicalRotation.Quaternion;
Vector4 ElbowRightRotation = skeletonRotations[JointType.ElbowRight].HierarchicalRotation.Quaternion;
Vector4 WristRightRotation = skeletonRotations[JointType.WristRight].HierarchicalRotation.Quaternion;
Vector4 HandRightRotation = skeletonRotations[JointType.HandRight].HierarchicalRotation.Quaternion;
Vector4 HipLeftRotation = skeletonRotations[JointType.HipLeft].HierarchicalRotation.Quaternion;
Vector4 KneeLeftRotation = skeletonRotations[JointType.KneeLeft].HierarchicalRotation.Quaternion;
Vector4 AnkleLeftRotation = skeletonRotations[JointType.AnkleLeft].HierarchicalRotation.Quaternion;
Vector4 FootLeftRotation = skeletonRotations[JointType.FootLeft].HierarchicalRotation.Quaternion;
Vector4 HipRightRotation = skeletonRotations[JointType.HipRight].HierarchicalRotation.Quaternion;
Vector4 KneeRightRotation = skeletonRotations[JointType.KneeRight].HierarchicalRotation.Quaternion;
Vector4 AnkleRightRotation = skeletonRotations[JointType.AnkleRight].HierarchicalRotation.Quaternion;
Vector4 FootRightRotation = skeletonRotations[JointType.FootRight].HierarchicalRotation.Quaternion;
// Now add new points with the time and values to the storage object
skeletonData.HipCenter.Add(new quaternionPoint(time, HipCenterRotation));
skeletonData.Spine.Add(new quaternionPoint(time, SpineRotation));
skeletonData.ShoulderCenter.Add(new quaternionPoint(time, ShoulderCenterRotation));
skeletonData.Head.Add(new quaternionPoint(time, HeadRotation));
skeletonData.ShoulderLeft.Add(new quaternionPoint(time, ShoulderLeftRotation));
skeletonData.ElbowLeft.Add(new quaternionPoint(time, ElbowLeftRotation));
skeletonData.WristLeft.Add(new quaternionPoint(time, WristLeftRotation));
skeletonData.HandLeft.Add(new quaternionPoint(time, HandLeftRotation));
skeletonData.ShoulderRight.Add(new quaternionPoint(time, ShoulderRightRotation));
skeletonData.ElbowRight.Add(new quaternionPoint(time, ElbowRightRotation));
skeletonData.WristRight.Add(new quaternionPoint(time, WristRightRotation));
skeletonData.HandRight.Add(new quaternionPoint(time, HandRightRotation));
skeletonData.HipLeft.Add(new quaternionPoint(time, HipLeftRotation));
skeletonData.KneeLeft.Add(new quaternionPoint(time, KneeLeftRotation));
skeletonData.AnkleLeft.Add(new quaternionPoint(time, AnkleLeftRotation));
skeletonData.FootLeft.Add(new quaternionPoint(time, FootLeftRotation));
skeletonData.HipRight.Add(new quaternionPoint(time, HipRightRotation));
skeletonData.KneeRight.Add(new quaternionPoint(time, KneeRightRotation));
skeletonData.AnkleRight.Add(new quaternionPoint(time, AnkleRightRotation));
skeletonData.FootRight.Add(new quaternionPoint(time, FootRightRotation));
}
private async void DrawSeatedSkeletons(DrawingContext drawingContext)
{
if (!canDraw)
{
return;
}
await System.Threading.Tasks.Task.Run(() => resetCanDraw());
foreach (Skeleton skeleton in skeletonData)
{
if (skeleton != null && skeleton.TrackingState != SkeletonTrackingState.NotTracked)
{
// debugger.WriteLine("\n\r Skeleton: " + (skeleton.TrackingId));
//debugger.WriteLine("\n\r - - Position: (" + (skeleton.Position.X + ", " + skeleton.Position.Y + ", " + skeleton.Position.Z) + " ");
JointCollection jointCollection = skeleton.Joints;
BoneOrientationCollection boneOrientationCollection = skeleton.BoneOrientations;
foreach (Joint joint in jointCollection)
{
if (joint.TrackingState != JointTrackingState.NotTracked)
{
//Blender plugin accepted format
//jointName x y z
string tcpMessage = joint.JointType.ToString() + " " + (joint.Position.X + " " + joint.Position.Y + " " + joint.Position.Z + " \n");
debugger.WriteLine("\n\r" + tcpMessage + " ");
sendTcpMessage(tcpMessage);
}
}
/*
* if (skeleton.TrackingState == SkeletonTrackingState.Tracked)
* {
* DrawTrackedSkeletonJoints(skeleton.Joints);
* }
* else if (skeleton.TrackingState == SkeletonTrackingState.PositionOnly)
* {
* DrawSkeletonPosition(skeleton.Position);
* }
*/
/*
* foreach (BoneOrientation boneOrientation in boneOrientationCollection)
* {
* Vector4 rotation = boneOrientation.AbsoluteRotation.Quaternion;
* //DrawBonewithRotation(orientation.StartJoint, orientation.EndJoint, orientation.AbsoluteRotation.Quaternion);
* debugger.WriteLine("\n\r - - - Bone (" + boneOrientation.StartJoint + " -> " + boneOrientation.EndJoint + ") Rotation: (" + (rotation.X + ", " + rotation.Y + ", " + rotation.Z + ", " + rotation.W) + ") ");
* }
*/
//debugger.WriteLine("\n\r\n\r==================\n\r\n\r");
}
}
}
private static string DoMatrixPrint(BoneOrientationCollection boneOrientationCollection)
{
string rez = "+++++++++++Данные по матрицам+++++++++" + Environment.NewLine;
foreach (BoneOrientation bone in boneOrientationCollection)
{
rez += "Кость: " + bone.StartJoint + "-" + bone.EndJoint + Environment.NewLine;
rez += "===AbsoluteRotation===" + PrintMatrix(bone.AbsoluteRotation.Matrix);
rez += "===HierarchicalRotation===" + PrintMatrix(bone.HierarchicalRotation.Matrix);
rez += "+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++" + Environment.NewLine;
}
return rez;
}
private static BoneOrientation FindBone(JointType start, JointType end, BoneOrientationCollection collection)
{
foreach (BoneOrientation bone in collection)
{
if (bone.StartJoint == start && bone.EndJoint == end)
return bone;
}
throw new Exception("Кость " + start.ToString() + "-" + end.ToString() + " не найдена в коллекции");
}
