chore(tools): AD-vejnet-værktøjer (45°-kurver, parkering, reparationer) brugt på savegame2 30/9
This commit is contained in:
@@ -0,0 +1,222 @@
|
||||
"""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])
|
||||
Reference in New Issue
Block a user