104 lines
3.1 KiB
Python
104 lines
3.1 KiB
Python
import sympy as sp
|
||
import numpy as np
|
||
from typing import Iterable, List, Sequence, Tuple, Optional
|
||
|
||
|
||
class Datumsfestlegung:
|
||
|
||
@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
|
||
|
||
|
||
@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 |