/**
* Function to check if a final stage has been reached
*/
bool Board::finalReached() {
if (!isInCheck() && isInCheckmate()) {
Stats *s = s->getInstance();
#pragma omp atomic
s->draw++;
winner = 2;
decision = calculateHeuristic(turn);
bestBoard = nullptr;
return true;
}
whiteKing = nullptr;
whiteKing = getWhiteKing();
if (isInCheckmateWithPieces(whiteKing, blackPieces)) {
winner = BLACK;
decision = calculateHeuristic(BLACK) * 2;
Stats *s = s->getInstance();
#pragma omp atomic
s->blackWins++;
bestBoard = nullptr;
return true;
}
blackKing = nullptr;
blackKing = getBlackKing();
if (isInCheckmateWithPieces(blackKing, whitePieces)) {
winner = WHITE;
decision = calculateHeuristic(WHITE) * 2;
Stats *s = s->getInstance();
#pragma omp atomic
s->whiteWins++;
bestBoard = nullptr;
return true;
}
if (turnsLeft == 0) {
winner = 2;
decision = calculateHeuristic(turn);
Stats *s = s->getInstance();
#pragma omp atomic
s->draw++;
bestBoard = nullptr;
return true;
}
return false;
}
void TRPathFinder::setDValueAll(int nd){
for(int i = 0; i < n; i++){
for(int j = 0; j < m; j++){
d[i][j] = nd;
}
}
calculateHeuristic(true);
}
std::vector<Vertex*>* Graph::AStar(Vertex* start, Vertex* goal) {
std::map<Vertex*, double> openList;
std::vector<Vertex*>* closedList = new std::vector<Vertex*>();
openList[start] = 0.0;
Vertex* current = start;
closedList->push_back(current);
double weightTillNow = 0;
while(current != goal) {
for(Edge* e : *current->getEdges()) {
if (std::find(closedList->begin(), closedList->end(), e->getChild()) == closedList->end()) {
int g = weightTillNow + e->getWeight();
int h = calculateHeuristic(goal, e->getChild());
int f = g + h;
openList[e->getChild()] = f;
}
}
//Shortest
Vertex* shortestNode = nullptr;
for(std::pair<Vertex*, double> mapPair : openList) {
if (shortestNode == nullptr || mapPair.second < openList[shortestNode]) {
shortestNode = mapPair.first;
}
}
for(size_t i = 0; i < current->getEdges()->size(); i++) {
if (current->getEdges()->at(i)->getChild() == shortestNode) {
weightTillNow += current->getEdges()->at(i)->getWeight();
break;
}
}
current = shortestNode;
closedList->push_back(current);
//Clear openList
openList = std::map<Vertex*, double>();
}
std::cout << "cl length: " << closedList->size() << std::endl;
return closedList;
}
bool TRPathFinder::findPath(){
calculateHeuristic();
for (int i = 0; i < n; i++) {
for(int j = 0; j < m; j++){
vis[i][j] = false;
g[i][j] = (int)INF;
}
}
if(!performAStar()){
return false;
}
while(!stk.empty()){
stk.pop();
}
int curX = tarX,curY = tarY;
while(curX != strX || curY != strY){
stk.push(std::make_pair(curX, curY));
int ncx = parX[curY][curX];
curY = parY[curY][curX];
curX = ncx;
}
stk.push(std::make_pair(curX, curY));
return true;
}
std::vector<goap::Action> goap::AStar::plan(const WorldState& start, const WorldState& goal, const std::vector<Action>& actions) {
// Feasible we'd re-use a planner, so clear out the prior results
open_.clear();
closed_.clear();
known_nodes_.clear();
Node starting_node(start, 0, calculateHeuristic(start, goal), 0, nullptr);
// TODO figure out a more memory-friendly way of doing this...
known_nodes_[starting_node.id_] = starting_node;
open_.push_back(std::move(starting_node));
//int iters = 0;
while (open_.size() > 0) {
//++iters;
//std::cout << "\n-----------------------\n";
//std::cout << "Iteration " << iters << std::endl;
// Look for Node with the lowest-F-score on the open list. Switch it to closed,
// and hang onto it -- this is our latest node.
Node& current(popAndClose());
//std::cout << "Open list\n";
//printOpenList();
//std::cout << "Closed list\n";
//printClosedList();
//std::cout << "\nCurrent is " << current << std::endl;
// Is our current state the goal state? If so, we've found a path, yay.
if (current.ws_.meetsGoal(goal)) {
std::vector<Action> the_plan;
do {
the_plan.push_back(*current.action_);
current = known_nodes_.at(current.parent_id_);
} while (current.parent_id_ != 0);
return the_plan;
}
// Check each node REACHABLE from current
for (const auto& action : actions) {
if (action.eligibleFor(current.ws_)) {
WorldState possibility = action.actOn(current.ws_);
//std::cout << "Hmm, " << action.name() << " could work..." << "resulting in " << possibility << std::endl;
// Skip if already closed
if (memberOfClosed(possibility)) {
//std::cout << "...but that one's closed out.\n";
continue;
}
auto needle = memberOfOpen(possibility);
if (needle==end(open_)) { // not a member of open list
// Make a new node, with current as its parent, recording G & H
Node found(possibility, current.g_ + action.cost(), calculateHeuristic(possibility, goal), current.id_, &action);
known_nodes_[found.id_] = found;
// Add it to the open list (maintaining sort-order therein)
addToOpenList(std::move(found));
} else { // already a member of the open list
// check if the current G is better than the recorded G
if ((current.g_ + action.cost()) < needle->g_) {
//std::cout << "My path is better\n";
needle->parent_id_ = current.id_; // make current its parent
needle->g_ = current.g_ + action.cost(); // recalc G & H
needle->h_ = calculateHeuristic(possibility, goal);
std::sort(open_.begin(), open_.end()); // resort open list to account for the new F
}
}
}
}
}
// If there's nothing left to evaluate, then we have no possible path left
throw std::runtime_error("A* planner could not find a path from start to goal");
}
Pathfinding::Route Pathfinding::getPath(Entity* entity, const Vector3& goal) {
stringstream prettyInfo;
Vector3 start = entity->getPosition();
double a = phantom::Util::getTime();
_layer.cleanPathfinding();
Route route;
if(_showDebug) {
cout << endl<< endl<< endl<< endl;
}
if(_visualize) {
getGraphics().clear();
}
// Goal space uses a "strict" heuristic. EG: we don't want to walk into a tree.
Space* goalSpace = _layer.getSpaceAtUsingHeuristic(goal, entity);
// There is no "space" available at the destination. The entity probably wants
// to walk into a tree. Returns an empty route.
if(goalSpace == nullptr) {
if(_showDebug) {
cout << "Goal vector is not a space." << endl;
}
return route;
}
// Start space, first try using a strict heuristic. EG: If we start near a tree
// we don't want to walk into that tree.
Space* startSpace = _layer.getSpaceAtUsingHeuristic(start, entity);
// Ok, did we find a start space with the strict heuristic? If not, it probably
// means that our entity is stuck in a tree. Proceed with a less strict
// heuristic. In most cases this will let the entity "leave" the solid object
// that it's currently stuck on.
if(startSpace == nullptr) {
startSpace = _layer.getSpaceAtUsingHeuristic(start);
}
// Ok, we really can't walk anywhere. This is a rather odd case. Most likely
// the user tried to walk outside of the BSP tree, or you've just found a bug
// in the BSP tree.
if(startSpace == nullptr) {
if(_showDebug) {
cout << "Start vector is not a space." << endl;
}
route.push_back(Vector3(goal));
return route;
}
if(_showDebug) {
cout << "Starting at: " << startSpace->getArea().toString();
cout << "Ending at : " << goalSpace->getArea().toString();
}
if(_visualize) {
Box3& m = startSpace->getArea();
getGraphics().beginPath().setFillStyle(Color(0, 0, 127, 20))
.rect(m.origin.x+4, m.origin.y+4, m.size.x-8, m.size.y-8)
.fill();
Box3& n = goalSpace->getArea();
getGraphics().beginPath().setFillStyle(Color(0, 0, 127, 20))
.rect(n.origin.x+4, n.origin.y+4, n.size.x-8, n.size.y-8)
.fill();
}
priority_queue<Space*, vector<Space*>, CompareShapesAstar> open;
startSpace->isInOpenList = true;
open.push(startSpace);
if(_showBasicDebug) {
prettyInfo << "Pathfinding overhead: " << std::fixed << (phantom::Util::getTime() - a) << " seconds. ";
}
int spacesScanned = 0;
const double startTime = phantom::Util::getTime();
int timeout = 0;
while(true) {
if(open.empty()) {
if(_showBasicDebug || _showDebug) {
cout << " Open list empty." << endl;
double now = phantom::Util::getTime();
if(_showBasicDebug) {
prettyInfo << "No route found, scanned "<< spacesScanned << " Tile(s) in " << std::fixed << (now - startTime) << " seconds.";
}
}
break;
}
if(timeout++ > 10000) {
cout << " I give up after " << timeout << " tries. " << endl;
double now = phantom::Util::getTime();
cout << "A* scanned " << spacesScanned << " Tile(s) in " << std::fixed << (now - startTime) << " seconds. " << endl;
break;
}
Space* current = open.top();
open.pop();
if(_visualize) {
//cout << " - Testing: " << current->getArea().toString();
drawRect(current, Color(0, 127, 127, 10));
}
if(current == goalSpace) {
if(_showDebug) {
cout << " **** found! This is a good sign. " << endl;
}
unfoldRoute(route, current, startSpace, entity);
if(!route.empty()) {
route.pop_front();
Vector3 lastpos = goal - entity->getBoundingBox().size * 0.5;
// Replace the last way-point with our mouse click coordinates:
if(route.empty()) {
route.push_back(lastpos);
} else {
route.back() = lastpos;
}
}
double now = phantom::Util::getTime();
if(_showBasicDebug) {
prettyInfo << "Found route, A* scanned " << spacesScanned << " Tile(s) in " << std::fixed << (now - startTime) << " seconds. Waypoint(s): " << route.size() << ".";
}
break;
}
vector<Space*>& neighbours = _layer.getNeighbours(current, entity);
if(_showDebug && neighbours.empty()) {
cout << " No neighbours found." << endl;
}
for(size_t i = 0; i < neighbours.size(); ++i) {
Space* testing = neighbours[i];
if(!testing->isInOpenList) {
spacesScanned++;
testing->astarParent = current;
testing->isInOpenList = true;
testing->g = current->g + Services::settings()->pathfinding_g_cost;
testing->h = calculateHeuristic(goalSpace, testing);
testing->h = testing->h * testing->h;
open.push(testing);
}
}
}
if(_showBasicDebug) {
//cout << prettyInfo.str() << endl;
Console::log(prettyInfo.str());
}
return route;
}
