|
|
|
|
|
#!/usr/bin/python
|
|
|
|
|
|
# -*- coding: UTF-8 -*-
|
|
|
|
|
|
|
|
|
|
|
|
from math import *
|
|
|
|
|
|
import queue
|
|
|
|
|
|
import sys
|
|
|
|
|
|
import time
|
|
|
|
|
|
import os
|
|
|
|
|
|
from datetime import datetime
|
|
|
|
|
|
import pymssql
|
|
|
|
|
|
import pandas as pd
|
|
|
|
|
|
import numpy as np
|
|
|
|
|
|
import datetime
|
|
|
|
|
|
import math
|
|
|
|
|
|
sys.path.append(r'C:\Users\Administrator\Desktop')
|
|
|
|
|
|
from CarRecord import *
|
|
|
|
|
|
from Arc import *
|
|
|
|
|
|
from ArcMany import *
|
|
|
|
|
|
from CarPath import *
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
arc_objects=[]
|
|
|
|
|
|
map_graph=[]
|
|
|
|
|
|
car_id_count=[]
|
|
|
|
|
|
#通过经纬度计算两点之间的距离
|
|
|
|
|
|
def DistanceBetween(geo1, geo2):
|
|
|
|
|
|
return pow(pow(float(geo1[0]) - float(geo2[0]), 2) + pow(float(geo1[1]) - float(geo2[1]), 2), 0.5)
|
|
|
|
|
|
|
|
|
|
|
|
# 知道三个顶点坐标ABC，看角ACB是否是锐角三角形
|
|
|
|
|
|
def CheckOxygon(A, C, B):
|
|
|
|
|
|
a = DistanceBetween(C, B)
|
|
|
|
|
|
b = DistanceBetween(A, C)
|
|
|
|
|
|
c = DistanceBetween(A, B)
|
|
|
|
|
|
if a ** 2 + b ** 2 < c ** 2:
|
|
|
|
|
|
return False
|
|
|
|
|
|
else:
|
|
|
|
|
|
return True
|
|
|
|
|
|
|
|
|
|
|
|
# 计算起点到终点线段方向与x轴的夹角 逆时针
|
|
|
|
|
|
# geo_list 第一个参数-1终点 0起点 第二个参数 0--y 1--x
|
|
|
|
|
|
def CalDirection(geo_list):
|
|
|
|
|
|
if geo_list[-1][0] == geo_list[0][0]:
|
|
|
|
|
|
if geo_list[-1][1] > geo_list[0][1]:
|
|
|
|
|
|
return float(0)
|
|
|
|
|
|
else:
|
|
|
|
|
|
return float(180)
|
|
|
|
|
|
else:
|
|
|
|
|
|
# tan=slope
|
|
|
|
|
|
# arctan(tan x)=x
|
|
|
|
|
|
slope = (geo_list[-1][1] - geo_list[0][1]) / (geo_list[-1][0] - geo_list[0][0])
|
|
|
|
|
|
if geo_list[-1][0] > geo_list[0][0]: # 如果射线在第1,2象限
|
|
|
|
|
|
return 90 - (atan(slope) / pi * 180) # 由于角度是跟x轴的夹角，所以需要90-
|
|
|
|
|
|
else: # 如果射线在第3,4象限
|
|
|
|
|
|
return 90 - (atan(slope) / pi * 180) + 180
|
|
|
|
|
|
|
|
|
|
|
|
# 传入geo1, geo2为直线y = kx + d上两点，求geo3在直线上的射影（即作垂直后与直线的交点）
|
|
|
|
|
|
# 如果形成钝角三角形，则垂点取geo1，geo2中靠近geo3的那个 就是直接过去
|
|
|
|
|
|
def CalProjection(geo1, geo2, geo3):
|
|
|
|
|
|
geo1 = [float(ele) for ele in geo1]
|
|
|
|
|
|
geo2 = [float(ele) for ele in geo2]
|
|
|
|
|
|
geo3 = [float(ele) for ele in geo3]
|
|
|
|
|
|
a = DistanceBetween(geo2, geo3)
|
|
|
|
|
|
b = DistanceBetween(geo1, geo3)
|
|
|
|
|
|
c = DistanceBetween(geo1, geo2)
|
|
|
|
|
|
if (a**2 + c**2) <= b**2: #钝角三角形，且geo3靠近geo2，包括点在线段延长线上的情况
|
|
|
|
|
|
return geo2
|
|
|
|
|
|
elif b**2 + c**2 <= a**2: #钝角三角形，且geo3靠近geo1，包括点在线段延长线上的情况
|
|
|
|
|
|
return geo1
|
|
|
|
|
|
elif a + b == c: # 说明点在线段上
|
|
|
|
|
|
return geo3
|
|
|
|
|
|
else:
|
|
|
|
|
|
if geo1[0] == geo2[0]: # 如果直线竖直（0---x;1---y)
|
|
|
|
|
|
return [geo1[0], geo3[1]]
|
|
|
|
|
|
elif geo1[1] == geo2[1]: # 如果直线水平
|
|
|
|
|
|
return [geo3[0], geo1[1]]
|
|
|
|
|
|
else:
|
|
|
|
|
|
k = (geo1[1] - geo2[1]) / (geo1[0] - geo2[0]) # y = kx+d中的k
|
|
|
|
|
|
d = geo1[1] - k * geo1[0] # y = kx+d中的d
|
|
|
|
|
|
x4 = (k * geo3[1] - k * d + geo3[0]) / (1 + k**2) #先求交点经度
|
|
|
|
|
|
y4 = k * x4 + d #再求交点纬度
|
|
|
|
|
|
return [x4, y4]
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#逐个计算路段的点、与路段延长点、出发点的的夹角
|
|
|
|
|
|
def CalProjectionOfArc(arc, geo):
|
|
|
|
|
|
length = len(arc.geometry_list)
|
|
|
|
|
|
# 先找一个虚拟点，其为点arc.geometry_list[-2]到arc.geometry_list[-1]连线延长线上延长相等距离的点virtual_p
|
|
|
|
|
|
virtual_p = [(2 * arc.geometry_list[-1][i] - arc.geometry_list[-2][i]) for i in range(2)]
|
|
|
|
|
|
# 找从哪个i开始，三角形ACB变成钝角，其中点A为car_record.geo（出发点）,点C为arc.geometry_list[i]（路段的点），点B为virtual_p（路段延长点）
|
|
|
|
|
|
i = 0
|
|
|
|
|
|
while i < length - 1 and CheckOxygon(geo, arc.geometry_list[i], virtual_p): # 判断是否是锐角
|
|
|
|
|
|
i += 1
|
|
|
|
|
|
# 如果最小的i为0或者len() - 1，则取最靠近目标点的两个点计算垂点
|
|
|
|
|
|
# 即使第一个点就出现钝角三角形的情况，calProjection函数中会处理
|
|
|
|
|
|
if i == 0: # 如果垂点不在线段上
|
|
|
|
|
|
projection = CalProjection(arc.geometry_list[i], arc.geometry_list[i + 1], geo)
|
|
|
|
|
|
else: # 如果垂点在线段上
|
|
|
|
|
|
projection = CalProjection(arc.geometry_list[i - 1], arc.geometry_list[i], geo)
|
|
|
|
|
|
return projection
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
# 传入geo1, geo2为直线y = kx + d上两点，求geo3到直线的距离
|
|
|
|
|
|
# 如果形成钝角三角形，则垂点取geo1，geo2中靠近geo3的那个点与geo3的距离
|
|
|
|
|
|
def CalDistance(geo1, geo2, geo3):
|
|
|
|
|
|
geo1 = [float(ele) for ele in geo1]
|
|
|
|
|
|
geo2 = [float(ele) for ele in geo2]
|
|
|
|
|
|
geo3 = [float(ele) for ele in geo3]
|
|
|
|
|
|
a = DistanceBetween(geo2, geo3)
|
|
|
|
|
|
b = DistanceBetween(geo1, geo3)
|
|
|
|
|
|
c = DistanceBetween(geo1, geo2)
|
|
|
|
|
|
if (a ** 2 + c ** 2) <= b ** 2: # 钝角三角形，且geo3靠近geo2，包括点在线段延长线上的情况
|
|
|
|
|
|
return DistanceBetween(geo2, geo3) * 5 # *5是geo点跑到外面去的panelty
|
|
|
|
|
|
elif b ** 2 + c ** 2 <= a ** 2: # 钝角三角形，且geo3靠近geo1，包括点在线段延长线上的情况
|
|
|
|
|
|
return DistanceBetween(geo1, geo3) * 5
|
|
|
|
|
|
elif a + b == c: # 说明点在线段上
|
|
|
|
|
|
return 0
|
|
|
|
|
|
else:
|
|
|
|
|
|
if geo1[0] == geo2[0]: # 如果直线竖直
|
|
|
|
|
|
return abs(geo3[0] - geo1[0])
|
|
|
|
|
|
elif geo1[1] == geo2[1]: # 如果直线水平
|
|
|
|
|
|
return abs(geo3[1] - geo1[1])
|
|
|
|
|
|
else:
|
|
|
|
|
|
k = (geo1[1] - geo2[1]) / (geo1[0] - geo2[0]) # y = kx+d中的k
|
|
|
|
|
|
d = geo1[1] - k * geo1[0] # y = kx+d中的d
|
|
|
|
|
|
dist = abs(k * geo3[0] + d - geo3[1]) / sqrt(1 + k ** 2) # 点到直线距离公式
|
|
|
|
|
|
return dist
|
|
|
|
|
|
|
|
|
|
|
|
# 如果起点定位跟路线的相差很远或者方向不对，则没有必要取该条arc
|
|
|
|
|
|
def CheckNecessary(arc, car_record):
|
|
|
|
|
|
# 计算该地图中最长的路，取一半的大一点，即1000，防止出现漏判的情况，因为下面这个if是只取每段道路的坐标的起点和终点来跟目标点计算距离
|
|
|
|
|
|
if DistanceActual(arc.geometry_list[0], car_record.geo) > 1000:
|
|
|
|
|
|
if DistanceActual(arc.geometry_list[-1], car_record.geo) > 1000:
|
|
|
|
|
|
return False
|
|
|
|
|
|
# 下面if是计算这条线路上有一gps点跟目标点的最短距离小于100m才返回True。
|
|
|
|
|
|
min_dist = float("inf")
|
|
|
|
|
|
for geo1 in arc.geometry_list:
|
|
|
|
|
|
tmp = DistanceActual(geo1, car_record.geo)
|
|
|
|
|
|
if tmp < min_dist:
|
|
|
|
|
|
min_dist = tmp
|
|
|
|
|
|
if min_dist < 10:#起点与路线距离的判断
|
|
|
|
|
|
return True
|
|
|
|
|
|
else:
|
|
|
|
|
|
return False
|
|
|
|
|
|
|
|
|
|
|
|
# 计算GPS匹配到该路段的成本系数cost
|
|
|
|
|
|
def CalCost(arc, car_record):
|
|
|
|
|
|
cost = float("inf")#正无穷
|
|
|
|
|
|
# CheckNecessary函数首先排除与起点定位点距离相差远的路段。
|
|
|
|
|
|
if CheckNecessary(arc, car_record):
|
|
|
|
|
|
# arc.geometry_list为路段点集合，length记录点个数。
|
|
|
|
|
|
length = len(arc.geometry_list)
|
|
|
|
|
|
# 先找一个虚拟点，其为点arc.geometry_list[-2]到arc.geometry_list[-1]连线延长线上延长相等距离的点
|
|
|
|
|
|
# 虚拟点virtual_p在道路最后一个点之后。
|
|
|
|
|
|
virtual_p = [(2 * arc.geometry_list[-1][i] - arc.geometry_list[-2][i]) for i in range(2)]
|
|
|
|
|
|
# 找从哪个i开始，三角形ACB变成钝角，其中点A为car_record.geo,点C为arc.geometry_list[i]，点B为virtual_p
|
|
|
|
|
|
i = 0
|
|
|
|
|
|
# CheckOxygon检查起点与线路第i点的连线，与线路的夹角是否为锐角，是锐角继续循环。/LOU
|
|
|
|
|
|
while i < length - 1 and CheckOxygon(car_record.geo, arc.geometry_list[i], virtual_p):
|
|
|
|
|
|
i += 1
|
|
|
|
|
|
# 如果最小的i为0或者len() - 1，则取最靠近目标点的两个点计算垂点
|
|
|
|
|
|
# 即使出现钝角三角形的情况，calProjection函数中会处理
|
|
|
|
|
|
if i == 0:
|
|
|
|
|
|
# dist为起点定位点与道路的距离，direction_road为线路方向，作为之后计算cost的直接、间接的参数。/LOU
|
|
|
|
|
|
dist = CalDistance(arc.geometry_list[i], arc.geometry_list[i + 1], car_record.geo)
|
|
|
|
|
|
|
|
|
|
|
|
direction_road = arc.direction
|
|
|
|
|
|
else: # 如果垂点在线段上
|
|
|
|
|
|
dist = CalDistance(arc.geometry_list[i - 1], arc.geometry_list[i], car_record.geo)
|
|
|
|
|
|
direction_road = CalDirection(arc.geometry_list[i - 1: i + 1])
|
|
|
|
|
|
# 综合考虑起点与道路距离，得出起点定位点与路段匹配的cost。
|
|
|
|
|
|
cost = dist
|
|
|
|
|
|
return cost
|
|
|
|
|
|
|
|
|
|
|
|
# 给定car_geo和car_direction，根据最短距离，计算在所有道路的Cost，返回Cost最小的道路id
|
|
|
|
|
|
def CalMinCostArcID(car_record):
|
|
|
|
|
|
min_cost = float("inf")
|
|
|
|
|
|
id = 0
|
|
|
|
|
|
# arc_objects存储着所有边的Arc对象实例，即所有路段信息。
|
|
|
|
|
|
for arc in arc_objects:
|
|
|
|
|
|
cost = CalCost(arc, car_record)
|
|
|
|
|
|
if cost < min_cost:
|
|
|
|
|
|
min_cost = cost
|
|
|
|
|
|
id = arc.id
|
|
|
|
|
|
return id
|
|
|
|
|
|
|
|
|
|
|
|
# 计算时长
|
|
|
|
|
|
def SecondsBetween(t1, t2):
|
|
|
|
|
|
big = datetime.datetime.strptime(str(t2), "%Y-%m-%d %H:%M:%S")#例：print(now.strftime('%a, %b %d %H:%M'))输出Mon, May 08 20:22
|
|
|
|
|
|
small = datetime.datetime.strptime(str(t1), "%Y-%m-%d %H:%M:%S")
|
|
|
|
|
|
return (big - small).total_seconds()#用秒统计
|
|
|
|
|
|
|
|
|
|
|
|
# 返回两个点的实际距离，x,y与米的换算约为1:111000。（不同的比例尺数值不同）
|
|
|
|
|
|
def DistanceActual(geo1, geo2):
|
|
|
|
|
|
return pow(pow(float(geo1[0]) - float(geo2[0]), 2) + pow(float(geo1[1]) - float(geo2[1]), 2), 0.5)*111000
|
|
|
|
|
|
|
|
|
|
|
|
# 输入arc_cover是arc_id和cover的列表，该函数输出这个arc_cover包含的道路总长
|
|
|
|
|
|
def CalLenCover(arc_cover):
|
|
|
|
|
|
travel_distance = 0
|
|
|
|
|
|
for k in range(len(arc_cover)):
|
|
|
|
|
|
# arc_objects存储着所有边的Arc对象实例，即所有路段信息。
|
|
|
|
|
|
travel_distance += arc_objects[arc_cover[k][0] - 1].len * arc_cover[k][1]
|
|
|
|
|
|
return round(travel_distance,1)
|
|
|
|
|
|
|
|
|
|
|
|
# 输入arc_list是arc_id的列表，该函数输出这个arc_list包含的道路总长
|
|
|
|
|
|
def CalLen(arc_list):
|
|
|
|
|
|
travel_distance = 0
|
|
|
|
|
|
for k in range(len(arc_list)):
|
|
|
|
|
|
travel_distance += arc_objects[arc_list[k] - 1].len
|
|
|
|
|
|
return round(travel_distance,1)
|
|
|
|
|
|
|
|
|
|
|
|
# 拿着集合找最短的
|
|
|
|
|
|
def findMinDist(dist, collected):
|
|
|
|
|
|
min_dist = float("inf")
|
|
|
|
|
|
min_key = -1
|
|
|
|
|
|
for key in dist:
|
|
|
|
|
|
if dist[key] < min_dist and (key not in collected.keys()):
|
|
|
|
|
|
min_dist = dist[key]
|
|
|
|
|
|
min_key = key
|
|
|
|
|
|
return min_key
|
|
|
|
|
|
|
|
|
|
|
|
# 用单源最短路径算法，求两条边的最短距离的arc_list
|
|
|
|
|
|
# graph：邻近路网关系图；start_arc_id：起点路段；end_arc_id：终点路段
|
|
|
|
|
|
def dijkstra(graph, start_arc_id, end_arc_id):
|
|
|
|
|
|
if start_arc_id == end_arc_id: # 当前一路段与后匹配路段一样
|
|
|
|
|
|
return [start_arc_id]
|
|
|
|
|
|
dist = {}
|
|
|
|
|
|
path = {}
|
|
|
|
|
|
collected = {}
|
|
|
|
|
|
# 初始化距离为无穷大
|
|
|
|
|
|
for key in graph:
|
|
|
|
|
|
dist[key] = float("inf")
|
|
|
|
|
|
for arc in graph[key]:
|
|
|
|
|
|
if not arc.to_node in dist.keys(): # 如果邻近路网关系图中没有路段的起点等于arc的终点，则距离设为无穷
|
|
|
|
|
|
dist[arc.to_node] = float("inf")
|
|
|
|
|
|
from_node = arc_objects[start_arc_id - 1].to_node # 起始路段起点
|
|
|
|
|
|
to_node = arc_objects[end_arc_id - 1].from_node # 终止路段终点
|
|
|
|
|
|
if from_node in graph.keys(): # 如果graph中有路段起点与from_node一致
|
|
|
|
|
|
for arc in graph[from_node]: # 遍历路段起点与from_node一致的路段
|
|
|
|
|
|
dist[arc.to_node] = arc.len # 更新始路段起点到该点的距离
|
|
|
|
|
|
path[arc.to_node] = from_node # 更新始路段起点到该点的路径
|
|
|
|
|
|
dist[from_node] = 0 # 起始路段起点到起始路段起点距离为0
|
|
|
|
|
|
collected[from_node] = True
|
|
|
|
|
|
while True:
|
|
|
|
|
|
node = findMinDist(dist, collected)
|
|
|
|
|
|
if node == -1:
|
|
|
|
|
|
break
|
|
|
|
|
|
collected[node] = True
|
|
|
|
|
|
if node in graph.keys():
|
|
|
|
|
|
for arc in graph[node]:
|
|
|
|
|
|
if arc.to_node not in collected.keys():
|
|
|
|
|
|
if dist[arc.to_node] > dist[node] + arc.len:
|
|
|
|
|
|
dist[arc.to_node] = dist[node] + arc.len
|
|
|
|
|
|
path[arc.to_node] = node
|
|
|
|
|
|
arc_list = [end_arc_id]# 先放入终点
|
|
|
|
|
|
while to_node in path.keys():
|
|
|
|
|
|
for arc in graph[path[to_node]]:
|
|
|
|
|
|
if arc.to_node == to_node:
|
|
|
|
|
|
arc_list.append(arc.id)
|
|
|
|
|
|
to_node = path[to_node]
|
|
|
|
|
|
arc_list.append(start_arc_id) #最后放入起点
|
|
|
|
|
|
arc_list = arc_list[::-1] #两级反转
|
|
|
|
|
|
return arc_list
|
|
|
|
|
|
|
|
|
|
|
|
#arc：上一个点所匹配的路径，car_record：点对象，dist：arc终点到点的距离
|
|
|
|
|
|
def BFSFindPathArc(arc, car_record, dist):
|
|
|
|
|
|
min_cost = 60 # min_cost初始值为60近似为无限大
|
|
|
|
|
|
arc_list = [] # arc_list是点从上一个点走过的路径集合
|
|
|
|
|
|
min_arc_id = 0 # min_arc_id是得到最小cost的路段
|
|
|
|
|
|
visited = {} # visited 里面记录着已经搜索过计算过cost的路段
|
|
|
|
|
|
graph_around = {} # graph_around是连接着上一匹配路段的临近路段集合。
|
|
|
|
|
|
q = queue.Queue() # q是一个队列，里面存放需要计算cost的备选路段
|
|
|
|
|
|
q.put(arc) # 将arc放入队列
|
|
|
|
|
|
visited[arc.id] = True # 表示arc已经搜索过
|
|
|
|
|
|
while not q.empty(): # 当q里还有需要计算cost的路段，则继续循环
|
|
|
|
|
|
arc1 = q.get() # 将q里最后放进去的路段拿出并从q里删除
|
|
|
|
|
|
# 生成arc附近连通的道路的图，后面用来找最短路径
|
|
|
|
|
|
if arc1.from_node in graph_around.keys():
|
|
|
|
|
|
graph_around[arc1.from_node].append(arc1) # 将起点ID相同的路段放入同一个索引
|
|
|
|
|
|
else:
|
|
|
|
|
|
graph_around[arc1.from_node] = [arc1]
|
|
|
|
|
|
cost = CalCost(arc1, car_record) # 计算arc1的cost
|
|
|
|
|
|
if cost < min_cost:
|
|
|
|
|
|
min_cost = cost # 更新最小cost
|
|
|
|
|
|
min_arc_id = arc1.id # 更新最小cost的路段
|
|
|
|
|
|
if arc1.to_node in map_graph.keys(): # 如果arc1路段的终点在地图临近关系表map_graph里
|
|
|
|
|
|
for arc2 in map_graph[arc1.to_node]: # 在以arc1路段终点为起点的路段集合里做循环，以arc2变量表示
|
|
|
|
|
|
if arc2.id not in visited.keys(): # 如果arc2不在visited集合中
|
|
|
|
|
|
if DistanceBetween(arc.geometry_list[-1], arc2.geometry_list[0]) < dist*1.5:
|
|
|
|
|
|
# 如果arc1终点到arc2的起点的距离 小于 arc终点到GPS点的距离
|
|
|
|
|
|
q.put(arc2) # 将arc2放进队列
|
|
|
|
|
|
visited[arc2.id] = True # 将arc2标记已搜索
|
|
|
|
|
|
if min_arc_id != 0:
|
|
|
|
|
|
arc_list = dijkstra(graph_around, arc.id, min_arc_id) # 计算前一匹配路段到现匹配路段的最短路径
|
|
|
|
|
|
return arc_list
|
|
|
|
|
|
|
|
|
|
|
|
# 计算car_i经过的路径，将路段index记录在car_i_start_end_inde_list中，路径以j表示
|
|
|
|
|
|
def CarRecordsDivide(car_i):
|
|
|
|
|
|
# car_id_count存储着从1到100每一条OD的定位数量 /LOU
|
|
|
|
|
|
index_start = sum(car_id_count[:car_i-1]) # 第i条OD数据的起始索引位置
|
|
|
|
|
|
index_end = sum(car_id_count[:car_i]) # 第i条OD数据的终止索引位置
|
|
|
|
|
|
car_i_start_end_index_list = []
|
|
|
|
|
|
car_i_start_end_index_list.append([index_start, index_end])
|
|
|
|
|
|
return car_i_start_end_index_list
|
|
|
|
|
|
|
|
|
|
|
|
#计算属于一天中的第几个小时
|
|
|
|
|
|
def Time_stage(gps_time):
|
|
|
|
|
|
today_time=datetime.datetime(2022, 7, 9, 0, 0, 0)
|
|
|
|
|
|
return math.ceil(SecondsBetween(today_time, gps_time)/3600)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
