交点法公路计算的一些相关方法函数(Python)

 1     # 弧度归一化
 2     @staticmethod
 3     def normalize_radian(radian):
 4         return radian % tau
 5 
 6     # 获取坐标差
 7     @staticmethod
 8     def get_coord_diff(start_x, start_y, end_x, end_y):
 9         return end_x - start_x, end_y - start_y
10 
11     # 切线垂距
12     @staticmethod
13     def get_tangent_distance(origin_x, origin_y, origin_azimuth, current_x, current_y):
14         delta_x, delta_y = get_coord_diff(origin_x, origin_y, current_x, current_y)
15         return delta_y * cos(origin_azimuth) - delta_x * sin(origin_azimuth)
16 
17     # 法线垂距
18     @staticmethod
19     def get_normal_distance(origin_x, origin_y, origin_azimuth, current_x, current_y):
20         # 法线方位角
21         normal_azimuth = normalize_radian(origin_azimuth - HALF_PI)
22         return MathUtils.get_tangent_distance(origin_x, origin_y, normal_azimuth, current_x, current_y)
23 
24     # 获取圆弧內移值
25     @staticmethod
26     def get_arc_inside_shift(delta_y, radius, turn_angle):
27         return delta_y - radius * (1.0 - cos(turn_angle))
28 
29     # 获取切线增长值
30     @staticmethod
31     def get_tangent_growth(delta_x, radius, turn_angle):
32         return delta_x - radius * sin(turn_angle)
33     # 获取交点切线长
34     @staticmethod
35     def get_intersection_tangent_length(origin_radius, tangent_growth, half_turn_angle):
36         return tangent_growth + origin_radius * tan(half_turn_angle)
37 
38     # 获取交点切线方位角
39     @staticmethod
40     def get_intersection_tangent_azimuth(previous_x, previous_y, current_x, current_y, latter_x, latter_y):
41         entry_dx, entry_dy = get_coord_diff(previous_x, previous_y, current_x, current_y)
42         exit_dx, exit_dy = get_coord_diff(current_x, current_y, latter_x, latter_y)
43         # 计算方位角
44         entry_azimuth = atan2(entry_dy, entry_dx)
45         exit_azimuth = atan2(exit_dy, exit_dx)
46         # 计算点积和叉积
47         dot_product = entry_dx * exit_dx + entry_dy * exit_dy
48         cross_product = entry_dx * exit_dy - entry_dy * exit_dx
49         # 计算夹角
50         turn_angle = atan2(cross_product, dot_product)
51         return entry_azimuth, exit_azimuth, turn_angle
52 
53     # 获取交点半幅夹角
54     @staticmethod
55     def get_intersection_half_angle(start_origin_radius, end_origin_radius, turn_angle):
56         return atan((end_origin_radius - start_origin_radius * cos(turn_angle)) / (start_origin_radius * sin(turn_angle)))