def init_backward(self,
g: nx.MultiDiGraph,
orig: NodeId,
dest: NodeId,
init_secs: float = 0,
init_cost: float = 0):
for end_node in g.adj[dest]:
edge = g.adj[dest][end_node][0]
secs = edge['length'] / self._speed + init_secs
cost = edge['length'] + init_cost
sort_cost = cost + self._get_heuristic_cost(g, end_node, orig)
idx = len(self._edge_labels_backward)
self._edge_labels_backward.append(
EdgeLabel(Cost(cost, secs, init_cost, init_secs),
sort_cost,
EdgeId(dest, end_node),
-1,
end_node,
is_origin=False,
is_destination=True))
self._adjacency_list_backward.insert(sort_cost, idx)
self._edges_status_backward[EdgeId(
dest, end_node)] = EdgeStatus(idx).set_temporary()
def append_bike_backward(self, g: nx.MultiDiGraph, bss_node: NodeId, orig: NodeId, init_secs: float = 0, init_cost: float = 0):
for end_node in g.adj[bss_node]:
edge = g.adj[bss_node][end_node][0]
secs = edge['length'] / self._bike_speed + init_secs
cost = edge['length'] * (self._walking_speed / self._bike_speed) + init_cost
sort_cost = cost + self._get_bike_heuristic_cost(g, end_node, orig)
edge_status = self._edges_status_bike_backward[EdgeId(bss_node, end_node)]
if edge_status.is_permanent():
# if it's permanent is this edge has less cost?
edge_label_idx = edge_status.edge_label_index
lab = weakref.proxy(self._edge_labels_bike_backward[edge_status.edge_label_index])
if lab.end_node == end_node:
if cost > lab.cost.cost:
continue
self._adjacency_list_bike_backward.insert(new_key=sort_cost,
item=edge_label_idx)
lab.pred_idx = -1
lab.end_node = end_node
lab.cost = Cost(cost, secs, init_cost, init_secs)
lab.is_origin = False
lab.is_destination = True
else:
idx = len(self._edge_labels_bike_backward)
self._edge_labels_bike_backward.append(EdgeLabel(Cost(cost, secs, init_cost, init_secs),
sort_cost,
EdgeId(bss_node, end_node),
-1,
end_node,
is_origin=False,
is_destination=True))
self._adjacency_list_bike_backward.insert(sort_cost, idx)
self._edges_status_bike_backward[EdgeId(bss_node, end_node)] = EdgeStatus(idx).set_temporary()
def append_bike(self, g: nx.MultiDiGraph, bss_node: NodeId, init_secs: float=0, init_cost: float=0):
for end_node in g.adj[bss_node]:
edge = g.adj[bss_node][end_node][0]
secs = edge['length'] / self._bike_speed + init_secs
cost = edge['length'] * (self._walking_speed / self._bike_speed) + init_cost
# let's see if the edge has already been visited?
edge_status = self._edges_status_bike[EdgeId(bss_node, end_node)]
if not edge_status.is_unreached():
# if it's visted, is this edge had less cost?
edge_label_idx = edge_status.edge_label_index
lab = weakref.proxy(self._edge_labels_bike[edge_status.edge_label_index])
if lab.end_node == end_node:
if cost > lab.cost.cost:
continue
self._adjacency_list_bike.insert(new_key=cost,
item=edge_label_idx)
lab.pred_idx = -1
lab.end_node = end_node
lab.cost = Cost(cost, secs, init_cost, init_secs)
lab.is_origin = True
lab.is_destination = False
else:
idx = len(self._edge_labels_bike)
self._edge_labels_bike.append(EdgeLabel(Cost(cost, secs, init_cost, init_secs),
cost,
EdgeId(bss_node, end_node),
-1,
end_node,
is_origin=True))
self._adjacency_list_bike.insert(cost, idx)
self._edges_status_bike[EdgeId(bss_node, end_node)] = EdgeStatus(idx).set_temporary()
def init(self):
self._edge_labels_forward = []
self._edge_labels_backward = []
self._adjacency_list_forward = PriorityQueue()
self._adjacency_list_backward = PriorityQueue()
self._edges_status_forward.clear()
self._edges_status_backward.clear()
self._best_path = BestConnection(EdgeId(-1, -1), EdgeId(-1, -1),
float('inf'))
self._threshold: float = float('inf')
def expand_backward(self, g, node, pred_idx, origin):
for end_node in g.adj[node]:
edge = g.adj[node][end_node][0]
edge_status = self._get_edge_status_backward(EdgeId(
node, end_node))
if edge_status.is_permanent():
continue
pred = self._edge_labels_backward[pred_idx]
new_cost = pred.cost + Cost(edge['length'],
edge['length'] / self._speed)
edge_id = EdgeId(node, end_node)
sort_cost = new_cost.cost + self._get_heuristic_cost(
g, end_node, origin)
# the edge has been visited
if edge_status.is_temporary():
lab = weakref.proxy(
self._edge_labels_backward[edge_status.edge_label_index])
if lab.end_node == end_node:
if new_cost < lab.cost:
self._adjacency_list_backward.insert(
new_key=sort_cost,
item=edge_status.edge_label_index)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
# Hmmm, we are visiting the edge in the opposing direction of last visit
elif lab.end_node == node:
if new_cost.cost < (lab.cost.cost - edge['length']):
self._adjacency_list_backward.insert(
new_key=sort_cost,
item=edge_status.edge_label_index)
lab.edge_id = EdgeId(node, end_node)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
continue
idx = len(self._edge_labels_backward)
self._edge_labels_backward.append(
EdgeLabel(new_cost, sort_cost, edge_id, pred_idx, end_node))
self._edges_status_backward[edge_id] = EdgeStatus(
idx).set_temporary()
self._adjacency_list_backward.insert(sort_cost, idx)
def expand_walking(self, g: nx.MultiDiGraph, node: NodeId, pred_idx: EdgeLabelIdx):
for end_node in g.adj[node]:
edge = g.adj[node][end_node][0]
pred = self._edge_labels_walking[pred_idx]
edge_id = EdgeId(node, end_node, pred.can_change_mode)
edge_status = self._edges_status_walking[edge_id]
new_cost = pred.cost + Cost(edge['length'], edge['length'] / self._walking_speed)
# the edge has been visited
if not edge_status.is_unreached():
lab = weakref.proxy(self._edge_labels_walking[edge_status.edge_label_index])
if lab.end_node == end_node:
if new_cost < lab.cost:
self._adjacency_list_walking.insert(new_key=new_cost.cost,
item=edge_status.edge_label_index)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
lab.can_change_mode = pred.can_change_mode
# Hmmm, we are visiting the edge in the opposing direction of last visit
elif lab.end_node == node:
if new_cost.cost < (lab.cost.cost - edge['length']):
self._adjacency_list_walking.insert(new_key=new_cost.cost,
item=edge_status.edge_label_index)
lab.edge_id = EdgeId(node, end_node)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
lab.can_change_mode = pred.can_change_mode
continue
idx = len(self._edge_labels_walking)
self._edge_labels_walking.append(EdgeLabel(new_cost,
new_cost.cost,
edge_id,
pred_idx,
end_node,
can_change_mode=pred.can_change_mode))
self._edges_status_walking[edge_id] = EdgeStatus(idx).set_temporary()
self._adjacency_list_walking.insert(new_cost.cost, idx)
def init_origin(self, g, orig, init_secs=0, init_cost=0):
# init origin
for end_node in g.adj[orig]:
edge = g.adj[orig][end_node][0]
secs = edge['length'] / self._speed + init_secs
cost = edge['length'] + init_cost
sort_cost = cost + self._get_heuristic_cost(
g, end_node, self._dest)
idx = len(self._edge_labels)
self._edge_labels.append(
EdgeLabel(Cost(cost, secs, init_cost, init_secs), sort_cost,
EdgeId(orig, end_node), -1, end_node, True))
self._adjacency_list.insert(sort_cost, idx)
self._edges_status[EdgeId(
orig, end_node)] = EdgeStatus(idx).set_temporary()
def get_best_path(
self,
g: nx.MultiDiGraph,
orig: NodeId,
dest: NodeId,
callback: Callable = lambda *args, **kwargs: None
) -> Tuple[List[NodeId], float]:
self._orig = orig
self._dest = dest
self.init()
self.init_origin(g, orig)
# init destination
for end_node in g.adj[dest]:
edge = g.adj[dest][end_node][0]
self._destinations[EdgeId(dest, end_node)] = edge['length']
i = 0
a = 0
# begin search
while True:
if i % 200 == 0:
callback(g, orig, dest, self._edges_status, self._edge_labels,
str(a).zfill(4))
a += 1
i += 1
current_labels = len(self._edge_labels)
if current_labels > PriorityQueue.QUEUE_MAX_SIZE:
return []
_, pred_index = self._adjacency_list.pop()
pred_edge_label = self._edge_labels[pred_index]
# Do we touch the destination?
d = self._destinations.get(pred_edge_label.edge_id)
if d:
if self._best_path.edge_label_index is -1:
self._best_path.edge_label_index = pred_index
self._best_path.cost = pred_edge_label.cost
return self.make_osm_path()
if not pred_edge_label.is_origin:
self._edges_status[pred_edge_label.edge_id].set_permanent()
self.expand_forward(g, pred_edge_label.end_node, pred_index, dest)
def append_walking(self, g, orig, can_change_mode: bool, init_secs: float=0, init_cost: float=0):
# init origin
for end_node in g.adj[orig]:
edge = g.adj[orig][end_node][0]
secs = edge['length'] / self._walking_speed + init_secs
cost = edge['length'] + init_cost
sort_cost = cost
# let's see if the edge has already been visited?
edge_status = self._edges_status_walking[EdgeId(orig, end_node, can_change_mode)]
if not edge_status.is_unreached():
# if it's visted, is this edge had less cost?
edge_label_idx = edge_status.edge_label_index
lab = weakref.proxy(self._edge_labels_walking[edge_label_idx])
if lab.end_node == end_node:
if cost > lab.cost.cost:
continue
self._adjacency_list_walking.insert(new_key=cost,
item=edge_label_idx)
lab.pred_idx = -1
lab.end_node = end_node
lab.cost = Cost(cost, secs, init_cost, init_secs)
lab.is_origin = True
lab.is_destination = False
else:
idx = len(self._edge_labels_walking)
self._edge_labels_walking.append(EdgeLabel(Cost(cost, secs, init_cost, init_secs),
sort_cost,
EdgeId(orig, end_node),
-1,
end_node,
is_origin=True,
can_change_mode=can_change_mode))
self._adjacency_list_walking.insert(sort_cost, idx)
self._edges_status_walking[EdgeId(orig, end_node, can_change_mode)] = EdgeStatus(idx).set_temporary()
class BestConnection:
forward: EdgeId = EdgeId(-1, -1)
backward: EdgeId = EdgeId(-1, -1)
cost: float = float('inf')
mode: str = None
def expand_forward(self, g: nx.MultiDiGraph, node: NodeId, pred_idx: EdgeLabelIdx, dest: NodeId, bss_nodes: Set[NodeId]):
def _get_speed(travel_mode: TravelMode):
return (self._walking_speed, self._bike_speed)[travel_mode.value]
def _normalize_factor(travel_mode: TravelMode):
return self._walking_speed / (self._walking_speed, self._bike_speed)[travel_mode.value]
def _get_heurestic_cost(travel_mode: TravelMode, *args, **kwargs):
fun = (self._get_walking_heuristic_cost, self._get_bike_heuristic_cost)[travel_mode.value]
return fun(*args, **kwargs)
for end_node in g.adj[node]:
edge = g.adj[node][end_node][0]
pred = self._edge_labels[pred_idx]
edge_status = self._get_edge_status(EdgeId(node, end_node, mode=pred.edge_id.mode))
if node not in bss_nodes and edge_status.is_permanent():
continue
if node not in bss_nodes and not edge_status.is_permanent():
new_cost = pred.cost + Cost(edge['length'] * _normalize_factor(pred.edge_id.mode),
edge['length'] / _get_speed(pred.edge_id.mode))
edge_id = EdgeId(node, end_node, pred.edge_id.mode)
sort_cost = new_cost.cost + _get_heurestic_cost(pred.edge_id.mode, g, end_node, dest)
# the edge has been visited
if edge_status.is_temporary():
lab = weakref.proxy(self._edge_labels[edge_status.edge_label_index])
if lab.end_node == end_node:
if new_cost < lab.cost:
self._adjacency_list.insert(new_key=sort_cost,
item=edge_status.edge_label_index)
lab.edge_id = EdgeId(node, end_node, pred.edge_id.mode)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
# Hmmm, we are visiting the edge in the opposing direction of last visit
elif lab.end_node == node:
if new_cost.cost < (lab.cost.cost - edge['length'] * _normalize_factor(pred.edge_id.mode)):
self._adjacency_list.insert(new_key=sort_cost,
item=edge_status.edge_label_index)
lab.edge_id = EdgeId(node, end_node, pred.edge_id.mode)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
continue
idx = len(self._edge_labels)
self._edge_labels.append(EdgeLabel(new_cost,
sort_cost,
edge_id,
pred_idx,
end_node))
self._edges_status[edge_id] = EdgeStatus(idx).set_temporary()
self._adjacency_list.insert(sort_cost, idx)
if node in bss_nodes:
for mode in TravelMode:
edge_status = self._get_edge_status(EdgeId(node, end_node, mode=mode))
if edge_status.is_permanent():
continue
new_cost = pred.cost + Cost(edge['length'] * _normalize_factor(mode),
edge['length'] / _get_speed(mode))
edge_id = EdgeId(node, end_node, mode)
if mode == TravelMode.WALKING:
sort_cost = new_cost.cost + self._get_walking_heuristic_cost(g, end_node, dest)
else:
sort_cost = new_cost.cost + self._get_bike_heuristic_cost(g, end_node, dest)
# the edge has been visited
if edge_status.is_temporary():
lab = weakref.proxy(self._edge_labels[edge_status.edge_label_index])
if lab.end_node == end_node:
if new_cost < lab.cost:
self._adjacency_list.insert(new_key=sort_cost,
item=edge_status.edge_label_index)
# lab.edge_id = EdgeId(node, end_node, mode)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
# Hmmm, we are visiting the edge in the opposing direction of last visit
elif lab.end_node == node:
if new_cost.cost < (lab.cost.cost - edge['length'] * _normalize_factor(mode)):
self._adjacency_list.insert(new_key=sort_cost,
item=edge_status.edge_label_index)
# lab.edge_id = EdgeId(node, end_node, mode)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
continue
idx = len(self._edge_labels)
self._edge_labels.append(EdgeLabel(new_cost,
sort_cost,
edge_id,
pred_idx,
end_node))
self._edges_status[edge_id] = EdgeStatus(idx).set_temporary()
self._adjacency_list.insert(sort_cost, idx)
def init_destination(self, g: nx.MultiDiGraph, dest: NodeId):
# init destination
for end_node in g.adj[dest]:
edge = g.adj[dest][end_node][0]
self._destinations[EdgeId(dest, end_node, mode=TravelMode.WALKING)] = edge['length']
class DoubleAstar(object):
_edge_labels_forward: List[EdgeLabel] = field(default_factory=list)
_edge_labels_backward: List[EdgeLabel] = field(default_factory=list)
_adjacency_list_forward: PriorityQueue = PriorityQueue()
_adjacency_list_backward: PriorityQueue = PriorityQueue()
_edges_status_forward: Dict[EdgeId, EdgeStatus] = defaultdict(EdgeStatus)
_edges_status_backward: Dict[EdgeId, EdgeStatus] = defaultdict(EdgeStatus)
_cost_factor: float = .5
_best_path: BestConnection = BestConnection(EdgeId(-1, -1), EdgeId(-1, -1),
float('inf'))
_speed: float = 1.4
_threshold: float = float('inf')
def __init__(self, speed=1.4, cost_factor=1):
self._speed = speed
self._cost_factor = cost_factor
def _get_heuristic_cost(self, g: nx.MultiDiGraph, start_node: NodeId,
end_node: NodeId) -> float:
if self._cost_factor == 0 or end_node is None:
return 0
start_ll = utils.osm_to_pointll(g, start_node)
end_ll = utils.osm_to_pointll(g, end_node)
return start_ll.distance_to(end_ll) * self._cost_factor
@staticmethod
def _get_edge_status_impl(container: Dict[EdgeId, EdgeStatus],
edge_id: EdgeId) -> EdgeStatus:
s = container.get(edge_id)
if not s:
return container[edge_id].set_unreached()
return s
def _get_edge_status_forward(self, edge_id: EdgeId) -> EdgeStatus:
return self._get_edge_status_impl(self._edges_status_forward, edge_id)
def _get_edge_status_backward(self, edge_id: EdgeId) -> EdgeStatus:
return self._get_edge_status_impl(self._edges_status_backward, edge_id)
def init_forward(self,
g: nx.MultiDiGraph,
orig: NodeId,
dest: NodeId,
init_secs: float = 0,
init_cost: float = 0):
for end_node in g.adj[orig]:
edge = g.adj[orig][end_node][0]
secs = edge['length'] / self._speed + init_secs
cost = edge['length'] + init_cost
sort_cost = cost + self._get_heuristic_cost(g, end_node, dest)
idx = len(self._edge_labels_forward)
self._edge_labels_forward.append(
EdgeLabel(Cost(cost, secs, init_cost, init_secs),
sort_cost,
EdgeId(orig, end_node),
-1,
end_node,
is_origin=True,
is_destination=False))
self._adjacency_list_forward.insert(sort_cost, idx)
self._edges_status_forward[EdgeId(
orig, end_node)] = EdgeStatus(idx).set_temporary()
def init_backward(self,
g: nx.MultiDiGraph,
orig: NodeId,
dest: NodeId,
init_secs: float = 0,
init_cost: float = 0):
for end_node in g.adj[dest]:
edge = g.adj[dest][end_node][0]
secs = edge['length'] / self._speed + init_secs
cost = edge['length'] + init_cost
sort_cost = cost + self._get_heuristic_cost(g, end_node, orig)
idx = len(self._edge_labels_backward)
self._edge_labels_backward.append(
EdgeLabel(Cost(cost, secs, init_cost, init_secs),
sort_cost,
EdgeId(dest, end_node),
-1,
end_node,
is_origin=False,
is_destination=True))
self._adjacency_list_backward.insert(sort_cost, idx)
self._edges_status_backward[EdgeId(
dest, end_node)] = EdgeStatus(idx).set_temporary()
def init(self):
self._edge_labels_forward = []
self._edge_labels_backward = []
self._adjacency_list_forward = PriorityQueue()
self._adjacency_list_backward = PriorityQueue()
self._edges_status_forward.clear()
self._edges_status_backward.clear()
self._best_path = BestConnection(EdgeId(-1, -1), EdgeId(-1, -1),
float('inf'))
self._threshold: float = float('inf')
# forward searching reach on a edge reached by backward searching
def set_forward_connection(self, pred: EdgeId):
edge_status_backward = self._edges_status_backward.get(pred)
edge_label_backward = edge_status_backward.edge_label_index
edge_status_forward = self._edges_status_forward.get(pred)
edge_label_forward = edge_status_forward.edge_label_index
pred_idx_forward = self._edge_labels_forward[
edge_label_forward].pred_idx
c = self._edge_labels_backward[edge_label_backward].cost \
+ self._edge_labels_forward[pred_idx_forward].cost
if c.cost < self._best_path.cost:
self._best_path = BestConnection(
self._edge_labels_forward[pred_idx_forward].edge_id,
self._edge_labels_backward[edge_label_backward].edge_id,
c.cost)
# backward searching reach on a edge reached by forward searching
def set_backward_connection(self, pred: EdgeId):
edge_status_forward = self._edges_status_forward.get(pred)
edge_label_forward = edge_status_forward.edge_label_index
edge_status_backward = self._edges_status_backward.get(pred)
edge_label_backward = edge_status_backward.edge_label_index
pred_idx_backward = self._edge_labels_backward[
edge_label_backward].pred_idx
c = self._edge_labels_backward[pred_idx_backward].cost \
+ self._edge_labels_forward[edge_label_forward].cost
if c.cost < self._best_path.cost:
self._best_path = BestConnection(
self._edge_labels_forward[edge_label_forward].edge_id,
self._edge_labels_backward[pred_idx_backward].edge_id, c.cost)
def expand_forward(self, g: nx.MultiDiGraph, node: NodeId,
pred_idx: EdgeLabelIdx, dest: NodeId):
for end_node in g.adj[node]:
edge = g.adj[node][end_node][0]
edge_status = self._get_edge_status_forward(EdgeId(node, end_node))
if edge_status.is_permanent():
continue
pred = self._edge_labels_forward[pred_idx]
new_cost = pred.cost + Cost(edge['length'],
edge['length'] / self._speed)
edge_id = EdgeId(node, end_node)
sort_cost = new_cost.cost + self._get_heuristic_cost(
g, end_node, dest)
# the edge has been visited
if edge_status.is_temporary():
lab = weakref.proxy(
self._edge_labels_forward[edge_status.edge_label_index])
if lab.end_node == end_node:
if new_cost < lab.cost:
self._adjacency_list_forward.insert(
new_key=sort_cost,
item=edge_status.edge_label_index)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
# Hmmm, we are visiting the edge in the opposing direction of last visit
elif lab.end_node == node:
if new_cost.cost < (lab.cost.cost - edge['length']):
self._adjacency_list_forward.insert(
new_key=sort_cost,
item=edge_status.edge_label_index)
lab.edge_id = EdgeId(node, end_node)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
continue
idx = len(self._edge_labels_forward)
self._edge_labels_forward.append(
EdgeLabel(new_cost, sort_cost, edge_id, pred_idx, end_node))
self._edges_status_forward[edge_id] = EdgeStatus(
idx).set_temporary()
self._adjacency_list_forward.insert(sort_cost, idx)
def expand_backward(self, g, node, pred_idx, origin):
for end_node in g.adj[node]:
edge = g.adj[node][end_node][0]
edge_status = self._get_edge_status_backward(EdgeId(
node, end_node))
if edge_status.is_permanent():
continue
pred = self._edge_labels_backward[pred_idx]
new_cost = pred.cost + Cost(edge['length'],
edge['length'] / self._speed)
edge_id = EdgeId(node, end_node)
sort_cost = new_cost.cost + self._get_heuristic_cost(
g, end_node, origin)
# the edge has been visited
if edge_status.is_temporary():
lab = weakref.proxy(
self._edge_labels_backward[edge_status.edge_label_index])
if lab.end_node == end_node:
if new_cost < lab.cost:
self._adjacency_list_backward.insert(
new_key=sort_cost,
item=edge_status.edge_label_index)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
# Hmmm, we are visiting the edge in the opposing direction of last visit
elif lab.end_node == node:
if new_cost.cost < (lab.cost.cost - edge['length']):
self._adjacency_list_backward.insert(
new_key=sort_cost,
item=edge_status.edge_label_index)
lab.edge_id = EdgeId(node, end_node)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
continue
idx = len(self._edge_labels_backward)
self._edge_labels_backward.append(
EdgeLabel(new_cost, sort_cost, edge_id, pred_idx, end_node))
self._edges_status_backward[edge_id] = EdgeStatus(
idx).set_temporary()
self._adjacency_list_backward.insert(sort_cost, idx)
def make_osm_path(self):
res_forward = []
edge = self._best_path.forward
edge_label_idx = self._edges_status_forward[edge].edge_label_index
edge_label = self._edge_labels_forward[edge_label_idx]
forward_secs = edge_label.cost.secs
while not edge_label.is_origin:
res_forward.append(edge_label.end_node)
edge_label_idx = edge_label.pred_idx
edge_label = self._edge_labels_forward[edge_label_idx]
res_forward.append(edge_label.edge_id.end)
res_forward.append(edge_label.edge_id.start)
res_forward = res_forward[::-1]
res_backward = []
edge = self._best_path.backward
edge_label_idx = self._edges_status_backward[edge].edge_label_index
edge_label = self._edge_labels_backward[edge_label_idx]
backward_secs = edge_label.cost.secs
while not edge_label.is_destination:
res_backward.append(edge_label.end_node)
edge_label_idx = edge_label.pred_idx
edge_label = self._edge_labels_backward[edge_label_idx]
res_backward.append(edge_label.edge_id.end)
res_backward.append(edge_label.edge_id.start)
if res_forward[-1] == res_backward[0]:
res_forward.pop(-1)
return res_forward + res_backward, forward_secs + backward_secs
def run(
self,
g: nx.MultiDiGraph,
orig: NodeId,
dest: NodeId,
callback: Callable = lambda *args, **kwargs: None
) -> Tuple[List[NodeId], float]:
expand_forward = True
expand_backward = True
forward_edge_label = None
forward_edge_label_idx = None
backward_edge_label = None
backward_edge_label_idx = None
a = 0
i = 0
# in order to balance the research in both directions
diff = None
while True:
if a % 15 == 0:
callback(g, orig, dest, self._edges_status_forward,
self._edge_labels_forward,
self._edges_status_backward,
self._edge_labels_backward,
str(i).zfill(4))
i += 1
a += 1
if expand_forward:
_, forward_edge_label_idx = self._adjacency_list_forward.pop()
forward_edge_label = self._edge_labels_forward[
forward_edge_label_idx]
# We don't want the expansion to go forever
if forward_edge_label.sort_cost - (diff if diff is not None
else 0) > self._threshold:
return self.make_osm_path()
if self._edges_status_backward[
forward_edge_label.edge_id].is_permanent():
if self._threshold == float('inf'):
self._threshold = forward_edge_label.sort_cost + kThresholdDelta
self.set_forward_connection(forward_edge_label.edge_id)
if expand_backward:
_, backward_edge_label_idx = self._adjacency_list_backward.pop(
)
backward_edge_label = self._edge_labels_backward[
backward_edge_label_idx]
# We don't want the expansion to go forever
if backward_edge_label.sort_cost > self._threshold:
return self.make_osm_path()
if self._edges_status_forward[
backward_edge_label.edge_id].is_permanent():
if self._threshold == float('inf'):
self._threshold = forward_edge_label.sort_cost + kThresholdDelta
self.set_backward_connection(backward_edge_label.edge_id)
if diff is None:
diff = forward_edge_label.sort_cost - backward_edge_label.sort_cost
if forward_edge_label.sort_cost <= backward_edge_label.sort_cost + diff:
expand_forward = True
expand_backward = False
if not forward_edge_label.is_origin:
self._edges_status_forward[
forward_edge_label.edge_id].set_permanent()
self.expand_forward(g, forward_edge_label.end_node,
forward_edge_label_idx, dest)
else:
expand_forward = False
expand_backward = True
if not backward_edge_label.is_destination:
self._edges_status_backward[
backward_edge_label.edge_id].set_permanent()
self.expand_backward(g, backward_edge_label.end_node,
backward_edge_label_idx, orig)
def get_best_path(
self,
g: nx.MultiDiGraph,
orig: NodeId,
dest: NodeId,
callback: Callable = lambda *args, **kwargs: None
) -> Tuple[List[NodeId], float]:
self.init()
self.init_forward(g, orig, dest)
self.init_backward(g, orig, dest)
return self.run(g, orig, dest, callback)
def expand_forward(self, g, node, pred_idx, dest):
for end_node in g.adj[node]:
edge = g.adj[node][end_node][0]
edge_status = self._get_edge_status(EdgeId(node, end_node))
if edge_status.is_permanent():
continue
pred = self._edge_labels[pred_idx]
new_cost = pred.cost + Cost(edge['length'],
edge['length'] / self._speed)
edge_id = EdgeId(node, end_node)
d = self._destinations.get(edge_id)
if d is not None:
if self._best_path.edge_label_index is -1 or new_cost < self._best_path.cost:
self._best_path.edge_label_index = edge_status.edge_label_index if edge_status.is_temporary() \
else len(self._edge_labels)
self._best_path.cost = new_cost
sort_cost = new_cost.cost + self._get_heuristic_cost(
g, end_node, self._dest)
# the edge has been visited
if edge_status.is_temporary():
lab = weakref.proxy(
self._edge_labels[edge_status.edge_label_index])
# Edge:
# 4242 ------------- 4141
# start -> end
# or
# end <- start
# the edge may have been visited either from its start or end,
# let's find out in this case which one is "cheaper"
# OK, we are visiting the edge at the same direction of last visit, nothing
# to be done
if lab.end_node == end_node:
if new_cost < lab.cost:
self._adjacency_list.insert(
new_key=sort_cost,
item=edge_status.edge_label_index)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
# Hmmm, we are visiting the edge in the opposing direction of last visit
elif lab.end_node == node:
if new_cost.cost < (lab.cost.cost - edge['length']):
self._adjacency_list.insert(
new_key=sort_cost,
item=edge_status.edge_label_index)
lab.edge_id = EdgeId(node, end_node)
lab.pred_idx = pred_idx
lab.end_node = end_node
lab.cost = new_cost
continue
idx = len(self._edge_labels)
self._edge_labels.append(
EdgeLabel(new_cost, sort_cost, edge_id, pred_idx, end_node))
self._edges_status[edge_id] = EdgeStatus(idx).set_temporary()
self._adjacency_list.insert(sort_cost, idx)