/// <summary>
/// Spawn a gameObject as target for the given IKSolver
/// </summary>
/// <param name="foot">Foot with IKSolver</param>
/// <param name="footNumber">foot id (naming purpose only)</param>
/// <returns>Transform of the spawned gameObject</returns>
private Transform SpawnFootTarget(IKSolverBase foot, int footNumber)
{
Transform footTarget = new GameObject(gameObject.name + string.Format(" Foot[{0}]-Target", footNumber)).transform;
footTarget.position = foot.transform.position;
footTarget.rotation = Quaternion.identity;
footTarget.localScale = Vector3.one;
footTarget.parent = transform;
return(footTarget);
}
/// <summary>
/// Move the FootIKs targets to the next position
/// </summary>
/// <returns>Rather a foot was moved or not</returns>
private bool SetFootIKTargets(Func <Vector3, Vector3, float> error, float threshold)
{
// Check if Stepping distance has been reached
bool moved = false;
for (int i = 0; i < FootPairs.Length; i++)
{
IKSolverBase footIK = _movePrimaryLeg ? FootPairs[i].primaryLeg : FootPairs[i].oppositeLeg;
IKSolverBase otherFootIK = _movePrimaryLeg ? FootPairs[i].oppositeLeg : FootPairs[i].primaryLeg;
int footTargetIndex = _movePrimaryLeg ? 2 * i : 2 * i + 1;
int otherFootTargetIndex = _movePrimaryLeg ? 2 * i + 1 : 2 * i;
Vector3 footCurrentPos = footIK.GetTargetPosition();
Vector3 otherFootCurrentPos = otherFootIK.GetTargetPosition();
Vector3 footTargetPos = _footTargets[footTargetIndex].position;
Vector3 otherFootTargetPos = _footTargets[otherFootTargetIndex].position;
if ((
// foot reached threshold
error(footTargetPos, footCurrentPos) > threshold
// // StepDistance reached or
// Vector3.Distance(footTargetPos, footCurrentPos) > LinearStepDistance ||
// // Angle between them reached AngleDistance and either
// Vector3.Angle(footTargetPos, footCurrentPos) > AngularStepDistance
) && (
// other foot reached current target or
otherFootIK.GetCurrentError() < otherFootIK.GetMaxError() ||
// other foot reached threshhold as well
error(otherFootTargetPos, otherFootCurrentPos) > threshold
// // other foot also has reached the StepDistance
// Vector3.Distance(otherFootTargetPos, otherFootCurrentPos) > LinearStepDistance ||
// // other foot also reached AngleDistance
// Vector3.Angle(otherFootTargetPos, otherFootCurrentPos) > AngularStepDistance
)
)
{
footIK.SetTargetPosition(footTargetPos);
moved = true;
}
}
return(moved);
}
