image thumbnail
from Ultrasonic Sensor Robot by Ligong Han
simulation of environment modeling for mobile robot based on ultrasonic sensor

[modelpt,mptadd,finished,mdl]=sonomodel(rs,r)
function [modelpt,mptadd,finished,mdl]=sonomodel(rs,r)
%Modeling
%A     : Faultages on two sides
%A1(01): j--->i, i--->k/k==nil
%A2(02): j--->i, i<---k
%A3(03): j<---i, i--->k/k==nil
%B     : Faultage on one side
%B1(04): j--->i, i<--k/i<-k, k<--m
%B2(05): j--->i, i<--k/i<-k, k~<--m
%B3(06): j--->i, i->k
%B4(07): l-->j, j-->i/j->i, i--->k/k==nil
%B5(08): l~-->j, j-->i/j->i, i--->k/k==nil
%B6(09): j<-i, i--->k/k==nil
%C     : Wall
%C (10): l~<-j/l~<--j, j-->i/j->i, i-->k/i->k
%D     : Outer corner
%D1(11): l<-j/l<--j, j<--i/j<-i, i-->k/i->k, k-->m/k->m
%D2(12): l~<-j/l~<--j, j<--i/j<-i, i-->k/i->k, k~-->m/k~->m
%D3(13): n<-l/n<--l, l<-j/l<--j, j->i/j-->i, i-->k/i->k
%D4(14): n~<-l/n~<--l, l<-j/l<--j, j->i/j-->i, i-->k/i->k
%E     : Inner corner
%E (15): j-->i/j->i, i<-k
%16    : for nil
%17    : for unknown
if nargin<2
    r=.56;
end
rx=0;ry=0;
ns=length(rs);
a=2*pi/ns;%angle between two adjacent sensors
finished=false(1,ns);%when finished, 'false' stands for unknown
modelpt=zeros(ns,4);%2 points for each sensor (sometimes 3, error avoided)
mptadd=[];
mdl=16*ones(1,ns);%!!
for k=1:ns
    if finished(k)%!!
        continue
    end
    unknown=false;
    if rs(k)>r/2
    dsi=rs(k);
    dsj=rs(mod(k,ns)+1);
    dsk=rs(mod(k-2,ns)+1);
    dsl=rs(mod(k+1,ns)+1);
    dsm=rs(mod(k-3,ns)+1);
    dsn=rs(mod(k+2,ns)+1);
    dso=rs(mod(k-4,ns)+1);
    idxi=k;
    idxj=mod(k,ns)+1;
    idxk=mod(k-2,ns)+1;
    idxl=mod(k+1,ns)+1;
    idxm=mod(k-3,ns)+1;
    idxn=mod(k+2,ns)+1;
    idxo=mod(k-4,ns)+1;
    xchng=false;
    if dsj<dsk%adjust index, dsj>dsk
        idxj=idxk;
        idxk=mod(k,ns)+1;
        idxl=idxm;
        idxm=mod(k+1,ns)+1;
        idxn=idxo;
        idxo=mod(k+2,ns)+1;
        dsj=dsk;
        dsk=rs(mod(k,ns)+1);
        dsl=dsm;
        dsm=rs(mod(k+1,ns)+1);
        dsn=dso;
        %dso=rs(mod(k+2,ns)+1);
        xchng=true;
    end
    if dsi>=dsk%i>k/j, %A1,3, B3,4,5,6, C, D1,2,3,4
        if dsi>=dsj%i>j/k, %A3, B6, D1,2
            if (dsi-dsk)>2*r%||dsk==inf; %i--->k/j||k/j==nil, %A3, B6
                if (dsi-dsj)>2*r%i--->j/k||j/k==nil
                    %A3
                    %mdlrecg='A3';
                    mdl(k)=3;
                    %finished(k)=true;
                    modelpt(k,1:2)=[rx+rs(k)*cos((k-3/4)*a),ry+rs(k)*sin((k-3/4)*a)];
                    modelpt(k,3:4)=[rx+rs(k)*cos((k-1/4)*a),ry+rs(k)*sin((k-1/4)*a)];
                elseif (dsi-dsj)>=0&&dsj<=dsi/cos(a)%i->j
                    %B6
                    %mdlrecg='B6';
                    mdl(k)=9;
                    %finished(k)=true;
                    if ~xchng
                        modelpt(k,1:2)=[rx+rs(k)*cos(k*a),ry+rs(k)*sin(k*a)];
                        modelpt(k,3:4)=[rx+rs(idxi)/cos(a)*cos(idxk*a),ry+rs(idxi)/cos(a)*sin(idxk*a)];
                    else
                        modelpt(k,1:2)=[rx+rs(k)*cos(idxj*a),ry+rs(k)*sin(idxj*a)];
                        modelpt(k,3:4)=[rx+rs(idxi)/cos(a)*cos(k*a),ry+rs(idxi)/cos(a)*sin(k*a)];
                    end
%                     if ~xchng
%                         modelpt(k,1:2)=[rx+rs(k)*cos(k*a),ry+rs(k)*sin(k*a)];
%                         modelpt(k,3:4)=[rx+rs(idxj)/cos(2*a)*cos(idxk*a),ry+rs(idxj)/cos(2*a)*sin(idxk*a)];
%                     else
%                         modelpt(k,1:2)=[rx+rs(k)*cos(idxj*a),ry+rs(k)*sin(idxj*a)];
%                         modelpt(k,3:4)=[rx+rs(idxj)/cos(2*a)*cos(k*a),ry+rs(idxj)/cos(2*a)*sin(k*a)];
%                     end
                else%?
                    unknown=true;
                    mdl(k)=17;
                end
            elseif (dsi-dsj)<2*r&&(dsi-dsk)<2*r%<--/-i--/->, %D1,2
                finished([idxj,idxk])=true;
                modelpt(k,:)=nan;
                if (dsj-dsl)<2*r&&(dsk-dsm)<2*r%l<-/--j && k--/->m
                    %finished(k)=true;
                    %D1 %!!! ->link!
                    %mdlrecg='D1';
                    mdl(k)=11;
                    if ~xchng
                        pt1=[rx+rs(idxl)*cos(idxl*a),ry+rs(idxl)*sin(idxl*a)];
                        pt2=[rx+rs(idxj)*cos(idxj*a),ry+rs(idxj)*sin(idxj*a)];
                        pt3=[rx+rs(idxm)*cos(idxo*a),ry+rs(idxm)*sin(idxo*a)];
                        pt4=[rx+rs(idxk)*cos(idxm*a),ry+rs(idxk)*sin(idxm*a)];
                    else
                        pt1=[rx+rs(idxl)*cos(idxn*a),ry+rs(idxl)*sin(idxn*a)];
                        pt2=[rx+rs(idxj)*cos(idxl*a),ry+rs(idxj)*sin(idxl*a)];
                        pt3=[rx+rs(idxm)*cos(idxm*a),ry+rs(idxm)*sin(idxm*a)];
                        pt4=[rx+rs(idxk)*cos(idxk*a),ry+rs(idxk)*sin(idxk*a)];
                    end
%                     if dsj<dsi/cos(a)&&dsk<dsi/cos(a)%&&(dsj>dsl/cos(a)&&dsk<dsm/cos(a))%D1_2
%                         %mdlrecg='D1_2';
%                         if ~xchng
%                             pt1=[rx+rs(idxl)*cos(idxl*a),ry+rs(idxl)*sin(idxl*a)];
%                             pt2=[rx+rs(idxj)*cos(idxj*a),ry+rs(idxj)*sin(idxj*a)];
%                             pt3=[rx+rs(idxm)/cos(a)*cos(idxm*a),ry+rs(idxm)/cos(a)*sin(idxm*a)];
%                             pt4=[rx+rs(idxk)/cos(2*a)*cos(idxk*a),ry+rs(idxk)/cos(2*a)*sin(idxk*a)];
%                         else
%                             pt1=[rx+rs(idxl)*cos(idxn*a),ry+rs(idxl)*sin(idxn*a)];
%                             pt2=[rx+rs(idxj)*cos(idxl*a),ry+rs(idxj)*sin(idxl*a)];
%                             pt3=[rx+rs(idxm)/cos(a)*cos(idxk*a),ry+rs(idxm)/cos(a)*sin(idxk*a)];
%                             pt4=[rx+rs(idxk)/cos(2*a)*cos(idxi*a),ry+rs(idxk)/cos(2*a)*sin(idxi*a)];
%                         end
%                     else%D1_1
%                         %mdlrecg='D1_1';
%                         if ~xchng
%                             pt1=[rx+rs(idxm)*cos(idxo*a),ry+rs(idxm)*sin(idxo*a)];
%                             pt2=[rx+rs(idxk)*cos(idxm*a),ry+rs(idxk)*sin(idxm*a)];
%                             pt3=[rx+rs(idxl)/cos(a)*cos(idxj*a),ry+rs(idxl)/cos(a)*sin(idxj*a)];
%                             pt4=[rx+rs(idxl)/cos(2*a)*cos(idxi*a),ry+rs(idxl)/cos(2*a)*sin(idxi*a)];
%                         else
%                             pt1=[rx+rs(idxm)*cos(idxm*a),ry+rs(idxm)*sin(idxm*a)];
%                             pt2=[rx+rs(idxk)*cos(idxk*a),ry+rs(idxk)*sin(idxk*a)];
%                             pt3=[rx+rs(idxl)/cos(a)*cos(idxl*a),ry+rs(idxl)/cos(a)*sin(idxl*a)];
%                             pt4=[rx+rs(idxl)/cos(2*a)*cos(idxj*a),ry+rs(idxl)/cos(2*a)*sin(idxj*a)];
%                         end
%                     end
                    intpt=[-((-(pt3(1)-pt4(1))*(pt2(1)*pt1(2)-pt1(1)*pt2(2))+(pt1(1)-pt2(1))*(pt4(1)*pt3(2)-pt3(1)*pt4(2)))/((pt3(1)-pt4(1))*(pt1(2)-pt2(2))+(pt1(1)-pt2(1))*(-pt3(2)+pt4(2)))),...
                        (pt4(1)*(pt1(2)-pt2(2))*pt3(2)+pt1(1)*pt2(2)*pt3(2)-pt3(1)*pt1(2)*pt4(2)-pt1(1)*pt2(2)*pt4(2)+pt3(1)*pt2(2)*pt4(2)+pt2(1)*pt1(2)*(-pt3(2)+pt4(2)))/(pt4(1)*(pt1(2)-pt2(2))+pt3(1)*(-pt1(2)+pt2(2))+(pt1(1)-pt2(1))*(pt3(2)-pt4(2)))];
                    modelpt([idxj,idxk],:)=[pt2,intpt;pt4,intpt];%!!!
                else
                    %D2
                    mdl(k)=12;
                    %finished(k)=true;
                    if dsj>dsi/cos(a)&&dsk>dsi/cos(a)%D2_1, link
                        %mdlrecg='D2_1';
                        if ~xchng
                            pt1=[rx+rs(idxj)*cos(idxj*a),ry+rs(idxj)*sin(idxj*a)];
                            pt2=[rx+rs(idxi)*cos(idxi*a),ry+rs(idxi)*sin(idxi*a)];
                            pt3=[rx+rs(idxk)*cos(idxm*a),ry+rs(idxk)*sin(idxm*a)];
                            pt4=[rx+rs(idxi)*cos(idxk*a),ry+rs(idxi)*sin(idxk*a)];
                        else
                            pt1=[rx+rs(idxj)*cos(idxl*a),ry+rs(idxj)*sin(idxl*a)];
                            pt2=[rx+rs(idxi)*cos(idxj*a),ry+rs(idxi)*sin(idxj*a)];
                            pt3=[rx+rs(idxk)*cos(idxk*a),ry+rs(idxk)*sin(idxk*a)];
                            pt4=[rx+rs(idxi)*cos(idxi*a),ry+rs(idxi)*sin(idxi*a)];
                        end
                    else%D2_2
                        %mdlrecg='D2_2';
                        if ~xchng
                            pt1=[rx+rs(idxj)*cos(idxj*a),ry+rs(idxj)*sin(idxj*a)];
                            pt2=[rx+rs(idxj)/cos(a)*cos(idxi*a),ry+rs(idxj)/cos(a)*sin(idxi*a)];
                            pt3=[rx+rs(idxk)*cos(idxm*a),ry+rs(idxk)*sin(idxm*a)];
                            pt4=[rx+rs(idxk)/cos(a)*cos(idxk*a),ry+rs(idxk)/cos(a)*sin(idxk*a)];
                        else
                            pt1=[rx+rs(idxj)*cos(idxl*a),ry+rs(idxj)*sin(idxl*a)];
                            pt2=[rx+rs(idxj)/cos(a)*cos(idxj*a),ry+rs(idxj)/cos(a)*sin(idxj*a)];
                            pt3=[rx+rs(idxk)*cos(idxk*a),ry+rs(idxk)*sin(idxk*a)];
                            pt4=[rx+rs(idxk)/cos(a)*cos(idxi*a),ry+rs(idxk)/cos(a)*sin(idxi*a)];
                        end
                    end
                    intpt=[-((-(pt3(1)-pt4(1))*(pt2(1)*pt1(2)-pt1(1)*pt2(2))+(pt1(1)-pt2(1))*(pt4(1)*pt3(2)-pt3(1)*pt4(2)))/((pt3(1)-pt4(1))*(pt1(2)-pt2(2))+(pt1(1)-pt2(1))*(-pt3(2)+pt4(2)))),...
                        (pt4(1)*(pt1(2)-pt2(2))*pt3(2)+pt1(1)*pt2(2)*pt3(2)-pt3(1)*pt1(2)*pt4(2)-pt1(1)*pt2(2)*pt4(2)+pt3(1)*pt2(2)*pt4(2)+pt2(1)*pt1(2)*(-pt3(2)+pt4(2)))/(pt4(1)*(pt1(2)-pt2(2))+pt3(1)*(-pt1(2)+pt2(2))+(pt1(1)-pt2(1))*(pt3(2)-pt4(2)))];
                    modelpt([idxj,idxk],:)=[pt1,intpt;pt3,intpt];
                end
            else%?
                unknown=true;
                mdl(k)=17;
            end
        else%A1, B3,4,5, C, D3,4
            if dsi-dsk>2*r%||dsk==inf %i--->k/j||k/j==nil, %A1,B4,5
                if dsj-dsi>2*r%j/k--->i
                    %A1
                    %mdlrecg='A1';
                    mdl(k)=1;
                    %finished(k)=true;
                    modelpt(k,1:2)=[rx+rs(k)*cos((k-3/4)*a),ry+rs(k)*sin((k-3/4)*a)];
                    modelpt(k,3:4)=[rx+rs(k)*cos((k-1/4)*a),ry+rs(k)*sin((k-1/4)*a)];
                elseif dsj-dsi<2*r%j/k--/->i %B4,5? THE SAME!
                    if (dsl>dsj)&&(dsl-dsj<2*r)%l-->j
                        %B4
                        mdl(k)=7;
                        %finished(k)=true;
                        if dsj<dsi/cos(a)%B4_2
                            %mdlrecg='B4_2';
                            if ~xchng
                                modelpt(k,1:2)=[rx+rs(idxi)*cos(idxk*a),ry+rs(idxi)*sin(idxk*a)];
                                modelpt(k,3:4)=[rx+rs(idxi)/cos(a)*cos(idxi*a),ry+rs(idxi)/cos(a)*sin(idxi*a)];
                            else
                                modelpt(k,1:2)=[rx+rs(idxi)*cos(idxi*a),ry+rs(idxi)*sin(idxi*a)];
                                modelpt(k,3:4)=[rx+rs(idxi)/cos(a)*cos(idxj*a),ry+rs(idxi)/cos(a)*sin(idxj*a)];
                            end
                        else%B4_1
                            %mdlrecg='B4_1';
                            if ~xchng
                                modelpt(k,1:2)=[rx+rs(idxi)*cos(idxk*a),ry+rs(idxi)*sin(idxk*a)];
                                modelpt(k,3:4)=[rx+rs(idxj)*cos(idxi*a),ry+rs(idxj)*sin(idxi*a)];
                            else
                                modelpt(k,1:2)=[rx+rs(idxi)*cos(idxi*a),ry+rs(idxi)*sin(idxi*a)];
                                modelpt(k,3:4)=[rx+rs(idxj)*cos(idxj*a),ry+rs(idxj)*sin(idxj*a)];
                            end
                        end
                    else
                        %B5
                        mdl(k)=8;
                        %finished(k)=true;
                        if dsj<dsi/cos(a)%B5_2
                            %mdlrecg='B5_2';
                            if ~xchng
                                modelpt(k,1:2)=[rx+rs(idxi)*cos(idxk*a),ry+rs(idxi)*sin(idxk*a)];
                                modelpt(k,3:4)=[rx+rs(idxi)/cos(a)*cos(idxi*a),ry+rs(idxi)/cos(a)*sin(idxi*a)];
                            else
                                modelpt(k,1:2)=[rx+rs(idxi)*cos(idxi*a),ry+rs(idxi)*sin(idxi*a)];
                                modelpt(k,3:4)=[rx+rs(idxi)/cos(a)*cos(idxj*a),ry+rs(idxi)/cos(a)*sin(idxj*a)];
                            end
                        else%B5_1
                            %mdlrecg='B5_1';
                            if ~xchng
                                modelpt(k,1:2)=[rx+rs(idxi)*cos(idxk*a),ry+rs(idxi)*sin(idxk*a)];
                                modelpt(k,3:4)=[rx+rs(idxj)*cos(idxi*a),ry+rs(idxj)*sin(idxi*a)];
                            else
                                modelpt(k,1:2)=[rx+rs(idxi)*cos(idxi*a),ry+rs(idxi)*sin(idxi*a)];
                                modelpt(k,3:4)=[rx+rs(idxj)*cos(idxj*a),ry+rs(idxj)*sin(idxj*a)];
                            end
                        end
                    end
                else%?
                    unknown=true;
                    mdl(k)=17;
                end
            elseif dsi-dsk<2*r%i--/->k, %B3, C, D3,4 %?
                if dsj-dsi>2*r%j--->i
                    %B3 %!!! ->link!
                    %mdlrecg='B3';
                    mdl(k)=6;
                    %finished(k)=true;
%                     if ~xchng
%                         modelpt(k,1:2)=[rx+rs(idxi)*cos(idxk*a),ry+rs(idxi)*sin(idxk*a)];
%                         modelpt(k,3:4)=[rx+rs(idxk)/cos(2*a)*cos(idxi*a),ry+rs(idxk)/cos(2*a)*sin(idxi*a)];
%                     else
%                         modelpt(k,1:2)=[rx+rs(idxi)*cos(idxi*a),ry+rs(idxi)*sin(idxi*a)];
%                         modelpt(k,3:4)=[rx+rs(idxk)/cos(2*a)*cos(idxj*a),ry+rs(idxk)/cos(2*a)*sin(idxj*a)];
%                     end
                    if ~xchng
                        modelpt(k,1:2)=[rx+rs(idxi)*cos(idxk*a),ry+rs(idxi)*sin(idxk*a)];
                        modelpt(k,3:4)=[rx+rs(idxj)*cos(idxi*a),ry+rs(idxj)*sin(idxi*a)];
                    else
                        modelpt(k,1:2)=[rx+rs(idxi)*cos(idxi*a),ry+rs(idxi)*sin(idxi*a)];
                        modelpt(k,3:4)=[rx+rs(idxj)*cos(idxj*a),ry+rs(idxj)*sin(idxj*a)];
                    end
                elseif dsj-dsi<2*r%j--/->i, %C, D3,4
                    if (dsj>dsl)&&(dsj<dsl/cos(a))%j--/->l, %D3,4 %!!!
                        modelpt(k,:)=nan;
                        if (dsl>dsn)&&(dsl-dsn<2*r)%n<-/--l
                            %D3
                            mdl(k)=13;
                            %finished(k)=true;
                            finished([idxj,idxk])=true;
                            if dsl<dsn/cos(a)%dsj<dsi/cos(a) %!!!
                                %D3_1
                                %!!! ->t: n/k; l: k/n
                                %mdlrecg='D3_1';
                                if ~xchng
                                    pt1=[rx+rs(idxl)*cos(idxl*a),ry+rs(idxl)*sin(idxl*a)];
                                    pt2=[rx+rs(idxn)/cos(2*a)*cos(idxj*a),ry+rs(idxn)/cos(2*a)*sin(idxj*a)];
                                    pt3=[rx+rs(idxk)*cos(idxm*a),ry+rs(idxk)*sin(idxm*a)];
                                    pt4=[rx+rs(idxi)*cos(idxk*a),ry+rs(idxi)*sin(idxk*a)];
                                else
                                    pt1=[rx+rs(idxl)*cos(idxn*a),ry+rs(idxl)*sin(idxn*a)];
                                    pt2=[rx+rs(idxn)/cos(2*a)*cos(idxl*a),ry+rs(idxn)/cos(2*a)*sin(idxl*a)];
                                    pt3=[rx+rs(idxk)*cos(idxk*a),ry+rs(idxk)*sin(idxk*a)];
                                    pt4=[rx+rs(idxi)*cos(idxi*a),ry+rs(idxi)*sin(idxi*a)];
                                end
                                intpt=[-((-(pt3(1)-pt4(1))*(pt2(1)*pt1(2)-pt1(1)*pt2(2))+(pt1(1)-pt2(1))*(pt4(1)*pt3(2)-pt3(1)*pt4(2)))/((pt3(1)-pt4(1))*(pt1(2)-pt2(2))+(pt1(1)-pt2(1))*(-pt3(2)+pt4(2)))),...
                                    (pt4(1)*(pt1(2)-pt2(2))*pt3(2)+pt1(1)*pt2(2)*pt3(2)-pt3(1)*pt1(2)*pt4(2)-pt1(1)*pt2(2)*pt4(2)+pt3(1)*pt2(2)*pt4(2)+pt2(1)*pt1(2)*(-pt3(2)+pt4(2)))/(pt4(1)*(pt1(2)-pt2(2))+pt3(1)*(-pt1(2)+pt2(2))+(pt1(1)-pt2(1))*(pt3(2)-pt4(2)))];
                                modelpt([idxj,idxk],:)=[pt1,intpt;pt4,intpt];%!!!
                            else%D3_2 %!!!
                                %mdlrecg='D3_2';
                                if ~xchng
                                    pt1=[rx+rs(idxl)*cos(idxl*a),ry+rs(idxl)*sin(idxl*a)];
                                    pt2=[rx+rs(idxj)*cos(idxj*a),ry+rs(idxj)*sin(idxj*a)];
                                    pt3=[rx+rs(idxk)*cos(idxm*a),ry+rs(idxk)*sin(idxm*a)];
                                    pt4=[rx+rs(idxk)/cos(a)*cos(idxk*a),ry+rs(idxk)/cos(a)*sin(idxk*a)];
                                else
                                    pt1=[rx+rs(idxl)*cos(idxn*a),ry+rs(idxl)*sin(idxn*a)];
                                    pt2=[rx+rs(idxj)*cos(idxl*a),ry+rs(idxj)*sin(idxl*a)];
                                    pt3=[rx+rs(idxk)*cos(idxk*a),ry+rs(idxk)*sin(idxk*a)];
                                    pt4=[rx+rs(idxk)/cos(a)*cos(idxi*a),ry+rs(idxk)/cos(a)*sin(idxi*a)];
                                end
                                intpt=[-((-(pt3(1)-pt4(1))*(pt2(1)*pt1(2)-pt1(1)*pt2(2))+(pt1(1)-pt2(1))*(pt4(1)*pt3(2)-pt3(1)*pt4(2)))/((pt3(1)-pt4(1))*(pt1(2)-pt2(2))+(pt1(1)-pt2(1))*(-pt3(2)+pt4(2)))),...
                                    (pt4(1)*(pt1(2)-pt2(2))*pt3(2)+pt1(1)*pt2(2)*pt3(2)-pt3(1)*pt1(2)*pt4(2)-pt1(1)*pt2(2)*pt4(2)+pt3(1)*pt2(2)*pt4(2)+pt2(1)*pt1(2)*(-pt3(2)+pt4(2)))/(pt4(1)*(pt1(2)-pt2(2))+pt3(1)*(-pt1(2)+pt2(2))+(pt1(1)-pt2(1))*(pt3(2)-pt4(2)))];
                                modelpt([idxj,idxk],:)=[pt2,intpt;pt3,intpt];%!!!
                            end
                        else
                            %D4
                            %mdlrecg='D4';
                            mdl(k)=14;
                            %finished(k)=true;
                            finished(idxk)=true;
                            if ~xchng
                                pt1=[rx+rs(idxi)*cos(idxi*a),ry+rs(idxi)*sin(idxi*a)];
                                pt2=[rx+rs(idxi)/cos(a)*cos(idxk*a),ry+rs(idxi)/cos(a)*sin(idxk*a)];
                                pt3=[rx+rs(idxk)*cos(idxm*a),ry+rs(idxk)*sin(idxm*a)];
                                pt4=[rx+rs(idxi)*cos(idxk*a),ry+rs(idxi)*sin(idxk*a)];
                            else
                                pt1=[rx+rs(idxi)*cos(idxj*a),ry+rs(idxi)*sin(idxj*a)];
                                pt2=[rx+rs(idxi)/cos(a)*cos(idxi*a),ry+rs(idxi)/cos(a)*sin(idxi*a)];
                                pt3=[rx+rs(idxk)*cos(idxk*a),ry+rs(idxk)*sin(idxk*a)];
                                pt4=[rx+rs(idxi)*cos(idxi*a),ry+rs(idxi)*sin(idxi*a)];
                            end
                            intpt=[-((-(pt3(1)-pt4(1))*(pt2(1)*pt1(2)-pt1(1)*pt2(2))+(pt1(1)-pt2(1))*(pt4(1)*pt3(2)-pt3(1)*pt4(2)))/((pt3(1)-pt4(1))*(pt1(2)-pt2(2))+(pt1(1)-pt2(1))*(-pt3(2)+pt4(2)))),...
                                (pt4(1)*(pt1(2)-pt2(2))*pt3(2)+pt1(1)*pt2(2)*pt3(2)-pt3(1)*pt1(2)*pt4(2)-pt1(1)*pt2(2)*pt4(2)+pt3(1)*pt2(2)*pt4(2)+pt2(1)*pt1(2)*(-pt3(2)+pt4(2)))/(pt4(1)*(pt1(2)-pt2(2))+pt3(1)*(-pt1(2)+pt2(2))+(pt1(1)-pt2(1))*(pt3(2)-pt4(2)))];
                            modelpt([idxj,idxk],:)=[pt1,intpt;pt3,intpt];
                        end
                    else
                        %C
                        %mdlrecg='C';
                        mdl(k)=10;
                        %finished(k)=true;
                        if ~xchng
                            modelpt(k,1:2)=[rx+rs(idxi)*cos(idxk*a),ry+rs(idxi)*sin(idxk*a)];
                            modelpt(k,3:4)=[rx+rs(idxj)*cos(idxi*a),ry+rs(idxj)*sin(idxi*a)];
                        else
                            modelpt(k,1:2)=[rx+rs(idxi)*cos(idxi*a),ry+rs(idxi)*sin(idxi*a)];
                            modelpt(k,3:4)=[rx+rs(idxj)*cos(idxj*a),ry+rs(idxj)*sin(idxj*a)];
                        end
%                         if dsi<dsk/cos(a)%dsj>dsi/cos(a)%C_2 %!!!
%                             %mdlrecg='C_2';
% %                             if ~xchng
% %                                 modelpt(k,1:2)=[rx+rs(idxk)/cos(a/2)*cos(idxk*a),ry+rs(idxk)/cos(a/2)*sin(idxk*a)];
% %                                 modelpt(k,3:4)=[rx+rs(idxk)/cos(3/2*a)*cos(idxi*a),ry+rs(idxk)/cos(3/2*a)*sin(idxi*a)];
% %                             else
% %                                 modelpt(k,1:2)=[rx+rs(idxk)/cos(a/2)*cos(idxi*a),ry+rs(idxk)/cos(a/2)*sin(idxi*a)];
% %                                 modelpt(k,3:4)=[rx+rs(idxk)/cos(3/2*a)*cos(idxj*a),ry+rs(idxk)/cos(3/2*a)*sin(idxj*a)];
% %                             end
%                             if ~xchng
%                                 modelpt(k,1:2)=[rx+rs(idxi)*cos(idxk*a),ry+rs(idxi)*sin(idxk*a)];
%                                 modelpt(k,3:4)=[rx+rs(idxk)/cos(2*a)*cos(idxi*a),ry+rs(idxk)/cos(2*a)*sin(idxi*a)];
%                             else
%                                 modelpt(k,1:2)=[rx+rs(idxi)*cos(idxi*a),ry+rs(idxi)*sin(idxi*a)];
%                                 modelpt(k,3:4)=[rx+rs(idxk)/cos(2*a)*cos(idxj*a),ry+rs(idxk)/cos(2*a)*sin(idxj*a)];
%                             end
%                         else%C_1
%                             %mdlrecg='C_1';
%                             if ~xchng
%                                 modelpt(k,1:2)=[rx+rs(idxi)*cos(idxk*a),ry+rs(idxi)*sin(idxk*a)];
%                                 modelpt(k,3:4)=[rx+rs(idxj)*cos(idxi*a),ry+rs(idxj)*sin(idxi*a)];
%                             else
%                                 modelpt(k,1:2)=[rx+rs(idxi)*cos(idxi*a),ry+rs(idxi)*sin(idxi*a)];
%                                 modelpt(k,3:4)=[rx+rs(idxj)*cos(idxj*a),ry+rs(idxj)*sin(idxj*a)];
%                             end
%                         end
                    end
                else%?
                    unknown=true;
                    mdl(k)=17;
                end
            else%?
                unknown=true;
                mdl(k)=17;
            end
        end
    else%A2, B1,2, E
        if dsj-dsi>2*r%j/k--->i, %A2, B1,2
            if dsk-dsi>2*r%k/j--->i
                %A2
                %mdlrecg='A2';
                mdl(k)=2;
                %finished(k)=true;
                modelpt(k,1:2)=[rx+rs(k)*cos((k-3/4)*a),ry+rs(k)*sin((k-3/4)*a)];
                modelpt(k,3:4)=[rx+rs(k)*cos((k-1/4)*a),ry+rs(k)*sin((k-1/4)*a)];
            else
                if (dsm>dsk)&&(dsm-dsk<2*r)%k<--m
                    %B1
                    mdl(k)=4;
                    %finished(k)=true;
                    if dsk<dsi/cos(a)%B1_2
                        %mdlrecg='B1_2';
                        if ~xchng
                            modelpt(k,1:2)=[rx+rs(idxi)*cos(idxi*a),ry+rs(idxi)*sin(idxi*a)];
                            modelpt(k,3:4)=[rx+rs(idxi)/cos(a)*cos(idxk*a),ry+rs(idxi)/cos(a)*sin(idxk*a)];
                        else
                            modelpt(k,1:2)=[rx+rs(idxi)*cos(idxj*a),ry+rs(idxi)*sin(idxj*a)];
                            modelpt(k,3:4)=[rx+rs(idxi)/cos(a)*cos(idxi*a),ry+rs(idxi)/cos(a)*sin(idxi*a)];
                        end
                    else%B1_1
                        %mdlrecg='B1_1';
                        if ~xchng
                            modelpt(k,1:2)=[rx+rs(idxi)*cos(idxi*a),ry+rs(idxi)*sin(idxi*a)];
                            modelpt(k,3:4)=[rx+rs(idxk)*cos(idxk*a),ry+rs(idxk)*sin(idxk*a)];
                        else
                            modelpt(k,1:2)=[rx+rs(idxi)*cos(idxj*a),ry+rs(idxj)*sin(idxi*a)];
                            modelpt(k,3:4)=[rx+rs(idxk)*cos(idxi*a),ry+rs(idxk)*sin(idxi*a)];
                        end
                    end
                else
                    %B2
                    %mdlrecg='B2';
                    mdl(k)=5;
                    %!!! ->model like A
                    modelpt(k,1:2)=[rx+rs(k)*cos((k-3/4)*a),ry+rs(k)*sin((k-3/4)*a)];
                    modelpt(k,3:4)=[rx+rs(k)*cos((k-1/4)*a),ry+rs(k)*sin((k-1/4)*a)];
                    %finished(k)=true;
%                     if ~xchng
%                         modelpt(k,1:2)=[rx+rs(idxi)*cos((idxi-1/2)*a),ry+rs(idxi)*sin((idxi-1/2)*a)];
%                         modelpt(k,3:4)=[rx+rs(idxk)*cos(idxk*a),ry+rs(idxk)*sin(idxk*a)];
%                     else
%                         modelpt(k,1:2)=[rx+rs(idxi)*cos((idxi-1/2)*a),ry+rs(idxi)*sin((idxi-1/2)*a)];
%                         modelpt(k,3:4)=[rx+rs(idxk)*cos(idxi*a),ry+rs(idxk)*sin(idxi*a)];
%                     end
                end
            end
        else
            %E
            %mdlrecg='E';
            mdl(k)=15;
            %finished(k)=true;
            if ~xchng
                modelpt(k,1:2)=[rx+rs(idxk)*cos(idxk*a),ry+rs(idxk)*sin(idxk*a)];
                modelpt(k,3:4)=[rx+rs(idxi)*cos((idxi-1/2)*a),ry+rs(idxi)*sin((idxi-1/2)*a)];
                mptadd=[mptadd;modelpt(k,3:4),[rx+rs(idxj)*cos(idxi*a),ry+rs(idxj)*sin(idxi*a)]];
            else
                modelpt(k,1:2)=[rx+rs(idxk)*cos(idxi*a),ry+rs(idxk)*sin(idxi*a)];
                modelpt(k,3:4)=[rx+rs(idxi)*cos((idxi-1/2)*a),ry+rs(idxi)*sin((idxi-1/2)*a)];
                mptadd=[mptadd;modelpt(k,3:4),[rx+rs(idxj)*cos(idxj*a),ry+rs(idxj)*sin(idxj*a)]];
            end
            if abs(vectorangle((modelpt(k,3:4)-modelpt(k,1:2)),(mptadd(end,1:2)-mptadd(end,3:4)),1)>2.96)
                modelpt(k,3:4)=mptadd(end,3:4);
                mptadd(end,:)=[];
                %mdlrecg='E2C';
            end
        end
    end
    end
    if ~unknown
        finished(k)=true;
    end
%     %debug
%     plot(modelpt(k,[1,3]),modelpt(k,[2,4]),'k');%
%     text(rx+3.2*cos((k-.5)*a),ry+3.2*sin((k-.5)*a),mdlrecg);%
%     %
end
end


Contact us