119 lines
3.7 KiB
Python
119 lines
3.7 KiB
Python
import sympy as sp
|
|
import numpy as np
|
|
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(Q_ll: np.ndarray, Q_AA: np.ndarray) -> np.ndarray:
|
|
if Q_ll.ndim != 2 or Q_ll.shape[0] != Q_ll.shape[1]:
|
|
raise ValueError("Q_ll muss quadratisch sein.")
|
|
if Q_AA.ndim != 2 or Q_AA.shape[0] != Q_AA.shape[1]:
|
|
raise ValueError("Q_AA muss quadratisch sein.")
|
|
Q_ext = np.block([[Q_ll, np.zeros((Q_ll.shape[0], Q_AA.shape[0]))],[np.zeros((Q_AA.shape[0], Q_ll.shape[0])), Q_AA]])
|
|
return Q_ext |