Как найти ближайшую 2d точку / баллы

Question

Я пытаюсь найти кратчайший путь на карте, используя модифицированный алгоритм A * с учетом динамики моего робота. Мое пространство поиска равно 2d, как и запланированный путь. Но моя карта динамическая c, и в начале каждого l oop карта немного меняется.

Если я пытаюсь найти путь в каждом l oop, fps падает ниже нуля, так как мое пространство поиска очень большое. То, что я хочу сделать, это проверить, является ли путь, нанесенный на предыдущем пути, по-прежнему действительным или нет, как в большинстве случаев.

Для этого я сделал следующее - Здесь Массив препятствий и массив содержат куча 2d очков (до 2000000)

def check_collision(path,obstacles):
max_distance = 5
collision = False
for i in len(path):
    for j in len(obstacles):
        distance = math.sqrt((path[i][0] - obstacles[j][0])**2 + (path[i][1] - obstacles[j][1])**2)
        if distance< max_distance:
            collision = True
            break

    return collison

Я думаю, что это сильно замедляет мои циклы

Есть ли более быстрый способ сделать это?