gistlib - create an a star algorithm with object oriented programming in matlab

Here's an implementation of the A* pathfinding algorithm in MATLAB using object-oriented programming.

First, let's define a Node class with properties x, y, fCost, gCost, and hCost. The fCost represents the total estimated cost of the node (including both the gCost and hCost) and is used to prioritize nodes in the open list during the algorithm. The gCost represents the cost of getting from the start node to the current node. The hCost represents the estimated cost of getting from the current node to the goal node.


main.m
classdef Node
    properties
        x
        y
        fCost
        gCost
        hCost
        parent % parent node
        obstacle % flag for whether node is an obstacle
    end
    
    methods
        function obj = Node(x, y)
            obj.x = x;
            obj.y = y;
            obj.fCost = inf;
            obj.gCost = inf;
            obj.hCost = inf;
            obj.parent = [];
            obj.obstacle = false;
        end
        
        function value = getXY(obj)
            value = [obj.x, obj.y];
        end
        
        function setFCost(obj, f)
            obj.fCost = f;
        end
        
        function setGCost(obj, g)
            obj.gCost = g;
        end
        
        function setHCost(obj, h)
            obj.hCost = h;
        end
        
        function setParent(obj, parent)
            obj.parent = parent;
        end
        
        function setObstacle(obj, obstacle)
            obj.obstacle = obstacle;
        end
        
        function value = getFCost(obj)
            value = obj.fCost;
        end
        
        function value = getGCost(obj)
            value = obj.gCost;
        end
        
        function value = getHCost(obj)
            value = obj.hCost;
        end
        
        function value = getParent(obj)
            value = obj.parent;
        end
        
        function value = isObstacle(obj)
            value = obj.obstacle;
        end
    end
end
1452 chars
68 lines

Next, let's define a Pathfinder class with properties startNode, goalNode, openList, and closedList. The openList is a priority queue of nodes to be evaluated, and the closedList is a list of nodes that have already been evaluated.


main.m
classdef Pathfinder
    properties
        startNode
        goalNode
        openList
        closedList
    end
    
    methods
        function obj = Pathfinder(start, goal)
            obj.startNode = start;
            obj.goalNode = goal;
            obj.openList = PriorityQueue();
            obj.closedList = containers.Map('KeyType', 'char', 'ValueType', 'any'); % hash map
            obj.addStartNode();
        end
        
        function addStartNode(obj)
            obj.startNode.setFCost(obj.getHCost(obj.startNode));
            obj.startNode.setGCost(0);
            obj.openList.insert(obj.startNode, obj.startNode.fCost);
        end
        
        function search(obj)
            while ~obj.openList.isEmpty()
                % get node with lowest fCost
                currentNode = obj.openList.get();
                % check if goal node has been reached
                if currentNode == obj.goalNode
                    return;
                end
                % add current node to closed list
                obj.closedList(currentNodeCoord(currentNode)) = currentNode;
                
                % look at adjacent nodes
                adjacentNodes = obj.getAdjacentNodes(currentNode);
                for i = 1:length(adjacentNodes)
                    neighbor = adjacentNodes(i);
                    % if node is an obstacle or already in closed list, skip
                    if neighbor.isObstacle() || isKey(obj.closedList, currentNodeCoord(neighbor))
                        continue;
                    end
                    % calculate new gCost and hCost
                    tentativeG = currentNode.getGCost() + obj.getGCost(currentNode, neighbor);
                    tentativeH = obj.getHCost(neighbor);
                    tentativeF = tentativeG + tentativeH;
                    % check if neighbor is already in open list
                    neighborIndex = obj.openList.find(neighbor);
                    if ~isnan(neighborIndex)
                        % if node is already in open list and new path is better, update node
                        if obj.openList.getPriority(neighborIndex) > tentativeF
                            neighbor.setFCost(tentativeF);
                            neighbor.setGCost(tentativeG);
                            neighbor.setHCost(tentativeH);
                            neighbor.setParent(currentNode);
                            obj.openList.changePriority(neighborIndex, tentativeF);
                        end
                    else
                        % add node to open list if not already there
                        neighbor.setFCost(tentativeF);
                        neighbor.setGCost(tentativeG);
                        neighbor.setHCost(tentativeH);
                        neighbor.setParent(currentNode);
                        obj.openList.insert(neighbor, neighbor.fCost);
                    end
                end
            end
        end
        
        function path = getPath(obj)
            if isempty(obj.goalNode.parent)
                path = [];
            else
                % trace path from goal node back to start node
                currentNode = obj.goalNode;
                path = currentNode.getXY();
                while currentNode ~= obj.startNode
                    currentNode = currentNode.getParent();
                    path = [currentNode.getXY(); path];
                end
            end
        end
        
        function nodes = getAdjacentNodes(obj, node)
            % perform bounds checking to make sure node is within grid
            x = node.x;
            y = node.y;
            nodes = Node.empty;
            if x > 1
                nodes(1) = Node(x-1, y); % left
            end
            if x < size(obj.map, 2)
                nodes(end+1) = Node(x+1, y); % right
            end
            if y > 1
                nodes(end+1) = Node(x, y-1); % down
            end
            if y < size(obj.map, 1)
                nodes(end+1) = Node(x, y+1); % up
            end
        end
        
        % heuristic function used to estimate distance from current node to goal node
        function value = getHCost(obj, node)
            value = sqrt((obj.goalNode.x - node.x)^2 + (obj.goalNode.y - node.y)^2);
        end
        
        % cost of moving from one node to adjacent node
        function value = getGCost(obj, node1, node2)
            value = sqrt((node1.x - node2.x)^2 + (node1.y - node2.y)^2);
        end
    end
end
4540 chars
114 lines

Finally, let's create a PriorityQueue class to serve as the openList in the Pathfinder class. The PriorityQueue maintains a sorted list of nodes to be evaluated based on their fCost.


main.m
classdef PriorityQueue
    properties
        heap
        priority
        size
    end
    
    methods
        function obj = PriorityQueue()
            obj.heap = Node.empty;
            obj.priority = [];
            obj.size = 0;
        end
    
        function insert(obj, node, priority)
            obj.size = obj.size + 1;
            obj.heap(obj.size) = node;
            obj.priority(obj.size) = priority;
            obj.heapifyUp(obj.size);
        end

        function node = get(obj)
            node = obj.heap(1);
            obj.heap(1) = obj.heap(obj.size);
            obj.priority(1) = obj.priority(obj.size);
            obj.size = obj.size - 1;
            obj.heapifyDown(1);
        end
        
        function changePriority(obj, index, priority)
            if index > obj.size || index < 1
                error('Index out of bounds');
            end
            obj.priority(index) = priority;
            if index > 1 && obj.priority(index) < obj.priority(obj.getParent(index))
                obj.heapifyUp(index);
            else
                obj.heapifyDown(index);
            end
        end
        
        function index = find(obj, node)
            index = nan;
            for i = 1:obj.size
                if isequal(obj.heap(i), node)
                    index = i;
                    return;
                end
            end
        end
        
        function value = getPriority(obj, index)
            value = obj.priority(index);
        end

        function value = isEmpty(obj)
            value = obj.size == 0;
        end
        
        function heapifyUp(obj, index)
            parent = obj.getParent(index);
            while index > 1 && obj.priority(index) < obj.priority(parent)
                obj.swap(index, parent);
                index = parent;
                parent = obj.getParent(index);
            end
        end
        
        function heapifyDown(obj, index)
            child = obj.getMinChild(index);
            while child <= obj.size && obj.priority(child) < obj.priority(index)
                obj.swap(child, index);
                index = child;
                child = obj.getMinChild(index);
            end
        end
        
        function swap(obj, i, j)
            temp = obj.heap(i);
            obj.heap(i) = obj.heap(j);
            obj.heap(j) = temp;
            temp = obj.priority(i);
            obj.priority(i) = obj.priority(j);
            obj.priority(j) = temp;
        end
        
        function value = getParent(obj, index)
            value = floor(index/2);
        end
        
        function value = getLeftChild(obj, index)
            value = 2*index;
        end
        
        function value = getRightChild(obj, index)
            value = 2*index + 1;
        end
        
        function value = getMinChild(obj, index)
            leftChild = obj.getLeftChild(index);
            rightChild = obj.getRightChild(index);
            if rightChild > obj.size || obj.priority(leftChild) < obj.priority(rightChild)
                value = leftChild;
            else
                value = rightChild;
            end
        end
    end
end
3193 chars
110 lines

With these classes in place, we can create a main function to run the algorithm on a sample grid.


main.m
function main()
    grid = [0 0 0 0 0 0 0 0;
            0 1 0 0 0 0 0 0;
            0 1 0 1 1 0 0 0;
            0 1 0 1 0 0 0 0;
            0 1 0 1 0 0 0 0;
            0 1 0 1 0 0 0 0;
            0 1 0 1 0 0 1 0;
            0 0 0 0 0 0 0 0];
    start = Node(1, 1);
    goal = Node(8, 8);
    pathfinder = Pathfinder(start, goal);
    % set obstacles
    for i = 1:size(grid, 1)
        for j = 1:size(grid, 2)
            if grid(i,j) == 1
                node = Node(j, i);
                node.setObstacle(true);
                pathfinder.closedList(currentNodeCoord(node)) = node;
            end
        end
    end
    % run algorithm
    pathfinder.search();
    path = pathfinder.getPath();
    disp(path);
end

function coord = currentNodeCoord(node)
    coord = append(num2str(node.x), ',', num2str(node.y));
end
831 chars
32 lines

To run the algorithm, simply call the main function. The resulting path will be a list of [x,y] pairs representing the optimal path from the start node to the goal node, with obstacles avoided.

Note that this implementation uses the Euclidean distance as the heuristic function, and assumes that the grid is a 2D array of 0's and 1's with 1's representing obstacles. If your use case is different, you may need to modify the code accordingly.

create an a star algorithm with object oriented programming in matlab

similar matlab code snippets

related categories