function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxThrottle)
% FailBoat 1 solver, does something
% Copyright 2010 Jonas Wallentin
%maxThrottle = floor(maxThrottle/2);
mRow = [];
mCol = [];
FuelCost = 1.5;
SpeedCost = 0;
TargetReached = 9;
TargetReachedHome = 8;
MaxIter = 200;
CloseEdgeCost = 50;
FarEdgeCost = 10;
VeryFarEdgeCost = 5;
chartrow=chart(:,:,1);
chartcol=chart(:,:,2);
%Hopefully this is right
rowsize = size(chartrow,1);
colsize = size(chartrow,2);
[aRow aCol]=ind2sub(size(chart(:,:,1)),aIndex);
[bRow bCol]=ind2sub(size(chart(:,:,1)),bIndex);
%Regular matrixes in the column direction
EdgeColUp = repmat(1:colsize,rowsize,1)-1;
EdgeColDown =colsize-EdgeColUp-1;
%Regular matrixes in the row direction
EdgeRowUp = repmat((1:rowsize)',1,colsize)-1;
EdgeRowDown =rowsize-EdgeRowUp-1;
%Hard speed limits to avoid leaving the map
SpeedLimColMax = (EdgeColDown-chartcol);
SpeedLimColMin= (-EdgeColUp-chartcol);
%Hard speed limits to avoid leaving the map
SpeedLimRowMax = (EdgeRowDown-chartrow);
SpeedLimRowMin= (-EdgeRowUp-chartrow);
%Initialise target
tRow = bRow;
tCol = bCol;
%Initialise current spot
cRow = aRow;
cCol = aCol;
%Initialise speed
cRowSpd = chartrow(aRow,aCol);
cColSpd = chartcol(aRow,aCol);
%Cost from points relative to current target
colcost=-(chartcol-(tCol-EdgeColUp));
rowcost=-(chartrow-(tRow-EdgeRowUp));
%%%%%%%%%%%%
%%%%%%%%%%%%
%Loop through steps
i = 0;
while 1
i=i+1;
%Calculate search matrix for next step from the current step.
%(Should be based on current position, speed and max thrust)
SearchRow = max(1,cRow-maxThrottle+cRowSpd):min(rowsize,cRow+maxThrottle+cRowSpd);
SearchCol = max(1,cCol-maxThrottle+cColSpd):min(colsize,cCol+maxThrottle+cColSpd);
if isempty(SearchCol) || isempty(SearchRow)
break
end
%Abort and go into next phase if close enough.
if (abs(tRow-cRow)<TargetReached && abs(tCol-cCol)<TargetReached) || cRow < 1 || cCol < 1 || cRow > rowsize || cCol > colsize || i>MaxIter
break
end
%Only consider target area for cost
trowcost=rowcost(SearchRow,SearchCol);
%Target speed after current step
%Motor input + Speed of current step.
MotorRowInput=EdgeRowUp(SearchRow,SearchCol)-cRow+1-cRowSpd;
tRowSpd=MotorRowInput+cRowSpd;
%Introduce cost for points where we would leave the map
trowcost(tRowSpd>SpeedLimRowMax(SearchRow,SearchCol))=1000;
trowcost(tRowSpd<SpeedLimRowMin(SearchRow,SearchCol))=1000;
% %Introduce cost for points where we would most likely leave the map
idx=(tRowSpd*2-maxThrottle)>SpeedLimRowMax(SearchRow,SearchCol);
trowcost(idx)=trowcost(idx)+tRowSpd(idx)*CloseEdgeCost;
idx=(tRowSpd*2+maxThrottle)<SpeedLimRowMin(SearchRow,SearchCol);
trowcost(idx)=trowcost(idx)-tRowSpd(idx)*CloseEdgeCost;
%Introduce cost for points where we would maybe leave the map
idx=(tRowSpd*3-maxThrottle*2)>SpeedLimRowMax(SearchRow,SearchCol);
trowcost(idx)= trowcost(idx)+tRowSpd(idx)*FarEdgeCost;
idx=(tRowSpd*3+maxThrottle*2)<SpeedLimRowMin(SearchRow,SearchCol);
trowcost(idx)= trowcost(idx)-tRowSpd(idx)*FarEdgeCost;
%Introduce cost for points where we would maybe leave the map
idx=(tRowSpd*4-maxThrottle*3)>SpeedLimRowMax(SearchRow,SearchCol);
trowcost(idx)= trowcost(idx)+tRowSpd(idx)*VeryFarEdgeCost;
idx=(tRowSpd*4+maxThrottle*3)<SpeedLimRowMin(SearchRow,SearchCol);
trowcost(idx)= trowcost(idx)-tRowSpd(idx)*VeryFarEdgeCost;
%Only consider target area for cost
tcolcost=colcost(SearchRow,SearchCol);
%Target speed after current step
%Motor input + Speed of current step.
MotorColInput = EdgeColUp(SearchRow,SearchCol)-cCol+1-cColSpd;
tColSpd=MotorColInput+cColSpd;
%Introduce cost for points where we would leave the map
tcolcost(tColSpd>SpeedLimColMax(SearchRow,SearchCol))=1000;
tcolcost(tColSpd<SpeedLimColMin(SearchRow,SearchCol))=1000;
%Introduce cost for points where we would most likely leave the map
idx=(tColSpd*2-maxThrottle)>SpeedLimColMax(SearchRow,SearchCol);
tcolcost(idx)= tcolcost(idx)+tColSpd(idx)*CloseEdgeCost;
idx=(tColSpd*2+maxThrottle)<SpeedLimColMin(SearchRow,SearchCol);
tcolcost(idx)= tcolcost(idx)-tColSpd(idx)*CloseEdgeCost;
%Introduce cost for points where we would maybe leave the map
idx=(tColSpd*3-maxThrottle*2)>SpeedLimColMax(SearchRow,SearchCol);
tcolcost(idx)= tcolcost(idx)+tColSpd(idx)*FarEdgeCost;
idx=(tColSpd*3+maxThrottle*2)<SpeedLimColMin(SearchRow,SearchCol);
tcolcost(idx)= tcolcost(idx)-tColSpd(idx)*FarEdgeCost;
%Introduce cost for points where we would maybe leave the map
idx=(tColSpd*4-maxThrottle*3)>SpeedLimColMax(SearchRow,SearchCol);
tcolcost(idx)= tcolcost(idx)+tColSpd(idx)*VeryFarEdgeCost;
idx=(tColSpd*4+maxThrottle*3)<SpeedLimColMin(SearchRow,SearchCol);
tcolcost(idx)= tcolcost(idx)-tColSpd(idx)*VeryFarEdgeCost;
%Cost formula
%Cost to reach target + Fuel cost for reaching the target with weight
%factor to force low fuel useage
MotorInput=abs(MotorColInput)+abs(MotorRowInput);
MotorInput(MotorInput>maxThrottle)=1000;
totcost = abs(trowcost)+abs(tcolcost)+FuelCost*MotorInput+SpeedCost;
[targetrow,targetcol] = find(totcost==min(totcost(:)),1);
%Return new coordinates, new speeds, and requested motor input.
%Needs to be tied in with the selection of the target area
mCol(i)=MotorColInput(1,targetcol);
cColSpd=tColSpd(targetrow,targetcol); %Only if not including the speed of next step earlier.
cCol = cCol+cColSpd;
mRow(i)=MotorRowInput(targetrow,1);
cRowSpd=tRowSpd(targetrow,targetcol); %Only if not including the speed of next step earlier.
cRow = cRow+cRowSpd;
if (cColSpd ==0 && cRowSpd ==0)
break
end
cColSpd=cColSpd+chartcol(cRow,cCol);
cRowSpd=cRowSpd+chartrow(cRow,cCol);
% rowvec(i)=cRow;
% rowspdvec(i)=cRowSpd;
% colspdvec(i)=cColSpd;
% colvec(i)=cCol;
% %%%%%%
end
i=i-1;
%%%%%%%%%%%%%%%%%
%Returning home
tRow = aRow;
tCol = aCol;
%Cost from points relative to current target
colcost=-(chartcol-(tCol-EdgeColUp));
rowcost=-(chartrow-(tRow-EdgeRowUp));
while 1
i=i+1;
%Calculate search matrix for next step from the current step.
%(Should be based on current position, speed and max thrust)
SearchRow = max(1,cRow-maxThrottle+cRowSpd):min(rowsize,cRow+maxThrottle+cRowSpd);
SearchCol = max(1,cCol-maxThrottle+cColSpd):min(colsize,cCol+maxThrottle+cColSpd);
if isempty(SearchCol) || isempty(SearchRow)
break
end
%Only consider target area for cost
trowcost=rowcost(SearchRow,SearchCol);
%Target speed after current step
%Motor input + Speed of current step.
MotorRowInput=EdgeRowUp(SearchRow,SearchCol)-cRow+1-cRowSpd;
tRowSpd=MotorRowInput+cRowSpd;
%Introduce cost for points where we would leave the map
trowcost(tRowSpd>SpeedLimRowMax(SearchRow,SearchCol))=1000;
trowcost(tRowSpd<SpeedLimRowMin(SearchRow,SearchCol))=1000;
% %Introduce cost for points where we would most likely leave the map
idx=(tRowSpd*2-maxThrottle)>SpeedLimRowMax(SearchRow,SearchCol);
trowcost(idx)=trowcost(idx)+tRowSpd(idx)*CloseEdgeCost;
idx=(tRowSpd*2+maxThrottle)<SpeedLimRowMin(SearchRow,SearchCol);
trowcost(idx)=trowcost(idx)-tRowSpd(idx)*CloseEdgeCost;
%Introduce cost for points where we would maybe leave the map
idx=(tRowSpd*3-maxThrottle*2)>SpeedLimRowMax(SearchRow,SearchCol);
trowcost(idx)= trowcost(idx)+tRowSpd(idx)*FarEdgeCost;
idx=(tRowSpd*3+maxThrottle*2)<SpeedLimRowMin(SearchRow,SearchCol);
trowcost(idx)= trowcost(idx)-tRowSpd(idx)*FarEdgeCost;
%Only consider target area for cost
tcolcost=colcost(SearchRow,SearchCol);
%Target speed after current step
%Motor input + Speed of current step.
MotorColInput = EdgeColUp(SearchRow,SearchCol)-cCol+1-cColSpd;
tColSpd=MotorColInput+cColSpd;
%Introduce cost for points where we would leave the map
tcolcost(tColSpd>SpeedLimColMax(SearchRow,SearchCol))=1000;
tcolcost(tColSpd<SpeedLimColMin(SearchRow,SearchCol))=1000;
%Introduce cost for points where we would most likely leave the map
idx=(tColSpd*2-maxThrottle)>SpeedLimColMax(SearchRow,SearchCol);
tcolcost(idx)= tcolcost(idx)+tColSpd(idx)*CloseEdgeCost;
idx=(tColSpd*2+maxThrottle)<SpeedLimColMin(SearchRow,SearchCol);
tcolcost(idx)= tcolcost(idx)-tColSpd(idx)*CloseEdgeCost;
%Introduce cost for points where we would maybe leave the map
idx=(tColSpd*3-maxThrottle*2)>SpeedLimColMax(SearchRow,SearchCol);
tcolcost(idx)= tcolcost(idx)+tColSpd(idx)*FarEdgeCost;
idx=(tColSpd*3+maxThrottle*2)<SpeedLimColMin(SearchRow,SearchCol);
tcolcost(idx)= tcolcost(idx)-tColSpd(idx)*FarEdgeCost;
%Cost formula
%Cost to reach target + Fuel cost for reaching the target with weight
%factor to force low fuel useage (No speed cost here)
MotorInput=abs(MotorColInput)+abs(MotorRowInput);
MotorInput(MotorInput>maxThrottle)=1000;
totcost = abs(trowcost)+abs(tcolcost)+FuelCost*MotorInput;
[targetrow,targetcol] = find(totcost==min(totcost(:)),1);
%Return new coordinates, new speeds, and requested motor input.
%Needs to be tied in with the selection of the target area
mCol(i)=MotorColInput(1,targetcol);
cColSpd=tColSpd(targetrow,targetcol); %Only if not including the speed of next step earlier.
cCol = cCol+cColSpd;
mRow(i)=MotorRowInput(targetrow,1);
cRowSpd=tRowSpd(targetrow,targetcol); %Only if not including the speed of next step earlier.
cRow = cRow+cRowSpd;
if (cColSpd ==0 && cRowSpd ==0)
break
end
cColSpd=cColSpd+chartcol(cRow,cCol);
cRowSpd=cRowSpd+chartrow(cRow,cCol);
% rowvec(i)=cRow;
% rowspdvec(i)=cRowSpd;
% colspdvec(i)=cColSpd;
% colvec(i)=cCol;
%%%%%%
%We are home!
if (abs(tRow-cRow)<TargetReachedHome && abs(tCol-cCol)<TargetReachedHome) || cRow < 1 || cCol < 1 || cRow > rowsize || cCol > colsize || i>MaxIter
break
end
end
%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%
thrustRow = mRow;
thrustCol = mCol;
end
|
