/// <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();
}
private void FindGround()
{
for (int i = 0; i < 2; i++)
{
Vector3 _endEffector = i == 0 ? rightLeg.GetEndEffector().position : leftLeg.GetEndEffector().position;
Ray _ray = new Ray(_endEffector + Vector3.up * maxStep, Vector3.down);
RaycastHit _hit = new RaycastHit();
if (Physics.Raycast(_ray, out _hit, rayLength, solverLayer))
{
Quaternion _ankleRot = RootIK.RotateFromTo(_hit.normal, RootIK.TransformVector(Vector3.up, Root().rotation));
rightLeg.SetIKRotation(i == 0 ? GenericMaths.ApplyQuaternion(_ankleRot, rightLeg.GetEndEffector().rotation) : rightLeg.GetIKRotation());
leftLeg.SetIKRotation(i == 1 ? GenericMaths.ApplyQuaternion(_ankleRot, leftLeg.GetEndEffector().rotation) : leftLeg.GetIKRotation());
IKTarget[i] = _hit.point;
}
}
rightLeg.SetIKPosition(IKTarget[0]);
leftLeg.SetIKPosition(IKTarget[1]);
}