/** * <summary>Inserts a static obstacle neighbor into the set of neighbors * of this agent.</summary> * * <param name="obstacle">The number of the static obstacle to be * inserted.</param> * <param name="rangeSq">The squared range around this agent.</param> */ internal void InsertObstacleNeighbor(Obstacle obstacle, float rangeSq) { Obstacle nextObstacle = obstacle.Next; float distSq = RVOMath.DistSqPointLineSegment(obstacle.Point, nextObstacle.Point, Position); if (distSq < rangeSq) { ObstacleNeighbors.Add(new KeyValuePair <float, Obstacle>(distSq, obstacle)); int i = ObstacleNeighbors.Count - 1; while (i != 0 && distSq < ObstacleNeighbors[i - 1].Key) { ObstacleNeighbors[i] = ObstacleNeighbors[i - 1]; --i; } ObstacleNeighbors[i] = new KeyValuePair <float, Obstacle>(distSq, obstacle); } }