function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)
%dodisp=0;
%if dodisp,disp([size(chart,1),size(chart,2),max(abs(chart(:))),maxThrottle]);end
thrustRow=[];thrustCol=[];
maxlength=100;
maxcost=10;
maxcomp=[128,128];
maxdepth=4;
maxwidth=3;
INF=1e6;
INFl=1e4;
schart=[size(chart,1),size(chart,2)];
[rBeg,cBeg]=ind2sub(schart,aIndex);
[rEnd,cEnd]=ind2sub(schart,bIndex);
maxthrottle=2*maxThrottle;
[BR,BC]=ndgrid(-maxthrottle:maxthrottle,-maxthrottle:maxthrottle);
idx=find(abs(BR)+abs(BC)<=maxthrottle);
BR=BR(idx);BC=BC(idx);
[DR,DC]=ndgrid(-maxThrottle:maxThrottle,-maxThrottle:maxThrottle);
idx=find(abs(DR)+abs(DC)<=maxThrottle);
DR=DR(idx);DC=DC(idx);
% [ER,EC]=ndgrid(1:schart(1),1:schart(2));
% idx=find(abs(2*rEnd-ER+chart(rEnd,cEnd,1)-max(1,min(schart(1),2*rEnd-ER+chart(rEnd,cEnd,1))))+abs(2*cEnd-EC+chart(rEnd,cEnd,2)-max(1,min(schart(2),2*cEnd-EC+chart(rEnd,cEnd,2))))<=maxThrottle);
% ER=ER(idx);EC=EC(idx);
baseboard=max(schart)+maxThrottle+max(abs(chart(:)));
maxboard=2*baseboard+max(schart);
DIST=1:maxboard;
DIST=DIST(ones(maxboard,1),:);
DIST=abs(DIST-DIST');
test_trajectory=zeros(maxlength+2,2);
tBegf={};tEndb={};tEndf={};tBegb={};
sBegf=[];sEndb=[];sEndf=[];sBegb=[];
cBegf=[];cEndb=[];cEndf=[];cBegb=[];
icBegf=[];icEndb=[];icEndf=[];icBegb=[];
aBegf=[];
DIST12=[];DIST23=[];DIST34=[];DIST2T=[];DISTT3=[];COST12=[];COST23=[];COST34=[];
create_trajectories(rBeg,cBeg,rBeg,cBeg,[1,1,1,1]);
[Pos,c]=link_trajectories;
[thrustRow,thrustCol,cost]=convert_trajectory_to_acceleration(Pos);
%if dodisp,disp(['Cost ',num2str([c,cost])]);end
%if dodisp,fh=gcf;figure(3);disptraj([]);disptraj(Pos);set(gcf,'name',num2str([c,cost]));figure(fh);end
function create_trajectories(trBegf,tcBegf,trBegb,tcBegb,computesegment)
%segment 1, BEGF, beginning forward (from origin to somewhere)
if computesegment(1)
tr=trBegf+chart(trBegf,tcBegf,1)+DR;
tc=tcBegf+chart(trBegf,tcBegf,2)+DC;
idx=find(tr>=1&tr<=schart(1)&tc>=1&tc<=schart(2));
dr=DR(idx);dc=DC(idx);
tBegf=cell(1,numel(dr));
sBegf=zeros(1,numel(dr));
cBegf=zeros(1,numel(dr));
aBegf=zeros(1,numel(dr));
for niter=1:numel(dr),
rdr=dr(niter);
cdc=dc(niter);
tt=single_trajectory_forward(trBegf,tcBegf,0,0,rdr,cdc);
tBegf{niter}=tt;
sBegf(niter)=size(tt,1);
aBegf(niter)=abs(rdr)+abs(cdc);
[tcost,itcost]=min( DIST(tt(2:end,1)+baseboard,rEnd+baseboard)+DIST(tt(2:end,2)+baseboard,cEnd+baseboard), [],1);
cBegf(niter)=tcost+sum(abs(tt(1+itcost,:)-tt(itcost,:)))+abs(rdr)+abs(cdc);
end
[nill,icBegf]=sort(cBegf);
end
%segment 2 ENDB, end backward (from somewhere to target)
if computesegment(2),
tr=rEnd-DR;
tc=cEnd-DC;
idx=find(tr>=1&tr<=schart(1)&tc>=1&tc<=schart(2));
dr=DR(idx);dc=DC(idx);
% dr=rEnd-ER;
% dc=cEnd-EC;
tEndb=cell(1,numel(dr));
sEndb=zeros(1,numel(dr));
cEndb=zeros(1,numel(dr));
for niter=1:numel(dr),
rdr=dr(niter);
cdc=dc(niter);
tt=single_trajectory_backward(rEnd,cEnd,rdr,cdc,0,0);
tEndb{niter}=tt;
sEndb(niter)=size(tt,1);
[tcost,itcost]=min( DIST(tt(1:end-1,1)+baseboard,trBegf+baseboard)+DIST(tt(1:end-1,2)+baseboard,tcBegf+baseboard), [],1);
cEndb(niter)=tcost+sum(abs(tt(1+itcost,:)-tt(itcost,:)))+abs(rdr)+abs(cdc);
end
[nill,icEndb]=sort(cEndb);
end
%segment 3 ENDF, end forward (from target to somewhere)
if computesegment(3),
tr=rEnd+chart(rEnd,cEnd,1)+BR;
tc=cEnd+chart(rEnd,cEnd,2)+BC;
idx=find(tr>=1&tr<=schart(1)&tc>=1&tc<=schart(2));
dr=BR(idx);dc=BC(idx);
% dr=ER-rEnd-chart(rEnd,cEnd,1);
% dc=EC-cEnd-chart(rEnd,cEnd,2);
tEndf=cell(1,numel(dr));
sEndf=zeros(1,numel(dr));
cEndf=zeros(1,numel(dr));
aEndf=zeros(1,numel(dr));
for niter=1:numel(dr),
rdr=dr(niter);
cdc=dc(niter);
tt=single_trajectory_forward(rEnd,cEnd,0,0,rdr,cdc);
tEndf{niter}=tt;
sEndf(niter)=size(tt,1);
aEndf(niter)=abs(rdr)+abs(cdc);
[tcost,itcost]=min( DIST(tt(2:end,1)+baseboard,trBegb+baseboard)+DIST(tt(2:end,2)+baseboard,tcBegb+baseboard), [],1);
cEndf(niter)=tcost+sum(abs(tt(1+itcost,:)-tt(itcost,:)))+abs(rdr)+abs(cdc);
end
[nill,icEndf]=sort(cEndf);
end
%segment 4 BEGB, beginning backward (from somewhere to origin)
if computesegment(4),
tr=trBegb-BR;
tc=tcBegb-BC;
idx=find(tr>=1&tr<=schart(1)&tc>=1&tc<=schart(2));
dr=BR(idx);dc=BC(idx);
tBegb=cell(1,numel(dr));
sBegb=zeros(1,numel(dr));
cBegb=zeros(1,numel(dr));
for niter=1:numel(dr),
rdr=dr(niter);
cdc=dc(niter);
tt=single_trajectory_backward(trBegb,tcBegb,rdr,cdc,0,0);
tBegb{niter}=tt;
sBegb(niter)=size(tt,1);
[tcost,itcost]=min( DIST(tt(1:end-1,1)+baseboard,rEnd+baseboard)+DIST(tt(1:end-1,2)+baseboard,cEnd+baseboard), [],1);
cBegb(niter)=tcost+sum(abs(tt(1+itcost,:)-tt(itcost,:)))+abs(rdr)+abs(cdc);
end
[nill,icBegb]=sort(cBegb);
end
end
function [Pos,costmin]=link_trajectories
x1=tBegf;s1=sBegf;ic1=icBegf;
x2=tEndb;s2=sEndb;ic2=icEndb;
x3=tEndf;s3=sEndf;ic3=icEndf;
x4=tBegb;s4=sBegb;ic4=icBegb;
l1=numel(ic1);
l2=numel(ic2);
l3=numel(ic3);
l4=numel(ic4);
is1=s1(ic1(1:min(l1,maxcomp(1))));
is2=s2(ic2(1:min(l2,maxcomp(1))));
is3=s3(ic3(1:min(l3,maxcomp(1))));
is4=s4(ic4(1:min(l4,maxcomp(1))));
bix1=[1;1+cumsum(is1(1:end-1))']; % index to beginning of each segment in x1 (set 1)
bix2=[1;1+cumsum(is2(1:end-1))']; % index to beginning of each segment in x2 (set 2)
bix3=[1;1+cumsum(is3(1:end-1))']; % index to beginning of each segment in x3 (set 3)
bix4=[1;1+cumsum(is4(1:end-1))']; % index to beginning of each segment in x4 (set 4)
eix1=cumsum(is1); % index to end of each segment (set 1)
eix2=cumsum(is2); % index to end of each segment (set 2)
eix3=cumsum(is3); % index to end of each segment (set 3)
eix4=cumsum(is4); % index to end of each segment (set 4)
ix1=zeros(eix1(end),1);ix1(bix1)=1;ix1=cumsum(ix1); % segment # (for each node) (set 1, index to ic1)
ix2=zeros(eix2(end),1);ix2(bix2)=1;ix2=cumsum(ix2); % segment # (for each node) (set 2, index to ic2)
ix3=zeros(eix3(end),1);ix3(bix3)=1;ix3=cumsum(ix3); % segment # (for each node) (set 3, index to ic3)
ix4=zeros(eix4(end),1);ix4(bix4)=1;ix4=cumsum(ix4); % segment # (for each node) (set 4, index to ic4)
% compute distances
xx1=cat(1,x1{ic1(1:min(l1,maxcomp(1)))})+baseboard;
xx2=cat(1,x2{ic2(1:min(l2,maxcomp(1)))})+baseboard;
xx3=cat(1,x3{ic3(1:min(l3,maxcomp(1)))})+baseboard;
xx4=cat(1,x4{ic4(1:min(l4,maxcomp(1)))})+baseboard;
if isempty(DIST12), DIST12=DIST(xx1(:,1),xx2(:,1))+DIST(xx1(:,2),xx2(:,2)); end % cityblock distance between nodes (set 1 vs. set 2)
if isempty(DIST23), DIST23=DIST(xx2(:,1),xx3(:,1))+DIST(xx2(:,2),xx3(:,2)); end % cityblock distance between nodes (set 2 vs. set 3)
if isempty(DIST34), DIST34=DIST(xx3(:,1),xx4(:,1))+DIST(xx3(:,2),xx4(:,2)); end % cityblock distance between nodes (set 3 vs. set 4)
if isempty(DIST2T), DIST2T=DIST(xx2(:,1),rEnd+baseboard).^2+DIST(xx2(:,2),cEnd+baseboard).^2; end
if isempty(DISTT3), DISTT3=DIST(rEnd+baseboard,xx3(:,1)).^2+DIST(cEnd+baseboard,xx3(:,2)).^2; end
% compute transition costs (forward viterbi step)
% segment 1 - segment 2a
c1=aBegf(ic1(1:min(l1,maxcomp(1))))';
c1=c1(ix1);
c1(bix1)=INF;c1(eix1)=INF; c1(bix1+1)=c1(bix1+1)+INFl;
if isempty(COST12),
c12a=DIST12(:,[1,1:end-1]);
c12b=DIST12([2:end,end],:);
COST12=c12a+c12b;
end
c12=c1(:,ones(1,eix2(end)))+COST12; % cost of jumping from node i (set 1) to node j (set 2) and maintaining the trajectory of node j
% c12(:,bix2)=INF;c12(:,eix2)=INF;
% idx=c12a>maxThrottle|c12b>maxThrottle;c12(idx)=c12(idx)+INFl;
[cost2a,idxset1]=min(c12,[],1); % best set 1 node for each set 2 node
idx=find(c12a(idxset1+size(c12,1)*(0:size(c12,2)-1))>maxThrottle|c12b(idxset1+size(c12,1)*(0:size(c12,2)-1))>maxThrottle);cost2a(idx)=cost2a(idx)+INFl;
% segment 2a - segment 2b
c2=cost2a'; % within-segment transition matrix
c2(bix2)=INF;c2(eix2)=INF; %c2(bix2+1)=c2(bix2+1)+INFl;
cost2b=zeros(1,numel(c2));idxset2a=cost2b;
c2_nset=1; c2_min=INF; c2_minidx=1;
for n1=1:numel(c2),
cost2b(n1)=c2_min; idxset2a(n1)=c2_minidx;
if c2(n1)<c2_min,c2_min=c2(n1);c2_minidx=n1;end;
if n1==eix2(c2_nset), c2_min=inf; c2_nset=c2_nset+1; end
end
% segment 2b - segment 3a
c2=cost2b';
c2(bix2)=INF;c2(eix2)=INF; %c2(bix2+1)=c2(bix2+1)+INFl;
if isempty(COST23),
c23a=DIST23(:,[1,1:end-1]);
c23b=DIST23([2:end,end],:);
COST23=c23a+c23b;
end
c23=c2(:,ones(1,eix3(end)))+COST23; % cost of jumping from node i (set 2) to node j (set 3) and maintaining the trajectory of node j
c23=c23+min(DISTT3(ones(eix2(end),1),:),DIST2T(:,ones(1,eix3(end))));
% c23(:,bix3)=INF;c23(:,eix3)=INF;
% idx=c23a>maxThrottle|c23b>maxThrottle;c23(idx)=c23(idx)+INFl;
[cost3a,idxset2b]=min(c23,[],1); % best set 2 node for each set 3 node
idx=find(c23a(idxset2b+size(c23,1)*(0:size(c23,2)-1))>maxThrottle|c23b(idxset2b+size(c23,1)*(0:size(c23,2)-1))>maxThrottle);cost3a(idx)=cost3a(idx)+INFl;
% segment 3a - segment 3b
c2=cost3a'; % within-segment transition matrix
c2(bix3)=INF;c2(eix3)=INF; %c2(bix3+1)=c2(bix3+1)+INFl;
cost3b=zeros(1,numel(c2));idxset3a=cost3b;
c2_nset=1; c2_min=INF; c2_minidx=1;
for n1=1:numel(c2),
cost3b(n1)=c2_min; idxset3a(n1)=c2_minidx;
if c2(n1)<c2_min,c2_min=c2(n1);c2_minidx=n1;end;
if n1==eix3(c2_nset), c2_min=inf; c2_nset=c2_nset+1; end
end
% segment 3b - segment 4
c3=cost3b';
c3(bix3)=INF;c3(eix3)=INF; %c3(bix3+1)=c3(bix3+1)+INFl;
if isempty(COST34),
c34a=DIST34(:,[1,1:end-1]);
c34b=DIST34([2:end,end],:);
COST34=c34a+c34b;
end
c34=c3(:,ones(1,eix4(end)))+COST34; % cost of jumping from node i (set 3) to node j (set 4) and maintaining the trajectory of node j
% c34(:,eix4)=INF;c34(:,eix4-1)=INF;
% idx=c34a>maxThrottle|c34b>maxThrottle;c34(idx)=c34(idx)+INFl;
[cost4,idxset3b]=min(c34,[],1); % best set 3 node for each set 4 node
idx=find(c34a(idxset3b+size(c34,1)*(0:size(c34,2)-1))>maxThrottle|c34b(idxset3b+size(c34,1)*(0:size(c34,2)-1))>maxThrottle);cost4(idx)=cost4(idx)+INFl;
% compute transition path (inverse viterbi step)
cost4(bix4)=INF;cost4(eix4)=INF;
[costmin,idxset4]=min(cost4); % set 4 node in optimal trajectory
idxset3b=idxset3b(idxset4); % set 3 node in optimal trajectory
idxset3a=idxset3a(idxset3b); % set 3 node in optimal trajectory
idxset2b=idxset2b(idxset3a); % set 2 node in optimal trajectory
idxset2a=idxset2a(idxset2b); % set 2 node in optimal trajectory
idxset1=idxset1(idxset2a); % set 1 node in optimal trajectory
% Pos=[];
% for n1=1:numel(idxset1),Pos=cat(1,Pos,xx1(bix1(ix1(idxset1{n1}))+1:idxset1{n1},:)); end
% Pos=cat(1,Pos,xx2(idxset2a:idxset2b,:),xx3(idxset3a:idxset3b,:));
% for n1=1:numel(idxset4),Pos=cat(1,Pos,xx4(idxset4{n1}:eix4(ix4(idxset4{n1}))-1,:)); end
% Pos=Pos-baseboard;
Pos=cat(1,...
xx1(bix1(ix1(idxset1))+1:idxset1,:),...
xx2(idxset2a:idxset2b,:),...
xx3(idxset3a:idxset3b,:),...
xx4(idxset4:eix4(ix4(idxset4))-1,:)) -baseboard;
%ix#(idxset#) % segment number (index to ic#)
%
end
function y=single_trajectory_forward(xr,xc,vr,vc,ar,ac)
test_trajectory(1,1)=xr-vr-ar;
test_trajectory(1,2)=xc-vc-ac;
for ns=2:maxlength+1,
test_trajectory(ns,1)=xr;
test_trajectory(ns,2)=xc;
vr=vr+chart(xr,xc,1)+ar;
vc=vc+chart(xr,xc,2)+ac;
xr=xr+vr;
xc=xc+vc;
ar=0;ac=0;
if xr<1||xr>schart(1)||xc<1||xc>schart(2)||(vr==0&&vc==0),
ns=ns+1;
test_trajectory(ns,1)=xr;
test_trajectory(ns,2)=xc;
break;
end
end
y=test_trajectory(1:ns,:);
end
function y=single_trajectory_backward(xr,xc,vr,vc,ar,ac)
test_trajectory(1,1)=xr+vr+chart(xr,xc,1)+ar;
test_trajectory(1,2)=xc+vc+chart(xr,xc,2)+ac;
for ns=2:maxlength+1,
test_trajectory(ns,1)=xr;
test_trajectory(ns,2)=xc;
xr=xr-vr;
xc=xc-vc;
if xr<1||xr>schart(1)||xc<1||xc>schart(2)||(vr==0&&vc==0),
ns=ns+1;
test_trajectory(ns,1)=xr;
test_trajectory(ns,2)=xc;
break;
end
ar=0;ac=0;
vr=vr-chart(xr,xc,1)-ar;
vc=vc-chart(xr,xc,2)-ac;
end
y=test_trajectory(ns:-1:1,:);
end
function [tar,tac,cost]=convert_trajectory_to_acceleration(ty)
ny=size(ty,1);
if ny<2, tar=[]; tac=[]; cost=inf; return; end
tar=zeros(ny-1,1);
tac=zeros(ny-1,1);
xr=ty(1,1);xc=ty(1,2);
vr=0;vc=0;
nns=1;
cost=min(abs(ty(:,1)-rEnd).^2+abs(ty(:,2)-cEnd).^2);
for ns=1:ny-1,
vr=vr+chart(xr,xc,1);
vc=vc+chart(xr,xc,2);
tar(nns)=ty(ns+1,1)-ty(ns,1)-vr;
tac(nns)=ty(ns+1,2)-ty(ns,2)-vc;
if abs(tar(nns))+abs(tac(nns))>maxThrottle
tar(nns)=-vr;
tac(nns)=-vc;
if abs(tar(nns))+abs(tac(nns))>maxThrottle,
tar=[];tac=[];
%if dodisp,disp(['not possible to fix ',num2str(ty(ns,:)),' to ',num2str(ty(ns+1,:))]);end
break;end
vr=vr+tar(nns);
vc=vc+tac(nns);
xr=xr+vr;
xc=xc+vc;
nns=nns+1;
vr=vr+chart(xr,xc,1);
vc=vc+chart(xr,xc,2);
tar(nns)=ty(ns+1,1)-ty(ns,1)-vr;
tac(nns)=ty(ns+1,2)-ty(ns,2)-vc;
if abs(tar(nns))+abs(tac(nns))>maxThrottle,
tar=[];tac=[];
%if dodisp,disp(['not possible to fix ',num2str(ty(ns,:)),' to ',num2str(ty(ns+1,:))]);end
break;end
vr=vr+tar(nns);
vc=vc+tac(nns);
xr=xr+vr;
xc=xc+vc;
nns=nns+1;
%if dodisp,disp('fixed');end
else
vr=vr+tar(nns);
vc=vc+tac(nns);
xr=xr+vr;
xc=xc+vc;
nns=nns+1;
end
end
cost=cost+sum(abs(tar))+sum(abs(tac));
if isempty(tar)
cost=inf;
if isempty(tar),
%if dodisp,disp('fix1');end
if ns==1,vr1=0;vc1=0; else vr1=ty(ns,1)-ty(ns-1,1); vc1=ty(ns,2)-ty(ns-1,2); end
if ns==ny-1,vr2=0;vc2=0; else vr2=ty(ns+2,1)-ty(ns+1,1); vc2=ty(ns+2,2)-ty(ns+1,2); end
[tylink,tylinkok]=find_link(ty(ns,1),ty(ns,2),ty(ns+1,1),ty(ns+1,2),vr1,vc1,vr2,vc2,0);
if tylinkok,
tty=[ty(1:ns-1,:);tylink;ty(ns+2:end,:)];
[tar,tac,cost]=convert_trajectory_to_acceleration(tty);
end
end
if isempty(tar)||cost>maxcost,
idxns=find(ty([ns,ns+1],1)==rEnd&ty([ns,ns+1],2)==cEnd);
if ~isempty(idxns)
%if dodisp,disp('fix2');end
ns=ns+idxns-1;
ty1=ty(1:ns-1,:);
ty2=ty(ns+1:end,:);
d=inf(ns-1,ny-ns-1);
for n=2:ns-1,
for m=1:ny-ns-1
a1 = (ty2(m,:)-ty1(n,:))-( (ty1(n,:)-ty1(n-1,:))+shiftdim(chart(ty1(n,1),ty1(n,2),:),1) );
a2 = (ty2(m+1,:)-ty2(m,:))-( (ty2(m,:)-ty1(n,:))+shiftdim(chart(ty2(m,1),ty2(m,2),:),1) );
c1 = sum(abs(a1));
c2 = sum(abs(a2));
d(n,m)=c1+c2+min(abs(ty1(n,1)-rEnd).^2+abs(ty1(n,2)-cEnd).^2,abs(ty2(m,1)-rEnd).^2+abs(ty2(m,2)-cEnd).^2);
end
end
[mind,imind]=sort(d(:));
imind=imind(~isinf(mind));
for nmind=1:numel(imind),
[n,m]=ind2sub(size(d),imind(nmind));
[tylink,tylinkok]=find_link(ty1(n,1),ty1(n,2),ty2(m,1),ty2(m,2),ty1(n,1)-ty1(n-1,1),ty1(n,2)-ty1(n-1,2),ty2(m+1,1)-ty2(m,1),ty2(m+1,2)-ty2(m,2),0);
if tylinkok,
tty=[ty1(1:n-1,:);tylink;ty2(m+1:end,:)];
[tar2,tac2,cost2]=convert_trajectory_to_acceleration(tty);
if cost2<=cost, tar=tar2;tac=tac2;cost=cost2; break; end
end
end
end
end
end
if isempty(tar), cost=inf; end
end
function [tylink,ok] = find_link(xr,xc,yr,yc,vxr,vxc,vyr,vyc,level)
%if dodisp, disp(['iter', num2str([level,xr,xc,yr,yc])]); end
if level<maxdepth,
tylink=[];ok=0;
tr=xr+chart(xr,xc,1)+vxr+DR;
tc=xc+chart(xr,xc,2)+vxc+DC;
if ~vxr&&~vxc, idx=find(tr>=1&tr<=schart(1)&tc>=1&tc<=schart(2)&(tr~=xr|tc~=xc));
else idx=find(tr>=1&tr<=schart(1)&tc>=1&tc<=schart(2)); end
tr=tr(idx);tc=tc(idx);
a1r = (yr-tr)-( (tr-xr)+chart(tr+schart(1)*(tc-1)) );
a1c = (yc-tc)-( (tc-xc)+chart(tr+schart(1)*(tc-1)+schart(1)*schart(2)) );
a2r = vyr-( (yr-tr)+chart(yr,yc,1) );
a2c = vyc-( (yc-tc)+chart(yr,yc,2) );
cdr=(abs(a1r)+abs(a1c)+abs(a2r)+abs(a2c))+(abs(tr-xr)+abs(tc-xc));
[nill,idr]=sort(cdr);
for ndr=1:min(maxwidth,numel(idr)),
if abs(a1r(idr(ndr)))+abs(a1c(idr(ndr)))<=maxThrottle && abs(a2r(idr(ndr)))+abs(a2c(idr(ndr)))<=maxThrottle,
tylink=[xr,xc;tr(idr(ndr)),tc(idr(ndr));yr,yc];
ok=1;
break;
elseif tr(idr(ndr))~=xr||tc(idr(ndr))~=xc||vxr||vxc
[tylink,ok] = find_link(tr(idr(ndr)),tc(idr(ndr)),yr,yc,tr(idr(ndr))-xr,tc(idr(ndr))-xc,vyr,vyc,level+1);
if ok,
tylink=cat(1,[xr,xc],tylink);
break;
end
end
end
else
tylink=[];ok=0;
end
end
% function disptraj(t)
% if isempty(t)
% clf;
% [ty,tx]=ndgrid(1:schart(1), 1:schart(2));
% quiver(tx,ty,chart(:,:,2),chart(:,:,1));
% hold all;
% plot(cBeg,rBeg,'ks',cEnd,rEnd,'kv','markerfacecolor','k');
% axis tight;
% else
% plot(t(:,2),t(:,1),'.-',t(1:2,2),t(1:2,1),'o');
% end
% end
end
|
