예제 #1
0
        /// <summary>
        /// given the end joint and the IK link length, setup the IK link
        /// </summary>
        public void SetBones(Transform endJoint, int len)
        {
            Dbg.Assert(len > 0, "BaseIKSolver.SetBones: the link length must be larger than 0");

            ClearBones();

            // the endJoint is the joint user moves, the link's last joint is its parent
            Transform joint    = endJoint;
            float     totalLen = 0;

            // add the endJoint
            JointInfo endInfo = new JointInfo();

            endInfo.joint     = endJoint;
            endInfo.boneLen   = 0;
            endInfo.remainLen = 0;
            m_Joints.Add(endInfo);

            // add joint from the end to the head
            for (int idx = 0; idx < len; ++idx)
            {
                Transform parentJoint = joint.parent;
                Dbg.Assert(parentJoint != null, "BaseIKSolver.SetBones: the link length is too big, there is already no parent joint for: {0}", joint);

                JointInfo info = new JointInfo();
                info.joint     = parentJoint;
                info.boneLen   = (parentJoint.position - joint.position).magnitude;
                info.remainLen = totalLen;

                totalLen += info.boneLen;

                m_Joints.Add(info);
                joint = parentJoint;
            }

            m_Joints.Reverse(); //reverse the array to make the most significant joint at first entry

            m_TransArr = new Transform[m_Joints.Count];
            for (int i = 0; i < m_TransArr.Length; ++i)
            {
                m_TransArr[i] = m_Joints[i].joint;
            }
        }
예제 #2
0
        /// <summary>
        /// execute calculation,
        /// will set the value for each Bones
        /// </summary>
        public IKExecRes Execute(float weight = 1.0f)
        {
            Dbg.Assert(m_Joints.Count > 0, "BaseIKSolver.Execute: the Joints list is empty!");

            bool bSuccess = false;

            //////////////////////////////////////////////////
            // from the most significant joint to the end effector
            // don't count the endJoint
            //////////////////////////////////////////////////
            for (int idx = 0; idx < this.Count; ++idx)
            {
                JointInfo jointInfo  = m_Joints[idx];
                Transform joint      = jointInfo.joint;
                Transform childJoint = m_Joints[idx + 1].joint;
                float     len_a      = jointInfo.boneLen;
                float     len_b      = jointInfo.remainLen;

                Vector3 vec_c = m_TargetPos - joint.position;
                float   len_c = vec_c.magnitude;

                // normalize both vec_a and vec_c
                vec_c.Normalize(); // vec_c is the direction with len = 1
                Vector3 vec_a = (childJoint.position - joint.position).normalized;

                // rotate the plane to contain IKtarget if there're still grandchild joint
                if (idx < this.Count - 1 && len_a + len_b > len_c && vec_c != Vector3.zero)
                {
                    Transform grandChildJoint = m_Joints[idx + 2].joint;
                    Vector3   vec_d           = (grandChildJoint.position - childJoint.position).normalized;

                    Vector3 n1 = Vector3.Cross(vec_a, vec_d);
                    Vector3 n2 = Vector3.Cross(vec_a, vec_c);

                    //don't do rotation if joint0-1-2 is almost in a line
                    if (n1.sqrMagnitude > PLANE_ROTATE_THRES && n2.sqrMagnitude > PLANE_ROTATE_THRES)
                    {
                        Quaternion q = Quaternion.FromToRotation(n1, n2);
                        joint.rotation = q * joint.rotation;

                        // need to fix vec_a
                        vec_a = (childJoint.position - joint.position).normalized;
                    }
                }

                // special condition fix
                if (vec_a == Vector3.zero)
                {
                    Dbg.LogWarn("What the?...childJoint at same pos with parentJoint!? : joint: {0}, childJoint: {1}", joint, childJoint);
                }
                if (vec_c == Vector3.zero)
                {
                    vec_c = vec_a;
                }

                // main work
                if (len_c >= len_a + len_b || //target is too far
                    idx == this.Count - 1     //the last joint should just look towards targetPos
                    )
                {
                    //IKSolverUtil.JointLookAt(joint, childJoint, vec_c, ref vec_a, len_a);
                    IKSolverUtil.JointLookAt(joint, vec_c, vec_a);
                }
                else if (len_c <= Mathf.Abs(len_a - len_b)) //target is too near
                {
                    //IKSolverUtil.JointLookAt(joint, childJoint, -vec_c, ref vec_a, len_a);
                    IKSolverUtil.JointLookAt(joint, vec_c, vec_a);
                }
                else
                {
                    float radian_b = Mathf.Acos((len_a * len_a + len_c * len_c - len_b * len_b) / (2 * len_a * len_c));
                    float vv       = Vector3.Dot(vec_a, vec_c);

                    vv = Mathf.Clamp(vv, -1f, 1f); //precision error protection, the direct result of dotP might cause Acos return NaN
                    float radian_rot = Mathf.Acos(vv) - radian_b;
                    if (float.IsNaN(radian_rot))
                    {
                        Dbg.LogWarn("NaN???!!!Not Again...");
                    }
                    float angle_rot = radian_rot * Mathf.Rad2Deg;

                    Vector3 rotAxis;
                    if (vec_a == vec_c || vec_a == -vec_c)
                    {
                        rotAxis = joint.up;
                        joint.Rotate(rotAxis, angle_rot, Space.World);
                    }
                    else
                    {
                        rotAxis = Vector3.Cross(vec_a, vec_c);
                        joint.Rotate(rotAxis, angle_rot, Space.World);
                    }
                }

                if (idx == this.Count - 1 && Mathf.Approximately(len_c, len_a))
                {
                    bSuccess = true;
                }
            }

            return(bSuccess ? IKExecRes.SUCCESS : IKExecRes.UNREACH_INLIMIT);
        }
예제 #3
0
        /// <summary>
        /// execute calculation,
        /// will set the value for each Bones
        /// </summary>
        public bool Execute(float weight = 1.0f)
        {
            Dbg.Assert(m_Joints.Count > 0, "IKRTSolver.FixRotation: the Joints list is empty!");

            //////////////////////////////////////////////////////////////////////////
            // cache the Transform data
            //////////////////////////////////////////////////////////////////////////
            for (int idx = 0; idx < this.Count; ++idx)
            {
                m_CachedTRS[idx].CopyFrom(m_Joints[idx].joint);
            }

            bool bSuccess = false;

            //////////////////////////////////////////////////
            // from the most significant joint to the end effector
            // don't count the endJoint
            //////////////////////////////////////////////////
            for (int idx = 0; idx < this.Count; ++idx)
            {
                JointInfo jointInfo  = m_Joints[idx];
                Transform joint      = jointInfo.joint;
                Transform childJoint = m_Joints[idx + 1].joint;
                float     len_a      = jointInfo.boneLen;
                float     len_b      = jointInfo.remainLen;

                Vector3 vec_c = m_TargetPos - joint.position;
                float   len_c = vec_c.magnitude;

                // normalize both vec_a and vec_c
                vec_c.Normalize(); // vec_c is the direction with len = 1
                Vector3 vec_a = (childJoint.position - joint.position).normalized;

                //// rotate the plane to contain IKtarget if there're still grandchild joint
                //if (idx < this.Count - 1 && vec_c != Vector3.zero)
                //{
                //    Transform grandChildJoint = m_Joints[idx + 2].joint;
                //    Vector3 vec_d = (grandChildJoint.position - childJoint.position).normalized;

                //    Vector3 n1 = Vector3.Cross(vec_a, vec_d);
                //    Vector3 n2 = Vector3.Cross(vec_a, vec_c);

                //    //don't do rotation if joint0-1-2 is almost in a line
                //    if (n1.sqrMagnitude > PLANE_ROTATE_THRES && n2.sqrMagnitude > PLANE_ROTATE_THRES)
                //    {
                //        Quaternion q = Quaternion.FromToRotation(n1, n2);
                //        joint.rotation = q * joint.rotation;

                //        // need to fix vec_a
                //        vec_a = (childJoint.position - joint.position).normalized;
                //    }
                //}

                // special condition fix
                if (vec_a == Vector3.zero)
                {
                    Dbg.LogWarn("WTH...vec_a is zero? : joint: {0}, childJoint: {1}", joint, childJoint);
                }
                if (vec_c == Vector3.zero)
                {
                    vec_c = vec_a;
                }

                // main work
                if (len_c >= len_a + len_b || //target is too far
                    idx == this.Count - 1     //the last joint should just look towards targetPos
                    )
                {
                    //_JointLookAt(joint, childJoint, vec_c, ref vec_a, len_a);
                    _JointLookAt(joint, vec_c, vec_a);
                }
                else if (len_c <= Mathf.Abs(len_a - len_b)) //target is too near
                {
                    //_JointLookAt(joint, childJoint, -vec_c, ref vec_a, len_a);
                    _JointLookAt(joint, vec_c, vec_a);
                }
                else
                {
                    float radian_b = Mathf.Acos((len_a * len_a + len_c * len_c - len_b * len_b) / (2 * len_a * len_c));
                    float vv       = Vector3.Dot(vec_a, vec_c);

                    vv = Mathf.Clamp(vv, -1f, 1f); //precision error protection, the direct result of dotP might cause Acos return NaN
                    float radian_rot = Mathf.Acos(vv) - radian_b;
                    if (float.IsNaN(radian_rot))
                    {
                        Dbg.LogWarn("NaN???!!!Not Again...");
                    }
                    float angle_rot = radian_rot * Mathf.Rad2Deg;

                    Vector3 rotAxis;
                    if (vec_a == vec_c || vec_a == -vec_c)
                    {
                        rotAxis = joint.up;
                        joint.Rotate(rotAxis, angle_rot, Space.World);
                    }
                    else
                    {
                        rotAxis = Vector3.Cross(vec_a, vec_c);
                        joint.Rotate(rotAxis, angle_rot, Space.World);
                    }
                }

                //Dbg.Log("BeforeFix: localRotation: {0}", joint.localEulerAngles);
                //joint.localRotation = m_Limits[idx].FixRotation(joint.localRotation);
                //Dbg.Log("AfterFix: localRotation: {0}", joint.localEulerAngles);


                if (idx == this.Count - 1 && Mathf.Approximately(len_c, len_a))
                {
                    bSuccess = true;
                }
            }

            //////////////////////////////////////////////////
            // lerp with weight
            //////////////////////////////////////////////////
            for (int idx = 0; idx < m_CachedTRS.Count; ++idx)
            {
                m_CachedTRS[idx].Apply(m_Joints[idx].joint, weight);
            }

            return(bSuccess);
        }
예제 #4
0
        /// <summary>
        /// execute calculation,
        /// will set the value for each Bones
        /// </summary>
        public IKExecRes Execute(float weight = 1.0f)
        {
            ++m_cnter;
            Dbg.Assert(m_Joints.Count > 0, "IKLimbSolver.Execute: the Joints list is empty!");

            bool bSuccess = false;

            //////////////////////////////////////////////////
            // from the most significant joint to the end effector
            // don't count the endJoint
            //////////////////////////////////////////////////
            for (int idx = 0; idx < this.Count; ++idx)
            {
                JointInfo jointInfo  = m_Joints[idx];
                Transform joint      = jointInfo.joint;
                Transform childJoint = m_Joints[idx + 1].joint;
                float     len_a      = jointInfo.boneLen;
                float     len_b      = jointInfo.remainLen;

                Vector3 vec_c = m_TargetPos - joint.position;
                float   len_c = vec_c.magnitude;

                // normalize both vec_a and vec_c
                vec_c.Normalize(); // vec_c is the direction with len = 1
                Vector3 vec_a = (childJoint.position - joint.position).normalized;

                // rotate the plane to contain IKtarget if there're still grandchild joint
                //NOTE1: "len_a+len_b > len_c" test is important to prevent rootJoint jitter when limb is straight
                if (idx == 0 && vec_c != Vector3.zero && len_a + len_b > len_c && !IKSolverUtil.DirNear(vec_a, vec_c))
                {
                    Vector3 n1 = joint.TransformDirection(m_Constraint.RotateAxis);
                    Vector3 n2 = Vector3.Cross(vec_c, vec_a).normalized;

                    //Quaternion q = Quaternion.FromToRotation(n1, n2);
                    Quaternion q1 = Quaternion.LookRotation(n1);
                    Quaternion q2 = Quaternion.LookRotation(n2);
                    Quaternion q  = q2 * Quaternion.Inverse(q1);


                    joint.rotation = q * joint.rotation;
                    // need to fix vec_a
                    vec_a = (childJoint.position - joint.position).normalized;


                #if IMDEBUGGING
                    float angle = Quaternion.Angle(q1, q2);
                    Dbg.Log("{0}, Align Plane: rot: {1}, angle: {2}, n1: {3}, n2:{4}", m_cnter, joint.rotation, angle, n1, n2);
                #endif
                }

                // special condition fix
                if (vec_a == Vector3.zero)
                {
                    Dbg.LogWarn("What the?...childJoint at same pos with parentJoint!? : joint: {0}, childJoint: {1}", joint, childJoint);
                }
                if (vec_c == Vector3.zero)
                {
                    vec_c = vec_a;
                }

                // main work
                if (len_c >= len_a + len_b || //target is too far
                    idx == this.Count - 1     //the last joint should just look towards targetPos
                    )
                {
                #if IMDEBUGGING
                    Quaternion qold = joint.rotation;
                #endif
                    IKSolverUtil.JointLookAt(joint, vec_c, vec_a);
                #if IMDEBUGGING
                    if (idx == 0)
                    {
                        Dbg.Log("{0}, LookAt1: oldRot: {1}, newRot: {2}", m_cnter, qold, joint.rotation);
                    }
                #endif
                }
                else if (len_c <= Mathf.Abs(len_a - len_b)) //target is too near
                {
                    IKSolverUtil.JointLookAt(joint, vec_c, vec_a);
                #if IMDEBUGGING
                    if (idx == 0)
                    {
                        Dbg.Log("{0}, LookAt2: Rot: {1}", m_cnter, joint.rotation);
                    }
                #endif
                }
                else
                {
                    float radian_b = Mathf.Acos((len_a * len_a + len_c * len_c - len_b * len_b) / (2 * len_a * len_c));
                    float vv       = Vector3.Dot(vec_a, vec_c);

                    vv = Mathf.Clamp(vv, -1f, 1f); //precision error protection, the direct result of dotP might cause Acos return NaN
                    float radian_rot = Mathf.Acos(vv) - radian_b;
                    if (float.IsNaN(radian_rot))
                    {
                        Dbg.LogWarn("NaN???!!!Not Again...");
                    }
                    float angle_rot = radian_rot * Mathf.Rad2Deg;

                    Vector3 rotAxis;
                    if (vec_a == vec_c || vec_a == -vec_c)
                    {
                        rotAxis = joint.up;
                        joint.Rotate(rotAxis, angle_rot, Space.World);
                    }
                    else
                    {
                        rotAxis = Vector3.Cross(vec_a, vec_c);
                        joint.Rotate(rotAxis, angle_rot, Space.World);
                    }

                #if IMDEBUGGING
                    if (idx == 0)
                    {
                        Dbg.Log("{0}, Rotate: rot: {1}", m_cnter, joint.rotation);
                    }
                #endif
                }

                // use constraint to prevent joint bend to wrong direction
                m_Constraint.EnsureConstraint();

                if (idx == this.Count - 1 && Mathf.Approximately(len_c, len_a))
                {
                    bSuccess = true;
                }
            }

            return(bSuccess ? IKExecRes.SUCCESS : IKExecRes.UNREACH_INLIMIT);
        }