Problem p02579 (C++) — buggy submission, verdict: Time Limit Exceeded

#include <bits/stdc++.h>
using namespace std;

#define REP(i, n) for (int i = 0, i##_len = (n); i < i##_len; ++i)
#define all(x) (x).begin(), (x).end()
using ll = long long;
using P = pair<ll, ll>;
string char_to_string(char val) { return string(1, val); }
int char_to_int(char val) { return val - '0'; }
template <class T> inline bool chmin(T &a, T b) {
  if (a > b) {
    a = b;
    return true;
  }
  return false;
}
template <class T> inline bool chmax(T &a, T b) {
  if (a < b) {
    a = b;
    return true;
  }
  return false;
}
int vector_finder(std::vector<ll> vec, int number) {
  auto itr = std::find(vec.begin(), vec.end(), number);
  size_t index = std::distance(vec.begin(), itr);
  if (index != vec.size()) { // 発見できたとき
    return 1;
  } else { // 発見できなかったとき
    return 0;
  }
}
struct edge {
  ll to, cost;
};

const int dx[4] = {1, 0, -1, 0};
const int dy[4] = {0, -1, 0, 1};
int main() {
  int H, W;
  cin >> H >> W;
  int ch, cw, dh, dw;
  cin >> ch >> cw >> dh >> dw;
  ch--;
  cw--;
  dh--;
  dw--;
  vector<string> S(H);
  REP(i, H) cin >> S[i];

  vector<vector<ll>> cnt(H, vector<ll>(W, pow(10, 9))); // 魔法を使った回数
  queue<P> q, visited; // これから見るマスと、行ったことのあるマスかどうか
  q.push(P(ch, cw));
  cnt[ch][cw] = 0;

  // だいたい01BFSと同じことをしている
  while (!q.empty() || !visited.empty()) {
    if (!q.empty()) {
      ll X = q.front().first;
      ll Y = q.front().second;
      visited.push(P(X, Y)); // 今見たマスを探索済み探索済みキューに追加する
      q.pop();
      REP(i, 4) {
        ll nextX = X + dx[i];
        ll nextY = Y + dy[i];
        if (0 <= nextX && nextX < H && 0 <= nextY && nextY < W) {
          if (S[nextX][nextY] == '#')
            continue;
          if (cnt[X][Y] < cnt[nextX][nextY]) {
            cnt[nextX][nextY] = cnt[X][Y];
            q.push(P(nextX, nextY));
          }
        }
      }
    } else { // 移動Aで行ける道をすべて探索したら、移動Bで行ける道を探索する
      ll X = visited.front().first;
      ll Y = visited.front().second;
      visited.pop();
      for (int x = -2; x <= 2; ++x) {
        for (int y = -2; y <= 2; ++y) {
          ll nextX = X + x;
          ll nextY = Y + y;
          if (0 <= nextX && nextX < H && 0 <= nextY && nextY < W) {
            if (S[nextX][nextY] == '#')
              continue;
            if (cnt[X][Y] < cnt[nextX][nextY]) {
              cnt[nextX][nextY] = cnt[X][Y] + 1;
              q.push(P(nextX, nextY));
            }
          }
        }
      }
    }
  }
  if (cnt[dh][dw] == pow(10, 9))
    cout << -1 << endl;
  else
    cout << cnt[dh][dw] << endl;
}

Error: TLE

Fixed submission:

#include <bits/stdc++.h>
using namespace std;

#define REP(i, n) for (int i = 0, i##_len = (n); i < i##_len; ++i)
#define all(x) (x).begin(), (x).end()
using ll = long long;
string char_to_string(char val) { return string(1, val); }
int char_to_int(char val) { return val - '0'; }
template <class T> inline bool chmin(T &a, T b) {
  if (a > b) {
    a = b;
    return true;
  }
  return false;
}
template <class T> inline bool chmax(T &a, T b) {
  if (a < b) {
    a = b;
    return true;
  }
  return false;
}
int vector_finder(std::vector<ll> vec, int number) {
  auto itr = std::find(vec.begin(), vec.end(), number);
  size_t index = std::distance(vec.begin(), itr);
  if (index != vec.size()) { // 発見できたとき
    return 1;
  } else { // 発見できなかったとき
    return 0;
  }
}
struct edge {
  ll to, cost;
};

int main() {
  int H, W;
  cin >> H >> W;
  int ch, cw, dh, dw;
  cin >> ch >> cw >> dh >> dw;
  ch--;
  cw--;
  dh--;
  dw--;
  vector<string> S(H);
  REP(i, H) cin >> S[i];
  vector<vector<int>> flag(H); // 行ったことのあるマスかどうか
  REP(i, H) {
    REP(k, W) { flag[i].push_back(-1); }
  }
  const int dx[4] = {1, 0, -1, 0};
  const int dy[4] = {0, -1, 0, 1};
  queue<pair<pair<int, int>, int>> P;
  P.push(make_pair(make_pair(dh, dw), 0));
  queue<pair<int, int>> Q;
  int ans = pow(10, 9) + 7;
  while (!P.empty()) {
    auto p = P.front();
    P.pop();
    Q.push(
        make_pair(p.first.first, p.first.second)); // まずはゴール地点から見る
    while (!Q.empty()) { // ある領域のみsがあるか検査
      auto q = Q.front();
      Q.pop();
      if (q.first == ch && q.second == cw) {
        if (ans > p.second)
          ans = p.second;
        break;
      }
      for (int i = 0; i < 4; ++i) {
        if (q.first + dx[i] < 0 || q.first + dx[i] >= H)
          continue;
        if (q.second + dy[i] < 0 || q.second + dy[i] >= W)
          continue;
        if (S[q.first + dx[i]][q.second + dy[i]] ==
            '#') { // 壁にぶつかったら...
          int a = dx[i] * 2;
          int b = dy[i] * 2;
          if (a == 0) {
            for (int k = -1; k <= 1; ++k) {
              if (q.first + dx[i] + k < 0 || q.first + dx[i] + k >= H)
                continue;
              if (q.second + dy[i] + dy[i] < 0 || q.second + dy[i] + dy[i] >= W)
                continue;
              if (S[q.first + dx[i] + k][q.second + dy[i] + dy[i]] == '#')
                continue;
              if (flag[q.first + dx[i] + k][q.second + dy[i] + dy[i]] == -1) {
                P.push(make_pair(
                    make_pair(q.first + dx[i] + k, q.second + dy[i] + dy[i]),
                    p.second + 1));
              }
            }
          } else if (b == 0) {
            for (int k = -1; k <= 1; ++k) {
              if (q.first + dx[i] + dx[i] < 0 || q.first + dx[i] + dx[i] >= H)
                continue;
              if (q.second + dy[i] + k < 0 || q.second + dy[i] + k >= W)
                continue;
              if (S[q.first + dx[i] + dx[i]][q.second + dy[i] + k] == '#')
                continue;
              if (flag[q.first + dx[i] + dx[i]][q.second + dy[i] + k] == -1) {
                P.push(make_pair(
                    make_pair(q.first + dx[i] + dx[i], q.second + dy[i] + k),
                    p.second + 1));
              }
            }
          }
          continue;
        }
        if (flag[q.first + dx[i]][q.second + dy[i]] != -1)
          continue;
        Q.push(make_pair(q.first + dx[i], q.second + dy[i]));
        flag[q.first + dx[i]][q.second + dy[i]] = flag[q.first][q.second] + 1;
      }
    }
  }
  if (ans == 1000000007)
    cout << -1 << endl;
  else
    cout << ans << endl;
}