948 lines
40 KiB
Python
948 lines
40 KiB
Python
#!/usr/bin/env python3
|
|
# -*- coding: utf-8 -*-
|
|
"""
|
|
export_neighbors.py - Ermittelt benachbarte Elemente ueber Bounding-Box-
|
|
Ueberschneidung in der x/y-Ebene (Grundriss, Z/Hoehe wird ignoriert).
|
|
|
|
Die Bloecke werden dazu in drei Gruppen eingeteilt und NUR innerhalb dieser
|
|
Kombinationen gegeneinander geprueft (nicht mehr alle Elemente einer Zeichnung
|
|
gemeinsam):
|
|
|
|
1. Kreisel/Eckrad gegen Kreisel/Eckrad
|
|
2. Kreisel/Eckrad gegen Gefaellestrecke/Foerderer/Strecke-Modul - diese
|
|
drei Kategorien werden NICHT gegeneinander getestet (nur relevant, wo
|
|
sie an einen Kreisel/Eckrad anschliessen)
|
|
3. Omniflo-Elemente (Bogen/Weiche/Gerade) gegen Omniflo-Elemente
|
|
|
|
Omniflo-Anlagen koennen mehrere hundert Elemente enthalten - ein Test jedes
|
|
gegen jedes (O(n^2)) waere dort zu teuer. Die Omniflo-Gruppe wird daher in
|
|
zwei Phasen geprueft:
|
|
|
|
* Broad-Phase: Ein shapely-STRtree ueber die (tolerierten) Bounding-Boxen
|
|
liefert alle Paare, deren BBoxen sich in x/y ueberschneiden - die
|
|
"moeglichen Nachbarn". Ist shapely nicht verfuegbar, faellt die
|
|
Broad-Phase auf ein Raster (Zellgroesse siehe [Nachbarschaft] ->
|
|
omniflo_zellgroesse_mm in cfg/export.cfg) mit 3x3-Zellennachbarschaft
|
|
zurueck.
|
|
* Narrow-Phase (KOS-Verfeinerung): Fuer jedes moegliche Nachbarpaar wird
|
|
zusaetzlich geprueft, ob sich ihre Anschluss-Koordinatensysteme K1-K4
|
|
tatsaechlich beruehren - nur wenn ein K-Punkt des einen Elements naeher
|
|
als omniflo_ks_toleranz_mm (Default 10mm, siehe [Nachbarschaft]) an einem
|
|
K-Punkt des anderen liegt, gelten sie als echte Nachbarn. Das trennt
|
|
Elemente, deren BBoxen sich zwar ueberlappen, die aber nicht an einem
|
|
gemeinsamen Anschlusspunkt zusammenstossen. Elemente ohne K1-K4 (z.B.
|
|
Altbestand) fallen auf die reine BBox-Ueberschneidung zurueck. Die
|
|
K-Punkte kommen aus den csv:trans-encode-Strings der CSV-Spalten K1-K4
|
|
(siehe _omniflo_kpoints); Geraden fuehren keine echten K-Bloecke, ihre
|
|
K1/K2 werden beim Export synthetisiert (Lisp/export.lsp).
|
|
|
|
Zwei Bounding-Boxes gelten als benachbart, wenn sie sich (nach Erweiterung
|
|
um die halbe Toleranz je Seite) in x/y ueberschneiden. Toleranz aus
|
|
cfg/export.cfg ([Nachbarschaft] -> toleranz_mm), Wert in mm.
|
|
|
|
compute_neighbor_errors() prueft zusaetzlich, ob jedes Element die fuer
|
|
seine TeileArt noetige Mindestanzahl an Partnern (Nachbarn) hat (siehe
|
|
MIN_PARTNER) - Ergebnis ist die CSV-Spalte "Fehler" in export_csv.py.
|
|
|
|
Nur von export_csv.py genutzt (EXPORTCSV), nicht von export_sivas.py.
|
|
"""
|
|
|
|
import math
|
|
import os
|
|
|
|
from export_blockpatterns import cfg_path_from_env, load_export_cfg, safe_float
|
|
|
|
# shapely-STRtree fuer die Broad-Phase der Omniflo-Nachbarschaft (optional -
|
|
# fehlt shapely, wird auf das Raster _candidate_pairs_grid zurueckgefallen).
|
|
try:
|
|
from shapely import STRtree
|
|
from shapely.geometry import box as _shapely_box
|
|
_HAVE_STRTREE = True
|
|
except ImportError: # pragma: no cover - shapely ist optional
|
|
STRtree = None
|
|
_shapely_box = None
|
|
_HAVE_STRTREE = False
|
|
|
|
# Base64-Alphabet + Fixed-Point-Faktor von csv:trans-encode (Lisp/export.lsp) -
|
|
# zum Zurueckrechnen der K1-K4-Positionsstrings in x/y-Weltkoordinaten (mm).
|
|
_B64_CHARS = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/"
|
|
_B64_INDEX = {c: i for i, c in enumerate(_B64_CHARS)}
|
|
_TRANS_FAKTOR = 10.0
|
|
|
|
KREISEL_TEILEARTEN = {"ILS 2.0 Kreisel", "ILS 2.0 Eckrad"}
|
|
STRECKEN_TEILEARTEN = {"ILS 2.0 Gefaellestrecke", "ILS 2.0 Strecke", "ILS 2.0 Strecke - Modul"}
|
|
OMNIFLO_TEILEARTEN = {"Omniflo Kurve", "Omniflo Weiche", "Omniflo Gerade"}
|
|
SENSOR_TEILEARTEN = {"ILS 2.0 Separator", "ILS 2.0 Scanner"}
|
|
|
|
# Nur "ILS 2.0 Kreisel" (echte Kreisel-Bloecke mit AN8/Antrieb + SP8/Spann-
|
|
# station + ABSTAND-Attribut) werden links/rechts gesplittet - "ILS 2.0
|
|
# Eckrad" hat keine dieser zwei Stationen und bleibt eine einzelne BBox.
|
|
KREISEL_SPLIT_TEILEART = "ILS 2.0 Kreisel"
|
|
|
|
# TeileArt der automatisch am Kreisel-Beruehrpunkt erzeugten Weiche
|
|
# (compute_kreisel_touch_switches / export_csv.py).
|
|
WEICHE_TEILEART = "ILS Weiche"
|
|
|
|
# Statische Default-Boundingbox einer solchen ILS-Weiche (Laenge x Breite x
|
|
# Hoehe in mm). Die Laengsseite (WEICHE_BOX_LAENGE_MM) wird senkrecht zur
|
|
# Achse desjenigen Kreisels ausgerichtet, dessen Endkreis den Beruehrpunkt
|
|
# bildet; ausgegeben wird die achsparallele Welt-Bounding-Box dieser
|
|
# gedrehten Box (siehe _oriented_box_aabb).
|
|
WEICHE_BOX_LAENGE_MM = 250.0
|
|
WEICHE_BOX_BREITE_MM = 100.0
|
|
WEICHE_BOX_HOEHE_MM = 30.0
|
|
|
|
# Mindestanzahl Partner (Nachbarn) je TeileArt fuer die Fehlerspalte "Fehler"
|
|
# (siehe compute_neighbor_errors): Kreisel/Eckrad und Omniflo-Elemente
|
|
# brauchen mindestens einen Partner, Gefaellestrecke/Foerderer/Strecke-Modul
|
|
# muessen an zwei Seiten anschliessen (mindestens zwei Partner). TeileArten,
|
|
# die hier nicht auftauchen (z.B. die synthetische "Omniflo Sum"-Zeile),
|
|
# werden nicht geprueft.
|
|
MIN_PARTNER = {}
|
|
MIN_PARTNER.update({t: 1 for t in KREISEL_TEILEARTEN})
|
|
MIN_PARTNER.update({t: 1 for t in OMNIFLO_TEILEARTEN})
|
|
MIN_PARTNER.update({t: 2 for t in STRECKEN_TEILEARTEN})
|
|
|
|
|
|
def load_neighbor_tolerance_mm(cfg_path=None):
|
|
"""Liest die Nachbarschafts-Toleranz (mm) aus export.cfg. Default: 0."""
|
|
if cfg_path is None:
|
|
cfg_path = cfg_path_from_env()
|
|
parser = load_export_cfg(cfg_path)
|
|
return parser.getfloat("Nachbarschaft", "toleranz_mm", fallback=0.0)
|
|
|
|
|
|
def load_omniflo_cell_size_mm(cfg_path=None):
|
|
"""Liest die Rastergroesse (mm) fuer die Omniflo-Nachbarschaftspruefung.
|
|
|
|
Default 3000mm: groesser als die ueblichen Omniflo-Bauteile (AP110-
|
|
Geraden ca. 2000mm), damit ein Element nicht durch ein zu kleines
|
|
Raster in mehr als eine Zelle ueberlappt.
|
|
"""
|
|
if cfg_path is None:
|
|
cfg_path = cfg_path_from_env()
|
|
parser = load_export_cfg(cfg_path)
|
|
return parser.getfloat("Nachbarschaft", "omniflo_zellgroesse_mm", fallback=3000.0)
|
|
|
|
|
|
def load_omniflo_ks_tolerance_mm(cfg_path=None):
|
|
"""Liest die Toleranz (mm) fuer die KOS-basierte Verfeinerung der Omniflo-
|
|
Nachbarschaft (K1-K4-Anschlusspunkte) aus export.cfg. Default 10mm.
|
|
|
|
Nachdem der STRtree zwei Omniflo-Elemente anhand ihrer Bounding-Boxen als
|
|
moegliche Nachbarn erkannt hat, gelten sie nur dann als echte Nachbarn,
|
|
wenn ein K-Punkt des einen naeher als dieser Wert an einem K-Punkt des
|
|
anderen liegt. 0 = Verfeinerung aus (nur BBox). Siehe [Nachbarschaft] ->
|
|
omniflo_ks_toleranz_mm.
|
|
"""
|
|
if cfg_path is None:
|
|
cfg_path = cfg_path_from_env()
|
|
parser = load_export_cfg(cfg_path)
|
|
return parser.getfloat("Nachbarschaft", "omniflo_ks_toleranz_mm", fallback=10.0)
|
|
|
|
|
|
def load_kreisel_durchmesser_mm(cfg_path=None):
|
|
"""Liest den Kreisel-Durchmesser (mm) aus export.cfg. Default: 800.0
|
|
(siehe [Kreisel] durchmesser_mm - gespiegelter Wert von
|
|
data/json/component_defaults.json "kreisel"."durchmesser")."""
|
|
if cfg_path is None:
|
|
cfg_path = cfg_path_from_env()
|
|
parser = load_export_cfg(cfg_path)
|
|
return parser.getfloat("Kreisel", "durchmesser_mm", fallback=800.0)
|
|
|
|
|
|
def load_collision_debug_target(cfg_path=None):
|
|
"""Zielpfad fuer das Kollisions-/Nachbarschafts-Debuglog aus export.cfg.
|
|
|
|
[Nachbarschaft] -> debug_log: leer/0/false/no/off = aus (Rueckgabe None).
|
|
1/true/yes/on = DXFM_LOG/export_collision.dbg (bzw. <repo>/logs/, falls
|
|
DXFM_LOG nicht gesetzt ist). Jeder andere Wert wird als expliziter
|
|
Dateipfad interpretiert.
|
|
"""
|
|
if cfg_path is None:
|
|
cfg_path = cfg_path_from_env()
|
|
parser = load_export_cfg(cfg_path)
|
|
raw = parser.get("Nachbarschaft", "debug_log", fallback="").strip()
|
|
if not raw or raw.lower() in ("0", "false", "no", "off", "nein", "aus"):
|
|
return None
|
|
if raw.lower() in ("1", "true", "yes", "on", "ja", "ein"):
|
|
log_dir = os.environ.get("DXFM_LOG")
|
|
if not log_dir:
|
|
# cfg_path zeigt auf <repo>/cfg/export.cfg -> <repo>/logs
|
|
log_dir = os.path.join(
|
|
os.path.dirname(os.path.dirname(os.path.abspath(cfg_path))), "logs")
|
|
return os.path.join(log_dir, "export_collision.dbg")
|
|
return raw # expliziter Pfad
|
|
|
|
|
|
def _bounds(bbox, half_tol):
|
|
"""(minx, maxx, miny, maxy) einer Bounding-Box, je Seite um half_tol erweitert."""
|
|
return (
|
|
bbox["cx"] - bbox["dx"] / 2.0 - half_tol,
|
|
bbox["cx"] + bbox["dx"] / 2.0 + half_tol,
|
|
bbox["cy"] - bbox["dy"] / 2.0 - half_tol,
|
|
bbox["cy"] + bbox["dy"] / 2.0 + half_tol,
|
|
)
|
|
|
|
|
|
def _overlaps(bounds_a, bounds_b):
|
|
"""True, wenn sich zwei (minx, maxx, miny, maxy)-Rechtecke in x/y ueberschneiden."""
|
|
a_minx, a_maxx, a_miny, a_maxy = bounds_a
|
|
b_minx, b_maxx, b_miny, b_maxy = bounds_b
|
|
return a_minx <= b_maxx and b_minx <= a_maxx and a_miny <= b_maxy and b_miny <= a_maxy
|
|
|
|
|
|
def _add_neighbor(result, idx_a, idx_b, id_a, id_b):
|
|
result[idx_a].append(id_b)
|
|
result[idx_b].append(id_a)
|
|
|
|
|
|
def kreisel_half_bboxes(block, durchmesser_mm):
|
|
"""Teilt die Welt-Bounding-Box eines "ILS 2.0 Kreisel"-Blocks entlang der
|
|
Kreiselachse (Linie durch die Mitten von Antriebs- (AN8) und Spann-
|
|
station (SP8)) in eine linke und eine rechte Haelfte.
|
|
|
|
Geometrie (siehe KreiselInsert.lsp::draw-module): AN8 liegt in lokalen
|
|
(unrotierten) Blockkoordinaten bei (radius, 0), SP8 bei
|
|
(radius+abstand, 0) - radius = durchmesser_mm/2 (Default 400mm, siehe
|
|
[Kreisel] in cfg/export.cfg). Beide Stationen liegen exakt auf der
|
|
lokalen X-Achse (Y=0); die gesamte Geometrie (zwei Kreise + zwei
|
|
Tangenten bei Y=+-radius) ist symmetrisch dazu. Die Trennlinie durch die
|
|
Stationsmitten IST also die lokale X-Achse selbst, und teilt die
|
|
Bounding-Box in zwei GLEICH GROSSE Haelften mit je "radius" (Default
|
|
400mm) Breite - "links" = lokal Y in [0, +radius], "rechts" = lokal Y in
|
|
[-radius, 0] (Blickrichtung von Antriebs- zu Spannstation, +X; mathe-
|
|
matische Standardkonvention: mathematisch positive/CCW-Drehung von +X
|
|
fuehrt nach +Y, also links).
|
|
|
|
Block-Insertpunkt (world x,y) und -rotation (world "rotation", Grad,
|
|
CCW positiv) kommen direkt aus block (siehe csv:block-to-json). Die
|
|
ABSTAND-Attribut wird fuer die lokale Laenge (0..abstand+durchmesser)
|
|
benoetigt.
|
|
|
|
Rueckgabe: (bbox_links, bbox_rechts), je ein dict {"cx","cy","dx","dy"} -
|
|
die Welt-AABB der jeweils rotierten Haelfte (bei Rotationen, die kein
|
|
Vielfaches von 90 Grad sind, umschliesst die AABB die tatsaechliche
|
|
Haelfte etwas grosszuegiger, wie jede achsparallele Bounding-Box einer
|
|
gedrehten Flaeche - konsistent mit jeder anderen Bounding-Box in diesem
|
|
Modul).
|
|
"""
|
|
attribs = block.get("attribs", {})
|
|
abstand_mm = safe_float(attribs.get("ABSTAND", "0"), 0.0)
|
|
radius_mm = durchmesser_mm / 2.0
|
|
x0 = block.get("x", 0.0) or 0.0
|
|
y0 = block.get("y", 0.0) or 0.0
|
|
rot_rad = math.radians(block.get("rotation", 0.0) or 0.0)
|
|
cos_r, sin_r = math.cos(rot_rad), math.sin(rot_rad)
|
|
|
|
def world(local_x, local_y):
|
|
return (
|
|
x0 + local_x * cos_r - local_y * sin_r,
|
|
y0 + local_x * sin_r + local_y * cos_r,
|
|
)
|
|
|
|
# AN8-Kreis reicht lokal bis X=0 (Mitte radius, Radius radius); SP8-Kreis
|
|
# bis X=abstand+durchmesser (Mitte radius+abstand, Radius radius).
|
|
local_x_max = abstand_mm + durchmesser_mm
|
|
|
|
def half_bbox(y_lo, y_hi):
|
|
corners = [
|
|
world(0.0, y_lo), world(local_x_max, y_lo),
|
|
world(local_x_max, y_hi), world(0.0, y_hi),
|
|
]
|
|
xs = [c[0] for c in corners]
|
|
ys = [c[1] for c in corners]
|
|
minx, maxx = min(xs), max(xs)
|
|
miny, maxy = min(ys), max(ys)
|
|
return {
|
|
"cx": (minx + maxx) / 2.0, "cy": (miny + maxy) / 2.0,
|
|
"dx": maxx - minx, "dy": maxy - miny,
|
|
}
|
|
|
|
bbox_links = half_bbox(0.0, radius_mm)
|
|
bbox_rechts = half_bbox(-radius_mm, 0.0)
|
|
return bbox_links, bbox_rechts
|
|
|
|
|
|
def _fmt_bounds(b):
|
|
"""(minx, maxx, miny, maxy) kompakt fuer das Debuglog."""
|
|
return f"[x {b[0]:.0f}..{b[1]:.0f} | y {b[2]:.0f}..{b[3]:.0f}]"
|
|
|
|
|
|
def _dbg_pair(dbg, id_a, id_b, overlap, detail=""):
|
|
if dbg:
|
|
dbg(f" {id_a} <-> {id_b}: "
|
|
f"{'NACHBARN' if overlap else 'kein Kontakt'}{detail}")
|
|
|
|
|
|
def _decode_trans_xy(text):
|
|
"""csv:trans-encode-String (K1-K4, 12 Zeichen, 3 Werte) -> (x, y) in mm
|
|
(Grundriss, Z wird nicht gebraucht), oder None bei leerem/ungueltigem
|
|
String.
|
|
|
|
Kodierung siehe Lisp/export.lsp (csv:trans-encode/csv:b64-encode-ints):
|
|
vorzeichenbehaftete 24-Bit-Fixed-Point-Werte (Faktor 10), je 4 Base64-
|
|
Zeichen, hoechstwertige 6 Bit zuerst. Es genuegen die ersten beiden Werte
|
|
(x, y) = die ersten 8 Zeichen.
|
|
"""
|
|
text = (text or "").strip()
|
|
if len(text) < 8:
|
|
return None
|
|
werte = []
|
|
for start in (0, 4):
|
|
wert = 0
|
|
for zeichen in text[start:start + 4]:
|
|
idx = _B64_INDEX.get(zeichen)
|
|
if idx is None:
|
|
return None
|
|
wert = (wert << 6) | idx
|
|
if wert >= 8388608: # 2^23 -> negativer 24-Bit-Wert (Zweierkompl.)
|
|
wert -= 16777216
|
|
werte.append(wert / _TRANS_FAKTOR)
|
|
return (werte[0], werte[1])
|
|
|
|
|
|
def _omniflo_kpoints(item):
|
|
"""Welt-(x,y)-Punkte der vorhandenen K1-K4-Koordinatensysteme eines
|
|
Omniflo-Elements, dekodiert aus den csv:trans-encode-Strings item["k1"]..
|
|
["k4"] (siehe export_csv.bbox_columns). Leere/fehlende Strings werden
|
|
uebersprungen - die Liste kann also 0 bis 4 Punkte enthalten. Fuer Geraden
|
|
stehen darin die beim Export synthetisierten Anfang/Ende-Punkte (K1/K2)."""
|
|
pts = []
|
|
for key in ("k1", "k2", "k3", "k4"):
|
|
p = _decode_trans_xy(item.get(key, ""))
|
|
if p is not None:
|
|
pts.append(p)
|
|
return pts
|
|
|
|
|
|
def _ks_within(kpts_a, kpts_b, tol_mm):
|
|
"""True, wenn irgendein K-Punkt aus a einem K-Punkt aus b naeher als tol_mm
|
|
kommt (euklidischer Abstand in x/y). Beide Listen muessen nicht leer sein -
|
|
das prueft der Aufrufer."""
|
|
for ax, ay in kpts_a:
|
|
for bx, by in kpts_b:
|
|
if math.hypot(ax - bx, ay - by) <= tol_mm:
|
|
return True
|
|
return False
|
|
|
|
|
|
def _test_group_pairs(group, result, dbg=None):
|
|
"""Alle Paare EINER Gruppe gegeneinander testen (O(n^2) - fuer Kreisel/
|
|
Eckrad-Stueckzahlen pro Zeichnung unkritisch).
|
|
|
|
idx_a == idx_b wird uebersprungen: seit dem Kreisel-Links/Rechts-Split
|
|
(siehe kreisel_half_bboxes) kann dieselbe Original-idx zweimal in der
|
|
Gruppe auftauchen (die linke und die rechte Haelfte DESSELBEN Kreisels) -
|
|
die beiden beruehren sich zwangslaeufig an der Trennlinie und waeren
|
|
sonst faelschlich gegenseitige "Nachbarn".
|
|
"""
|
|
n = len(group)
|
|
for a in range(n):
|
|
idx_a, bounds_a, id_a = group[a]
|
|
for b in range(a + 1, n):
|
|
idx_b, bounds_b, id_b = group[b]
|
|
if idx_a == idx_b:
|
|
continue
|
|
overlap = _overlaps(bounds_a, bounds_b)
|
|
_dbg_pair(dbg, id_a, id_b, overlap)
|
|
if overlap:
|
|
_add_neighbor(result, idx_a, idx_b, id_a, id_b)
|
|
|
|
|
|
def _test_group_against_group(group_a, group_b, result, dbg=None):
|
|
"""Jedes Element aus group_a gegen jedes Element aus group_b testen.
|
|
group_b wird NICHT gegen sich selbst getestet (Aufgabe des Aufrufers)."""
|
|
for idx_a, bounds_a, id_a in group_a:
|
|
for idx_b, bounds_b, id_b in group_b:
|
|
overlap = _overlaps(bounds_a, bounds_b)
|
|
_dbg_pair(dbg, id_a, id_b, overlap)
|
|
if overlap:
|
|
_add_neighbor(result, idx_a, idx_b, id_a, id_b)
|
|
|
|
|
|
def _grid_cell(bbox, cell_size_mm):
|
|
"""Rasterzelle (Quadrant) einer Bounding-Box nach ihrem Mittelpunkt."""
|
|
return (int(bbox["cx"] // cell_size_mm), int(bbox["cy"] // cell_size_mm))
|
|
|
|
|
|
def _candidate_pairs_strtree(group):
|
|
"""Broad-Phase per shapely-STRtree: liefert alle Gruppen-Indexpaare (a<b),
|
|
deren (bereits um die halbe Toleranz erweiterte) Bounding-Boxen sich in
|
|
x/y ueberschneiden. Gruppen-Eintrag = (idx, bounds, teileid, bbox, kpts)
|
|
mit bounds = (minx, maxx, miny, maxy)."""
|
|
boxes = [_shapely_box(bounds[0], bounds[2], bounds[1], bounds[3])
|
|
for (_idx, bounds, _tid, _bbox, _kpts) in group]
|
|
tree = STRtree(boxes)
|
|
pairs = set()
|
|
for a, geom in enumerate(boxes):
|
|
for b in tree.query(geom): # Envelope-Ueberschneidung (shapely 2.x: Indizes)
|
|
b = int(b)
|
|
if a < b:
|
|
pairs.add((a, b))
|
|
elif b < a:
|
|
pairs.add((b, a))
|
|
return pairs
|
|
|
|
|
|
def _candidate_pairs_grid(group, cell_size_mm, dbg=None):
|
|
"""Fallback-Broad-Phase ohne shapely: Omniflo-Elemente nach x/y-Koordinate
|
|
in Rasterzellen einteilen und nur Paare aus derselben oder einer der 8
|
|
angrenzenden Zellen (3x3-Nachbarschaft) als Kandidaten liefern - ohne
|
|
Ueberschneidungen an Zellgrenzen zu verpassen. Rueckgabe: Menge von
|
|
Gruppen-Indexpaaren (a<b)."""
|
|
buckets = {}
|
|
for gi, entry in enumerate(group):
|
|
bbox = entry[3]
|
|
buckets.setdefault(_grid_cell(bbox, cell_size_mm), []).append(gi)
|
|
|
|
if dbg:
|
|
dbg(f" {len(buckets)} belegte Rasterzelle(n): "
|
|
+ ", ".join(f"{cell}={len(v)}" for cell, v in sorted(buckets.items())))
|
|
|
|
pairs = set()
|
|
for (cx, cy), cell_items in buckets.items():
|
|
candidates = []
|
|
for dx in (-1, 0, 1):
|
|
for dy in (-1, 0, 1):
|
|
candidates.extend(buckets.get((cx + dx, cy + dy), []))
|
|
for a in cell_items:
|
|
for b in candidates:
|
|
if a < b:
|
|
pairs.add((a, b))
|
|
elif b < a:
|
|
pairs.add((b, a))
|
|
return pairs
|
|
|
|
|
|
def _test_omniflo(group, cell_size_mm, ks_tol_mm, result, dbg=None):
|
|
"""Omniflo-Nachbarschaft in zwei Phasen (siehe Modul-Docstring):
|
|
|
|
1. Broad-Phase (STRtree bzw. Raster-Fallback): alle Paare mit sich
|
|
ueberschneidenden Bounding-Boxen ("moegliche Nachbarn").
|
|
2. Narrow-Phase (KOS-Verfeinerung): ein Paar gilt nur dann als benachbart,
|
|
wenn beide Elemente K-Punkte fuehren UND ein K-Punkt des einen naeher
|
|
als ks_tol_mm an einem K-Punkt des anderen liegt. Hat mindestens eines
|
|
der beiden keine K-Punkte (Altbestand) oder ist ks_tol_mm <= 0, bleibt
|
|
es bei der reinen BBox-Ueberschneidung.
|
|
"""
|
|
if not group:
|
|
return
|
|
|
|
if _HAVE_STRTREE:
|
|
pairs = _candidate_pairs_strtree(group)
|
|
broad = "shapely-STRtree"
|
|
else:
|
|
pairs = _candidate_pairs_grid(group, cell_size_mm, dbg)
|
|
broad = "Raster-Fallback (shapely nicht verfuegbar)"
|
|
|
|
if dbg:
|
|
dbg(f" Broad-Phase: {broad}, {len(group)} Omniflo-Element(e), "
|
|
f"{len(pairs)} BBox-Kandidatenpaar(e); "
|
|
f"KOS-Verfeinerung Toleranz={ks_tol_mm}mm")
|
|
|
|
for a, b in sorted(pairs):
|
|
idx_a, bounds_a, id_a, _bbox_a, kpts_a = group[a]
|
|
idx_b, bounds_b, id_b, _bbox_b, kpts_b = group[b]
|
|
overlap = _overlaps(bounds_a, bounds_b)
|
|
detail = ""
|
|
if overlap and ks_tol_mm > 0 and kpts_a and kpts_b:
|
|
if _ks_within(kpts_a, kpts_b, ks_tol_mm):
|
|
detail = f" (KOS-Kontakt <= {ks_tol_mm:.0f}mm)"
|
|
else:
|
|
overlap = False
|
|
detail = f" (BBox ueberschneidet, aber kein KOS-Paar <= {ks_tol_mm:.0f}mm)"
|
|
_dbg_pair(dbg, id_a, id_b, overlap, detail)
|
|
if overlap:
|
|
_add_neighbor(result, idx_a, idx_b, id_a, id_b)
|
|
|
|
|
|
def compute_neighbor_ids(items, tolerance_mm, omniflo_cell_size_mm=3000.0,
|
|
kreisel_durchmesser_mm=800.0,
|
|
omniflo_ks_tolerance_mm=10.0, dbg=None):
|
|
"""Ermittelt je Item die IDs (item["teileid"]) benachbarter Elemente.
|
|
|
|
items = Liste von dict mit "teileart", "teileid" und optional "_bbox"
|
|
({"cx","cy","dx","dy",...}, siehe export_csv.py bbox_columns). Items
|
|
ohne _bbox (z.B. die synthetische Omniflo-Sum-Zeile) bleiben ohne
|
|
Nachbarn. Omniflo-Items koennen zusaetzlich "k1".."k4" (csv:trans-encode-
|
|
Strings) tragen - daraus wird die KOS-Verfeinerung gespeist (siehe
|
|
_test_omniflo / omniflo_ks_tolerance_mm).
|
|
|
|
omniflo_ks_tolerance_mm = Toleranz (mm) fuer die KOS-Verfeinerung der
|
|
Omniflo-Nachbarschaft (K1-K4). 0 = aus (nur BBox-Ueberschneidung).
|
|
|
|
"ILS 2.0 Kreisel"-Items werden dabei NICHT als eine BBox, sondern als
|
|
ZWEI gegeneinander getestet: eine linke und eine rechte Haelfte (siehe
|
|
kreisel_half_bboxes) - die Kreisel-Gruppe fuer die Kollisionspruefung
|
|
besteht also aus <teileid>-L/<teileid>-R statt <teileid> je Kreisel
|
|
("ILS 2.0 Eckrad" bleibt unveraendert eine einzelne BBox). Ein
|
|
angrenzendes Element (z.B. eine Gefaellestrecke) bekommt dadurch in
|
|
seiner eigenen Nachbarliste "<teileid>-L" oder "<teileid>-R" statt nur
|
|
"<teileid>" - je nachdem, welche Haelfte tatsaechlich ueberschneidet.
|
|
|
|
dbg = optionale Callable(str) fuer ein Debugprotokoll (Klassifikation,
|
|
Gruppen, jeder Ueberschneidungstest, Ergebnis). None = kein Protokoll.
|
|
|
|
Rueckgabe: Liste von kommaseparierten Nachbar-ID-Strings, positionsgleich
|
|
zu items. Innerhalb eines Items dedupliziert (identische IDs entfernt -
|
|
das passiert z.B., wenn ein Kreisel dieselbe Strecke ueber BEIDE
|
|
Haelften beruehrt), sonst nicht global ueber alle Items hinweg (TeileId
|
|
ist nicht zwingend eindeutig).
|
|
"""
|
|
half_tol = tolerance_mm / 2.0
|
|
result = [[] for _ in items]
|
|
|
|
kreisel_group = []
|
|
strecken_group = []
|
|
omniflo_group = []
|
|
skipped = 0
|
|
|
|
if dbg:
|
|
dbg("=== Nachbarschafts-/Kollisionserkennung ===")
|
|
dbg(f"Toleranz={tolerance_mm}mm (Bounding-Box je Seite +{half_tol}mm erweitert), "
|
|
f"Omniflo-Zellgroesse={omniflo_cell_size_mm}mm (nur Raster-Fallback), "
|
|
f"Omniflo-KOS-Toleranz={omniflo_ks_tolerance_mm}mm, "
|
|
f"Kreisel-Durchmesser={kreisel_durchmesser_mm}mm (Radius je Haelfte)")
|
|
dbg(f"Elemente gesamt: {len(items)}")
|
|
dbg("")
|
|
dbg("Klassifikation je Element:")
|
|
|
|
for idx, item in enumerate(items):
|
|
bbox = item.get("_bbox")
|
|
if not bbox:
|
|
skipped += 1
|
|
if dbg:
|
|
dbg(f" idx={idx} teileid={item.get('teileid', '')!r} "
|
|
f"teileart={item.get('teileart', '')!r} -> UEBERSPRUNGEN "
|
|
f"(keine Bounding-Box)")
|
|
continue
|
|
teileart = item.get("teileart", "")
|
|
teileid = item.get("teileid", "")
|
|
if teileart == KREISEL_SPLIT_TEILEART:
|
|
gruppe = "Kreisel (links/rechts gesplittet)"
|
|
bbox_links, bbox_rechts = kreisel_half_bboxes(item, kreisel_durchmesser_mm)
|
|
bounds_links = _bounds(bbox_links, half_tol)
|
|
bounds_rechts = _bounds(bbox_rechts, half_tol)
|
|
kreisel_group.append((idx, bounds_links, f"{teileid}-L"))
|
|
kreisel_group.append((idx, bounds_rechts, f"{teileid}-R"))
|
|
if dbg:
|
|
dbg(f" idx={idx} teileid={teileid!r} teileart={teileart!r} "
|
|
f"-> {gruppe}; links bbox(cx={bbox_links['cx']:.0f},cy={bbox_links['cy']:.0f},"
|
|
f"dx={bbox_links['dx']:.0f},dy={bbox_links['dy']:.0f}) bounds={_fmt_bounds(bounds_links)}; "
|
|
f"rechts bbox(cx={bbox_rechts['cx']:.0f},cy={bbox_rechts['cy']:.0f},"
|
|
f"dx={bbox_rechts['dx']:.0f},dy={bbox_rechts['dy']:.0f}) bounds={_fmt_bounds(bounds_rechts)}")
|
|
continue
|
|
bounds = _bounds(bbox, half_tol)
|
|
if teileart in KREISEL_TEILEARTEN:
|
|
gruppe = "Kreisel/Eckrad"
|
|
kreisel_group.append((idx, bounds, teileid))
|
|
elif teileart in STRECKEN_TEILEARTEN:
|
|
gruppe = "Strecke/Foerderer"
|
|
strecken_group.append((idx, bounds, teileid))
|
|
elif teileart in OMNIFLO_TEILEARTEN:
|
|
gruppe = "Omniflo"
|
|
omniflo_group.append((idx, bounds, teileid, bbox, _omniflo_kpoints(item)))
|
|
else:
|
|
gruppe = "(keine Gruppe - wird nicht geprueft)"
|
|
if dbg:
|
|
dbg(f" idx={idx} teileid={teileid!r} teileart={teileart!r} "
|
|
f"-> {gruppe}; bbox(cx={bbox.get('cx', 0):.0f},cy={bbox.get('cy', 0):.0f},"
|
|
f"dx={bbox.get('dx', 0):.0f},dy={bbox.get('dy', 0):.0f}) "
|
|
f"bounds={_fmt_bounds(bounds)}")
|
|
|
|
if dbg:
|
|
dbg("")
|
|
dbg(f"Gruppengroessen: Kreisel/Eckrad={len(kreisel_group)}, "
|
|
f"Strecke/Foerderer={len(strecken_group)}, Omniflo={len(omniflo_group)}, "
|
|
f"ohne Bounding-Box={skipped}")
|
|
dbg("")
|
|
dbg("[1] Kreisel/Eckrad gegen Kreisel/Eckrad:")
|
|
# 1. Kreisel/Eckrad gegen Kreisel/Eckrad
|
|
_test_group_pairs(kreisel_group, result, dbg)
|
|
|
|
if dbg:
|
|
dbg("[2] Kreisel/Eckrad gegen Strecke/Foerderer/Gefaellestrecke:")
|
|
# 2. Kreisel/Eckrad gegen Gefaellestrecke/Foerderer/Strecke-Modul -
|
|
# diese drei Kategorien nicht gegeneinander (siehe Modul-Docstring)
|
|
_test_group_against_group(kreisel_group, strecken_group, result, dbg)
|
|
|
|
if dbg:
|
|
dbg("[3] Omniflo gegen Omniflo (STRtree-Broad-Phase + KOS-Verfeinerung):")
|
|
# 3. Omniflo gegen Omniflo: BBox-Broad-Phase (STRtree bzw. Raster-Fallback),
|
|
# dann KOS-Verfeinerung ueber K1-K4 (siehe _test_omniflo)
|
|
_test_omniflo(omniflo_group, omniflo_cell_size_mm,
|
|
omniflo_ks_tolerance_mm, result, dbg)
|
|
|
|
# dict.fromkeys() dedupliziert unter Erhalt der Reihenfolge - relevant
|
|
# seit dem Kreisel-Split: beruehrt eine Strecke BEIDE Haelften desselben
|
|
# Kreisels nicht (normalerweise ausgeschlossen), waere sie sonst nicht
|
|
# betroffen; umgekehrt kann derselbe Strecken-Nachbar im Kreisel-Eintrag
|
|
# theoretisch doppelt auftauchen, wenn (Rand-/Toleranzfall) beide
|
|
# Haelften ihn beruehren.
|
|
neighbor_id_lists = [", ".join(dict.fromkeys(neighbor_ids)) for neighbor_ids in result]
|
|
|
|
if dbg:
|
|
dbg("")
|
|
dbg("Ergebnis je Element:")
|
|
for idx, item in enumerate(items):
|
|
dbg(f" idx={idx} teileid={item.get('teileid', '')!r}: "
|
|
f"{len(result[idx])} Nachbar(n) [{neighbor_id_lists[idx]}]")
|
|
dbg("")
|
|
dbg(f"Summe gerichteter Nachbarschaftsbeziehungen: "
|
|
f"{sum(len(r) for r in result)}")
|
|
|
|
return neighbor_id_lists
|
|
|
|
|
|
def compute_neighbor_errors(items, neighbor_ids):
|
|
"""Prueft je Item die Mindestanzahl an Partnern (siehe MIN_PARTNER).
|
|
|
|
items = dieselbe Liste wie bei compute_neighbor_ids (braucht "teileart").
|
|
neighbor_ids = Rueckgabe von compute_neighbor_ids (positionsgleich zu items).
|
|
|
|
Rueckgabe: Liste von Fehlertexten, positionsgleich zu items:
|
|
"unverbunden" - keine Partner, obwohl mindestens einer noetig waere
|
|
"nur ein Partner" - genau ein Partner, obwohl mindestens zwei noetig waeren
|
|
"" - Mindestanzahl erreicht, oder TeileArt wird nicht geprueft
|
|
"""
|
|
errors = []
|
|
for item, nachbarn in zip(items, neighbor_ids):
|
|
min_partner = MIN_PARTNER.get(item.get("teileart", ""))
|
|
if min_partner is None:
|
|
errors.append("")
|
|
continue
|
|
anzahl = len([p for p in nachbarn.split(",") if p.strip()])
|
|
if anzahl == 0:
|
|
errors.append("unverbunden")
|
|
elif anzahl == 1 and min_partner >= 2:
|
|
errors.append("nur ein Partner")
|
|
else:
|
|
errors.append("")
|
|
return errors
|
|
|
|
|
|
def compute_sensor_zuordnung(items, tolerance_mm, kreisel_durchmesser_mm=800.0, dbg=None):
|
|
"""Ordnet jeden Separator/Scanner (SENSOR_TEILEARTEN) per Bounding-Box-
|
|
Ueberschneidung seiner Gefaellestrecke, seinem Foerderer oder einer
|
|
Kreiselhaelfte (links/rechts, siehe kreisel_half_bboxes) zu.
|
|
|
|
Eigener, unabhaengiger Durchlauf NACH compute_neighbor_ids (nicht in
|
|
dessen Gruppenaufbau integriert, da Separatoren/Scanner dort bewusst
|
|
keine eigene Gruppe sind - Nachbarschaft und Zuordnung sind zwei
|
|
unterschiedliche Fragen: Nachbarschaft = "beruehrt sich", Zuordnung =
|
|
"gehoert zu genau einem Trägerelement"). Prioritaet bei mehreren
|
|
Treffern: Gefaellestrecke vor Foerderer vor Kreiselhaelfte (ein
|
|
Separator/Scanner haengt fachlich an genau einem dieser drei).
|
|
|
|
items = dieselbe Liste wie bei compute_neighbor_ids (braucht "teileart",
|
|
"teileid" und "_bbox").
|
|
|
|
Rueckgabe: Liste von Zuordnungs-Strings, positionsgleich zu items -
|
|
"" fuer Items, die keine SENSOR_TEILEART sind oder keine BBox haben.
|
|
Bei Treffer: die TeileId der Gefaellestrecke/des Foerderers, oder
|
|
"<KreiselTeileId>-L"/"-R" bei einem Kreisel-Treffer. Kein Treffer bei
|
|
vorhandener BBox: "nicht zugeordnet".
|
|
"""
|
|
half_tol = tolerance_mm / 2.0
|
|
|
|
gf_entries = []
|
|
vf_entries = []
|
|
kreisel_entries = []
|
|
|
|
for item in items:
|
|
bbox = item.get("_bbox")
|
|
if not bbox:
|
|
continue
|
|
teileart = item.get("teileart", "")
|
|
teileid = item.get("teileid", "")
|
|
if teileart == "ILS 2.0 Gefaellestrecke":
|
|
gf_entries.append((teileid, _bounds(bbox, half_tol)))
|
|
elif teileart in ("ILS 2.0 Strecke", "ILS 2.0 Strecke - Modul"):
|
|
vf_entries.append((teileid, _bounds(bbox, half_tol)))
|
|
elif teileart == KREISEL_SPLIT_TEILEART:
|
|
bbox_links, bbox_rechts = kreisel_half_bboxes(item, kreisel_durchmesser_mm)
|
|
kreisel_entries.append((f"{teileid}-L", _bounds(bbox_links, half_tol)))
|
|
kreisel_entries.append((f"{teileid}-R", _bounds(bbox_rechts, half_tol)))
|
|
|
|
if dbg:
|
|
dbg("")
|
|
dbg("=== Separator/Scanner-Zuordnung (GF > Foerderer > Kreiselhaelfte) ===")
|
|
dbg(f"Kandidaten: {len(gf_entries)} Gefaellestrecke(n), "
|
|
f"{len(vf_entries)} Foerderer/Streckenmodul(e), "
|
|
f"{len(kreisel_entries)} Kreiselhaelfte(n)")
|
|
|
|
def _find(bounds, candidates):
|
|
for teileid, other_bounds in candidates:
|
|
if _overlaps(bounds, other_bounds):
|
|
return teileid
|
|
return None
|
|
|
|
result = []
|
|
for item in items:
|
|
teileart = item.get("teileart", "")
|
|
if teileart not in SENSOR_TEILEARTEN:
|
|
result.append("")
|
|
continue
|
|
bbox = item.get("_bbox")
|
|
if not bbox:
|
|
result.append("")
|
|
continue
|
|
bounds = _bounds(bbox, half_tol)
|
|
treffer = (_find(bounds, gf_entries)
|
|
or _find(bounds, vf_entries)
|
|
or _find(bounds, kreisel_entries)
|
|
or "nicht zugeordnet")
|
|
if dbg:
|
|
dbg(f" {item.get('teileart', '')} {item.get('teileid', '')!r} "
|
|
f"bounds={_fmt_bounds(bounds)} -> {treffer!r}")
|
|
result.append(treffer)
|
|
return result
|
|
|
|
|
|
def compute_scanner_nearest_separator(items, dbg=None):
|
|
"""Sucht fuer jeden Scanner ("ILS 2.0 Scanner") den raeumlich naechst-
|
|
gelegenen Separator ("ILS 2.0 Separator") - euklidischer Abstand der
|
|
BBox-Mittelpunkte (cx/cy, Grundriss, Z ignoriert wie ueberall in diesem
|
|
Modul).
|
|
|
|
Eigener, unabhaengiger Durchlauf, der NACH compute_sensor_zuordnung
|
|
aufgerufen wird (die Zuordnung der Separatoren zu ihrer Gefaellestrecke/
|
|
ihrem Foerderer/ihrer Kreiselhaelfte steht damit bereits fest - diese
|
|
Funktion aendert daran nichts, sie sucht unabhaengig davon zusaetzlich
|
|
den naechsten Separator zu jedem Scanner). Es gibt KEINE Abstands-
|
|
obergrenze - "immer der naechstgelegene" bedeutet auch bei grosser
|
|
Entfernung einen Treffer, solange ueberhaupt ein Separator existiert.
|
|
|
|
Rueckgabe: Liste von TeileId-Strings (des jeweils naechsten Separators),
|
|
positionsgleich zu items - "" fuer Nicht-Scanner-Items, fuer Scanner ohne
|
|
BBox oder wenn ueberhaupt kein Separator mit BBox in der Zeichnung ist.
|
|
"""
|
|
separators = [
|
|
(item.get("teileid", ""), item["_bbox"])
|
|
for item in items
|
|
if item.get("teileart", "") == "ILS 2.0 Separator" and item.get("_bbox")
|
|
]
|
|
|
|
if dbg:
|
|
dbg("")
|
|
dbg("=== Scanner -> naechstgelegener Separator ===")
|
|
dbg(f"Separator-Kandidaten mit BBox: {len(separators)}")
|
|
|
|
result = []
|
|
for item in items:
|
|
if item.get("teileart", "") != "ILS 2.0 Scanner":
|
|
result.append("")
|
|
continue
|
|
bbox = item.get("_bbox")
|
|
if not bbox or not separators:
|
|
result.append("")
|
|
if dbg:
|
|
dbg(f" Scanner {item.get('teileid', '')!r}: "
|
|
f"{'keine BBox' if not bbox else 'keine Separatoren vorhanden'} -> kein Treffer")
|
|
continue
|
|
cx, cy = bbox["cx"], bbox["cy"]
|
|
best_id, best_dist = "", None
|
|
for sep_id, sep_bbox in separators:
|
|
dist = math.hypot(sep_bbox["cx"] - cx, sep_bbox["cy"] - cy)
|
|
if best_dist is None or dist < best_dist:
|
|
best_dist = dist
|
|
best_id = sep_id
|
|
if dbg:
|
|
dbg(f" Scanner {item.get('teileid', '')!r} -> naechster Separator "
|
|
f"{best_id!r} (Abstand {best_dist:.0f}mm)")
|
|
result.append(best_id)
|
|
return result
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# Kreisel-Beruehrpunkte -> ILS-Weichen
|
|
# ---------------------------------------------------------------------------
|
|
|
|
def _clamp(v, lo, hi):
|
|
return lo if v < lo else (hi if v > hi else v)
|
|
|
|
|
|
def _kreisel_capsule(item, durchmesser_mm):
|
|
"""Achsen-Segment (Kapsel) eines "ILS 2.0 Kreisel"-Blocks in Welt-
|
|
koordinaten.
|
|
|
|
Geometrie siehe kreisel_half_bboxes / KreiselInsert.lsp::draw-module: die
|
|
AN8-(Antriebs-) Stationsmitte liegt lokal bei (radius, 0), die SP8-(Spann-)
|
|
Stationsmitte bei (radius+abstand, 0), radius = durchmesser_mm/2. Beide auf
|
|
der lokalen X-Achse. Die gesamte Kreisel-Flaeche ist das um radius
|
|
"aufgeblasene" Segment zwischen diesen beiden Mitten (Stadium/Kapsel: zwei
|
|
Endkreise mit Radius radius + zwei Tangenten bei Y=+-radius).
|
|
|
|
Rueckgabe: dict {"a": (ax,ay), "b": (bx,by), "r": radius, "rot": rot_rad,
|
|
"z": z-Mitte} - a = AN8-Mitte, b = SP8-Mitte (Welt).
|
|
"""
|
|
attribs = item.get("attribs", {})
|
|
abstand = safe_float(attribs.get("ABSTAND", "0"), 0.0)
|
|
r = durchmesser_mm / 2.0
|
|
x0 = item.get("x", 0.0) or 0.0
|
|
y0 = item.get("y", 0.0) or 0.0
|
|
rot = math.radians(item.get("rotation", 0.0) or 0.0)
|
|
cos_r, sin_r = math.cos(rot), math.sin(rot)
|
|
|
|
def world(lx, ly):
|
|
return (x0 + lx * cos_r - ly * sin_r, y0 + lx * sin_r + ly * cos_r)
|
|
|
|
bbox = item.get("_bbox") or {}
|
|
z = bbox.get("cz", item.get("z", 0.0) or 0.0)
|
|
return {
|
|
"a": world(r, 0.0),
|
|
"b": world(r + abstand, 0.0),
|
|
"r": r,
|
|
"rot": rot,
|
|
"z": z,
|
|
}
|
|
|
|
|
|
def _closest_pts_segments(p1, q1, p2, q2):
|
|
"""Naechstgelegene Punkte zweier 2D-Strecken [p1,q1] und [p2,q2] (Algorithmus
|
|
aus Ericson, Real-Time Collision Detection). Rueckgabe (c1, c2, s, t) mit
|
|
c1 auf Strecke 1, c2 auf Strecke 2 und Parametern s,t in [0,1] (0=Anfang,
|
|
1=Ende) - s/t am Rand bedeutet: der naechste Punkt liegt auf dem Endkreis
|
|
(Cap), dazwischen auf der Tangentenflanke."""
|
|
eps = 1e-9
|
|
d1 = (q1[0] - p1[0], q1[1] - p1[1])
|
|
d2 = (q2[0] - p2[0], q2[1] - p2[1])
|
|
r = (p1[0] - p2[0], p1[1] - p2[1])
|
|
a = d1[0] * d1[0] + d1[1] * d1[1]
|
|
e = d2[0] * d2[0] + d2[1] * d2[1]
|
|
f = d2[0] * r[0] + d2[1] * r[1]
|
|
if a <= eps and e <= eps:
|
|
s = t = 0.0
|
|
elif a <= eps:
|
|
s = 0.0
|
|
t = _clamp(f / e, 0.0, 1.0)
|
|
else:
|
|
c = d1[0] * r[0] + d1[1] * r[1]
|
|
if e <= eps:
|
|
t = 0.0
|
|
s = _clamp(-c / a, 0.0, 1.0)
|
|
else:
|
|
b = d1[0] * d2[0] + d1[1] * d2[1]
|
|
denom = a * e - b * b
|
|
s = _clamp((b * f - c * e) / denom, 0.0, 1.0) if denom > eps else 0.0
|
|
t = (b * s + f) / e
|
|
if t < 0.0:
|
|
t = 0.0
|
|
s = _clamp(-c / a, 0.0, 1.0)
|
|
elif t > 1.0:
|
|
t = 1.0
|
|
s = _clamp((b - c) / a, 0.0, 1.0)
|
|
c1 = (p1[0] + d1[0] * s, p1[1] + d1[1] * s)
|
|
c2 = (p2[0] + d2[0] * t, p2[1] + d2[1] * t)
|
|
return c1, c2, s, t
|
|
|
|
|
|
def _oriented_box_aabb(rot_rad, laenge, breite, hoehe):
|
|
"""Achsparallele Welt-Bounding-Box (dx,dy,dz) einer Box, deren Laengsseite
|
|
(laenge) senkrecht zur Achsenrichtung rot_rad liegt und deren Breite
|
|
(breite) entlang der Achse - konsistent mit allen anderen BBox-Spalten
|
|
(Ausdehnung der gedrehten Flaeche in Welt-X/Y).
|
|
|
|
Achse = (cos rot, sin rot); Laengsseite senkrecht dazu = (-sin rot, cos rot).
|
|
"""
|
|
c = abs(math.cos(rot_rad))
|
|
s = abs(math.sin(rot_rad))
|
|
dx = laenge * s + breite * c
|
|
dy = laenge * c + breite * s
|
|
return (dx, dy, hoehe)
|
|
|
|
|
|
def compute_kreisel_touch_switches(items, tolerance_mm, durchmesser_mm=800.0, dbg=None):
|
|
"""Erzeugt fuer jedes sich beruehrende Paar echter Kreisel ("ILS 2.0
|
|
Kreisel") eine ILS-Weiche am Beruehrpunkt.
|
|
|
|
Kreisel-Modell: eine Kapsel (Stadium) - das Achsensegment AN8-Mitte ->
|
|
SP8-Mitte, aufgeblasen um r = durchmesser_mm/2 (siehe _kreisel_capsule).
|
|
Zwei Kreisel beruehren sich, wenn der minimale Abstand ihrer Achsen-
|
|
segmente <= 2r + tolerance_mm ist (2r = durchmesser_mm). Eigenstaendige,
|
|
praezise Geometriepruefung - unabhaengig vom groben Links/Rechts-AABB-
|
|
Nachbarschaftstest (compute_neighbor_ids), der nur ueberschneidende
|
|
Bounding-Boxen, nicht den exakten Beruehrpunkt liefert.
|
|
|
|
Beruehrpunkt (= Weichen-Position) = Mittelpunkt der beiden naechstgelegenen
|
|
Segmentpunkte. Die Weichen-Box (WEICHE_BOX_*) wird mit ihrer Laengsseite
|
|
senkrecht zur Achse desjenigen Kreisels ausgerichtet, dessen beruehrendes
|
|
Merkmal ein Endkreis ist (naechster Segmentpunkt am Segmentende). Beruehren
|
|
sich zwei Endkreise oder zwei Tangentenflanken, wird der erstgenannte
|
|
Kreisel des Paares (kleinerer items-Index) genommen. Ausgegeben wird die
|
|
achsparallele Welt-Bounding-Box der gedrehten Box (siehe _oriented_box_aabb).
|
|
|
|
Rueckgabe: Liste von dicts (in items-Reihenfolge der Paare), je erzeugte
|
|
Weiche:
|
|
{"x","y","z": Beruehrpunkt (mm),
|
|
"dx","dy","dz": Welt-AABB der ausgerichteten Box (mm),
|
|
"nachbarn": [teileid_a, teileid_b],
|
|
"achse_von": teileid des orientierungsgebenden Kreisels,
|
|
"beruehrung": "Kreis/Tangente" | "Kreis/Kreis" | "Tangente/Tangente"}
|
|
"""
|
|
eps = 1e-6
|
|
kreisel = []
|
|
for item in items:
|
|
if item.get("teileart", "") != KREISEL_SPLIT_TEILEART:
|
|
continue
|
|
kreisel.append((item.get("teileid", ""), _kreisel_capsule(item, durchmesser_mm)))
|
|
|
|
if dbg:
|
|
dbg("")
|
|
dbg("=== Kreisel-Beruehrpunkte -> ILS-Weichen ===")
|
|
dbg(f"Echte Kreisel ('{KREISEL_SPLIT_TEILEART}'): {len(kreisel)}; "
|
|
f"Beruehrschwelle Abstand <= {durchmesser_mm + tolerance_mm:.0f}mm "
|
|
f"(2r={durchmesser_mm:.0f} + Toleranz {tolerance_mm:.0f})")
|
|
|
|
switches = []
|
|
for i in range(len(kreisel)):
|
|
id_a, cap_a = kreisel[i]
|
|
for j in range(i + 1, len(kreisel)):
|
|
id_b, cap_b = kreisel[j]
|
|
c1, c2, s, t = _closest_pts_segments(cap_a["a"], cap_a["b"],
|
|
cap_b["a"], cap_b["b"])
|
|
dist = math.hypot(c1[0] - c2[0], c1[1] - c2[1])
|
|
schwelle = cap_a["r"] + cap_b["r"] + tolerance_mm
|
|
if dist > schwelle:
|
|
if dbg:
|
|
dbg(f" {id_a} <-> {id_b}: kein Kontakt "
|
|
f"(Achsabstand {dist:.0f}mm > {schwelle:.0f}mm)")
|
|
continue
|
|
|
|
# Beruehrpunkt = Mitte der beiden naechstgelegenen Segmentpunkte
|
|
tx = (c1[0] + c2[0]) / 2.0
|
|
ty = (c1[1] + c2[1]) / 2.0
|
|
tz = (cap_a["z"] + cap_b["z"]) / 2.0
|
|
|
|
# Endkreis (Cap) beruehrt, wenn der naechste Segmentpunkt am Ende
|
|
# liegt (s bzw. t == 0 oder 1); dazwischen Tangentenflanke.
|
|
cap1 = s <= eps or s >= 1.0 - eps
|
|
cap2 = t <= eps or t >= 1.0 - eps
|
|
if cap1 and not cap2:
|
|
achse_id, achse_rot, beruehrung = id_a, cap_a["rot"], "Kreis/Tangente"
|
|
elif cap2 and not cap1:
|
|
achse_id, achse_rot, beruehrung = id_b, cap_b["rot"], "Kreis/Tangente"
|
|
else:
|
|
achse_id, achse_rot = id_a, cap_a["rot"]
|
|
beruehrung = "Kreis/Kreis" if cap1 else "Tangente/Tangente"
|
|
|
|
dx, dy, dz = _oriented_box_aabb(
|
|
achse_rot, WEICHE_BOX_LAENGE_MM, WEICHE_BOX_BREITE_MM, WEICHE_BOX_HOEHE_MM)
|
|
switches.append({
|
|
"x": tx, "y": ty, "z": tz,
|
|
"dx": dx, "dy": dy, "dz": dz,
|
|
"nachbarn": [id_a, id_b],
|
|
"achse_von": achse_id,
|
|
"beruehrung": beruehrung,
|
|
})
|
|
if dbg:
|
|
dbg(f" {id_a} <-> {id_b}: BERUEHRUNG ({beruehrung}, Achsabstand "
|
|
f"{dist:.0f}mm) -> Weiche bei ({tx:.0f}, {ty:.0f}, {tz:.0f}), "
|
|
f"Box-AABB ({dx:.0f}, {dy:.0f}, {dz:.0f}), Achse von Kreisel {achse_id}")
|
|
|
|
if dbg:
|
|
dbg(f"Erzeugte ILS-Weichen: {len(switches)}")
|
|
|
|
return switches
|