I have a collection of Points which represents a grid, I'm looking for an algorithm that gets me the shortest distance between point A and B. The catch being any point (excluding A and B) can have an obstacle obstructing the path, and thus must be detoured. The path may not move in diagonals.
For anyone else looking to solve this type of problem, I found these references to be very useful:
http://optlab-server.sce.carleton.ca/POAnimations2007/DijkstrasAlgo.html
This is an excellent spot to use the A* search algorithm, a heuristic search algorithm that finds optimal paths between points very quickly even when there are obstacles present. The idea is to convert the grid into a graph where each cell in the grid is a node and in which there is an edge between any two adjacent cells that aren't obstructed from one another. Once you have this graph, the answer you're looking for is the shortest path in the graph from the start node to the destination node.
In order to use A*, you'll need a heuristic function that "guesses" the distance from any point on the grid to the destination square. One good heuristic for this would be to use the Manhattan distance between the two points.
If you're looking for an easier but still extremely efficient algorithm for finding the shortest path, consider looking into Dijkstra's algorithm, which can be thought of as a simpler version of A*. It's a bit slower than A*, but still runs extremely quickly and guarantees an optimal answer.
Hope this helps!
This is a simple problem that can be solved using Breadth First Search
/**
* computing distance of each cell from the starting cell
* avoiding obstacles, 0 represents obstacle 1 represents non obstacle
* we can take only one step x-1;x+1;y-1;y+1
*/
#include<iostream>
#include<queue>
#include<stdio.h>
using namespace std;
class XY
{
public :
int x;
int y;
};
int main()
{
int grid[8][8] = {
{1,1,1,1,1,1,1,1},
{1,0,0,0,1,1,1,1},
{1,1,0,0,1,1,1,1},
{1,1,0,0,1,1,1,1},
{1,1,1,2,0,1,0,0},
{1,1,1,1,1,1,1,1},
{1,1,1,1,1,1,1,1},
{1,1,1,1,1,1,1,1}
};
int rows = 8;
int cols = 8;
int distance[rows][cols];
for(int m = 0;m<rows;m++)
{
for(int n =0;n<cols;n++)
{
distance[m][n] = -1;
}
}
queue<XY> iterator;
XY xy;
xy.x = 0;
xy.y = 0;
distance[0][0] = 0;
iterator.push(xy);
while(!iterator.empty())
{
xy = iterator.front();
iterator.pop();
//printf("popped %d+%d\n",xy.x ,xy.y);
for(int p = -1;p<2;p++)
{
for(int q = -1;q<2;q++)
{
if(p == q)continue;
int i = xy.x + p;
int j = xy.y + q;
if(i<0)i=0;
if(j<0)j=0;
if(i>rows-1)i=rows-1;
if(j>cols-1)j=cols-1;
if(i== xy.x && j == xy.y)continue;
// printf("i,j = %d,%d\n",i,j);
if(distance[i][j] == -1)
{
// printf("******\n");
if(grid[i][j] != 0)
{
// printf("assigned distance %d to %d+%d\n",distance[xy.x][xy.y] + 1,i,i);
distance[i][j] = distance[xy.x][xy.y] + 1;
XY xyn;
xyn.x = i;
xyn.y = j;
iterator.push(xyn);
// printf("pushed %d+%d\n",xyn.x,xyn.y);
}
}
}
}
}
for(int x = 0;x<rows;x++)
{
for(int y =0;y<cols;y++)
{
printf("%d ",distance[x][y]);
}
printf("\n");
}
return 0;
}
I believe that the given problem can be solved by Breadth First Search(BFS) as mentioned earlier. Establishing a problem statement is important. So let's describe the problem and then we move to the solution part of it.
Problem Description:
You are in charge of preparing a recently purchased lot for one of a new building. The lot is covered with trenches and has a single obstacle that needs to be taken down before the foundation can be prepared for the building. The demolition robot must remove the obstacle before progress can be made on the building. Write an algorithm to determine the minimum distance required for the demolition robot to remove the obstacle.
Assumptions:
- The lot is flat, except for trenches, and can be represented as a 2-D grid.
- The demolition robot must start from the top left corner of the lot, which is always flat and can move one block up, down, right or left at a time.
- The demolition robot cannot enter trenches and cannot leave the lot.
- The flat areas are represented as 1, areas with trenches as 0 and obstacle by 9.
Output:
Return an integer representing the minimum distance traversed to remove the obstacle else return -1.
Solution
from collections import defaultdict
from collections import deque
def is_valid(i, j, m, n, matrix, visited):
if i >= 0 and j >= 0 \
and i < m and j < n \
and visited[i][j] is False \
and matrix[i][j] in (1, 9):
return True
return False
def remove_obstacle(matrix, m, n):
queue = deque()
i = 0
j = 0
queue.append((i, j, 0))
visited = [[False for _ in range(n)] for _ in range(m)]
distance_matrix = [[100 for _ in range(n)] for _ in range(m)]
visited[i][j] = True
while queue:
i, j, distance = queue.popleft()
distance_matrix[i][j] = distance
visited[i][j] = True
if matrix[i][j] == 9:
return distance
new_distance = distance + 1
if is_valid(i + 1, j, m, n, matrix, visited):
queue.append((i + 1, j, new_distance))
if is_valid(i - 1, j, m, n, matrix, visited):
queue.append((i - 1, j, new_distance))
if is_valid(i, j + 1, m, n, matrix, visited):
queue.append((i, j + 1, new_distance))
if is_valid(i, j - 1, m, n, matrix, visited):
queue.append((i, j - 1, new_distance))
return -1
if __name__ == '__main__':
m = 5
n = 4
l = [
[1, 1, 1, 1],
[0, 1, 1, 1],
[0, 1, 0, 1],
[1, 1, 9, 1],
[0, 0, 1, 1]
]
bfs = remove_obstacle(l, m, n)
assert bfs == 5
m = 3
n = 3
l = [
[1, 0, 0],
[1, 0, 0],
[1, 9, 1]
]
bfs = remove_obstacle(l, m, n)
assert bfs == 3
来源：https://stackoverflow.com/questions/5303538/algorithm-to-find-the-shortest-path-with-obstacles