// Token: 0x0600272F RID: 10031 RVA: 0x000B58E8 File Offset: 0x000B3AE8
private Quaternion TwistAndSwing()
{
object obj = new Func <Quaternion, float, Quaternion>(delegate(Quaternion q, float x)
{
if (x == 0f)
{
return(Quaternion.identity);
}
float num2 = GenericMath.QuaternionAngle(Quaternion.identity, q);
float t = Mathf.Clamp01(x / num2);
return(Quaternion.Slerp(Quaternion.identity, q, t));
});
Func <float, float> func = (float x) => x * x;
Quaternion quaternion = GenericMath.RotateFromTo(GenericMath.TransformVector(this.axis, this.joint.localRotation), this.axis);
object obj2 = obj;
Quaternion qB = obj2(quaternion, this.maxAngle);
Quaternion quaternion2 = GenericMath.ApplyQuaternion(Quaternion.Inverse(quaternion), this.joint.localRotation);
float num = Mathf.Sqrt(func(quaternion2.w) + func(quaternion2.x) + func(quaternion2.y) + func(quaternion2.z));
float w = quaternion2.w / num;
float x2 = quaternion2.x / num;
float y = quaternion2.y / num;
float z = quaternion2.z / num;
Quaternion qA = obj2(new Quaternion(x2, y, z, w), this.maxTwist);
this.joint.localRotation = GenericMath.ApplyQuaternion(qA, qB);
return(this.joint.localRotation);
}
/// <summary>
/// The Full-Restricted motion limit
/// </summary>
/// <param name="_localRot"></param>
/// <returns></returns>
private Quaternion TwistAndSwing()
{
Func <Quaternion, float, Quaternion> LimitByAngle = (q, x) =>
{
if (x == 0)
{
return(Quaternion.identity);
}
float angle = GenericMath.QuaternionAngle(Quaternion.identity, q);
float t = Mathf.Clamp01(x / angle);
Quaternion output = Quaternion.Slerp(Quaternion.identity, q, t); //lerp doesnt work :(
return(output);
};
Func <float, float> Sqr = x => x * x;
Vector3 _localAxis = GenericMath.TransformVector(axis, joint.localRotation);
//swing only quaternion
Quaternion swing = GenericMath.RotateFromTo(_localAxis, axis);
Quaternion limitedSwing = LimitByAngle(swing, maxAngle);
//twist only quaternion
Quaternion twist = GenericMath.ApplyQuaternion(Quaternion.Inverse(swing), joint.localRotation);
//twist decomposition
float qM = Mathf.Sqrt(Sqr(twist.w) + Sqr(twist.x) + Sqr(twist.y) + Sqr(twist.z));
float qw = twist.w / qM;
float qx = twist.x / qM;
float qy = twist.y / qM;
float qz = twist.z / qM;
Quaternion limitedTwist = LimitByAngle(new Quaternion(qx, qy, qz, qw), maxTwist);
joint.localRotation = GenericMath.ApplyQuaternion(limitedTwist, limitedSwing);
return(joint.localRotation);
}