create an a star algorithm in matlab

main.m
function [path, cost] = A_star(start_node, end_node, edges)
% Inputs: start_node - starting node ID
%         end_node   - ending node ID
%         edges      - adjacency matrix, where edges(i,j) is the cost of going from node i to node j 
% Outputs: path     - an array containing the node IDs of the shortest path from the start node to the end node
%          cost     - the cost of the shortest path

    % Initialize the open and closed sets
    open_set = start_node;
    closed_set = [];

    % Initialize the G score (cost of getting from the start node to each node)
    G_score = Inf(size(edges, 1), 1);
    G_score(start_node) = 0;

    % Initialize the F score (estimated total cost from the start node to the end node through each node)
    F_score = Inf(size(edges, 1), 1);
    F_score(start_node) = heuristic(start_node, end_node);

    % Start the search
    while ~isempty(open_set)
        % Find the node with the lowest F score in the open set
        [current_node, idx] = min(F_score(open_set));

        % If we've reached the goal node, reconstruct the path and return
        if current_node == end_node
            path = reconstruct_path(start_node, end_node, closed_set);
            cost = G_score(end_node);
            return;
        end

        % Move current node from the open set to the closed set
        open_set(idx) = [];
        closed_set = [closed_set; current_node];

        % Check the neighbors of the current node
        for neighbor = find(edges(current_node, :))
            % If neighbor is already in the closed set, skip it
            if any(closed_set == neighbor)
                continue;
            end

            % Calculate the tentative G score for going from the start node to the neighbor through the current node
            tentative_G_score = G_score(current_node) + edges(current_node, neighbor);

            % If the neighbor is not in the open set, add it
            if ~any(open_set == neighbor)
                open_set = [open_set; neighbor];
            % If the tentative G score is not better than the existing G score for the neighbor, skip it
            elseif tentative_G_score >= G_score(neighbor)
                continue;
            end

            % The neighbor is reachable via a better path, update its G and F scores and set its parent to the current node
            G_score(neighbor) = tentative_G_score;
            F_score(neighbor) = G_score(neighbor) + heuristic(neighbor, end_node);
        end
    end

    % If we've exhausted all nodes in the open set without reaching the goal node, there's no valid path
    error('No valid path found.');
end

% Calculates the heuristic cost of going from node1 to node2 (in this case, Euclidean distance)
function h = heuristic(node1, node2)
    node1_pos = ind2sub(size(edges), node1);
    node2_pos = ind2sub(size(edges), node2);
    h = norm(node1_pos - node2_pos);
end

% Reconstructs the path by following the parent nodes from the end node to the start node
function path = reconstruct_path(start_node, end_node, parent)
    path = end_node;
    while path(1) ~= start_node
        path = [parent(path(1)); path];
    end
end
3175 chars
78 lines