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

		public float InsertAgentNeighbour (Agent agent, float rangeSq) {
			if (this == agent) return rangeSq;

			if ((agent.layer & collidesWith) == 0) return rangeSq;

			//2D Dist
			float dist = Sqr(agent.position.x-position.x) + Sqr(agent.position.z - position.z);

			if (dist < rangeSq) {
				if (neighbours.Count < maxNeighbours) {
					neighbours.Add(agent);
					neighbourDists.Add(dist);
				}

				int i = neighbours.Count-1;
				if (dist < neighbourDists[i]) {
					while (i != 0 && dist < neighbourDists[i-1]) {
						neighbours[i] = neighbours[i-1];
						neighbourDists[i] = neighbourDists[i-1];
						i--;
					}
					neighbours[i] = agent;
					neighbourDists[i] = dist;
				}

				if (neighbours.Count == maxNeighbours) {
					rangeSq = neighbourDists[neighbourDists.Count-1];
				}
			}
			return rangeSq;
		}