예제 #1
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);
        }
예제 #2
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);
        }