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); }