From 70080edf95f184af16206cc9c9a14944f045c882 Mon Sep 17 00:00:00 2001 From: ptvlk6bi2 <2584981545@qq.com> Date: Sat, 9 Jul 2022 11:35:59 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9C=80=E7=9F=AD=E8=B7=AF=E5=BE=84=E5=87=BD?= =?UTF-8?q?=E6=95=B0=E4=BB=A3=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ShortestPath.py | 313 ++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 313 insertions(+) create mode 100644 ShortestPath.py diff --git a/ShortestPath.py b/ShortestPath.py new file mode 100644 index 0000000..ea25f83 --- /dev/null +++ b/ShortestPath.py @@ -0,0 +1,313 @@ +#!/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) + + + + +