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

#include <bits/stdc++.h>
#define ALL(A) (A).begin(), (A).end()
#define ll long long
#define rep(i, n) for (int i = 0; i < (n); i++)

using namespace std;
template <class T> inline bool chmax(T &a, T b) {
  if (a < b) {
    a = b;
    return 1;
  }
  return 0;
}
template <class T> inline bool chmin(T &a, T b) {
  if (a > b) {
    a = b;
    return 1;
  }
  return 0;
}

int dx[] = {0, 1, -1, 0, 1, -1, 1, -1}; // i<4:4way i<8:8way
int dy[] = {1, 0, 0, -1, 1, -1, -1, 1};

const ll mod = 1e9 + 7;
const ll INF = -1 * ((1LL << 63) + 1);
const ll inf = 1e18 + 7;

string s[1005];
ll dis[1005][1005];

int main(void) {
  cin.tie(0);
  ios::sync_with_stdio(false);
  cout << fixed << setprecision(20);
  int h, w;
  int sx, sy, gx, gy;

  cin >> h >> w;
  cin >> sx >> sy;
  cin >> gx >> gy;
  rep(i, h) cin >> s[i];

  sx--;
  sy--;
  gx--;
  gy--;

  rep(i, 1005) rep(j, 1005) dis[i][j] = inf;

  dis[sx][sy] = 0;

  queue<pair<int, int>> q;
  queue<pair<int, int>> q2;
  q.push({sx, sy});
  while (!q.empty()) {
    while (!q.empty()) {
      auto now = q.front();
      q2.push(now);
      q.pop();
      // まずは現在地から歩いていける点を全て埋める
      for (int i = 0; i < 4; i++) {
        int nx = dx[i] + now.first;
        int ny = dy[i] + now.second;
        if (nx < 0 || ny < 0 || nx >= h || ny >= w)
          continue;
        if (s[nx][ny] == '#')
          continue;
        if (dis[nx][ny] <= dis[now.first][now.second])
          continue;
        dis[nx][ny] = dis[now.first][now.second];
        q.push({nx, ny});
        q2.push({nx, ny});
      }
    }
    // 歩いていった点からテレポートを使う
    while (!q2.empty()) {
      auto now = q2.front();
      q2.pop();
      for (int i = 0; i < 5; i++) {
        for (int j = 0; j < 5; j++) {
          int nx = now.first - 2 + i;
          int ny = now.second - 2 + j;
          if (nx < 0 || ny < 0 || nx >= h || ny >= w)
            continue;
          if (s[nx][ny] == '#')
            continue;
          if (dis[nx][ny] <= dis[now.first][now.second] + 1)
            continue;
          dis[nx][ny] = dis[now.first][now.second] + 1;
          q.push({nx, ny});
        }
      }
    }
  }
  cout << (dis[gx][gy] == inf ? -1 : dis[gx][gy]) << endl;
}

Error: TLE

Fixed submission:

#include <bits/stdc++.h>
#define ALL(A) (A).begin(), (A).end()
#define ll long long
#define rep(i, n) for (int i = 0; i < (n); i++)

using namespace std;
template <class T> inline bool chmax(T &a, T b) {
  if (a < b) {
    a = b;
    return 1;
  }
  return 0;
}
template <class T> inline bool chmin(T &a, T b) {
  if (a > b) {
    a = b;
    return 1;
  }
  return 0;
}

int dx[] = {0, 1, -1, 0, 1, -1, 1, -1}; // i<4:4way i<8:8way
int dy[] = {1, 0, 0, -1, 1, -1, -1, 1};

const ll mod = 1e9 + 7;
const ll INF = -1 * ((1LL << 63) + 1);
const int inf = 1e9 + 7;

char s[1005][1005];
int dis[1005][1005];

int main(void) {
  cin.tie(0);
  ios::sync_with_stdio(false);
  cout << fixed << setprecision(20);
  int h, w;
  int sx, sy, gx, gy;

  cin >> h >> w;
  cin >> sx >> sy;
  cin >> gx >> gy;

  rep(i, h) cin >> s[i];
  sx--;
  sy--;
  gx--;
  gy--;
  rep(i, 1005) rep(j, 1005) dis[i][j] = 1e9 + 7;
  dis[sx][sy] = 0;
  queue<pair<int, int>> q;
  q.push({sx, sy});
  while (!q.empty()) {
    auto now = q.front();
    q.pop();
    // まずは現在地から上下左右の距離を見る
    for (int i = 0; i < 4; i++) {
      int nx = now.first + dx[i];
      int ny = now.second + dy[i];
      if (nx < 0 || ny < 0 || nx >= h || ny >= w)
        continue;
      if (s[nx][ny] == '#')
        continue;
      if (dis[nx][ny] <= dis[now.first][now.second])
        continue;
      else {
        dis[nx][ny] = dis[now.first][now.second];
        q.push({nx, ny});
      }
    }
    bool ok = false;
    for (int i = 0; i < 4; i++) {
      int nx = now.first + dx[i];
      int ny = now.second + dy[i];
      if (nx < 0 || ny < 0 || nx >= h || ny >= w)
        continue;
      if (s[nx][ny] == '#')
        ok = true;
    }
    if (!ok)
      continue;
    // 次に現在地から5*5の範囲内にワープできたらする
    for (int i = 0; i < 5; i++) {
      for (int j = 0; j < 5; j++) {
        int nx = now.first - 2 + i;
        int ny = now.second - 2 + j;
        if (nx < 0 || ny < 0 || nx >= h || ny >= w)
          continue;
        if (s[nx][ny] == '#')
          continue;
        if (dis[nx][ny] <= dis[now.first][now.second] + 1)
          continue;
        else {
          dis[nx][ny] = dis[now.first][now.second] + 1;
          q.push({nx, ny});
        }
      }
    }
  }
  cout << (dis[gx][gy] == inf ? -1 : dis[gx][gy]) << endl;
}