private void printRoute(ArrayList<Node> route) {
System.out.printf(
"Start: %s :: End: %s\n", route.get(0).toString(), route.get(route.size() - 1).toString());
for (Node n : route) {
System.out.println(n.toString());
}
System.out.print("\n");
}
@Override
public void plan() {
short[] currentPos = this.getPos();
Node parent = null;
int parent_to_reach = 0;
char parent_heading = 'r';
if (this.route_taken.size() != 0) {
parent = this.route_taken.get(this.route_taken.size() - 1);
parent_to_reach = parent.getCostToReach();
parent_heading = parent.getHeading();
}
ArrayList<Node> successors =
this.getSuccessors(parent, currentPos[0], currentPos[1], currentMem);
for (Node s : successors) {
int to_reach = parent_to_reach + s.turnCost(parent_heading) + 1;
s.setCost(to_reach, this.evaluatePosition(s));
}
;
Node cheapest = this.getCheapestSuccessor(successors);
this.planned_route.add(cheapest);
}
public Node getCheapestSuccessor(ArrayList<Node> succ, Boolean tiebreak) {
int cheapest_cost = 99999;
Node cheapest = null;
for (Node s : succ) {
if (s.getCost() < cheapest_cost) {
cheapest_cost = s.getCost();
cheapest = s;
} else if ((s.getCost() == cheapest_cost) && (tiebreak)) {
// Tie breaker
cheapest = PathPlanner.tiebreaker(s, cheapest);
cheapest_cost = cheapest.getCost();
}
}
return cheapest;
}
public ArrayList<Node> getSuccessors(Node n, Memory mem) {
return this.getSuccessors(n, n.getX(), n.getY(), mem);
}
public void move(PPP ppp) throws InvalidMoveError {
Node move_to = this.planned_route.remove(0);
if ((move_to.getX() == this.getX()) && (move_to.getY() == this.getY())) {
this.totalStationairyMoves++;
}
boolean invalid = false;
if (ppp.isOccupied(move_to.getX(), move_to.getY())) {
// Invalid move made!
invalid = true;
this.invalidMoves++;
throw new InvalidMoveError("Invalid move - " + move_to.toString() + " is Occupied\n");
}
if (invalid) {
System.out.println("invalid move beyond throw");
}
move_to.incVisits();
if (this.route_taken.contains(move_to)) {
this.route_taken.get(this.route_taken.indexOf(move_to)).incVisits();
}
this.route_taken.add(move_to);
short turns =
(short) (this.state.getStateValue().getTurn() + move_to.turnCost(this.getHeading()));
short adv = (short) (this.state.getStateValue().getAdvance() + 1);
short moves = (short) (turns + adv);
this.state.setStateValue(turns, adv, moves);
this.state.setAgentState(move_to.getX(), move_to.getY(), move_to.getHeading());
}
private int evaluatePosition(Node n) {
// Subsidise nodes which keep the wall on the correct side
char wallSide = this.keepWallOnSide(n.getHeading());
// Cost == Cost of about turn - higher encourages cycles
// Too low encourages movement away from the wall if a turn is required
int cost = 10;
switch (wallSide) {
case 'u':
if (this.currentMem.occupied(n.getX(), n.getY() - 1)) {
cost = 0;
}
break;
case 'd':
if (this.currentMem.occupied(n.getX(), n.getY() + 1)) {
cost = 0;
}
break;
case 'l':
if (this.currentMem.occupied(n.getX() - 1, n.getY())) {
cost = 0;
}
break;
case 'r':
if (this.currentMem.occupied(n.getX() + 1, n.getY())) {
cost = 0;
}
break;
default:
break;
}
if (n.isPos(this.getX(), this.getY())) {
// Discourage not moving
cost = 100;
}
if (this.currentMem.isGoal(n.getX(), n.getY())) {
// subsidise the goal position
cost = 0;
}
// Intended fault: This bot will happily be trapped in a cycle.
// if (this.route_taken.contains(n)){
// //Previously been at this position
// Node p = this.route_taken.get(this.route_taken.indexOf(n));
// p.incVisits();
// n.setVisits(p.getVisits());
// }
int c = cost; // +(2*n.getVisits());
return c;
}