/// <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);
}
/// <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);
}
