Vector2 Trace (VO[] vos, int voCount, Vector2 p, float cutoff, out float score) {
score = 0;
float stepScale = simulator.stepScale;
float bestScore = float.PositiveInfinity;
Vector2 bestP = p;
for (int s = 0; s < 50; s++) {
float step = 1.0f - (s/50.0f);
step *= stepScale;
Vector2 dir = Vector2.zero;
float mx = 0;
for (int i = 0; i < voCount; i++) {
float w;
Vector2 d = vos[i].Sample(p, out w);
dir += d;
if (w > mx) mx = w;
//mx = System.Math.Max (mx, d.sqrMagnitude);
}
// This didn't work out as well as I though
// Code left here because I might reenable it later
//Vector2 bonusForDesiredSpeed = p.normalized * new Vector2(desiredVelocity.x,desiredVelocity.z).magnitude - p;
Vector2 bonusForDesiredVelocity = (new Vector2(desiredVelocity.x, desiredVelocity.z) - p);
float weight = bonusForDesiredVelocity.magnitude*DesiredVelocityWeight;// + bonusForDesiredSpeed.magnitude*DesiredSpeedScale;
dir += bonusForDesiredVelocity*DesiredVelocityScale;// + bonusForDesiredSpeed*DesiredSpeedScale;
mx = System.Math.Max(mx, weight);
score = mx;
if (score < bestScore) {
bestScore = score;
}
bestP = p;
if (score <= cutoff && s > 10) break;
float sq = dir.sqrMagnitude;
if (sq > 0) dir *= mx/Mathf.Sqrt(sq);
dir *= step;
Vector2 prev = p;
p += dir;
if (DebugDraw) Debug.DrawLine(To3D(prev)+position, To3D(p)+position, Rainbow(0.1f/score) * new Color(1, 1, 1, 0.2f));
}
score = bestScore;
return bestP;
}