private void CalculateJointOrients(ref KinectInterop.BodyData bodyData)
{
int jointCount = bodyData.joint.Length;
for (int j = 0; j < jointCount; j++)
{
int joint = j;
KinectInterop.JointData jointData = bodyData.joint[joint];
bool bJointValid = ignoreInferredJoints ? jointData.trackingState == KinectInterop.TrackingState.Tracked : jointData.trackingState != KinectInterop.TrackingState.NotTracked;
if (bJointValid)
{
int nextJoint = (int)sensorData.sensorInterface.GetNextJoint((KinectInterop.JointType)joint);
if (nextJoint != joint && nextJoint >= 0 && nextJoint < sensorData.jointCount)
{
KinectInterop.JointData nextJointData = bodyData.joint[nextJoint];
bool bNextJointValid = ignoreInferredJoints ? nextJointData.trackingState == KinectInterop.TrackingState.Tracked : nextJointData.trackingState != KinectInterop.TrackingState.NotTracked;
Vector3 baseDir = KinectInterop.JointBaseDir[nextJoint];
Vector3 jointDir = nextJointData.direction;
jointDir = new Vector3(jointDir.x, jointDir.y, -jointDir.z).normalized;
Quaternion jointOrientNormal = jointData.normalRotation;
if (bNextJointValid)
{
jointOrientNormal = Quaternion.FromToRotation(baseDir, jointDir);
}
if ((joint == (int)KinectInterop.JointType.ShoulderLeft) ||
(joint == (int)KinectInterop.JointType.ShoulderRight))
{
float angle = -bodyData.bodyTurnAngle;
Vector3 axis = jointDir;
Quaternion armTurnRotation = Quaternion.AngleAxis(angle, axis);
jointData.normalRotation = armTurnRotation * jointOrientNormal;
}
else if ((joint == (int)KinectInterop.JointType.ElbowLeft) ||
(joint == (int)KinectInterop.JointType.WristLeft) ||
(joint == (int)KinectInterop.JointType.HandLeft))
{
// if(joint == (int)KinectInterop.JointType.WristLeft)
// {
// KinectInterop.JointData handData = bodyData.joint[(int)KinectInterop.JointType.HandLeft];
// KinectInterop.JointData handTipData = bodyData.joint[(int)KinectInterop.JointType.HandTipLeft];
//
// if(handData.trackingState != KinectInterop.TrackingState.NotTracked &&
// handTipData.trackingState != KinectInterop.TrackingState.NotTracked)
// {
// jointDir = handData.direction + handTipData.direction;
// jointDir = new Vector3(jointDir.x, jointDir.y, -jointDir.z).normalized;
// }
// }
KinectInterop.JointData shCenterData = bodyData.joint[(int)KinectInterop.JointType.SpineShoulder];
if (shCenterData.trackingState != KinectInterop.TrackingState.NotTracked &&
jointDir != Vector3.zero && shCenterData.direction != Vector3.zero &&
Mathf.Abs(Vector3.Dot(jointDir, shCenterData.direction.normalized)) < 0.5f)
{
Vector3 spineDir = shCenterData.direction;
spineDir = new Vector3(spineDir.x, spineDir.y, -spineDir.z).normalized;
Vector3 fwdDir = Vector3.Cross(-jointDir, spineDir).normalized;
Vector3 upDir = Vector3.Cross(fwdDir, -jointDir).normalized;
jointOrientNormal = Quaternion.LookRotation(fwdDir, upDir);
}
else
{
jointOrientNormal = Quaternion.FromToRotation(baseDir, jointDir);
}
bool bRotated = (allowedHandRotations == AllowedRotations.None) &&
(joint != (int)KinectInterop.JointType.ElbowLeft); // false;
if ((allowedHandRotations == AllowedRotations.All) &&
(sensorData.sensorIntPlatform == KinectInterop.DepthSensorPlatform.KinectSDKv2)
/**&& (joint != (int)KinectInterop.JointType.ElbowLeft)*/)
{
// KinectInterop.JointData handData = bodyData.joint[(int)KinectInterop.JointType.HandLeft];
// KinectInterop.JointData handTipData = bodyData.joint[(int)KinectInterop.JointType.HandTipLeft];
KinectInterop.JointData thumbData = bodyData.joint[(int)KinectInterop.JointType.ThumbLeft];
// if(handData.trackingState != KinectInterop.TrackingState.NotTracked &&
// handTipData.trackingState != KinectInterop.TrackingState.NotTracked &&
if (thumbData.trackingState != KinectInterop.TrackingState.NotTracked)
{
Vector3 rightDir = -nextJointData.direction; // -(handData.direction + handTipData.direction);
rightDir = new Vector3(rightDir.x, rightDir.y, -rightDir.z).normalized;
Vector3 fwdDir = thumbData.direction;
fwdDir = new Vector3(fwdDir.x, fwdDir.y, -fwdDir.z).normalized;
if (rightDir != Vector3.zero && fwdDir != Vector3.zero)
{
Vector3 upDir = Vector3.Cross(fwdDir, rightDir).normalized;
fwdDir = Vector3.Cross(rightDir, upDir).normalized;
jointData.normalRotation = Quaternion.LookRotation(fwdDir, upDir);
//bRotated = true;
// // fix invalid wrist rotation
// KinectInterop.JointData elbowData = bodyData.joint[(int)KinectInterop.JointType.ElbowLeft];
// if(elbowData.trackingState != KinectInterop.TrackingState.NotTracked)
// {
// Quaternion quatLocalRot = Quaternion.Inverse(elbowData.normalRotation) * jointData.normalRotation;
// float angleY = quatLocalRot.eulerAngles.y;
//
// if(angleY >= 90f && angleY < 270f && bodyData.leftHandOrientation != Quaternion.identity)
// {
// jointData.normalRotation = bodyData.leftHandOrientation;
// }
//
// bodyData.leftHandOrientation = jointData.normalRotation;
// }
//bRotated = true;
}
}
bRotated = true;
}
if (!bRotated)
{
float angle = -bodyData.bodyTurnAngle;
Vector3 axis = jointDir;
Quaternion armTurnRotation = Quaternion.AngleAxis(angle, axis);
jointData.normalRotation = //(allowedHandRotations != AllowedRotations.None || joint == (int)KinectInterop.JointType.ElbowLeft) ?
armTurnRotation * jointOrientNormal; // : armTurnRotation;
}
}
else if ((joint == (int)KinectInterop.JointType.ElbowRight) ||
(joint == (int)KinectInterop.JointType.WristRight) ||
(joint == (int)KinectInterop.JointType.HandRight))
{
// if(joint == (int)KinectInterop.JointType.WristRight)
// {
// KinectInterop.JointData handData = bodyData.joint[(int)KinectInterop.JointType.HandRight];
// KinectInterop.JointData handTipData = bodyData.joint[(int)KinectInterop.JointType.HandTipRight];
//
// if(handData.trackingState != KinectInterop.TrackingState.NotTracked &&
// handTipData.trackingState != KinectInterop.TrackingState.NotTracked)
// {
// jointDir = handData.direction + handTipData.direction;
// jointDir = new Vector3(jointDir.x, jointDir.y, -jointDir.z).normalized;
// }
// }
KinectInterop.JointData shCenterData = bodyData.joint[(int)KinectInterop.JointType.SpineShoulder];
if (shCenterData.trackingState != KinectInterop.TrackingState.NotTracked &&
jointDir != Vector3.zero && shCenterData.direction != Vector3.zero &&
Mathf.Abs(Vector3.Dot(jointDir, shCenterData.direction.normalized)) < 0.5f)
{
Vector3 spineDir = shCenterData.direction;
spineDir = new Vector3(spineDir.x, spineDir.y, -spineDir.z).normalized;
Vector3 fwdDir = Vector3.Cross(jointDir, spineDir).normalized;
Vector3 upDir = Vector3.Cross(fwdDir, jointDir).normalized;
jointOrientNormal = Quaternion.LookRotation(fwdDir, upDir);
}
else
{
jointOrientNormal = Quaternion.FromToRotation(baseDir, jointDir);
}
bool bRotated = (allowedHandRotations == AllowedRotations.None) &&
(joint != (int)KinectInterop.JointType.ElbowRight); // false;
if ((allowedHandRotations == AllowedRotations.All) &&
(sensorData.sensorIntPlatform == KinectInterop.DepthSensorPlatform.KinectSDKv2)
/**&& (joint != (int)KinectInterop.JointType.ElbowRight)*/)
{
// KinectInterop.JointData handData = bodyData.joint[(int)KinectInterop.JointType.HandRight];
// KinectInterop.JointData handTipData = bodyData.joint[(int)KinectInterop.JointType.HandTipRight];
KinectInterop.JointData thumbData = bodyData.joint[(int)KinectInterop.JointType.ThumbRight];
// if(handData.trackingState != KinectInterop.TrackingState.NotTracked &&
// handTipData.trackingState != KinectInterop.TrackingState.NotTracked &&
if (thumbData.trackingState != KinectInterop.TrackingState.NotTracked)
{
Vector3 rightDir = nextJointData.direction; // handData.direction + handTipData.direction;
rightDir = new Vector3(rightDir.x, rightDir.y, -rightDir.z).normalized;
Vector3 fwdDir = thumbData.direction;
fwdDir = new Vector3(fwdDir.x, fwdDir.y, -fwdDir.z).normalized;
if (rightDir != Vector3.zero && fwdDir != Vector3.zero)
{
Vector3 upDir = Vector3.Cross(fwdDir, rightDir).normalized;
fwdDir = Vector3.Cross(rightDir, upDir).normalized;
jointData.normalRotation = Quaternion.LookRotation(fwdDir, upDir);
//bRotated = true;
// // fix invalid wrist rotation
// KinectInterop.JointData elbowData = bodyData.joint[(int)KinectInterop.JointType.ElbowRight];
// if(elbowData.trackingState != KinectInterop.TrackingState.NotTracked)
// {
// Quaternion quatLocalRot = Quaternion.Inverse(elbowData.normalRotation) * jointData.normalRotation;
// float angleY = quatLocalRot.eulerAngles.y;
//
// if(angleY >= 90f && angleY < 270f && bodyData.rightHandOrientation != Quaternion.identity)
// {
// jointData.normalRotation = bodyData.rightHandOrientation;
// }
//
// bodyData.rightHandOrientation = jointData.normalRotation;
// }
//bRotated = true;
}
}
bRotated = true;
}
if (!bRotated)
{
float angle = -bodyData.bodyTurnAngle;
Vector3 axis = jointDir;
Quaternion armTurnRotation = Quaternion.AngleAxis(angle, axis);
jointData.normalRotation = //(allowedHandRotations != AllowedRotations.None || joint == (int)KinectInterop.JointType.ElbowRight) ?
armTurnRotation * jointOrientNormal; // : armTurnRotation;
}
}
else
{
jointData.normalRotation = jointOrientNormal;
}
if ((joint == (int)KinectInterop.JointType.SpineMid) ||
(joint == (int)KinectInterop.JointType.SpineShoulder) ||
(joint == (int)KinectInterop.JointType.Neck))
{
Vector3 baseDir2 = Vector3.right;
Vector3 jointDir2 = Vector3.Lerp(bodyData.shouldersDirection, -bodyData.shouldersDirection, bodyData.turnAroundFactor);
jointDir2.z = -jointDir2.z;
jointData.normalRotation *= Quaternion.FromToRotation(baseDir2, jointDir2);
}
else if ((joint == (int)KinectInterop.JointType.SpineBase) ||
(joint == (int)KinectInterop.JointType.HipLeft) || (joint == (int)KinectInterop.JointType.HipRight) ||
(joint == (int)KinectInterop.JointType.KneeLeft) || (joint == (int)KinectInterop.JointType.KneeRight) ||
(joint == (int)KinectInterop.JointType.AnkleLeft) || (joint == (int)KinectInterop.JointType.AnkleRight))
{
Vector3 baseDir2 = Vector3.right;
Vector3 jointDir2 = Vector3.Lerp(bodyData.hipsDirection, -bodyData.hipsDirection, bodyData.turnAroundFactor);
jointDir2.z = -jointDir2.z;
jointData.normalRotation *= Quaternion.FromToRotation(baseDir2, jointDir2);
}
if (joint == (int)KinectInterop.JointType.Neck &&
sensorData != null && sensorData.sensorInterface != null)
{
if (sensorData.sensorInterface.IsFaceTrackingActive() &&
sensorData.sensorInterface.IsFaceTracked(bodyData.liTrackingID))
{
KinectInterop.JointData neckData = bodyData.joint[(int)KinectInterop.JointType.Neck];
KinectInterop.JointData headData = bodyData.joint[(int)KinectInterop.JointType.Head];
if (neckData.trackingState == KinectInterop.TrackingState.Tracked &&
headData.trackingState == KinectInterop.TrackingState.Tracked)
{
Quaternion headRotation = Quaternion.identity;
if (sensorData.sensorInterface.GetHeadRotation(bodyData.liTrackingID, ref headRotation))
{
Vector3 rotAngles = headRotation.eulerAngles;
rotAngles.x = -rotAngles.x;
rotAngles.y = -rotAngles.y;
bodyData.headOrientation = bodyData.headOrientation != Quaternion.identity ?
Quaternion.Slerp(bodyData.headOrientation, Quaternion.Euler(rotAngles), 5f * Time.deltaTime) :
Quaternion.Euler(rotAngles);
jointData.normalRotation = bodyData.headOrientation;
}
}
}
}
Vector3 mirroredAngles = jointData.normalRotation.eulerAngles;
mirroredAngles.y = -mirroredAngles.y;
mirroredAngles.z = -mirroredAngles.z;
jointData.mirroredRotation = Quaternion.Euler(mirroredAngles);
}
else
{
// get the orientation of the parent joint
int prevJoint = (int)sensorData.sensorInterface.GetParentJoint((KinectInterop.JointType)joint);
if (prevJoint != joint && prevJoint >= 0 && prevJoint < sensorData.jointCount)
{
jointData.normalRotation = bodyData.joint[prevJoint].normalRotation;
jointData.mirroredRotation = bodyData.joint[prevJoint].mirroredRotation;
}
else
{
jointData.normalRotation = Quaternion.identity;
jointData.mirroredRotation = Quaternion.identity;
}
}
}
bodyData.joint[joint] = jointData;
if (joint == (int)KinectInterop.JointType.SpineBase)
{
bodyData.normalRotation = jointData.normalRotation;
bodyData.mirroredRotation = jointData.mirroredRotation;
}
}
}