/// <summary>
/// AddJointConstraint - Adds a joint constraint to the system.
/// </summary>
/// <param name="joint">The skeleton joint/bone.</param>
/// <param name="dir">The absolute dir for the center of the constraint cone.</param>
/// <param name="angle">The angle of the constraint cone that the bone can move in.</param>
public void AddBoneOrientationConstraint(JointType joint, Vector3 dir, float angle)
{
this.constraintsMirrored = false;
BoneOrientationConstraint jc = new BoneOrientationConstraint(joint, dir, angle);
this.jointConstraints.Add(jc);
}
// AddJointConstraint - Adds a joint constraint to the system.
private void AddBoneOrientationConstraint(int thisJoint, CT consType, Vector3 axis, float angleMin, float angleMax)
{
int index = FindBoneOrientationConstraint(thisJoint);
BoneOrientationConstraint jc = index >= 0 ? jointConstraints[index] : new BoneOrientationConstraint(thisJoint);
if (index < 0)
{
index = jointConstraints.Count;
jointConstraints.Add(jc);
}
AxisOrientationConstraint constraint = new AxisOrientationConstraint(consType, axis, angleMin, angleMax);
jc.axisConstrainrs.Add(constraint);
jointConstraints[index] = jc;
}
// AddJointConstraint - Adds a joint constraint to the system.
public void AddBoneOrientationConstraint(int joint, ConstraintAxis axis, float angleMin, float angleMax)
{
int index = FindBoneOrientationConstraint(joint);
BoneOrientationConstraint jc = index >= 0 ? jointConstraints[index] : new BoneOrientationConstraint(joint);
if (index < 0)
{
index = jointConstraints.Count;
jointConstraints.Add(jc);
}
AxisOrientationConstraint constraint = new AxisOrientationConstraint(axis, angleMin, angleMax);
jc.axisConstrainrs.Add(constraint);
jointConstraints[index] = jc;
}
/// <summary>
/// Helper method to calculate a rotated line for the constraint cones.
/// </summary>
private void AddRotatedLine(Vector3 rotatedLine, Skeleton skeleton, int i, Color color)
{
BoneOrientationConstraint jc = this.jointConstraints[i];
Vector3 coneTipVertex = KinectHelper.Position(skeleton, KinectHelper.ParentBoneJoint(jc.Joint));
// Scale dir vector for display
rotatedLine *= LineScale;
Vector3 coneBaseVertex = coneTipVertex + rotatedLine;
coneTipVertex *= this.SkeletonTranslationScaleFactor; // This will scale and optionally mirror the skeleton
coneBaseVertex *= this.SkeletonTranslationScaleFactor;
this.lineVertices.Add(new VertexPositionColor(coneTipVertex, color));
this.lineIndices.Add((short)(this.lineVertices.Count - 1));
this.lineVertices.Add(new VertexPositionColor(coneBaseVertex, color));
this.lineIndices.Add((short)(this.lineVertices.Count - 1));
}
/// <summary>
/// Helper method to swap two joint types in the skeleton when mirroring the avatar.
/// </summary>
/// <param name="left">The left joint type.</param>
/// <param name="right">The right joint type.</param>
private void SwapJointTypes(JointType left, JointType right)
{
for (int i = 0; i < this.jointConstraints.Count; i++)
{
BoneOrientationConstraint jc = this.jointConstraints[i];
if (jc.Joint == left)
{
jc.Joint = right;
}
else if (jc.Joint == right)
{
jc.Joint = left;
}
this.jointConstraints[i] = jc;
}
}
/// <summary>
/// Helper method to swap mirror the skeleton bone constraints when the skeleton is mirrored.
/// </summary>
private void MirrorConstraints()
{
this.SwapJointTypes(JointType.ShoulderLeft, JointType.ShoulderRight);
this.SwapJointTypes(JointType.ElbowLeft, JointType.ElbowRight);
this.SwapJointTypes(JointType.WristLeft, JointType.WristRight);
this.SwapJointTypes(JointType.HandLeft, JointType.HandRight);
this.SwapJointTypes(JointType.HipLeft, JointType.HipRight);
this.SwapJointTypes(JointType.KneeLeft, JointType.KneeRight);
this.SwapJointTypes(JointType.AnkleLeft, JointType.AnkleRight);
this.SwapJointTypes(JointType.FootLeft, JointType.FootRight);
for (int i = 0; i < this.jointConstraints.Count; i++)
{
BoneOrientationConstraint jc = this.jointConstraints[i];
// Here we negate the X axis to change the skeleton to mirror the user's movements.
jc.Dir.X = -jc.Dir.X;
this.jointConstraints[i] = jc;
}
this.constraintsMirrored = !this.constraintsMirrored;
}
// Apply the orientation constraints
public void Constrain(ref KinectInterop.BodyData bodyData)
{
KinectManager manager = KinectManager.Instance;
frameNum++;
for (int i = 0; i < jointConstraints.Count; i++)
{
BoneOrientationConstraint jc = this.jointConstraints[i];
if (jc.thisJoint == (int)KinectInterop.JointType.SpineBase || bodyData.joint[jc.thisJoint].normalRotation == Quaternion.identity)
{
continue;
}
if (bodyData.joint[jc.thisJoint].trackingState == KinectInterop.TrackingState.NotTracked)
{
continue;
}
int prevJoint = (int)manager.GetParentJoint((KinectInterop.JointType)jc.thisJoint);
if (bodyData.joint[prevJoint].trackingState == KinectInterop.TrackingState.NotTracked)
{
continue;
}
Quaternion rotParentN = bodyData.joint[prevJoint].normalRotation;
Quaternion rotDefaultN = Quaternion.FromToRotation(KinectInterop.JointBaseDir[prevJoint], KinectInterop.JointBaseDir[jc.thisJoint]);
rotParentN = rotParentN * rotDefaultN;
Quaternion rotJointN = bodyData.joint[jc.thisJoint].normalRotation;
Quaternion rotLocalN = Quaternion.Inverse(rotParentN) * rotJointN;
Vector3 eulerAnglesN = rotLocalN.eulerAngles;
// if(jc.thisJoint == (int)KinectInterop.JointType.KneeLeft)
// {
// float angle1X = eulerAnglesN.x <= 180f ? eulerAnglesN.x : eulerAnglesN.x - 360f;
// float angle1Y = eulerAnglesN.y <= 180f ? eulerAnglesN.y : eulerAnglesN.y - 360f;
// float angle1Z = eulerAnglesN.z <= 180f ? eulerAnglesN.z : eulerAnglesN.z - 360f;
//
// string sDebugText = string.Format("{0}. {1} - ({2:000}, {3:000}, {4:000})",
// frameNum, ((KinectInterop.JointType)jc.thisJoint).ToString(), angle1X, angle1Y, angle1Z);
//
// if(debugText != null && (Time.time - currentTime) >= 0.5f)
// {
// currentTime = Time.time;
// //debugText.GetComponent<GUIText>().text = sDebugText;
// }
//
// //Debug.Log(sDebugText);
// }
bool isConstrained = false;
string sDebug = string.Empty;
for (int a = 0; a < jc.axisConstrainrs.Count; a++)
{
AxisOrientationConstraint ac = jc.axisConstrainrs[a];
Quaternion rotLimited = rotLocalN;
switch (ac.consType)
{
case 0:
break;
case CT.LimA:
eulerAnglesN = LimitAngles(eulerAnglesN, ac.axis, ac.angleMin, ac.angleMax);
rotLimited = Quaternion.Euler(eulerAnglesN);
break;
case CT.LimST:
rotLimited = LimitSwing(rotLocalN, ac.axis, ac.angleMin);
rotLimited = LimitTwist(rotLimited, ac.axis, ac.angleMax);
break;
case CT.LimH:
float lastAngle = bodyData.joint[jc.thisJoint].lastAngle;
rotLimited = LimitHinge(rotLocalN, ac.axis, ac.angleMin, ac.angleMax, ref lastAngle);
bodyData.joint[jc.thisJoint].lastAngle = lastAngle;
break;
default:
throw new Exception("Undefined constraint type found: " + (int)ac.consType);
}
if (rotLimited != rotLocalN)
{
rotLocalN = rotLimited;
isConstrained = true;
}
}
if (sDebug.Length > 0)
{
// if(debugText != null && jc.thisJoint == (int)KinectInterop.JointType.ElbowLeft)
// {
// debugText.GetComponent<GUIText>().text = sDebug;
// }
Debug.Log(sDebug);
}
if (isConstrained)
{
rotJointN = rotParentN * rotLocalN;
Vector3 eulerJoint = rotJointN.eulerAngles;
Vector3 eulerJointM = new Vector3(eulerJoint.x, -eulerJoint.y, -eulerJoint.z);
Quaternion rotJointM = Quaternion.Euler(eulerJointM);
// put it back into the bone orientations
bodyData.joint[jc.thisJoint].normalRotation = rotJointN;
bodyData.joint[jc.thisJoint].mirroredRotation = rotJointM;
}
}
}
/// <summary>
/// AddJointConstraint - Adds a joint constraint to the system.
/// </summary>
/// <param name="joint">The skeleton joint/bone.</param>
/// <param name="dir">The absolute dir for the center of the constraint cone.</param>
/// <param name="angle">The angle of the constraint cone that the bone can move in.</param>
public void AddBoneOrientationConstraint(JointType joint, Vector3 dir, float angle)
{
this.constraintsMirrored = false;
BoneOrientationConstraint jc = new BoneOrientationConstraint(joint, dir, angle);
this.jointConstraints.Add(jc);
}
// 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;
}
}
// ApplyBoneOrientationConstraints and constrain rotations.
public void Constrain(ref KinectInterop.BodyData bodyData)
{
KinectManager manager = KinectManager.Instance;
for (int i = 0; i < this.jointConstraints.Count; i++)
{
BoneOrientationConstraint jc = this.jointConstraints[i];
if (jc.thisJoint == (int)KinectInterop.JointType.SpineBase || bodyData.joint[jc.thisJoint].normalRotation == Quaternion.identity)
{
continue;
}
if (bodyData.joint[jc.thisJoint].trackingState == KinectInterop.TrackingState.NotTracked)
{
continue;
}
int prevJoint = (int)manager.GetParentJoint((KinectInterop.JointType)jc.thisJoint);
if (bodyData.joint[prevJoint].trackingState == KinectInterop.TrackingState.NotTracked)
{
continue;
}
Quaternion rotJointN = bodyData.joint[jc.thisJoint].normalRotation;
Quaternion rotParentN = bodyData.joint[prevJoint].normalRotation;
Quaternion rotLocalN = Quaternion.Inverse(rotParentN) * rotJointN;
Vector3 eulerAnglesN = rotLocalN.eulerAngles;
Quaternion rotJointM = bodyData.joint[jc.thisJoint].mirroredRotation;
Quaternion rotParentM = bodyData.joint[prevJoint].mirroredRotation;
Quaternion rotLocalM = Quaternion.Inverse(rotParentM) * rotJointM;
Vector3 eulerAnglesM = rotLocalM.eulerAngles;
bool isConstrained = false;
for (int a = 0; a < jc.axisConstrainrs.Count; a++)
{
AxisOrientationConstraint ac = jc.axisConstrainrs[a];
Quaternion axisRotation = Quaternion.AngleAxis(eulerAnglesN[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))
{
// correct the axis that has fallen out of range.
if (angleFromMin > angleFromMax)
{
eulerAnglesN[ac.axis] = ac.angleMax;
}
else
{
eulerAnglesN[ac.axis] = ac.angleMin;
}
// fix mirrored rotation as well
if (ac.axis == 0)
{
eulerAnglesM[ac.axis] = eulerAnglesN[ac.axis];
}
else
{
eulerAnglesM[ac.axis] = -eulerAnglesN[ac.axis];
}
isConstrained = true;
}
}
if (isConstrained)
{
rotLocalN = Quaternion.Euler(eulerAnglesN);
rotJointN = rotParentN * rotLocalN;
rotLocalM = Quaternion.Euler(eulerAnglesM);
rotJointM = rotParentM * rotLocalM;
// Put it back into the bone directions
bodyData.joint[jc.thisJoint].normalRotation = rotJointN;
bodyData.joint[jc.thisJoint].mirroredRotation = rotJointM;
// dirJoint = constrainedRotation * dirParent;
// bodyData.joint[jc.thisJoint].direction = dirJoint;
}
}
}
// Apply the orientation constraints
public void Constrain(ref KinectInterop.BodyData bodyData)
{
KinectManager kinectManager = KinectManager.Instance;
frameNum++;
for (int i = 0; i < jointConstraints.Count; i++)
{
BoneOrientationConstraint jc = this.jointConstraints[i];
if (jc.thisJoint == (int)KinectInterop.JointType.Pelvis || bodyData.joint[jc.thisJoint].normalRotation == Quaternion.identity)
continue;
if (bodyData.joint[jc.thisJoint].trackingState == KinectInterop.TrackingState.NotTracked)
continue;
int prevJoint = (int)KinectInterop.GetParentJoint((KinectInterop.JointType)jc.thisJoint);
if (bodyData.joint[prevJoint].trackingState == KinectInterop.TrackingState.NotTracked)
continue;
Quaternion rotParentN = bodyData.joint[prevJoint].normalRotation;
//Quaternion rotDefaultN = Quaternion.identity; // Quaternion.FromToRotation(KinectInterop.JointBaseDir[prevJoint], KinectInterop.JointBaseDir[jc.thisJoint]);
//rotParentN = rotParentN * rotDefaultN;
Quaternion rotJointN = bodyData.joint[jc.thisJoint].normalRotation;
Quaternion rotLocalN = Quaternion.Inverse(rotParentN) * rotJointN;
Vector3 eulerAnglesN = rotLocalN.eulerAngles;
bool isConstrained = false;
//string sDebug = string.Empty;
for (int a = 0; a < jc.axisConstrainrs.Count; a++)
{
AxisOrientationConstraint ac = jc.axisConstrainrs[a];
Quaternion rotLimited = rotLocalN;
switch (ac.consType)
{
case 0:
break;
case CT.LimA:
eulerAnglesN = LimitAngles(eulerAnglesN, ac.axis, ac.angleMin, ac.angleMax);
rotLimited = Quaternion.Euler(eulerAnglesN);
break;
case CT.LimST:
rotLimited = LimitSwing(rotLocalN, ac.axis, ac.angleMin);
rotLimited = LimitTwist(rotLimited, ac.axis, ac.angleMax);
break;
case CT.LimH:
float lastAngle = bodyData.joint[jc.thisJoint].lastAngle;
rotLimited = LimitHinge(rotLocalN, ac.axis, ac.angleMin, ac.angleMax, ref lastAngle);
bodyData.joint[jc.thisJoint].lastAngle = lastAngle;
break;
default:
throw new Exception("Undefined constraint type found: " + (int)ac.consType);
}
if (rotLimited != rotLocalN)
{
rotLocalN = rotLimited;
isConstrained = true;
}
}
//if (sDebug.Length > 0)
//{
// if (debugText != null && jc.thisJoint == (int)KinectInterop.JointType.ElbowLeft)
// {
// // debugText.text = sDebug;
// }
// Debug.Log(sDebug);
//}
if (isConstrained)
{
rotJointN = rotParentN * rotLocalN;
Vector3 eulerJoint = rotJointN.eulerAngles;
Vector3 eulerJointM = new Vector3(eulerJoint.x, -eulerJoint.y, -eulerJoint.z);
Quaternion rotJointM = Quaternion.Euler(eulerJointM);
// put it back into the bone orientations
bodyData.joint[jc.thisJoint].normalRotation = rotJointN;
bodyData.joint[jc.thisJoint].mirroredRotation = rotJointM;
}
}
}
/// <summary>
/// Helper method to add the constraint cone for drawing.
/// </summary>
/// <param name="skeleton">The skeleton.</param>
/// <param name="seatedMode">Set true if in seated mode.</param>
private void AddConstraintDirectionCones(Skeleton skeleton, bool seatedMode)
{
// 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];
JointType joint = this.jointConstraints[i].Joint;
// Do not draw bone if not tracked
if (skeleton.Joints[joint].TrackingState == JointTrackingState.NotTracked)
{
continue;
}
// Do not draw the following bones if in seated mode
if (
seatedMode &&
(JointType.HipCenter == joint ||
JointType.Spine == joint ||
JointType.ShoulderCenter == joint ||
JointType.HipLeft == joint ||
JointType.KneeLeft == joint ||
JointType.AnkleLeft == joint ||
JointType.FootLeft == joint ||
JointType.HipRight == joint ||
JointType.KneeRight == joint ||
JointType.AnkleRight == joint ||
JointType.FootRight == joint))
{
continue;
}
// Get the constraint direction in world space from the parent orientation
Matrix matConstraintLocalToWorld = KinectHelper.Matrix4ToXNAMatrix(skeleton.BoneOrientations[skeleton.BoneOrientations[jc.Joint].StartJoint].AbsoluteRotation.Matrix);
Vector3 constraintDirWs = jc.Dir;
constraintDirWs.Normalize();
Vector3 rotatedConstraintDirWs = Vector3.Transform(constraintDirWs, matConstraintLocalToWorld);
rotatedConstraintDirWs.Normalize();
// Draw the constraint direction line itself
rotatedConstraintDirWs *= 0.5f;
this.AddRotatedLine(rotatedConstraintDirWs, skeleton, i, Color.DeepPink);
// Get the bone direction in world space
Vector3 boneDirWs = KinectHelper.VectorBetween(
skeleton,
skeleton.BoneOrientations[jc.Joint].StartJoint,
skeleton.BoneOrientations[jc.Joint].EndJoint);
boneDirWs.Normalize();
Quaternion constraintRotation = KinectHelper.GetShortestRotationBetweenVectors(constraintDirWs, Vector3.Up);
for (int c = 0; c < Tesselation; c++)
{
Vector3 circlePoint = GetCircleVector(c, Tesselation);
// Calculate distance based on known radius (opposite side) of 1 and angle
circlePoint.Y = 1.0f / (float)Math.Tan(MathHelper.ToRadians(jc.Angle));
circlePoint.Normalize();
Vector3 tranCirclePoint = Vector3.Transform(circlePoint, constraintRotation);
// now transform this by the parent rotation
Vector3 rotatedConstraintConePointWs = Vector3.Transform(tranCirclePoint, matConstraintLocalToWorld);
rotatedConstraintConePointWs *= LineScale; // scale
this.AddRotatedLine(rotatedConstraintConePointWs, skeleton, i, Color.HotPink);
}
}
}
/// <summary>
/// ApplyBoneOrientationConstraints and constrain rotations.
/// </summary>
/// <param name="skeleton">The skeleton to correct.</param>
/// <param name="mirrorView">Set this true if the skeleton joints are mirrored.</param>
public void Constrain(Skeleton skeleton, bool mirrorView)
{
if (null == skeleton || skeleton.TrackingState != SkeletonTrackingState.Tracked)
{
return;
}
if (this.jointConstraints.Count == 0)
{
this.AddDefaultConstraints();
}
if (mirrorView != this.constraintsMirrored)
{
this.MirrorConstraints();
}
// 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 (skeleton.Joints[jc.Joint].TrackingState == JointTrackingState.NotTracked || jc.Joint == JointType.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
JointType parentIdx = skeleton.BoneOrientations[jc.Joint].StartJoint;
// Local bone orientation relative to parent
Matrix boneOrientationRelativeToParent = KinectHelper.Matrix4ToXNAMatrix(skeleton.BoneOrientations[jc.Joint].HierarchicalRotation.Matrix);
Quaternion boneOrientationRelativeToParentQuat = KinectHelper.Vector4ToXNAQuaternion(skeleton.BoneOrientations[jc.Joint].HierarchicalRotation.Quaternion);
// Local bone direction is +Y vector in parent coordinate system
Vector3 boneRelDirVecLs = new Vector3(boneOrientationRelativeToParent.M21, boneOrientationRelativeToParent.M22, boneOrientationRelativeToParent.M23);
boneRelDirVecLs.Normalize();
// Constraint is relative to the parent orientation, which is +Y/identity relative rotation
Vector3 constraintDirLs = jc.Dir;
constraintDirLs.Normalize();
// Test this against the vector of the bone to find angle
float boneDotConstraint = Vector3.Dot(boneRelDirVecLs, constraintDirLs);
// Calculate the constraint value. 0.0 is in the center of the constraint cone, 1.0 and above are outside the cone.
jc.Constraint = (float)Math.Acos(boneDotConstraint) / MathHelper.ToRadians(jc.Angle);
this.jointConstraints[i] = jc;
// Slerp between identity and the inverse of the current constraint rotation by the amount over the constraint amount
if (jc.Constraint > 1)
{
Quaternion inverseRotation = Quaternion.Inverse(boneOrientationRelativeToParentQuat);
Quaternion slerpedInverseRotation = Quaternion.Slerp(Quaternion.Identity, inverseRotation, jc.Constraint - 1);
Quaternion constrainedRotation = boneOrientationRelativeToParentQuat * slerpedInverseRotation;
// Put it back into the bone orientations
skeleton.BoneOrientations[jc.Joint].HierarchicalRotation.Quaternion = KinectHelper.XNAQuaternionToVector4(constrainedRotation);
skeleton.BoneOrientations[jc.Joint].HierarchicalRotation.Matrix = KinectHelper.XNAMatrixToMatrix4(Matrix.CreateFromQuaternion(constrainedRotation));
}
}
// Recalculate the absolute rotations from the hierarchical relative rotations
Array jointTypeValues = Enum.GetValues(typeof(JointType));
foreach (JointType j in jointTypeValues)
{
if (j != JointType.HipCenter)
{
JointType parentIdx = skeleton.BoneOrientations[j].StartJoint;
// Calculate the absolute/world equivalents of the hierarchical rotation
Quaternion parentRotation = KinectHelper.Vector4ToXNAQuaternion(skeleton.BoneOrientations[parentIdx].AbsoluteRotation.Quaternion);
Quaternion relativeRotation = KinectHelper.Vector4ToXNAQuaternion(skeleton.BoneOrientations[j].HierarchicalRotation.Quaternion);
// Create a new world rotation
Quaternion worldRotation = Quaternion.Multiply(parentRotation, relativeRotation);
skeleton.BoneOrientations[j].AbsoluteRotation.Quaternion = KinectHelper.XNAQuaternionToVector4(worldRotation);
skeleton.BoneOrientations[j].AbsoluteRotation.Matrix = KinectHelper.XNAMatrixToMatrix4(Matrix.CreateFromQuaternion(worldRotation));
}
}
}
