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', ()) ]