128 {
129
130 assert(rotation <= 36000);
131 double x = 0.0;
132 double y = 0.0;
133 double z = 0.0;
134
135 double distance = raw_distance + corrections.dist_correction;
136
137
138
139 double cos_rot_angle =
142 double sin_rot_angle =
145
146
147
148
149 double xy_distance = distance * corrections.cos_vert_correction;
150
151
152 double xx = fabs(xy_distance * sin_rot_angle -
153 corrections.horiz_offset_correction * cos_rot_angle);
154
155 double yy = fabs(xy_distance * cos_rot_angle +
156 corrections.horiz_offset_correction * sin_rot_angle);
157
158
159
160
161 double distance_corr_x = 0;
162 double distance_corr_y = 0;
163
165 distance_corr_x =
166 (corrections.dist_correction - corrections.dist_correction_x) *
167 (xx - 2.4) / 22.64 +
168 corrections.dist_correction_x;
169 distance_corr_y =
170 (corrections.dist_correction - corrections.dist_correction_y) *
171 (yy - 1.93) / 23.11 +
172 corrections.dist_correction_y;
173 } else {
174 distance_corr_x = distance_corr_y = corrections.dist_correction;
175 }
176
177 double distance_x = raw_distance + distance_corr_x;
178
179 xy_distance = distance_x * corrections.cos_vert_correction;
180
181
182
183 x = xy_distance * sin_rot_angle -
184 corrections.horiz_offset_correction * cos_rot_angle;
185
186 double distance_y = raw_distance + distance_corr_y;
187 xy_distance = distance_y * corrections.cos_vert_correction;
188
189
190 y = xy_distance * cos_rot_angle +
191 corrections.horiz_offset_correction * sin_rot_angle;
192
193 z = distance * corrections.sin_vert_correction +
194 corrections.vert_offset_correction;
195
196
198 point->set_x(static_cast<float>(y));
199 point->set_y(static_cast<float>(-x));
200 point->set_z(static_cast<float>(z));
201}
float cos_rot_table_[ROTATION_MAX_UNITS]
bool need_two_pt_correction_
float sin_rot_table_[ROTATION_MAX_UNITS]