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)
|
||
|
||
|