// 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;
}
}
}
