133 lines
5 KiB
Python
133 lines
5 KiB
Python
"""Bushveld Joy road centreline from the PI data in the road-layout DWG, and 20 m chainage bands."""
|
|
import math
|
|
import re
|
|
|
|
import numpy as np
|
|
|
|
from dxf_reader import read_sections
|
|
|
|
BAND = 20
|
|
|
|
|
|
class Alignment:
|
|
def __init__(self, at, length):
|
|
self.at = at # at(ch) -> (point[e, n], unit direction)
|
|
self.length = length
|
|
|
|
def chainage_of(self, e, n, step=1.0):
|
|
"""Chainage of the closest centreline point (sampled every `step` m, refined along the tangent)."""
|
|
best = (1e18, 0.0)
|
|
ch = 0.0
|
|
while ch <= self.length:
|
|
p, d = self.at(ch)
|
|
v = np.array([e, n]) - p
|
|
t = float(v @ d)
|
|
if -step <= t <= step:
|
|
dist = float(np.hypot(*(v - d * t)))
|
|
if dist < best[0]:
|
|
best = (dist, ch + t)
|
|
ch += step
|
|
return min(max(best[1], 0.0), self.length)
|
|
|
|
|
|
def load_alignment(dxf_path, ox, oy):
|
|
"""Bushveld Joy alignment from the PI data printed in the DWG (Start, PI1 R/T, PI2 R/T, PI3, End).
|
|
at(ch) -> (point, unit direction) in local e/n (east, north of the model origin)."""
|
|
sec = read_sections(dxf_path)
|
|
txt = []
|
|
for typ, t in sec['ENTITIES']:
|
|
if typ in ('MTEXT', 'MULTILEADER', 'TEXT'):
|
|
txt.append(re.sub(r'\\[A-Za-z][^;]*;|[{}]', ' ', ' '.join(v for c, v in t if c in (1, 3, 304))))
|
|
blob = ' '.join(t for t in txt if 'BUSHVELD' in t.upper() or re.search(r'\bY[0-4]\s*=', t))
|
|
pts = {}
|
|
for k in range(5):
|
|
m = re.search(rf'\bY{k}\s*=\s*([\d.]+)\s*\\?P?\s*X{k}\s*=\s*([\d.]+)', blob) or \
|
|
re.search(rf'Y{k}\s*=\s*([\d.]+).*?X{k}\s*=\s*([\d.]+)', blob)
|
|
if m: pts[k] = (-float(m.group(1)) - ox, -float(m.group(2)) - oy)
|
|
curves = {}
|
|
for k in (1, 2, 3):
|
|
m = re.search(rf'PI{k}\s*(?:\\P)?\s*PY{k}.*?PR\s*=\s*([\d.]+).*?PT\s*=\s*([\d.]+)', blob)
|
|
if m: curves[k] = (float(m.group(1)), float(m.group(2)))
|
|
keys = sorted(pts)
|
|
P = [np.array(pts[k]) for k in keys]
|
|
# build pieces: ('line', a, b) / ('arc', centre, r, a0, sweep)
|
|
pieces, cur = [], P[0]
|
|
for i in range(1, len(P) - 1):
|
|
k = keys[i]
|
|
u_in = (P[i] - P[i - 1]) / np.hypot(*(P[i] - P[i - 1])); u_out = (P[i + 1] - P[i]) / np.hypot(*(P[i + 1] - P[i]))
|
|
if k in curves:
|
|
R, T = curves[k]
|
|
bc, ec = P[i] - u_in * T, P[i] + u_out * T
|
|
pieces.append(('line', cur, bc))
|
|
turn = np.sign(u_in[0] * u_out[1] - u_in[1] * u_out[0])
|
|
nrm = np.array([-u_in[1], u_in[0]]) * turn
|
|
c = bc + nrm * R
|
|
a0 = math.atan2(*(bc - c)[::-1]); a1 = math.atan2(*(ec - c)[::-1])
|
|
sweep = (a1 - a0 + math.pi) % (2 * math.pi) - math.pi
|
|
pieces.append(('arc', c, R, a0, sweep))
|
|
cur = ec
|
|
else:
|
|
pieces.append(('line', cur, P[i])); cur = P[i]
|
|
pieces.append(('line', cur, P[-1]))
|
|
lens = [np.hypot(*(p[2] - p[1])) if p[0] == 'line' else abs(p[2] * p[4]) for p in pieces]
|
|
starts = np.concatenate([[0], np.cumsum(lens)])
|
|
def at(ch):
|
|
ch = min(max(ch, 0), starts[-1])
|
|
i = min(int(np.searchsorted(starts, ch, side='right') - 1), len(pieces) - 1)
|
|
s = ch - starts[i]; p = pieces[i]
|
|
if p[0] == 'line':
|
|
d = (p[2] - p[1]) / (lens[i] or 1); return p[1] + d * s, d
|
|
c, R, a0, sw = p[1:]
|
|
a = a0 + np.sign(sw) * s / R
|
|
pt = c + R * np.array([math.cos(a), math.sin(a)])
|
|
d = np.array([-math.sin(a), math.cos(a)]) * np.sign(sw)
|
|
return pt, d
|
|
return Alignment(at, float(starts[-1]))
|
|
|
|
|
|
def band_key(lo, hi):
|
|
return f"ch{int(lo)}-{int(hi)}"
|
|
|
|
|
|
def bands(ch_min, ch_max, size=BAND):
|
|
lo = int(math.floor(ch_min / size) * size)
|
|
out = []
|
|
while lo < ch_max:
|
|
out.append((lo, lo + size))
|
|
lo += size
|
|
return out
|
|
|
|
|
|
def _clip(ring, nx, ny, c):
|
|
"""Sutherland-Hodgman: keep the part of ring where nx*x + ny*y <= c."""
|
|
out = []
|
|
if not ring:
|
|
return out
|
|
prev = ring[-1]
|
|
pv = nx * prev[0] + ny * prev[1] - c
|
|
for cur in ring:
|
|
cv = nx * cur[0] + ny * cur[1] - c
|
|
if cv <= 0:
|
|
if pv > 0:
|
|
t = pv / (pv - cv)
|
|
out.append((prev[0] + t * (cur[0] - prev[0]), prev[1] + t * (cur[1] - prev[1])))
|
|
out.append(cur)
|
|
elif pv <= 0:
|
|
t = pv / (pv - cv)
|
|
out.append((prev[0] + t * (cur[0] - prev[0]), prev[1] + t * (cur[1] - prev[1])))
|
|
prev, pv = cur, cv
|
|
return out
|
|
|
|
|
|
def clip_ring_to_band(ring, at, lo, hi, first, last):
|
|
"""Part of `ring` between the planes square to the alignment at chainage lo and hi.
|
|
The first band has no lower cut and the last band no upper cut, so nothing is lost
|
|
beyond the alignment's ends."""
|
|
out = list(ring)
|
|
if not first:
|
|
p, d = at(lo)
|
|
out = _clip(out, -d[0], -d[1], -(d @ p)) # keep d.x >= d.p(lo)
|
|
if not last:
|
|
p, d = at(hi)
|
|
out = _clip(out, d[0], d[1], d @ p) # keep d.x <= d.p(hi)
|
|
return out
|