def fprime(s):
x2 = (X2 @ intp.lagrange_polynomial(n_nodes_2 - 1, [s])).flatten()
dx2 = (X2 @ intp.lagrange_polynomial_derivative(n_nodes_2 - 1,
[s])).flatten()
ddx2 = (X2 @ intp.lagrange_polynomial_2_derivative(n_nodes_2 - 1,
[s])).flatten()
d3x2 = (X2 @ intp.lagrange_polynomial_3_derivative(n_nodes_2 - 1,
[s])).flatten()
d = x1 - x2
v = _triple(dx1, dx2, x1 - x2)
dv = _triple(dx1, ddx2, d)
ddv = _triple(dx1, d3x2, d)
R1_ = 1 / v * (np.dot(dx2, d) * np.cross(d, dx1) +
0.5 * np.dot(d, d) * np.cross(dx1, dx2))
dR1_ = 1 / v * (1 / 2 * np.dot(d, d) * np.cross(dx1, ddx2) +
(np.dot(ddx2, d) - np.dot(dx2, dx2) -
dv / v * np.dot(dx2, d)) * np.cross(d, dx1) -
1 / 2 * dv / v * np.dot(d, d) * np.cross(dx1, dx2))
ddR1_ = (np.cross(dx1, dx2) *
((-(ddv * v) + dv *
(2 * dv + v)) * np.dot(d, d) + 2 * v**2 * np.dot(d, ddx2) +
2 * v**2 * np.dot(dx2, dx2)) + 2 * np.cross(d, dx1) *
((-(ddv * v) + dv *
(2 * dv + v)) * np.dot(d, dx2) + v * v * np.dot(d, d3x2) -
2 * v * dv * np.dot(d, ddx2) - 3 * v**2 * np.dot(dx2, ddx2) +
2 * v * dv * np.dot(dx2, dx2)) + v**2 * np.cross(dx1, d3x2) *
np.dot(d, d) - 2 * v * np.cross(dx1, ddx2) *
(dv * np.dot(d, d) + v * np.dot(d, dx2))) / (2 * v**3)
return np.dot(ddR1_, R1_) + np.dot(dR1_, dR1_)
def circular_point_projection(interpolation: str,
X1: np.ndarray,
X2: np.ndarray,
s1: float,
TOLER: float = 1e-8,
MAXITER: int = 100) -> np.ndarray:
n_nodes_1 = len(X1[0])
n_nodes_2 = len(X2[0])
x1 = (X1 @ intp.lagrange_polynomial(n_nodes_1 - 1, [s1])).flatten()
dx1 = (X1 @ intp.lagrange_polynomial_derivative(n_nodes_1 - 1,
[s1])).flatten()
x2a = (X2 @ intp.lagrange_polynomial(n_nodes_2 - 1, [s1 + 0.01])).flatten()
x2b = (X2 @ intp.lagrange_polynomial(n_nodes_2 - 1, [s1 - 0.01])).flatten()
u = np.zeros((5))
u[0] = s1
u[1:3] = (x1 - x2b) / 2
u[3:5] = (x2a - x1) / 2
R = np.ones((5))
i = 0
while np.linalg.norm(R) > TOLER and i < MAXITER:
K = np.zeros((5, 5))
if interpolation == "Lagrange polynoms":
s2 = u[0]
v1 = u[1:3]
v2 = u[3:5]
x2 = (X2 @ intp.lagrange_polynomial(n_nodes_2 - 1, [s2])).flatten()
dx2 = (X2 @ intp.lagrange_polynomial_derivative(
n_nodes_2 - 1, [s2])).flatten()
ddx2 = (X2 @ intp.lagrange_polynomial_2_derivative(
n_nodes_2 - 1, [s2])).flatten()
R[:2] = x1 - (x2 + v2 - v1)
R[2] = 0 - (v2.dot(v2) - v1.dot(v1))
R[3] = 0 - (v1.dot(dx1))
R[4] = 0 - (v2.dot(dx2))
K[0, :2] = dx2
K[0, 2] = 0
K[0, 3] = 0
K[0, 4] = ddx2.dot(v2)
K[1:3, :2] = -np.identity(2)
K[1:3, 2] = -2 * v1
K[1:3, 3] = dx1
K[1:3, 4] = 0
K[3:5, :2] = np.identity(2)
K[3:5, 2] = 2 * v2
K[3:5, 3] = 0
K[3:5, 4] = dx2
u += np.linalg.solve(K.T, R)
i += 1
if i == MAXITER:
print("No convergence")
return False
return u
def f(s):
x2 = (X2 @ intp.lagrange_polynomial(n_nodes_2 - 1, [s])).flatten()
dx2 = (X2 @ intp.lagrange_polynomial_derivative(n_nodes_2 - 1,
[s])).flatten()
ddx2 = (X2 @ intp.lagrange_polynomial_2_derivative(n_nodes_2 - 1,
[s])).flatten()
d = x1 - x2
v = _triple(dx1, dx2, x1 - x2)
R1_ = 1 / v * (np.dot(dx2, d) * np.cross(d, dx1) +
0.5 * np.dot(d, d) * np.cross(dx1, dx2))
dv = _triple(dx1, ddx2, d)
dR1_ = 1 / v * (1 / 2 * np.dot(d, d) * np.cross(dx1, ddx2) +
(np.dot(ddx2, d) - np.dot(dx2, dx2) -
dv / v * np.dot(dx2, d)) * np.cross(d, dx1) -
1 / 2 * dv / v * np.dot(d, d) * np.cross(dx1, dx2))
return np.dot(dR1_, R1_)
def test_nearest_point_projection_1(self):
correct = np.array([0, 0, 0, 5])
X = np.array([[0, 0, 0], [10, 0, 0]]).T
P = np.array([5, 0, 5])
interpolation_d0 = lambda x: intp.lagrange_polynomial(
degree=X.shape[1] - 1, eval_pts=x)
interpolation_d1 = lambda x: intp.lagrange_polynomial_derivative(
degree=X.shape[1] - 1, eval_pts=x)
interpolation_d2 = lambda x: intp.lagrange_polynomial_2_derivative(
degree=X.shape[1] - 1, eval_pts=x)
u = proj.nearest_point_projection(interpolation_d0,
interpolation_d1,
interpolation_d2,
X,
P,
s0=0,
TOLER=1e-8,
MAXITER=10)
self.assertTrue(np.allclose(u, correct, rtol=1e-10))
def test_nearest_point_projection_2(self):
correct = np.array([-0.23045933, 0.38268789, -0.0406458, -0.4820473])
X = np.array([[0, 0, 0], [1, 0, 0.4], [1.5, 0.4, 1.0], [1.2, 1.0,
1.7]]).T
P = np.array([1.5, 0.0, 0.0])
interpolation_d0 = lambda x: intp.lagrange_polynomial(
degree=X.shape[1] - 1, eval_pts=x)
interpolation_d1 = lambda x: intp.lagrange_polynomial_derivative(
degree=X.shape[1] - 1, eval_pts=x)
interpolation_d2 = lambda x: intp.lagrange_polynomial_2_derivative(
degree=X.shape[1] - 1, eval_pts=x)
u = proj.nearest_point_projection(interpolation_d0,
interpolation_d1,
interpolation_d2,
X,
P,
s0=0,
TOLER=1e-8,
MAXITER=10)
self.assertTrue(np.allclose(u, correct, rtol=1e-10))
def _spherical_point_projection(interpolation: str, X1: np.ndarray,
X2: np.ndarray, s1: float, s2_0: float,
TOLER: float, MAXITER: int) -> tuple:
n_nodes_1 = len(X1[0])
n_nodes_2 = len(X2[0])
x1 = (X1 @ intp.lagrange_polynomial(n_nodes_1 - 1, [s1])).flatten()
dx1 = (X1 @ intp.lagrange_polynomial_derivative(n_nodes_1 - 1,
[s1])).flatten()
s2 = s2_0
R = 1
n = 0
while abs(R) > TOLER and n < MAXITER:
K = 0
if interpolation == "Lagrange polynoms":
x2 = (X2 @ intp.lagrange_polynomial(n_nodes_2 - 1, [s2])).flatten()
dx2 = (X2 @ intp.lagrange_polynomial_derivative(
n_nodes_2 - 1, [s2])).flatten()
ddx2 = (X2 @ intp.lagrange_polynomial_2_derivative(
n_nodes_2 - 1, [s2])).flatten()
dddx2 = (X2 @ intp.lagrange_polynomial_2_derivative(
n_nodes_2 - 1, [s2])).flatten()
Ai = A_(x1, x2, dx1, dx2)
dAi = dA_(x1, x2, dx1, dx2, ddx2)
ddAi = ddA_(x1, x2, dx1, dx2, ddx2, dddx2)
Bi = B_(x1, x2, dx1, dx2)
dBi = dB_(x1, x2, dx1, dx2, ddx2)
ddBi = ddB_(x1, x2, dx1, dx2, ddx2, dddx2)
dRR = np.zeros(3)
ddRR = np.ones(3)
for i in range(3):
if Bi[i] != 0:
dRR[i] = (dAi[i] * Ai[i] / Bi[i]**2 -
Ai[i]**2 * dBi[i] / Bi[i]**3)
ddRR[i] = (ddAi[i] * Ai[i] / Bi[i]**2 +
dAi[i]**2 / Bi[i]**2 -
2 * dAi[i] * Ai[i] * dBi[i] / Bi[i]**3 -
2 * Ai[i] * dAi[i] * dBi[i] / Bi[i]**3 -
Ai[i]**2 * ddBi[i] / Bi[i]**3 +
3 * Ai[i]**2 * dBi[i]**2 / Bi[i]**4)
R = -(dRR).dot(dx1**2)
K = (ddRR).dot(dx1**2)
ds = R / K
s2 += ds
n += 1
if n == MAXITER:
return False
x2 = (X2 @ intp.lagrange_polynomial(n_nodes_2 - 1, [s2])).flatten()
dx2 = (X2 @ intp.lagrange_polynomial_derivative(n_nodes_2 - 1,
[s2])).flatten()
ddx2 = (X2 @ intp.lagrange_polynomial_2_derivative(n_nodes_2 - 1,
[s2])).flatten()
dddx2 = (X2 @ intp.lagrange_polynomial_2_derivative(n_nodes_2 - 1,
[s2])).flatten()
Ai = A_(x1, x2, dx1, dx2)
Bi = B_(x1, x2, dx1, dx2)
R1 = np.zeros(3)
for i in range(3):
if Bi[i] != 0:
R1[i] = Ai[i] / Bi[i] * dx1[i]
return (s2, x2, R1)
def test_lagrange_polynomial_2_derivative(self):
correct = np.array([[1.0, 1.0], [-2.0, -2.0], [1.0, 1.0]])
u = intp.lagrange_polynomial_2_derivative(degree=2,
eval_pts=[-0.3, 0.3])
self.assertTrue(np.allclose(u, correct, rtol=1e-10))
def __init__(self,
nodes,
mesh_dof_per_node: int,
ref_vec: np.ndarray,
coordinates: np.ndarray,
beam=None,
angular_velocities: np.ndarray = None,
angular_accelerations: np.ndarray = None,
distributed_load: np.ndarray = np.zeros(shape=(6)),
area: float = 1.0,
density: float = 0.0,
elastic_modulus: float = 1.0,
shear_modulus: float = 1.0,
inertia_primary: float = 1.0,
inertia_secondary: float = None,
inertia_torsion: float = None,
shear_coefficient: float = 1,
contact_radius: float = 1):
# --------------------------------------------------------------
# nodes
dof = np.zeros(mesh_dof_per_node, dtype=np.bool)
dof[:6] = True
super().__init__(nodes, dof)
# --------------------------------------------------------------
# defualt values
if angular_velocities is None:
angular_velocities = np.zeros(shape=(3, self.n_nodes))
if angular_accelerations is None:
angular_accelerations = np.zeros(shape=(3, self.n_nodes))
if inertia_secondary is None:
inertia_secondary = inertia_primary
if inertia_torsion is None:
inertia_torsion = inertia_primary
# --------------------------------------------------------------
# displacement interpolation
self.Ndis = [
lambda x: intp.lagrange_polynomial(self.n_nodes - 1, x),
lambda x: intp.lagrange_polynomial_derivative(self.n_nodes - 1, x),
lambda x: intp.lagrange_polynomial_2_derivative(
self.n_nodes - 1, x),
lambda x: intp.lagrange_polynomial_3_derivative(
self.n_nodes - 1, x)
]
# rotation interpolation
self.Nrot = [
lambda x: intp.lagrange_polynomial(self.n_nodes - 1, x),
lambda x: intp.lagrange_polynomial_derivative(self.n_nodes - 1, x),
lambda x: intp.lagrange_polynomial_2_derivative(
self.n_nodes - 1, x),
lambda x: intp.lagrange_polynomial_3_derivative(
self.n_nodes - 1, x)
]
# integration points
lgf = np.polynomial.legendre.leggauss(self.n_nodes)
lgr = np.polynomial.legendre.leggauss(self.n_nodes - 1)
self.int_pts = [
struct.BeamIntegrationPoint(displacement_interpolation=self.Ndis,
rotation_interpolation=self.Nrot,
points_location=lgf[0],
weights=lgf[1]),
struct.BeamIntegrationPoint(displacement_interpolation=self.Ndis,
rotation_interpolation=self.Nrot,
points_location=lgr[0],
weights=lgr[1])
]
# Interpolation derivatives need to be corrected for
# isoparametric formulation (inverse of jacobian). This is
# done, when the element length is computed.
# --------------------------------------------------------------
# initial element length
dxds = coordinates @ self.int_pts[1].dN_displacement
intg = np.zeros(shape=(3))
for i in range(len(intg)):
intg[i] = np.dot(dxds[i], self.int_pts[1].wgt)
L = np.linalg.norm(intg)
self.jacobian = L / 2
# --------------------------------------------------------------
# initial rotation
for i in range(len(self.int_pts)):
self.int_pts[i].rot = np.zeros(shape=(3, 4, self.int_pts[i].n_pts))
dx = 1 / self.jacobian * coordinates @ self.int_pts[
i].dN_displacement
for g in range(self.int_pts[i].n_pts):
rotmat = np.zeros(shape=(3, 3))
rotmat[:, 0] = math.normalized(dx[:, g])
rotmat[:, 1] = math.normalized(np.cross(ref_vec, rotmat[:, 0]))
rotmat[:, 2] = np.cross(rotmat[:, 0], rotmat[:, 1])
self.int_pts[i].rot[:, :, g] = math.rotmat_to_quat(rotmat)
# --------------------------------------------------------------
# interpolate velocity, acceleration, load
self.int_pts[0].w[2] = angular_velocities @ self.int_pts[0].N_rotation
self.int_pts[0].a[
2] = angular_accelerations @ self.int_pts[0].N_rotation
self.int_pts[1].om[2] = np.zeros(shape=(3, self.int_pts[1].n_pts))
self.int_pts[1].q[2] = np.tile(distributed_load,
reps=(self.int_pts[1].n_pts, 1)).T
# --------------------------------------------------------------
# element properties
self.prop = struct.BeamElementProperties(
length=L,
area=area,
density=density,
elastic_modulus=elastic_modulus,
shear_modulus=shear_modulus,
inertia_primary=inertia_primary,
inertia_secondary=inertia_secondary,
inertia_torsion=inertia_torsion,
shear_coefficient=shear_coefficient,
contact_radius=contact_radius)