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