I've written an algorithm that given a set of 2-D co-ordinates,generates the shortest path to traverse a building given that there are certain prohibited points, i.e., the points where the building is present.
I create a list of these points by using a nested while loops and incrementing between the boundaries by 0.01.
I also then remove these points from a set of potential neighbors that is generated while trying to figure out the next best point to go to.
The heuristic I'm using is to minimize the sum of the distances of the potential point to the start and to the goal.
I have 4 different goals (nodes) and iterate through them and changing the starting points for that iteration when needed.
In spite of this, there are some points being included in the end result that are located where the building is supposed to be, essentially making this a non-viable path.
Any help would be appreciated!
import math
import copy
def distance(point1, point2):
'''
Computers distance between two points, two dimensional for now
'''
dist = math.sqrt(math.pow(point1[0] - point2[0], 2) + math.pow(point1[1] - point2[1], 2))
return dist
def check(point1, point2):
'''
Checks if they are the same point or not
'''
for i in range(len(point1)):
if round(point1[i], 1) != round(point2[i], 1):
return False
return True
def round_point(point):
'''
Rounds all the 2 co-ordinates of a point
'''
for i in range(len(point)):
for j in range(len(point[i])):
point[i][j] = round(point[i][j], 2)
return point
def check_in(points, prohibited, visited):
'''
Removes all prohibited points from points and returns
'''
points = round_point(points)
temp = copy.deepcopy(points)
for i in range(len(points)):
if points[i] in prohibited:
#print('b')
temp.remove(points[i])
elif len(visited) != 0:
if points[i] in visited:
#print('c')
temp.remove(points[i])
return temp
def heuristic(points, start, destination):
'''
Calculates heuristic for each point
'''
l = []
for i in range(len(points)):
dist = distance(start, points[i]) + distance(destination, points[i])
l.append(dist)
return l
def neighbours(point):
'''
Finds all the points at a distance of 0.1m from the given point
'''
points = []
for i in range(360):
y = point[1] + 0.1 * math.sin((i * 2 * math.pi) / 180)
x = point[0] + 0.1 * math.cos((i * 2 * math.pi) / 180)
points.append([x, y])
return points
def best(points, prohibited, start, end, visited):
'''
Chooses the point which is least distance that is not prohibited
'''
points = check_in(points, prohibited, visited)
distances = heuristic(points, start, end)
loc = distances.index(min(distances))
return points[loc]
def search():
'''
Main function that searches
'''
nodes = [[2.5, 1.5], [2.5, 8.5], [7.5, 1.5], [7.5, 8.5]]
# Creates a list of prohibited points
prohibited = []
x = 2.5
y = 1.5
visited = []
while x <= 7.5:
while y <= 8.5:
prohibited.append([x, y])
y += 0.01
x += 0.01
# Path to be traversed
path = []
for i in range(len(nodes)):
if i == 0:
# Start has to be origin for first iteration
start = [0, 0]
else:
start = nodes[i - 1]
destination = nodes[i]
# Calculates points for first iteration
points = neighbours(start)
loc = best(points, prohibited, start, destination, visited)
path.append(loc)
visited.append(loc)
while True:
# If the current location is the destination, loop is done
if check(loc, destination): break
else:
points = neighbours(loc)
loc = best(points, prohibited, start, destination, visited)
path.append(loc)
visited.append(loc)
continue
return path
path = search()
print(path)
print(len(path))
