// 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)JointType.SpineBase || bodyData.joint[jc.thisJoint].normalRotation == Quaternion.identity) { continue; } if (bodyData.joint[jc.thisJoint].trackingState == TrackingState.NotTracked) { continue; } int prevJoint = (int)KinectInterop.GetParentJoint((JointType)jc.thisJoint); if (bodyData.joint[prevJoint].trackingState == 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; } } }