def _check_status(self, b_obj): gs = GridSpace(self.blackboard.grid) self.start_state = gs.get_state_coords(self.start_coords) self.target_state = gs.get_state_coords(self.target_coords) go = gs.can_go(self.start_state, self.target_state) if not go: log.msg('cannot go between %s %s' % (self.start_state, self.target_state)) return Status.failure if not self.was_at_target: self.was_at_target = self.target_state.vertical_center_in(b_obj.position) if self.target_state.base_in(b_obj.aabb) and self.target_state.touch_platform(b_obj.position): return Status.success return Status.running
def maybe_commander(self, entity): if self.world.commander.eid != entity.eid: return gpos = entity.grid_position block = self.world.grid.standing_on_block( AABB.from_player_coords(entity.position)) if block is None: return if self.world.commander.last_block is not None and self.world.commander.last_block == block: return self.world.commander.last_block = block lpos = self.world.commander.last_possition in_nodes = self.world.navgrid.graph.has_node(block.coords) gs = GridSpace(self.world.grid, block=block) msg = "P in nm %s nm nodes %d\n" % \ (in_nodes, self.world.navgrid.graph.node_count) msg += "gs_stand %s\n" % str(gs.bb_stand) msg += str(block) + '\n' msg += str(block.grid_bounding_box) try: msg += "\nsucessors %s" % str(self.world.navgrid.graph.get_succ(block.coords)) except: pass if lpos is not None: gsl = GridSpace(self.world.grid, coords=lpos) if gsl.can_stand_on: pass else: gsl = GridSpace( self.world.grid, coords=(lpos[0], lpos[1] - 1, lpos[2])) if gsl.can_stand_on: lpos = gsl.coords if not(gsl.bb_stand is None or gs.bb_stand is None): msg += "\ncost from %s to %s %s\n" % \ (lpos, block.coords, self.world.navgrid.graph.get_edge(lpos, block.coords)) msg += "last stand %s now stand %s from %s to %s\n" % \ (gsl.can_stand_on, gs.can_stand_on, gsl.bb_stand, gs.bb_stand) if gsl.can_go_between(gs, debug=True): msg += "can go True with cost %s\n" % gsl.edge_cost else: msg += "can go False\n" msg += "can stand between %s intersection %s" % ( gsl.can_stand_between(gs, debug=True), gsl.intersection) log.msg(msg) self.world.commander.last_possition = gpos
def __init__(self, dimension, start_coords, goal_coords): self.goal_coords = goal_coords self.goal_node = PathNode(goal_coords) self.start_node = PathNode(start_coords) self.astar = AStarAlgo(graph=GridSpace(dimension.grid), start_node=self.start_node, goal_node=self.goal_node, is_goal=self.is_goal, heuristics=self.heuristics) self.t_start = time.time() self.path = None
def __init__(self, dimension=None, start_coords=None, end_coords=None, path_max=config.PATHFIND_MAX, estimate=True): self.t_start = time.time() self.dimension = dimension self.grid = dimension.grid self.start_node = PathNode(start_coords) self.goal_node = PathNode(end_coords) self.gridspace = GridSpace(self.grid) # keep max_cost between the configured values conf_max, conf_min = config.PATHFIND_MAX, config.PATHFIND_MIN values = [conf_min, path_max, conf_max] self.max_cost = list(sorted(values))[1] self.path = None self.closed_set = set() goal_state = self.gridspace.get_state_coords(end_coords) if goal_state.can_stand or goal_state.can_hold or estimate: self.open_heap = [self.start_node] self.open_set = set([self.start_node]) else: self.open_heap = [] self.open_set = set([]) self.start_node.set_score(0, self.heuristic_cost_estimate( self.start_node, self.goal_node)) self.iter_count = 0 self.best = self.start_node self.estimate = estimate self.excessive = config.PATHFIND_EXEC_TIME_LIMIT self.distance = self.start_node.coords.distance(self.goal_node.coords) self.start = time.time()
def check_status(self, b_obj): if time() - self.start_time >= self.max_time: return Status.failure gs = GridSpace(self.world.grid) self.start_state = gs.get_state_coords(self.start_coords) self.target_state = gs.get_state_coords(self.target_coords) go = gs.can_go(self.start_state, self.target_state) if not go: log.msg('Cannot go between %s and %s' % (self.start_state, self.target_state)) return Status.failure if not self.was_at_target: self.was_at_target = self.target_state.vertical_center_in( b_obj.position) if (self.target_state.base_in(b_obj.aabb) and self.target_state.touch_platform(b_obj.position)): return Status.success return Status.running
def check_status(self, b_obj): bb_stand = self.target_space.bb_stand elev = bb_stand.min_y - b_obj.aabb.min_y gs = GridSpace(self.world.grid, bb=b_obj.aabb) if not self.target_space._can_stand_on(): self.world.grid.navgrid.delete_node(self.target_space.coords) log.msg('CANNOT STAND ON %s' % self.target_space) return Status.failure if b_obj.horizontally_blocked and not gs.can_go_between(self.target_space, debug=True): log.msg('CANNOT GO BETWEEN %s AND %s' % (b_obj.aabb, self.target_space)) return Status.failure if self.bot.is_on_ladder(b_obj) or self.bot.is_in_water(b_obj): if b_obj.position_grid == self.target_space.coords: return Status.success if b_obj.aabb.horizontal_distance(bb_stand) < self.bot.current_motion(b_obj): self.was_at_target = True if fops.eq(elev, 0): return Status.success if b_obj.horizontally_blocked and b_obj.on_ground: if not gs.can_go(self.target_space): log.msg("I am stuck, let's try again? vels %s" % str(b_obj.velocities)) return Status.failure return Status.running
def __init__(self, dimension=None, start_coords=None, end_coords=None, max_cost=config.PATHFIND_LIMIT): self.dimension = dimension self.grid = dimension.grid self.start_node = PathNode(start_coords) self.goal_node = PathNode(end_coords) self.gridspace = GridSpace(self.grid) self.max_cost = max_cost self.path = None self.closed_set = set() goal_state = self.gridspace.get_state_coords(end_coords) if goal_state.can_stand or goal_state.can_hold: self.open_heap = [self.start_node] self.open_set = set([self.start_node]) else: self.open_heap = [] self.open_set = set([]) self.start_node.set_score(0, self.heuristic_cost_estimate(self.start_node, self.goal_node)) self.iter_count = 0 self.t_start = time.time()
def positions_to_dig(self, coords): gs = GridSpace(self.grid) return list(gs.positions_to_dig(coords))
class AStar(object): def __init__(self, dimension=None, start_coords=None, end_coords=None, max_cost=config.PATHFIND_LIMIT): self.dimension = dimension self.grid = dimension.grid self.start_node = PathNode(start_coords) self.goal_node = PathNode(end_coords) self.gridspace = GridSpace(self.grid) self.max_cost = max_cost self.path = None self.closed_set = set() goal_state = self.gridspace.get_state_coords(end_coords) if goal_state.can_stand or goal_state.can_hold: self.open_heap = [self.start_node] self.open_set = set([self.start_node]) else: self.open_heap = [] self.open_set = set([]) self.start_node.set_score(0, self.heuristic_cost_estimate(self.start_node, self.goal_node)) self.iter_count = 0 self.t_start = time.time() def reconstruct_path(self, current): nodes = [] nodes.append(current) while current.parent is not None: nodes.append(current.parent) current = current.parent nodes.reverse() return nodes def get_edge_cost(self, node_from, node_to): return config.COST_DIRECT def neighbours(self, node): for state in self.gridspace.neighbours_of(node.coords): if state.coords not in self.closed_set: yield PathNode(state.coords) def heuristic_cost_estimate(self, start, goal): adx = abs(start.coords.x - goal.coords.x) adz = abs(start.coords.z - goal.coords.z) h_diagonal = min(adx, adz) h_straight = adx + adz h = config.COST_DIAGONAL * h_diagonal + config.COST_DIRECT * (h_straight - 2 * h_diagonal) return h def next(self): self.iter_count += 1 if not self.open_set: log.msg("time consumed %s sec, made %d iterations" % (time.time() - self.t_start, self.iter_count)) log.err("Did not find path between %s and %s" % (self.start_node.coords, self.goal_node.coords)) raise StopIteration() x = heapq.heappop(self.open_heap) if x == self.goal_node: self.path = Path(dimension=self.dimension, nodes=self.reconstruct_path(x)) self.gridspace = None log.msg( "finished in %s sec, length %d, made %d iterations" % (time.time() - self.t_start, len(self.path.nodes), self.iter_count) ) log.msg("nodes %s" % self.path.nodes) raise StopIteration() self.open_set.remove(x) self.closed_set.add(x.coords) for y in self.neighbours(x): if y.coords in self.closed_set: continue tentative_g_core = x.g + self.get_edge_cost(x, y) if y not in self.open_set or tentative_g_core < y.g: y.set_score(tentative_g_core, self.heuristic_cost_estimate(y, self.goal_node)) y.parent = x if y not in self.open_set: heapq.heappush(self.open_heap, y) self.open_set.add(y) if y.step > self.max_cost: log.err( "Finding path over limit between %s and %s" % (self.start_node.coords, self.goal_node.coords) ) raise StopIteration()
class AStar(object): #TODO: explore limiting by execution time rather than by path distance def __init__(self, dimension=None, start_coords=None, end_coords=None, path_max=config.PATHFIND_MAX, estimate=True): self.t_start = time.time() self.dimension = dimension self.grid = dimension.grid self.start_node = PathNode(start_coords) self.goal_node = PathNode(end_coords) self.gridspace = GridSpace(self.grid) # keep max_cost between the configured values conf_max, conf_min = config.PATHFIND_MAX, config.PATHFIND_MIN values = [conf_min, path_max, conf_max] self.max_cost = list(sorted(values))[1] self.path = None self.closed_set = set() goal_state = self.gridspace.get_state_coords(end_coords) if goal_state.can_stand or goal_state.can_hold or estimate: self.open_heap = [self.start_node] self.open_set = set([self.start_node]) else: self.open_heap = [] self.open_set = set([]) self.start_node.set_score(0, self.heuristic_cost_estimate( self.start_node, self.goal_node)) self.iter_count = 0 self.best = self.start_node self.estimate = estimate self.excessive = config.PATHFIND_EXEC_TIME_LIMIT self.distance = self.start_node.coords.distance(self.goal_node.coords) self.start = time.time() def get_edge_cost(self, node_from, node_to, x_neighbors=None, y_neighbors=None): return config.COST_DIRECT def neighbours(self, node): for state in self.gridspace.neighbours_of(node.coords): if state.coords not in self.closed_set: yield PathNode(state.coords) def heuristic_cost_estimate(self, start, goal): """Takes a path node, and tries to estimate the cost to the goal.""" # y = start.coords.y - goal.coords.y # adx = abs(start.coords.x - goal.coords.x) # adz = abs(start.coords.z - goal.coords.z) # # fall, rise = (abs(y), 0) if y < 0 else (0, abs(y)) # h_diagonal = min(adx, adz) # h_straight = adx + adz # h = (config.COST_DIAGONAL * h_diagonal + # config.COST_DIRECT * (h_straight - 2 * h_diagonal) + # config.COST_FALL * fall + # config.COST_JUMP * rise) vertical = start.coords.y - goal.coords.y if vertical < 0: vertical = abs(vertical) * config.COST_FALL else: vertical = vertical * config.COST_JUMP distance = start.coords.distance(goal.coords) h = distance + vertical return h def _excessive_path(self, start): """Cheap evalutation of whether this path is excessive, only usable on a scored node.""" # Pathfinding is the most expensive operation in a tick. # self.excessive should ideally be less than 1/20th of a second, but # can realistically go higher. # this means the pathfinding effectiveness of the bot is affected by # the speed of the machine it's on -- and by it's cost. return time.time() - self.start > self.excessive # excessive = start.step > self.distance * self.excessive # if excessive: # debug and log.msg(msg % (start.h, start.g)) # return excessive def report(self): nodes = '' if not self.path: path = '<PATH NOT FOUND>' else: estimated = '' if self.path.estimated else "(estimated)" path = 'path length %s %s' % (self.best.step, estimated) nodes = 'Nodes: %s' % self.path.nodes msg = "Finished in %s sec, %s iterations, %s" log.msg(msg % (time.time() - self.t_start, self.iter_count, path)) debug and log.msg(nodes) def finish(self): estimated = self.best.coords != self.goal_node.coords if not estimated or (estimated and self.estimate): self.path = Path(dimension=self.dimension, nodes=self.best.path, estimated=estimated) self.gridspace = None self.report() def next(self): self.iter_count += 1 if not self.open_set: self.finish() raise StopIteration() x = heapq.heappop(self.open_heap) if x.coords == self.goal_node.coords: self.best = x self.finish() raise StopIteration() self.open_set.remove(x) self.closed_set.add(x.coords) x_neighbours = self.neighbours(x) for y in x_neighbours: if y.coords in self.closed_set: continue tentative_g_core = x.g + self.get_edge_cost(x, y, x_neighbours) if y not in self.open_set or tentative_g_core < y.g: y.set_score(tentative_g_core, self.heuristic_cost_estimate(y, self.goal_node)) if y.h < self.best.h: self.best = y y.parent = x if y not in self.open_set: heapq.heappush(self.open_heap, y) self.open_set.add(y) if self._excessive_path(y): msg = "Find path timed out at %s steps between %s and %s" log.msg(msg % (y.step, self.start_node.coords, self.goal_node.coords)) self.finish() raise StopIteration() if y.step > self.max_cost: msg = "Find path over limit %s between %s and %s" log.msg(msg % (self.max_cost, self.start_node.coords, self.goal_node.coords)) self.finish() raise StopIteration()