#!/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)