160 lines
6.1 KiB
Python
160 lines
6.1 KiB
Python
"""Blødgør skarpe sving (>45°) i et AutoDrive-vejnet ved at tilføje kurve-omveje.
|
|
|
|
Eksisterende waypoints og id'er røres ikke (kun nye noder tilføjes), så markører og
|
|
mod-referencer holder. For hvert sving p->b->c:
|
|
- kanten p-b deles i Pin (r meter før b), kanten b-c deles i Pout (r meter efter b)
|
|
- Pin -> kvadratisk Bézier (kontrolpunkt b) -> Pout, med nok punkter til at hvert
|
|
knæk er <= MAX_STEP grader
|
|
- er det omvendte sving (c,b,p) også på listen, bliver kurven tovejs
|
|
"""
|
|
import json, math, re, sys
|
|
|
|
MAX_STEP = 20.0 # grader pr. knæk inde i kurven
|
|
R_CAP = 10.0 # max benlængde (m) -> max afvigelse fra hjørnet ~3,2 m
|
|
R_FRAC = 0.45 # max andel af en kant der bruges til benet
|
|
R_MIN = 1.5 # kortere ben end dette -> spring over
|
|
|
|
|
|
def read(path):
|
|
raw = open(path, "rb").read()
|
|
bom = raw.startswith(b"\xef\xbb\xbf")
|
|
s = raw.decode("utf-8-sig")
|
|
w0 = s.index("<waypoints>"); w1 = s.index("</waypoints>")
|
|
w = s[w0:w1]
|
|
g = lambda t: re.search(rf"<{t}>(.*?)</{t}>", w, re.S).group(1)
|
|
ids = [int(v) for v in g("id").split(",")]
|
|
assert ids == list(range(1, len(ids) + 1)), "id'er er ikke 1..N"
|
|
xs = [float(v) for v in g("x").split(",")]
|
|
ys = [float(v) for v in g("y").split(",")]
|
|
zs = [float(v) for v in g("z").split(",")]
|
|
lst = lambda t: [[int(v) for v in e.split(",") if v != "-1"] for e in g(t).split(";")]
|
|
out, inc = lst("out"), lst("incoming")
|
|
fl = [int(v) for v in g("flags").split(",")]
|
|
N = {i: dict(x=xs[k], y=ys[k], z=zs[k], out=out[k], inc=inc[k], fl=fl[k]) for k, i in enumerate(ids)}
|
|
return dict(s=s, bom=bom, w0=w0, w1=w1, w=w, N=N)
|
|
|
|
|
|
def write(doc, path):
|
|
N = doc["N"]; ids = sorted(N)
|
|
f3 = lambda v: "%.3f" % v
|
|
joinl = lambda L: ",".join(map(str, L)) if L else "-1"
|
|
parts = {
|
|
"id": ",".join(map(str, ids)),
|
|
"x": ",".join(f3(N[i]["x"]) for i in ids),
|
|
"y": ",".join(f3(N[i]["y"]) for i in ids),
|
|
"z": ",".join(f3(N[i]["z"]) for i in ids),
|
|
"out": ";".join(joinl(N[i]["out"]) for i in ids),
|
|
"incoming": ";".join(joinl(N[i]["inc"]) for i in ids),
|
|
"flags": ",".join(str(N[i]["fl"]) for i in ids),
|
|
}
|
|
w = doc["w"]
|
|
for t, v in parts.items():
|
|
w = re.sub(rf"<{t}>.*?</{t}>", lambda m, v=v, t=t: f"<{t}>{v}</{t}>", w, count=1, flags=re.S)
|
|
s = doc["s"][:doc["w0"]] + w + doc["s"][doc["w1"]:]
|
|
data = s.encode("utf-8")
|
|
open(path, "wb").write((b"\xef\xbb\xbf" if doc["bom"] else b"") + data)
|
|
|
|
|
|
def xz(n):
|
|
return (n["x"], n["z"])
|
|
|
|
|
|
def dist(a, b):
|
|
return math.hypot(a["x"] - b["x"], a["z"] - b["z"])
|
|
|
|
|
|
def new_node(N, x, y, z, fl):
|
|
i = max(N) + 1
|
|
N[i] = dict(x=x, y=y, z=z, out=[], inc=[], fl=fl)
|
|
return i
|
|
|
|
|
|
def link(N, a, b):
|
|
if b not in N[a]["out"]:
|
|
N[a]["out"].append(b)
|
|
if a not in N[b]["inc"]:
|
|
N[b]["inc"].append(a)
|
|
|
|
|
|
def apply_splits(N, requests):
|
|
"""requests: {(u,v) u<v: set(afstand fra u)} -> {(u,v,afstand): ny node}."""
|
|
made = {}
|
|
for (u, v), ds in requests.items():
|
|
L = dist(N[u], N[v])
|
|
pts = []
|
|
for d in sorted(ds):
|
|
if pts and d - pts[-1] < 0.5:
|
|
made[(u, v, d)] = made[(u, v, pts[-1])]; continue
|
|
t = d / L
|
|
m = new_node(N, *(N[u][a] + (N[v][a] - N[u][a]) * t for a in "xyz"),
|
|
N[v]["fl"] if t > 0.5 else N[u]["fl"])
|
|
made[(u, v, d)] = m; pts.append(d)
|
|
chain = [u] + [made[(u, v, d)] for d in pts] + [v]
|
|
for a, b in ((u, v), (v, u)):
|
|
if b in N[a]["out"]:
|
|
seq = chain if a == u else chain[::-1]
|
|
N[a]["out"] = [seq[1] if o == b else o for o in N[a]["out"]]
|
|
N[b]["inc"] = [seq[-2] if o == a else o for o in N[b]["inc"]]
|
|
for x, y in zip(seq[1:-1], seq[2:]):
|
|
link(N, x, y)
|
|
N[seq[1]]["inc"].append(a) if a not in N[seq[1]]["inc"] else None
|
|
return made
|
|
|
|
|
|
def smooth(doc, turns):
|
|
N = doc["N"]; want = set(turns); plan = []; req = {}; seen = set()
|
|
for p, b, c in turns:
|
|
if (p, b, c) in seen:
|
|
continue
|
|
dual = (c, b, p) in want
|
|
seen.add((p, b, c)); seen.add((c, b, p)) if dual else None
|
|
r = min(R_CAP, R_FRAC * dist(N[p], N[b]), R_FRAC * dist(N[b], N[c]))
|
|
if r < R_MIN:
|
|
continue
|
|
keys = []
|
|
for o in (p, c):
|
|
u, v = min(o, b), max(o, b)
|
|
d = r if u == b else dist(N[u], N[v]) - r
|
|
req.setdefault((u, v), set()).add(round(d, 3)); keys.append((u, v, round(d, 3)))
|
|
plan.append((p, b, c, dual, r, keys))
|
|
orig = {i: dict(N[i]) for i in N}
|
|
made_split = apply_splits(N, req)
|
|
made = []
|
|
for p, b, c, dual, r, keys in plan:
|
|
pin, pout = made_split[keys[0]], made_split[keys[1]]
|
|
P0, P1, P2 = N[pin], N[b], N[pout]
|
|
th = turn_angle(orig[p], orig[b], orig[c])
|
|
n = max(2, math.ceil(th / MAX_STEP) + 1)
|
|
chain = [pin]
|
|
for k in range(1, n):
|
|
t = k / n
|
|
q = lambda a: (1 - t) ** 2 * P0[a] + 2 * (1 - t) * t * P1[a] + t * t * P2[a]
|
|
chain.append(new_node(N, q("x"), q("y"), q("z"), N[b]["fl"]))
|
|
chain.append(pout)
|
|
for a, bb in zip(chain, chain[1:]):
|
|
link(N, a, bb)
|
|
if dual:
|
|
link(N, bb, a)
|
|
made.append(dict(turn=[p, b, c], dual=dual, angle=round(th, 1), r=round(r, 1), nodes=chain))
|
|
return made
|
|
|
|
|
|
def turn_angle(A, B, C):
|
|
v1 = (B["x"] - A["x"], B["z"] - A["z"]); v2 = (C["x"] - B["x"], C["z"] - B["z"])
|
|
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)))))
|
|
|
|
|
|
if __name__ == "__main__":
|
|
src, sharp_json, dst, log = sys.argv[1:5]
|
|
doc = read(src)
|
|
n0 = len(doc["N"])
|
|
turns = [tuple(x[:3]) for x in json.load(open(sharp_json)) if x[4] > 45.5]
|
|
made = smooth(doc, turns)
|
|
write(doc, dst)
|
|
json.dump(made, open(log, "w"), indent=0)
|
|
print(f"sving: {len(turns)} kurver lavet: {len(made)} (tovejs {sum(m['dual'] for m in made)})"
|
|
f" nye noder: {len(doc['N']) - n0} sprunget over: {len(turns) - len(made) - sum(m['dual'] for m in made)}")
|