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