// returns the local joint position of the specified user, relative to the parent joint, in meters
public Vector3 GetJointLocalPosition(int joint)
{
int parent = KinectWrapper.GetSkeletonJointParent(joint);
return(joint >= 0 && joint < player1JointsPos.Length ?
(player1JointsPos[joint] - player1JointsPos[parent]) : Vector3.zero);
}
// returns the joint rotation of the specified user, relative to the parent joint
public Quaternion GetJointLocalOrientation(uint UserId, int joint, bool flip)
{
int parent = KinectWrapper.GetSkeletonJointParent(joint);
if (UserId == Player1ID)
{
if (joint >= 0 && joint < player1JointsOri.Length && player1JointsTracked[joint])
{
Matrix4x4 localMat = (player1JointsOri[parent].inverse * player1JointsOri[joint]);
return(Quaternion.LookRotation(localMat.GetColumn(2), localMat.GetColumn(1)));
}
}
return(Quaternion.identity);
}
// returns the local joint position of the specified user, relative to the parent joint, in meters
public Vector3 GetJointLocalPosition(uint UserId, int joint)
{
int parent = KinectWrapper.GetSkeletonJointParent(joint);
if (UserId == _player1Id)
{
return(joint >= 0 && joint < _player1JointsPos.Length ?
(_player1JointsPos[joint] - _player1JointsPos[parent]) : Vector3.zero);
}
else if (UserId == _player2Id)
{
return(joint >= 0 && joint < _player2JointsPos.Length ?
(_player2JointsPos[joint] - _player2JointsPos[parent]) : Vector3.zero);
}
return(Vector3.zero);
}
// ApplyBoneOrientationConstraints and constrain rotations.
public void Constrain(ref Matrix4x4[] jointOrientations, ref bool[] jointTracked)
{
// Constraints are defined as a vector with respect to the parent bone vector, and a constraint angle,
// which is the maximum angle with respect to the constraint axis that the bone can move through.
// Calculate constraint values. 0.0-1.0 means the bone is within the constraint cone. Greater than 1.0 means
// it lies outside the constraint cone.
for (int i = 0; i < this.jointConstraints.Count; i++)
{
BoneOrientationConstraint jc = this.jointConstraints[i];
if (!jointTracked[i] || jc.joint == (int)KinectWrapper.NuiSkeletonPositionIndex.HipCenter)
{
// End joint is not tracked or Hip Center has no parent to perform this calculation with.
continue;
}
// If the joint has a parent, constrain the bone direction to be within the constraint cone
int parentIdx = KinectWrapper.GetSkeletonJointParent(jc.joint);
// Local bone orientation relative to parent
Matrix4x4 localOrientationMatrix = jointOrientations[parentIdx].inverse * jointOrientations[jc.joint];
Vector3 localOrientationZ = (Vector3)localOrientationMatrix.GetColumn(2);
Vector3 localOrientationY = (Vector3)localOrientationMatrix.GetColumn(1);
if (localOrientationZ == Vector3.zero || localOrientationY == Vector3.zero)
{
continue;
}
Quaternion localRotation = Quaternion.LookRotation(localOrientationZ, localOrientationY);
Vector3 eulerAngles = localRotation.eulerAngles;
bool isConstrained = false;
//Matrix4x4 globalOrientationMatrix = jointOrientations[jc.joint];
//Quaternion globalRotation = Quaternion.LookRotation(globalOrientationMatrix.GetColumn(2), globalOrientationMatrix.GetColumn(1));
for (int a = 0; a < jc.axisConstrainrs.Count; a++)
{
AxisOrientationConstraint ac = jc.axisConstrainrs[a];
Quaternion axisRotation = Quaternion.AngleAxis(localRotation.eulerAngles[ac.axis], ac.rotateAround);
//Quaternion axisRotation = Quaternion.AngleAxis(globalRotation.eulerAngles[ac.axis], ac.rotateAround);
float angleFromMin = Quaternion.Angle(axisRotation, ac.minQuaternion);
float angleFromMax = Quaternion.Angle(axisRotation, ac.maxQuaternion);
if (!(angleFromMin <= ac.angleRange && angleFromMax <= ac.angleRange))
{
// Keep the current rotations around other axes and only
// correct the axis that has fallen out of range.
//Vector3 euler = globalRotation.eulerAngles;
if (angleFromMin > angleFromMax)
{
eulerAngles[ac.axis] = ac.angleMax;
}
else
{
eulerAngles[ac.axis] = ac.angleMin;
}
isConstrained = true;
}
}
if (isConstrained)
{
Quaternion constrainedRotation = Quaternion.Euler(eulerAngles);
// Put it back into the bone orientations
localOrientationMatrix.SetTRS(Vector3.zero, constrainedRotation, Vector3.one);
jointOrientations[jc.joint] = jointOrientations[parentIdx] * localOrientationMatrix;
//globalOrientationMatrix.SetTRS(Vector3.zero, constrainedRotation, Vector3.one);
//jointOrientations[jc.joint] = globalOrientationMatrix;
switch (jc.joint)
{
case (int)KinectWrapper.NuiSkeletonPositionIndex.ShoulderCenter:
jointOrientations[(int)KinectWrapper.NuiSkeletonPositionIndex.Head] = jointOrientations[jc.joint];
break;
case (int)KinectWrapper.NuiSkeletonPositionIndex.WristLeft:
jointOrientations[(int)KinectWrapper.NuiSkeletonPositionIndex.HandLeft] = jointOrientations[jc.joint];
break;
case (int)KinectWrapper.NuiSkeletonPositionIndex.WristRight:
jointOrientations[(int)KinectWrapper.NuiSkeletonPositionIndex.HandRight] = jointOrientations[jc.joint];
break;
case (int)KinectWrapper.NuiSkeletonPositionIndex.AnkleLeft:
jointOrientations[(int)KinectWrapper.NuiSkeletonPositionIndex.FootLeft] = jointOrientations[jc.joint];
break;
case (int)KinectWrapper.NuiSkeletonPositionIndex.AnkleRight:
jointOrientations[(int)KinectWrapper.NuiSkeletonPositionIndex.FootRight] = jointOrientations[jc.joint];
break;
}
}
// globalRotation = Quaternion.LookRotation(globalOrientationMatrix.GetColumn(2), globalOrientationMatrix.GetColumn(1));
// string stringToDebug = string.Format("{0}, {2}", (KinectWrapper.NuiSkeletonPositionIndex)jc.joint,
// globalRotation.eulerAngles, localRotation.eulerAngles);
// Debug.Log(stringToDebug);
//
// if(debugText != null)
// debugText.guiText.text = stringToDebug;
}
}