def rotate_around_axis(self, axis, angle):
transform_matrix = Quaternion.from_axis_angle(
(axis[0], axis[1], axis[2]), angle).to_rotation_matrix(
(self.center[0], self.center[1], self.center[2]))
for i in range(8):
p = self.points[i]
spp = sp.Point3D(p[0], p[1], p[2])
spp = spp.transform(transform_matrix)
self.points[i] = [spp.x.evalf(), spp.y.evalf(), spp.z.evalf()]
def test_quaternion_construction():
q = Quaternion(x, y, z, w)
assert q + q == Quaternion(2*x, 2*y, 2*z, 2*w)
q2 = Quaternion.from_axis_angle((sqrt(3)/3, sqrt(3)/3, sqrt(3)/3), 2*pi/3)
assert q2 == Quaternion(Rational(1/2), Rational(1/2),
Rational(1/2), Rational(1,2))
M = Matrix([[cos(x), -sin(x), 0], [sin(x), cos(x), 0], [0, 0, 1]])
q3 = trigsimp(Quaternion.from_rotation_matrix(M))
assert q3 == Quaternion(sqrt(2)*sqrt(cos(x) + 1)/2, 0, 0, sqrt(-2*cos(x) + 2)/2)
def test_quaternion_construction():
q = Quaternion(x, y, z, w)
assert q + q == Quaternion(2*x, 2*y, 2*z, 2*w)
q2 = Quaternion.from_axis_angle((sqrt(3)/3, sqrt(3)/3, sqrt(3)/3), 2*pi/3)
assert q2 == Quaternion(Rational(1, 2), Rational(1, 2),
Rational(1, 2), Rational(1, 2))
M = Matrix([[cos(x), -sin(x), 0], [sin(x), cos(x), 0], [0, 0, 1]])
q3 = trigsimp(Quaternion.from_rotation_matrix(M))
assert q3 == Quaternion(sqrt(2)*sqrt(cos(x) + 1)/2, 0, 0, sqrt(-2*cos(x) + 2)/2)
nc = Symbol('nc', commutative=False)
raises(ValueError, lambda: Quaternion(x, y, nc, w))
def test_quaternion_construction():
q = Quaternion(w, x, y, z)
assert q + q == Quaternion(2*w, 2*x, 2*y, 2*z)
q2 = Quaternion.from_axis_angle((sqrt(3)/3, sqrt(3)/3, sqrt(3)/3),
pi*Rational(2, 3))
assert q2 == Quaternion(S.Half, S.Half,
S.Half, S.Half)
M = Matrix([[cos(phi), -sin(phi), 0], [sin(phi), cos(phi), 0], [0, 0, 1]])
q3 = trigsimp(Quaternion.from_rotation_matrix(M))
assert q3 == Quaternion(sqrt(2)*sqrt(cos(phi) + 1)/2, 0, 0, sqrt(2 - 2*cos(phi))*sign(sin(phi))/2)
nc = Symbol('nc', commutative=False)
raises(ValueError, lambda: Quaternion(w, x, nc, z))
def test_quaternion_axis_angle():
test_data = [ # axis, angle, expected_quaternion
((1, 0, 0), 0, (1, 0, 0, 0)),
((1, 0, 0), pi/2, (sqrt(2)/2, sqrt(2)/2, 0, 0)),
((0, 1, 0), pi/2, (sqrt(2)/2, 0, sqrt(2)/2, 0)),
((0, 0, 1), pi/2, (sqrt(2)/2, 0, 0, sqrt(2)/2)),
((1, 0, 0), pi, (0, 1, 0, 0)),
((0, 1, 0), pi, (0, 0, 1, 0)),
((0, 0, 1), pi, (0, 0, 0, 1)),
((1, 1, 1), pi, (0, 1/sqrt(3),1/sqrt(3),1/sqrt(3))),
((sqrt(3)/3, sqrt(3)/3, sqrt(3)/3), pi*2/3, (S.Half, S.Half, S.Half, S.Half))
]
for axis, angle, expected in test_data:
assert Quaternion.from_axis_angle(axis, angle) == Quaternion(*expected)
def rotate_face(self, fid):
connected_edges = [
self.get_connected_edge(self.faces[fid][i], fid) for i in range(4)
]
# rotate face plane around random axis with origin at face centre
plane = self.face2plane(fid)
origin = self.face_centre(fid)
axis = plane.random_point() - plane.random_point()
transform_matrix = Quaternion.from_axis_angle((axis[0], axis[1], axis[2]),
random.uniform(-self.max_angle_offset, self.max_angle_offset)
)\
.to_rotation_matrix((origin[0], origin[1], origin[2]))
norm = sp.Point3D(plane.normal_vector)
plane = sp.Plane(origin,
normal_vector=norm.transform(transform_matrix))
# find new points for face
new_face_points = []
for ce in connected_edges:
line = sp.Line3D(sp.Point3D(self.points[ce[0]]),
sp.Point3D(self.points[ce[1]]))
new_point = plane.intersection(line)[0]
new_point = [
new_point.x.evalf(),
new_point.y.evalf(),
new_point.z.evalf()
]
new_face_points.append(new_point)
if not self.is_b_not_far_than_a(self.points[ce[0]],
self.points[ce[1]], new_point):
print("skip face rotating - hex will not be valid")
return
# replace old points with new
for i in range(4):
pid = self.faces[fid][i]
self.points[pid] = new_face_points[i]
def test_quaternion_axis_angle_simplification():
result = Quaternion.from_axis_angle((1, 2, 3), asin(4))
assert result.a == cos(asin(4)/2)
assert result.b == sqrt(14)*sin(asin(4)/2)/14
assert result.c == sqrt(14)*sin(asin(4)/2)/7
assert result.d == 3*sqrt(14)*sin(asin(4)/2)/14
class WBCellSymb4Param(bu.SymbExpr):
a, b, c = sp.symbols('a, b, c', positive=True)
u_2, u_3 = sp.symbols('u_2, u_3', positive=True)
gamma = sp.symbols('gamma', positive=True)
U0_a = sp.Matrix([a, b, 0])
W0_a = sp.Matrix([c, 0, 0])
UW0_a = W0_a - U0_a
L2_U_0 = (U0_a.T * U0_a)[0]
L2_UW_0 = (UW0_a.T * UW0_a)[0]
U1_a = sp.Matrix([a, u_2, u_3])
W1_a = sp.Matrix([c * sp.sin(gamma), 0, c * sp.cos(gamma)])
UW1_a = U1_a - W1_a
L2_U_1 = (U1_a.T * U1_a)[0]
L2_UW_1 = (UW1_a.T * UW1_a)[0]
u2_sol = sp.solve(L2_U_1 - L2_U_0, u_2)[0]
u3_sol = sp.solve((L2_UW_1 - L2_UW_0).subs(u_2, u2_sol), u_3)[0]
u_3_ = u3_sol
u_2_ = u2_sol.subs(u_3, u3_sol)
U_pp_a = U1_a.subs({u_2: u_2_, u_3: u_3_})
U_mm_a = sp.Matrix([-U_pp_a[0], -U_pp_a[1], U_pp_a[2]])
U_mp_a = sp.Matrix([-U_pp_a[0], U_pp_a[1], U_pp_a[2]])
U_pm_a = sp.Matrix([U_pp_a[0], -U_pp_a[1], U_pp_a[2]])
W_p_a = W1_a.subs({u_2: u_2_, u_3: u_3_})
W_m_a = sp.Matrix([-W_p_a[0], W_p_a[1], W_p_a[2]])
V_UW = U_pp_a - W_p_a
L_UW = sp.sqrt(V_UW[1] ** 2 + V_UW[2] ** 2)
theta_sol = sp.simplify(2 * sp.asin(V_UW[2] / L_UW))
theta = sp.Symbol(r'theta')
q_theta = Quaternion.from_axis_angle([1, 0, 0], theta)
d_1, d_2, d_3 = sp.symbols('d_1, d_2, d_3')
D_a = sp.Matrix([d_1, d_2, d_3])
UD_pp_a = U_pp_a + D_a
WD_p_a = W_p_a + D_a
d_subs = sp.solve(UD_pp_a - W_m_a, [d_1, d_2, d_3])
# center of rotation
UD_pp_a_ = UD_pp_a.subs(d_subs)
# rotated point
WD_p_a_ = WD_p_a.subs(d_subs)
# pull back
WD_p_a_pb = WD_p_a_ - UD_pp_a_
# rotate using quaternion
WD_p_a_rot = q_theta.rotate_point(WD_p_a_pb.T, q_theta)
# push forward
WD_p_a_pf = sp.Matrix(WD_p_a_rot) + UD_pp_a_
# rotated compatibility point
WD_p_a_theta = WD_p_a_pf.subs(theta, -theta_sol)
# rotate the center of the neighbour cell
DD_a_pb = D_a.subs(d_subs) - UD_pp_a_
DD_a_rot = q_theta.rotate_point(DD_a_pb.T, q_theta)
DD_a_pf = sp.simplify(sp.Matrix(DD_a_rot) + UD_pp_a_)
DD_a_theta = DD_a_pf.subs(theta, -theta_sol)
H = W_p_a[2]
rho = (U_mm_a[2] - DD_a_theta[2]) / (U_mm_a[1] - DD_a_theta[1]) * U_mm_a[1]
R_0 = U_mm_a[2] - rho
delta_phi = sp.asin(DD_a_theta[1] / R_0)
delta_x = a + W_p_a[0]
# theta = sp.symbols('theta')
# x_1, x_2, x_3 = sp.symbols('x_1, x_2, x_3')
#
# q_theta = Quaternion.from_axis_angle([1, 0, 0], theta)
# X_rot = q_theta.rotate_point((x_1, x_2, x_3), q_theta)
# X_theta_a = sp.simplify(sp.Matrix(X_rot))
symb_model_params = ['gamma', 'a', 'b', 'c', ]
symb_expressions = [
('u_2_', ()),
('u_3_', ()),
('R_0', ()),
('delta_phi', ()),
('delta_x', ()),
('H', ()),
('theta_sol', ())
]
