/// <summary> /// Solve the IK for a chain /// </summary> /// <param name="chain"></param> public static bool Process(Core.Chain chain) { if (chain.Joints.Count <= 0) { return(false); } chain.MapVirtualJoints(); for (int j = 0; j < chain.Iterations; j++) { for (int i = chain.Joints.Count - 1; i >= 0; i--) { float _weight = chain.Weight * chain.Joints[i].weight; //relative weight //direction vectors Vector3 _v0 = chain.GetIKTarget() - chain.Joints[i].joint.position; Vector3 _v1 = chain.Joints[chain.Joints.Count - 1].joint.position - chain.Joints[i].joint.position; //Weight Quaternion _sourceRotation = chain.Joints[i].joint.rotation; Quaternion _targetRotation = Quaternion.Lerp(Quaternion.identity, GenericMath.RotateFromTo(_v0, _v1), _weight); //Rotation Math chain.Joints[i].rot = Quaternion.Lerp(_sourceRotation, GenericMath.ApplyQuaternion(_targetRotation, _sourceRotation), _weight); chain.Joints[i].ApplyVirtualMap(false, true); chain.Joints[i].ApplyRestrictions(); } } return(true); }
/// <summary> /// Solve the chain to make the offset look at the target /// </summary> /// <param name="chain"></param> /// <param name="endEffector"></param> private static void Solve(Core.Chain chain, Transform endEffector, Vector3 LookAtAxis) { for (int i = 0; i < chain.Joints.Count; i++) { //Vector3 axis = GenericMath.TransformVector(LookAtAxis, Quaternion.Inverse(offsetObj.rotation)); Vector3 axis = GenericMath.TransformVector(LookAtAxis, endEffector.rotation); Quaternion delta = GenericMath.RotateFromTo(chain.GetIKTarget() - endEffector.position, axis); Quaternion final = Quaternion.Lerp(Quaternion.identity, delta, chain.Weight * chain.Joints[i].weight); chain.Joints[i].joint.rotation = GenericMath.ApplyQuaternion(final, chain.Joints[i].joint.rotation); chain.Joints[i].ApplyRestrictions(); } }
/// <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; }
/// <summary> /// Find the virtual new solved position of joints in the chain inward /// </summary> /// <param name="chain"></param> public static void SolveInward(Core.Chain chain) { int c = chain.Joints.Count; //Use Weight first chain.Joints[c - 1].pos = Vector3.Lerp(chain.GetVirtualEE(), chain.GetIKTarget(), chain.Weight); //find the joint on the chain's virtual line for (int i = c - 2; i >= 0; i--) { Vector3 _p = chain.Joints[i + 1].pos; //point Vector3 _d = chain.Joints[i].pos - _p; //direction _d.Normalize(); _d *= Vector3.Distance(chain.Joints[i + 1].joint.position, chain.Joints[i].joint.position); //all points in a direction along a length chain.Joints[i].pos = _p + _d; } }