I have implemented A* search in MATLAB, but I am looking for ways to increase the speed and optimize it. I have tried using a priority queue but I found it doesn't work that well, so I am using a different way to implement the search. I will explain the details, so it might get a bit long. I appreciate your patience.
The grid that I am performing the search on is called
workSpace. I am using the cell indexes, so by checking
workSpace[inx] == 0 I can tell if the cell is occupied or not,
0 -> free and
1 -> occupied. This is the main body of the A*. I am passing the work space, the index for the start cell, and the index of the goal cell. As well as the heauristic
h, and cost
nNodes is the total number of nodes, which I use to find the successor nodes.
function [visitedNodes, f, cameFrom] = aStar(workSpace, startIndx, goalIndx, nNodes, h, g) dim = sqrt(nNodes); node = startIndx; cameFrom(nNodes, 1) = 0; cameFrom(node) = node; closedSet(nNodes, 1) = 0; openSet(nNodes, 1) = 0; costSoFar(nNodes, 1) = 0; f = inf(nNodes, 1); openSet(node) = 1; costSoFar(node) = 0; f(node) = 0; visitedNodes = 0; while sum(openSet) ~= 0 [~, minFIndx] = min(f); f(minFIndx) = inf; currentNode = minFIndx; if currentNode == goalIndx disp('goal Found') return end openSet(currentNode) = 0; closedSet(currentNode) = 1; childNodes = search.getNeighboursByIndx(workSpace, currentNode, nNodes, dim); for i = 1:numel(childNodes) if closedSet(childNodes(i)) == 1 continue end tentativeGScore = costSoFar(currentNode) + g(currentNode); if openSet(childNodes(i)) ~= 1 || tentativeGScore < costSoFar(childNodes(i)) cameFrom(childNodes(i)) = currentNode; costSoFar(childNodes(i)) = tentativeGScore; f(childNodes(i)) = costSoFar(childNodes(i)) + h(childNodes(i)); if openSet(childNodes(i)) == 0 openSet(childNodes(i)) = 1; end end end end end
As I mentioned, I am not using a priority queue. I am using the below mechanism to simulate the priority queue.
[~, minFIndx] = min(f); f(minFIndx) = inf; currentNode = minFIndx;
min searches through
f, which is
f = g+h, and returns the index of the lowest cell and then I set the value of that cell to
inf so it doesn't come up again in the next round. I use the below function to get the successors, it is also very simple:
function successors = getNeighboursByIndx(workSpace, nodeIndx, nNodes, dim) delta = [ 1; dim;... -1; -dim]; neighbours = bsxfun(@plus, delta, nodeIndx); % Create the successor matrix and check if all neighbours are within the grid/freeSpace % if not, don't add them to the successors matrix successors(4, 1) = 0; for i=1:4 % (1) the index can't be negative % (2) the index should be smaller than the total number of nodes in the grid % (3) the index should not be on the wall around the working space % (1) (2) (3) if (neighbours(i) > 0) && (neighbours(i) <= nNodes) && (mod(neighbours(i), dim) ~= 1) if ~(workSpace(neighbours(i)) == 1) % if the index is not in the wallSpace(1) it is in the freeSpace(0) successors(i) = neighbours(i); end end end % remove the neighbours that are not eligible as a successor % again, `successors` contains the indexes of neighbouring cells successors(successors == 0) = ; end
This function is very simple. I use only 4-neighbours, Up(1)-Down(-1) | Right(dim)-Left(-dim).
The current algorithm completes the search on 1681 cells in around
0.05 seconds. The profiler is telling me that the
getNeighboursByIndx function takes almost 50% of the total time. In this specific work space it gets called 411 times. Please let me know if it is not clear and you need more information.
Edit: I am running the search on a dynamic
workSpace, the state of a cell is not static and it might change from a free cell to an occupied one or vice-versa. This is the algorithm I am using:
set all cells in workSpace to free `0` while True: Check for changes in the workSpace if thereIsAChange update the workSpace perform the search on the new workSpace end end
I don't change the state of the
workSpace during the search. Is it a bad idea?