create a theta star algorithm in matlab

main.m
function path = Theta_star(start, goal, obstacles)
    % Define grid parameters
    GRID_STEP = 0.1;
    ROBOT_RADIUS = 0.2;

    % Calculate grid limits
    MIN_X = min([start(1), goal(1)]) - 1;
    MAX_X = max([start(1), goal(1)]) + 1;
    MIN_Y = min([start(2), goal(2)]) - 1;
    MAX_Y = max([start(2), goal(2)]) + 1;

    % Generate grid
    [X, Y] = meshgrid(MIN_X:GRID_STEP:MAX_X, MIN_Y:GRID_STEP:MAX_Y);

    % Calculate costmap
    costmap = calculate_costmap(X, Y, ROBOT_RADIUS, obstacles);

    % Initialize start node
    start_node = [start, heuristic(start, goal), 0];

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

    while ~isempty(open_set)
        % Get node with lowest f from open set
        [~, min_idx] = min(open_set(:, 3));
        current_node = open_set(min_idx, :);

        % Check if current node is the goal
        if current_node(1:2) == goal
            % Reconstruct path from start to goal
            path = reconstruct_path(current_node);
            return;
        end

        % Move current node from open to closed set
        open_set(min_idx, :) = [];
        closed_set = [closed_set; current_node];

        % Get neighbors of current node
        neighbors = get_neighbors(current_node, X, Y, costmap);

        for i = 1:size(neighbors, 1)
            neighbor = neighbors(i, :);

            % Check if neighbor is in closed set
            if ismember(neighbor(1:2), closed_set(:, 1:2), 'rows')
                continue;
            end

            % Calculate tentative g score
            tentative_g = current_node(4) + heuristic(current_node(1:2), neighbor(1:2));

            % Check if neighbor is in open set
            idx = find(ismember(open_set(:, 1:2), neighbor(1:2), 'rows'));

            if isempty(idx)
                % Add neighbor to open set
                neighbor(3) = tentative_g + heuristic(neighbor(1:2), goal);
                neighbor(4) = tentative_g;
                neighbor(5) = size(closed_set, 1) + 1;
                neighbor(6) = current_node(5);
                open_set = [open_set; neighbor];
            else
                % Update neighbor in open set if tentative g is better
                if tentative_g < open_set(idx, 4)
                    open_set(idx, 4) = tentative_g;
                    open_set(idx, 3) = tentative_g + heuristic(neighbor(1:2), goal);
                    open_set(idx, 6) = current_node(5);
                end
            end
        end
    end

    % No path found
    path = [];
end

function costmap = calculate_costmap(X, Y, radius, obstacles)
    % Initialize costmap
    costmap = ones(size(X));

    % Check distance to each obstacle
    for i = 1:size(obstacles, 1)
        obstacle = obstacles(i, :);
        dx = X - obstacle(1);
        dy = Y - obstacle(2);
        distance = sqrt(dx.^2 + dy.^2);
        cost = (distance < radius/2) + (distance >= radius/2 & distance < radius) .* (1 - (2/radius) .* (distance - radius/2));
        costmap = costmap .* cost;
    end
end

function neighbors = get_neighbors(node, X, Y, costmap)
    % Define tolerance for collision detection
    COLLISION_TOL = 0.1;

    % Get possible neighbors based on grid connectivity
    [i, j] = find(X == node(1) & Y == node(2));
    possible_neighbors = [i-1, j-1;
                          i-1, j;
                          i-1, j+1;
                          i, j-1;
                          i, j+1;
                          i+1, j-1;
                          i+1, j;
                          i+1, j+1];

    % Filter neighbors based on collision with obstacles
    neighbors = [];

    for k = 1:size(possible_neighbors, 1)
        if possible_neighbors(k, 1) < 1 || possible_neighbors(k, 1) > size(X, 1) ||...
           possible_neighbors(k, 2) < 1 || possible_neighbors(k, 2) > size(X, 2)
            % Neighbor out of bounds
            continue;
        end

        % Check if there is a collision with an obstacle
        if costmap(possible_neighbors(k, 1), possible_neighbors(k, 2)) < COLLISION_TOL
            continue;
        end

        % Check if there is a collision with a line-of-sight obstacle
        skip = false;

        for l = node(5):-1:2
            origin = [X(node(6), node(5)), Y(node(6), node(5))];
            target = [X(node(6), l), Y(node(6), l)];
            obstacle = [X(possible_neighbors(k, 1), possible_neighbors(k, 2)), Y(possible_neighbors(k, 1), possible_neighbors(k, 2))];
            if line_of_sight_clear(origin, target, obstacle, costmap)
                skip = true;
                break;
            end
        end

        if skip
            continue;
        end

        % Add valid neighbor
        neighbors = [neighbors; X(possible_neighbors(k, 1), possible_neighbors(k, 2)), Y(possible_neighbors(k, 1), possible_neighbors(k, 2))];
    end
end

function clear = line_of_sight_clear(origin, target, obstacle, costmap)
    % Define steps
    N = 20;

    % Define line equation parameters
    dx = target(1) - origin(1);
    dy = target(2) - origin(2);
    m = dy/dx;
    b = origin(2) - m*origin(1);

    % Calculate line points
    x = linspace(origin(1), target(1), N);
    y = m*x + b;

    % Check if any point is obstructed
    for i = 1:length(x)-1
        xi = x(i);
        yi = y(i);

        % Calculate distance to obstacle
        dx = obstacle(1) - xi;
        dy = obstacle(2) - yi;
        distance = sqrt(dx^2 + dy^2);

        % Check if obstacle blocks line of sight
        if distance < 0.1 && costmap(round(yi*10), round(xi*10)) < 0.5
            clear = false;
            return;
        end
    end

    % Line of sight is clear
    clear = true;
end

function path = reconstruct_path(goal_node)
    % Initialize path with goal node
    path = [goal_node(1:2)];

    % Traverse parents until start node is reached
    while goal_node(6) > 0
        goal_node = closed_set(goal_node(6), :);
        path = [goal_node(1:2); path];
    end
end

function h = heuristic(node, goal)
    % Use Euclidean distance as heuristic
    h = norm(node - goal);
end
6101 chars
197 lines