Push
This commit is contained in:
@@ -5,110 +5,6 @@ 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]:
|
||||
@@ -116,4 +12,93 @@ class Datumsfestlegung:
|
||||
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
|
||||
return Q_ext
|
||||
|
||||
|
||||
@staticmethod
|
||||
def make_index(unbekannten_liste):
|
||||
names = [str(s).strip() for s in unbekannten_liste]
|
||||
return names, {n: i for i, n in enumerate(names)}
|
||||
|
||||
|
||||
def build_G_from_names(x0, unbekannten_liste, liste_punktnummern, mit_massstab=True):
|
||||
x0 = np.asarray(x0, float).reshape(-1)
|
||||
names, idx = Datumsfestlegung.make_index(unbekannten_liste)
|
||||
|
||||
u = len(names)
|
||||
d = 7 if mit_massstab else 6
|
||||
G = np.zeros((u, d), dtype=float)
|
||||
|
||||
for pid in liste_punktnummern:
|
||||
sx, sy, sz = f"X{pid}", f"Y{pid}", f"Z{pid}"
|
||||
if sx not in idx or sy not in idx or sz not in idx:
|
||||
# Punkt nicht als voller XYZ-Unbekannter vorhanden -> skip
|
||||
continue
|
||||
|
||||
ix, iy, iz = idx[sx], idx[sy], idx[sz]
|
||||
xi, yi, zi = x0[ix], x0[iy], x0[iz]
|
||||
|
||||
# Translationen
|
||||
G[ix, 0] = 1.0
|
||||
G[iy, 1] = 1.0
|
||||
G[iz, 2] = 1.0
|
||||
|
||||
# Rotationen (δr = ω × r)
|
||||
# Rx: δY=-Z, δZ=+Y
|
||||
G[iy, 3] = -zi; G[iz, 3] = yi
|
||||
# Ry: δX=+Z, δZ=-X
|
||||
G[ix, 4] = zi; G[iz, 4] = -xi
|
||||
# Rz: δX=-Y, δY=+X
|
||||
G[ix, 5] = -yi; G[iy, 5] = xi
|
||||
|
||||
# Maßstab
|
||||
if mit_massstab:
|
||||
G[ix, 6] = xi
|
||||
G[iy, 6] = yi
|
||||
G[iz, 6] = zi
|
||||
return G
|
||||
|
||||
|
||||
def aktive_indices_from_selection(auswahl, unbekannten_liste):
|
||||
names, idx = Datumsfestlegung.make_index(unbekannten_liste)
|
||||
aktive = []
|
||||
for pid, comp in auswahl:
|
||||
key = f"{comp.upper()}{str(pid)}"
|
||||
if key not in idx:
|
||||
raise KeyError(f"{key} nicht im Unbekanntenvektor.")
|
||||
aktive.append(idx[key])
|
||||
# unique
|
||||
out = []
|
||||
seen = set()
|
||||
for i in aktive:
|
||||
if i not in seen:
|
||||
seen.add(i)
|
||||
out.append(i)
|
||||
return out
|
||||
|
||||
|
||||
def auswahlmatrix_E(u, aktive_indices):
|
||||
E = np.zeros((u, u), dtype=float)
|
||||
for i in aktive_indices:
|
||||
E[int(i), int(i)] = 1.0
|
||||
return E
|
||||
|
||||
|
||||
def berechne_dx_geraendert(N, n, G):
|
||||
N = np.asarray(N, float)
|
||||
n = np.asarray(n, float).reshape(-1, 1)
|
||||
G = np.asarray(G, float)
|
||||
|
||||
u = N.shape[0]
|
||||
d = G.shape[1]
|
||||
|
||||
K = np.block([
|
||||
[N, G],
|
||||
[G.T, np.zeros((d, d))]
|
||||
])
|
||||
rhs = np.vstack([n, np.zeros((d, 1))])
|
||||
|
||||
sol = np.linalg.solve(K, rhs)
|
||||
dx = sol[:u]
|
||||
k = sol[u:]
|
||||
return dx, k
|
||||
Reference in New Issue
Block a user