//Apply the hinge rotation limit
private Quat LimitHinge(Quat rotation)
{
// If limit is zero return rotation fixed to axis
if (min == 0 && max == 0 && useLimits)
{
return(Quat.AngleAxis(0, axis));
}
// Get 1 degree of freedom rotation along axis
Quat free1DOF = Limit1DOF(rotation, axis);
if (!useLimits)
{
return(free1DOF);
}
// Get offset from last rotation in angle-axis representation
Quat addR = free1DOF * Quat.Inverse(lastRotation);
float addA = Quat.Angle(Quat.identity, addR);
Vec3 secondaryAxis = new Vec3(axis.z, axis.x, axis.y);
Vec3 cross = secondaryAxis.CrossProduct(axis);
if (cross.DotProduct(addR * secondaryAxis) > 0f)
{
addA = -addA;
}
// Clamp to limits
lastAngle = Mathf.Clamp(lastAngle + addA, min, max);
return(Quat.AngleAxis(lastAngle, axis));
}
/// <summary>
/// <para>Rotates a rotation /from/ towards /to/.</para>
/// </summary>
/// <param name="from"></param>
/// <param name="to"></param>
/// <param name="maxDegreesDelta"></param>
public static Quat RotateTowards(Quat from, Quat to, float maxDegreesDelta)
{
float num = Quat.Angle(from, to);
if (num == 0f)
{
return(to);
}
float t = Mathf.Min(1f, maxDegreesDelta / num);
return(Quat.SlerpUnclamped(from, to, t));
}
public static Quat RotateTowards(Quat from, Quat to, float maxDegreesDelta)
{
float num = Quat.Angle(from, to);
Quat result;
if (num == 0f)
{
result = to;
}
else
{
float t = Mathf.Min(1f, maxDegreesDelta / num);
result = Quat.Slerp(from, to, t);
}
return(result);
}
public void Quat4()
{
FQuat fq = FQuat.Euler(( Fix64 )45, ( Fix64 )(-23), ( Fix64 )(-48.88));
FQuat fq2 = FQuat.Euler(( Fix64 )23, ( Fix64 )(-78), ( Fix64 )(-132.43f));
Quat q = Quat.Euler(45, -23, -48.88f);
Quat q2 = Quat.Euler(23, -78, -132.43f);
FVec3 fv = new FVec3(12.5f, 9, 8);
FVec3 fv2 = new FVec3(1, 0, 0);
Vec3 v = new Vec3(12.5f, 9, 8);
Vec3 v2 = new Vec3(1, 0, 0);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
this._output.WriteLine(fq2.ToString());
this._output.WriteLine(q2.ToString());
Fix64 fa = FQuat.Angle(fq, fq2);
float a = Quat.Angle(q, q2);
this._output.WriteLine(fa.ToString());
this._output.WriteLine(a.ToString());
fq = FQuat.AngleAxis(( Fix64 )(-123.324), fv);
q = Quat.AngleAxis(-123.324f, v);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fa = FQuat.Dot(fq, fq2);
a = Quat.Dot(q, q2);
this._output.WriteLine(fa.ToString());
this._output.WriteLine(a.ToString());
fq = FQuat.FromToRotation(FVec3.Normalize(fv), fv2);
q = Quat.FromToRotation(Vec3.Normalize(v), v2);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fq = FQuat.Lerp(fq, fq2, ( Fix64 )0.66);
q = Quat.Lerp(q, q2, 0.66f);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fq = FQuat.Normalize(fq);
q.Normalize();
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fq.Inverse();
q = Quat.Inverse(q);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fv = FQuat.Orthogonal(fv);
v = Quat.Orthogonal(v);
this._output.WriteLine(fv.ToString());
this._output.WriteLine(v.ToString());
fq = FQuat.Slerp(fq, fq2, ( Fix64 )0.66);
q = Quat.Slerp(q, q2, 0.66f);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fq = FQuat.LookRotation(FVec3.Normalize(fv), fv2);
q = Quat.LookRotation(Vec3.Normalize(v), v2);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fq.ToAngleAxis(out fa, out fv);
q.ToAngleAxis(out a, out v);
this._output.WriteLine(fa.ToString());
this._output.WriteLine(a.ToString());
this._output.WriteLine(fv.ToString());
this._output.WriteLine(v.ToString());
fq = fq.Conjugate();
q = q.Conjugate();
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
fq.SetLookRotation(FVec3.Normalize(fv), fv2);
q.SetLookRotation(Vec3.Normalize(v), v2);
this._output.WriteLine(fq.ToString());
this._output.WriteLine(q.ToString());
}
// 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)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 = GetSkeletonJointParent(jc.joint);
// Local bone orientation relative to parent
Matrix4x4 localOrientationMatrix = Invert(jointOrientations[parentIdx]) * jointOrientations[jc.joint];
Vector3 localOrientationZ = (Vector3)localOrientationMatrix.GetColumn(2);
Vector3 localOrientationY = (Vector3)localOrientationMatrix.GetColumn(1);
if (localOrientationZ == Vector3.zero || localOrientationY == Vector3.zero)
{
continue;
}
Quat localRotation = Quat.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];
Quat axisRotation = Quat.AngleAxis(localRotation.eulerAngles[ac.axis], ac.rotateAround);
//Quaternion axisRotation = Quaternion.AngleAxis(globalRotation.eulerAngles[ac.axis], ac.rotateAround);
float angleFromMin = Quat.Angle(axisRotation, quatToQuat2(ac.minQuaternion));
float angleFromMax = Quat.Angle(axisRotation, quatToQuat2(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)
{
Quat constrainedRotation = Quat.Euler(eulerAngles);
// Put it back into the bone orientations
localOrientationMatrix = Transform(localOrientationMatrix, quatToQuat(constrainedRotation));
jointOrientations[jc.joint] = jointOrientations[parentIdx] * localOrientationMatrix;
//globalOrientationMatrix.SetTRS(Vector3.zero, constrainedRotation, Vector3.one);
//jointOrientations[jc.joint] = globalOrientationMatrix;
switch (jc.joint)
{
case (int)NuiSkeletonPositionIndex.ShoulderCenter:
jointOrientations[(int)NuiSkeletonPositionIndex.Head] = jointOrientations[jc.joint];
break;
case (int)NuiSkeletonPositionIndex.WristLeft:
jointOrientations[(int)NuiSkeletonPositionIndex.HandLeft] = jointOrientations[jc.joint];
break;
case (int)NuiSkeletonPositionIndex.WristRight:
jointOrientations[(int)NuiSkeletonPositionIndex.HandRight] = jointOrientations[jc.joint];
break;
case (int)NuiSkeletonPositionIndex.AnkleLeft:
jointOrientations[(int)NuiSkeletonPositionIndex.FootLeft] = jointOrientations[jc.joint];
break;
case (int)NuiSkeletonPositionIndex.AnkleRight:
jointOrientations[(int)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;
}
}