// Token: 0x06002730 RID: 10032 RVA: 0x000B5A1C File Offset: 0x000B3C1C
private Quaternion SingleDegree()
{
Vector3 target = GenericMath.TransformVector(this.axis, this.joint.transform.localRotation);
float num;
Vector3 rhs;
GenericMath.QuaternionToAngleAxis(GenericMath.ApplyQuaternion(GenericMath.RotateFromTo(this.axis, target), this.joint.localRotation), out num, out rhs);
float x = this.hingLimit.x;
float y = this.hingLimit.y;
float num2 = Vector3.Dot(this.axis, rhs);
num = GenericMath.Clamp(num * num2, x, y);
this.joint.localRotation = GenericMath.QuaternionFromAngleAxis(num, this.axis);
return(this.joint.localRotation);
}
/// <summary>
/// Process a 2 bones chain with a specific "epsilon" value
/// </summary>
/// <param name="chain"></param>
/// <param name="eps">a specific value, not bounded to the global Epsilon</param>
public static void Process(Core.Chain chain, float eps)
{
if (chain.Initiated == false)
{
chain.InitiateJoints();
}
if (chain.Joints.Count != 3)
{
Debug.LogError("The Analytical Solver only works with 3-joints(2 bones) chain configurations");
return;
}
Core.Joint A = chain.Joints[0];
Core.Joint B = chain.Joints[1];
Core.Joint C = chain.Joints[2];
Vector3 T = chain.GetIKTarget();
Vector3 AB = Vector3.Normalize(B.joint.position - A.joint.position);
Vector3 AC = Vector3.Normalize(C.joint.position - A.joint.position);
Vector3 CB = Vector3.Normalize(B.joint.position - C.joint.position);
Vector3 TA = A.joint.position - T;
float l_ab = A.length;
float l_cb = B.length;
float l_at = GenericMath.Clamp(TA.magnitude, eps, l_ab + l_cb - eps);
float kneeCurrent = GenericMath.VectorsAngle(AB, CB);
float kneeTarget = GenericMath.CosineRule(A.length, B.length, l_at);
float kneeDelta = kneeTarget - kneeCurrent;
Vector3 axis = GenericMath.TransformVector(Vector3.Normalize(Vector3.Cross(AC, AB)),
Quaternion.Inverse(B.joint.rotation));
Quaternion q1 = Quaternion.AngleAxis(kneeDelta, axis);
Quaternion knee = Quaternion.Lerp(B.joint.rotation, GenericMath.ApplyQuaternion(B.joint.rotation, q1),
chain.Weight);
B.joint.rotation = knee;
Quaternion q2 = Quaternion.FromToRotation(A.joint.position - C.joint.position, TA);
Quaternion thigh = Quaternion.Lerp(A.joint.rotation, GenericMath.ApplyQuaternion(q2, A.joint.rotation),
chain.Weight);
A.joint.rotation = thigh;
}
/// <summary>
/// Limit the motion to 1 Degree of freedom
/// </summary>
/// <param name="_localRot"></param>
/// <returns></returns>
private Quaternion SingleDegree()
{
float angle;
Vector3 axis;
//Hinge only Quaternion
Vector3 _localAxis = GenericMath.TransformVector(this.axis, joint.transform.localRotation);
Quaternion _delta = GenericMath.RotateFromTo(this.axis, _localAxis);
Quaternion _legalRot = GenericMath.ApplyQuaternion(_delta, joint.localRotation);
GenericMath.QuaternionToAngleAxis(_legalRot, out angle, out axis);
float min = hingLimit.x;
float max = hingLimit.y;
float dot = Vector3.Dot(this.axis, axis);
//clamp values
//angle = Mathf.Clamp(angle * dot, min, max); //Unity's Clamp gives NaN values in particular cases so use our own
angle = GenericMath.Clamp(angle * dot, min, max);
joint.localRotation = GenericMath.QuaternionFromAngleAxis(angle, this.axis);
return(joint.localRotation);
}