/// <summary>
/// we need to offset the root to achieve the results
/// </summary>
private void OffsetTargets()
{
Vector3 _newRootPos = Root().position;
float _toRight = Vector3.Distance(_newRootPos, rightLeg.GetIKPosition());
float _toLeft = Vector3.Distance(_newRootPos, leftLeg.GetIKPosition());
float _rootOffset = Mathf.Max(_toLeft, _toRight) - Mathf.Min(_toRight, _toLeft);
rootY = Mathf.Lerp(rootY, _rootOffset, rootLerp * Time.fixedDeltaTime);
_newRootPos.y -= rootY;
for (int i = 0; i < 2; i++)
{
Vector3 _targetOffset = i == 0 ? rightLeg.GetIKPosition() : leftLeg.GetIKPosition();
_targetOffset += Vector3.up * footHeight;
offsetedIK[i] = GenericMaths.Interpolate(offsetedIK[i], _targetOffset, footLerp * Time.fixedDeltaTime);
}
rightLeg.SetIKPosition(offsetedIK[0]);
leftLeg.SetIKPosition(offsetedIK[1]);
Root().position = _newRootPos;
//RayArtist.DrawHitPoints(rightLeg.GetEndEffector().position + Vector3.up * maxStep + Vector3.right * 0.3f, rightLeg.GetIKPosition() + Vector3.right * 0.3f, Color.red);
//RayArtist.DrawHitPoints(leftLeg.GetEndEffector().position + Vector3.up * maxStep + Vector3.left * 0.2f, leftLeg.GetIKPosition() + Vector3.left * 0.2f, Color.red);
}
/// <summary>
/// solve the joints forward
/// </summary>
private void SolveOutward()
{
chain.joints[0].solvePos = chain.joints[0].transform.position;
for (int i = 1; i < chain.joints.Count; i++)
{
Vector3 _v1 = chain.joints[i - 1].solvePos;
Vector3 _v0 = chain.joints[i].solvePos - _v1;
_v0.Normalize();
_v0 *= Vector3.Distance(chain.joints[i].transform.position, chain.joints[i - 1].transform.position);
chain.joints[i].solvePos = GenericMaths.Interpolate(chain.joints[i].transform.position, _v0 + _v1, chain.weight);
}
}
/// <summary>
/// solve the joints backward
/// </summary>
private void SolveInward()
{
chain.joints[chain.joints.Count - 1].solvePos = GenericMaths.Interpolate(chain.GetEndEffector().position, chain.GetIKPosition(), chain.weight);
for (int i = chain.joints.Count - 2; i >= 0; i--)
{
Vector3 _v1 = chain.joints[i + 1].solvePos;
Vector3 _v0 = chain.joints[i].solvePos - _v1;
_v0.Normalize();
_v0 *= Vector3.Distance(chain.joints[i].transform.position, chain.joints[i + 1].transform.position);
chain.joints[i].solvePos = GenericMaths.Interpolate(chain.joints[i].transform.position, _v0 + _v1, chain.weight);
}
}