diff --git a/tools/ad-smooth/adfix.py b/tools/ad-smooth/adfix.py new file mode 100644 index 0000000..87c5db6 --- /dev/null +++ b/tools/ad-smooth/adfix.py @@ -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 +""" +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"\s*([\d.]+)", 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]) diff --git a/tools/ad-smooth/adprune.py b/tools/ad-smooth/adprune.py new file mode 100644 index 0000000..2eb9823 --- /dev/null +++ b/tools/ad-smooth/adprune.py @@ -0,0 +1,36 @@ +"""Fjern kurver som ingen markør-rute bruger, og omnummerér nye noder fortløbende.""" +import sys, json, re, collections +sys.path.insert(0, __file__.rsplit("/", 1)[0]) +import adfix +from adsmooth import read, write +from adroutes import transit, dijkstra + +src, dst, n0 = sys.argv[1], sys.argv[2], int(sys.argv[3]) +logs = sys.argv[4:] +doc = read(src); N = doc["N"] +mks = [m for m in adfix.markers(doc) if m in N] +T = transit(N); used = collections.Counter() +for st in mks: + best, pre = dijkstra(N, T, st) + for tg in mks: + if tg != st and tg in best: + for x in adfix.node_path(best, pre, tg): + if x > n0: + used[x] += 1 +drop = set() +for lp in logs: + for p in json.load(open(lp))["plans"]: + inner = p["nodes"][1:-1] + if inner and not any(used[x] for x in inner): + drop.update(inner) +for i in drop: + del N[i] +for n in N.values(): + n["out"] = [o for o in n["out"] if o not in drop] + n["inc"] = [o for o in n["inc"] if o not in drop] +old_new = sorted(i for i in N if i > n0) +remap = {o: n0 + 1 + k for k, o in enumerate(old_new)} +f = lambda i: remap.get(i, i) +doc["N"] = {f(i): dict(n, out=[f(o) for o in n["out"]], inc=[f(o) for o in n["inc"]]) for i, n in N.items()} +write(doc, dst) +print(f"fjernet {len(drop)} ubrugte kurvenoder; noder nu {len(doc['N'])} (nye: {len(doc['N']) - n0})") diff --git a/tools/ad-smooth/adroutes.py b/tools/ad-smooth/adroutes.py new file mode 100644 index 0000000..9e4efb2 --- /dev/null +++ b/tools/ad-smooth/adroutes.py @@ -0,0 +1,68 @@ +import re,math,heapq,collections,sys,json +sys.path.insert(0, __file__.rsplit("/", 1)[0]) +from adturns import load +def angle(N,p,b,c): + v1=(N[b]["x"]-N[p]["x"],N[b]["z"]-N[p]["z"]); v2=(N[c]["x"]-N[b]["x"],N[c]["z"]-N[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))))) +def speed(a): + for lim,sp in ((3,50),(5,38),(8,27),(12,20),(15,13),(20,10),(30,7)): + if adist.get(s,1e18): continue + p,b=s + if b not in best or d alle udgange er tilladt + for c in T.get(s,N[b]["out"]): + a=angle(N,p,b,c); l=math.hypot(N[c]["x"]-N[b]["x"],N[c]["z"]-N[b]["z"]) + nd=d+l/(speed(a)/3.6)*(20 if N[c]["fl"]&1 else 1) + ns=(b,c) + if nd\s*([\d.]+)\s*([^<]*)\s*([^<]*)",s)] + T=transit(N) + ids=[m[0] for m in mm if m[0] in N] + sharp=collections.Counter(); rev=0 + for k,st in enumerate(ids): + best,pre=dijkstra(N,T,st) + for tg in ids: + if tg==st or tg not in best: continue + for (p,b,c) in path_turns(N,best,pre,tg): + a=angle(N,p,b,c) + if a>45: + if is_reverse(N,p,b,c): rev+=1; continue + sharp[(p,b,c)]+=1 + json.dump([[p,b,c,n,round(angle(N,p,b,c),1)] for (p,b,c),n in sharp.items()],open(sys.argv[2],"w")) + print("markører",len(ids),"forskellige skarpe sving på ruter:",len(sharp),"bakke-sving sprunget over:",rev) + print(collections.Counter(int(angle(N,*k)//5*5) for k in sharp)) diff --git a/tools/ad-smooth/adsmooth.py b/tools/ad-smooth/adsmooth.py new file mode 100644 index 0000000..bd9995d --- /dev/null +++ b/tools/ad-smooth/adsmooth.py @@ -0,0 +1,159 @@ +"""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(""); w1 = s.index("") + w = s[w0:w1] + g = lambda t: re.search(rf"<{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}>.*?", lambda m, v=v, t=t: f"<{t}>{v}", 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 {(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)}") diff --git a/tools/ad-smooth/adturns.py b/tools/ad-smooth/adturns.py new file mode 100644 index 0000000..22424ca --- /dev/null +++ b/tools/ad-smooth/adturns.py @@ -0,0 +1,36 @@ +import re,math,collections,sys +def load(path): + s=open(path,encoding="utf-8-sig").read() + w=re.search(r"(.*?)",s,re.S).group(1) + g=lambda t:re.search(rf"<{t}>(.*?)",w,re.S).group(1) + ids=[int(v) for v in g("id").split(",")] + X=[float(v) for v in g("x").split(",")]; Z=[float(v) for v in g("z").split(",")] + sp=lambda t:[[int(v) for v in e.split(",") if v not in("","-1")] for e in g(t).split(";")] + out=sp("out"); inc=sp("incoming"); fl=[int(v) for v in g("flags").split(",")] + N={i:dict(x=X[k],z=Z[k],out=out[k],inc=inc[k],fl=fl[k]) for k,i in enumerate(ids)} + mk=[(int(a),b) for a,b in re.findall(r'b og b->c er reelle kanter + if b not in N[a]["out"]: continue + t,l1,l2=ang(N,a,b,c) + if t>135: rev+=1; continue # U-vending / bakke-par, ikke et sving + if t>45: sharp.append((round(t),a,b,c,len(n["inc"]),len(n["out"]),round(l1,1),round(l2,1))) + kinds=collections.Counter("kæde" if (x[4]==1 and x[5]==1) else "kryds" for x in sharp) + print("sving >45°:",len(sharp),kinds,"(>135° udeladt:",rev,")") + print("fordeling:",collections.Counter(min(x[0]//15*15,135) for x in sharp)) + nodes={x[2] for x in sharp}; print("unikke knæk-noder",len(nodes)) diff --git a/tools/ad-smooth/netfix.py b/tools/ad-smooth/netfix.py new file mode 100644 index 0000000..936f5dc --- /dev/null +++ b/tools/ad-smooth/netfix.py @@ -0,0 +1,66 @@ +"""Savegame2 30/9: reparationer af AD-vejnettet fundet ved gennemgangen. + +1. Punkt 19564 (spor til 'P Forage Wagon') lå 5,6 km væk -> midt mellem naboerne. +2. Biogas Drop 1/2/4/5 var blindgyder (kun vej ind) -> sidste kant gøres tovejs som Drop 3. +3. Chicken Wait 5: 81° knæk ved 12808 (AD tillader 80°) -> privat spor 12808..12815 lagt om i en blød kurve. +4. 'Food And More' lå på en afbrudt stump oven på vejen -> markøren flyttes til vejpunkt 8416. + +Brug: netfix.py +""" +import math, re, sys, os +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) +from adsmooth import read, write, link, dist + +src, dst = sys.argv[1], sys.argv[2] +doc = read(src); N = doc["N"] +mk = {m.group(2): int(float(m.group(1))) for m in re.finditer(r"\s*([\d.]+)\s*([^<]*)", doc["s"])} + +# 1 +a, b = N[19563], N[19565] +assert dist(N[19564], a) > 1000, "19564 ser allerede rigtig ud" +for ax in "xyz": + N[19564][ax] = (a[ax] + b[ax]) / 2 +print("19564 ->", round(N[19564]["x"], 2), round(N[19564]["z"], 2)) + +# 2 +for name in ("Biogas Drop 1", "Biogas Drop 2", "Biogas Drop 4", "Biogas Drop 5"): + w = mk[name]; assert not N[w]["out"], name + for p in list(N[w]["inc"]): + link(N, w, p) + print(name, "tovejs mod", N[w]["out"]) + +# 3 kurve fra 12802 (retning fra 12801) til 12697 (retning mod 12698) +chain = [12808, 12809, 12810, 12811, 12812, 12813, 12814, 12815] +for i in chain: + assert len(N[i]["inc"]) == 1 and len(N[i]["out"]) == 1, i +P0, P3 = N[12802], N[12697] +def unit(u, v): + L = dist(u, v); return ((v["x"] - u["x"]) / L, (v["z"] - u["z"]) / L) +t0, t3 = unit(N[12801], P0), unit(P3, N[12698]) +seq = [12801, 12802] + chain + [12697, 12698] +def ang(p, b, c): + v1 = (N[b]["x"] - N[p]["x"], N[b]["z"] - N[p]["z"]); v2 = (N[c]["x"] - N[b]["x"], N[c]["z"] - N[b]["z"]) + return math.degrees(math.acos(max(-1, min(1, (v1[0] * v2[0] + v1[1] * v2[1]) / (math.hypot(*v1) * math.hypot(*v2)))))) +def shape(k0, k3, g): + P1 = {"x": P0["x"] + t0[0] * k0, "z": P0["z"] + t0[1] * k0} + P2 = {"x": P3["x"] - t3[0] * k3, "z": P3["z"] - t3[1] * k3} + n = len(chain) + 1 + for j, i in enumerate(chain, 1): + t = (j / n) ** g # g skubber punkterne mod den skarpe ende + for ax in "xz": + N[i][ax] = (1 - t) ** 3 * P0[ax] + 3 * (1 - t) ** 2 * t * P1[ax] + 3 * (1 - t) * t * t * P2[ax] + t ** 3 * P3[ax] + N[i]["y"] = P0["y"] + (P3["y"] - P0["y"]) * t + return max(ang(*seq[i:i + 3]) for i in range(len(seq) - 2)) +D = dist(P0, P3) +best = min(((shape(a * D, b * D, g), a, b, g) for a in (0.2, 0.35, 0.5, 0.7, 0.9, 1.2) for b in (0.2, 0.35, 0.5, 0.7, 0.9, 1.2) for g in (0.7, 0.85, 1.0, 1.2))) +worst = shape(best[1] * D, best[2] * D, best[3]) +print("Chicken Wait 5-kurve: største knæk %.1f°" % worst) +assert worst <= 45 + +write(doc, dst) +# 4 markør +s = open(dst, encoding="utf-8-sig").read() +s, cnt = re.subn(r"()8481\.000000(\s*Food And More)", r"\g<1>8416.000000\g<2>", s) +assert cnt == 1 +open(dst, "wb").write(b"\xef\xbb\xbf" + s.encode("utf-8")) +print("Food And More -> 8416") diff --git a/tools/ad-smooth/parkall.py b/tools/ad-smooth/parkall.py new file mode 100644 index 0000000..0dc7f4b --- /dev/null +++ b/tools/ad-smooth/parkall.py @@ -0,0 +1,211 @@ +"""Savegame2 30/9: parkering til alle køretøjer + Sell Slurry-vognfejlen. + +- REX-vintraktorer: parkDestination = deres vinredskabs parkering (faste sæt). +- Motorkøretøjer uden plads (FH16 x3, Finsnitter): ledig bås omdøbes "P ", parkDestination sættes. + Køretøjet flyttes ikke — AutoDrive bakker det ind, når det parkeres. +- Frakoblede vogne uden plads: ledig bås + "Connect P " (nyt punkt delt ind på bakkesporet ved + vognens trækøje) og vognen stilles ind i båsen, bagende 0,5 m fra P. +- Sell Slurry 2: ploven kobles af og stilles på "P Plov"; flowets vogn bliver gylletanken på + "P Sell Slurry vogn 2". + +Skriver .new i savegame-mappen. Brug: parkall.py +""" +import math, os, re, sys, zipfile +import xml.etree.ElementTree as ET +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) +from adsmooth import read, write, apply_splits, dist + +SG = sys.argv[1].rstrip("/") + "/" +GAME = os.path.expanduser("~/.local/share/Steam/steamapps/common/Farming Simulator 25/") +MODS = os.path.expanduser("~/FS25-data/mods/") +REAR_GAP = 0.5 # vognens bagende så langt fra P (m) +KINGPIN_BACK = 1.2 # sættevogn: kongebolt bag forenden (ukalibreret skøn) + +# motorkøretøjer: driverName -> kolonne; vogne: fil -> (navn, kolonne, sættevogn?) +MOTOR = ["FH16 780 1", "FH16 780 2", "Milk Sale", "Finsnitter"] +TRAILERS = {"dpw1800": ("DPW 1800", "syd", False), "bigBodyS750": ("Big Body 750", "syd", False), + "gtw330": ("Overlæssevogn GTW 330", "syd", False), "steelDropDeck": ("Steel Drop Deck", "vest", True)} + + +def xmltext(fn): + if fn.startswith("data/"): + return open(GAME + fn, encoding="utf-8", errors="ignore").read() + mod, rest = fn.replace("$moddir$", "").split("/", 1) + return zipfile.ZipFile(MODS + mod + ".zip").read(rest).decode("utf-8", "ignore") + + +def size_of(fn): + m = re.search(r"]*>", xmltext(fn)).group(0) + L = float(re.search(r'length="([\d.]+)"', m).group(1)) + lo = re.search(r'lengthOffset="(-?[\d.]+)"', m) + return L, float(lo.group(1)) if lo else 0.0 + + +def heading(el): + r = [float(t) for t in el.find("component").get("rotation").split()] + return math.radians(r[1] + (180 if abs(r[0]) > 90 else 0)) + + +def angn(a): + return (a + math.pi) % (2 * math.pi) - math.pi + + +doc = read(SG + "AutoDrive_config.xml"); N = doc["N"] +mk = {} # mm-index -> dict(wp, name, group) +for m in re.finditer(r"\s*([\d.]+)\s*([^<]*)\s*([^<]*)", doc["s"]): + mk[int(m.group(1))] = dict(wp=int(float(m.group(2))), name=m.group(3), group=m.group(4)) +byname = {m["name"]: i for i, m in mk.items()} + +VT = ET.parse(SG + "vehicles.xml"); VR = VT.getroot() +veh = {v.get("uniqueId"): v for v in VR.iter("vehicle")} +def adname(v): + ad = v.find("AutoDrive"); return ad.get("driverName") if ad is not None else None +def park_of(v): + ad = v.find("AutoDrive"); p = ad.get("parkDestination") if ad is not None else None + return int(p) if p and p.lstrip("-").isdigit() and int(p) > 0 else None +def set_park(v, idx): + ad = v.find("AutoDrive") + if ad is None: + last = list(v)[-1]; ad = ET.SubElement(v, "AutoDrive"); ad.tail = last.tail; last.tail = "\n " + ad.set("parkDestination", str(idx)) +parent = {a.get("attachedVehicleUniqueId"): v.get("uniqueId") for v in VR.iter("vehicle") for a in v.iter("attachedImplement")} + + +def bay_axis(p_wp): + """Retning fra P ud mod båsens munding, og sporets noder sorteret efter afstand fra P.""" + P = N[p_wp] + # nabo-kæden fra P: gå til nabo længst væk, 6 skridt, og brug den retning + cur, prev = p_wp, None + for _ in range(6): + nb = [x for x in set(N[cur]["out"]) | set(N[cur]["inc"]) if x != prev] + if not nb: break + nxt = max(nb, key=lambda x: dist(N[x], P)); prev, cur = cur, nxt + e = math.atan2(N[cur]["x"] - P["x"], N[cur]["z"] - P["z"]) + return e + + +def along_lat(p_wp, e, x, z): + dx, dz = x - N[p_wp]["x"], z - N[p_wp]["z"] + return dx * math.sin(e) + dz * math.cos(e), dx * math.cos(e) - dz * math.sin(e) + + +def split_point_on_track(p_wp, e, d): + """(u, v, afstand fra min(u,v)) for punktet d m fra P på bakkesporet.""" + track = [] + for i, n in N.items(): + a, l = along_lat(p_wp, e, n["x"], n["z"]) + if abs(l) < 0.35 and -0.1 < a < 26: + track.append((a, i)) + track.sort() + for (a1, i1), (a2, i2) in zip(track, track[1:]): + linked = i2 in N[i1]["out"] or i1 in N[i2]["out"] + if linked and a1 <= d <= a2: + u, v = min(i1, i2), max(i1, i2) + du = d - (a1 if u == i1 else a2) + return u, v, round(abs(du), 3) + raise SystemExit(f"fandt ikke sporet {d:.1f} m fra P (wp {p_wp})") + + +def ground_offset(v): + c = v.find("component"); x, y, z = [float(t) for t in c.get("position").split()] + near = min(N.values(), key=lambda n: (n["x"] - x) ** 2 + (n["z"] - z) ** 2) + return y - near["y"] + + +def place(v, p_wp, e, root_along): + """Stil køretøjet med roden root_along m fra P langs e, front mod mundingen.""" + tx = N[p_wp]["x"] + root_along * math.sin(e); tz = N[p_wp]["z"] + root_along * math.cos(e) + ty = N[p_wp]["y"] + ground_offset(v) + cs = v.findall("component"); p0 = [float(t) for t in cs[0].get("position").split()] + d = angn(e - heading(v)) + for c in cs: + q = [float(t) for t in c.get("position").split()]; r = [float(t) for t in c.get("rotation").split()] + rx, rz = q[0] - p0[0], q[2] - p0[2] + nx = tx + rx * math.cos(d) + rz * math.sin(d); nz = tz - rx * math.sin(d) + rz * math.cos(d) + c.set("position", "%.3f %.3f %.3f" % (nx, q[1] - p0[1] + ty, nz)) + r[1] = (r[1] + math.degrees(d) + 180) % 360 - 180 + c.set("rotation", "%.2f %.2f %.2f" % tuple(r)) + + +free = {"syd": [], "vest": []} +for i, m in mk.items(): + if re.fullmatch(r"P Ledig \d+", m["name"]): + x, z = N[m["wp"]]["x"], N[m["wp"]]["z"] + if -2328 < z < -2320 and x > 1003: free["syd"].append(i) + elif 905 < x < 920: free["vest"].append(i) +free["syd"].sort(key=lambda i: N[mk[i]["wp"]]["x"]) +free["vest"].sort(key=lambda i: N[mk[i]["wp"]]["z"]) +renames, connects, log = {}, [], [] + +# 1) REX -> vinredskabets plads +for v in VR.iter("vehicle"): + if v.get("filename", "").endswith("seriesREX4.xml") and park_of(v) is None: + kids = [a.get("attachedVehicleUniqueId") for a in v.iter("attachedImplement")] + pk = [park_of(veh[k]) for k in kids if k in veh and park_of(veh[k])] + if pk: + set_park(v, pk[0]); log.append(f"{adname(v)} -> {mk[pk[0]]['name']} (fast sæt)") + +# 2) motorkøretøjer +for v in VR.iter("vehicle"): + if adname(v) in MOTOR and park_of(v) is None: + slot = free["vest"].pop(0); renames[slot] = "P " + adname(v); set_park(v, slot) + log.append(f"{adname(v)} -> {mk[slot]['name']} => 'P {adname(v)}'") + +# 3) frakoblede vogne +req, pending = {}, [] +for v in VR.iter("vehicle"): + f = v.get("filename", "").split("/")[-1].replace(".xml", "") + if f in TRAILERS and park_of(v) is None and v.get("uniqueId") not in parent: + name, col, semi = TRAILERS[f] + slot = free[col].pop(0); pw = mk[slot]["wp"]; e = bay_axis(pw) + L, lo = size_of(v.get("filename")) + root = REAR_GAP - lo + L / 2 + d = REAR_GAP + L - (KINGPIN_BACK if semi else 0.0) + u, w, du = split_point_on_track(pw, e, d) + req.setdefault((u, w), set()).add(du) + place(v, pw, e, root); set_park(v, slot) + renames[slot] = "P " + name + pending.append((slot, name, (u, w, du), d)) + log.append(f"{name} ({f}) -> {mk[slot]['name']} => 'P {name}', Connect {d:.2f} m, rod {root:.2f} m") + +# 4) Sell Slurry: plov af, gylletank som flowets vogn +plow_park = byname["P Plov"]; tank_park = byname["P Sell Slurry vogn 2"] +plow = next(v for v in VR.iter("vehicle") if park_of(v) == plow_park) +tank = next(v for v in VR.iter("vehicle") if park_of(v) == tank_park) +tractor = veh[parent[plow.get("uniqueId")]] +aj = tractor.find("attacherJoints") +for a in list(aj): + if a.get("attachedVehicleUniqueId") == plow.get("uniqueId"): + aj.remove(a) +pw = mk[plow_park]["wp"]; e = bay_axis(pw) +cw = mk[byname["Connect P Plov"]]["wp"]; dcon, _ = along_lat(pw, e, N[cw]["x"], N[cw]["z"]) +L, lo = size_of(plow.get("filename")) +place(plow, pw, e, dcon - 0.1 - lo - L / 2) # liftmonteret: forkant + 0,1 m = Connect +log.append(f"plov koblet af '{adname(tractor)}' og stillet på P Plov (Connect {dcon:.2f} m)") +RP = SG + "ADSmartPickup_runs.xml"; runs = open(RP, encoding="utf-8-sig", newline="").read() # bevar CRLF +m = re.search(r'(]*name="Sell Slurry"[^>]*wagons=")([^"]*)(")', runs) +pairs = dict(p.split("=") for p in m.group(2).split(";") if "=" in p) +pairs[tractor.get("uniqueId")] = tank.get("uniqueId") +runs = runs[:m.start(2)] + ";".join(f"{k}={pairs[k]}" for k in sorted(pairs)) + runs[m.end(2):] +log.append(f"Sell Slurry: '{adname(tractor)}' får gylletanken på P Sell Slurry vogn 2") + +# skriv AD-config: del spor, så markørnavne + nye Connect-markører +made = apply_splits(N, req) +write(doc, SG + "AutoDrive_config.xml.new") +s = open(SG + "AutoDrive_config.xml.new", encoding="utf-8-sig").read() +bom = open(SG + "AutoDrive_config.xml", "rb").read(3) == b"\xef\xbb\xbf" +for slot, new in renames.items(): + s, n = re.subn(r"(\s*[\d.]+\s*)[^<]*(\s*)[^<]*()" % slot, + r"\g<1>%s\g<2>Parkering\g<3>" % new, s) + assert n == 1, slot +nxt = max(mk) + 1; add = "" +for slot, name, key, d in pending: + add += (" \n %d.000000\n Connect P %s\n" + " Connect\n \n") % (nxt, made[key], name, nxt) + nxt += 1 +i = s.rfind(" "); s = s[:i] + add + s[i:] +open(SG + "AutoDrive_config.xml.new", "wb").write((b"\xef\xbb\xbf" if bom else b"") + s.encode("utf-8")) +body = ET.tostring(VR, encoding="unicode") +open(SG + "vehicles.xml.new", "w", encoding="utf-8").write('\n' + body + "\n") +open(SG + "ADSmartPickup_runs.xml.new", "w", encoding="utf-8", newline="").write("\ufeff" + runs) +print("\n".join(log)); print("skrevet .new")