in rts/engine/player.cc [183:357]
bool Player::PathPlanning(Tick tick, UnitId id, const PointF &s, const PointF &t, int max_iteration, bool verbose, Coord *first_block, float *dist) const {
const RTSMap &m = *_map;
Coord cs = s.ToCoord();
Coord ct = t.ToCoord();
Loc ls = m.GetLoc(cs);
Loc lt = m.GetLoc(ct);
if (verbose) {
cout << "[PathPlanning] Tick: " << tick << ", id: " << id << " Start: (" << s << ")" << " Target: (" << t << ") " << " ls = " << ls << ", lt = " << lt << endl;
}
first_block->x = first_block->y = -1;
// Initialize to be the maximal distance.
*dist = 1e38;
// Check cache. If the recomputation is fresh, just use it.
auto it_cache = _cache.find(make_pair(ls, lt));
if (it_cache != _cache.end()) {
if (tick - it_cache->second.first < 10) {
Loc loc = it_cache->second.second;
if (verbose) cout << "Cache hit! Tick: " << tick << " cache timestamp: " << it_cache->second.first << " Loc: " << loc << endl;
if (loc != INVALID) {
*first_block = m.GetCoord(loc);
}
return true;
} else {
if (verbose) cout << "Cache out of date! Tick: " << tick << " cache timestamp: " << it_cache->second.first << endl;
_cache.erase(it_cache);
}
}
// Check if the two points are passable by a straight line. (Most common case).
if (line_passable(id, s, t)) {
_cache[make_pair(ls, lt)] = make_pair(tick, INVALID);
return true;
}
// 8 neighbors.
// const int dx[] = { 1, 0, -1, 0, 1, 1, -1, -1 };
// const int dy[] = { 0, 1, 0, -1, 1, -1, 1, -1 };
// const float dists[] = { 1.0, 1.0, 1.0, 1.0, kSqrt2, kSqrt2, kSqrt2, kSqrt2 };
const int dx[] = { 1, 0, -1, 0 };
const int dy[] = { 0, 1, 0, -1 };
const float dists[] = { 1.0, 1.0, 1.0, 1.0 };
// All "from" information.
// Loc -> Loc_from, dist_so_far.
map<Loc, pair<Loc, float> > c_from;
vector<Loc> c_popped;
// If s and t is not passable by a straight line.
priority_queue<Item> q;
float h0 = get_path_dist_heuristic(ls, lt);
q.emplace(Item(0.0, h0, ls, INVALID));
c_from.emplace(make_pair(ls, make_pair(INVALID, 0.0)));
if (verbose) {
cout << "Initial h0 = " << h0 << endl;
}
int iter = 1;
Loc l = INVALID;
bool found = false;
while (!q.empty()) {
Item v = q.top();
// cout << "Poped: " << v.PrintInfo(m) << endl;
// Save the current node to "from" information.
// Only the first occurrence matters (since given the same state, smallest g will come first).
c_popped.push_back(v.loc);
q.pop();
// Find the target, stop.
if (v.loc == lt || iter == max_iteration) {
if (verbose) {
cout << "[PathFinding] Tick: " << tick << " Find the target! cost = " << v.cost << endl;
}
*dist = v.cost;
found = true;
l = v.loc;
break;
}
Coord c_curr = m.GetCoord(v.loc);
// Expand to 8 neighbor.
for (size_t i = 0; i < sizeof(dx) / sizeof(int); ++i) {
Coord next(c_curr.x + dx[i], c_curr.y + dy[i]);
Loc l_next = m.GetLoc(next);
// If we already push that before, skip.
if (c_from.find(l_next) != c_from.end()) continue;
// if we met with impassable location and has not reached the target (lt), skip.
if (l_next != lt) {
if (GetDistanceSquared(s, next) >= 4 && ! m.CanPass(next, id, false)) continue;
if (GetDistanceSquared(s, next) < 4 && ! m.CanPass(next, id)) continue;
}
float h = get_path_dist_heuristic(l_next, lt);
float next_dist = v.g + dists[i];
if (verbose) {
cout << "push: l_next = " << l_next << ", next_dist = " << next_dist << ", h = " << h << ", parent_loc = " << v.loc << endl;
}
q.emplace(Item(next_dist, h, l_next, v.loc));
c_from.emplace(make_pair(l_next, make_pair(v.loc, next_dist)));
}
iter ++;
}
if (verbose) {
cout << "Total iter = " << iter << " optimal dist = " << *dist << endl;
}
if (! found) {
// Not found.
return false;
}
// Then do a backtrace to get the path.
vector<Loc> traj;
// traj[0] is the last part of the trajectory, depending on max_iteration,
// it might end in the target location, or reach some intermediate location, which is the most promising.
// traj[-1] is the starting point.
while (l != INVALID) {
// cout << "Loc: " << m.GetCoord(l) << endl;
auto it = c_from.find(l);
if (it == c_from.end()) {
cout << "Path-finding error!" << endl;
return false;
}
traj.push_back(l);
update_heuristic(l, lt, *dist - it->second.second);
l = it->second.first;
}
// Delete trajectory from the map.
// cout << " Traj: ";
for (const Loc &l : traj) {
// cout << "(" << m.GetCoord(l) << ") ";
c_from.erase(l);
}
// cout << endl;
// For all visited node other than the true solution,
// their heuristic will be set to be opt + eps - g
for (Loc l : c_popped) {
auto it = c_from.find(l);
if (it != c_from.end()) {
update_heuristic(l, lt, (*dist + 1e-5f) - it->second.second);
}
}
// Compute the first waypoint from the starting.
// Starting from the end of path and check.
for (size_t i = 0; i < traj.size(); i++) {
Coord waypoint = m.GetCoord(traj[i]);
if (line_passable(id, s, PointF(waypoint.x, waypoint.y))) {
*first_block = waypoint;
_cache[make_pair(ls, lt)] = make_pair(tick, traj[i]);
return true;
}
}
// cout << "PathPlanning. No valid path, leave to local planning" << endl;
_cache[make_pair(ls, lt)] = make_pair(tick, INVALID);
return false;
}