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); }
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 static void DrawCross(Vector2 p, float size = 1f) { Agent.DrawCross(p, Color.white, size); }
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)); }