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