/// <summary>
/// Solve the IK chain using FABRIK method
/// </summary>
/// <param name="_IKChain"></param>
public void SolveFABRIK(RootIK.Chain _IKChain)
{
chain = _IKChain;
if (_IKChain.weight <= 0)
{
return;
}
if (_IKChain.joints.Count <= 0)
{
return;
}
for (int i = 0; i < _IKChain.joints.Count - 1; i++)
{
//reset from last iteration
_IKChain.joints[i].solvePos = _IKChain.joints[i].transform.position;
}
for (int i = 0; i < _IKChain.iterations; i++)
{
SolveInward();
SolveOutward();
CorrectRotation();
}
}
/// <summary>
/// Solve the IK chain using CCD method
/// </summary>
/// <param name="_IKChain"></param>
public void SolveCCD(RootIK.Chain _IKChain)
{
if (_IKChain.weight <= 0f)
{
return;
}
if (_IKChain.joints.Count <= 0)
{
return;
}
_IKChain.SetIKPosition(_IKChain.target ? Vector3.zero : _IKChain.GetIKPosition());
for (int j = 0; j < _IKChain.iterations; j++)
{
for (int i = _IKChain.joints.Count - 1; i >= 0; i--)
{
_IKChain.weight = Mathf.Clamp(_IKChain.weight, 0f, 1f);
float _weight = _IKChain.weight * _IKChain.joints[i].weight;
Vector3 _v0 = _IKChain.GetIKPosition() - _IKChain.joints[i].transform.position;
Vector3 _v1 = _IKChain.joints[_IKChain.joints.Count - 1].transform.position - _IKChain.joints[i].transform.position;
Quaternion _sourceRotation = _IKChain.joints[i].transform.rotation;
Quaternion _targetRotation = Quaternion.Lerp(Quaternion.identity, RootIK.RotateFromTo(_v0, _v1), _weight);
_IKChain.joints[i].transform.rotation = Quaternion.Lerp(_sourceRotation, GenericMaths.ApplyQuaternion(_targetRotation, _sourceRotation), _weight);
}
}
_IKChain.joints[_IKChain.joints.Count - 1].transform.rotation = _IKChain.GetIKRotation();
}
/// <summary>
/// Prepare the solver and solve the IK problem analytically with specific axis of rotation
/// (the chain must contain 2 joints, no more and no less)
/// </summary>
/// <param name="_hingeChain">the chain</param>
/// <param name="_direction">direction of the player</param>
/// <param name="_axis">axis of rotation for the 2nd joint (the Hinge joint)</param>
public void SolveAnalytically(RootIK.Chain _hingeChain, Vector3 _direction, Vector3 _axis)
{
if (_hingeChain.joints.Count > 3)
{
return;
}
if (_hingeChain.iterations <= 0)
{
return;
}
if (_hingeChain.joints.Count <= 0)
{
return;
}
//calculate bone length;
upperLength = Vector3.Distance(_hingeChain.joints[0].transform.position, _hingeChain.joints[1].transform.position);
lowerLength = Vector3.Distance(_hingeChain.joints[1].transform.position, _hingeChain.joints[2].transform.position);
systemLength = Vector3.Distance(_hingeChain.joints[0].transform.position, _hingeChain.GetIKPosition());
//lowerjoint 1DOF
float _angle = GenericMaths.Formula(upperLength, lowerLength, systemLength) + Mathf.PI * Mathf.Rad2Deg;
if (_axis == Vector3.zero)
{
_axis = Vector3.Cross(RootIK.TransformVector(Vector3.up, _hingeChain.joints[1].transform.rotation), _direction);
}
else
{
_axis = Vector3.Cross(RootIK.TransformVector(_axis, _hingeChain.joints[1].transform.rotation), _direction);
}
Quaternion _src = _hingeChain.joints[1].transform.rotation;
Quaternion _t = GenericMaths.QuaternionFromAngleAxis(_axis, _angle + Mathf.Acos(Mathf.Clamp(_src.w, -1f, 1f)) * 10f);
Quaternion _finalRot = Quaternion.Lerp(Quaternion.identity, _t, _hingeChain.weight);
_hingeChain.joints[1].transform.rotation = GenericMaths.ApplyQuaternion(_finalRot, _src);
//Upperjoint 3DOF
Vector3 _v1 = _hingeChain.GetIKPosition() - _hingeChain.joints[0].transform.position;
Vector3 _v2 = _hingeChain.joints[2].transform.position - _hingeChain.joints[0].transform.position;
Quaternion _src2 = _hingeChain.joints[0].transform.rotation;
Quaternion _t2 = RootIK.RotateFromTo(_v2, _v1);
Quaternion _finalRot2 = Quaternion.Lerp(Quaternion.identity, _t2, _hingeChain.weight);
_hingeChain.joints[0].transform.rotation = GenericMaths.ApplyQuaternion(Quaternion.Inverse(_finalRot2), _src2);
//foot 6DOF
_hingeChain.GetEndEffector().rotation = _hingeChain.GetIKRotation();
}
/// <summary>
/// A helping method to update 2 IK chains based on the dist to their IK target
/// </summary>
/// <param name="_IKChain"></param>
/// <param name="_IKChain2"></param>
public void SolveCCD(RootIK.Chain _IKChain, RootIK.Chain _IKChain2)
{
//in CCD algorithm, the last chain to be updated will have the biggest influence on the overall results
if (_IKChain.joints.Count <= 0 || _IKChain2.joints.Count <= 0)
{
return;
}
float _distToTarget = Vector3.Distance(_IKChain.GetIKPosition(), _IKChain.joints[0].transform.position);
float _distToTarget2 = Vector3.Distance(_IKChain2.GetIKPosition(), _IKChain2.joints[0].transform.position);
if (_distToTarget > _distToTarget2)
{
SolveCCD(_IKChain);
SolveCCD(_IKChain2);
}
else
{
SolveCCD(_IKChain2);
SolveCCD(_IKChain);
}
}
