function [ reachabilityFlag ] = isReachable( dx, dy )
%isReachable: Can the robotic arm reach the given point (dx,dy)
%   if the point (dx, dy) is in reach 1 is returned, else 0

global L1;
global L2;
global SUM_L1_L2;

msg = sprintf('function called: isReachable( dx=%f, dy=%f )', dx, dy);
debug_L1(msg);

reachabilityFlag = 0;

% Case 1 : Arm's length is shorter
distance_from_origin = sqrt(dx*dx + dy*dy);
if distance_from_origin > SUM_L1_L2
    msg = sprintf('Outside Work Envelope: distance_from_origin(%f) > SUM_L1_L2(%f)', distance_from_origin, SUM_L1_L2);
    debug_L1(msg);
   return;
end

% Case 2: Unrechable semi-circle in Q1 (and Q2 )
if dy >= 0 % Since dy is positive, the point is in Q1 or Q2
    xc = L1;
    yc = 0;
    radius = L2;
    if (dx-xc)*(dx-xc) + (dy-yc)*(dy-yc) < radius*radius
        debug_L1('Case 2: Unrechable semi-circle in Q1');
        msg = sprintf('Outside Work Envelope: (dx-L1)*(dx-L1) + (dy-0)*(dy-0) < L2*L2');
        debug_L1(msg);
        return;
    end
end

% Case 3: Unreachable semicircle below x-axis (Q3 & Q4)
if dy <= 0 % dx can be positive or negative
    xc = 0;
    yc = 0;
    radius = abs(L1)-abs(L2);

    if (dx-xc)*(dx-xc) + (dy-yc)*(dy-yc) < (radius * radius)
        debug_L1('Case 3: Unreachable semicircle below x-axis (Q3 & Q4)');
        msg = sprintf('Outside Work Envelope: (dx-L1)*(dx-L1) + (dy-0)*(dy-0) < L2*L2');
        debug_L1(msg);
        return;
    end
end

% Case 4: The point lies in Q4
if dy <= 0 && dx >=0
    debug_L1('Case 4: The point lies in Q4');
        xc = -L1;
        yc =  0;
        radius = L2;
        if (dx-xc)*(dx-xc) + (dy-yc)*(dy-yc) < radius * radius
            debug_L1('Case 4a: Inside Q4 but inside reachable area');
            msg = sprintf('Inside Work Envelope: (dx-xc)*(dx-xc) + (dy-yc)*(dy-yc) > radius * radius');
            debug_L1(msg);
            reachabilityFlag = 1;
            return;
        end
    % Inside Q4 but outside reachable area
    return;
end

% Case 5: Inside Q3 but outside reachable area
if dy < 0 && dx < 0  % point lies inside Q3
    xc = -L1;
    yc =  0;
    radius = L2;
    if (dx-xc)*(dx-xc) + (dy-yc)*(dy-yc) > radius * radius
        % Inside Q3 but outside reachable area
        debug_L1('Case 5: Inside Q3 but outside reachable area');
        msg = sprintf('Outside Work Envelope: (dx-xc)*(dx-xc) + (dy-yc)*(dy-yc) > radius * radius');
        debug_L1(msg);
        return;
    end
end


reachabilityFlag = 1;
end

	 DEBUG: function called: isReachable( dx=200.000000, dy=200.000000 )
	 DEBUG: Outside Work Envelope: distance_from_origin(282.842712) > SUM_L1_L2(250.000000)
reachabilityFlag =

     0

Published with MATLAB® R2012b