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=32;
maxdepth=100;
maxwidth=1;
schart=[size(chart,1),size(chart,2)];
[rBeg,cBeg]=ind2sub(schart,aIndex);
[rEnd,cEnd]=ind2sub(schart,bIndex);
[DR,DC]=ndgrid(-maxThrottle:maxThrottle,-maxThrottle:maxThrottle);
idx=find(abs(DR)+abs(DC)<=maxThrottle);
DR=DR(idx);DC=DC(idx);
[tBegf,sBegf,cBegf,icBegf,tEndb,sEndb,cEndb,icEndb,dBegfEndb,iBegfiEndb,okBegfEndb,c0Begf]=trajlink(rBeg,cBeg,rEnd,cEnd);
[tEndf,sEndf,cEndf,icEndf,tBegb,sBegb,cBegb,icBegb,dEndfBegb,iEndfiBegb,okEndfBegb]=trajlink(rEnd,cEnd,rBeg,cBeg);
[dEndbEndf,okEndbEndf]=trajdistbf(tEndb,tEndf,sEndb,sEndf,icEndb,icEndf);
c=c0Begf;
c=repmat(c',[1,size(dBegfEndb,2)])+dBegfEndb;
[c,i1]=min(c,[],1);
c=repmat(c',[1,size(dEndbEndf,2)])+dEndbEndf;
[c,i2]=min(c,[],1);
c=repmat(c',[1,size(dEndfBegb,2)])+dEndfBegb;
[c,i3]=min(c,[],1);
[c,i4]=min(c);
i3=i3(i4);
i2=i2(i3);
i1=i1(i2);
%if dodisp,if ~okBegfEndb(i1,i2), disp('1-2 break'); end;if ~okEndbEndf(i2,i3), disp('2-3 break'); end;if ~okEndfBegb(i3,i4), disp('3-4 break'); end;end
if ~isinf(c),
[iBegf,iEndb]=ind2sub([sBegf(i1)-2,sEndb(i2)-2],iBegfiEndb(i1,i2));iBegf=iBegf+1;iEndb=iEndb+2;
[iEndf,iBegb]=ind2sub([sEndf(i3)-2,sBegb(i4)-2],iEndfiBegb(i3,i4));iEndf=iEndf+1;iBegb=iBegb+2;
Pos=cat(1,tBegf{i1}(2:iBegf,:),tEndb{i2}(iEndb:end-1,:),tEndf{i3}(2+1:iEndf,:),tBegb{i4}(iBegb:end-1,:));
[thrustRow,thrustCol,cost]=trajinv(Pos);
%if dodisp,fh=gcf;figure(3);disptraj([]);disptraj(Pos);figure(fh);end
end
if isempty(thrustRow)||cost>maxcost
%if dodisp,disp('trying 1-4');end
[dBegfBegb,iBegfiBegb]=trajdistfbm(tBegf,tBegb,sBegf,sBegb,icBegf,icBegb);
c=dBegfBegb;
[c,i1]=min(c,[],1);
[c,i2]=min(c);
i1=i1(i2);
if ~isinf(c),
[iBegf,iBegb]=ind2sub([sBegf(i1)-2,sBegb(i2)-2],iBegfiBegb(i1,i2));iBegf=iBegf+1;iBegb=iBegb+2;
Pos=cat(1,tBegf{i1}(2:iBegf,:),tBegb{i2}(iBegb:end-1,:));
[thrustRow2,thrustCol2,cost2]=trajinv(Pos);
if isempty(thrustRow)||cost2<cost, thrustRow=thrustRow2; thrustCol=thrustCol2; cost=cost2; end
end
end
%if dodisp,disp(['Cost ',num2str([c,cost])]);end
function [tBegf,sBegf,cBegf,icBegf,tEndb,sEndb,cEndb,icEndb,dBegfEndb,iBegfiEndb,okBegfEndb,c0Begf]=trajlink(trBeg,tcBeg,trEnd,tcEnd)
tr=trBeg+chart(trBeg,tcBeg,1)+DR;
tc=tcBeg+chart(trBeg,tcBeg,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));
c0Begf=zeros(1,numel(dr));
for niter=1:numel(dr),
rdr=dr(niter);
cdc=dc(niter);
tt=trajforw(trBeg,tcBeg,0,0,rdr,cdc);
tBegf{niter}=tt;
sBegf(niter)=size(tt,1);
cBegf(niter)=sum(abs(tt(end-1,:)-[trEnd,tcEnd]).^2)+sum(abs(tt(end,:)-tt(end-1,:)).^2);
c0Begf(niter)=abs(rdr)+abs(cdc);
end
[nill,icBegf]=sort(cBegf);
tr=trEnd-DR;
tc=tcEnd-DC;
idx=find(tr>=1&tr<=schart(1)&tc>=1&tc<=schart(2));
dr=DR(idx);dc=DC(idx);
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=trajback(trEnd,tcEnd,rdr,cdc,0,0);
tEndb{niter}=tt;
sEndb(niter)=size(tt,1);
cEndb(niter)=sum(abs(tt(2,:)-[trBeg,tcBeg]).^2)+sum(abs(tt(end,:)-tt(end-1,:)).^2);
end
[nill,icEndb]=sort(cEndb);
[dBegfEndb,iBegfiEndb,okBegfEndb]=trajdistfb(tBegf,tEndb,sBegf,sEndb,icBegf,icEndb);
end
function [md,id,ok]=trajdistfb(x1,x2,s1,s2,ic1,ic2)
m1=numel(x1);
m2=numel(x2);
md=inf(m1,m2);
id=zeros(m1,m2);
ok=ones(m1,m2);
for nn1=1:min(m1,maxcomp),
n1=ic1(nn1);
if s1(n1)>2,
for nn2=1:min(m2,maxcomp),
n2=ic2(nn2);
if s2(n2)>2,
dt=abs(x1{n1}(:,ones(1,size(x2{n2},1)))-x2{n2}(:,ones(1,size(x1{n1},1)))')+...
abs(x1{n1}(:,2+zeros(1,size(x2{n2},1)))-x2{n2}(:,2+zeros(1,size(x1{n1},1)))');
dt2a=dt(2:end-1,2:end-1);
dt2b=dt(3:end,3:end);
dt2=dt2a+dt2b;
penalize=dt2a>maxThrottle|dt2b>maxThrottle;
penalize(1,:)=true;penalize(:,end)=true;%if size(penalize,2)>1,penalize(:,end-1)=true;end
dt2(penalize)=dt2(penalize)+1e3;
[md(n1,n2),id(n1,n2)]=min(dt2(:));
if dt2a(id(n1,n2))>maxThrottle||dt2b(id(n1,n2))>maxThrottle, ok(n1,n2)=0; end
end
end
end
end
end
function [md,ok]=trajdistbf(x1,x2,s1,s2,ic1,ic2)
m1=numel(x1);
m2=numel(x2);
md=inf(m1,m2);
ok=ones(m1,m2);
for nn1=1:min(m1,maxcomp),
n1=ic1(nn1);
if s1(n1)>2,
x1t=x1{n1}(s1(n1)-1:end,:);
for nn2=1:min(m2,maxcomp),
n2=ic2(nn2);
if s2(n2)>2,
dt=abs(x1t(:,1)-x2{n2}(2:3,1))+...
abs(x1t(:,2)-x2{n2}(2:3,2));
md(n1,n2)=dt(1)+dt(2);
if dt(1)>maxThrottle||dt(2)>maxThrottle, ok(n1,n2)=0; end
end
end
end
end
end
function [md,id]=trajdistfbm(x1,x2,s1,s2,ic1,ic2)
m1=numel(x1);
m2=numel(x2);
md=inf(m1,m2);
id=zeros(m1,m2);
for nn1=1:min(m1,maxcomp),
n1=ic1(nn1);
if s1(n1)>2,
dt0=abs(x1{n1}(2:end-1,1)-rEnd).^2+abs(x1{n1}(2:end-1,2)-cEnd).^2;
for nn2=1:min(m2,maxcomp),
n2=ic2(nn2);
if s2(n2)>2,
dt=abs(x1{n1}(:,ones(1,size(x2{n2},1)))-x2{n2}(:,ones(1,size(x1{n1},1)))')+...
abs(x1{n1}(:,2+zeros(1,size(x2{n2},1)))-x2{n2}(:,2+zeros(1,size(x1{n1},1)))');
dt2a=dt(2:end-1,2:end-1);
dt2b=dt(3:end,3:end);
dt2=dt2a+dt2b;
penalize=dt2a>maxThrottle|dt2b>maxThrottle;
penalize(1,:)=true;penalize(:,end)=true;%if size(penalize,2)>1,penalize(:,end-1)=true;end
dt2(penalize)=dt2(penalize)+1e3;
dt2=dt2+dt0(:,ones(1,size(x2{n2},1)-2));
[md(n1,n2),id(n1,n2)]=min(dt2(:));
end
end
end
end
end
function y=trajforw(xr,xc,vr,vc,ar,ac)
x=zeros(maxlength+2,2);
x(1,:)=[xr-vr,xc-vc];
for ns=2:maxlength+1,
x(ns,:)=[xr,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;
x(ns,:)=[xr,xc];
break;
end
end
y=x(1:ns,:);
end
function y=trajback(xr,xc,vr,vc,ar,ac)
x=zeros(maxlength+2,2);
x(1,:)=[xr+vr+chart(xr,xc,1)+ar,xc+vc+chart(xr,xc,2)+ac];
for ns=2:maxlength+1,
x(ns,:)=[xr,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;
x(ns,:)=[xr,xc];
break;
end
ar=0;ac=0;
vr=vr-chart(xr,xc,1)-ar;
vc=vc-chart(xr,xc,2)-ac;
end
y=x(ns:-1:1,:);
end
function [tar,tac,cost]=trajinv(ty)
ny=size(ty,1);
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');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');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]=findlink(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]=trajinv(tty);
end
end
if isempty(tar)||cost>maxcost,
minc=inf;
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]=findlink(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]=trajinv(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] = findlink(xr,xc,yr,yc,vxr,vxc,vyr,vyc,level)
%if dodisp, disp(['iter', num2str([level,xr,xc,yr,yc])]); end
a1r = (yr-xr)-( vxr+chart(xr,xc,1) );
a1c = (yc-xc)-( vxc+chart(xr,xc,2) );
a2r = vyr-( (yr-xr)+chart(yr,yc,1) );
a2c = vyc-( (yc-xc)+chart(yr,yc,2) );
if abs(a1r)+abs(a1c)<=maxThrottle && abs(a2r)+abs(a2c)<=maxThrottle,
tylink=[xr,xc;yr,yc];ok=1;
elseif 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 tr(idr(ndr))~=xr||tc(idr(ndr))~=xc||vxr||vxc
[tylink,ok] = findlink(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)
% cla;
% [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
|
