|
| 1 | +import xtrack as xt |
| 2 | +import xobjects as xo |
| 3 | +import numpy as np |
| 4 | +import matplotlib.pyplot as plt |
| 5 | +from cgeom.aperture import Aperture, transform_matrix |
| 6 | +from cgeom.structures import ApertureModel, ApertureType, Circle, Profile, ProfilePosition, Rectangle, TypePosition |
| 7 | + |
| 8 | + |
| 9 | +env = xt.Environment() |
| 10 | + |
| 11 | +l = 1 |
| 12 | +dx = 1 |
| 13 | +angle = np.deg2rad(30) |
| 14 | +l_straight = dx / np.sin(angle / 2) |
| 15 | +rho = 0.5 * l_straight / np.sin(angle / 2) |
| 16 | +l_curv = rho * angle |
| 17 | + |
| 18 | +drift = env.new('drift', xt.Drift, length=l) |
| 19 | +rot_plus = env.new('rot_plus', xt.Bend, length=l_curv, angle=angle, k0=0) |
| 20 | +rot_minus = env.new('rot_minus', xt.Bend, length=l_curv, angle=-angle, k0=0) |
| 21 | + |
| 22 | +line = env.new_line( |
| 23 | + name='line', |
| 24 | + components=[drift, rot_plus, drift, drift, rot_minus, drift], |
| 25 | +) |
| 26 | + |
| 27 | +sv = line.survey() |
| 28 | + |
| 29 | +circle = Circle(radius=2) |
| 30 | +rectangle = Rectangle(half_width=2, half_height=0.5) |
| 31 | + |
| 32 | +profiles = [ |
| 33 | + Profile(shape=circle, tol_r=0, tol_x=0, tol_y=0), |
| 34 | + Profile(shape=rectangle, tol_r=0, tol_x=0, tol_y=0), |
| 35 | +] |
| 36 | + |
| 37 | +profile_positions = [ |
| 38 | + ProfilePosition(profile_index=0, s_position=s) |
| 39 | + for s in [0, 11] |
| 40 | +] |
| 41 | + |
| 42 | +types = [ |
| 43 | + ApertureType(curvature=0., positions=profile_positions), |
| 44 | +] |
| 45 | + |
| 46 | +type_positions = [ |
| 47 | + TypePosition( |
| 48 | + type_index=0, |
| 49 | + survey_reference_name='drift::0', |
| 50 | + survey_index=sv.name.tolist().index('drift::0'), |
| 51 | + transformation=transform_matrix(dx=-1.5), |
| 52 | + ), |
| 53 | +] |
| 54 | + |
| 55 | +model = ApertureModel( |
| 56 | + line_name='line', |
| 57 | + type_positions=type_positions, |
| 58 | + types=types, |
| 59 | + profiles=profiles, |
| 60 | + type_names=['type0'], |
| 61 | + profile_names=['circle', 'rectangle'], |
| 62 | +) |
| 63 | + |
| 64 | +ax = plt.figure().add_subplot(projection='3d') |
| 65 | +ax.plot(sv.Z, sv.X, sv.Y, c='b') |
| 66 | +ax.set_xlabel('Z [m]') |
| 67 | +ax.set_ylabel('X [m]') |
| 68 | +ax.set_zlabel('Y [m]') |
| 69 | + |
| 70 | +ax.auto_scale_xyz([0, 12], [-6, 6], [-6, 6]) |
| 71 | + |
| 72 | +aper = Aperture(env, model, cross_sections=None) |
| 73 | + |
| 74 | + |
| 75 | +def matrix_from_survey_point(sv_row): |
| 76 | + matrix = np.identity(4) |
| 77 | + matrix[:3, 0] = sv_row.ex |
| 78 | + matrix[:3, 1] = sv_row.ey |
| 79 | + matrix[:3, 2] = sv_row.ez |
| 80 | + matrix[:3, 3] = np.hstack([sv_row.X, sv_row.Y, sv_row.Z]) |
| 81 | + return matrix |
| 82 | + |
| 83 | + |
| 84 | +def poly2d_to_hom(poly2d): |
| 85 | + num_points = poly2d.shape[0] |
| 86 | + poly_hom = np.column_stack((poly2d, np.zeros(num_points), np.ones(num_points))).T |
| 87 | + return poly_hom |
| 88 | + |
| 89 | + |
| 90 | +for type_pos in aper.model.type_positions: |
| 91 | + aper_type = aper.model.type_for_position(type_pos) |
| 92 | + sv_ref = sv.rows[type_pos.survey_index] |
| 93 | + |
| 94 | + sv_ref_matrix = matrix_from_survey_point(sv_ref) |
| 95 | + type_matrix = type_pos.transformation.to_nparray() |
| 96 | + |
| 97 | + for profile_pos in aper_type.positions: |
| 98 | + profile = aper.model.profile_for_position(profile_pos) |
| 99 | + |
| 100 | + num_points = 100 |
| 101 | + poly = aper.polygon_for_profile(profile, num_points) |
| 102 | + poly_hom = poly2d_to_hom(poly) |
| 103 | + |
| 104 | + profile_position_matrix = transform_matrix( |
| 105 | + dx=profile_pos.shift_x, |
| 106 | + dy=profile_pos.shift_y, |
| 107 | + ds=profile_pos.s_position, |
| 108 | + theta=profile_pos.rot_y, |
| 109 | + phi=profile_pos.rot_x, |
| 110 | + psi=profile_pos.rot_z, |
| 111 | + ) |
| 112 | + |
| 113 | + poly_in_sv_frame = sv_ref_matrix @ type_matrix @ profile_position_matrix @ poly_hom |
| 114 | + |
| 115 | + xs, ys, zs = poly_in_sv_frame[:3] |
| 116 | + ax.plot(zs, xs, ys, c='r') |
| 117 | + |
| 118 | + |
| 119 | +def tangents_at_s(line, s_positions): |
| 120 | + """Return a local coordinate system (each represented by a homogeneous matrix) at all ``s_positions``.""" |
| 121 | + tangents = np.zeros(shape=(len(s_positions), 4, 4), dtype=np.float32) |
| 122 | + line_sliced = line.copy() |
| 123 | + line_sliced.cut_at_s(s_positions) |
| 124 | + survey_sliced = line_sliced.survey() |
| 125 | + sv_indices = np.searchsorted(survey_sliced.s, s_positions) |
| 126 | + |
| 127 | + for idx, sv_idx in enumerate(sv_indices): |
| 128 | + row = survey_sliced.rows[sv_idx] |
| 129 | + tangents[idx, :3, 0] = row.ex |
| 130 | + tangents[idx, :3, 1] = row.ey |
| 131 | + tangents[idx, :3, 2] = row.ez |
| 132 | + tangents[idx, :, 3] = np.hstack([row.X, row.Y, row.Z, 1]) |
| 133 | + |
| 134 | + return tangents |
| 135 | + |
| 136 | + |
| 137 | +s_for_cuts = np.linspace(1, 11, 20) |
| 138 | +profiles, tangents = aper.profiles_at_s('line', s_for_cuts) |
| 139 | +tangents2 = tangents_at_s(line, s_for_cuts) |
| 140 | + |
| 141 | +xo.assert_allclose(tangents, tangents2, atol=1e-6, rtol=1e-6) |
| 142 | + |
| 143 | +for idx, s in enumerate(s_for_cuts): |
| 144 | + profile = profiles[idx] |
| 145 | + profile_hom = poly2d_to_hom(profile) |
| 146 | + profile_in_sv_frame = tangents[idx] @ profile_hom |
| 147 | + profile_in_sv_frame2 = tangents2[idx] @ profile_hom |
| 148 | + |
| 149 | + xs, ys, zs = profile_in_sv_frame[:3] |
| 150 | + ax.plot(zs, xs, ys, c='g') |
| 151 | + |
| 152 | +plt.show() |
0 commit comments