def get_trace_dyn(last_edge, cur_edge, last_point, cur_point, cnt=-1):
"""
通过路网计算两点之间的路径
:param last_edge: 上一条匹配边
:param cur_edge: 当前匹配的边
:param last_point: 上一个GPS点
:param cur_point: 当前GPS点
:return: trace, dist of trace
"""
spoint, _, _ = point_project_edge(last_point, last_edge)
try:
epoint, _, _ = point_project_edge(cur_point, cur_edge)
except TypeError:
print 'get_trace_dyn', cnt
if last_edge == cur_edge:
return [spoint, epoint], calc_dist(spoint, epoint)
node_set = set()
q = Queue.PriorityQueue(maxsize=-1)
init_candidate_queue(last_point, last_edge, q, node_set)
# 使用Dijkstra算法搜索路径
edge_found = False
while not q.empty() and not edge_found:
dnode = q.get()
cur_node, cur_dist = dnode.node, dnode.dist
for edge, node in cur_node.link_list:
if node.nodeid in node_set:
continue
node_set.add(node.nodeid)
next_dnode = DistNode(node, cur_dist + edge.edge_length)
node.prev_node = cur_node
q.put(next_dnode)
if edge.edge_index == cur_edge.edge_index:
edge_found = True
trace = [cur_point]
n0, n1 = cur_edge.node0, cur_edge.node1
if n0.prev_node == n1: # n1 is nearer from last point
cur_node = n1
else:
cur_node = n0
while cur_node != last_edge.node0 and cur_node != last_edge.node1:
trace.append(cur_node.point)
cur_node = cur_node.prev_node
trace.append(cur_node.point)
trace.append(spoint)
return trace, get_trace_dist(trace)
def get_trace(last_edge, edge, last_point, point):
"""
use prev_node to generate the path reversely
:param last_edge: last matched edge
:param edge: current matched edge
:param last_point: last position point
:param point: current matched(projected) point
:return:
"""
spoint, _, _ = point_project_edge(last_point, last_edge)
if last_edge == edge:
return [spoint, point]
trace = [point]
n0, n1 = edge.node0, edge.node1
if n0.prev_node == n1: # n1 is nearer from last point
cur_node = n1
else:
cur_node = n0
while cur_node != last_edge.node0 and cur_node != last_edge.node1:
trace.append(cur_node.point)
cur_node = cur_node.prev_node
trace.append(cur_node.point)
trace.append(spoint)
return trace
def init_candidate_queue(last_point, last_edge, can_queue, node_set):
"""
initialize the queue, add one or two points of the last edge
"""
_, ac, state = point_project_edge(last_point, last_edge)
project_dist = np.linalg.norm(np.array(ac))
dist0, dist1 = project_dist, last_edge.edge_length - project_dist
if dist0 > last_edge.edge_length:
dist0, dist1 = last_edge.edge_length, 0
if last_edge.oneway:
node = last_edge.node1
dnode = DistNode(node, dist1)
can_queue.put(dnode)
else:
node = last_edge.node0
dnode = DistNode(node, dist0)
can_queue.put(dnode)
node_set.add(node.nodeid)
node = last_edge.node1
dnode = DistNode(node, dist1)
can_queue.put(dnode)
node_set.add(node.nodeid)
def init_candidate_queue(self, last_point, last_edge, can_queue, node_set, cur_point):
"""
initialize the queue, add one or two points of the last edge
"""
_, ac, state = point_project_edge(last_point, last_edge)
project_dist = np.linalg.norm(np.array(ac))
dist0, dist1 = project_dist, last_edge.edge_length - project_dist
if dist0 > last_edge.edge_length:
dist0, dist1 = last_edge.edge_length, 0
p0, p1 = last_edge.node0.point, last_edge.node1.point
angle = calc_included_angle_math(last_point, cur_point, p0, p1)
if angle > 0:
node = last_edge.node1
dnode = DistNode(node, dist1)
can_queue.put(dnode)
else:
node = last_edge.node0
dnode = DistNode(node, dist0)
can_queue.put(dnode)
# if last_edge.oneway:
# node = last_edge.node1
# dnode = DistNode(node, dist1)
# can_queue.put(dnode)
# else:
# node = last_edge.node0
# dnode = DistNode(node, dist0)
# can_queue.put(dnode)
# node_set.add(node.nodeid)
#
# node = last_edge.node1
# dnode = DistNode(node, dist1)
# can_queue.put(dnode)
node_set.add(last_edge.node0.nodeid)
node_set.add(last_edge.node1.nodeid)