Exemple #1
0
            // 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);
            }
Exemple #2
0
            /// <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);
            }