//---------
void RouteFind::step(const Problem & problem, Route visited) {
if (visited.cost > this->bestSolution.cost)
return;
if (visited.back() == this->end) {
if (visited.cost < this->bestSolution.cost) {
this->bestSolution = visited;
}
return;
}
for (int i=0; i<problem.nodeCount; i++) {
if (!hasVisited(i, visited)) {
Journey j(visited.back(), i);
float cost = problem.getSymmetricCost(j);
if (cost == -1)
continue;
else {
Route visitBranch(visited);
visitBranch.addStep(i, cost);
step(problem, visitBranch);
}
}
}
}
Network::RouteInfo Network::updateRoute(Route& newRoute) throw(NetworkException) {
std::pair<const Node&, const Node&> np(newRoute.front(), newRoute.back());
auto ori = routes.find(np);
assert(ori != routes.end());
Route& oldRoute = ori->second.first;
const double traffic = ori->second.second;
// Remove the traffic from the old route
for (auto i = oldRoute.begin(); i < oldRoute.end() - 1; i++) {
Link l = getLink(make_pair(*i, *(i + 1)));
l.removeTraffic(traffic);
i->removeTraffic(*(i + 1), traffic);
}
// Add it to the new route
double cost = 0.0;
for (auto i = newRoute.begin(); i < newRoute.end() - 1; i++) {
Link l = getLink(make_pair(*i, *(i + 1)));
l.addTraffic(traffic);
i->addTraffic(*(i + 1), traffic);
cost += l.getCurrentCost(traffic);
}
ori->second = RouteInfo(newRoute, traffic);
return RouteInfo(newRoute, cost);
}
void Network::addNewRoute(const Route& r, double traffic) throw (TrancasException) {
const Node& orig = r.front();
const Node& dst = r.back();
pair<string, string> np(orig, dst);
auto ci = routes.find(np);
if (ci != routes.end()) {
throw RouteException("There is already a route from " + orig.getName() + " to " + dst.getName());
}
routes[np] = make_pair(r, traffic);
// Tell nodes to adjust neighbour probabilities
for (Node n : r) {
if (n != r.getDst())
n.cleanRoute(make_pair(getNode(orig), getNode(dst)));
}
}
void plot_shared_points(const Route& r, const char* filename) {
std::ofstream os(filename);
SvgWriter svg(r, os);
if(r.empty())
return;
std::vector<Point> sharedPoints;
find_shared_points(r, sharedPoints);
for(const Path& path : r) {
svg.write(path, "stroke:rgb(0,0,0);stroke-width:10;");
}
for(const Point& p : sharedPoints) {
svg.write(p, "stroke:rgb(255,0,0);stroke-width:10;fill:none");
}
svg.write(r.front().front(), "stroke:rgb(0,255,0);stroke-width:40;");
svg.write(r.back().back(), "stroke:rgb(0,0,255);stroke-width:40;");
}
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;
}
