function [thrustRow, thrustCol] = solver(chart, aIndex, bIndex, maxT)
%{
draw = 0;
if(draw == 1)
rwind = chart(:,:,1);
cwind = chart(:,:,2);
quiver(cwind,rwind); %note that cols correspond to the x direction and rows to the y
axis ij;
axis equal;
end
%}
%Generate all the throttle options
inThrust = maxT;
maxT = min(maxT,7);
thrOptions = [];
for i = 0:maxT
for j=0:maxT-i
if (i == 0)
if (j == 0)
thrOptions = [thrOptions; i j;];
else
thrOptions = [thrOptions; i j; i -j];
end
else
if (j == 0)
thrOptions = [thrOptions; i j; -i j;];
else
thrOptions = [thrOptions; i j; -i j; -i -j; i -j];
end
end
end
end
maxThrOptions = [];
for i = 0:inThrust
for j=0:inThrust-i
if (i == 0)
if (j == 0)
maxThrOptions = [maxThrOptions; i j;];
else
maxThrOptions = [maxThrOptions; i j; i -j];
end
else
if (j == 0)
maxThrOptions = [maxThrOptions; i j; -i j;];
else
maxThrOptions = [maxThrOptions; i j; -i j; -i -j; i -j];
end
end
end
end
%thrOptions
%size(thrOptions)
%size(chart)
%Get indexes
[aRow, aCol] = ind2sub(size(chart(:,:,1)),aIndex);
[bRow, bCol] = ind2sub(size(chart(:,:,2)),bIndex);
%Get the start velocities
srcVRow = chart(aRow,aCol,1);
srcVCol = chart(aRow,aCol,2);
gotClose = 16;
% [routeThrustRow,routeThrustCol,finRow,finCol,finVRow,finVCol] = thereAndBack(aRow,aCol,srcVRow,srcVCol,bRow,bCol,chart,thrOptions,inThrust,1,1);
% [backThrustRow,backThrustCol,finRow2,finCol2,finVRow,finVCol] = thereAndBack(finRow,finCol,finVRow,finVCol,aRow,aCol,chart,thrOptions,inThrust,0,1);
%Find the best node, close to B
[routeThrustRow,routeThrustCol,finRow,finCol,finVRow,finVCol] = thereAndBack(aRow,aCol,srcVRow,srcVCol,bRow,bCol,chart,thrOptions,maxT,1,1);
%length(routeThrustRow)
distFromDest = (bRow-finRow)^2 + (bCol-finCol)^2;
if (distFromDest > gotClose)
%if (length(routeThrustRow) < 2) && (distFromDest > gotClose)
%redoFirstStep = 1
[routeThrustRow,routeThrustCol,finRow,finCol,finVRow,finVCol] = thereAndBack(aRow,aCol,srcVRow,srcVCol,bRow,bCol,chart,maxThrOptions,inThrust,1,1);
distFromDest = (bRow-finRow)^2 + (bCol-finCol)^2;
end;
%Now work back from the last node to find the path back to A
[backThrustRow,backThrustCol,finRow2,finCol2,finVRow2,finVCol2] = thereAndBack(finRow,finCol,finVRow,finVCol,aRow,aCol,chart,thrOptions,maxT,0,1);
%length(backThrustRow)
distFromDest = (finRow2-aRow)^2 + (finCol2-aCol)^2;
%if (length(backThrustRow) < 2) && (distFromDest > gotClose)
if (distFromDest > gotClose)
%redoSecondStep = 1
[backThrustRow,backThrustCol,finRow2,finCol2,finVRow2,finVCol2] = thereAndBack(finRow,finCol,finVRow,finVCol,aRow,aCol,chart,maxThrOptions,inThrust,0,1);
%length(backThrustRow)
distFromDest = (finRow2-aRow)^2 + (finCol2-aCol)^2;
%if (length(routeThrustRow) < 2) && (distFromDest > gotClose)
if (distFromDest > gotClose)
%redoThirdStep = 1
[routeThrustRow,routeThrustCol,finRow,finCol,finVRow,finVCol] = thereAndBack(aRow,aCol,srcVRow,srcVCol,bRow,bCol,chart,maxThrOptions,inThrust,1,1);
[backThrustRow,backThrustCol,finRow2,finCol2,finVRow2,finVCol2] = thereAndBack(finRow,finCol,finVRow,finVCol,aRow,aCol,chart,maxThrOptions,inThrust,0,1);
%{
%If you can't come back its better to stay where you are
if(finRow2 == finRow) && (finCol2 == finCol)
routeThrustRow = [];
backThrustRow = [];
routeThrustCol = [];
backThrustCol = [];
end;
%}
%It's even better to try to come back from the second last position
%instead.
if(finRow2 == finRow) && (finCol2 == finCol)
redoFourthStep = 1
[routeThrustRow,routeThrustCol,finRow,finCol,finVRow,finVCol] = thereAndBack(aRow,aCol,srcVRow,srcVCol,bRow,bCol,chart,maxThrOptions,inThrust,1,2);
[backThrustRow,backThrustCol,finRow2,finCol2,finVRow,finVCol] = thereAndBack(finRow,finCol,finVRow,finVCol,aRow,aCol,chart,maxThrOptions,inThrust,0,1);
end
end;
end;
thrustRow = [routeThrustRow backThrustRow]
thrustCol = [routeThrustCol backThrustCol]
end
function [thrustRow, thrustCol,finRow,finCol,finVRow,finVCol] = thereAndBack(srcRow, srcCol, startVRow, startVCol,destRow, destCol, chart, thrOptions,maxThrust,mustTurn,returnNum)
% Calculate the distance cost between source and destination
cost = (destRow - srcRow)^2 + (destCol - srcCol)^2;
thrustUsedSoFar = 0;
%Row Col rowVel colVel totalCost thurstCostSoFar thurstRow thrustCol fromRow fromCol fromRVel fromCVel
nodes = [srcRow srcCol startVRow startVCol cost thrustUsedSoFar 0 0 -1 -1 -1 -1];
evaluatedNodes = [];
mvCount = 0; % Just for save stop
prevLowestCost = cost;
distToDest = cost;
while (size(nodes,1) > 0) && (distToDest > 0)
tempnodes = []; %Used to store nodes that slightly increases the path cost
size(nodes,1);
currentLowestCost = prevLowestCost;
mvCount = mvCount + 1;
%s_ source node
sRow = nodes(1,1);
sCol = nodes(1,2);
sVRow = nodes(1,3);
sVCol = nodes(1,4);
currentCost = nodes(1,5);
thrustUsedSoFar = nodes(1,6);
sIndex = sub2ind(size(chart(:,:,1)),sRow,sCol);
validFound = 0;
tempprevLowestCost = 0;
%All nodes reachable from A
for i=1:length(thrOptions)
%Total velocity at current node:
tcnVRow = sVRow + thrOptions(i,1);
tcnVCol = sVCol + thrOptions(i,2);
%nn_ new node
nnRow = sRow + tcnVRow;
nnCol = sCol + tcnVCol;
if (nnRow >= 1) && (nnCol >= 1) && (nnRow <= size(chart,1)) && (nnCol <= size(chart,2))
nnVRow = tcnVRow + chart(nnRow,nnCol,1);
nnVCol = tcnVCol + chart(nnRow,nnCol,2);
% Check the cost
distToDest = (destRow - nnRow)^2 + (destCol - nnCol)^2;
newThrustCost = (abs(thrOptions(i,1)) + abs(thrOptions(i,2)));
cost = distToDest + newThrustCost + thrustUsedSoFar;
% Evaluate if the ship will be able to turn around only for
% the first part. Second part we don't care if the ship
% won't be able to turn around.
canTurn = 1;
if (mustTurn == 1)
%Should actually check if the ship will move off the
%board, no matter what. So have to look at the
%velocities AND position.
if (abs(nnVRow) + abs(nnVCol)) > maxThrust
canTurn = 0;
end;
end;
%Do a greedy search
%if (cost < (currentLowestCost+maxThrust)) && (canTurn == 1)
if (cost < (currentLowestCost)) && (canTurn == 1)
%if (cost < currentLowestCost) && (canTurn == 1)
%TODO: Check if the node is already on the list
nodes = [nodes; nnRow nnCol nnVRow nnVCol cost (newThrustCost + thrustUsedSoFar) thrOptions(i,1) thrOptions(i,2) sRow sCol sVRow sVCol];
prevLowestCost = cost;
%if (cost < prevLowestCost) prevLowestCost = cost; end;
%validFound = 1;
end;
%{
if (cost < (currentLowestCost*1.2)) && (canTurn == 1)
tempnodes = [tempnodes; nnRow nnCol nnVRow nnVCol cost (newThrustCost + thrustUsedSoFar) thrOptions(i,1) thrOptions(i,2) sRow sCol sVRow sVCol];
tempprevLowestCost = cost;
end;
%}
end
end
%{
if (validFound == 0) && (length(nodes) > 1)
nodes = [nodes; tempnodes];
prevLowestCost = tempprevLowestCost;
end;
%}
evaluatedNodes = [evaluatedNodes; nodes(1,:)];
nodes = nodes(2:end,:);
nodes = sortrows(nodes,5);
end;
%nodes
%fprintf('R C vR vC cost tCost tRow tCol fR fC fRV FCV');
nodesInList = 1;
if(size(evaluatedNodes) > 0)
evaluatedNodes = sortrows(evaluatedNodes,5);
if(nodesInList == returnNum)
finRow = evaluatedNodes(1,1);
finCol = evaluatedNodes(1,2);
finVRow = evaluatedNodes(1,3);
finVCol = evaluatedNodes(1,4);
end
searchPattern = [evaluatedNodes(1,9) evaluatedNodes(1,10) evaluatedNodes(1,11) evaluatedNodes(1,12)];
%if (searchPattern(1) ~= -1)
routeThrustRow = [evaluatedNodes(1,7)];
routeThrustCol = [evaluatedNodes(1,8)];
%end
start = 1;
while (searchPattern(1) ~= -1)
found = 0;
%Find the search pattern in the evaluatedNodes
for i=start:size(evaluatedNodes,1)
if (evaluatedNodes(i,1) == searchPattern(1)) && (evaluatedNodes(i,2) == searchPattern(2)) && (evaluatedNodes(i,3) == searchPattern(3)) && (evaluatedNodes(i,4) == searchPattern(4))
%start = i;
found = 1;
searchPattern = [evaluatedNodes(i,9) evaluatedNodes(i,10) evaluatedNodes(i,11) evaluatedNodes(i,12)];
if (searchPattern(1) ~= -1)
routeThrustRow = [evaluatedNodes(i,7) routeThrustRow ];
routeThrustCol = [evaluatedNodes(i,8) routeThrustCol ];
nodesInList = nodesInList + 1;
if(nodesInList == returnNum)
finRow = evaluatedNodes(i,1);
finCol = evaluatedNodes(i,2);
finVRow = evaluatedNodes(i,3);
finVCol = evaluatedNodes(i,4);
end
end;
end;
end
if (found == 0)
ERROR = 1;
end;
end
thrustRow = routeThrustRow(1:end-returnNum+1);
thrustCol = routeThrustCol(1:end-returnNum+1);
else
finRow = -1;
finCol = -1;
finVRow = -1;
finVCol = -1;
thrustRow = [];
thrustCol = [];
end
end
|
