def cost_func(path):
""" This cost function just returns the linear distance between the nodes (assumed to be spatial
nodes)"""
cost = 0
for i in range(1, len(path)):
cost += VecMath.length(
VecMath.sub(path[i - 1].position, path[i].position))
return cost
def linear_distance_heuristic(path, goal):
return VecMath.length(VecMath.sub(goal.position, path[-1].position))
