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 void GenerateObstacleVOs(Agent.VOBuffer vos)
{
for (int i = 0; i < this.simulator.obstacles.Count; i++)
{
ObstacleVertex obstacleVertex = this.simulator.obstacles[i];
ObstacleVertex obstacleVertex2 = obstacleVertex;
do
{
if (obstacleVertex2.ignore || (obstacleVertex2.layer & this.collidesWith) == (RVOLayer)0)
{
obstacleVertex2 = obstacleVertex2.next;
}
else
{
Vector2 vector = Agent.To2D(obstacleVertex2.position);
Vector2 vector2 = Agent.To2D(obstacleVertex2.next.position);
float num = Agent.VO.SignedDistanceFromLine(vector, obstacleVertex2.dir, this.position);
if (num >= -0.01f && num < this.neighbourDist)
{
float t = Vector2.Dot(this.position - vector, vector2 - vector) / (vector2 - vector).sqrMagnitude;
float num2 = Mathf.Lerp(obstacleVertex2.position.y, obstacleVertex2.next.position.y, t);
float sqrMagnitude = (Vector2.Lerp(vector, vector2, t) - this.position).sqrMagnitude;
if (sqrMagnitude < this.neighbourDist * this.neighbourDist && this.elevationCoordinate <= num2 + obstacleVertex2.height && this.elevationCoordinate + this.height >= num2)
{
vos.Add(Agent.VO.SegmentObstacle(vector2 - this.position, vector - this.position, Vector2.zero, this.radius * 0.01f, 1f / this.ObstacleTimeHorizon, 1f / this.simulator.DeltaTime));
}
}
obstacleVertex2 = obstacleVertex2.next;
}
}while (obstacleVertex2 != obstacleVertex && obstacleVertex2 != null && obstacleVertex2.next != null);
}
}
private Vector2 GradientDescent(Agent.VOBuffer vos, Vector2 sampleAround1, Vector2 sampleAround2)
{
float num;
Vector2 vector = this.Trace(vos, sampleAround1, out num);
if (this.DebugDraw)
{
Agent.DrawCross(vector + this.position, Color.yellow, 0.5f);
}
float num2;
Vector2 vector2 = this.Trace(vos, sampleAround2, out num2);
if (this.DebugDraw)
{
Agent.DrawCross(vector2 + this.position, Color.magenta, 0.5f);
}
return((num >= num2) ? vector2 : vector);
}
private void GenerateNeighbourAgentVOs(Agent.VOBuffer vos)
{
float num = 1f / this.agentTimeHorizon;
Vector2 a = this.currentVelocity;
for (int i = 0; i < this.neighbours.Count; i++)
{
Agent agent = this.neighbours[i];
if (agent != this)
{
float num2 = Math.Min(this.elevationCoordinate + this.height, agent.elevationCoordinate + agent.height);
float num3 = Math.Max(this.elevationCoordinate, agent.elevationCoordinate);
if (num2 - num3 >= 0f)
{
float num4 = this.radius + agent.radius;
Vector2 vector = agent.position - this.position;
float num5;
if (agent.locked || agent.manuallyControlled)
{
num5 = 1f;
}
else if (agent.Priority > 1E-05f || this.Priority > 1E-05f)
{
num5 = agent.Priority / (this.Priority + agent.Priority);
}
else
{
num5 = 0.5f;
}
Vector2 b = Vector2.Lerp(agent.currentVelocity, agent.desiredVelocity, 2f * num5 - 1f);
Vector2 vector2 = Vector2.Lerp(a, b, num5);
vos.Add(new Agent.VO(vector, vector2, num4, num, 1f / this.simulator.DeltaTime));
if (this.DebugDraw)
{
Agent.DrawVO(this.position + vector * num + vector2, num4 * num, this.position + vector2);
}
}
}
}
}
private Vector2 EvaluateGradient(Agent.VOBuffer vos, Vector2 p, out float value)
{
Vector2 vector = Vector2.zero;
value = 0f;
for (int i = 0; i < vos.length; i++)
{
float num;
Vector2 vector2 = vos.buffer[i].ScaledGradient(p, out num);
if (num > value)
{
value = num;
vector = vector2;
}
}
Vector2 a = this.desiredVelocity - p;
float magnitude = a.magnitude;
if (magnitude > 0.0001f)
{
vector += a * (0.1f / magnitude);
value += magnitude * 0.1f;
}
float sqrMagnitude = p.sqrMagnitude;
if (sqrMagnitude > this.desiredSpeed * this.desiredSpeed)
{
float num2 = Mathf.Sqrt(sqrMagnitude);
if (num2 > this.maxSpeed)
{
value += 3f * (num2 - this.maxSpeed);
vector -= 3f * (p / num2);
}
float num3 = 0.2f;
value += num3 * (num2 - this.desiredSpeed);
vector -= num3 * (p / num2);
}
return(vector);
}
private static bool BiasDesiredVelocity(Agent.VOBuffer vos, ref Vector2 desiredVelocity, ref Vector2 targetPointInVelocitySpace, float maxBiasRadians)
{
float magnitude = desiredVelocity.magnitude;
float num = 0f;
for (int i = 0; i < vos.length; i++)
{
float b;
vos.buffer[i].Gradient(desiredVelocity, out b);
num = Mathf.Max(num, b);
}
bool result = num > 0f;
if (magnitude < 0.001f)
{
return(result);
}
float d = Mathf.Min(maxBiasRadians, num / magnitude);
desiredVelocity += new Vector2(desiredVelocity.y, -desiredVelocity.x) * d;
targetPointInVelocitySpace += new Vector2(targetPointInVelocitySpace.y, -targetPointInVelocitySpace.x) * d;
return(result);
}
internal void CalculateVelocity(Simulator.WorkerContext context)
{
if (this.manuallyControlled)
{
return;
}
if (this.locked)
{
this.calculatedSpeed = 0f;
this.calculatedTargetPoint = this.position;
return;
}
this.desiredVelocity = this.desiredTargetPointInVelocitySpace.normalized * this.desiredSpeed;
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)
{
Agent.DrawCross(this.calculatedTargetPoint, 1f);
}
return;
}
Vector2 vector = Vector2.zero;
vector = this.GradientDescent(vos, this.currentVelocity, this.desiredVelocity);
if (this.DebugDraw)
{
Agent.DrawCross(vector + this.position, 1f);
}
this.calculatedTargetPoint = this.position + vector;
this.calculatedSpeed = Mathf.Min(vector.magnitude, this.maxSpeed);
}
private void GenerateObstacleVOs(Agent.VOBuffer vos)
{
float num = this.maxSpeed * this.obstacleTimeHorizon;
for (int i = 0; i < this.simulator.obstacles.Count; i++)
{
ObstacleVertex obstacleVertex = this.simulator.obstacles[i];
ObstacleVertex obstacleVertex2 = obstacleVertex;
do
{
if (obstacleVertex2.ignore || (obstacleVertex2.layer & this.collidesWith) == (RVOLayer)0)
{
obstacleVertex2 = obstacleVertex2.next;
}
else
{
float a;
Vector2 vector = this.To2D(obstacleVertex2.position, out a);
float b;
Vector2 vector2 = this.To2D(obstacleVertex2.next.position, out b);
Vector2 normalized = (vector2 - vector).normalized;
float num2 = Agent.VO.SignedDistanceFromLine(vector, normalized, this.position);
if (num2 >= -0.01f && num2 < num)
{
float t = Vector2.Dot(this.position - vector, vector2 - vector) / (vector2 - vector).sqrMagnitude;
float num3 = Mathf.Lerp(a, b, t);
float sqrMagnitude = (Vector2.Lerp(vector, vector2, t) - this.position).sqrMagnitude;
if (sqrMagnitude < num * num && this.elevationCoordinate <= num3 + obstacleVertex2.height && this.elevationCoordinate + this.height >= num3)
{
vos.Add(Agent.VO.SegmentObstacle(vector2 - this.position, vector - this.position, Vector2.zero, this.radius * 0.01f, 1f / this.ObstacleTimeHorizon, 1f / this.simulator.DeltaTime));
}
}
obstacleVertex2 = obstacleVertex2.next;
}
}while (obstacleVertex2 != obstacleVertex && obstacleVertex2 != null && obstacleVertex2.next != null);
}
}
