tangent bug algorithm motion planning in matlab

main.m
function [path, target_reached] = tangent_bug(start_point, goal_point, map)
% start_point: [x,y] coordinates of starting point
% goal_point: [x,y] coordinates of goal point
% map: occupancy grid map

% Parameters
delta = 0.1; % step size
min_distance = 0.01; % minimum distance to obstacle
reached_tolerance = 0.1; % tolerance for reaching goal

% Initialize variables
current_point = start_point;
target_reached = false;
path = [start_point];

while ~target_reached
    % Calculate distance to goal
    distance_to_goal = norm(goal_point - current_point);
    
    % Check if current point is close to goal
    if distance_to_goal <= reached_tolerance
        target_reached = true;
        continue;
    end
    
    % Check if current point is inside obstacle
    if map(round(current_point(2)), round(current_point(1))) == 1
        % Follow obstacle boundary
        phi = atan2(goal_point(2) - current_point(2), goal_point(1) - current_point(1));
        d = delta;
        while true
            % Move along boundary
            new_point = current_point + d * [cos(phi), sin(phi)];
            if norm(new_point - start_point) > distance_to_goal
                break;
            end
            if map(round(new_point(2)), round(new_point(1))) == 0
                % Found obstacle-free region
                path = [path; new_point];
                current_point = new_point;
                break;
            end
            % Update d
            delta_angle = atan2(sin(phi - pi/2), cos(phi - pi/2));
            if d > min_distance
                d = d - min_distance;
            end
            phi = phi + delta_angle;
        end
    else
        % Move towards goal
        phi = atan2(goal_point(2) - current_point(2), goal_point(1) - current_point(1));
        new_point = current_point + delta * [cos(phi), sin(phi)];
        if map(round(new_point(2)), round(new_point(1))) == 0
            % No obstacle, move towards goal
            current_point = new_point;
        else
            % Obstacle, switch to boundary following mode
            continue;
        end
        path = [path; current_point];
    end
end
end
2151 chars
65 lines