create an a star algorithm with object oriented in matlab

main.m
% Node class
classdef Node
    properties
        x
        y
        f
        g
        h
        parent
        valid
    end
    
    methods
        function obj = Node(x, y)
            obj.x = x;
            obj.y = y;
            obj.f = 0;
            obj.g = 0;
            obj.h = 0;
            obj.parent = [];
            obj.valid = true;
        end
        
        function setParent(obj, parent)
            obj.parent = parent;
        end
        
        function setValidity(obj, valid)
            obj.valid = valid;
        end
        
        function heuristic(obj, goal)
            obj.h = sqrt((obj.x-goal.x)^2 + (obj.y-goal.y)^2);
        end
        
        function manhattan(obj, goal)
            obj.h = abs(obj.x-goal.x) + abs(obj.y-goal.y);
        end
        
        function cost(obj, parent)
            obj.g = parent.g + sqrt((obj.x-parent.x)^2 + (obj.y-parent.y)^2);
            obj.f = obj.g + obj.h;
        end
    end
end

% Grid class
classdef Grid
    properties
        nodes
        width
        height
    end
    
    methods
        function obj = Grid(width, height)
            obj.width = width;
            obj.height = height;
            obj.nodes = Node.empty(width, height);
            for x = 1:width
                for y = 1:height
                    obj.nodes(x,y) = Node(x,y);
                end
            end
        end
        
        function node = getNode(obj, x, y)
            if x < 1 || x > obj.width || y < 1 || y > obj.height
                node = [];
            else
                node = obj.nodes(x,y);
            end
        end
        
        function setObstacle(obj, x, y)
            node = obj.getNode(x,y);
            if ~isempty(node)
                node.setValidity(false);
            end
        end
        
        function reset(obj)
            for x = 1:obj.width
                for y = 1:obj.height
                    obj.nodes(x,y).f = 0;
                    obj.nodes(x,y).g = 0;
                    obj.nodes(x,y).h = 0;
                    obj.nodes(x,y).parent = [];
                end
            end
        end
    end
end

% A* algorithm class
classdef AStar
    properties
        openList
        closedList
        grid
        start
        goal
    end
    
    methods
        function obj = AStar(grid, start, goal)
            obj.grid = grid;
            obj.start = start;
            obj.goal = goal;
            obj.openList = [start];
            obj.closedList = [];
        end
        
        function path = findPath(obj)
            path = [];
            current = obj.start;
            while ~isempty(current) && ~(current.x == obj.goal.x && current.y == obj.goal.y)
                % move current node from open to closed list
                obj.openList(obj.openList == current) = [];
                obj.closedList = [obj.closedList current];
                % expand neighbors
                neighbors = obj.getNeighbors(current);
                for i = 1:length(neighbors)
                    neighbor = neighbors(i);
                    if ~obj.isValid(neighbor) || obj.isClosed(neighbor)
                        continue
                    end
                    if obj.isOpen(neighbor)
                        if current.g + neighbor.cost(current) < neighbor.g
                            neighbor.setParent(current);
                            neighbor.cost(current);
                        end
                    else
                        obj.openList(end+1) = neighbor;
                        neighbor.setParent(current);
                        neighbor.heuristic(obj.goal);
                        neighbor.cost(current);
                    end
                end
                % choose node with lowest f
                if isempty(obj.openList)
                    break
                end
                [~, idx] = min([obj.openList.f]);
                current = obj.openList(idx);
            end
            % trace path
            if ~(current.x == obj.goal.x && current.y == obj.goal.y)
                return
            end
            path = current;
            while ~isempty(current.parent)
                current = current.parent;
                path(end+1) = current;
            end
        end
        
        function neighbors = getNeighbors(obj, node)
            neighbors = [];
            for x = -1:1
                for y = -1:1
                    % skip if not a neigbor or current node
                    if x == 0 && y == 0
                        continue
                    end
                    neighbor = obj.grid.getNode(node.x+x, node.y+y);
                    if ~isempty(neighbor)
                        neighbors(end+1) = neighbor;
                    end
                end
            end
        end
        
        function valid = isValid(obj, node)
            valid = node.valid;
        end
        
        function closed = isClosed(obj, node)
            closed = any(arrayfun(@(n) n == node, obj.closedList));
        end
        
        function open = isOpen(obj, node)
            open = any(arrayfun(@(n) n == node, obj.openList));
        end
    end
end
5207 chars
187 lines

main.m
% create grid
grid = Grid(10,10);
grid.setObstacle(5,1);
grid.setObstacle(5,2);
grid.setObstacle(5,3);
grid.setObstacle(5,4);

% create start and goal nodes
start = grid.getNode(1,1);
goal = grid.getNode(10,10);

% run algorithm
astar = AStar(grid, start, goal);
path = astar.findPath();

% visualize path
imshow(~ones(10)); % black image
hold on
for i = 1:length(path)
    plot(path(i).x, path(i).y, 'r.')
end
hold off
420 chars
23 lines