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)
