gusucode.com > Mobile Robotics Simulation Toolbox > src/environment/+internal/circleLineIntersection.m

    function ranges = circleLineIntersection(sensorPose,scanAngles,maxRange,targetPose,targetRadius)
% Finds the range of a lidar sensor intersecting with a circular robot
% Source: http://www.ambrsoft.com/TrigoCalc/Circles2/circlrLine_.htm

numScans = numel(scanAngles);
ranges = nan(numScans,1);

for idx = 1:numScans
    
    scanAngle = scanAngles(idx);
    
    % Line coefficients
    lineAngle = sensorPose(3) + scanAngle;
    m = tan(lineAngle);
    d = sensorPose(2) - m*sensorPose(1);
    
    % Circle coefficients
    a = targetPose(1);
    b = targetPose(2);
    r = targetRadius;
    
    % Find overlap
    del = (r^2 *(1+ m^2)) - (b - m*a - d)^2;
    
    % Find intersection points
    range = NaN;
    
    if del >= 0
        % Solve the quadratic formula for x
        cx = a + m*(b-d);
        c2 = sqrt(del);
        c3 = 1 + m^2;
        x1 = (cx + c2) / c3;
        x2 = (cx - c2) / c3;
        
        % Find y points
        if abs(x1-x2) < 1e-6
            % If x points are close to each other, line is nearly vertical
            % Plug x value into circle equation to get y
            c1 = sqrt(a + r - x1);
            c2 = sqrt(r - a + x1);
            y1 = b + c1*c2;
            y2 = b - c1*c2;
        else
            % Else, plug x values into line equation to get y
            y1 = m*x1 + d;
            y2 = m*x2 + d;
        end
        
        % Find the angles and ranges
        v1 = [x1;y1] - [sensorPose(1);sensorPose(2)];
        v2 = [x2;y2] - [sensorPose(1);sensorPose(2)];
        v = [cos(lineAngle);sin(lineAngle)];
        r1 = norm(v1);
        r2 = norm(v2);
        a1 = v1'*v/r1;
        a2 = v2'*v/r2;
        
        % Find the nearest valid range (based on distance and direction)
        if (r1 <= maxRange) || (r2 <= maxRange)
            if (a1 > 0) && (a2 > 0)
                range = min(r1,r2);
            elseif (a1 > 0)
                range = r1;
            elseif (a2 > 0)
                range = r2;
            end
        end
    end
    
    ranges(idx) = range;
    
end

end