CreateKinectJoints(
IReadOnlyDictionary <Microsoft.Kinect.JointType, Microsoft.Kinect.Joint>
baseJoints,
Z3Target target)
{
// Acquire all vectors from Z3 target
var jointVectors = new Dictionary <Microsoft.Kinect.JointType, Vector3D>();
// Spine base is the root of the system, as it has no direction to store, it stores its own position
jointVectors.Add(
Microsoft.Kinect.JointType.SpineBase,
new Vector3D(
baseJoints[Microsoft.Kinect.JointType.SpineBase].Position.X,
baseJoints[Microsoft.Kinect.JointType.SpineBase].Position.Y,
baseJoints[Microsoft.Kinect.JointType.SpineBase].Position.Z));
var z3JointTypes = EnumUtil.GetValues <PreposeGestures.JointType>();
var targetJoints = target.TransformedJoints;
foreach (var jointType in z3JointTypes)
{
if (jointType != PreposeGestures.JointType.SpineBase)
{
// If target has calculated the joint add target jointType
if (targetJoints.Contains(jointType))
{
AddZ3JointToVectors3D(target.Body, baseJoints, jointVectors, jointType);
}
// Or else use the joint from current Kinect data (baseJoints)
else
{
jointVectors.Add(
(Microsoft.Kinect.JointType)jointType,
new Vector3D(
baseJoints[(Microsoft.Kinect.JointType)jointType].Position.X,
baseJoints[(Microsoft.Kinect.JointType)jointType].Position.Y,
baseJoints[(Microsoft.Kinect.JointType)jointType].Position.Z));
}
}
}
var result = HipsSpineToKinectCoordinateSystem(jointVectors, baseJoints, targetJoints);
return(result);
}
CreateKinectJoints(
IReadOnlyDictionary <Microsoft.Kinect.JointType, Microsoft.Kinect.Joint>
baseJoints,
Z3Target target)
{
// Calc base rotation matrix
var rotationMatrix = new Matrix3D();
InitMatrix(out rotationMatrix, baseJoints);
// Acquire all vectors from Z3 target
var jointVectors = new Dictionary <Microsoft.Kinect.JointType, Vector3D>();
// Spine base is the root of the system, as it has no direction to store, it stores its own position
jointVectors.Add(
Microsoft.Kinect.JointType.SpineBase,
new Vector3D(
baseJoints[Microsoft.Kinect.JointType.SpineBase].Position.X,
baseJoints[Microsoft.Kinect.JointType.SpineBase].Position.Y,
baseJoints[Microsoft.Kinect.JointType.SpineBase].Position.Z));
var z3JointTypes = EnumUtil.GetValues <PreposeGestures.JointType>();
var targetJoints = target.TransformedJoints;
foreach (var jointType in z3JointTypes)
{
if (jointType != PreposeGestures.JointType.SpineBase)
{
// If target has calculated the joint add target jointType
if (targetJoints.Contains(jointType))
{
AddZ3JointToVectors3D(target.Body, baseJoints, jointVectors, jointType);
}
// Or else use the joint from current Kinect data (baseJoints)
else
{
jointVectors.Add(
(Microsoft.Kinect.JointType)jointType,
new Vector3D(
baseJoints[(Microsoft.Kinect.JointType)jointType].Position.X,
baseJoints[(Microsoft.Kinect.JointType)jointType].Position.Y,
baseJoints[(Microsoft.Kinect.JointType)jointType].Position.Z));
}
}
}
// Rotate all vectors to the current base body coordinate system
var kinectJointTypes = EnumUtil.GetValues <Microsoft.Kinect.JointType>();
foreach (var jointType in kinectJointTypes)
{
if (jointType != Microsoft.Kinect.JointType.SpineBase &&
targetJoints.Contains((PreposeGestures.JointType)jointType))
{
jointVectors[jointType] *= rotationMatrix;
}
}
// Add base body position (spineBase position) to translate result
// The operations order matter in this case
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.SpineMid);
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.SpineShoulder);
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.Neck);
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.Head);
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.ShoulderLeft);
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.ElbowLeft);
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.WristLeft);
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.HandLeft);
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.HandTipLeft);
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.ThumbLeft);
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.ShoulderRight);
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.ElbowRight);
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.WristRight);
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.HandRight);
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.HandTipRight);
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.ThumbRight);
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.HipLeft);
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.KneeLeft);
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.AnkleLeft);
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.FootLeft);
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.HipRight);
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.KneeRight);
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.AnkleRight);
SumWithFatherPosition(jointVectors, targetJoints, Microsoft.Kinect.JointType.FootRight);
// Filling the results
var result = new Dictionary <Microsoft.Kinect.JointType, Microsoft.Kinect.Joint>();
foreach (var jointType in kinectJointTypes)
{
var joint = new Joint();
var position = new CameraSpacePoint();
position.X = (float)jointVectors[jointType].X;
position.Y = (float)jointVectors[jointType].Y;
position.Z = (float)jointVectors[jointType].Z;
joint.Position = position;
joint.TrackingState = TrackingState.Tracked;
result.Add(jointType, joint);
}
return(result);
}
