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