402 lines
11 KiB
Python
402 lines
11 KiB
Python
|
import math
|
|||
|
|
|
|||
|
|
|
|||
|
|
# 定义点
|
|||
|
|
class MyPoint:
|
|||
|
|
def __init__(self, x, y):
|
|||
|
|
self.X = x
|
|||
|
|
self.Y = y
|
|||
|
|
|
|||
|
|
@classmethod
|
|||
|
|
def makeOne(cls, pt):
|
|||
|
|
return MyPoint(pt.lng, pt.lat)
|
|||
|
|
|
|||
|
|
def get_Manhattan_dist(self, other):
|
|||
|
|
return math.fabs(self.X - other.X) + math.fabs(self.Y - other.Y)
|
|||
|
|
|
|||
|
|
def get_dist(self, other):
|
|||
|
|
return math.sqrt((self.X - other.X)*(self.X - other.X) + (self.Y - other.Y)*(self.Y - other.Y))
|
|||
|
|
|
|||
|
|
|
|||
|
|
def calc_left_pt(self, target_pt: 'MyPoint', left_dist: float):
|
|||
|
|
"""计算中间插值点"""
|
|||
|
|
dist = self.get_dist(target_pt)
|
|||
|
|
mid_pt = MyPoint(0, 0)
|
|||
|
|
mid_pt.X = self.X + left_dist/dist*(target_pt.X - self.X)
|
|||
|
|
mid_pt.Y = self.Y + left_dist / dist * (target_pt.Y - self.Y)
|
|||
|
|
return mid_pt
|
|||
|
|
|
|||
|
|
|
|||
|
|
# 定义一系列全局变量
|
|||
|
|
PI = 3.1415926535897932384626433832795
|
|||
|
|
PI_120D = 2.0943951023931954923084289221863
|
|||
|
|
PI_90D = 1.5707963267948966192313216916398
|
|||
|
|
PI_80D = 1.3962634015954636615389526147909
|
|||
|
|
PI_30D = 0.52333333333
|
|||
|
|
|
|||
|
|
|
|||
|
|
g_srcDir_to_angle_map = {'N': 180, 'NE': 225, 'E': 270, 'SE': 315, 'S': 0, 'SW': 45, 'W': 90, 'NW':135}
|
|||
|
|
|
|||
|
|
class GeoFunc:
|
|||
|
|
|
|||
|
|
@classmethod
|
|||
|
|
def calc_vec_radian(cls, pA: MyPoint, pB: MyPoint):
|
|||
|
|
dx = pB.X - pA.X
|
|||
|
|
dy = pB.Y - pA.Y
|
|||
|
|
if dx == 0:
|
|||
|
|
if dy > 0:
|
|||
|
|
return PI_90D
|
|||
|
|
elif dy < 0:
|
|||
|
|
return -1 * PI_90D
|
|||
|
|
else:
|
|||
|
|
# 两点重合，暂未处理这种异常情况
|
|||
|
|
return 0
|
|||
|
|
|
|||
|
|
if dy == 0:
|
|||
|
|
if dx > 0:
|
|||
|
|
return 0
|
|||
|
|
|
|||
|
|
elif dx < 0:
|
|||
|
|
return PI
|
|||
|
|
|
|||
|
|
else:
|
|||
|
|
# 两点重合，暂未处理这种异常情况
|
|||
|
|
return 0
|
|||
|
|
|
|||
|
|
dK = dy / dx
|
|||
|
|
dA = math.atan(dK)
|
|||
|
|
# 如果在第二项限
|
|||
|
|
if (pB.Y > pA.Y and dA < 0):
|
|||
|
|
dA += PI
|
|||
|
|
|
|||
|
|
# 如果在第三项限
|
|||
|
|
if (pB.Y < pA.Y and dA > 0):
|
|||
|
|
dA -= PI
|
|||
|
|
|
|||
|
|
return dA
|
|||
|
|
|
|||
|
|
@classmethod
|
|||
|
|
def calc_turn_radian(cls, pA: MyPoint, pB: MyPoint, pC: MyPoint, pD: MyPoint):
|
|||
|
|
"""
|
|||
|
|
计算AB->CD转弯的弧度。右转为正值，左转为负值
|
|||
|
|
:param pA:
|
|||
|
|
:param pB:
|
|||
|
|
:param pC:
|
|||
|
|
:param pD:
|
|||
|
|
:return: [-PI, PI]
|
|||
|
|
"""
|
|||
|
|
fa = GeoFunc.calc_vec_radian(pA, pB)
|
|||
|
|
fb = GeoFunc.calc_vec_radian(pC, pD)
|
|||
|
|
fSign = fa * fb
|
|||
|
|
if fSign >= 0:
|
|||
|
|
return fa - fb
|
|||
|
|
|
|||
|
|
else:
|
|||
|
|
ft = math.fabs(fa - fb)
|
|||
|
|
if ft >= PI:
|
|||
|
|
r = 2 * PI - ft
|
|||
|
|
if fa - fb < 0:
|
|||
|
|
return r
|
|||
|
|
else:
|
|||
|
|
return -1 * r
|
|||
|
|
else:
|
|||
|
|
return fa - fb
|
|||
|
|
|
|||
|
|
@classmethod
|
|||
|
|
def radian2angle(cls, radian):
|
|||
|
|
"""将弧度转换为角度"""
|
|||
|
|
return radian * 180 / PI
|
|||
|
|
|
|||
|
|
@classmethod
|
|||
|
|
def judge_turn_type(cls, turn_angle):
|
|||
|
|
turn_type = -1
|
|||
|
|
if math.fabs(turn_angle) <= 30:
|
|||
|
|
turn_type = 0 # straight
|
|||
|
|
elif -150 < turn_angle < -30:
|
|||
|
|
turn_type = 1 # turn left
|
|||
|
|
elif 30 < turn_angle < 150:
|
|||
|
|
turn_type = 2 # turn left
|
|||
|
|
else: # means: abs(turn_angle) >= 150
|
|||
|
|
turn_type = 3 # turn back
|
|||
|
|
|
|||
|
|
return turn_type
|
|||
|
|
|
|||
|
|
@classmethod
|
|||
|
|
def calc_turn_type(cls, pA, pB, pC, pD):
|
|||
|
|
turn_radian = GeoFunc.calc_turn_radian(pA, pB, pC, pD)
|
|||
|
|
turn_angle = GeoFunc.radian2angle(turn_radian)
|
|||
|
|
return GeoFunc.judge_turn_type(turn_angle)
|
|||
|
|
|
|||
|
|
@classmethod
|
|||
|
|
def calc_turn_type_ex(cls, pA, pB, pC, pD):
|
|||
|
|
"""
|
|||
|
|
计算AB->CD的转向类型，附加偏差角度
|
|||
|
|
@param pA:
|
|||
|
|
@param pB:
|
|||
|
|
@param pC:
|
|||
|
|
@param pD:
|
|||
|
|
@return: turn_type, diff
|
|||
|
|
"""
|
|||
|
|
turn_radian = GeoFunc.calc_turn_radian(pA, pB, pC, pD)
|
|||
|
|
turn_angle = GeoFunc.radian2angle(turn_radian)
|
|||
|
|
turn_type = GeoFunc.judge_turn_type(turn_angle)
|
|||
|
|
diff = 0
|
|||
|
|
if turn_type == 0:
|
|||
|
|
diff = abs(abs(turn_angle))
|
|||
|
|
elif turn_type == 3:
|
|||
|
|
diff = abs(180 - abs(turn_angle))
|
|||
|
|
else:
|
|||
|
|
diff = abs(90 - abs(turn_angle))
|
|||
|
|
return turn_type, diff
|
|||
|
|
|
|||
|
|
|
|||
|
|
# 根据向量角度，判定来源方向
|
|||
|
|
@classmethod
|
|||
|
|
def vecAngle_to_srcDir(cls, vec_angle):
|
|||
|
|
srcDir = ''
|
|||
|
|
if math.fabs(vec_angle) <= 22.5:
|
|||
|
|
srcDir = 'W'
|
|||
|
|
elif 22.5 < vec_angle <= 67.5:
|
|||
|
|
srcDir = 'SW'
|
|||
|
|
elif 67.5 < vec_angle <= 112.5:
|
|||
|
|
srcDir = 'S'
|
|||
|
|
elif 112.5 < vec_angle <= 157.5:
|
|||
|
|
srcDir = 'SE'
|
|||
|
|
elif math.fabs(vec_angle) > 157.5:
|
|||
|
|
srcDir = 'E'
|
|||
|
|
elif -157.5 <= vec_angle <= -112.5:
|
|||
|
|
srcDir = 'NE'
|
|||
|
|
elif -112.5 < vec_angle <= -67.5:
|
|||
|
|
srcDir = 'N'
|
|||
|
|
elif -67.5 < vec_angle <= -22.5:
|
|||
|
|
srcDir = 'NW'
|
|||
|
|
|
|||
|
|
return srcDir
|
|||
|
|
|
|||
|
|
@classmethod
|
|||
|
|
def calc_srcDir(cls, pA: MyPoint, pB: MyPoint):
|
|||
|
|
vec_radian = GeoFunc.calc_vec_radian(pA, pB)
|
|||
|
|
vec_angle = GeoFunc.radian2angle(vec_radian)
|
|||
|
|
return GeoFunc.vecAngle_to_srcDir(vec_angle)
|
|||
|
|
|
|||
|
|
@classmethod
|
|||
|
|
def calc_pole_angle(cls, pA: MyPoint, pB: MyPoint):
|
|||
|
|
"""
|
|||
|
|
计算gps角度：以正北为0度，顺时针方向角度增大
|
|||
|
|
:param pA:
|
|||
|
|
:param pB:
|
|||
|
|
:return: [0, 360)
|
|||
|
|
"""
|
|||
|
|
vec_radian = GeoFunc.calc_vec_radian(pA, pB)
|
|||
|
|
vec_angle = GeoFunc.radian2angle(vec_radian)
|
|||
|
|
pole_angle = (90 - vec_angle + 360) % 360
|
|||
|
|
return pole_angle
|
|||
|
|
|
|||
|
|
@classmethod
|
|||
|
|
def diff_pole_angle(cls, a, b):
|
|||
|
|
"""
|
|||
|
|
计算两个gps方位角的差值
|
|||
|
|
:param a:
|
|||
|
|
:param b:
|
|||
|
|
:return:
|
|||
|
|
"""
|
|||
|
|
diff = abs(a -b)
|
|||
|
|
if diff > 180:
|
|||
|
|
diff = 360 - diff
|
|||
|
|
return diff
|
|||
|
|
|
|||
|
|
@classmethod
|
|||
|
|
def calc_srcDir_tail(cls, pt_list):
|
|||
|
|
"""
|
|||
|
|
计算点串末段的车流来向
|
|||
|
|
:param pt_list:
|
|||
|
|
:return:
|
|||
|
|
"""
|
|||
|
|
num = len(pt_list)
|
|||
|
|
if num < 2:
|
|||
|
|
return 'unknown'
|
|||
|
|
pB = MyPoint.makeOne(pt_list[num - 1])
|
|||
|
|
cur_pA_index = num - 2
|
|||
|
|
while True:
|
|||
|
|
pA = MyPoint.makeOne(pt_list[cur_pA_index])
|
|||
|
|
if cur_pA_index <= 0 or pA.get_Manhattan_dist(pB) >= 0.0005:
|
|||
|
|
return GeoFunc.calc_srcDir(pA, pB)
|
|||
|
|
cur_pA_index -= 1
|
|||
|
|
# 保底措施
|
|||
|
|
if cur_pA_index < 0:
|
|||
|
|
break
|
|||
|
|
# 不应该走到的分支
|
|||
|
|
return 'unknown'
|
|||
|
|
|
|||
|
|
@classmethod
|
|||
|
|
def calc_srcDir_head(cls, pt_list):
|
|||
|
|
"""
|
|||
|
|
计算点串起始段的车流来向
|
|||
|
|
:param pt_list:
|
|||
|
|
:return:
|
|||
|
|
"""
|
|||
|
|
num = len(pt_list)
|
|||
|
|
if num < 2:
|
|||
|
|
return 'unknown'
|
|||
|
|
pA = MyPoint.makeOne(pt_list[0])
|
|||
|
|
cur_pB_index = 1
|
|||
|
|
while True:
|
|||
|
|
pB = MyPoint.makeOne(pt_list[cur_pB_index])
|
|||
|
|
if cur_pB_index >= num-1 or pA.get_Manhattan_dist(pB) >= 0.0005:
|
|||
|
|
return GeoFunc.calc_srcDir(pA, pB)
|
|||
|
|
cur_pB_index += 1
|
|||
|
|
# 不应该走到的分支
|
|||
|
|
return 'unknown'
|
|||
|
|
|
|||
|
|
|
|||
|
|
@classmethod
|
|||
|
|
def calc_angle_head(cls, pt_list):
|
|||
|
|
"""
|
|||
|
|
计算点串起始段的车流来向
|
|||
|
|
:param pt_list:
|
|||
|
|
:return:
|
|||
|
|
"""
|
|||
|
|
num = len(pt_list)
|
|||
|
|
if num < 2:
|
|||
|
|
return 'unknown'
|
|||
|
|
pA = MyPoint.makeOne(pt_list[0])
|
|||
|
|
cur_pB_index = 1
|
|||
|
|
while True:
|
|||
|
|
pB = MyPoint.makeOne(pt_list[cur_pB_index])
|
|||
|
|
if cur_pB_index >= num-1 or pA.get_Manhattan_dist(pB) >= 0.0005:
|
|||
|
|
return GeoFunc.calc_pole_angle(pA, pB)
|
|||
|
|
cur_pB_index += 1
|
|||
|
|
# 不应该走到的分支
|
|||
|
|
return 'unknown'
|
|||
|
|
|
|||
|
|
@classmethod
|
|||
|
|
def flip_gps_angle(self, original_angle):
|
|||
|
|
flipped_angle = (original_angle + 180) % 360
|
|||
|
|
return flipped_angle
|
|||
|
|
|
|||
|
|
@classmethod
|
|||
|
|
def calc_angle_tail(cls, pt_list):
|
|||
|
|
"""
|
|||
|
|
计算点串末段的车流来向
|
|||
|
|
:param pt_list:
|
|||
|
|
:return:
|
|||
|
|
"""
|
|||
|
|
num = len(pt_list)
|
|||
|
|
if num < 2:
|
|||
|
|
return 'unknown'
|
|||
|
|
pB = MyPoint.makeOne(pt_list[num - 1])
|
|||
|
|
cur_pA_index = num - 2
|
|||
|
|
while True:
|
|||
|
|
pA = MyPoint.makeOne(pt_list[cur_pA_index])
|
|||
|
|
if cur_pA_index <= 0 or pA.get_Manhattan_dist(pB) >= 0.0005:
|
|||
|
|
return GeoFunc.calc_pole_angle(pA, pB)
|
|||
|
|
cur_pA_index -= 1
|
|||
|
|
# 保底措施
|
|||
|
|
if cur_pA_index < 0:
|
|||
|
|
break
|
|||
|
|
# 不应该走到的分支
|
|||
|
|
return 'unknown'
|
|||
|
|
|
|||
|
|
|
|||
|
|
@classmethod
|
|||
|
|
def diff_srcDir(cls, a: str, b: str):
|
|||
|
|
"""
|
|||
|
|
计算两个车流来向的角度差
|
|||
|
|
:param a:
|
|||
|
|
:param b:
|
|||
|
|
:return: 0/45/90/135/180
|
|||
|
|
"""
|
|||
|
|
if a == b:
|
|||
|
|
return 0
|
|||
|
|
|
|||
|
|
angle_a = g_srcDir_to_angle_map[a]
|
|||
|
|
angle_b = g_srcDir_to_angle_map[b]
|
|||
|
|
diff_angle = abs(angle_a - angle_b)
|
|||
|
|
if diff_angle > 180:
|
|||
|
|
diff_angle = 360 - diff_angle
|
|||
|
|
return diff_angle
|
|||
|
|
|
|||
|
|
@classmethod
|
|||
|
|
def pick_path_head(cls, pt_list) -> (MyPoint, MyPoint):
|
|||
|
|
"""
|
|||
|
|
从点序列中，提取满足50米长度的开头的一段
|
|||
|
|
:param pt_list: Pt的数组
|
|||
|
|
:return: pA, pB
|
|||
|
|
"""
|
|||
|
|
num = len(pt_list)
|
|||
|
|
if num < 2:
|
|||
|
|
return None, None
|
|||
|
|
if num == 2:
|
|||
|
|
return MyPoint.makeOne(pt_list[0]), MyPoint.makeOne(pt_list[1])
|
|||
|
|
pA = MyPoint.makeOne(pt_list[0])
|
|||
|
|
cur_pB_index = 1
|
|||
|
|
while True:
|
|||
|
|
pB = MyPoint.makeOne(pt_list[cur_pB_index])
|
|||
|
|
if cur_pB_index >= num-1 or pA.get_dist(pB) >= 0.0005:
|
|||
|
|
return pA, pB
|
|||
|
|
cur_pB_index += 1
|
|||
|
|
# 保底措施
|
|||
|
|
if cur_pB_index > num-1:
|
|||
|
|
break
|
|||
|
|
# 不应该走到的分支
|
|||
|
|
return None, None
|
|||
|
|
|
|||
|
|
@classmethod
|
|||
|
|
def pick_path_tail(cls, pt_list) -> (MyPoint, MyPoint):
|
|||
|
|
"""
|
|||
|
|
从点序列中，提取满足50米长度的末尾的一段
|
|||
|
|
:param pt_list: Pt的数组
|
|||
|
|
:return: pA, pB
|
|||
|
|
"""
|
|||
|
|
num = len(pt_list)
|
|||
|
|
if num < 2:
|
|||
|
|
return None, None
|
|||
|
|
if num == 2:
|
|||
|
|
return MyPoint.makeOne(pt_list[0]), MyPoint.makeOne(pt_list[1])
|
|||
|
|
pB = MyPoint.makeOne(pt_list[num - 1])
|
|||
|
|
cur_pA_index = num - 2
|
|||
|
|
while True:
|
|||
|
|
pA = MyPoint.makeOne(pt_list[cur_pA_index])
|
|||
|
|
if cur_pA_index <= 0 or pA.get_dist(pB) >= 0.0005:
|
|||
|
|
return pA, pB
|
|||
|
|
cur_pA_index -= 1
|
|||
|
|
# 保底措施
|
|||
|
|
if cur_pA_index < 0:
|
|||
|
|
break
|
|||
|
|
# 不应该走到的分支
|
|||
|
|
return None, None
|
|||
|
|
|
|||
|
|
|
|||
|
|
@classmethod
|
|||
|
|
def calc_earth_dist(cls, x1, y1, x2, y2):
|
|||
|
|
"""
|
|||
|
|
计算两个经纬度坐标之间的球面距离
|
|||
|
|
"""
|
|||
|
|
_PI_ = 3.14159265358979323846264
|
|||
|
|
EARTHS_RADIUS = 6371008.8
|
|||
|
|
RAD_TO_DEG = 180.0 / _PI_
|
|||
|
|
DEG_TO_RAD = _PI_ / 180.0
|
|||
|
|
radLat1 = y1 * DEG_TO_RAD
|
|||
|
|
radLat2 = y2 * DEG_TO_RAD
|
|||
|
|
a = radLat1 - radLat2
|
|||
|
|
b = (x1 - x2) * DEG_TO_RAD
|
|||
|
|
s = 2 * math.asin(math.sqrt(pow(math.sin(a / 2), 2) + math.cos(radLat1) * math.cos(radLat2) * pow(math.sin(b / 2), 2)))
|
|||
|
|
return s * EARTHS_RADIUS
|
|||
|
|
|
|||
|
|
|
|||
|
|
if __name__ == '__main__':
|
|||
|
|
# for a in g_srcDir_to_angle_map.keys():
|
|||
|
|
# for b in g_srcDir_to_angle_map.keys():
|
|||
|
|
# diff_angle = GeoFunc.diff_srcDir(a, b)
|
|||
|
|
# print('%s %s => %d' % (a, b, diff_angle))
|
|||
|
|
geo = '98.327637,27.994401,118.366699,41.508577'
|
|||
|
|
ss = geo.split(',')
|
|||
|
|
x1 = float(ss[0])
|
|||
|
|
y1 = float(ss[1])
|
|||
|
|
x2 = float(ss[2])
|
|||
|
|
y2 = float(ss[3])
|
|||
|
|
dist = GeoFunc.calc_earth_dist(x1, y1, x2, y2)
|
|||
|
|
print(dist)
|
|||
|
|
|
|||
|
|
|