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

#include <algorithm>
#include <math.h>
#include <queue>
#include <stdio.h>
#include <utility>
#include <vector>
using namespace std;

struct point {
  int x, y, z;
  point(int x_, int y_, int z_) : x(x_), y(y_), z(z_) {}
  bool operator<(const point &rhs) const { return false; }
};

const int dv = 18;
const int sd = 420;

int b[5][5][5];
int dvs[dv + 1] = {0,   60,  70,  84,  105, 120, 140, 168, 180,
                   210, 240, 252, 280, 300, 315, 336, 350, 360};
int rot[dv * 4 + 1][2];

int xtov[dv * 3 + 1];

vector<point> ed[dv * 3 + 1][dv * 3 + 1][dv * 3 + 1];
priority_queue<pair<double, point>> pq;
bool vis[dv * 3 + 1][dv * 3 + 1][dv * 3 + 1];

double dist(int xa, int ya, int za, int xb, int yb, int zb) {
  int dx = xa - xb, dy = ya - yb, dz = za - zb;
  double d = sqrt(dx * dx + dy * dy + dz * dz);
  return d;
}

int main() {
  int i, j, k;
  for (i = 0; i <= dv * 3 + 1; i++) {
    if (i < dv)
      xtov[i] = dvs[i];
    else
      xtov[i] = xtov[i - dv] + sd;
  }
  for (i = 0; i < dv; i++) {
    rot[i][0] = i;
    rot[i][1] = 0;
    rot[dv + i][0] = dv;
    rot[dv + i][1] = i;
    rot[2 * dv + i][0] = dv - i;
    rot[2 * dv + i][1] = dv;
    rot[3 * dv + i][0] = 0;
    rot[3 * dv + i][1] = dv - i;
  }

  for (;;) {
    int x1, y1, z1;
    int x2, y2, z2;
    scanf("%d%d%d", &x1, &y1, &z1);
    scanf("%d%d%d", &x2, &y2, &z2);
    if (x1 == 0 && y1 == 0 && z1 == 0 && x2 == 0 && y2 == 0 && z2 == 0)
      break;

    for (i = 0; i <= 3 * dv; i++) {
      for (j = 0; j <= 3 * dv; j++) {
        for (k = 0; k <= 3 * dv; k++) {
          ed[i][j][k].clear();
          vis[i][j][k] = false;
        }
      }
    }
    while (!pq.empty())
      pq.pop();

    char tmp[5];
    for (i = 0; i < 3; i++) {
      for (j = 0; j < 3; j++) {
        scanf("%s", tmp);
        for (k = 0; k < 3; k++) {
          b[k][j][i] = (tmp[k] == '#');
        }
      }
    }

    for (i = 0; i <= 3; i++) {
      for (j = 0; j <= 3; j++) {
        for (k = 0; k <= 3; k++) {
          // (i, j, k) (i + 1, j, k) (i, j + 1, k) (i + 1, j + 1, k)
          // square (i, j, k - 1), (i, j, k)
          if (i < 3 && j < 3 &&
              (k == 0 ? 0 : b[i][j][k - 1]) + b[i][j][k] == 1) {
            for (int l0 = 0; l0 < 4 * dv; l0++) {
              for (int l1 = l0 + 1; l1 < 4 * dv; l1++) {
                // (i * dv + rot[l][0], j * dv + rot[l][1], k * dv)
                int nx0 = i * dv + rot[l0][0], ny0 = j * dv + rot[l0][1],
                    nz0 = k * dv;
                int nx1 = i * dv + rot[l1][0], ny1 = j * dv + rot[l1][1],
                    nz1 = k * dv;
                ed[nx0][ny0][nz0].push_back(point(nx1, ny1, nz1));
                ed[nx1][ny1][nz1].push_back(point(nx0, ny0, nz0));
              }
            }
          }
          // (i, j, k) (i + 1, j, k) (i, j, k + 1) (i + 1, j, k + 1)
          // square (i, j - 1, k), (i, j, k)
          if (i < 3 && k < 3 &&
              (j == 0 ? 0 : b[i][j - 1][k]) + b[i][j][k] == 1) {
            for (int l0 = 0; l0 < 4 * dv; l0++) {
              for (int l1 = l0 + 1; l1 < 4 * dv; l1++) {
                // (i * dv + rot[l][0], j * dv, k * dv + rot[l][1])
                int nx0 = i * dv + rot[l0][0], ny0 = j * dv,
                    nz0 = k * dv + rot[l0][1];
                int nx1 = i * dv + rot[l1][0], ny1 = j * dv,
                    nz1 = k * dv + rot[l1][1];
                ed[nx0][ny0][nz0].push_back(point(nx1, ny1, nz1));
                ed[nx1][ny1][nz1].push_back(point(nx0, ny0, nz0));
              }
            }
          }
          // (i, j, k) (i, j + 1, k) (i, j, k + 1) (i, j + 1, k + 1)
          // square (i - 1, j, k), (i, j, k)
          if (j < 3 && k < 3 &&
              (i == 0 ? 0 : b[i - 1][j][k]) + b[i][j][k] == 1) {
            for (int l0 = 0; l0 < 4 * dv; l0++) {
              for (int l1 = l0 + 1; l1 < 4 * dv; l1++) {
                // (i * dv, j * dv + rot[l][0], k * dv + rot[l][1])
                int nx0 = i * dv, ny0 = j * dv + rot[l0][0],
                    nz0 = k * dv + rot[l0][1];
                int nx1 = i * dv, ny1 = j * dv + rot[l1][0],
                    nz1 = k * dv + rot[l1][1];
                ed[nx0][ny0][nz0].push_back(point(nx1, ny1, nz1));
                ed[nx1][ny1][nz1].push_back(point(nx0, ny0, nz0));
              }
            }
          }
        }
      }
    }

    pq.push(make_pair(0.0, point(x1 * dv, y1 * dv, z1 * dv)));
    double ans = 0.0;

    while (!pq.empty()) {
      double nd = pq.top().first;
      int nx = pq.top().second.x, ny = pq.top().second.y,
          nz = pq.top().second.z;
      pq.pop();

      if (vis[nx][ny][nz])
        continue;
      vis[nx][ny][nz] = true;

      if (nx == x2 * dv && ny == y2 * dv && nz == z2 * dv) {
        ans = nd;
        break;
      }

      for (point pp : ed[nx][ny][nz]) {
        if (!vis[pp.x][pp.y][pp.z]) {
          pq.push(make_pair(nd - dist(xtov[nx], xtov[ny], xtov[nz], xtov[pp.x],
                                      xtov[pp.y], xtov[pp.z]),
                            pp));
        }
      }
    }
    printf("%.20lf\\n", -ans / sd);
  }
  return 0;
}

Error: TLE

Fixed submission:

#include <bits/stdc++.h>
#define F first
#define S second
using namespace std;
typedef pair<double, double> XY;
typedef pair<XY, double> XYZ;

set<double> divs;
map<XYZ, int> enc;

int gcd(int a, int b) {
  if (a && b)
    while (a %= b ^= a ^= b ^= a)
      ;
  return a + b;
}

void init() {
  divs.insert(0.0);
  for (int i = 1; i < 12; i++)
    for (int j = 1; i + j <= 12; j++)
      if (gcd(i, i + j) == 1)
        divs.insert((double)i / (i + j));
  divs.insert(1.0);

  set<double> d(divs);
  for (auto it : divs) {
    d.insert(1.0 + it);
    d.insert(2.0 + it);
  }
  for (auto k : d) {
    for (double i = 0; i <= 3.0; i += 1.0)
      for (double j = 0; j <= 3.0; j += 1.0) {
        enc[XYZ(XY(i, j), k)];
        enc[XYZ(XY(i, k), j)];
        enc[XYZ(XY(k, i), j)];
      }
  }
  int sz = 0;
  for (auto &it : enc) {
    it.S = sz++;
  }
}
const int MAXE = 5555555;
int eprev[MAXE], eto[MAXE], elast[7777], ecnt;
double ecost[MAXE];
char br[9][9][9];
int a, b, c, d, e, f;

#define SQ(X) ((X) * (X))
double dist(XYZ a, XYZ b) {
  return pow(SQ(a.F.F - b.F.F) + SQ(a.F.S - b.F.S) + SQ(a.S - b.S), 0.5);
}

inline void addEdge(XYZ a, XYZ b) {
  assert(enc.count(a));
  assert(enc.count(b));
  int u = enc[a], v = enc[b];
  double c = dist(a, b);
  if (u == v)
    return;
  assert(ecnt + 1 <= MAXE);
  eprev[ecnt] = elast[u];
  eto[ecnt] = v;
  ecost[ecnt] = c;
  elast[u] = ecnt++;
  eprev[ecnt] = elast[v];
  eto[ecnt] = u;
  ecost[ecnt] = c;
  elast[v] = ecnt++;
}
char buf[9];

int main() {
  init();
  while (1) {
    scanf("%d%d%d%d%d%d", &a, &b, &c, &d, &e, &f);
    if (a == 0 && b == 0 && c == 0 && d == 0 && e == 0 && f == 0)
      break;
    for (int i = 1; i <= 3; i++) {
      for (int j = 1; j <= 3; j++) {
        scanf("%s", buf + 1);
        for (int k = 1; k <= 3; k++)
          br[k][j][i] = (buf[k] == '#');
      }
    }
    memset(elast, -1, sizeof elast);
    ecnt = 0;
    for (int x = 0; x <= 3; x++) {
      for (int y = 1; y <= 3; y++) {
        for (int z = 1; z <= 3; z++) {
          if (br[x][y][z] ^ br[x + 1][y][z]) { // only one blocks
            // plane with fixed x, [y-1, y], [z-1, z];
            for (auto it : divs)
              for (auto jt : divs) {
                addEdge(XYZ(XY(x, y - 1 + it), z - 1),
                        XYZ(XY(x, y - 1 + jt), z));
                addEdge(XYZ(XY(x, y - 1), z - 1 + it),
                        XYZ(XY(x, y), z - 1 + jt));
              }
          }
        }
      }
    }
    for (int x = 1; x <= 3; x++) {
      for (int y = 0; y <= 3; y++) {
        for (int z = 1; z <= 3; z++) {
          if (br[x][y][z] ^ br[x][y + 1][z]) { // only one blocks
            // plane with fixed y, [x-1, y], [z-1, z];
            for (auto it : divs)
              for (auto jt : divs) {
                addEdge(XYZ(XY(x - 1 + it, y), z - 1),
                        XYZ(XY(x - 1 + jt, y), z));
                addEdge(XYZ(XY(x - 1, y), z - 1 + it),
                        XYZ(XY(x, y), z - 1 + jt));
              }
          }
        }
      }
    }
    for (int x = 1; x <= 3; x++) {
      for (int y = 1; y <= 3; y++) {
        for (int z = 0; z <= 3; z++) {
          if (br[x][y][z] ^ br[x][y][z + 1]) { // only one blocks
            // plane with fixed z, [y-1, y], [z-1, z];
            for (auto it : divs)
              for (auto jt : divs) {
                addEdge(XYZ(XY(x - 1 + it, y - 1), z),
                        XYZ(XY(x - 1 + jt, y), z));
                addEdge(XYZ(XY(x - 1, y - 1 + it), z),
                        XYZ(XY(x, y - 1 + jt), z));
              }
          }
        }
      }
    }

    vector<double> dist(7777, -1.0);
    int bg = enc[XYZ(XY(a, b), c)], en = enc[XYZ(XY(d, e), f)];
    priority_queue<pair<double, int>> pq;
    pq.push({0.0, bg});
    dist[bg] = 0.0;
    while (!pq.empty()) {
      double nd = -pq.top().F;
      int nw = pq.top().S;
      pq.pop();
      if (dist[nw] < nd)
        continue;
      if (nw == en)
        break;
      for (int e = elast[nw]; e != -1; e = eprev[e]) {
        double nc = ecost[e];
        int nxt = eto[e];
        if (dist[nxt] == -1.0 || dist[nxt] > nd + nc) {
          dist[nxt] = nd + nc;
          pq.push({-dist[nxt], nxt});
        }
      }
    }
    printf("%.20f\\n", dist[en]);
  }
  return 0;
}