create an a star algorithm with obstacles avoidance in matlab

main.m
% Define map size and obstacles
map = zeros(100);
map(30:80, 30:35) = 1;
map(30:35, 30:80) = 1;
map(10:15, 50:70) = 1;
119 chars
6 lines

main.m
% Define start and goal nodes
start_node = [10, 10];
goal_node = [90, 90];

% Initialize open and closed lists
open_list = [start_node 0 heuristic(start_node, goal_node)];
closed_list = [];

% Define heuristic function (Euclidean distance)
function h = heuristic(node, goal)
    h = sqrt((node(1) - goal(1))^2 + (node(2) - goal(2))^2);
end

% Define neighbor function (4-direction)
function neighbors = get_neighbors(node, map)
    neighbors = [];
    if node(1) > 1 && map(node(1)-1, node(2)) == 0 % up
        neighbors = [neighbors; node(1)-1, node(2)];
    end
    if node(1) < size(map, 1) && map(node(1)+1, node(2)) == 0 % down
        neighbors = [neighbors; node(1)+1, node(2)];
    end
    if node(2) > 1 && map(node(1), node(2)-1) == 0 % left
        neighbors = [neighbors; node(1), node(2)-1];
    end
    if node(2) < size(map, 2) && map(node(1), node(2)+1) == 0 % right
        neighbors = [neighbors; node(1), node(2)+1];
    end
end

% Start A* algorithm
while ~isempty(open_list)
    % Find node with lowest f-score
    [~, idx] = min(open_list(:, 3));
    current_node = open_list(idx, :);
    
    % Check if goal is reached
    if isequal(current_node(1:2), goal_node)
        break;
    end
    
    % Move current node from open to closed list
    open_list(idx, :) = [];
    closed_list = [closed_list; current_node];
    
    % Find neighbors
    neighbors = get_neighbors(current_node(1:2), map);
    
    % Check each neighbor
    for i = 1:size(neighbors, 1)
        % Compute f-score
        g = current_node(3) + 1;
        h = heuristic(neighbors(i, :), goal_node);
        f = g + h;
        
        % Check if neighbor is already visited
        if any(ismember(open_list(:, 1:2), neighbors(i, :), 'rows'))
            idx = find(ismember(open_list(:, 1:2), neighbors(i, :), 'rows'));
            if open_list(idx, 3) > f
                open_list(idx, :) = [neighbors(i, :) g f];
            end
        elseif any(ismember(closed_list(:, 1:2), neighbors(i, :), 'rows'))
            idx = find(ismember(closed_list(:, 1:2), neighbors(i, :), 'rows'));
            if closed_list(idx, 3) > f
                closed_list(idx, :) = [neighbors(i, :) g f];
                open_list = [open_list; closed_list(idx, :)];
                closed_list(idx, :) = [];
            end
        else
            open_list = [open_list; [neighbors(i, :) g f]];
        end
    end
end
2402 chars
74 lines

main.m
% Visualize path
path = [goal_node];
while ~isequal(path(1,:), start_node)
    idx = find(ismember(closed_list(:,1:2), path(1,:), 'rows'));
    path = [closed_list(idx, 1:2); path];
end

imshow(map, 'InitialMagnification', 'fit')
hold on
plot(start_node(2), start_node(1), 'bo')
plot(goal_node(2), goal_node(1), 'rx')
for i = 1:size(path,1)-1
    plot([path(i,2), path(i+1,2)], [path(i,1), path(i+1,1)], 'r', 'LineWidth', 3)
    pause(0.1)
end
444 chars
16 lines