155 lines
4.8 KiB
Python
155 lines
4.8 KiB
Python
import sympy as sp
|
||
from typing import Iterable, List, Sequence, Tuple, Optional
|
||
|
||
|
||
class Datumsfestlegung:
|
||
|
||
@staticmethod
|
||
def datumskomponenten(
|
||
auswahl: Iterable[Tuple[str, str]],
|
||
liste_punktnummern: Sequence[str],
|
||
*,
|
||
layout: str = "XYZ"
|
||
) -> List[int]:
|
||
punkt2pos = {str(p): i for i, p in enumerate(liste_punktnummern)}
|
||
|
||
layout = layout.upper()
|
||
if layout != "XYZ":
|
||
raise ValueError("Nur layout='XYZ' unterstützt (wie bei euch).")
|
||
comp2off = {"X": 0, "Y": 1, "Z": 2}
|
||
|
||
aktive: List[int] = []
|
||
for pt, comp in auswahl:
|
||
spt = str(pt)
|
||
c = comp.upper()
|
||
if spt not in punkt2pos:
|
||
raise KeyError(f"Punkt '{pt}' nicht in liste_punktnummern.")
|
||
if c not in comp2off:
|
||
raise ValueError(f"Komponente '{comp}' ungültig. Nur X,Y,Z.")
|
||
p = punkt2pos[spt]
|
||
aktive.append(3 * p + comp2off[c])
|
||
|
||
# Duplikate entfernen
|
||
out, seen = [], set()
|
||
for i in aktive:
|
||
if i not in seen:
|
||
seen.add(i)
|
||
out.append(i)
|
||
return out
|
||
|
||
@staticmethod
|
||
def auswahlmatrix_E(u: int, aktive_unbekannte_indices: Iterable[int]) -> sp.Matrix:
|
||
E = sp.zeros(u, u)
|
||
for idx in aktive_unbekannte_indices:
|
||
i = int(idx)
|
||
if not (0 <= i < u):
|
||
raise IndexError(f"Aktiver Index {i} außerhalb [0,{u-1}]")
|
||
E[i, i] = 1
|
||
return E
|
||
|
||
@staticmethod
|
||
def raenderungsmatrix_G(
|
||
x0: sp.Matrix,
|
||
liste_punktnummern: Sequence[str],
|
||
*,
|
||
mit_massstab: bool = True,
|
||
layout: str = "XYZ",
|
||
) -> sp.Matrix:
|
||
if x0.cols != 1:
|
||
raise ValueError("x0 muss Spaltenvektor sein.")
|
||
layout = layout.upper()
|
||
if layout != "XYZ":
|
||
raise ValueError("Nur layout='XYZ' unterstützt (wie bei euch).")
|
||
|
||
nP = len(liste_punktnummern)
|
||
u = x0.rows
|
||
d = 7 if mit_massstab else 6
|
||
G = sp.zeros(u, d)
|
||
|
||
for p in range(nP):
|
||
ix, iy, iz = 3*p, 3*p+1, 3*p+2
|
||
xi, yi, zi = x0[ix, 0], x0[iy, 0], x0[iz, 0]
|
||
|
||
# Translationen
|
||
G[ix, 0] = 1
|
||
G[iy, 1] = 1
|
||
G[iz, 2] = 1
|
||
|
||
# Rotationen
|
||
G[iy, 3] = -zi; G[iz, 3] = yi # Rx
|
||
G[ix, 4] = zi; G[iz, 4] = -xi # Ry
|
||
G[ix, 5] = -yi; G[iy, 5] = xi # Rz
|
||
|
||
# Maßstab
|
||
if mit_massstab:
|
||
G[ix, 6] = xi
|
||
G[iy, 6] = yi
|
||
G[iz, 6] = zi
|
||
|
||
return G
|
||
|
||
@staticmethod
|
||
def berechne_dx_geraendert(N: sp.Matrix, n: sp.Matrix, Gi: sp.Matrix) -> sp.Matrix:
|
||
if N.rows != N.cols:
|
||
raise ValueError("N muss quadratisch sein.")
|
||
if n.cols != 1:
|
||
raise ValueError("n muss Spaltenvektor sein.")
|
||
if Gi.rows != N.rows:
|
||
raise ValueError("Gi hat falsche Zeilenzahl.")
|
||
|
||
u = N.rows
|
||
d = Gi.cols
|
||
K = N.row_join(Gi)
|
||
K = K.col_join(Gi.T.row_join(sp.zeros(d, d)))
|
||
rhs = n.col_join(sp.zeros(d, 1))
|
||
sol = K.LUsolve(rhs)
|
||
return sol[:u, :]
|
||
|
||
@staticmethod
|
||
def weiches_datum(
|
||
A: sp.Matrix,
|
||
dl: sp.Matrix,
|
||
Q_ll: sp.Matrix,
|
||
x0: sp.Matrix,
|
||
anschluss_indices: Sequence[int],
|
||
anschluss_werte: sp.Matrix,
|
||
Sigma_AA: Optional[sp.Matrix] = None,
|
||
) -> Tuple[sp.Matrix, sp.Matrix, sp.Matrix]:
|
||
if dl.cols != 1 or x0.cols != 1:
|
||
raise ValueError("dl und x0 müssen Spaltenvektoren sein.")
|
||
if A.rows != dl.rows:
|
||
raise ValueError("A.rows muss dl.rows entsprechen.")
|
||
if A.cols != x0.rows:
|
||
raise ValueError("A.cols muss x0.rows entsprechen.")
|
||
if Q_ll.rows != Q_ll.cols or Q_ll.rows != A.rows:
|
||
raise ValueError("Q_ll muss (n×n) sein und zu A.rows passen.")
|
||
|
||
u = A.cols
|
||
idx = [int(i) for i in anschluss_indices]
|
||
m = len(idx)
|
||
|
||
if anschluss_werte.cols != 1 or anschluss_werte.rows != m:
|
||
raise ValueError("anschluss_werte muss (m×1) sein.")
|
||
if Sigma_AA is None:
|
||
Sigma_AA = sp.eye(m)
|
||
if Sigma_AA.rows != m or Sigma_AA.cols != m:
|
||
raise ValueError("Sigma_AA muss (m×m) sein.")
|
||
|
||
A_A = sp.zeros(m, u)
|
||
for r, j in enumerate(idx):
|
||
if not (0 <= j < u):
|
||
raise IndexError(f"Anschluss-Index {j} außerhalb [0,{u-1}]")
|
||
A_A[r, j] = 1
|
||
|
||
x0_A = sp.Matrix([[x0[j, 0]] for j in idx])
|
||
dl_A = anschluss_werte - x0_A
|
||
|
||
A_ext = A.col_join(A_A)
|
||
dl_ext = dl.col_join(dl_A)
|
||
|
||
Q_ext = sp.zeros(Q_ll.rows + m, Q_ll.cols + m)
|
||
Q_ext[:Q_ll.rows, :Q_ll.cols] = Q_ll
|
||
Q_ext[Q_ll.rows:, Q_ll.cols:] = Sigma_AA
|
||
|
||
return A_ext, dl_ext, Q_ext
|