%
% Macro to compute and display the set of attainable accelerations.
% (c) Marc Bodson, June 18, 2020
% Polygonal facets are determined if columns of CB are not linearly independent.
% Input: cb is a matrix defining the relationship from controls to accelerations
%        umax is a vector containing the upper control limits
%        umin is a vector containing the lower control limits
%
% Prepare plots
%
clf reset;hold on;grid;rotate3d on;view(60,30);
xlabel('Pitch accel.');ylabel('Roll accel.');zlabel('Yaw Accel.');
colormap('gray');caxis([0.0 1.0]);caxis('manual');
%
% Initialization
%
nf=0;  % number of polygonal facets
epsx=1e-12;  % small number used to assess linear dependence
[n,m]=size(cb); % m = number of controls
if (n~=3) error('The number of rows of cb must be equal to 3');end
mt=0;  % true number of controls
for j=1:m  % pre-calculation of various matrices obtained from cb
    bup=cb(:,j)*umax(j);blp=cb(:,j)*umin(j);bn=norm(bup-blp);
    if (bn>epsx)
        mt=mt+1;db(:,mt)=bup-blp;dbn(:,mt)=db(:,mt)/bn;
        bmax(:,mt)=bup;bmin(:,mt)=blp;
    end
end
done=zeros(mt,mt); % matrix used to avoid plotting the same facet twice
%
% Computation and plot of the facets
%
for i=1:mt-1
    for j=i+1:mt
        % obtain the vector normal to the facet and test that the facet is 2-dimensional
        ax=dbn(1,i);ay=dbn(2,i);az=dbn(3,i);
        bx=dbn(1,j);by=dbn(2,j);bz=dbn(3,j);
        ab=[ay*bz-az*by;bx*az-ax*bz;ax*by-bx*ay];
        if (norm(ab))>epsx
            % determine the controls needed to obtain the two farthest facets
            % keep track in the vector ind of the control vectors that are coplanar with the facet
            % npar is the total number of controls asssociated with the facet
            % done and dupl are used to avoid plotting the same facet twice
            done(i,j)=1;done(j,i)=1;
            vmin=zeros(3,1);vmax=zeros(3,1);
            ind=zeros(1,mt);ind(1)=i;ind(2)=j;npar=2;dupl=0;
            for k=1:mt
                if and(and((k~=i),(k~=j)),(dupl~=1))
                    cmin=bmin(:,k);cmax=bmax(:,k);pmin=cmin'*ab;pmax=cmax'*ab;
                    if (pmax>pmin+epsx) vmax=vmax+cmax;vmin=vmin+cmin;
                    else if (pmax<pmin-epsx) vmax=vmax+cmin;vmin=vmin+cmax;
                        else % in this case, the control k is coplanar with the facet
                            if (or((done(k,i)==1),(done(k,j)==1))) dupl=1;else
                                npar=npar+1;ind(npar)=k;
                            end
                        end
                    end
                end
            end  % end loop k=1:mt
            % show the facet (unless it has been computed before) as well as its complementary
            if (dupl~=1)
                tnpar=2*npar;  % tnpar is the number of vertices of the polygon
                bf=zeros(3,tnpar);
                %          zcolor=[0.6 0.6 0.6];
                zcolor=[0.7 0.8 1];
                %           zcolor=1*ones(tnpar,1); % alternative color scheme
                if (npar==2) % basic case: no coplanar controls and the facet is a parallelogram
                    bf(:,1)=vmax+bmin(:,i)+bmin(:,j);bf(:,2)=vmax+bmax(:,i)+bmin(:,j);
                    bf(:,3)=vmax+bmax(:,i)+bmax(:,j);bf(:,4)=vmax+bmin(:,i)+bmax(:,j);
                else  % the facet is a polygon
                    % find a first vertex of the polygon by finding the farthest edge
                    % associated with the first control
                    % ctr gives the controls associated with the current vertex
                    ctr=zeros(1,mt);
                    bf(:,1)=vmax+bmin(:,i);
                    aba=[ab(2)*az-ab(3)*ay;ax*ab(3)-ab(1)*az;ab(1)*ay-ax*ab(2)];
                    for k=2:npar
                        cmin=bmin(:,ind(k));cmax=bmax(:,ind(k));
                        pmin=cmin'*aba;pmax=cmax'*aba;
                        if (pmax>=pmin) bf(:,1)=bf(:,1)+cmax;ctr(k)=1;
                        else bf(:,1)=bf(:,1)+cmin;ctr(k)=0;
                        end
                    end
                    % continue by finding edges that form the smallest angles with
                    % previous edges, and close the polygon when done
                    dbf(:,1)=db(:,i);bf(:,2)=bf(:,1)+dbf(:,1);
                    odbn=dbn(:,i);
                    for k=2:npar
                        cosal=-1;lmax=0;
                        for l=2:npar
                            if (ctr(l)==0) ndbn=dbn(:,ind(l));else ndbn=-dbn(:,ind(l));end
                            ncosal=odbn'*ndbn;
                            if (ncosal>cosal) cosal=ncosal;lmax=l;end
                        end
                        if (ctr(lmax)==0) odbn=dbn(:,ind(lmax));dbf(:,k)=db(:,ind(lmax));ctr(lmax)=1;
                        else odbn=-dbn(:,ind(lmax));dbf(:,k)=-db(:,ind(lmax));ctr(lmax)=0;
                        end
                        bf(:,k+1)=bf(:,k)+dbf(:,k);
                    end
                    for k=npar+1:tnpar-1;bf(:,k+1)=bf(:,k)-dbf(:,k-npar);end
                end % end npar=2 test
                % show the facet and its complementary; count the number of facets
                %               fill3(bf(1,1:tnpar)',bf(2,1:tnpar)',bf(3,1:tnpar)',0.027*bf(3,1:tnpar));
                fill3(bf(1,1:tnpar)',bf(2,1:tnpar)',bf(3,1:tnpar)',zcolor);
                for k=1:tnpar; bf(:,k)=bf(:,k)+vmin-vmax;end
                %            fill3(bf(1,1:tnpar)',bf(2,1:tnpar)',bf(3,1:tnpar)',0.027*bf(3,1:tnpar));
                fill3(bf(1,1:tnpar)',bf(2,1:tnpar)',bf(3,1:tnpar)',zcolor);
                nf=nf+2;
            end % end dupl=1 test
        end   % end ab test
    end   % end loop j=i+1:mt
end   % end loop i=1:mt-1
hold off