public static Int32 GetClosestPositionInList(List <Location> waypoints, Location pos)
{
int result = 0;
Double nearest = -1;
int i = 0;
foreach (double distance in waypoints.Select(p => LazyMath.Distance3D(pos.X, pos.Y, pos.Z, p.X, p.Y, p.Z)))
{
if (nearest == -1 || distance < nearest)
{
result = i;
nearest = distance;
}
i++;
}
return(result);
}