public static float CyclicLerp(float a, float b, float t, float period)
{
if (Mathf.Abs(b - a) <= period / 2)
{
return(a * (1 - t) + b * t);
}
if (b < a)
{
a -= period;
}
else
{
b -= period;
}
return(LocomotionUtil.Mod(a * (1 - t) + b * t));
}
public void CalculateTimeOffsets()
{
float[] offsets = new float[cycleMotions.Length];
float[] offsetChanges = new float[cycleMotions.Length];
for (int i = 0; i < cycleMotions.Length; i++)
{
offsets[i] = 0;
}
int springs = (cycleMotions.Length * cycleMotions.Length - cycleMotions.Length) / 2;
int iteration = 0;
bool finished = false;
while (iteration < 100 && finished == false)
{
for (int i = 0; i < cycleMotions.Length; i++)
{
offsetChanges[i] = 0;
}
// Calculate offset changes
for (int i = 1; i < cycleMotions.Length; i++)
{
for (int j = 0; j < i; j++)
{
for (int leg = 0; leg < legs.Length; leg++)
{
float ta = cycleMotions[i].cycles[leg].stanceTime + offsets[i];
float tb = cycleMotions[j].cycles[leg].stanceTime + offsets[j];
Vector2 va = new Vector2(Mathf.Cos(ta * 2 * Mathf.PI), Mathf.Sin(ta * 2 * Mathf.PI));
Vector2 vb = new Vector2(Mathf.Cos(tb * 2 * Mathf.PI), Mathf.Sin(tb * 2 * Mathf.PI));
Vector2 abVector = vb - va;
Vector2 va2 = va + abVector * 0.1f;
Vector2 vb2 = vb - abVector * 0.1f;
float ta2 = LocomotionUtil.Mod(Mathf.Atan2(va2.y, va2.x) / 2 / Mathf.PI);
float tb2 = LocomotionUtil.Mod(Mathf.Atan2(vb2.y, vb2.x) / 2 / Mathf.PI);
float aChange = LocomotionUtil.Mod(ta2 - ta);
float bChange = LocomotionUtil.Mod(tb2 - tb);
if (aChange > 0.5f)
{
aChange = aChange - 1;
}
if (bChange > 0.5f)
{
bChange = bChange - 1;
}
offsetChanges[i] += aChange * 5.0f / springs;
offsetChanges[j] += bChange * 5.0f / springs;
}
}
}
// Apply new offset changes
float maxChange = 0;
for (int i = 0; i < cycleMotions.Length; i++)
{
offsets[i] += offsetChanges[i];
maxChange = Mathf.Max(maxChange, Mathf.Abs(offsetChanges[i]));
}
iteration++;
if (maxChange < 0.0001)
{
finished = true;
}
}
// Apply the offsets to the motions
for (int m = 0; m < cycleMotions.Length; m++)
{
cycleMotions[m].cycleOffset = offsets[m];
for (int leg = 0; leg < legs.Length; leg++)
{
cycleMotions[m].cycles[leg].stanceTime =
LocomotionUtil.Mod(cycleMotions[m].cycles[leg].stanceTime + offsets[m]);
}
}
}