private Vector2 Trace(Agent.VOBuffer vos, Vector2 p, out float score)
{
float num = Mathf.Max(this.radius, 0.2f * this.desiredSpeed);
float num2 = float.PositiveInfinity;
Vector2 result = p;
for (int i = 0; i < 50; i++)
{
float num3 = 1f - (float)i / 50f;
num3 = Agent.Sqr(num3) * num;
float num4;
Vector2 vector = this.EvaluateGradient(vos, p, out num4);
if (num4 < num2)
{
num2 = num4;
result = p;
}
vector.Normalize();
vector *= num3;
Vector2 a = p;
p += vector;
if (this.DebugDraw)
{
Debug.DrawLine(Agent.FromXZ(a + this.position), Agent.FromXZ(p + this.position), Agent.Rainbow((float)i * 0.1f) * new Color(1f, 1f, 1f, 1f));
}
}
score = num2;
return(result);
}
private static void DrawVO(Vector2 circleCenter, float radius, Vector2 origin)
{
float num = Mathf.Atan2((origin - circleCenter).y, (origin - circleCenter).x);
float num2 = radius / (origin - circleCenter).magnitude;
float num3 = (num2 <= 1f) ? Mathf.Abs(Mathf.Acos(num2)) : 0f;
Draw.Debug.CircleXZ(Agent.FromXZ(circleCenter), radius, Color.black, num - num3, num + num3, 40);
Vector2 vector = new Vector2(Mathf.Cos(num - num3), Mathf.Sin(num - num3)) * radius;
Vector2 vector2 = new Vector2(Mathf.Cos(num + num3), Mathf.Sin(num + num3)) * radius;
Vector2 p = -new Vector2(-vector.y, vector.x);
Vector2 p2 = new Vector2(-vector2.y, vector2.x);
vector += circleCenter;
vector2 += circleCenter;
Debug.DrawRay(Agent.FromXZ(vector), Agent.FromXZ(p).normalized * 100f, Color.black);
Debug.DrawRay(Agent.FromXZ(vector2), Agent.FromXZ(p2).normalized * 100f, Color.black);
}
private Vector2 GradientDescent(Agent.VOBuffer vos, Vector2 sampleAround1, Vector2 sampleAround2)
{
float num;
Vector2 vector = this.Trace(vos, sampleAround1, out num);
if (this.DebugDraw)
{
Draw.Debug.CrossXZ(Agent.FromXZ(vector + this.position), Color.yellow, 0.5f);
}
float num2;
Vector2 vector2 = this.Trace(vos, sampleAround2, out num2);
if (this.DebugDraw)
{
Draw.Debug.CrossXZ(Agent.FromXZ(vector2 + this.position), Color.magenta, 0.5f);
}
return((num >= num2) ? vector2 : vector);
}
internal void CalculateVelocity(Simulator.WorkerContext context)
{
if (this.manuallyControlled)
{
return;
}
if (this.locked)
{
this.calculatedSpeed = 0f;
this.calculatedTargetPoint = this.position;
return;
}
Agent.VOBuffer vos = context.vos;
vos.Clear();
this.GenerateObstacleVOs(vos);
this.GenerateNeighbourAgentVOs(vos);
if (!Agent.BiasDesiredVelocity(vos, ref this.desiredVelocity, ref this.desiredTargetPointInVelocitySpace, this.simulator.symmetryBreakingBias))
{
this.calculatedTargetPoint = this.desiredTargetPointInVelocitySpace + this.position;
this.calculatedSpeed = this.desiredSpeed;
if (this.DebugDraw)
{
Draw.Debug.CrossXZ(Agent.FromXZ(this.calculatedTargetPoint), Color.white, 1f);
}
return;
}
Vector2 vector = Vector2.zero;
vector = this.GradientDescent(vos, this.currentVelocity, this.desiredVelocity);
if (this.DebugDraw)
{
Draw.Debug.CrossXZ(Agent.FromXZ(vector + this.position), Color.white, 1f);
}
this.calculatedTargetPoint = this.position + vector;
this.calculatedSpeed = Mathf.Min(vector.magnitude, this.maxSpeed);
}
