Matlab Program to check the length of the path traversed by a circular Tool inside a Polygon

checkAllonsy.m

function check = checkAllonsy(coeffLines,cyclecount,diameter)
if((coeffLines(cyclecount-1,1).a*coeffLines(cyclecount-1,3).b - coeffLines(cyclecount-1,3).a/coeffLines(cyclecount-1,1).b)~= 0)%Check whether 1st and 3rd Line
    %Find Interesection of 1st and 3rd lines of the penultimate Polygon
    delta = (coeffLines(cyclecount-1,1).a* coeffLines(cyclecount-1,3).b)- (coeffLines(cyclecount-1,3).a * coeffLines(cyclecount-1,1).b );
    tempx1= ((coeffLines(cyclecount-1,3).b* (-1)* coeffLines(cyclecount-1,1).c)- (coeffLines(cyclecount-1,1).b * (-1)* coeffLines(cyclecount-1,3).c))/delta;
    tempy1= ((coeffLines(cyclecount-1,1).a* (-1)* coeffLines(cyclecount-1,3).c)- (coeffLines(cyclecount-1,3).a *(-1)*  coeffLines(cyclecount-1,1).c))/delta;
    %Find Interesection of 1st and 3rd lines of the final Polygon
    delta = (coeffLines(cyclecount,1).a* coeffLines(cyclecount,3).b)- (coeffLines(cyclecount,3).a * coeffLines(cyclecount,1).b );
    tempx2= ((coeffLines(cyclecount,3).b* (-1)* coeffLines(cyclecount,1).c)- (coeffLines(cyclecount,1).b * (-1)* coeffLines(cyclecount,3).c))/delta;
    tempy2= ((coeffLines(cyclecount,1).a* (-1)* coeffLines(cyclecount,3).c)- (coeffLines(cyclecount,3).a *(-1)*  coeffLines(cyclecount,1).c))/delta;
    %Check whether they lie on the same side of 2nd Line
    if(((coeffLines(cyclecount-1,2).a * tempx1)+(coeffLines(cyclecount-1,2).b*tempy1) + coeffLines(cyclecount-1,2).c) * ((coeffLines(cyclecount-1,2).a*tempx2) +(coeffLines(cyclecount-1,2).b*tempy2)+coeffLines(cyclecount-1,2).c)>0 && ((coeffLines(cyclecount-1,4).a * tempx1)+(coeffLines(cyclecount-1,4).b*tempy1) + coeffLines(cyclecount-1,4).c) * ((coeffLines(cyclecount-1,4).a*tempx2) +(coeffLines(cyclecount-1,4).b*tempy2)+coeffLines(cyclecount-1,4).c)>0)% They are on same side
        check=true;
    else
        check=false;
        return
    end
else
% if((coeffLines(cyclecount-1,1).a*coeffLines(cyclecount-1,3).b - coeffLines(cyclecount-1,3).a/coeffLines(cyclecount-1,1).b)== 0)
    %If they are parallel check for 1.6D
    if(abs(coeffLines(cyclecount-1,1).c-coeffLines(cyclecount-1,3).c)/sqrt(coeffLines(cyclecount-1,3).a*coeffLines(cyclecount-1,1).a + coeffLines(cyclecount-1,1).b*coeffLines(cyclecount-1,1).b)> 1.6*diameter)
        display('XXXXASASAS');
        check=true;
    else
        display('XXXXASASAS1');
        check=false;
        return
    end
end
if((coeffLines(cyclecount-1,2).a*coeffLines(cyclecount-1,4).b - coeffLines(cyclecount-1,4).a*coeffLines(cyclecount-1,2).b) ~= 0)%Check whether 2nd and 4th Line
    %Find Interesection of 2nd and 4th lines of the penultimate Polygon
    delta = (coeffLines(cyclecount-1,2).a* coeffLines(cyclecount-1,4).b)- (coeffLines(cyclecount-1,4).a * coeffLines(cyclecount-1,2).b );
    tempx1= ((coeffLines(cyclecount-1,4).b* (-1)* coeffLines(cyclecount-1,2).c)- (coeffLines(cyclecount-1,2).b * (-1)* coeffLines(cyclecount-1,4).c))/delta;
    tempy1= ((coeffLines(cyclecount-1,2).a* (-1)* coeffLines(cyclecount-1,4).c)- (coeffLines(cyclecount-1,4).a *(-1)*  coeffLines(cyclecount-1,2).c))/delta;
    %Find Interesection of 2nd and 4th lines of the final Polygon
    delta = (coeffLines(cyclecount,2).a* coeffLines(cyclecount,4).b)- (coeffLines(cyclecount,4).a * coeffLines(cyclecount,2).b );
    tempx2= ((coeffLines(cyclecount,4).b* (-1)* coeffLines(cyclecount,2).c)- (coeffLines(cyclecount,2).b * (-1)* coeffLines(cyclecount,4).c))/delta;
    tempy2= ((coeffLines(cyclecount,2).a* (-1)* coeffLines(cyclecount,4).c)- (coeffLines(cyclecount,4).a *(-1)*  coeffLines(cyclecount,2).c))/delta;
    %Check whether they lie on the same side of 2nd Line
    if(((coeffLines(cyclecount-1,1).a * tempx1)+(coeffLines(cyclecount-1,1).b*tempy1) + coeffLines(cyclecount-1,1).c) * ((coeffLines(cyclecount-1,1).a*tempx2) +(coeffLines(cyclecount-1,1).b*tempy2)+coeffLines(cyclecount-1,1).c)>0 && ((coeffLines(cyclecount-1,3).a * tempx1)+(coeffLines(cyclecount-1,3).b*tempy1) + coeffLines(cyclecount-1,3).c) * ((coeffLines(cyclecount-1,3).a*tempx2) +(coeffLines(cyclecount-1,3).b*tempy2)+coeffLines(cyclecount-1,3).c)>0)% They are on same side
        check=true;
    else
        check=false;
        return
    end
else
% if((coeffLines(cyclecount-1,2).a*coeffLines(cyclecount-1,4).b - coeffLines(cyclecount-1,4).a*coeffLines(cyclecount-1,2).b) == 0)
    %If they are parallel check for 1.6D
    if(abs(coeffLines(cyclecount-1,2).c-coeffLines(cyclecount-1,4).c)/sqrt((coeffLines(cyclecount-1,2).a*coeffLines(cyclecount-1,2).a) + (coeffLines(cyclecount-1,2).b*coeffLines(cyclecount-1,2).b))> 1.6*diameter)
        display('XXXXASASAS2');
        check=true;
    else
        display('XXXXASASAS3');
        check=false;
        return
    end
end
end

checkAllonsyDP.m

function check = checkAllonsyDP(coeffLines,cyclecount,centroidX,centroidY)
if((-coeffLines(cyclecount,1).c/coeffLines(cyclecount,1).b)<centroidY && (-coeffLines(cyclecount,3).c/coeffLines(cyclecount,3).b)>centroidY && (-coeffLines(cyclecount,2).c/coeffLines(cyclecount,2).a)>centroidX && (-coeffLines(cyclecount,4).c/coeffLines(cyclecount,4).a)<centroidX )
    check = true;
else
    check = false;
end

getCoefficients.m

function coeffLines = getCoefficients(coeffLines,i,checkDirection,nSides,diameter)
disp('cyclecount')
disp(i)
for j=1:1:nSides-1
    dist = sqrt((coeffLines(1,j).a*coeffLines(1,j).a)+(coeffLines(1,j).b*coeffLines(1,j).b)) * diameter;
    coeffLines(i+1,j).a = coeffLines(i,j).a ;
    coeffLines(i+1,j).b = coeffLines(i,j).b ;
    coeffLines(i+1,j).c = coeffLines(i,j).c + (checkDirection(j)*dist) ;
end
end

getConnections.m

function [xcoord,ycoord] = getConnections(xcoord,ycoord,coeffLines,i,nSides)
    delta = (coeffLines(i,1).a* coeffLines(i-1,nSides-1).b)- (coeffLines(i-1,nSides-1).a * coeffLines(i,1).b );
    xcoord(i-1,nSides)= ((coeffLines(i-1,nSides-1).b* (-1)* coeffLines(i,1).c)- (coeffLines(i,1).b * (-1)* coeffLines(i-1,nSides-1).c))/delta;
    ycoord(i-1,nSides)= ((coeffLines(i,1).a* (-1)* coeffLines(i-1,nSides-1).c)- (coeffLines(i-1,nSides-1).a *(-1)*  coeffLines(i,1).c))/delta;
    end

getCoordinates.m

function [xcoord,ycoord] = getCoordinates(xcoord,ycoord,coeffLines,nSides,i,k)
for j=1:1:nSides-2
%     disp('loopnumber')
%     disp(j)
%     disp('cyclecount')
%     disp(k)
    delta = (coeffLines(i,j).a* coeffLines(i,j+1).b)- (coeffLines(i,j+1).a * coeffLines(i,j).b )
    xcoord(k+1,j+1)= ((coeffLines(i,j+1).b* (-1)* coeffLines(i,j).c)- (coeffLines(i,j).b * (-1)* coeffLines(i,j+1).c))/delta;
    ycoord(k+1,j+1)= ((coeffLines(i,j).a* (-1)* coeffLines(i,j+1).c)- (coeffLines(i,j+1).a *(-1)*  coeffLines(i,j).c))/delta;
end
delta = (coeffLines(i,1).a* coeffLines(i,nSides-1).b)- (coeffLines(i,nSides-1).a * coeffLines(i,1).b )
xcoord(k+1,1)= ((coeffLines(i,nSides-1).b* (-1)* coeffLines(i,1).c)- (coeffLines(i,1).b * (-1)* coeffLines(i,nSides-1).c))/delta;
ycoord(k+1,1)= ((coeffLines(i,1).a* (-1)* coeffLines(i,nSides-1).c)- (coeffLines(i,nSides-1).a *(-1)*  coeffLines(i,1).c))/delta;
end

Toolpath.m

polygondata = dlmread('polypoints.txt');
nSides = length(polygondata);
xcoord = zeros(1,nSides);
ycoord = zeros(1,nSides);
%coeffLines
doubleParallel=false;
centroidX =0;
centroidY =0;
checkDirection = zeros(1,nSides-1);
diameter = (polygondata(3,1));
areatool = pi*diameter*diameter / 4;
cyclecount = 1;
distTravelled = 0;
for i=1:1:nSides
    xcoord(1,i) = polygondata(1,i);
    ycoord(i) = polygondata(2,i);
end
for i=1:1:nSides
    centroidX = centroidX + xcoord(1,i);
    centroidY = centroidY + ycoord(i);
end
centroidX = centroidX/(nSides -1);
centroidY = centroidY/(nSides -1);
% for i=1:1:nSides
%     DirVector1=[centroidX,centroidY]-[xcoord(1,i),ycoord(i)];
%     DirVector2=[0,2]-[3,0];
%     Angle=acos( dot(DirVector1,DirVector2)/norm(DirVector1)/norm(DirVector2) );
% end
for i=1:1:nSides-1
    coeffLines(1,i).a = ycoord(i+1) - ycoord(i);
    coeffLines(1,i).b = xcoord(1,i)- xcoord(1,i+1);
    coeffLines(1,i).c = (xcoord(1,i+1)- xcoord(1,i)) * ycoord(i) + (ycoord(i) - ycoord(i+1))*xcoord(1,i);
end
for i=1:1:nSides-1
    distblines = sqrt((coeffLines(1,i).a*coeffLines(1,i).a)+(coeffLines(1,i).b*coeffLines(1,i).b)) * diameter;
    dist1(i) = ((coeffLines(1,i).a * centroidX) + (coeffLines(1,i).b * centroidY) + (coeffLines(1,i).c + distblines));
    
    l1 = (sqrt((coeffLines(1,i).a*coeffLines(1,i).a)+(coeffLines(1,i).b*coeffLines(1,i).b)));
    
    dist1(i) = abs(dist1(i) / l1);
    dist2(i) = abs(((coeffLines(1,i).a * centroidX) + (coeffLines(1,i).b * centroidY) + (coeffLines(1,i).c - distblines))/(sqrt((coeffLines(1,i).a*coeffLines(1,i).a)+(coeffLines(1,i).b*coeffLines(1,i).b))));
    if(dist1(i)<dist2(i))
        checkDirection(i)= 1;
    else
        checkDirection(i)=-1;
    end
end
coeffLines = getCoefficients(coeffLines,cyclecount,checkDirection,nSides,(diameter/2));
[xcoord,ycoord] = getCoordinates(xcoord,ycoord,coeffLines,nSides,cyclecount+1,cyclecount);
cyclecount=cyclecount+1;
ycoord(cyclecount,5) = ycoord(cyclecount,1);
xcoord(cyclecount,5) = xcoord(cyclecount,1);
if((coeffLines(cyclecount-1,1).a*coeffLines(cyclecount-1,3).b - coeffLines(cyclecount-1,3).a/coeffLines(cyclecount-1,1).b)== 0 && (coeffLines(cyclecount-1,2).a*coeffLines(cyclecount-1,4).b - coeffLines(cyclecount-1,4).a*coeffLines(cyclecount-1,2).b) == 0)
    doubleParallel = true;
end
if(doubleParallel==false)
    while(checkAllonsy(coeffLines,cyclecount,diameter))
        coeffLines = getCoefficients(coeffLines,cyclecount,checkDirection,nSides,0.8*diameter);
        [xcoord,ycoord] = getCoordinates(xcoord,ycoord,coeffLines,nSides,cyclecount+1,cyclecount);
        cyclecount=cyclecount+1;
        ycoord(cyclecount,5) = ycoord(cyclecount,1);
        xcoord(cyclecount,5) = xcoord(cyclecount,1);
    end
else
    display(':D');
    while(checkAllonsyDP(coeffLines,cyclecount,centroidX,centroidY))
        coeffLines = getCoefficients(coeffLines,cyclecount,checkDirection,nSides,0.8*diameter);
        [xcoord,ycoord] = getCoordinates(xcoord,ycoord,coeffLines,nSides,cyclecount+1,cyclecount);
        cyclecount=cyclecount+1;
        ycoord(cyclecount,5) = ycoord(cyclecount,1);
        xcoord(cyclecount,5) = xcoord(cyclecount,1);
    end
end
for i=cyclecount-1:-1:2
    [xcoord,ycoord] = getConnections(xcoord,ycoord,coeffLines,i,nSides);
end
for i=1:1:cyclecount-1
    plot(xcoord(i,:),ycoord(i,:),'k')
    hold all
end
for i=2:1:cyclecount-2
    plot([xcoord(i,nSides) xcoord(i+1,1)],[ycoord(i,nSides) ycoord(i+1,1)],'k');
    display('ctaea');
    display(i);
    hold all
end
for i=2:1:cyclecount-2
    for j=1:1:nSides-1
        distTravelled = distTravelled + pdist([xcoord(i,j),xcoord(i,j+1);ycoord(i,j),ycoord(i,j+1)],'euclidean');
    end
    distTravelled = distTravelled + pdist([xcoord(i,nSides),xcoord(i+1,1);ycoord(i,nSides),ycoord(i+1,1)],'euclidean');
end