AOJ 1352: Cornering at Poles - SSRS の競プロ日記

https://judge.u-aizu.ac.jp/onlinejudge/description.jsp?id=1352

問題概要

座標平面上に半径 $100$ の円形のロボットがあり、その中心を原点からある点に移動させたい。ただし、ロボットと重なってはならない点が $N$ 個指定される。ロボットの最短移動距離を求めよ。

解法

中心を移動させ、半径 $100$ の円形が障害物であると考えると、中心は以下の直線・円弧の組み合わせで表される経路を通るとすることができます。

始点からある円への接線
終点からある円への接線
二円の共通接線
ある円上のこれら三種類の接線の接点である二点の間の弧

よって、接線と接点をすべて列挙したあと、各円について接点を偏角の順に並べ隣接する二点の間の弧が他の円と交差していないか、つまり他の円に含まれる部分の弧と重なっているかを判定すればよいです。

候補となる点・直線・円弧をすべて求めたら、グラフを作りダイクストラ法を行うと最短路の長さが答えになります。

実装

#include <bits/stdc++.h>
using namespace std;
const double EPS = 0.0000001;
const double PI = acos(-1);
struct point{
  double x, y;
  point(){
  }
  point(double x, double y): x(x), y(y){
  }
  point operator +(point P){
    return point(x + P.x, y + P.y);
  }
  point operator -(point P){
    return point(x - P.x, y - P.y);
  }
  point operator *(double k){
    return point(x * k, y * k);
  }
  point operator /(double k){
    return point(x / k, y / k);
  }
};
point rotate90(point P){
  return point(-P.y, P.x);
}
double arg(point P){
  return atan2(P.y, P.x);
}
double abs(point P){
  return sqrt(P.x * P.x + P.y * P.y);
}
double dist(point P, point Q){
  return abs(Q - P);
}
double dot(point P, point Q){
  return P.x * Q.x + P.y * Q.y;
}
double cross(point P, point Q){
  return P.x * Q.y - P.y * Q.x;
}
struct line{
  point A, B;
  line(point A, point B): A(A), B(B){
  }
};
point vec(line L){
  return L.B - L.A;
}
double point_line_distance(point P, line L){
  return abs(cross(vec(L), P - L.A)) / abs(vec(L));
}
double point_segment_distance(point P, line L){
  if (dot(P - L.A, vec(L)) < 0){
    return dist(P, L.A);
  } else if (dot(P - L.B, vec(L)) > 0){
    return dist(P, L.B);
  } else {
    return point_line_distance(P, L);
  }
}
struct circle{
  point C;
  double r;
  circle(point C, double r): C(C), r(r){
  }
};
pair<point, point> circle_intersection(circle C1, circle C2){
  double d = dist(C1.C, C2.C);
  double m = (C1.r * C1.r + d * d - C2.r * C2.r) / (2 * d);
  point H = C1.C + (C2.C - C1.C) / d * m;
  double h = sqrt(C1.r * C1.r - m * m);
  point D = rotate90(C2.C - C1.C) / d * h;
  return make_pair(H - D, H + D);
}
pair<point, point> tangent(point P, point C){
  double d = dist(P, C);
  double d2 = sqrt(d * d - 100 * 100);
  circle C1(P, d2);
  circle C2(C, 100);
  return circle_intersection(C1, C2);
}
vector<line> common_tangent(point A, point B){
  vector<line> ans;
  double d = dist(A, B);
  point D = rotate90(B - A) / d * 100;
  ans.push_back(line(A + D, B + D));
  ans.push_back(line(A - D, B - D));
  if (d > 200){
    double d2 = sqrt(d * d - 200 * 200);
    circle C1(A, 200);
    circle C2(B, d2);
    pair<point, point> P = circle_intersection(C1, C2);
    ans.push_back(line((A + P.first) / 2, B - (P.first - A) / 2));
    ans.push_back(line((A + P.second) / 2, B - (P.second - A) / 2));
  }
  return ans;
}
int main(){
  cout << fixed << setprecision(20);
  int N;
  point G;
  cin >> N >> G.x >> G.y;
  vector<point> C(N);
  for (int i = 0; i < N; i++){
    cin >> C[i].x >> C[i].y;
  }
  point S(0, 0);
  vector<vector<point>> T(N + 2);
  T[N].push_back(S);
  T[N + 1].push_back(G);
  vector<tuple<int, int, int, int>> E;
  E.push_back(make_tuple(N, 0, N + 1, 0));
  for (int i = 0; i < N; i++){
    pair<point, point> T1 = tangent(S, C[i]);
    T[i].push_back(T1.first);
    T[i].push_back(T1.second);
    E.push_back(make_tuple(N, 0, i, 0));
    E.push_back(make_tuple(N, 0, i, 1));
    pair<point, point> T2 = tangent(G, C[i]);
    T[i].push_back(T2.first);
    T[i].push_back(T2.second);
    E.push_back(make_tuple(i, 2, N + 1, 0));
    E.push_back(make_tuple(i, 3, N + 1, 0));
  }
  for (int i = 0; i < N; i++){
    for (int j = i + 1; j < N; j++){
      vector<line> L = common_tangent(C[i], C[j]);
      int cnt = L.size();
      for (int k = 0; k < cnt; k++){
        int id1 = T[i].size();
        int id2 = T[j].size();
        T[i].push_back(L[k].A);
        T[j].push_back(L[k].B);
        E.push_back(make_tuple(i, id1, j, id2));
        E.push_back(make_tuple(j, id2, i, id1));
      }
    }
  }
  vector<vector<int>> id(N + 2);
  int V = 0;
  for (int i = 0; i < N + 2; i++){
    int cnt = T[i].size();
    id[i] = vector<int>(cnt);
    for (int j = 0; j < cnt; j++){
      id[i][j] = V + j;
    }
    V += cnt;
  }
  int M = E.size();
  vector<vector<pair<double, int>>> E2(V);
  for (int i = 0; i < M; i++){
    point P = T[get<0>(E[i])][get<1>(E[i])];
    point Q = T[get<2>(E[i])][get<3>(E[i])];
    bool ok = true;
    for (int j = 0; j < N; j++){
      if (point_segment_distance(C[j], line(P, Q)) < 100 - EPS){
        ok = false;
      }
    }
    if (ok){
      double d = dist(P, Q);
      int u = id[get<0>(E[i])][get<1>(E[i])];
      int v = id[get<2>(E[i])][get<3>(E[i])];
      E2[u].push_back(make_pair(d, v));
    }
  }
  for (int i = 0; i < N; i++){
    int cnt = T[i].size();
    vector<pair<double, int>> P(cnt);
    for (int j = 0; j < cnt; j++){
      P[j] = make_pair(arg(T[i][j] - C[i]), j);
    }
    sort(P.begin(), P.end());
    P.push_back(pair<double, int>());
    P[cnt] = P[0];
    for (int j = 0; j < cnt; j++){
      double L = P[j].first;
      double R = P[j + 1].first;
      if (R < L){
        R += PI * 2;
      }
      bool ok = true;
      for (int k = 0; k < N; k++){
        if (i != k && dist(C[i], C[k]) < 200){
          pair<point, point> Q = circle_intersection(circle(C[i], 100), circle(C[k], 100));
          double l = arg(Q.first - C[i]);
          double r = arg(Q.second - C[i]);
          if (r < l){
            r += PI * 2;
          }
          if (l < R && r > L){
            ok = false;
          }
        }
      }
      if (ok){
        double d = (R - L) * 100;
        int u = id[i][P[j].second];
        int v = id[i][P[j + 1].second];
        E2[u].push_back(make_pair(d, v));
        E2[v].push_back(make_pair(d, u));
      }
    }
  }
  vector<double> d(V, -1);
  priority_queue<pair<double, int>, vector<pair<double, int>>, greater<pair<double, int>>> pq;
  pq.push(make_pair(0, id[N][0]));
  while (!pq.empty()){
    double dd = pq.top().first;
    int v = pq.top().second;
    pq.pop();
    if (d[v] == -1){
      d[v] = dd;
      for (auto P : E2[v]){
        int w = P.second;
        if (d[w] == -1){
          pq.push(make_pair(dd + P.first, w));
        }
      }
    }
  }
  if (d[id[N + 1][0]] == -1){
    cout << 0 << endl;
  } else {
    cout << d[id[N + 1][0]] << endl;
  }
}