Problem:
Problem L: RedBlue

Story

天空都市AIZUのUZIA高校では、競技プログラミングの部活動がとても盛んである。
この部活には、n人のRed Coderとn人のBlue Coderが所属している。

ある日の部活の時間に、Red CoderとBlue Coderでペアを組み、この部活からn組、KCPというコンテストに参加することとなった。この高校では、ペアを組む学生同士は握手するという習わしがあるので、部員たちは今すぐ自分のパートナーを見つけることにした。

部員たちは全速力で走るため、まっすぐにしか進めない。また、部員たちは、それぞれの部員が移動する距離の総和をできるだけ小さくしたいと思っている。

なお、部室には2つの円形のテーブルが置いてある。

Problem

二次元平面上に2つの円、n個の赤色の点、n個の青色の点がある。
2つの円の中心座標はそれぞれ(x1, y1), (x2, y2)であり、半径はそれぞれr1, r2である。
赤い点iは、座標(rxi, ryi)にあり、青い点jは、座標(bxj, byj)にある。

あなたは、以下の操作をn回繰り返す必要がある。

まだ選ばれていない点の中から、赤色の点と青色の点を１つずつ選んで、2点の共通の目的地を設定し、その目的地に向かって2点をそれぞれまっすぐ移動させる。
目的地は二次元平面上であれば、どこに設定しても構わない。ただし、選んだ2点は移動の際に円の内部を通過することはできないので、そのような移動が発生する目的地を設定することはできない。

n回の操作をしたときの移動距離の総和を最小化せよ。
n回の操作を行えない場合は、代わりに"Impossible" (""は除く) と出力せよ。

Input

入力は以下の形式で与えられる。

n
x1 y1 r1
x2 y2 r2
rx1 ry1
rx2 ry2
...
rxn ryn
bx1 by1
bx2 by2
...
bxn byn

入力は全て整数で与えられる。

1行目にnが与えられる。

2行目にx1, y1, r1が空白区切りで与えられる。

3行目にx2, y2, r2が空白区切りで与えられる。

4から3+n行に赤い点の座標(rxi, ryi)が空白区切りで与えられる。

4+nから3+2×n行に青い点の座標(bxj, byj)が空白区切りで与えられる。

Constraints

入力は以下の条件を満たす。

1 ≤ n ≤ 100

-1000 ≤ xi, yi ≤ 1000

1 ≤ ri ≤ 50

-1000 ≤ rxi, ryi, bxi, byi ≤ 1000

同じ座標に複数の点が存在していることはない

任意の円の半径を絶対値10-9以内で変化させても、高々絶対値10-3しか変化しない

任意の円の半径を絶対値10-9以内で変化させても、"Impossible"なケースは"Impossible"のままである

解は10000を超えない

どの点も、円から10-3以上離れていて、点が円周上に存在したり、点が円に内包されることはない

2つの円は共通面積を持たず、10-3以上離れていることが保証される

Output

n回の操作をしたときの移動距離の総和の最小値を1行に出力せよ。なお、出力はジャッジ解の出力との絶対誤差が10-2以内であれば許容される。

n回の操作を行えない場合は、代わりに"Impossible" (""は除く) と出力せよ。

Sample Input 1

2
3 3 2
8 3 2
0 3
3 7
8 0
8 7

Sample Output 1

13.8190642862

Sample Input 2

2
3 3 2
8 3 2
3 0
3 7
8 0
8 7

Sample Output 2

10.0000000000

Sample Input 3

2
3 3 2
8 3 2
0 0
0 5
11 0
11 5

Sample Output 3

22.0000000000

Sample Input 4

1
10 10 10
31 10 10
15 19
26 1

Sample Output 4

Impossible

点同士をつなぐことはできません。

Solution in Scala (judge verdict: Wrong Answer)


object Main extends App {

  import scala.math.{sqrt, abs}
  import scala.io.StdIn.readLine

  val n = readLine().toInt
  val Array(ca, cb) = Array.tabulate(2){_ ⇒
    val Array(x, y, r) = readLine().split(' ').map(_.toDouble)
    Circle(Point(x, y), r)
  }
  val red = Array.tabulate(n){_ ⇒
    val Array(x, y) = readLine().split(' ').map(_.toDouble)
    Point(x, y)
  }
  val blue = Array.tabulate(n){_ ⇒
    val Array(x, y) = readLine().split(' ').map(_.toDouble)
    Point(x, y)
  }
  val rTuple = red.map{p ⇒ (p, tangentialLine(p, ca), tangentialLine(p, cb))}
  val bTuple = blue.map{p ⇒ (p, tangentialLine(p, ca), tangentialLine(p, cb))}
  val edges = rTuple.map{r ⇒ bTuple.map{b ⇒ minDistanceByCircle(r, b, (ca, cb))}}
  searchMinCostMatch(edges) match{
    case Some(d) ⇒ println(d)
    case None ⇒ println("Impossible")
  }

  case class Point(x: Double, y: Double) {
    def + (other: Point): Point = Point(x + other.x, y + other.y)

    def - (other: Point): Point = Point(x - other.x, y - other.y)
  }

  case class Circle(center: Point, radius: Double)

  object Line {
    def apply(point: Point, angle: Angle): Line = new Line(-angle.sin, angle.cos, angle.sin * point.x - angle.cos * point.y)

    def apply(a: Point, b: Point): Line = new Line(b.y - a.y, a.x - b.x, (a.y - b.y) * b.x + (b.x - a.x) * b.y)

    def isParallel(lineA: Line, lineB: Line): Boolean = abs(lineA.mA * lineB.mB - lineA.mB * lineB.mA) < 1e-10

    def crossPoint(lineA: Line, lineB: Line): Option[Point] = {
      if (isParallel(lineA, lineB)) None
      else {
        val x = (lineB.mC * lineA.mB - lineA.mC * lineB.mB) / (lineA.mA * lineB.mB - lineB.mA * lineA.mB)
        val y = (lineB.mA * lineA.mC - lineA.mA * lineB.mC) / (lineA.mA * lineB.mB - lineB.mA * lineA.mB)
        Some(Point(x, y))
      }
    }
  }

  class Line private(val mA: Double, val mB: Double, val mC: Double) {
    def angle: Angle = {
      val len = sqrt(mA * mA + mB * mB)
      Angle(-mA / len, mB / len)
    }
  }

  case class Angle(sin: Double, cos: Double) {
    def + (other: Angle): Angle = Angle(sin * other.cos + cos * other.sin, cos * other.cos - sin * other.sin)

    def - (other: Angle): Angle = Angle(sin * other.cos - cos * other.sin, cos * other.cos + sin * other.sin)
  }

  case class Edge(endA: Point, endB: Point)

  def distance(a: Point, b: Point): Double = {
    sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y))
  }

  def isBlocked(edge: Edge, circle: Circle): Boolean = {
    val epsilon = 1e-10
    if (distance(edge.endA, circle.center) < circle.radius || distance(edge.endB, circle.center) < circle.radius) true
    val eLine = Line(edge.endA, edge.endB)
    val cLine = Line(circle.center, eLine.angle + Angle(1, 0))
    val cp = Line.crossPoint(eLine, cLine).get
    if (distance(cp, circle.center) > (1 - epsilon) * circle.radius) false
    else distance(edge.endA, edge.endB) > distance(cp, edge.endA) && distance(edge.endA, edge.endB) > distance(cp, edge.endB)
  }

  def minDistanceByCircle(a: (Point, Iterable[Line], Iterable[Line]), b: (Point, Iterable[Line], Iterable[Line]), circle: (Circle, Circle)): Option[Double] = {
    val (ca, cb) = circle
    val (ap, aaLines, abLines) = a
    val (bp, baLines, bbLines) = b
    if (!isBlocked(Edge(ap, bp), ca) && !isBlocked(Edge(ap, bp), cb)) Some(distance(ap, bp))
    else {
      val r = (aaLines.flatMap { aa ⇒ baLines.flatMap { ba ⇒ Line.crossPoint(aa, ba) } }.filterNot { p ⇒ isBlocked(Edge(ap, p), cb) || isBlocked(Edge(bp, p), cb) } ++
        aaLines.flatMap { aa ⇒ bbLines.flatMap { bb ⇒ Line.crossPoint(aa, bb) } }.filterNot { p ⇒ isBlocked(Edge(ap, p), cb) || isBlocked(Edge(bp, p), ca) } ++
        abLines.flatMap { ab ⇒ baLines.flatMap { ba ⇒ Line.crossPoint(ab, ba) } }.filterNot { p ⇒ isBlocked(Edge(ap, p), ca) || isBlocked(Edge(bp, p), cb) } ++
        abLines.flatMap { ab ⇒ bbLines.flatMap { bb ⇒ Line.crossPoint(ab, bb) } }.filterNot { p ⇒ isBlocked(Edge(ap, p), ca) || isBlocked(Edge(bp, p), ca) })
        .map { p ⇒ distance(ap, p) + distance(bp, p) }.filter {
        _ <= 10000
      }
      if (r.isEmpty) None
      else Some(r.min)
    }
  }

  def tangentialLine(point: Point, circle: Circle): Iterable[Line] = {
    val relationalCenter = circle.center - point
    val sinSquare = (circle.radius * circle.radius) / (relationalCenter.x * relationalCenter.x + relationalCenter.y * relationalCenter.y)
    val cosSquare = 1 - sinSquare
    val rAngle = Angle(sqrt(sinSquare), sqrt(cosSquare))
    val dist = distance(point, circle.center)
    val sin = relationalCenter.y / dist
    val cos = relationalCenter.x / dist
    val aAngle = Angle(sin, cos)
    List(Line(point, aAngle + rAngle), Line(point, aAngle - rAngle))
  }

  object Node {

    class Red(val id: Int, private val mStart: Start, private val mEdges: Array[Option[Double]]) extends Node {
      private var mPotential: Double = 0.0
      private var mMatchingBlue: Option[Blue] = None
      private var mBlueNodes: List[(Double, Blue)] = List()

      override def edges: Iterable[(Double, Node)] = mMatchingBlue match {
        case Some(blue) ⇒ (0.0, mStart) :: mBlueNodes.filterNot {
          case (_, `blue`) ⇒ true
          case _ ⇒ false
        }
        case None ⇒ mBlueNodes
      }

      override def potential: Double = mPotential

      override def resetPath(): Unit = {
        mPotential += minCost
        minCost = Double.MaxValue
        prev = None
      }

      def matchingCost: Double = {
        mEdges(mMatchingBlue.get.id).get
      }

      override def ensureIncrementalPath(next: Node): Unit = {
        mMatchingBlue = Some(next.asInstanceOf[Blue])
        prev.get.ensureIncrementalPath(this)
      }

      def setBlueNodes(blue: Array[Blue]): Unit = {
        mBlueNodes = blue.flatMap { b ⇒ mEdges(b.id).map { d ⇒ (d, b) } }.toList
      }
    }

    class Blue(val id: Int, private val mGoal: Goal, private val mEdges: Array[Option[Double]]) extends Node {
      private var mPotential: Double = mEdges.flatten.min
      private var mMatchingRed: Option[Red] = None

      override def potential: Double = mPotential

      override def resetPath(): Unit = {
        mPotential += minCost
        minCost = Double.MaxValue
        prev = None
      }

      def edges: Iterable[(Double, Node)] = {
        mMatchingRed.map[(Double, Node)] { it ⇒ (-mEdges(it.id).get, it) }.getOrElse((0.0, mGoal)) :: Nil
      }

      override def ensureIncrementalPath(next: Node): Unit = {
        mMatchingRed = Some(prev.get.asInstanceOf[Red])
        prev.get.ensureIncrementalPath(this)
      }
    }

    class Start extends Node {
      private var mIsUsed: Array[Boolean] = Array()
      private var mRedNodes: Array[Red] = Array()
      minCost = 0.0

      override def edges: Iterable[(Double, Node)] = {
        mIsUsed.zipWithIndex.filterNot {
          _._1
        }.map { z ⇒ (0.0, mRedNodes(z._2)) }
      }

      override def potential: Double = 0.0

      override def ensureIncrementalPath(next: Node): Unit = {
        mIsUsed(next.asInstanceOf[Red].id) = true
      }

      override def resetPath(): Unit = {}

      def setRedNodes(red: Array[Red]): Unit = {
        mRedNodes = red
        mIsUsed = Array.tabulate(red.length) { _ ⇒ false }
      }
    }

    class Goal extends Node {
      private var mIsUsed: Array[Boolean] = Array()
      private var mBlueNodes: Array[Blue] = Array()
      private var mPotential: Double = 0.0

      override def edges: Iterable[(Double, Node)] = mIsUsed.zipWithIndex.filter {
        _._1
      }.map { z ⇒ (0.0, mBlueNodes(z._2)) }

      override def potential: Double = mPotential

      override def ensureIncrementalPath(next: Node): Unit = ???

      override def resetPath(): Unit = {
        mPotential += minCost
        minCost = Double.MaxValue
        mIsUsed(prev.get.asInstanceOf[Blue].id) = true
        prev = None
      }

      def setBlueNodes(blue: Array[Blue]): Unit = {
        mBlueNodes = blue
        mIsUsed = Array.tabulate(blue.length) { _ ⇒ false }
      }
    }

  }

  sealed trait Node {
    def edges: Iterable[(Double, Node)]

    def potential: Double

    var minCost: Double = Double.MaxValue
    var prev: Option[Node] = None

    def ensureIncrementalPath(next: Node)

    def resetPath(): Unit
  }

  implicit object Ord extends Ordering[(Double, Node)]{
    override def compare(x: (Double, Node), y: (Double, Node)): Int = x._1.compare(y._1)
  }
  def searchMinCostMatch(edges: Array[Array[Option[Double]]]): Option[Double] = {
    if (edges.exists {
      _.forall {
        case Some(_) ⇒ true
        case None ⇒ false
      }
    }) None
    val n = edges.length
    val start = new Node.Start
    val goal = new Node.Goal
    val red = Array.tabulate(n) { i ⇒ new Node.Red(i, start, edges(i)) }
    val blue = Array.tabulate(n) { i ⇒ new Node.Blue(i, goal, edges.map {
      _ (i)
    })
    }
    red.foreach(_.setBlueNodes(blue))
    start.setRedNodes(red)
    goal.setBlueNodes(blue)
    val priorityQueue = scala.collection.mutable.PriorityQueue[(Double, Node)]()
    (0 until n).foreach { _ ⇒
      priorityQueue.enqueue((0.0, start))
      while (!priorityQueue.isEmpty) {
        val (dist, node) = priorityQueue.dequeue()
        if (node.minCost == dist) {
          node.edges.foreach { case (e, to) ⇒
            if (to.minCost > node.potential + e - to.potential + dist) {
              to.minCost = node.potential + e - to.potential + dist
              to.prev = Some(node)
              priorityQueue.enqueue((to.minCost, to))
            }
          }
        }
      }
      goal.prev match {
        case Some(n) ⇒ n.ensureIncrementalPath(goal)
        case None ⇒ return None
      }
      start.resetPath()
      goal.resetPath()
      red.foreach(_.resetPath())
      blue.foreach(_.resetPath())
    }
    Some(red.map {
      _.matchingCost
    }.sum)
  }
}