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