public void InsertObstacleNeighbour(ObstacleVertex ob1, float rangeSq)
{
ObstacleVertex obstacleVertex = ob1.next;
float num = VectorMath.SqrDistancePointSegment(ob1.position, obstacleVertex.position, this.Position);
if (num < rangeSq)
{
this.obstacles.Add(ob1);
this.obstacleDists.Add(num);
int num2 = this.obstacles.Count - 1;
while (num2 != 0 && num < this.obstacleDists[num2 - 1])
{
this.obstacles[num2] = this.obstacles[num2 - 1];
this.obstacleDists[num2] = this.obstacleDists[num2 - 1];
num2--;
}
this.obstacles[num2] = ob1;
this.obstacleDists[num2] = num;
}
}
/*public void UpdateNeighbours () {
* neighbours.Clear ();
* float sqrDist = neighbourDistance*neighbourDistance;
* for ( int i = 0; i < simulator.agents.Count; i++ ) {
* float dist = (simulator.agents[i].position - position).sqrMagnitude;
* if ( dist <= sqrDist ) {
* neighbours.Add ( simulator.agents[i] );
* }
* }
* }*/
public void InsertObstacleNeighbour(ObstacleVertex ob1, float rangeSq)
{
ObstacleVertex ob2 = ob1.next;
float dist = VectorMath.SqrDistancePointSegment(ob1.position, ob2.position, Position);
if (dist < rangeSq)
{
obstacles.Add(ob1);
obstacleDists.Add(dist);
int i = obstacles.Count - 1;
while (i != 0 && dist < obstacleDists[i - 1])
{
obstacles[i] = obstacles[i - 1];
obstacleDists[i] = obstacleDists[i - 1];
i--;
}
obstacles[i] = ob1;
obstacleDists[i] = dist;
}
}
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);
}
}
public void InsertObstacleNeighbour(ObstacleVertex ob1, float rangeSq)
{
ObstacleVertex obstacleVertex = ob1.next;
float num = AstarMath.DistancePointSegmentStrict(ob1.position, obstacleVertex.position, this.Position);
if (num < rangeSq)
{
this.obstacles.Add(ob1);
this.obstacleDists.Add(num);
int num2 = this.obstacles.Count - 1;
while (num2 != 0 && num < this.obstacleDists[num2 - 1])
{
this.obstacles[num2] = this.obstacles[num2 - 1];
this.obstacleDists[num2] = this.obstacleDists[num2 - 1];
num2--;
}
this.obstacles[num2] = ob1;
this.obstacleDists[num2] = num;
}
}
internal void CalculateVelocity(Simulator.WorkerContext context)
{
if (this.locked)
{
this.newVelocity = Vector2.zero;
return;
}
if (context.vos.Length < this.neighbours.Count + this.simulator.obstacles.Count)
{
context.vos = new Agent.VO[Mathf.Max(context.vos.Length * 2, this.neighbours.Count + this.simulator.obstacles.Count)];
}
Vector2 vector = new Vector2(this.position.x, this.position.z);
Agent.VO[] vos = context.vos;
int num = 0;
Vector2 vector2 = new Vector2(this.velocity.x, this.velocity.z);
float num2 = 1f / this.agentTimeHorizon;
float wallThickness = this.simulator.WallThickness;
float num3 = (this.simulator.algorithm != Simulator.SamplingAlgorithm.GradientDecent) ? 5f : 1f;
for (int i = 0; i < this.simulator.obstacles.Count; i++)
{
ObstacleVertex obstacleVertex = this.simulator.obstacles[i];
ObstacleVertex obstacleVertex2 = obstacleVertex;
do
{
if (obstacleVertex2.ignore || this.position.y > obstacleVertex2.position.y + obstacleVertex2.height || this.position.y + this.height < obstacleVertex2.position.y || (obstacleVertex2.layer & this.collidesWith) == (RVOLayer)0)
{
obstacleVertex2 = obstacleVertex2.next;
}
else
{
float num4 = Agent.VO.Det(new Vector2(obstacleVertex2.position.x, obstacleVertex2.position.z), obstacleVertex2.dir, vector);
float num5 = num4;
float num6 = Vector2.Dot(obstacleVertex2.dir, vector - new Vector2(obstacleVertex2.position.x, obstacleVertex2.position.z));
bool flag = num6 <= wallThickness * 0.05f || num6 >= (new Vector2(obstacleVertex2.position.x, obstacleVertex2.position.z) - new Vector2(obstacleVertex2.next.position.x, obstacleVertex2.next.position.z)).magnitude - wallThickness * 0.05f;
if (Mathf.Abs(num5) < this.neighbourDist)
{
if (num5 <= 0f && !flag && num5 > -wallThickness)
{
vos[num] = new Agent.VO(vector, new Vector2(obstacleVertex2.position.x, obstacleVertex2.position.z) - vector, obstacleVertex2.dir, num3 * 2f);
num++;
}
else if (num5 > 0f)
{
Vector2 p = new Vector2(obstacleVertex2.position.x, obstacleVertex2.position.z) - vector;
Vector2 p2 = new Vector2(obstacleVertex2.next.position.x, obstacleVertex2.next.position.z) - vector;
Vector2 normalized = p.normalized;
Vector2 normalized2 = p2.normalized;
vos[num] = new Agent.VO(vector, p, p2, normalized, normalized2, num3);
num++;
}
}
obstacleVertex2 = obstacleVertex2.next;
}
}while (obstacleVertex2 != obstacleVertex);
}
for (int j = 0; j < this.neighbours.Count; j++)
{
Agent agent = this.neighbours[j];
if (agent != this)
{
float num7 = Math.Min(this.position.y + this.height, agent.position.y + agent.height);
float num8 = Math.Max(this.position.y, agent.position.y);
if (num7 - num8 >= 0f)
{
Vector2 vector3 = new Vector2(agent.Velocity.x, agent.velocity.z);
float num9 = this.radius + agent.radius;
Vector2 vector4 = new Vector2(agent.position.x, agent.position.z) - vector;
Vector2 sideChooser = vector2 - vector3;
Vector2 vector5;
if (agent.locked)
{
vector5 = vector3;
}
else
{
vector5 = (vector2 + vector3) * 0.5f;
}
vos[num] = new Agent.VO(vector4, vector5, num9, sideChooser, num2, 1f);
num++;
if (this.DebugDraw)
{
Agent.DrawVO(vector + vector4 * num2 + vector5, num9 * num2, vector + vector5);
}
}
}
}
Vector2 vector6 = Vector2.zero;
if (this.simulator.algorithm == Simulator.SamplingAlgorithm.GradientDecent)
{
if (this.DebugDraw)
{
for (int k = 0; k < 40; k++)
{
for (int l = 0; l < 40; l++)
{
Vector2 vector7 = new Vector2((float)k * 15f / 40f, (float)l * 15f / 40f);
Vector2 a = Vector2.zero;
float num10 = 0f;
for (int m = 0; m < num; m++)
{
float num11 = 0f;
a += vos[m].Sample(vector7 - vector, out num11);
if (num11 > num10)
{
num10 = num11;
}
}
Vector2 a2 = new Vector2(this.desiredVelocity.x, this.desiredVelocity.z) - (vector7 - vector);
a += a2 * Agent.DesiredVelocityScale;
if (a2.magnitude * Agent.DesiredVelocityWeight > num10)
{
num10 = a2.magnitude * Agent.DesiredVelocityWeight;
}
if (num10 > 0f)
{
a /= num10;
}
UnityEngine.Debug.DrawRay(Agent.To3D(vector7), Agent.To3D(a2 * 0f), Color.blue);
float num12 = 0f;
Vector2 vector8 = vector7 - Vector2.one * 15f * 0.5f;
Vector2 vector9 = this.Trace(vos, num, vector8, 0.01f, out num12);
if ((vector8 - vector9).sqrMagnitude < this.Sqr(0.375f) * 2.6f)
{
UnityEngine.Debug.DrawRay(Agent.To3D(vector9 + vector), Vector3.up * 1f, Color.red);
}
}
}
}
float num13 = float.PositiveInfinity;
Vector2 vector10 = new Vector2(this.velocity.x, this.velocity.z);
float cutoff = vector10.magnitude * this.simulator.qualityCutoff;
vector6 = this.Trace(vos, num, new Vector2(this.desiredVelocity.x, this.desiredVelocity.z), cutoff, out num13);
if (this.DebugDraw)
{
Agent.DrawCross(vector6 + vector, Color.yellow, 0.5f);
}
Vector2 p3 = this.Velocity;
float num14;
Vector2 vector11 = this.Trace(vos, num, p3, cutoff, out num14);
if (num14 < num13)
{
vector6 = vector11;
num13 = num14;
}
if (this.DebugDraw)
{
Agent.DrawCross(vector11 + vector, Color.magenta, 0.5f);
}
}
else
{
Vector2[] samplePos = context.samplePos;
float[] sampleSize = context.sampleSize;
int num15 = 0;
Vector2 vector12 = new Vector2(this.desiredVelocity.x, this.desiredVelocity.z);
float num16 = Mathf.Max(this.radius, Mathf.Max(vector12.magnitude, this.Velocity.magnitude));
samplePos[num15] = vector12;
sampleSize[num15] = num16 * 0.3f;
num15++;
samplePos[num15] = vector2;
sampleSize[num15] = num16 * 0.3f;
num15++;
Vector2 a3 = vector2 * 0.5f;
Vector2 a4 = new Vector2(a3.y, -a3.x);
for (int n = 0; n < 8; n++)
{
samplePos[num15] = a4 * Mathf.Sin((float)n * 3.14159274f * 2f / 8f) + a3 * (1f + Mathf.Cos((float)n * 3.14159274f * 2f / 8f));
sampleSize[num15] = (1f - Mathf.Abs((float)n - 4f) / 8f) * num16 * 0.5f;
num15++;
}
a3 *= 0.6f;
a4 *= 0.6f;
for (int num17 = 0; num17 < 6; num17++)
{
samplePos[num15] = a4 * Mathf.Cos(((float)num17 + 0.5f) * 3.14159274f * 2f / 6f) + a3 * (1.66666663f + Mathf.Sin(((float)num17 + 0.5f) * 3.14159274f * 2f / 6f));
sampleSize[num15] = num16 * 0.3f;
num15++;
}
for (int num18 = 0; num18 < 6; num18++)
{
samplePos[num15] = vector2 + new Vector2(num16 * 0.2f * Mathf.Cos(((float)num18 + 0.5f) * 3.14159274f * 2f / 6f), num16 * 0.2f * Mathf.Sin(((float)num18 + 0.5f) * 3.14159274f * 2f / 6f));
sampleSize[num15] = num16 * 0.2f * 2f;
num15++;
}
samplePos[num15] = vector2 * 0.5f;
sampleSize[num15] = num16 * 0.4f;
num15++;
Vector2[] bestPos = context.bestPos;
float[] bestSizes = context.bestSizes;
float[] bestScores = context.bestScores;
for (int num19 = 0; num19 < 3; num19++)
{
bestScores[num19] = float.PositiveInfinity;
}
bestScores[3] = float.NegativeInfinity;
Vector2 vector13 = vector2;
float num20 = float.PositiveInfinity;
for (int num21 = 0; num21 < 3; num21++)
{
for (int num22 = 0; num22 < num15; num22++)
{
float num23 = 0f;
for (int num24 = 0; num24 < num; num24++)
{
num23 = Math.Max(num23, vos[num24].ScalarSample(samplePos[num22]));
}
float magnitude = (samplePos[num22] - vector12).magnitude;
float num25 = num23 + magnitude * Agent.DesiredVelocityWeight;
num23 += magnitude * 0.001f;
if (this.DebugDraw)
{
Agent.DrawCross(vector + samplePos[num22], Agent.Rainbow(Mathf.Log(num23 + 1f) * 5f), sampleSize[num22] * 0.5f);
}
if (num25 < bestScores[0])
{
for (int num26 = 0; num26 < 3; num26++)
{
if (num25 >= bestScores[num26 + 1])
{
bestScores[num26] = num25;
bestSizes[num26] = sampleSize[num22];
bestPos[num26] = samplePos[num22];
break;
}
}
}
if (num23 < num20)
{
vector13 = samplePos[num22];
num20 = num23;
if (num23 == 0f)
{
num21 = 100;
break;
}
}
}
num15 = 0;
for (int num27 = 0; num27 < 3; num27++)
{
Vector2 a5 = bestPos[num27];
float num28 = bestSizes[num27];
bestScores[num27] = float.PositiveInfinity;
float num29 = num28 * 0.6f * 0.5f;
samplePos[num15] = a5 + new Vector2(num29, num29);
samplePos[num15 + 1] = a5 + new Vector2(-num29, num29);
samplePos[num15 + 2] = a5 + new Vector2(-num29, -num29);
samplePos[num15 + 3] = a5 + new Vector2(num29, -num29);
num28 *= num28 * 0.6f;
sampleSize[num15] = num28;
sampleSize[num15 + 1] = num28;
sampleSize[num15 + 2] = num28;
sampleSize[num15 + 3] = num28;
num15 += 4;
}
}
vector6 = vector13;
}
if (this.DebugDraw)
{
Agent.DrawCross(vector6 + vector, 1f);
}
this.newVelocity = Agent.To3D(Vector2.ClampMagnitude(vector6, this.maxSpeed));
}
