A* search in MATLAB

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 g functions. 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(ch

You say getNeighboursByIndx takes 50% of the total time. That means that you should try to reduce either:

• The number of calls to getNeighboursByIndx

• The time it takes to run getNeighboursByIndx

There's a lot of data I don't have, so trying to improve the number of calls to getNeighboursByIndx is hard. Improving the performance of the function itself however, should be possible.

If you look at the function you'll see that you are doing a lot of equality checks inside a loop, where only one element at a time is checked against some other value. This is a prime example of something that should be vectorized.

If you use & instead of && you can avoid the loop and if's quite simply. From the documentation:


expr1 && expr2 represents a logical AND operation that employs short-circuiting behavior.

Where as &:


A & B performs a logical AND of arrays A and B and returns an array containing elements set to either logical 1 (true) or logical 0 (false).

What you can do here is to create a logical mask, with true and false in the positions corresponding to the logical conditions, and use that mask to populate successor:

mask = neighbours > 0 & neighbours <= nNodes & (mod(neighbours, dim) ~= 1) ...
& ~(workSpace(neighbours) == 1)
successors(mask) = neighbours(mask);

You can do the same simplification quite easily with the for loop for i = 1:numel(childNodes) too.

Also, you don't need to use bsxfun when adding scalars to a vector (I'm assuming nodeIndx is a scalar).

The getNeighboursByIndx function can thus be rewritten as:

function successors = getNeighboursByIndx(workSpace, nodeIndx, nNodes, dim)
delta = [ 1;  dim; -1; -dim];
neighbours = delta + nodeIndx;

mask = neighbours > 0 & neighbours <= nNodes & (mod(neighbours, dim) ~= 1) ...
    & ~(workSpace(neighbours) == 1)
    successors = neighbours(mask);

end

Much faster, much simpler, and much cleaner! =)

StackExchange Code Review Q#114351, answer score: 6