Pathfinding.RVO.Sampled.Agent.Trace C# (CSharp) Method

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