Beispiel #1
0
        /// <summary>
        /// correct the orientation of the bones after the solve
        /// </summary>
        private void CorrectRotation()
        {
            for (int i = 0; i < chain.joints.Count - 1; i++)
            {
                //apply the new pos
                chain.joints[i].transform.position = chain.joints[i].solvePos;

                Vector3    _targetDir = chain.joints[i + 1].transform.position - chain.joints[i].transform.position;
                Quaternion _sourceRot = chain.joints[i].transform.rotation;
                Quaternion _targetRot = RootIK.RotateFromTo(RootIK.TransformVector(Vector3.up, _sourceRot), _targetDir);

                _targetRot = Quaternion.Inverse(_targetRot);
                chain.joints[i].transform.rotation = Quaternion.Lerp(_sourceRot, GenericMaths.ApplyQuaternion(_targetRot, _sourceRot), chain.weight);
            }

            //Bounce back to the 1st bone before the endeffector and correct its rotation
            Vector3    _t  = chain.GetIKPosition() - chain.joints[chain.joints.Count - 2].transform.position;
            Vector3    _t2 = RootIK.TransformVector(Vector3.up, chain.joints[chain.joints.Count - 2].transform.rotation);
            Quaternion _s  = chain.joints[chain.joints.Count - 2].transform.rotation;
            Quaternion _tr = RootIK.RotateFromTo(_t, _t2);

            chain.joints[chain.joints.Count - 2].transform.rotation = Quaternion.Lerp(_s, GenericMaths.ApplyQuaternion(_tr, _s), chain.weight);

            chain.joints[chain.joints.Count - 1].transform.rotation = chain.GetIKRotation();   //apply end effector rotation
        }
        /// <summary>
        /// Prepare the solver and solve the IK problem analytically with specific axis of rotation
        /// (the chain must contain 2 joints, no more and no less)
        /// </summary>
        /// <param name="_hingeChain">the chain</param>
        /// <param name="_direction">direction of the player</param>
        /// <param name="_axis">axis of rotation for the 2nd joint (the Hinge joint)</param>
        public void SolveAnalytically(RootIK.Chain _hingeChain, Vector3 _direction, Vector3 _axis)
        {
            if (_hingeChain.joints.Count > 3)
            {
                return;
            }
            if (_hingeChain.iterations <= 0)
            {
                return;
            }
            if (_hingeChain.joints.Count <= 0)
            {
                return;
            }


            //calculate bone length;
            upperLength  = Vector3.Distance(_hingeChain.joints[0].transform.position, _hingeChain.joints[1].transform.position);
            lowerLength  = Vector3.Distance(_hingeChain.joints[1].transform.position, _hingeChain.joints[2].transform.position);
            systemLength = Vector3.Distance(_hingeChain.joints[0].transform.position, _hingeChain.GetIKPosition());

            //lowerjoint 1DOF
            float _angle = GenericMaths.Formula(upperLength, lowerLength, systemLength) + Mathf.PI * Mathf.Rad2Deg;

            if (_axis == Vector3.zero)
            {
                _axis = Vector3.Cross(RootIK.TransformVector(Vector3.up, _hingeChain.joints[1].transform.rotation), _direction);
            }
            else
            {
                _axis = Vector3.Cross(RootIK.TransformVector(_axis, _hingeChain.joints[1].transform.rotation), _direction);
            }
            Quaternion _src      = _hingeChain.joints[1].transform.rotation;
            Quaternion _t        = GenericMaths.QuaternionFromAngleAxis(_axis, _angle + Mathf.Acos(Mathf.Clamp(_src.w, -1f, 1f)) * 10f);
            Quaternion _finalRot = Quaternion.Lerp(Quaternion.identity, _t, _hingeChain.weight);

            _hingeChain.joints[1].transform.rotation = GenericMaths.ApplyQuaternion(_finalRot, _src);


            //Upperjoint 3DOF
            Vector3    _v1        = _hingeChain.GetIKPosition() - _hingeChain.joints[0].transform.position;
            Vector3    _v2        = _hingeChain.joints[2].transform.position - _hingeChain.joints[0].transform.position;
            Quaternion _src2      = _hingeChain.joints[0].transform.rotation;
            Quaternion _t2        = RootIK.RotateFromTo(_v2, _v1);
            Quaternion _finalRot2 = Quaternion.Lerp(Quaternion.identity, _t2, _hingeChain.weight);

            _hingeChain.joints[0].transform.rotation = GenericMaths.ApplyQuaternion(Quaternion.Inverse(_finalRot2), _src2);

            //foot 6DOF
            _hingeChain.GetEndEffector().rotation = _hingeChain.GetIKRotation();
        }
Beispiel #3
0
        /// <summary>
        /// Solve the KinematicBone
        /// </summary>
        /// <param name="_bone"></param>
        public void SolveBone(RootIK.KinematicBone _bone)
        {
            if (_bone.weight <= 0)
            {
                return;
            }
            if (_bone.target == null)
            {
                Debug.LogError("No target was assigned");
                return;
            }

            Vector3    _v0        = RootIK.TransformVector(_bone.axis == Vector3.zero ? Vector3.forward : _bone.axis, _bone.bone.rotation);
            Vector3    _v1        = _bone.target.position - _bone.bone.position;
            Quaternion _targetRot = Quaternion.Lerp(Quaternion.identity, RootIK.RotateFromTo(_v0, _v1), _bone.weight);

            _targetRot          = Quaternion.Inverse(_targetRot);
            _bone.bone.rotation = GenericMaths.ApplyQuaternion(_targetRot, _bone.bone.rotation);
        }
        private void FindGround()
        {
            for (int i = 0; i < 2; i++)
            {
                Vector3    _endEffector = i == 0 ? rightLeg.GetEndEffector().position : leftLeg.GetEndEffector().position;
                Ray        _ray         = new Ray(_endEffector + Vector3.up * maxStep, Vector3.down);
                RaycastHit _hit         = new RaycastHit();

                if (Physics.Raycast(_ray, out _hit, rayLength, solverLayer))
                {
                    Quaternion _ankleRot = RootIK.RotateFromTo(_hit.normal, RootIK.TransformVector(Vector3.up, Root().rotation));

                    rightLeg.SetIKRotation(i == 0 ? GenericMaths.ApplyQuaternion(_ankleRot, rightLeg.GetEndEffector().rotation) : rightLeg.GetIKRotation());
                    leftLeg.SetIKRotation(i == 1 ? GenericMaths.ApplyQuaternion(_ankleRot, leftLeg.GetEndEffector().rotation) : leftLeg.GetIKRotation());

                    IKTarget[i] = _hit.point;
                }
            }

            rightLeg.SetIKPosition(IKTarget[0]);
            leftLeg.SetIKPosition(IKTarget[1]);
        }
        /// <summary>
        /// Solve the IK chain using CCD method
        /// </summary>
        /// <param name="_IKChain"></param>
        public void SolveCCD(RootIK.Chain _IKChain)
        {
            if (_IKChain.weight <= 0f)
            {
                return;
            }
            if (_IKChain.joints.Count <= 0)
            {
                return;
            }

            _IKChain.SetIKPosition(_IKChain.target ? Vector3.zero : _IKChain.GetIKPosition());

            for (int j = 0; j < _IKChain.iterations; j++)
            {
                for (int i = _IKChain.joints.Count - 1; i >= 0; i--)
                {
                    _IKChain.weight = Mathf.Clamp(_IKChain.weight, 0f, 1f);

                    float _weight = _IKChain.weight * _IKChain.joints[i].weight;

                    Vector3 _v0 = _IKChain.GetIKPosition() - _IKChain.joints[i].transform.position;
                    Vector3 _v1 = _IKChain.joints[_IKChain.joints.Count - 1].transform.position - _IKChain.joints[i].transform.position;

                    Quaternion _sourceRotation = _IKChain.joints[i].transform.rotation;
                    Quaternion _targetRotation = Quaternion.Lerp(Quaternion.identity, RootIK.RotateFromTo(_v0, _v1), _weight);

                    _IKChain.joints[i].transform.rotation = Quaternion.Lerp(_sourceRotation, GenericMaths.ApplyQuaternion(_targetRotation, _sourceRotation), _weight);
                }
            }

            _IKChain.joints[_IKChain.joints.Count - 1].transform.rotation = _IKChain.GetIKRotation();
        }