Save The Tree implementation and acceptance work in progress
Preserve production code, regression fixtures, audio assets and provenance, test infrastructure, and the original-completion plan and progress ledger. Final game and Windows acceptance remain pending; excluded local artifacts and native experiments are not part of this snapshot.
This commit is contained in:
Takeshi
@@ -76,11 +76,14 @@ var last_no_route_reason: StringName = ROUTE_OK
|
||||
var last_path_cost := 0
|
||||
|
||||
var _passable_cells: Dictionary = {}
|
||||
var _sorted_passable_cell_keys: Array = []
|
||||
var _connectors: Dictionary = {}
|
||||
var _adjacency: Dictionary = {}
|
||||
var _vertical_walk_blockers: Dictionary = {}
|
||||
var _vertical_walk_blocked_cells: Dictionary = {}
|
||||
var _path_cache: Dictionary = {}
|
||||
# Derived state limited to one synchronous candidate projection, never serialized.
|
||||
var _query_batch: Dictionary = {}
|
||||
var _cell_traffic: Dictionary = {}
|
||||
var _actor_cells: Dictionary = {}
|
||||
var _actor_progress: Dictionary = {}
|
||||
@@ -272,6 +275,7 @@ func configure_connector(
|
||||
and not _active_request_id_exists(StringName(request_id)):
|
||||
last_error = "Connector configuration reuses a retired elevator request id."
|
||||
return false
|
||||
var previous_configuration: Dictionary = _connectors.get(connector_id, {})
|
||||
_connectors[connector_id] = configuration
|
||||
var next_active: Dictionary = index_result[&"active_units"]
|
||||
for unit_id: Variant in _active_elevator_units:
|
||||
@@ -282,7 +286,7 @@ func configure_connector(
|
||||
_refresh_transit_actor_ids()
|
||||
for request_id: Variant in index_result[&"known_request_ids"]:
|
||||
_known_elevator_request_ids[request_id] = true
|
||||
_rebuild_derived_graph()
|
||||
_rebuild_connector_endpoint_edges(previous_configuration, configuration)
|
||||
_mark_graph_changed()
|
||||
last_error = ""
|
||||
return true
|
||||
@@ -1021,7 +1025,14 @@ func _find_path_internal(
|
||||
_path_cache.erase(cache_key)
|
||||
|
||||
cache_misses += 1
|
||||
var path := _run_a_star(start, goal, allowed_layers, allowed_access_ids)
|
||||
if start.z != goal.z and not _guest_batch_has_permitted_layer_crossing(start, goal, allowed_layers, allowed_access_ids):
|
||||
return _route_failure(ROUTE_GOAL_UNREACHABLE)
|
||||
var shared: Variant = _query_batch_path(start, goal, allowed_layers, allowed_access_ids)
|
||||
var path: Array[Vector3i] = []
|
||||
if shared == null:
|
||||
path = _run_a_star(start, goal, allowed_layers, allowed_access_ids)
|
||||
else:
|
||||
path.assign(shared)
|
||||
if path.is_empty():
|
||||
return _route_failure(_diagnose_no_route_reason(
|
||||
start,
|
||||
@@ -1031,11 +1042,282 @@ func _find_path_internal(
|
||||
))
|
||||
_path_cache[cache_key] = _copy_path(path)
|
||||
last_no_route_reason = ROUTE_OK
|
||||
last_path_cost = _calculate_path_cost_for_access(path, allowed_access_ids)
|
||||
var batch_cost := -1
|
||||
if allowed_access_ids.is_empty() and allowed_layers == [FRONT,BACK]:
|
||||
batch_cost = _guest_batch_path_cost(start,goal,path)
|
||||
last_path_cost = batch_cost if batch_cost >= 0 else _calculate_path_cost_for_access(path, allowed_access_ids)
|
||||
last_error = ""
|
||||
return path
|
||||
|
||||
|
||||
# Only the default guest access profile is shared. A mutation invalidates the
|
||||
# whole batch through the same public cache invalidation paths as ordinary routes.
|
||||
func begin_guest_path_query_batch(start: Vector3i, goals: Array) -> void:
|
||||
_query_batch.clear()
|
||||
# Without connectors the original Manhattan solver stops on goal pop.
|
||||
# Equal-cost canonical parents can then differ: retain that solver exactly.
|
||||
if _connectors.is_empty(): return
|
||||
var layers: Array[int] = [FRONT, BACK]
|
||||
var access: Array[StringName] = []
|
||||
var targets: Dictionary = {}
|
||||
var crossings: Dictionary = {}
|
||||
for value: Variant in goals:
|
||||
if not value is Vector3i: continue
|
||||
var goal: Vector3i = value
|
||||
if not _passable_cells.has(goal) or _cell_is_temporarily_blocked(goal): continue
|
||||
if start.z != goal.z:
|
||||
if not crossings.has(goal.z): crossings[goal.z] = _has_permitted_layer_crossing(start.z, goal.z, access)
|
||||
if not bool(crossings[goal.z]): continue
|
||||
targets[goal] = true
|
||||
if not targets.is_empty() and _passable_cells.has(start) and not _cell_is_temporarily_blocked(start):
|
||||
_query_batch = {&"start":start, &"targets":targets, &"ready":false, &"tree":{}, &"crossings":crossings}
|
||||
|
||||
|
||||
# Directed layer permission depends only on connectors/access in this synchronous
|
||||
# default guest batch. All existing invalidation and restore paths clear it.
|
||||
func _guest_batch_has_permitted_layer_crossing(start: Vector3i, goal: Vector3i, layers: Array[int], access: Array[StringName]) -> bool:
|
||||
if not _query_batch.is_empty() and start == _query_batch[&"start"] and layers == [FRONT,BACK] and access.is_empty():
|
||||
var crossings: Dictionary = _query_batch[&"crossings"]
|
||||
if crossings.has(goal.z): return bool(crossings[goal.z])
|
||||
return _has_permitted_layer_crossing(start.z,goal.z,access)
|
||||
|
||||
|
||||
func end_guest_path_query_batch() -> void:
|
||||
_query_batch.clear()
|
||||
|
||||
|
||||
func _query_batch_path(start: Vector3i, goal: Vector3i, layers: Array[int], access: Array[StringName]) -> Variant:
|
||||
if _query_batch.is_empty() or start != _query_batch[&"start"] \
|
||||
or layers != [FRONT, BACK] or not access.is_empty() or not _query_batch[&"targets"].has(goal):
|
||||
return null
|
||||
if start == goal: return [start]
|
||||
if not bool(_query_batch[&"ready"]):
|
||||
_query_batch[&"tree"] = _build_guest_query_tree(start, _query_batch[&"targets"], layers, access)
|
||||
_query_batch[&"arithmetic_wrapped"] = _guest_tree_arithmetic_wrapped
|
||||
_query_batch[&"ready"] = true
|
||||
var tree: Dictionary = _query_batch[&"tree"]
|
||||
if not bool(tree.get(&"usable", false)): return null
|
||||
var settled: Dictionary = tree[&"settled"]
|
||||
if not settled.has(goal): return []
|
||||
return _reconstruct_path(tree[&"parents"], goal)
|
||||
|
||||
|
||||
# Positive edge costs make every shortest predecessor settle before its child.
|
||||
# Resolving equal costs by the original coordinate parent rule gives the same
|
||||
# canonical paths as the admissible goal search. Stop after requested goals, not
|
||||
# after visiting the entire graph. Any nonpositive edge retains the old solver.
|
||||
var _guest_tree_workspace: Dictionary = {}
|
||||
# Derived search state only. Wrapped accumulated costs cannot price a cell path.
|
||||
var _guest_tree_arithmetic_wrapped: bool = false
|
||||
|
||||
func _build_guest_query_tree(start: Vector3i, goals: Dictionary, layers: Array[int], access: Array[StringName]) -> Dictionary:
|
||||
_guest_tree_arithmetic_wrapped = false
|
||||
if get_script().resource_path != "res://scripts/world/path_service.gd" or layers != [FRONT,BACK] or not access.is_empty():
|
||||
return _build_guest_query_tree_uncached(start,goals,layers,access)
|
||||
return _build_guest_query_tree_shared(start,goals,layers,access)
|
||||
|
||||
func _build_guest_query_tree_uncached(start: Vector3i, goals: Dictionary, layers: Array[int], access: Array[StringName]) -> Dictionary:
|
||||
# The sorted graph cells preserve the original x/y/layer heap and parent order.
|
||||
# All numeric workspace is local to this search, including the coordinate map.
|
||||
var cells: Array = _sorted_passable_cell_keys
|
||||
var indices: Dictionary = {}
|
||||
for index: int in range(cells.size()): indices[cells[index]] = index
|
||||
if not indices.has(start): return {&"usable":false}
|
||||
var costs := PackedInt64Array()
|
||||
var parents := PackedInt64Array()
|
||||
var finalized := PackedByteArray()
|
||||
if costs.resize(cells.size()) != OK or parents.resize(cells.size()) != OK or finalized.resize(cells.size()) != OK:
|
||||
return {&"usable":false}
|
||||
costs.fill(_INFINITE_COST)
|
||||
parents.fill(-1)
|
||||
var start_index: int = indices[start]
|
||||
costs[start_index] = 0
|
||||
var discovered: Array[int] = [start_index]
|
||||
var parent_discovered: Array[int] = []
|
||||
var heap_nodes: Array[int] = []
|
||||
var heap_costs: Array[int] = []
|
||||
_indexed_tree_heap_push(heap_nodes, heap_costs, start_index, 0)
|
||||
var pending: Dictionary = goals.duplicate()
|
||||
var settled: Dictionary = {}
|
||||
while not heap_nodes.is_empty():
|
||||
var entry_cost: int = heap_costs[0]
|
||||
var current_index := _indexed_tree_heap_pop(heap_nodes, heap_costs)
|
||||
if finalized[current_index] != 0 or entry_cost != costs[current_index]: continue
|
||||
var current: Vector3i = cells[current_index]
|
||||
finalized[current_index] = 1
|
||||
settled[current] = true
|
||||
pending.erase(current)
|
||||
if pending.is_empty(): break
|
||||
var edges: Array = _adjacency.get(current, [])
|
||||
for value: Variant in edges:
|
||||
var edge: Dictionary = value
|
||||
var neighbor: Vector3i = edge[&"to"]
|
||||
if neighbor.z not in layers or _cell_is_temporarily_blocked(neighbor) \
|
||||
or not _edge_meets_vertical_policy(current,edge) or not _edge_is_accessible(edge,access): continue
|
||||
var edge_cost := _edge_total_cost(edge)
|
||||
if edge_cost <= 0: return {&"usable":false}
|
||||
if not indices.has(neighbor): return {&"usable":false}
|
||||
var neighbor_index: int = indices[neighbor]
|
||||
var cost := costs[current_index] + edge_cost
|
||||
var known := costs[neighbor_index]
|
||||
var better_parent := cost == known and parents[neighbor_index] >= 0 and current_index < parents[neighbor_index]
|
||||
if cost > known or (cost == known and not better_parent): continue
|
||||
if parents[neighbor_index] < 0:
|
||||
parent_discovered.append(neighbor_index)
|
||||
if neighbor_index != start_index: discovered.append(neighbor_index)
|
||||
parents[neighbor_index] = current_index
|
||||
costs[neighbor_index] = cost
|
||||
finalized[neighbor_index] = 0
|
||||
settled.erase(neighbor)
|
||||
_indexed_tree_heap_push(heap_nodes, heap_costs, neighbor_index, cost)
|
||||
# Keep the existing result dictionaries and their first-discovery order.
|
||||
var result_parents: Dictionary = {}
|
||||
var result_costs: Dictionary = {}
|
||||
for index: int in discovered:
|
||||
var cell: Vector3i = cells[index]
|
||||
result_costs[cell] = costs[index]
|
||||
for index: int in parent_discovered:
|
||||
result_parents[cells[index]] = cells[parents[index]]
|
||||
return {&"usable":true,&"settled":settled,&"parents":result_parents,&"costs":result_costs}
|
||||
|
||||
|
||||
func _build_guest_query_tree_shared(start: Vector3i, goals: Dictionary, layers: Array[int], access: Array[StringName]) -> Dictionary:
|
||||
# The sorted graph cells preserve the original x/y/layer heap and parent order.
|
||||
# All numeric workspace is local to this search, including the coordinate map.
|
||||
var cells: Array = _sorted_passable_cell_keys
|
||||
if _guest_tree_workspace.is_empty():
|
||||
var fresh_indices: Dictionary = {}
|
||||
for index: int in range(cells.size()): fresh_indices[cells[index]] = index
|
||||
_guest_tree_workspace = {&"indices":fresh_indices, &"edges":{}}
|
||||
var indices: Dictionary = _guest_tree_workspace[&"indices"]
|
||||
if not indices.has(start): return {&"usable":false}
|
||||
var costs := PackedInt64Array()
|
||||
var parents := PackedInt64Array()
|
||||
var finalized := PackedByteArray()
|
||||
if costs.resize(cells.size()) != OK or parents.resize(cells.size()) != OK or finalized.resize(cells.size()) != OK:
|
||||
return {&"usable":false}
|
||||
costs.fill(_INFINITE_COST)
|
||||
parents.fill(-1)
|
||||
var start_index: int = indices[start]
|
||||
costs[start_index] = 0
|
||||
var discovered: Array[int] = [start_index]
|
||||
var parent_discovered: Array[int] = []
|
||||
var heap_nodes: Array[int] = []
|
||||
var heap_costs: Array[int] = []
|
||||
_indexed_tree_heap_push(heap_nodes, heap_costs, start_index, 0)
|
||||
var pending: Dictionary = goals.duplicate()
|
||||
var settled: Dictionary = {}
|
||||
while not heap_nodes.is_empty():
|
||||
var entry_cost: int = heap_costs[0]
|
||||
var current_index := _indexed_tree_heap_pop(heap_nodes, heap_costs)
|
||||
if finalized[current_index] != 0 or entry_cost != costs[current_index]: continue
|
||||
var current: Vector3i = cells[current_index]
|
||||
finalized[current_index] = 1
|
||||
settled[current] = true
|
||||
pending.erase(current)
|
||||
if pending.is_empty(): break
|
||||
var edges: Array = _guest_tree_shared_edges(current, layers, access)
|
||||
for value: Variant in edges:
|
||||
var row: Array = value
|
||||
var neighbor: Vector3i = row[0]
|
||||
var edge_cost: int = row[1]
|
||||
if edge_cost <= 0: return {&"usable":false}
|
||||
if not indices.has(neighbor): return {&"usable":false}
|
||||
var neighbor_index: int = indices[neighbor]
|
||||
var cost := costs[current_index] + edge_cost
|
||||
if cost < costs[current_index]: _guest_tree_arithmetic_wrapped = true
|
||||
var known := costs[neighbor_index]
|
||||
var better_parent := cost == known and parents[neighbor_index] >= 0 and current_index < parents[neighbor_index]
|
||||
if cost > known or (cost == known and not better_parent): continue
|
||||
if parents[neighbor_index] < 0:
|
||||
parent_discovered.append(neighbor_index)
|
||||
if neighbor_index != start_index: discovered.append(neighbor_index)
|
||||
parents[neighbor_index] = current_index
|
||||
costs[neighbor_index] = cost
|
||||
finalized[neighbor_index] = 0
|
||||
settled.erase(neighbor)
|
||||
_indexed_tree_heap_push(heap_nodes, heap_costs, neighbor_index, cost)
|
||||
# Keep the existing result dictionaries and their first-discovery order.
|
||||
var result_parents: Dictionary = {}
|
||||
var result_costs: Dictionary = {}
|
||||
for index: int in discovered:
|
||||
var cell: Vector3i = cells[index]
|
||||
result_costs[cell] = costs[index]
|
||||
for index: int in parent_discovered:
|
||||
result_parents[cells[index]] = cells[parents[index]]
|
||||
return {&"usable":true,&"settled":settled,&"parents":result_parents,&"costs":result_costs}
|
||||
|
||||
|
||||
func _guest_tree_shared_edges(current: Vector3i, layers: Array[int], access: Array[StringName]) -> Array:
|
||||
var shared: Dictionary = _guest_tree_workspace[&"edges"]
|
||||
if shared.has(current): return shared[current]
|
||||
var result: Array = []
|
||||
for value: Variant in _adjacency.get(current,[]):
|
||||
var edge: Dictionary = value
|
||||
var neighbor: Vector3i = edge[&"to"]
|
||||
if neighbor.z not in layers or _cell_is_temporarily_blocked(neighbor) \
|
||||
or not _edge_meets_vertical_policy(current,edge) or not _edge_is_accessible(edge,access): continue
|
||||
var cost := _edge_total_cost(edge)
|
||||
result.append([neighbor,cost])
|
||||
if cost <= 0 or not (_guest_tree_workspace[&"indices"] as Dictionary).has(neighbor): break
|
||||
shared[current]=result
|
||||
return result
|
||||
|
||||
func _indexed_tree_heap_push(nodes: Array[int], costs: Array[int], node: int, cost: int) -> void:
|
||||
nodes.append(node)
|
||||
costs.append(cost)
|
||||
var index := nodes.size() - 1
|
||||
while index > 0:
|
||||
var parent := (index - 1) >> 1
|
||||
if cost > costs[parent] or (cost == costs[parent] and node >= nodes[parent]): break
|
||||
nodes[index] = nodes[parent]
|
||||
costs[index] = costs[parent]
|
||||
index = parent
|
||||
nodes[index] = node
|
||||
costs[index] = cost
|
||||
|
||||
|
||||
func _indexed_tree_heap_pop(nodes: Array[int], costs: Array[int]) -> int:
|
||||
var first := nodes[0]
|
||||
var last_node: int = nodes.pop_back()
|
||||
var last_cost: int = costs.pop_back()
|
||||
if nodes.is_empty(): return first
|
||||
var index := 0
|
||||
while index * 2 + 1 < nodes.size():
|
||||
var child := index * 2 + 1
|
||||
if child + 1 < nodes.size() and (costs[child + 1] < costs[child] or (costs[child + 1] == costs[child] and nodes[child + 1] < nodes[child])):
|
||||
child += 1
|
||||
if costs[child] > last_cost or (costs[child] == last_cost and nodes[child] >= last_node): break
|
||||
nodes[index] = nodes[child]
|
||||
costs[index] = costs[child]
|
||||
index = child
|
||||
nodes[index] = last_node
|
||||
costs[index] = last_cost
|
||||
return first
|
||||
|
||||
|
||||
# A matching current batch route without arithmetic wrap has an exact cost. Compare
|
||||
# the whole path, not only its endpoints: a valid alternate may cost differently.
|
||||
# Mutation/restore invalidation clears the batch even for equal revision numbers.
|
||||
func _guest_batch_path_cost(start: Vector3i, goal: Vector3i, path: Array) -> int:
|
||||
if _query_batch.is_empty() or start != _query_batch[&"start"] or not _query_batch[&"targets"].has(goal): return -1
|
||||
if start == goal and path == [start]: return 0
|
||||
if not bool(_query_batch[&"ready"]): return -1
|
||||
if bool(_query_batch.get(&"arithmetic_wrapped", false)): return -1
|
||||
var tree: Dictionary = _query_batch[&"tree"]
|
||||
if not bool(tree.get(&"usable",false)) or not tree[&"settled"].has(goal): return -1
|
||||
if path != _reconstruct_path(tree[&"parents"],goal): return -1
|
||||
return int(tree[&"costs"][goal])
|
||||
|
||||
|
||||
# Same public last_error behavior as default-access calculate_path_cost.
|
||||
func calculate_guest_batch_path_cost(start: Vector3i, goal: Vector3i, path: Array) -> int:
|
||||
var cost := _guest_batch_path_cost(start,goal,path)
|
||||
if cost < 0: return calculate_path_cost(path)
|
||||
last_error = ""
|
||||
return cost
|
||||
|
||||
|
||||
func find_path_report(
|
||||
start: Vector3i,
|
||||
goal: Vector3i,
|
||||
@@ -1087,6 +1369,8 @@ func invalidate_cache_for_cells(cells: Variant) -> int:
|
||||
if not normalization[&"ok"]:
|
||||
last_error = normalization[&"error"]
|
||||
return 0
|
||||
_query_batch.clear()
|
||||
_guest_tree_workspace.clear()
|
||||
var affected: Dictionary = normalization[&"cells"]
|
||||
var keys_to_remove: Array = []
|
||||
for cache_key: Variant in _path_cache:
|
||||
@@ -1104,6 +1388,8 @@ func invalidate_local_cache(cells: Variant) -> int:
|
||||
|
||||
|
||||
func invalidate_all_cache() -> int:
|
||||
_query_batch.clear()
|
||||
_guest_tree_workspace.clear()
|
||||
var removed := _path_cache.size()
|
||||
_path_cache.clear()
|
||||
return removed
|
||||
@@ -1158,8 +1444,20 @@ func get_edge_report(from_cell: Vector3i, to_cell: Vector3i, connector_access: V
|
||||
return report
|
||||
|
||||
|
||||
# The caller only needs the selected connector id. Avoid constructing the
|
||||
# detailed report and its extra cost calculation. Without a connector edge to
|
||||
# this destination every possible selected edge has the empty connector id.
|
||||
func get_step_connector(from_cell: Vector3i, to_cell: Vector3i, connector_access: Variant = null) -> StringName:
|
||||
return StringName(get_edge_report(from_cell, to_cell, connector_access).get(&"connector_id", &""))
|
||||
var normalized := _normalize_access_ids(connector_access)
|
||||
if not bool(normalized[&"ok"]):
|
||||
last_error = String(normalized[&"error"])
|
||||
return &""
|
||||
if _cell_is_temporarily_blocked(from_cell) or _cell_is_temporarily_blocked(to_cell):
|
||||
return &""
|
||||
for edge: Dictionary in _adjacency.get(from_cell, []):
|
||||
if edge[&"to"] == to_cell and not StringName(edge[&"connector_id"]).is_empty():
|
||||
return StringName(_find_edge(from_cell,to_cell,normalized[&"values"]).get(&"connector_id", &""))
|
||||
return &""
|
||||
|
||||
|
||||
func get_neighbors(cell: Vector3i) -> Array:
|
||||
@@ -1322,8 +1620,7 @@ func create_snapshot() -> Dictionary:
|
||||
connector[&"request_queue"] = queue
|
||||
saved_connectors.append(connector)
|
||||
|
||||
var cells: Array = _passable_cells.keys()
|
||||
cells.sort_custom(Callable(self, "_coordinate_less"))
|
||||
var cells: Array = _sorted_passable_cell_keys
|
||||
var saved_cell_traffic: Array = []
|
||||
for cell_value: Variant in cells:
|
||||
var cell: Vector3i = cell_value
|
||||
@@ -1536,10 +1833,56 @@ func restore_snapshot(snapshot: Dictionary) -> bool:
|
||||
return true
|
||||
|
||||
|
||||
## A single connector changes only the outgoing edges of its old/new endpoints.
|
||||
## Keep the same walk-edge rules, sorted connector iteration and edge ordering
|
||||
## as the full rebuild. Topology changes still use the full graph rebuild.
|
||||
func _rebuild_connector_endpoint_edges(previous: Dictionary, current: Dictionary) -> void:
|
||||
var affected: Dictionary = {}
|
||||
for configuration: Dictionary in [previous, current]:
|
||||
if configuration.is_empty():
|
||||
continue
|
||||
for key: StringName in [&"from", &"to"]:
|
||||
var cell: Vector3i = configuration[key]
|
||||
if _passable_cells.has(cell):
|
||||
affected[cell] = true
|
||||
var cells: Array = affected.keys()
|
||||
cells.sort_custom(Callable(self, "_coordinate_less"))
|
||||
for cell_value: Variant in cells:
|
||||
var cell: Vector3i = cell_value
|
||||
_adjacency[cell] = []
|
||||
for direction in _CARDINAL_DIRECTIONS:
|
||||
var neighbor := cell + direction
|
||||
if _passable_cells.has(neighbor) and _ordinary_walk_edge_allowed(cell, neighbor):
|
||||
_append_edge(cell, neighbor, ORTHOGONAL_MOVE_COST, &"", &"", MOVEMENT_WALK)
|
||||
var connector_ids: Array = _connectors.keys()
|
||||
connector_ids.sort_custom(Callable(self, "_string_name_less"))
|
||||
for connector_id_value: Variant in connector_ids:
|
||||
var connector_id: StringName = connector_id_value
|
||||
var connector: Dictionary = _connectors[connector_id]
|
||||
if not _connector_is_operational(connector):
|
||||
continue
|
||||
var from_cell: Vector3i = connector[&"from"]
|
||||
var to_cell: Vector3i = connector[&"to"]
|
||||
if not _passable_cells.has(from_cell) or not _passable_cells.has(to_cell):
|
||||
continue
|
||||
var cost: int = connector[&"cost"]
|
||||
var access_id: StringName = connector[&"access_id"]
|
||||
var movement_type: StringName = connector[&"movement_type"]
|
||||
if affected.has(from_cell):
|
||||
_append_edge(from_cell, to_cell, cost, connector_id, access_id, movement_type)
|
||||
if bool(connector[&"bidirectional"]) and affected.has(to_cell):
|
||||
_append_edge(to_cell, from_cell, cost, connector_id, access_id, movement_type)
|
||||
for cell_value: Variant in cells:
|
||||
var edges: Array = _adjacency[cell_value]
|
||||
edges.sort_custom(Callable(self, "_edge_less"))
|
||||
_adjacency[cell_value] = edges
|
||||
|
||||
|
||||
func _rebuild_derived_graph() -> void:
|
||||
_adjacency.clear()
|
||||
var cells: Array = _passable_cells.keys()
|
||||
cells.sort_custom(Callable(self, "_coordinate_less"))
|
||||
_sorted_passable_cell_keys = cells
|
||||
for cell_value: Variant in cells:
|
||||
var cell: Vector3i = cell_value
|
||||
_adjacency[cell] = []
|
||||
@@ -1611,19 +1954,26 @@ func _run_a_star(
|
||||
if start == goal:
|
||||
return [start]
|
||||
|
||||
var connector_scale := _positive_connector_heuristic_scale()
|
||||
var settled_goal_cost := _INFINITE_COST
|
||||
var frontier: Array[Array] = []
|
||||
_heap_push(frontier, [start, 0, _heuristic(start, goal)])
|
||||
_heap_push(frontier, [start, 0, _query_distance_lower_bound(start, goal, connector_scale)])
|
||||
var closed_set: Dictionary = {}
|
||||
var came_from: Dictionary = {}
|
||||
var g_score: Dictionary = {start: 0}
|
||||
|
||||
while not frontier.is_empty():
|
||||
var entry := _heap_pop(frontier)
|
||||
if connector_scale > 0 and int(entry[2]) > settled_goal_cost:
|
||||
break
|
||||
var current: Vector3i = entry[0]
|
||||
if closed_set.has(current) or int(entry[1]) != int(g_score[current]):
|
||||
continue
|
||||
if current == goal:
|
||||
return _reconstruct_path(came_from, current)
|
||||
if connector_scale <= 0:
|
||||
return _reconstruct_path(came_from, current)
|
||||
settled_goal_cost = int(entry[1])
|
||||
continue
|
||||
|
||||
closed_set[current] = true
|
||||
var edges: Array = _adjacency.get(current, [])
|
||||
@@ -1647,11 +1997,37 @@ func _run_a_star(
|
||||
g_score[neighbor] = tentative_cost
|
||||
if closed_set.has(neighbor):
|
||||
closed_set.erase(neighbor)
|
||||
_heap_push(frontier, [neighbor, tentative_cost, tentative_cost + _heuristic(neighbor, goal)])
|
||||
_heap_push(frontier, [neighbor, tentative_cost, tentative_cost + _query_distance_lower_bound(neighbor, goal, connector_scale)])
|
||||
|
||||
if settled_goal_cost < _INFINITE_COST:
|
||||
return _reconstruct_path(came_from, goal)
|
||||
return []
|
||||
|
||||
|
||||
## Every permitted edge costs at least scale * its Manhattan displacement.
|
||||
## A zero-cost connector keeps the existing conservative search unchanged.
|
||||
func _positive_connector_heuristic_scale() -> int:
|
||||
if _connectors.is_empty():
|
||||
return 0
|
||||
var scale := ORTHOGONAL_MOVE_COST
|
||||
for connector_value: Variant in _connectors.values():
|
||||
var connector: Dictionary = connector_value
|
||||
var from_cell: Vector3i = connector[&"from"]
|
||||
var to_cell: Vector3i = connector[&"to"]
|
||||
var displacement := absi(from_cell.x - to_cell.x) + absi(from_cell.y - to_cell.y) + absi(from_cell.z - to_cell.z)
|
||||
if displacement > 0:
|
||||
scale = mini(scale, int(connector[&"cost"]) / displacement)
|
||||
if scale <= 0:
|
||||
return 0
|
||||
return scale
|
||||
|
||||
|
||||
func _query_distance_lower_bound(from_cell: Vector3i, to_cell: Vector3i, connector_scale: int) -> int:
|
||||
if connector_scale <= 0:
|
||||
return _heuristic(from_cell, to_cell)
|
||||
return (absi(from_cell.x - to_cell.x) + absi(from_cell.y - to_cell.y) + absi(from_cell.z - to_cell.z)) * connector_scale
|
||||
|
||||
|
||||
# Query-local heap. Stale improved-cost entries are skipped on removal.
|
||||
# Keep the original f-score / x / y / layer order, including reopened nodes.
|
||||
func _heap_less(left: Array, right: Array) -> bool:
|
||||
@@ -1796,6 +2172,23 @@ func _cell_is_temporarily_blocked(cell: Vector3i) -> bool:
|
||||
return not traffic.is_empty() and bool(traffic[&"temporary_block"])
|
||||
|
||||
|
||||
## Cardinal walking never changes layers. Even an unavailable or blocked
|
||||
## connector must be retained here so the ordinary solver preserves its reason.
|
||||
## Only the absence of any permitted directed crossing proves unreachable.
|
||||
func _has_permitted_layer_crossing(from_layer: int, to_layer: int, allowed_access_ids: Array[StringName]) -> bool:
|
||||
for connector: Dictionary in _connectors.values():
|
||||
var access_id: StringName = connector[&"access_id"]
|
||||
if not access_id.is_empty() and access_id not in allowed_access_ids:
|
||||
continue
|
||||
var from_cell: Vector3i = connector[&"from"]
|
||||
var to_cell: Vector3i = connector[&"to"]
|
||||
if from_cell.z == from_layer and to_cell.z == to_layer:
|
||||
return true
|
||||
if bool(connector[&"bidirectional"]) and to_cell.z == from_layer and from_cell.z == to_layer:
|
||||
return true
|
||||
return false
|
||||
|
||||
|
||||
func _diagnose_no_route_reason(
|
||||
start: Vector3i,
|
||||
goal: Vector3i,
|
||||
|
||||
Reference in New Issue
Block a user