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