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]); } } }