AOJ1183 鎖中経路

解法

---

2つの円の交点を通る線分に交わっているか確かめながら、Dijkstraした。

ソース

---

#include<complex>
#include<algorithm>
#include<vector>
#include<map>
#include<iomanip>
#include<iostream>
#include<queue>
using namespace std;
#define fr first
#define sc second
 
struct line: public vector< complex<double> >{
  line(){};
  line( const complex<double>& a, const complex<double>& b){
    push_back(a);
    push_back(b);
  }
};
struct circle {
  complex<double> p; double r;
  circle():p(0,0),r(0){};
  circle(const complex<double> &p, double r) : p(p),r(r){}
};
 
typedef complex < double > P;
typedef line               L;
typedef pair < P, P >      Ls;
typedef vector< P >        G;
typedef vector< P >        Ps;
typedef vector< L >        LLL;
typedef circle             C;
const double EPS = 1e-9;
const double INF = 1e8;
 
bool   eq(P,P); //点:点 一緒か
double cross(P,P); //外積
double dot(P,P); //内積
int    ccw(P,P,P); //3点の位置関係
bool   parallel(L,L); // 直線//直線
bool   orthogonal(L,L); //直線⊥直線
bool   intersect(L,L); //線分:線分交差
bool   intersect(L,P); //線分:点交差
bool   intersect(Ls,Ls); //直線:直線交差
bool   intersect(Ls,L); //直線:線分交差
bool   intersect(Ls,P); //直線:点交差
int    intersect(C,L); //円:線分交点数
double distance(L,L); //線分:線分の距離
double distance(L,P); //線分:点の距離
double distance(P,P); //点:点の距離
double distance(Ls,P); //直線:点距離
double distance(Ls,Ls); //直線:直線距離
double distance(Ls,L); //直線:線分距離
P      crosspoint(L,L); //線分:線分交点計算
L      crosspoint(C,Ls); //円:直線交点計算
L      crosspoint(C,L); //円:線分交点計算
Ps     crosspoint(C,C); //円:円交点計算
int    contains(G,P); //GがPが包容か
double area2(G); //Gの面積
bool   isconvex(G); //凸性判定
Ps     convex(G); //凸包
 
 
namespace std {
  bool operator < (const P& a, const P& b) {
    return real(a) != real(b) ? real(a) < real(b) : imag(a) < imag(b);
  }
}
L llcomb(Ls a){
  L line( a.fr, a.sc);
  return line;
}
Ls llrcomb(L a){
  Ls line( a[0], a[1]);
  return line;
}
bool eq( P a, P b){
  return abs( a - b) < EPS;
}
double cross( P a, P b){
  return imag( conj(a) * b);
}
double dot( P a, P b){
  return real( conj(a) * b);
}
P projection( L l, P p) {
  double t = dot( p - l[0], l[0] - l[1]) / norm( l[0] - l[1]);
  return l[0] + t * ( l[0] - l[1]);
}
 
int ccw( P a, P b, P c){
  b -= a, c -= a;
  if(cross(b,c) > EPS)    return 1;  // a → b で 時計方向におれて c
  if(cross(b,c) < -EPS)    return -1; // a → b で 反時計方向におれて c
  if(dot(b,c) < -EPS)      return 2;  // c -- a -- b
  if(norm(b) < norm(c) - EPS) return -2; // a -- b -- c
                        return 0;  // a -- c -- b
}
bool intersect( L a, L b){
  return ccw( a[0], a[1], b[0]) * ccw( a[0], a[1], b[1]) <= 0 &&
    ccw( b[0], b[1], a[0]) * ccw( b[0], b[1], a[1]) <= 0;
}
bool intersect( L a, P p){
   return abs( a[0] - p) + abs( a[1] - p) - abs( a[1] - a[0]) < EPS;
}
bool intersect( Ls l, Ls m) {
  return abs(cross(l.sc-l.fr, m.sc-m.fr)) > EPS ||
         abs(cross(l.sc-l.fr, m.fr-l.fr)) < EPS;
}
bool intersect(Ls l, L s) {
  return cross( l.sc - l.fr, s[0] - l.fr) *      // s[0] is left of l
         cross( l.sc - l.fr, s[1] - l.fr) < EPS; // s[1] is right of l
}
bool intersect(Ls l, P p) {
  return abs( cross( l.sc - p, l.fr - p)) < EPS;
}
int intersect( C c, L l){
  if( norm( projection( l, c.p) - c.p) - c.r * c.r > EPS) return 0;
  const double d1 = abs( c.p - l[0]), d2 = abs( c.p - l[1]);
  if( d1 < c.r + EPS && d2 < c.r + EPS) return 0;
  if( d1 < c.r - EPS && d2 > c.r + EPS
      || d1 > c.r + EPS && d2 < c.r - EPS ) return 1;
  const P h = projection(l, c.p);
  if( dot( l[0] - h, l[1] - h) < 0) return 2;
  return 0;
}
double distance( L s, P p){
  P r = projection(s, p);
 
  if ( intersect( s, r)) return abs( r - p);
  return min( abs( s[0] - p), abs( s[1] - p));
}
double distance( L a, L b){
  if(intersect( a, b)) return 0;
  return min( min( distance( a, b[0]), distance( a, b[1])),
              min( distance( b, a[0]), distance( b, a[1])));
}
double distance( Ls l, P p) {
  return abs(p - projection( llcomb(l), p));
}
double distance( Ls l, Ls m) {
  return intersect(llcomb(l), llcomb(m)) ? 0 : distance(llcomb(l), m.fr);
}
double distance( Ls l, L s) {
  if (intersect(l, s)) return 0;
  return min(distance(l, s[0]), distance(l, s[1]));
}
double distance( P a, P b){
  return abs( a - b);
}
bool parallel( L a, L b){
  return abs( cross( a[1] - a[0], b[1] - b[0])) < EPS;
}
bool orthogonal( L a, L b){
  return dot( a[0] - a[1], b[0] - b[1]) < EPS;
}
#define curr(P,i) P[i]
#define next(P,i) P[(i+1)%P.size()]
#define prev(P, i) P[(i+P.size()-1) % P.size()]
enum { OUT, ON, IN };
int conteins(G Q, P p){
  bool in = false;
  for(int i = 0 ; i < Q.size() ; i++ ){
    P a = curr(Q,i) - p, b = next(Q,i) - p;
    if(imag(a) > imag(b)) swap(a,b);
    if(imag(a) <= 0 && 0 < imag(b) && cross(a,b) < 0) in = !in;
    if(cross(a,b) == 0 && dot(a,b) <= 0) return ON;
  }
  return in ? IN : OUT;
}
double area2(G p){
  double A = 0;
  for (int i = 0; i < p.size(); ++i){
    A += cross(curr(p, i), next(p, i));
  }
  return A;
}
bool isconvex(G p) {
  for (int i = 0; i < p.size(); ++i){
    if (ccw(prev(p, i), curr(p, i), next(p, i)) > 0) return false;
  }
  return true;
}
Ps convex(Ps ps) { //n>=3
  int k = 0;
  sort(ps.begin(), ps.end());
  Ps ch(2 * ps.size());
  for (int i = 0; i < ps.size(); ch[k++] = ps[i++]){
    while (k >= 2 and ccw(ch[k-2], ch[k-1], ps[i]) <= 0) --k;
  }
  for (int i = ps.size()-2, t = k+1; i >= 0; ch[k++] = ps[i--]){
    while (k >= t and ccw(ch[k-2], ch[k-1], ps[i]) <= 0) --k;
  }
  ch.resize(k-1);
  return ch;
}
P crosspoint(L l, L m) {
  double A = cross(l[1] - l[0], m[1] - m[0]);
  double B = cross(l[1] - l[0], l[1] - m[0]);
  if (abs(A) < EPS && abs(B) < EPS) return m[0]; // same line
  return m[0] + B / A * (m[1] - m[0]);
}
L crosspoint( C c, Ls l) {
  const P hp = projection( llcomb(l), c.p), h =  hp - c.p;
  const double d2 = norm(h);
  P v = sqrt( c.r * c.r - d2) * ( l.sc - l.fr) / abs( l.sc - l.fr);
  return L(hp - v, hp + v);
}
L crosspoint( C c, L l) {
  if(intersect(c, l) == 2) return crosspoint(c, llrcomb(l));
  L ret = crosspoint(c, llrcomb(l));
  if(dot(l[0] - ret[0], l[1] - ret[0]) < 0) ret[1] = ret[0];
  else ret[0] = ret[1];
  return ret;
}
Ps crosspoint(C c1, C c2){
  double d = abs(c1.p - c2.p);
  double s = (c1.r + c2.r + d) / 2;
  double S = sqrt( s * ( s - c1.r) * ( s - c2.r) * ( s - d));
  double h = 2 * S / d;
  P v = ( c2.p - c1.p) / ( abs( c2.p - c1.p));
  double m = sqrt( c1.r * c1.r - h * h);
  Ps ret;
  ret.push_back( c1.p + m * v + h * v * P(0,1));
  ret.push_back( c1.p + m * v - h * v * P(0,1));
  return ret;
}
struct dC{
  double cost;
  P pos;
  int nowi,nowj;
  bool operator < (const dC &left) const {
    return cost > left.cost;
  }
};
L make_2_cross(C c1, C c2){
  Ps a(crosspoint( c1, c2));
  return L( a[0], a[1]);
}
 
int main(){
  C prev, now;
  int n;
 
  while(cin >> n , n){
    LLL ls;
 
    cin >> prev.p.real() >> prev.p.imag() >> prev.r;
    ls.push_back( L( prev.p, prev.p));
 
    for(int i = 1 ; i < n ; i++ ){
      cin >> now.p.real() >> now.p.imag() >> now.r;
      ls.push_back( make_2_cross( prev, now));
      prev = now;
    }
    ls.push_back( L( prev.p, prev.p)); // G
 
    priority_queue< dC > que;
    que.push((dC){ 0, ls[0][0], 0, 0});
    bool used[101][2] = {{}};
    double ret = INF;
    while(!que.empty()){
      dC p = que.top();
      que.pop();
      if(p.nowj == n){
        ret = p.cost;
        break;
      }
      if(used[p.nowj][p.nowi]++) continue;
      for(int i = 0 ; i < 2 ; i++ ){
        for(int j = p.nowj + 1 ; j <= n ; j++ ){
          L l = L( p.pos, ls[j][i]);
 
          bool flag = true;
          for(int k = j - 1 ; k > p.nowj ; k-- ){
            if(!intersect( l, ls[k])){
              flag = false;
              break;
            }
          }
          if(flag) que.push( (dC){ abs( l[0] - l[1]) + p.cost, l[1], i, j});
        }
      }
    }
    cout << fixed << setprecision(7) << ret << endl;
  }
}