private void ProcessBodySpecialData(ref KinectInterop.BodyData bodyData)
{
if (bodyData.joint[(int)KinectInterop.JointType.HipLeft].trackingState == KinectInterop.TrackingState.NotTracked &&
bodyData.joint[(int)KinectInterop.JointType.SpineBase].trackingState != KinectInterop.TrackingState.NotTracked &&
bodyData.joint[(int)KinectInterop.JointType.HipRight].trackingState != KinectInterop.TrackingState.NotTracked)
{
bodyData.joint[(int)KinectInterop.JointType.HipLeft].trackingState = KinectInterop.TrackingState.Inferred;
bodyData.joint[(int)KinectInterop.JointType.HipLeft].kinectPos = bodyData.joint[(int)KinectInterop.JointType.SpineBase].kinectPos +
(bodyData.joint[(int)KinectInterop.JointType.SpineBase].kinectPos - bodyData.joint[(int)KinectInterop.JointType.HipRight].kinectPos);
bodyData.joint[(int)KinectInterop.JointType.HipLeft].position = bodyData.joint[(int)KinectInterop.JointType.SpineBase].position +
(bodyData.joint[(int)KinectInterop.JointType.SpineBase].position - bodyData.joint[(int)KinectInterop.JointType.HipRight].position);
bodyData.joint[(int)KinectInterop.JointType.HipLeft].direction = bodyData.joint[(int)KinectInterop.JointType.HipLeft].position -
bodyData.joint[(int)KinectInterop.JointType.SpineBase].position;
}
if (bodyData.joint[(int)KinectInterop.JointType.HipRight].trackingState == KinectInterop.TrackingState.NotTracked &&
bodyData.joint[(int)KinectInterop.JointType.SpineBase].trackingState != KinectInterop.TrackingState.NotTracked &&
bodyData.joint[(int)KinectInterop.JointType.HipLeft].trackingState != KinectInterop.TrackingState.NotTracked)
{
bodyData.joint[(int)KinectInterop.JointType.HipRight].trackingState = KinectInterop.TrackingState.Inferred;
bodyData.joint[(int)KinectInterop.JointType.HipRight].kinectPos = bodyData.joint[(int)KinectInterop.JointType.SpineBase].kinectPos +
(bodyData.joint[(int)KinectInterop.JointType.SpineBase].kinectPos - bodyData.joint[(int)KinectInterop.JointType.HipLeft].kinectPos);
bodyData.joint[(int)KinectInterop.JointType.HipRight].position = bodyData.joint[(int)KinectInterop.JointType.SpineBase].position +
(bodyData.joint[(int)KinectInterop.JointType.SpineBase].position - bodyData.joint[(int)KinectInterop.JointType.HipLeft].position);
bodyData.joint[(int)KinectInterop.JointType.HipRight].direction = bodyData.joint[(int)KinectInterop.JointType.HipRight].position -
bodyData.joint[(int)KinectInterop.JointType.SpineBase].position;
}
if ((bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].trackingState == KinectInterop.TrackingState.NotTracked &&
bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].trackingState != KinectInterop.TrackingState.NotTracked &&
bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].trackingState != KinectInterop.TrackingState.NotTracked))
{
bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].trackingState = KinectInterop.TrackingState.Inferred;
bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].kinectPos = bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].kinectPos +
(bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].kinectPos - bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].kinectPos);
bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].position = bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].position +
(bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].position - bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].position);
bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].direction = bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].position -
bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].position;
}
if ((bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].trackingState == KinectInterop.TrackingState.NotTracked &&
bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].trackingState != KinectInterop.TrackingState.NotTracked &&
bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].trackingState != KinectInterop.TrackingState.NotTracked))
{
bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].trackingState = KinectInterop.TrackingState.Inferred;
bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].kinectPos = bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].kinectPos +
(bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].kinectPos - bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].kinectPos);
bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].position = bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].position +
(bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].position - bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].position);
bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].direction = bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].position -
bodyData.joint[(int)KinectInterop.JointType.SpineShoulder].position;
}
// calculate special directions
if (bodyData.joint[(int)KinectInterop.JointType.HipLeft].trackingState != KinectInterop.TrackingState.NotTracked &&
bodyData.joint[(int)KinectInterop.JointType.HipRight].trackingState != KinectInterop.TrackingState.NotTracked)
{
Vector3 posRHip = bodyData.joint[(int)KinectInterop.JointType.HipRight].position;
Vector3 posLHip = bodyData.joint[(int)KinectInterop.JointType.HipLeft].position;
bodyData.hipsDirection = posRHip - posLHip;
bodyData.hipsDirection -= Vector3.Project(bodyData.hipsDirection, Vector3.up);
}
if (bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].trackingState != KinectInterop.TrackingState.NotTracked &&
bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].trackingState != KinectInterop.TrackingState.NotTracked)
{
Vector3 posRShoulder = bodyData.joint[(int)KinectInterop.JointType.ShoulderRight].position;
Vector3 posLShoulder = bodyData.joint[(int)KinectInterop.JointType.ShoulderLeft].position;
bodyData.shouldersDirection = posRShoulder - posLShoulder;
bodyData.shouldersDirection -= Vector3.Project(bodyData.shouldersDirection, Vector3.up);
Vector3 shouldersDir = bodyData.shouldersDirection;
shouldersDir.z = -shouldersDir.z;
Quaternion turnRot = Quaternion.FromToRotation(Vector3.right, shouldersDir);
bodyData.bodyTurnAngle = turnRot.eulerAngles.y;
}
// if(bodyData.joint[(int)KinectInterop.JointType.ElbowLeft].trackingState != KinectInterop.TrackingState.NotTracked &&
// bodyData.joint[(int)KinectInterop.JointType.WristLeft].trackingState != KinectInterop.TrackingState.NotTracked)
// {
// Vector3 pos1 = bodyData.joint[(int)KinectInterop.JointType.ElbowLeft].position;
// Vector3 pos2 = bodyData.joint[(int)KinectInterop.JointType.WristLeft].position;
//
// bodyData.leftArmDirection = pos2 - pos1;
// }
// if(allowHandRotations && bodyData.leftArmDirection != Vector3.zero &&
// bodyData.joint[(int)KinectInterop.JointType.WristLeft].trackingState != KinectInterop.TrackingState.NotTracked &&
// bodyData.joint[(int)KinectInterop.JointType.ThumbLeft].trackingState != KinectInterop.TrackingState.NotTracked)
// {
// Vector3 pos1 = bodyData.joint[(int)KinectInterop.JointType.WristLeft].position;
// Vector3 pos2 = bodyData.joint[(int)KinectInterop.JointType.ThumbLeft].position;
//
// Vector3 armDir = bodyData.leftArmDirection;
// armDir.z = -armDir.z;
//
// bodyData.leftThumbDirection = pos2 - pos1;
// bodyData.leftThumbDirection.z = -bodyData.leftThumbDirection.z;
// bodyData.leftThumbDirection -= Vector3.Project(bodyData.leftThumbDirection, armDir);
//
// bodyData.leftThumbForward = Quaternion.AngleAxis(bodyData.bodyTurnAngle, Vector3.up) * Vector3.forward;
// bodyData.leftThumbForward -= Vector3.Project(bodyData.leftThumbForward, armDir);
//
// if(bodyData.leftThumbForward.sqrMagnitude < 0.01f)
// {
// bodyData.leftThumbForward = Vector3.zero;
// }
// }
// else
// {
// if(bodyData.leftThumbDirection != Vector3.zero)
// {
// bodyData.leftThumbDirection = Vector3.zero;
// bodyData.leftThumbForward = Vector3.zero;
// }
// }
// if(bodyData.joint[(int)KinectInterop.JointType.ElbowRight].trackingState != KinectInterop.TrackingState.NotTracked &&
// bodyData.joint[(int)KinectInterop.JointType.WristRight].trackingState != KinectInterop.TrackingState.NotTracked)
// {
// Vector3 pos1 = bodyData.joint[(int)KinectInterop.JointType.ElbowRight].position;
// Vector3 pos2 = bodyData.joint[(int)KinectInterop.JointType.WristRight].position;
//
// bodyData.rightArmDirection = pos2 - pos1;
// }
// if(allowHandRotations && bodyData.rightArmDirection != Vector3.zero &&
// bodyData.joint[(int)KinectInterop.JointType.WristRight].trackingState != KinectInterop.TrackingState.NotTracked &&
// bodyData.joint[(int)KinectInterop.JointType.ThumbRight].trackingState != KinectInterop.TrackingState.NotTracked)
// {
// Vector3 pos1 = bodyData.joint[(int)KinectInterop.JointType.WristRight].position;
// Vector3 pos2 = bodyData.joint[(int)KinectInterop.JointType.ThumbRight].position;
//
// Vector3 armDir = bodyData.rightArmDirection;
// armDir.z = -armDir.z;
//
// bodyData.rightThumbDirection = pos2 - pos1;
// bodyData.rightThumbDirection.z = -bodyData.rightThumbDirection.z;
// bodyData.rightThumbDirection -= Vector3.Project(bodyData.rightThumbDirection, armDir);
//
// bodyData.rightThumbForward = Quaternion.AngleAxis(bodyData.bodyTurnAngle, Vector3.up) * Vector3.forward;
// bodyData.rightThumbForward -= Vector3.Project(bodyData.rightThumbForward, armDir);
//
// if(bodyData.rightThumbForward.sqrMagnitude < 0.01f)
// {
// bodyData.rightThumbForward = Vector3.zero;
// }
// }
// else
// {
// if(bodyData.rightThumbDirection != Vector3.zero)
// {
// bodyData.rightThumbDirection = Vector3.zero;
// bodyData.rightThumbForward = Vector3.zero;
// }
// }
if (bodyData.joint[(int)KinectInterop.JointType.KneeLeft].trackingState != KinectInterop.TrackingState.NotTracked &&
bodyData.joint[(int)KinectInterop.JointType.AnkleLeft].trackingState != KinectInterop.TrackingState.NotTracked &&
bodyData.joint[(int)KinectInterop.JointType.FootLeft].trackingState != KinectInterop.TrackingState.NotTracked)
{
Vector3 vFootProjected = Vector3.Project(bodyData.joint[(int)KinectInterop.JointType.FootLeft].direction, bodyData.joint[(int)KinectInterop.JointType.AnkleLeft].direction);
bodyData.joint[(int)KinectInterop.JointType.AnkleLeft].kinectPos += vFootProjected;
bodyData.joint[(int)KinectInterop.JointType.AnkleLeft].position += vFootProjected;
bodyData.joint[(int)KinectInterop.JointType.FootLeft].direction -= vFootProjected;
}
if (bodyData.joint[(int)KinectInterop.JointType.KneeRight].trackingState != KinectInterop.TrackingState.NotTracked &&
bodyData.joint[(int)KinectInterop.JointType.AnkleRight].trackingState != KinectInterop.TrackingState.NotTracked &&
bodyData.joint[(int)KinectInterop.JointType.FootRight].trackingState != KinectInterop.TrackingState.NotTracked)
{
Vector3 vFootProjected = Vector3.Project(bodyData.joint[(int)KinectInterop.JointType.FootRight].direction, bodyData.joint[(int)KinectInterop.JointType.AnkleRight].direction);
bodyData.joint[(int)KinectInterop.JointType.AnkleRight].kinectPos += vFootProjected;
bodyData.joint[(int)KinectInterop.JointType.AnkleRight].position += vFootProjected;
bodyData.joint[(int)KinectInterop.JointType.FootRight].direction -= vFootProjected;
}
}
