def get_cohesion_heading(agent_name, neighbours): # initialise the heading vector that will hold the separation heading cohesion_heading_vector = [0, 0, 0] # get the number of neighbouring agents in the neighbours list [find the length of the list] number_of_neighbours = len(neighbours) # IF we have 1 or more agents in the neighbours list if number_of_neighbours > 0: # get the position of the agent called agent_name agent_pos = agent_name.get_agent_position() # initialise a a vector to hold the average of all the neighbours positions to [0, 0, 0] neighbours_average_vector = [0, 0, 0] #FOR every neighbour in the neighbours list for neighbour in neighbours: # get the neighbours position neighbour_pos = neighbour.get_agent_position() # add it to the vector to hold the average positions neighbours_average_vector = vectorMath.vector_add( neighbours_average_vector, neighbour_pos) # calculate the averaged neighbours position neighbours_average_vector[ 0] = neighbours_average_vector[0] / number_of_neighbours neighbours_average_vector[ 1] = neighbours_average_vector[1] / number_of_neighbours neighbours_average_vector[ 2] = neighbours_average_vector[2] / number_of_neighbours # calculate the cohesion_heading_vector from the position of the agent called agent_name to the averaged neighbours position cohesion_heading_vector = vectorMath.get_vector_between_points( agent_pos, neighbours_average_vector) # normalise the cohesion_heading_vector cohesion_heading_vector = vectorMath.vector_normalise( cohesion_heading_vector) return cohesion_heading_vector
def get_separation_heading(agent_name, neighbours): # initialise the heading vector that will hold the separation heading separation_heading_vector = [0, 0, 0] heading_vector = [0, 0, 0] # get the number of neighbouring agents in the neighbours list [find the length of the list] number_of_neighbours = len(neighbours) # IF we have 1 or more agents in the neighbours list if number_of_neighbours > 0: # get the position of the agent called agent_name agent_pos = agent_name.get_agent_position() # FOR every neighbour in the neighbours list for neighbour in neighbours: # get the position of the neighbour neighbour_pos = neighbour.get_agent_position() # calculate the heading vector from the neighbours position to the position of our agent heading_vector = vectorMath.get_vector_between_points( neighbour_pos, agent_pos) # normalise the heading vector to give it a size of 1 [needed so averaging the vector will work as expected heading_vector = vectorMath.vector_normalise(heading_vector) # add this heading vector to the separation_heading_vector separation_heading_vector = vectorMath.vector_add( separation_heading_vector, heading_vector) # calculate the averaged separation_heading_vector separation_heading_vector[ 0] = separation_heading_vector[0] / number_of_neighbours separation_heading_vector[ 1] = separation_heading_vector[1] / number_of_neighbours separation_heading_vector[ 2] = separation_heading_vector[2] / number_of_neighbours # normalise the separation_heading_vector separation_heading_vector = vectorMath.vector_normalise( separation_heading_vector) return separation_heading_vector
def get_cohesion_heading(agent_name, neighbours): # initialise the heading vector that will hold the separation heading cohesion_heading_vector = [0, 0, 0] # get the number of neighbouring agents in the neighbours list [find the length of the list] number_of_neighbours = len(neighbours) # IF we have 1 or more agents in the neighbours list if number_of_neighbours > 0: # get the position of the agent called agent_name agent_pos = agent_name.get_agent_position() # initialise a a vector to hold the average of all the neighbours positions to [0, 0, 0] neighbours_average_vector = [0, 0, 0] #FOR every neighbour in the neighbours list for neighbour in neighbours: # get the neighbours position neighbour_pos = neighbour.get_agent_position() # add it to the vector to hold the average positions neighbours_average_vector = vectorMath.vector_add(neighbours_average_vector, neighbour_pos) # calculate the averaged neighbours position neighbours_average_vector[0] = neighbours_average_vector[0] / number_of_neighbours neighbours_average_vector[1] = neighbours_average_vector[1] / number_of_neighbours neighbours_average_vector[2] = neighbours_average_vector[2] / number_of_neighbours # calculate the cohesion_heading_vector from the position of the agent called agent_name to the averaged neighbours position cohesion_heading_vector = vectorMath.get_vector_between_points(agent_pos, neighbours_average_vector) # normalise the cohesion_heading_vector cohesion_heading_vector = vectorMath.vector_normalise(cohesion_heading_vector) return cohesion_heading_vector
def get_separation_heading(agent_name, neighbours): # initialise the heading vector that will hold the separation heading separation_heading_vector = [0, 0, 0] heading_vector = [0, 0, 0] # get the number of neighbouring agents in the neighbours list [find the length of the list] number_of_neighbours = len(neighbours) # IF we have 1 or more agents in the neighbours list if number_of_neighbours > 0: # get the position of the agent called agent_name agent_pos = agent_name.get_agent_position() # FOR every neighbour in the neighbours list for neighbour in neighbours: # get the position of the neighbour neighbour_pos = neighbour.get_agent_position() # calculate the heading vector from the neighbours position to the position of our agent heading_vector = vectorMath.get_vector_between_points(neighbour_pos, agent_pos) # normalise the heading vector to give it a size of 1 [needed so averaging the vector will work as expected heading_vector = vectorMath.vector_normalise(heading_vector) # add this heading vector to the separation_heading_vector separation_heading_vector = vectorMath.vector_add(separation_heading_vector, heading_vector) # calculate the averaged separation_heading_vector separation_heading_vector[0] = separation_heading_vector[0] / number_of_neighbours separation_heading_vector[1] = separation_heading_vector[1] / number_of_neighbours separation_heading_vector[2] = separation_heading_vector[2] / number_of_neighbours # normalise the separation_heading_vector separation_heading_vector = vectorMath.vector_normalise(separation_heading_vector) return separation_heading_vector
def get_goal_heading(agent): # initialise the heading vector that will hold the goal heading goal_heading_vector = [0, 0, 0] # if we have reached the end of the path or otherwise require a new goal do it here if(pathfinding.need_new_goal(agent.get_goal(),agent.get_rounded_pos())): agent.new_goal() # check if we need to start heading towards the next step in the path agent.check_step() #get goal vector from our current position and the next spot in our path goal_heading_vector = vectorMath.get_vector_between_points(agent.get_agent_position(),agent.get_current_step()) # normalise goal vector goal_heading_vector = vectorMath.vector_normalise(goal_heading_vector) return goal_heading_vector
def get_goal_heading(agent): # initialise the heading vector that will hold the goal heading goal_heading_vector = [0, 0, 0] # if we have reached the end of the path or otherwise require a new goal do it here if (pathfinding.need_new_goal(agent.get_goal(), agent.get_rounded_pos())): agent.new_goal() # check if we need to start heading towards the next step in the path agent.check_step() #get goal vector from our current position and the next spot in our path goal_heading_vector = vectorMath.get_vector_between_points( agent.get_agent_position(), agent.get_current_step()) # normalise goal vector goal_heading_vector = vectorMath.vector_normalise(goal_heading_vector) return goal_heading_vector
def pathfind(start,goal,path): global open_list global node_list reset(path) #could have a bounds check here if i wanted #if start.x<0 : start.x = 0 # Add the starting square (or node) to the open list. open_list.insert(0,node_list[start[0]][start[2]]) # Repeat the following: searching = True while(searching): #print("in open_list") # Pop the first item off the open list. (as it's the one with lowest f) pNode = open_list.pop(0) pNode.setClosed() # Switch it to the closed list. # For each of the 8 squares adjacent to this current node for xIter in range (-1, 2): #for -1,0,1 for zIter in range (-1, 2): iX=pNode.getX()+xIter iZ=pNode.getZ()+zIter if ((iZ<0 or iZ>map_height-1) or (iX<0 or iX>map_width-1)): #if out of array bounds #print "out of bounds" pass else: cNode = node_list[iX][iZ] # If it is not walkable or if it is on the closed list, ignore it. if ( (xIter==0 and zIter==0) or (cNode.isWalkable() == False) or (cNode.whichList() == "closed") ) : #print "ignore" pass elif (cornersOK(pNode.getX(),pNode.getZ(),xIter,zIter) == True): #Otherwise do the following. # If it isnt on the open list, add it to the open list. set the parent also if cNode.whichList() != "open" : #print("new node") cNode.setOpen() open_list.append(cNode) cNode.setParent(pNode.getpos()) # Record the F, G, and H costs of the square. #G = the movement cost to move from the starting point A to a given square on the grid, following the path generated to get there. g = pNode.getG() + math.sqrt((abs(xIter)+abs(zIter))) #H = the estimated movement cost to move from that given square on the grid to the final destination, point B. h = vectorMath.get_vector_length(vectorMath.get_vector_between_points(goal,cNode.getpos())) #F = The combined total of g and h f = g + h #print(f,g,h) cNode.setFGH(f,g,h) else:# If it is on the open list already, # using G cost as the measure. A lower G cost means that this is a better path. #G = the movement cost to move from the starting point A to a given square on the grid, following the path generated to get there. g = pNode.getG() + math.sqrt((abs(xIter)+abs(zIter))) # check to see if this path to that square is better, if g < cNode.getG(): # If so, change the parent of the square to the current square, cNode.setParent(pNode.getpos()) # and recalculate the F score of the square. f = g + h cNode.setFGH(f,g,h) else: #print ("bad corner") pass # end searching through neighbours # resort the list to account for the change. # sorting by h can be faster computatationally, but is less # likely to give the fastest path open_list.sort(key=lambda x: x.m_f) # Stop when you: # Add the target square to the closed list, in which case the path has been found if node_list[goal[0]][goal[2]].whichList() == 'closed' : #print("goal found") searching = False # Fail to find the target square, and the open list is empty. In this case, there is no path. if len(open_list) == 0 : #print("open list empty") searching = False return start # no path so path to finish is where we are. we will not move. # Save the path. Working backwards from the target square, go from each square to its parent square until you reach the starting square. That is your path. path.insert(0,node_list[goal[0]][goal[2]].getpos()) while path[0] != start: p_pos = node_list[path[0][0]][path[0][2]].getParent() path.insert(0,p_pos)
def pathfind(start, goal, path): global open_list global node_list reset(path) #could have a bounds check here if i wanted #if start.x<0 : start.x = 0 # Add the starting square (or node) to the open list. open_list.insert(0, node_list[start[0]][start[2]]) # Repeat the following: searching = True while (searching): #print("in open_list") # Pop the first item off the open list. (as it's the one with lowest f) pNode = open_list.pop(0) pNode.setClosed() # Switch it to the closed list. # For each of the 8 squares adjacent to this current node for xIter in range(-1, 2): #for -1,0,1 for zIter in range(-1, 2): iX = pNode.getX() + xIter iZ = pNode.getZ() + zIter if ((iZ < 0 or iZ > map_height - 1) or (iX < 0 or iX > map_width - 1)): #if out of array bounds #print "out of bounds" pass else: cNode = node_list[iX][iZ] # If it is not walkable or if it is on the closed list, ignore it. if ((xIter == 0 and zIter == 0) or (cNode.isWalkable() == False) or (cNode.whichList() == "closed")): #print "ignore" pass elif (cornersOK( pNode.getX(), pNode.getZ(), xIter, zIter) == True): #Otherwise do the following. # If it isnt on the open list, add it to the open list. set the parent also if cNode.whichList() != "open": #print("new node") cNode.setOpen() open_list.append(cNode) cNode.setParent(pNode.getpos()) # Record the F, G, and H costs of the square. #G = the movement cost to move from the starting point A to a given square on the grid, following the path generated to get there. g = pNode.getG() + math.sqrt( (abs(xIter) + abs(zIter))) #H = the estimated movement cost to move from that given square on the grid to the final destination, point B. h = vectorMath.get_vector_length( vectorMath.get_vector_between_points( goal, cNode.getpos())) #F = The combined total of g and h f = g + h #print(f,g,h) cNode.setFGH(f, g, h) else: # If it is on the open list already, # using G cost as the measure. A lower G cost means that this is a better path. #G = the movement cost to move from the starting point A to a given square on the grid, following the path generated to get there. g = pNode.getG() + math.sqrt( (abs(xIter) + abs(zIter))) # check to see if this path to that square is better, if g < cNode.getG(): # If so, change the parent of the square to the current square, cNode.setParent(pNode.getpos()) # and recalculate the F score of the square. f = g + h cNode.setFGH(f, g, h) else: #print ("bad corner") pass # end searching through neighbours # resort the list to account for the change. # sorting by h can be faster computatationally, but is less # likely to give the fastest path open_list.sort(key=lambda x: x.m_f) # Stop when you: # Add the target square to the closed list, in which case the path has been found if node_list[goal[0]][goal[2]].whichList() == 'closed': #print("goal found") searching = False # Fail to find the target square, and the open list is empty. In this case, there is no path. if len(open_list) == 0: #print("open list empty") searching = False return start # no path so path to finish is where we are. we will not move. # Save the path. Working backwards from the target square, go from each square to its parent square until you reach the starting square. That is your path. path.insert(0, node_list[goal[0]][goal[2]].getpos()) while path[0] != start: p_pos = node_list[path[0][0]][path[0][2]].getParent() path.insert(0, p_pos)