// AddJointConstraint - Adds a joint constraint to the system. public void AddBoneOrientationConstraint(int thisJoint, ConstraintAxis 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(axis, angleMin, angleMax); jc.axisConstrainrs.Add(constraint); jointConstraints[index] = 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; }
// 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; } } }
// 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; } } }