Высота уже сохраняется в live state

raw_pos_x = float(telemetry.get("PositionX", 0.0))
raw_pos_y = float(telemetry.get("PositionY", 0.0))  # высота
raw_pos_z = float(telemetry.get("PositionZ", 0.0))
raw_map_x, raw_map_y = forza_to_map(raw_pos_x, raw_pos_z)

self.snapshot = TelemetrySnapshot(
position_x=pos_x,
position_y=pos_y,
position_z=pos_z,
map_x=map_x,
map_y=map_y,
)

Поэтому высоту я не выбрасываю. Сейчас она в основном диагностическая, но дальше из нее можно сделать слой уточнений для сложных мест: проехать развязку вручную, записать X/Y/Z/heading и потом сказать графу: “вот здесь верхняя дорога, здесь нижняя, тут не соединять, тут рампа”.

Снап к ближайшей дороге - опасная штука

Чтобы построить маршрут, нужно сначала приклеить машину к дорожному графу. Это называется snap. Берем текущую позицию, ищем ближайший узел дороги и считаем, что машина на нем.

На прямой дороге все отлично. На развязке это превращается в рулетку. Машина может ехать по эстакаде, а ближайший узел на карте окажется на дороге под ней. Если взять только один ближайший узел - навигатор может начать маршрут из параллельной реальности.

Поэтому я ищу не один узел, а несколько кандидатов вокруг точки.

Не один ближайший узел, а список кандидатов

def nearest_graph_nodes(graph, x, y, limit=10, max_distance=900.0):
coords = graph["coords"]
spatial = graph.get("spatial", {})
cell_size = float(graph.get("spatial_cell_size", 320.0))
cx, cy = int(x // cell_size), int(y // cell_size)

candidates = []
seen = set()
for ring in range(max_ring + 1):
for gx in range(cx - ring, cx + ring + 1):
for gy in range(cy - ring, cy + ring + 1):
for idx in spatial.get((gx, gy), []):
if idx in seen:
continue
seen.add(idx)
nx, ny = coords[idx]
d = math.hypot(nx - x, ny - y)
if d <= max_distance:
candidates.append((d, idx))
candidates.sort(key=lambda item: item[0])
return [(idx, dist) for dist, idx in candidates[:limit]]

Компоненты графа: не каждый кусок дороги должен победить

После этого стартовые и целевые кандидаты перебираются парами. Пара отбрасывается, если узлы лежат в разных компонентах графа. Еще добавляется штраф за маленькую компоненту, потому что рядом с машиной может быть крошечный ложный фрагмент дороги, который случайно выжил после распознавания.

Это примерно как не доверять подозрительному “короткому пути” через двор, если он на самом деле нарисован одним пикселем и честным словом.

Выбор разумных пар старта и цели

candidate_pairs = []
for start_idx, start_snap in start_candidates:
for goal_idx, goal_snap in goal_candidates:
if component and component[start_idx] != component[goal_idx]:
continue

comp_size = graph.get("component_sizes", [0])[component[start_idx]] if component else 0
component_penalty = 600.0 / math.sqrt(max(1, comp_size))
candidate_pairs.append((
start_snap + goal_snap + component_penalty,
start_idx, goal_idx, start_snap, goal_snap,
))

candidate_pairs.sort(key=lambda item: item[0])

Разрывы дорог и синтетические мостики

Карта не обязана быть удобной для алгоритма. Дорогу может перекрыть иконка, подпись, декоративный элемент, шов тайла. Человек видит, что дорога продолжается. Граф видит: “связи нет, до свидания”.

Для таких мест появились синтетические мостики. Но тут важный момент: они должны быть дорогими. Если сделать заплатки дешевыми, навигатор быстро превратится в короля телепортов. Поэтому короткая заплатка терпимая, средняя дорогая, длинная почти запретительная.

Заплатка есть, но она не должна стать любимой дорогой

ROAD_CLASS_MULTIPLIERS = {
"white": 1.00,
"orange": 1.08,
"orange_dashed": 2.60,
"synthetic_short": 2.40,
"synthetic_medium": 7.00,
"synthetic_bridge": 34.00,
"unknown": 9.50,
}


def _synthetic_multiplier_for_length(length, profile):
if length <= 70.0:
return float(profile.get("synthetic_short", 34.0))
if length <= 180.0:
return float(profile.get("synthetic_medium", 34.0))
return float(profile.get("synthetic_bridge", 34.0))

Один стиль маршрута не спасает

Еще выяснилось, что “идеального профиля” нет. Если слишком любить белые дороги, маршрут делает абсурдную петлю, лишь бы не использовать короткий оранжевый соединитель. Если разрешить все подряд, он начинает вести себя как раллист, которому сказали, что физика сегодня выходная.

Поэтому появились профили маршрута. Практичный асфальт, режим выхода из идиотского объезда и shortest sane - кратчайший вариант, но без полной потери совести.

Несколько профилей маршрутизации вместо одного самоуверенного

ROUTING_COST_PROFILES = {
"asphalt_practical": {
"white": 1.00,
"orange": 1.08,
"orange_dashed": 2.60,
"synthetic_short": 2.40,
"synthetic_medium": 7.00,
"synthetic_bridge": 34.00,
"heuristic_weight": 1.45,
},
"detour_escape": {
"white": 1.00,
"orange": 1.02,
"orange_dashed": 1.85,
"synthetic_short": 1.65,
"heuristic_weight": 2.10,
},
"shortest_sane": {
"white": 1.00,
"orange": 1.00,
"orange_dashed": 1.25,
"synthetic_short": 1.25,
"heuristic_weight": 3.00,
},
}

Лучше честная ошибка, чем красивая ложь

В ранних версиях очень хотелось, чтобы маршрут был всегда. Даже если граф не справился - ну нарисуем прямую линию, пользователь поймет.

Нет, пользователь не поймет. Прямая линия на экране выглядит как маршрут. Если она ведет сквозь все подряд, это уже не fallback, а ложь с хорошим дизайном.

Поэтому сейчас, если граф есть, но путь не найден, сервер не рисует диагональ. Он возвращает ошибку и пустую polyline. Лучше честно сказать “дорожный маршрут не найден”, чем нарисовать уверенную ерунду.

Честный fail вместо фальшивого маршрута

def graph_failed(mode: str, message: str, routing=None):
# Do not silently draw a diagonal when a road graph exists but cannot produce a path.
# A diagonal fallback looked like a real route and made debugging impossible.
return {
"ok": False,
"mode": mode,
"polyline": [],
"message": message,
"routing": routing or {},
}

Reroute тоже нельзя делать истеричным

Навигатор должен перестраивать маршрут, когда ты уехал не туда. Но если он будет перестраивать его при каждом чихе, получится не помощник, а тревожный диспетчер.

Поэтому появилась липкость. Чуть съехал с линии - навигатор сначала держит маршрут и просит вернуться. Сильно уехал или долго едешь мимо - пересчитывает. Это звучит как мелочь, но именно такие мелочи отличают “работает” от “почему оно дергается каждые две секунды”.

Липкое перестроение маршрута

const REROUTE_STICKY_OFF_PX = 170;
const REROUTE_STICKY_MS = 22000;
const REROUTE_HARD_OFF_PX = 360;
const REROUTE_HARD_MS = 9000;

function maybeRerouteNavigation(){
const off = distanceToCurrentRoute(px, py);

if(off <= REROUTE_STICKY_OFF_PX){
offRouteSince = 0;
return;
}

if(!offRouteSince) offRouteSince = now;
const offMs = now - offRouteSince;

const shouldReroute =
(off > REROUTE_HARD_OFF_PX && offMs > REROUTE_HARD_MS) ||
(off > REROUTE_STICKY_OFF_PX && offMs > REROUTE_STICKY_MS);

if(shouldReroute) requestRoute(routeTarget, false);
}

Итог серии

Вот почему развязки - это ад. Не потому, что их сложно красиво нарисовать. А потому что “дорога рядом” и “дорога доступна” - разные вещи.

Текущее решение - это набор защит: несколько кандидатов для snap, компоненты графа, штрафы, дорогие синтетические мостики, разные профили маршрута, честный fail и неистеричный reroute.

Следующий логичный уровень - Z-логгер. Проехать сложные развязки вручную, записать высоту и фактическую траекторию, а потом использовать это как слой уточнения. Игра высоту уже отдает. Осталось заставить ее работать на нас, а не просто лежать красивой цифрой в JSON.

В следующей серии будет менее больно и более красиво: приборка на телефоне, семисегментные цифры, мини-карта, ретро-киберпанк и мемные звуки на столкновения и прыжки.