Weichen werden bei der Kollisionspunkten zweier Kreisel erzeugt und im csv Export mit heraus geschrieben
This commit is contained in:
@@ -79,6 +79,19 @@ SENSOR_TEILEARTEN = {"ILS 2.0 Separator", "ILS 2.0 Scanner"}
|
||||
# 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
|
||||
@@ -740,3 +753,195 @@ def compute_scanner_nearest_separator(items, dbg=None):
|
||||
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
|
||||
|
||||
Reference in New Issue
Block a user