223 lines
9.3 KiB
Python
223 lines
9.3 KiB
Python
"""Fjern sving > MAX_TURN på AD-ruter mellem markører ved at tilføje kurve-omveje.
|
|
|
|
Kun nye waypoints tilføjes; eksisterende id'er, koordinater og forbindelser bevares
|
|
(kanter der deles får et mellempunkt på samme linje). AD's pathfinder vælger selv
|
|
kurven, fordi den er kortere og har mindre vinkel-straf.
|
|
|
|
Brug: adfix.py <ind.xml> <ud.xml> <log.json>
|
|
"""
|
|
import json, math, re, sys, collections
|
|
sys.path.insert(0, __file__.rsplit("/", 1)[0])
|
|
from adsmooth import read, write, dist, new_node, link, apply_splits
|
|
from adroutes import transit, dijkstra, angle, is_reverse
|
|
|
|
MAX_TURN = 45.5 # grænse (45° præcis er tilladt)
|
|
CURVE_STEP = 7.9 # max knæk i kurven (<8° => AD regner 27 km/h, så kurven vinder)
|
|
R_TRY = (8.0, 6.0, 4.5, 3.0, 2.0, 1.5, 1.0)
|
|
TARGET_EDGE = 7.9 # max knæk overalt i kurven inkl. ind-/udgang
|
|
|
|
|
|
def markers(doc):
|
|
return [int(float(i)) for i in re.findall(r"<mm\d+>\s*<id>([\d.]+)</id>", doc["s"])]
|
|
|
|
|
|
def node_path(best, pre, target):
|
|
s = best.get(target); seq = []
|
|
while s:
|
|
seq.append(s); s = pre[s]
|
|
seq.reverse()
|
|
return [seq[0][0]] + [b for _, b in seq] if seq else []
|
|
|
|
|
|
def collect(N, mks):
|
|
"""Returnér {(p,b,c): (antal, path, index_of_b)} for sving > MAX_TURN på ruter."""
|
|
T = transit(N); found = {}
|
|
cnt = collections.Counter()
|
|
for st in mks:
|
|
best, pre = dijkstra(N, T, st)
|
|
for tg in mks:
|
|
if tg == st or tg not in best:
|
|
continue
|
|
path = node_path(best, pre, tg)
|
|
backing = False
|
|
for i in range(1, len(path) - 1):
|
|
p, b, c = path[i - 1], path[i], path[i + 1]
|
|
a = angle(N, p, b, c)
|
|
# bakning: starter på en bakforbindelse (p ikke i b.incoming), slutter når retningen vender (>90°)
|
|
if p not in N[b]["inc"]:
|
|
backing = True
|
|
if backing:
|
|
if a > 90:
|
|
backing = False
|
|
continue
|
|
# vendepunktet lige før baklængs-kørsel (bakforbindelse c -> næste) er bevidst skarpt
|
|
if i + 2 < len(path) and c not in N[path[i + 2]]["inc"]:
|
|
continue
|
|
if a > MAX_TURN and not is_reverse(N, p, b, c):
|
|
key = (p, b, c) + window(N, path, i)
|
|
cnt[key] += 1
|
|
back = sum(dist(N[path[j]], N[path[j + 1]]) for j in range(i))
|
|
fwd = sum(dist(N[path[j]], N[path[j + 1]]) for j in range(i, len(path) - 1))
|
|
cur = found.get(key)
|
|
if cur is None:
|
|
found[key] = [(path, i, back), (path, i, fwd)]
|
|
else:
|
|
if back < cur[0][2]: cur[0] = (path, i, back)
|
|
if fwd < cur[1][2]: cur[1] = (path, i, fwd)
|
|
return {k: (cnt[k], v[0], v[1]) for k, v in found.items()}
|
|
|
|
|
|
def window(N, path, i, reach=8.5):
|
|
"""Rutens noder inden for `reach` meter før og efter path[i] (skelner grene)."""
|
|
lo = i; acc = 0.0
|
|
while lo > 0 and acc < reach:
|
|
acc += dist(N[path[lo - 1]], N[path[lo]]); lo -= 1
|
|
hi = i; acc = 0.0
|
|
while hi < len(path) - 1 and acc < reach:
|
|
acc += dist(N[path[hi]], N[path[hi + 1]]); hi += 1
|
|
return (tuple(path[lo:i]), tuple(path[i + 1:hi + 1]))
|
|
|
|
|
|
def point_along(N, path, i, R, direction):
|
|
"""Gå R meter fra path[i] bagud (-1) eller fremad (+1). Returnér
|
|
(u, v, d_from_u) for punktet på kanten u->v i kørselsretning, eller None."""
|
|
acc = 0.0; j = i
|
|
while True:
|
|
k = j + direction
|
|
if k < 0 or k >= len(path):
|
|
return None
|
|
a, b = (path[k], path[j]) if direction < 0 else (path[j], path[k])
|
|
L = dist(N[a], N[b])
|
|
if acc + L >= R:
|
|
rest = R - acc # meter fra path[j] mod path[k]
|
|
return (a, b, L - rest) if direction < 0 else (a, b, rest)
|
|
acc += L; j = k
|
|
|
|
|
|
def tangent(N, u, v):
|
|
L = dist(N[u], N[v])
|
|
return ((N[v]["x"] - N[u]["x"]) / L, (N[v]["z"] - N[u]["z"]) / L)
|
|
|
|
|
|
def pos_on(N, u, v, d):
|
|
t = d / dist(N[u], N[v])
|
|
return {a: N[u][a] + (N[v][a] - N[u][a]) * t for a in "xyz"}
|
|
|
|
|
|
def ang2(v1, v2):
|
|
l1 = math.hypot(*v1); l2 = math.hypot(*v2)
|
|
if l1 < 1e-6 or l2 < 1e-6:
|
|
return 0.0
|
|
return math.degrees(math.acos(max(-1, min(1, (v1[0] * v2[0] + v1[1] * v2[1]) / (l1 * l2)))))
|
|
|
|
|
|
def design(N, back, fwd, R):
|
|
"""Kubisk Bézier fra punkt R før b til R efter b. back/fwd = (path, i, plads) for
|
|
de ruter der har mindst plads før hhv. efter b. Returnér plan eller None."""
|
|
a = point_along(N, back[0], back[1], R, -1); b = point_along(N, fwd[0], fwd[1], R, +1)
|
|
if not a or not b:
|
|
return None
|
|
(u1, v1, d1), (u2, v2, d2) = a, b
|
|
# ligger punktet (næsten) oven i en eksisterende node, bruges noden selv
|
|
snap = lambda u, v, d: ("node", u) if d < 0.3 else ("node", v) if dist(N[u], N[v]) - d < 0.3 else ("split", u, v, d)
|
|
e_in, e_out = snap(u1, v1, d1), snap(u2, v2, d2)
|
|
if e_in[0] == "node":
|
|
d1 = 0.0 if e_in[1] == u1 else dist(N[u1], N[v1])
|
|
if e_out[0] == "node":
|
|
d2 = 0.0 if e_out[1] == u2 else dist(N[u2], N[v2])
|
|
P0 = pos_on(N, u1, v1, d1); P3 = pos_on(N, u2, v2, d2)
|
|
t0 = tangent(N, u1, v1); t3 = tangent(N, u2, v2)
|
|
# snappet til en node: retningen skal matche rutens kant IND i (start) / UD af (slut) noden
|
|
bp, fp = back[0], fwd[0]
|
|
if e_in[0] == "node" and e_in[1] == u1:
|
|
k = bp.index(u1)
|
|
if k == 0:
|
|
return None
|
|
t0 = tangent(N, bp[k - 1], u1)
|
|
if e_out[0] == "node" and e_out[1] == v2:
|
|
k = fp.index(v2)
|
|
if k >= len(fp) - 1:
|
|
return None
|
|
t3 = tangent(N, v2, fp[k + 1])
|
|
chord = math.hypot(P3["x"] - P0["x"], P3["z"] - P0["z"])
|
|
k = chord * 0.4
|
|
P1 = {"x": P0["x"] + t0[0] * k, "z": P0["z"] + t0[1] * k, "y": P0["y"] + (P3["y"] - P0["y"]) / 3}
|
|
P2 = {"x": P3["x"] - t3[0] * k, "z": P3["z"] - t3[1] * k, "y": P0["y"] + 2 * (P3["y"] - P0["y"]) / 3}
|
|
total = ang2(t0, t3)
|
|
n = max(3, math.ceil(total / CURVE_STEP) + 2)
|
|
while True:
|
|
pts = []
|
|
for s in range(n + 1):
|
|
t = s / n
|
|
pts.append({ax: (1 - t) ** 3 * P0[ax] + 3 * (1 - t) ** 2 * t * P1[ax] + 3 * (1 - t) * t * t * P2[ax] + t ** 3 * P3[ax] for ax in "xyz"})
|
|
# tjek alle knæk inkl. ind- og udgang
|
|
dirs = [t0] + [(pts[s + 1]["x"] - pts[s]["x"], pts[s + 1]["z"] - pts[s]["z"]) for s in range(n)] + [t3]
|
|
worst = max(ang2(dirs[s], dirs[s + 1]) for s in range(len(dirs) - 1))
|
|
if worst <= TARGET_EDGE:
|
|
break
|
|
n += 1
|
|
if n > 80 or chord / n < 0.15:
|
|
return None
|
|
return dict(end_in=e_in, end_out=e_out, split_in=(u1, v1, d1), split_out=(u2, v2, d2), inner=pts[1:-1], R=R, total=round(total, 1), worst=round(worst, 1))
|
|
|
|
|
|
def run(src, dst, logp):
|
|
doc = read(src); N = doc["N"]; n0 = len(N)
|
|
mks = [m for m in markers(doc) if m in N]
|
|
turns = collect(N, mks)
|
|
order = sorted(turns.items(), key=lambda kv: -kv[1][0])
|
|
plans = []; covered = set(); skipped = []
|
|
for key, (cnt, back, fwd) in order:
|
|
p, b, c = key[:3]
|
|
if key in covered:
|
|
continue
|
|
path, i = back[0], back[1]
|
|
plan = None
|
|
for R in R_TRY:
|
|
if R > back[2] - 0.3 or R > fwd[2] - 0.3:
|
|
continue
|
|
plan = design(N, back, fwd, R)
|
|
if plan:
|
|
break
|
|
if not plan:
|
|
skipped.append([p, b, c, cnt, round(angle(N, p, b, c), 1)]); continue
|
|
# marker alle sving i vinduet som dækket (fx 63°+27° i samme hjørne)
|
|
lo = path.index(plan["split_in"][1]) if plan["split_in"][1] in path else i
|
|
hi = path.index(plan["split_out"][0]) if plan["split_out"][0] in path else i
|
|
for j in range(max(1, lo), min(len(path) - 1, hi + 1)):
|
|
covered.add((path[j - 1], path[j], path[j + 1]) + window(N, path, j))
|
|
plan.update(turn=[p, b, c], count=cnt); plans.append(plan)
|
|
# 1) del kanter (samlet pr. kant), 2) byg kurver
|
|
req = {}; keyof = []
|
|
for pl in plans:
|
|
ks = []
|
|
for e in (pl["end_in"], pl["end_out"]):
|
|
if e[0] == "node":
|
|
ks.append(("node", e[1])); continue
|
|
_, u, v, d = e
|
|
lo, hi = min(u, v), max(u, v)
|
|
dd = round(d if u == lo else dist(N[u], N[v]) - d, 3)
|
|
req.setdefault((lo, hi), set()).add(dd); ks.append((lo, hi, dd))
|
|
keyof.append(ks)
|
|
made = apply_splits(N, req)
|
|
for pl, ks in zip(plans, keyof):
|
|
res = lambda k: k[1] if k[0] == "node" else made[k]
|
|
a, b = res(ks[0]), res(ks[1])
|
|
# bivej (bit 1) kun hvis begge ender er bivej — ellers ganger AD kurven med 20
|
|
sub = N[a]["fl"] & N[b]["fl"] & 1
|
|
fl = (N[pl["turn"][1]]["fl"] & ~1) | sub
|
|
chain = [a] + [new_node(N, q["x"], q["y"], q["z"], fl) for q in pl["inner"]] + [b]
|
|
for x, y in zip(chain, chain[1:]):
|
|
link(N, x, y)
|
|
pl["nodes"] = chain
|
|
del pl["inner"]
|
|
write(doc, dst)
|
|
json.dump(dict(plans=plans, skipped=skipped), open(logp, "w"))
|
|
print(f"sving>{MAX_TURN}°: {len(turns)} kurver: {len(plans)} dækket af andre kurver: {len(turns)-len(plans)-len(skipped)}"
|
|
f" sprunget over: {len(skipped)} nye noder: {len(N)-n0}")
|
|
return len(turns)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
run(*sys.argv[1:4])
|