81 lines
1.8 KiB
Python
81 lines
1.8 KiB
Python
import sympy as sp
|
|
from typing import List, Iterable, Tuple
|
|
|
|
def raenderungsmatrix_G(
|
|
x0: sp.Matrix,
|
|
idx_X: List[int],
|
|
idx_Y: List[int],
|
|
idx_Z: List[int],
|
|
mit_massstab: bool = True,
|
|
) -> sp.Matrix:
|
|
|
|
u = x0.rows
|
|
d = 7 if mit_massstab else 6
|
|
G = sp.zeros(u, d)
|
|
|
|
# --- Translationen ---
|
|
for i in idx_X:
|
|
G[i, 0] = 1
|
|
for i in idx_Y:
|
|
G[i, 1] = 1
|
|
for i in idx_Z:
|
|
G[i, 2] = 1
|
|
|
|
# --- Rotationen ---
|
|
# Rotation um X-Achse
|
|
for iy, iz in zip(idx_Y, idx_Z):
|
|
zi = x0[iz, 0]
|
|
yi = x0[iy, 0]
|
|
G[iy, 3] = -zi
|
|
G[iz, 3] = yi
|
|
|
|
# Rotation um Y-Achse
|
|
for ix, iz in zip(idx_X, idx_Z):
|
|
zi = x0[iz, 0]
|
|
xi = x0[ix, 0]
|
|
G[ix, 4] = zi
|
|
G[iz, 4] = -xi
|
|
|
|
# Rotation um Z-Achse
|
|
for ix, iy in zip(idx_X, idx_Y):
|
|
yi = x0[iy, 0]
|
|
xi = x0[ix, 0]
|
|
G[ix, 5] = -yi
|
|
G[iy, 5] = xi
|
|
|
|
# --- Maßstab ---
|
|
if mit_massstab:
|
|
for ix, iy, iz in zip(idx_X, idx_Y, idx_Z):
|
|
xi = x0[ix, 0]
|
|
yi = x0[iy, 0]
|
|
zi = x0[iz, 0]
|
|
G[ix, 6] = xi
|
|
G[iy, 6] = yi
|
|
G[iz, 6] = zi
|
|
return G
|
|
|
|
|
|
def auswahlmatrix_E(u: int, aktive_unbekannte_indices: Iterable[int]) -> sp.Matrix:
|
|
E = sp.zeros(u, u)
|
|
for idx in aktive_unbekannte_indices:
|
|
E[int(idx), int(idx)] = 1
|
|
return E
|
|
|
|
|
|
def teilspurminimierung_Gi(G: sp.Matrix, E: sp.Matrix) -> sp.Matrix:
|
|
Gi = E * G
|
|
return Gi
|
|
|
|
|
|
def berechne_dx_geraendert(N: sp.Matrix, n: sp.Matrix, Gi: sp.Matrix) -> sp.Matrix:
|
|
u = N.rows
|
|
d = Gi.shape[1]
|
|
|
|
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)
|
|
dx = sol[:u, :]
|
|
return dx |