function [thrustRow, thrustCol] = pwsolver2(chart, aIndex, bIndex, maxThrottle)
[thrustRowVer1,thrustColVer1,ScoreVer1]= IndividualSolution(chart,aIndex,bIndex,maxThrottle,0.6,0);
thrustRow= thrustRowVer1;
thrustCol= thrustColVer1;
%ScoreVer1
if (ScoreVer1>200)
[thrustRowVer2,thrustColVer2,ScoreVer2]= IndividualSolution(chart,aIndex,bIndex,maxThrottle,0.6,1.3);
[thrustRowVer3,thrustColVer3,ScoreVer3]= IndividualSolution(chart,aIndex,bIndex,maxThrottle,0.8,0);
[thrustRowVer4,thrustColVer4,ScoreVer4]= IndividualSolution(chart,aIndex,bIndex,maxThrottle,0.8,1.3);
Scores= [ScoreVer1,ScoreVer2,ScoreVer3,ScoreVer4];
MinScore= min(Scores);
if (MinScore==ScoreVer1) Best=1;
elseif (MinScore==ScoreVer2) Best=2;
elseif (MinScore==ScoreVer3) Best=3;
else Best=4;
end
if (Best==1)
thrustRow= thrustRowVer1;
thrustCol= thrustColVer1;
elseif (Best==2)
thrustRow= thrustRowVer2;
thrustCol= thrustColVer2;
elseif (Best==3)
thrustRow= thrustRowVer3;
thrustCol= thrustColVer3;
else
thrustRow= thrustRowVer4;
thrustCol= thrustColVer4;
end
end
end
function [thrustRow, thrustCol, IndividualScore] = IndividualSolution(chart,aIndex,bIndex,maxThrottle,StdLimit,VelLimit)
thrustRow= [];
thrustCol= [];
ExtremeLimit= 1.0;
UseExtremeLimit= false;
rowWind= chart(:,:,1);
colWind= chart(:,:,2);
[rN,cN]= size(rowWind);
Size= [rN,cN];
[rA,cA]= ind2sub([rN,cN],aIndex);
[rB,cB]= ind2sub([rN,cN],bIndex);
PosA= [rA,cA];
PosB= [rB,cB];
Pos= PosA;
Vel= [0,0];
Destination= PosB;
Direction= 0;
Continue= 1;
%maxThrottle
Step=0;
while (Continue==true)
Step=Step+1;
if (Step==70) break; end
Limit=StdLimit;
if (UseExtremeLimit==true)
Limit=ExtremeLimit;
UseExtremeLimit= false;
end
rWind= rowWind(Pos(1),Pos(2));
cWind= colWind(Pos(1),Pos(2));
Wind= [rWind,cWind];
Input= [0,0];
BestInput= Input;
[BestScore,BestCost]= ScoreInput(Pos,Vel,Wind,Input,rowWind,colWind,Destination,Size,maxThrottle,Limit,VelLimit);
if (isnan(BestScore)) BestScore= -inf; BestCost= nan; end
for rInp= -maxThrottle:maxThrottle
for cInp= -maxThrottle:maxThrottle
if (abs(rInp)+abs(cInp)<=maxThrottle)
Input= [rInp,cInp];
[Score,Cost]= ScoreInput(Pos,Vel,Wind,Input,rowWind,colWind,Destination,Size,maxThrottle,Limit,VelLimit);
if (Score>BestScore) BestInput= Input; BestScore= Score; BestCost= Cost; end
%
%if (Step==5) Score, end
%Score
%[Score,maxThrottle]
%
end
end
end
SquDist= SquaredDistance(Pos,Destination);
%if (BestScore==-Inf) Check=true; end
%[nan nan nan nan nan nan nan Step]
%[nan nan nan nan BestScore Pos Pos+Vel+Wind Size maxThrottle]
%BestScore==-Inf
%PosOld= Pos
%Vel
%Wind
%BestInput
%[Step BestScore]
%Step
%NextOutside= ~ValidPosition(Pos+Vel+Wind+BestInput,Size);
Near=0;
NextFar=0;
if (Direction==0)
SquDistToPosB= SquaredDistance(Pos,PosB);
NextSquDistToPosB= SquaredDistance(Pos+Vel+Wind+BestInput,PosB);
Near= SquDistToPosB<=10;
NextFar= NextSquDistToPosB>SquDistToPosB;
end
%if (Direction==1)
% [HomeInRange,HomeInput]= DestinationInRange(Pos+Vel+Wind,Destination,maxThrottle);
%end
%[nan Pos,Direction]
%if (Direction==1 && NextOutside && HomeInRange==true)
% Pos= Pos+Vel+Wind+HomeInput;
% Vel= Vel+Wind+BestInput;
% thrustRow= [thrustRow;BestInput(1)];
% thrustCol= [thrustCol;BestInput(2)];
% Continue= 0;
% [Pos, Destination]
if (Near==true && NextFar==true)
Destination= PosA;
Direction= 1;
elseif (BestScore==-Inf && ~ValidPosition(Pos+Vel+Wind+BestInput,Size) && Direction==0)
Destination= PosA;
Direction= 1;
% 'here i am, Dir0',
elseif (BestScore==-Inf && ~ValidPosition(Pos+Vel+Wind+BestInput,Size) && Direction==1)
UseExtremeLimit= true;
% 'here i am, Dir1',
elseif (BestCost>SquDist)
if (Direction==0)
Destination= PosA;
Direction= 1;
else
Continue= 0;
end
else
Pos= Pos+Vel+Wind+BestInput;
Vel= Vel+Wind+BestInput;
thrustRow= [thrustRow;BestInput(1)];
thrustCol= [thrustCol;BestInput(2)];
end
%PosNew= Pos
if (~ValidPosition(Pos,Size))
return;
end
end
%[thrustRow,thrustCol]
ResultRow= thrustRow;
ResultCol= thrustCol;
[ResultRow,ResultCol] = myvalidatesolution(ResultRow,ResultCol,maxThrottle);
[dA,dB] = myrunsolution(ResultRow,ResultCol,chart,aIndex,bIndex);
IndividualScore = myscoresolution(dA,dB,ResultRow,ResultCol);
end
function [Score,Cost]= ScoreInput(Pos,Vel,Wind,Inp,rowWind,colWind,Dest,Size,MaxThrottle,Limit,VelLimit)
FirstPos= Pos+Vel+Wind+Inp;
NewVel= Vel+Wind+Inp;
if (VelLimit>0)
NewVelOK= sum(abs(NewVel))<=VelLimit*MaxThrottle;
else
NewVelOK= true;
end
NoStationarySolution= any(Vel~=0) | any(Pos~=FirstPos) | any(Inp~=-Wind);
if (ValidPosition(FirstPos,Size)==true && NoStationarySolution && NewVelOK)
rWind= rowWind(FirstPos(1),FirstPos(2));
cWind= colWind(FirstPos(1),FirstPos(2));
FirstWind= [rWind,cWind];
SecondPosDrift= FirstPos+Vel+Wind+Inp+FirstWind;
if (DistanceToValidPosition(SecondPosDrift,Size)<round(Limit*MaxThrottle))
CurSquDist= SquaredDistance(Pos,Dest);
FirstSquDist= SquaredDistance(FirstPos,Dest);
SecondSquDist= SquaredDistance(SecondPosDrift,Dest);
NewSquDist= min(FirstSquDist,SecondSquDist);
Improvement= CurSquDist-NewSquDist;
Cost= sum(abs(Inp));
%Penalty= (Cost+1)^2;
Penalty= Cost+1;
if (Improvement<0) Penalty= 1/Penalty; end
Score= Improvement/Penalty;
else
Score= nan;
Cost= nan;
%'inner if...',
end
else
Score= nan;
Cost= nan;
%'outer if...',
end
end
function Valid = ValidPosition(Pos,Size)
if (Pos(1)<1 || Pos(1)>Size(1) || Pos(2)<1 || Pos(2)>Size(2)) Valid= false;
else Valid= true; end
end
function Distance = DistanceToValidPosition(Pos,Size)
Distance= [];
if (Pos(1)<1) Distance= [Distance,1-Pos(1)]; end
if (Pos(1)>Size(1)) Distance= [Distance,Pos(1)-Size(1)]; end
if (Pos(2)<1) Distance= [Distance,1-Pos(2)]; end
if (Pos(2)>Size(2)) Distance= [Distance,Pos(2)-Size(2)]; end
if (numel(Distance)>0) Distance= max(Distance); else Distance= 0; end
end
function SquDist = SquaredDistance(PosA,PosB)
SquDist= (PosA(1)-PosB(1))^2+(PosA(2)-PosB(2))^2;
end
function [thrustRow, thrustCol] = myvalidatesolution(thrustRow, thrustCol, maxThrottle)
% VALIDATESOLUTION Validates the solution vectors given by the solver
% Ensure column vectors and reals
thrustRow = real(thrustRow(:));
thrustCol = real(thrustCol(:));
% Check maximum number of moves
mnmoves = 1000;
if numel(thrustRow) > mnmoves
thrustRow = thrustRow(1:mnmoves);
end
if numel(thrustCol) > mnmoves
thrustCol = thrustCol(1:mnmoves);
end
% Ensure integers
if any(rem(thrustRow,1))
thrustRow = round(thrustRow);
end
if any(rem(thrustCol,1))
thrustCol = round(thrustCol);
end
% Ensure same length
ntR = numel(thrustRow);
ntC = numel(thrustCol);
if ntR~=ntC
n = min(ntR,ntC);
thrustRow = thrustRow(1:n);
thrustCol = thrustCol(1:n);
end
% Check maximum throttle
h = (abs(thrustRow) + abs(thrustCol)) > maxThrottle;
if any(h)
% Motor is blown for every turn the throttle exceeds the motor capacity!
thrustRow(h) = 0;
thrustCol(h) = 0;
end
end
function [dA, dB] = myrunsolution(thrustRow, thrustCol, chart, aIndex, bIndex)
% RUNSOLUTION Simulates the navigation trajectory given the winds and the
% motor thrust.
rowWind = chart(:,:,1);
colWind = chart(:,:,2);
[nR,nC] = size(rowWind);
[AR,AC] = ind2sub([nR,nC],aIndex);
[BR,BC] = ind2sub([nR,nC],bIndex);
% Initialize variables at start point (A)
fR = AR; fC =AC;
fvR = 0; fvC = 0;
dB = (fR-BR)^2 + (fC-BC)^2;
for i = 1:numel(thrustRow)
ivR = fvR + thrustRow(i) + rowWind(fR,fC);
ivC = fvC + thrustCol(i) + colWind(fR,fC);
iR = fR + ivR;
iC = fC + ivC;
if iR>nR || iR<1 || iC>nC || iC<1
break % out of bounds
end
fR = iR;
fC = iC;
fvR = ivR;
fvC = ivC;
% Verify if this is the closest point to B
if ((fR-BR)^2 + (fC-BC)^2) < dB
dB = (fR-BR)^2 + (fC-BC)^2;
end
end
dA = (fR-AR)^2 + (fC-AC)^2; % Final distance to A
end
function score = myscoresolution(dA,dB,thrustRow,thrustCol)
% SCORESOLUTION Calculates the score for the solution.
score = dB + dA + sum(abs(thrustRow)) + sum(abs(thrustCol));
end
|
