public void Add(Agent.VO vo)
{
if (this.length >= this.buffer.Length)
{
Agent.VO[] array = new Agent.VO[this.buffer.Length * 2];
this.buffer.CopyTo(array, 0);
this.buffer = array;
}
this.buffer[this.length++] = vo;
}
public void Add(Agent.VO vo)
{
if (this.length >= this.buffer.Length)
{
Agent.VO[] array = new Agent.VO[this.buffer.Length * 2];
this.buffer.CopyTo(array, 0);
this.buffer = array;
}
Agent.VO[] array2 = this.buffer;
int num = this.length;
this.length = num + 1;
array2[num] = vo;
}
public static Agent.VO SegmentObstacle(Vector2 segmentStart, Vector2 segmentEnd, Vector2 offset, float radius, float inverseDt, float inverseDeltaTime)
{
Agent.VO result = default(Agent.VO);
result.weightFactor = 1f;
result.weightBonus = Mathf.Max(radius, 1f) * 40f;
Vector3 vector = VectorMath.ClosestPointOnSegment(segmentStart, segmentEnd, Vector2.zero);
if (vector.magnitude <= radius)
{
result.colliding = true;
result.line1 = vector.normalized * (vector.magnitude - radius) * 0.3f * inverseDeltaTime;
Vector2 vector2 = new Vector2(result.line1.y, -result.line1.x);
result.dir1 = vector2.normalized;
result.line1 += offset;
result.cutoffDir = Vector2.zero;
result.cutoffLine = Vector2.zero;
result.dir2 = Vector2.zero;
result.line2 = Vector2.zero;
result.radius = 0f;
result.segmentStart = Vector2.zero;
result.segmentEnd = Vector2.zero;
result.segment = false;
}
else
{
result.colliding = false;
segmentStart *= inverseDt;
segmentEnd *= inverseDt;
radius *= inverseDt;
Vector2 normalized = (segmentEnd - segmentStart).normalized;
result.cutoffDir = normalized;
result.cutoffLine = segmentStart + new Vector2(-normalized.y, normalized.x) * radius;
result.cutoffLine += offset;
float sqrMagnitude = segmentStart.sqrMagnitude;
Vector2 a = -Agent.VO.ComplexMultiply(segmentStart, new Vector2(radius, Mathf.Sqrt(Mathf.Max(0f, sqrMagnitude - radius * radius)))) / sqrMagnitude;
float sqrMagnitude2 = segmentEnd.sqrMagnitude;
Vector2 a2 = -Agent.VO.ComplexMultiply(segmentEnd, new Vector2(radius, -Mathf.Sqrt(Mathf.Max(0f, sqrMagnitude2 - radius * radius)))) / sqrMagnitude2;
result.line1 = segmentStart + a * radius + offset;
result.line2 = segmentEnd + a2 * radius + offset;
result.dir1 = new Vector2(a.y, -a.x);
result.dir2 = new Vector2(a2.y, -a2.x);
result.segmentStart = segmentStart;
result.segmentEnd = segmentEnd;
result.radius = radius;
result.segment = true;
}
return(result);
}
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;
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));
}
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));
}
