public Pos getNearestNode(Pos p) {
int row = getRow(p);
int col = getCol(p);
Pos nearest = createPosFromNode(row, col);
double minDist = p.dist(nearest);
if (nearest.equals(p)) return p;
Pos tmp;
if (nearest.x == p.x) {
tmp = createPosFromNode(row + 1, col);
} else /* if (nearest.y == p.y) */ {
tmp = createPosFromNode(row, col + 1);
}
if (p.dist(tmp) < minDist) nearest = tmp;
return nearest;
}
