Pathfinding.RVO.Sampled.Agent.InsertObstacleNeighbour C# (CSharp) Method

		public void InsertObstacleNeighbour (ObstacleVertex ob1, float rangeSq) {
			ObstacleVertex ob2 = ob1.next;

			float dist = VectorMath.SqrDistancePointSegment(ob1.position, ob2.position, Position);

			if (dist < rangeSq) {
				obstacles.Add(ob1);
				obstacleDists.Add(dist);

				int i = obstacles.Count-1;
				while (i != 0 && dist < obstacleDists[i-1]) {
					obstacles[i] = obstacles[i-1];
					obstacleDists[i] = obstacleDists[i-1];
					i--;
				}
				obstacles[i] = ob1;
				obstacleDists[i] = dist;
			}
		}