Submission #53157091

---

Source Code Expand

use proconio::input_interactive;
use rand::SeedableRng;
use rand_distr::{Distribution, Normal, WeightedAliasIndex};
use rand_pcg::Pcg64Mcg;
use std::collections::BinaryHeap;
use std::mem::swap;
use std::process::exit;

use num_complex::Complex;
use std::f64::consts::PI;

pub trait ChangeMinMax {
    fn chmin(&mut self, x: Self) -> bool;
    fn chmax(&mut self, x: Self) -> bool;
}

impl<T: PartialOrd> ChangeMinMax for T {
    fn chmin(&mut self, x: T) -> bool {
        *self > x && {
            *self = x;
            true
        }
    }

    fn chmax(&mut self, x: T) -> bool {
        *self < x && {
            *self = x;
            true
        }
    }
}

const TIME_LIMIT: f64 = 1.95;
const AREA_SIZE: f64 = 1e5;
const MAX_ACCEL: f64 = 5e2;
const GOAL_RADIUS: f64 = 1e3;
const MAX_TURNS: usize = 5000;

const NUM_PARTICLES: usize = 2000;
const TOO_NEAR_DIST: f64 = 2.0 * GOAL_RADIUS;
const VELOCITY_COEFFICIENT: f64 = 0.9;
const WEIGHT_COEFFICIENT: f64 = 5e-4;
const NUM_SELECTS: usize = 10;

fn main() {
    get_time();
    let input = Input::read_input();
    let mut controller = Controller::new(&input);
    let mut filter = ParticleFilter::new(&input, NUM_PARTICLES);
    for turn in 0..MAX_TURNS {
        if get_time() > TIME_LIMIT {
            Action {
                action_type: ActionType::Accel,
                x: 0,
                y: 0,
            }.print_action();
            let move_result = MoveResult::read_move_result();
            if controller.update_trips(&move_result) {
                break;
            }
            continue;
        }
        let action = controller.decide_action(turn, &filter);
        action.print_action();
        match action.action_type {
            ActionType::Accel => {
                filter.accelerate(&Point::new(action.x as f64, action.y as f64));
            },
            ActionType::Measure => {
                input_interactive! {
                    distance: f64,
                };
                filter.update_from_measure(&Point::new(action.x as f64, action.y as f64), distance);
            },
        }
        let move_result = MoveResult::read_move_result();
        if controller.update_trips(&move_result) {
            break;
        }
        filter.update_from_move(&move_result, turn);
    }
    exit(0);
}

struct Input {
    wind_std: f64,
    measure_std: f64,
    start_point: Point,
    goals: Vec<Point>,
    internal_walls: Vec<Segment>,
    walls: Vec<Segment>,
}

impl Input {
    fn read_input() -> Self {
        input_interactive! {
            num_goals: usize,
            num_walls: usize,
            wind_std: f64,
            measure_std: f64,
            start_point_f: (f64, f64),
            goals_f: [(f64, f64); num_goals],
            internal_walls_f: [(f64, f64, f64, f64); num_walls],
        };
        let start_point = Complex::new(start_point_f.0, start_point_f.1);
        let goals = goals_f.into_iter().map(|(x, y)| Point::new(x, y)).collect();
        let internal_walls: Vec<Segment> = internal_walls_f.into_iter().map(|(x1, y1, x2, y2)| Segment { a: Point::new(x1, y1), b: Point::new(x2, y2) }).collect();
        let surrounding_walls = vec![
            Segment {
                a: Complex::new(-AREA_SIZE, -AREA_SIZE),
                b: Complex::new(-AREA_SIZE, AREA_SIZE)
            },
            Segment {
                a: Complex::new(-AREA_SIZE, AREA_SIZE),
                b: Complex::new(AREA_SIZE, AREA_SIZE),
            },
            Segment {
                a: Complex::new(AREA_SIZE, AREA_SIZE),
                b: Complex::new(AREA_SIZE, -AREA_SIZE),
            },
            Segment {
                a: Complex::new(AREA_SIZE, -AREA_SIZE),
                b: Complex::new(-AREA_SIZE, -AREA_SIZE),
            }
        ];
        let mut walls = internal_walls.clone();
        walls.extend(surrounding_walls);

        Input {
            wind_std,
            measure_std,
            start_point,
            goals,
            internal_walls,
            walls,
        }
    }
}

enum ActionType {
    Accel,
    Measure,
}

struct Action {
    action_type: ActionType,
    x: i32,
    y: i32,
}

impl Action {
    fn print_action(&self) {
        let c = match self.action_type {
            ActionType::Accel => 'A',
            ActionType::Measure => 'S',
        };
        println!("{} {} {}", c, self.x, self.y);
    }
}

struct MoveResult {
    collision: bool,
    visited_goals: Vec<usize>,
}

impl MoveResult {
    fn read_move_result() -> Self {
        input_interactive! {
            collision: i32,
            num_visited_goals: usize,
            visited_goals: [usize; num_visited_goals],
        };
        MoveResult {
            collision: collision == 1,
            visited_goals,
        }
    }
}

struct Trip {
    progress: usize,
    targets: Vec<Point>,
    goal_id: usize,
}

struct Controller {
    progress: usize,
    trips: Vec<Trip>,
    visited: Vec<bool>,
    walls: Vec<Segment>,
    graph: Graph,
    wind_std: f64,
}

impl Controller {
    fn new(input: &Input) -> Self {
        // make cost matrix
        let graph = Graph::new(input);

        // solve tsp
        let order = solve_tsp(&graph.cost_matrix.into_iter().map(|v| v[0..(input.goals.len() + 1)].to_vec()).collect(), false);
        debug_assert_eq!(order.len(), input.goals.len() + 1);
        debug_assert_eq!(order[0], input.goals.len());

        // make trips
        let mut trips = Vec::with_capacity(input.goals.len());
        for i in 0..input.goals.len() {
            trips.push(Trip {
                progress: 0,
                targets: graph.paths[order[i]][order[i + 1]].clone(),
                goal_id: order[i + 1],
            });
        }
        Controller {
            progress: 0,
            trips,
            visited: vec![false; input.goals.len()],
            walls: input.walls.clone(),
            graph: Graph::new(input),
            wind_std: input.wind_std,
        }
    }

    fn decide_action(&mut self, turn: usize, filter: &ParticleFilter) -> Action {
        debug_assert!(self.progress < self.trips.len() && !self.trips[self.progress].targets.is_empty());
        let average_particle = filter.calc_average();
        // todo!("Exploration vs. Exploitation Tradeoff");
        if turn % 2 == 0 || self.wind_std == 0.0 {
            self.update_targets(&average_particle);
            self.decide_accel(&average_particle)
        } else {
            self.decide_measure(&average_particle, filter)
        }
    }

    fn update_targets(&mut self, average_particle: &Particle) {
        let pos = average_particle.position;
        let trip = &mut self.trips[self.progress];
        while trip.progress + 1 < trip.targets.len() {
            if (pos - trip.targets[trip.progress]).norm() < GOAL_RADIUS {
                trip.progress += 1;
            } else if !can_move_linearly(&self.walls, &pos, &trip.targets[trip.progress + 1]) {
                break;
            } else if calc_wall_distance(&self.walls, &Segment { a: pos, b: trip.targets[trip.progress + 1], }) > TOO_NEAR_DIST {
                trip.progress += 1;
            } else {
                break;
            }
        }
        if !can_move_linearly(&self.walls, &pos, &trip.targets[trip.progress]) {
            // update the route
            trip.targets = self.graph.calc_path(&self.walls, self.wind_std, trip.goal_id, &pos);
            trip.progress = 0;
        }
    }

    fn decide_accel(&self, average_particle: &Particle) -> Action {
        let vel = average_particle.velocity;
        let trip = &self.trips[self.progress];
        let target_point = trip.targets[trip.progress];
        let goal = target_point - average_particle.position;
        let path = Segment {
            a: Point::new(0.0, 0.0),
            b: goal,
        };
        let acc = projection(&path, &vel);
        let approaching = dot(&goal, &acc) > 0.0;
        let fix = acc - vel;
        let accel =
            if fix.norm() >= MAX_ACCEL {
                Point::from_polar(MAX_ACCEL, fix.arg())
            } else {
                let max_proceed = (MAX_ACCEL * MAX_ACCEL - fix.norm_sqr()).sqrt();
                let proceed = (self.max_velocity(goal.norm()) + if approaching { -acc.norm() } else { acc.norm() }).clamp(-max_proceed, max_proceed);
                fix + Point::from_polar(proceed, goal.arg())
            };
        let (x, y) = trunc_accel(&accel);
        Action {
            action_type: ActionType::Accel,
            x,
            y,
        }
    }

    fn decide_measure(&self, average_particle: &Particle, filter: &ParticleFilter) -> Action {
        let p = average_particle.position;
        let mut selector = BinaryHeap::new();
        for wall in self.walls.iter() {
            let mut candidates = vec![wall.a, wall.b];
            let nearest_point = calc_nearest_point(wall, &p);
            if nearest_point != wall.a && nearest_point != wall.b {
                candidates.push(nearest_point);
            }
            for q in candidates {
                if !can_move_linearly(&self.walls, &p, &q) || q == p {
                    continue;
                }
                let (x, y) = trunc_accel(&Point::from_polar(MAX_ACCEL, (q - p).arg()));
                let item = ((q - p).norm() as i64, x, y);
                if selector.len() < NUM_SELECTS {
                    selector.push(item);
                } else if item < *selector.peek().unwrap() {
                    selector.pop();
                    selector.push(item);
                }
            }
        }
        let mut max_profit = f64::MIN;
        let mut ret = Action {
            action_type: ActionType::Measure,
            x: 0,
            y: 0,
        };
        for (_, x, y) in selector.iter() {
            if max_profit.chmax(self.calc_measure_profit(filter, &Point::new(*x as f64, *y as f64))) {
                ret.x = *x;
                ret.y = *y;
            }
        }
        debug_assert!(ret.x != 0 || ret.y != 0);
        ret
    }

    // [standard deviation of true distances] / [mean of true distances]
    fn calc_measure_profit(&self, filter: &ParticleFilter, direction: &Point) -> f64 {
        let mut sum_d = 0.0;
        let mut sum_d2 = 0.0;
        for particle in filter.particles.iter() {
            let p = particle.position;
            let q = calc_collision_point(&self.walls, &get_ray(&p, direction));
            let distance = (q - p).norm();
            sum_d += distance;
            sum_d2 += distance * distance;
        }
        sum_d /= filter.particles.len() as f64;
        sum_d2 /= filter.particles.len() as f64;
        (sum_d2 - sum_d * sum_d).sqrt() / sum_d
    }

    fn update_trips(&mut self, move_result: &MoveResult) -> bool {
        debug_assert!(self.progress < self.trips.len() && !self.trips[self.progress].targets.is_empty());
        for goal_id in move_result.visited_goals.iter() {
            debug_assert!(!self.visited[*goal_id]);
            self.visited[*goal_id] = true;
        }
        while self.progress < self.trips.len() {
            if self.visited[self.trips[self.progress].goal_id] {
                self.progress += 1;
            } else {
                break;
            }
        }
        self.progress == self.trips.len()
    }

    fn max_velocity(&self, distance: f64) -> f64 {
        // Theoretically, we can accelerate up to arround (2.0 * a * d).sqrt() where a is the acceleration and d is the distance.
        if self.wind_std == 0.0 {
            VELOCITY_COEFFICIENT * (2.0 * MAX_ACCEL * distance).sqrt()
        } else {
            VELOCITY_COEFFICIENT * (MAX_ACCEL * distance).sqrt()
        }
    }
}

struct Graph {
    points: Vec<Point>,
    cost_matrix: Vec<Vec<i64>>,
    paths: Vec<Vec<Vec<Point>>>,
}

impl Graph {
    fn new(input: &Input) -> Self {
        let mut points = input.goals.clone();
        points.push(input.start_point);
        points.extend(make_waypoints(input));

        let mut edges = vec![Vec::new(); points.len()];
        for u in 0..points.len() {
            for v in 0..points.len() {
                if can_move_linearly(&input.walls, &points[u], &points[v]) {
                    edges[u].push((v, calc_edge_weight(&input.walls, input.wind_std, &points[u], &points[v]) as i64));
                }
            }
        }
        let n = input.goals.len() + 1;
        let inf = i64::max_value() / 2;
        let mut cost_matrix = vec![vec![0; points.len()]; n];
        let mut paths = vec![vec![Vec::new(); points.len()]; n];
        let mut costs = vec![0; points.len()];
        let mut from = vec![0; points.len()];
        for s in 0..n {
            // Dijkstra's algorithm
            costs.fill(-inf);
            costs[s] = 0;
            from.fill(points.len());
            let mut todo = BinaryHeap::new();
            todo.push((0, s));
            while !todo.is_empty() {
                let (c, u) = todo.pop().unwrap();
                if c < costs[u] {
                    continue;
                }
                for (v, w) in edges[u].iter() {
                    let cost = c - w;
                    if costs[*v].chmax(cost) {
                        from[*v] = u;
                        todo.push((cost, *v));
                    }
                }
            }
            // restore paths
            for t in 0..points.len() {
                debug_assert_ne!(costs[t], -inf);
                cost_matrix[s][t] = -costs[t];
                let mut v = t;
                while v != s {
                    paths[s][t].push(points[v]);
                    v = from[v];
                }
                paths[s][t].reverse();
            }
        }
        Graph {
            points,
            cost_matrix,
            paths,
        }
    }

    fn calc_path(&self, walls: &Vec<Segment>, wind_std: f64, goal_id: usize, p: &Point) -> Vec<Point> {
        let mut min_cost = i64::max_value() / 2;
        let mut min_cost_v = self.points.len();
        for v in 0..self.points.len() {
            if !can_move_linearly(walls, p, &self.points[v]) {
                continue;
            }
            if min_cost.chmin(self.cost_matrix[goal_id][v] + calc_edge_weight(walls, wind_std, p, &self.points[v])) {
                min_cost_v = v;
            }
        }
        debug_assert_ne!(min_cost_v, self.points.len());
        let mut ret = self.paths[goal_id][min_cost_v].clone();
        ret.reverse();
        ret.push(self.points[goal_id]);
        ret
    }
}

fn calc_edge_weight(walls: &Vec<Segment>, wind_std: f64, p: &Point, q: &Point) -> i64 {
    let mut weight = (p - q).norm().sqrt();
    let wall_distance = calc_wall_distance(walls, &Segment { a: *p, b: *q, });
    if wall_distance < TOO_NEAR_DIST {
        weight *= 1.0 + WEIGHT_COEFFICIENT * wind_std * (TOO_NEAR_DIST / wall_distance.max(1.0)).log2();
    }
    weight as i64
}

fn make_waypoints(input: &Input) -> Vec<Point> {
    let mut waypoints = Vec::new();
    for i in 0..(input.internal_walls.len() + input.goals.len() + 1) {
        let points = 
            if i < input.internal_walls.len() {
                vec![input.internal_walls[i].a, input.internal_walls[i].b]
            } else if i - input.internal_walls.len() < input.goals.len() {
                vec![input.goals[i - input.internal_walls.len()]]
            } else {
                vec![input.start_point]
            };
        for p in points {
            if p.re.abs() == AREA_SIZE || p.im.abs() == AREA_SIZE {
                continue;
            }
            for wall in input.walls.iter() {
                if p == wall.a || p == wall.b {
                    continue;
                }
                let q = calc_nearest_point(wall, &p);
                if can_move_linearly(&input.walls, &p, &q) {
                    waypoints.push((p + q).scale(0.5));
                }
            }
        }
    }
    waypoints
}

fn trunc_accel(accel: &Point) -> (i32, i32) {
    let x = accel.re.trunc() as i32;
    let y = accel.im.trunc() as i32;
    debug_assert!(x * x + y * y <= MAX_ACCEL as i32 * MAX_ACCEL as i32);
    (x, y)
}

#[derive(Clone)]
struct Particle {
    position: Point,
    velocity: Point,
    likelihood: f64,
}

struct ParticleFilter {
    rng: Pcg64Mcg,
    num_particles: usize,
    particles: Vec<Particle>,
    particles_buffer: Vec<Particle>,
    measure_std: f64,
    goals: Vec<Point>,
    walls: Vec<Segment>,
    internal_walls: Vec<Segment>,
    wind_norm: Normal<f64>,
    visited: Vec<bool>,
}

impl ParticleFilter {
    fn new(input: &Input, num_particles: usize) -> Self {
        let particle = Particle {
            position: input.start_point,
            velocity: Complex::new(0.0, 0.0),
            likelihood: 1.0 / num_particles as f64,
        };
        ParticleFilter {
            rng: Pcg64Mcg::seed_from_u64(0),
            num_particles,
            particles: vec![particle; num_particles],
            particles_buffer: Vec::with_capacity(num_particles),
            measure_std: input.measure_std,
            goals: input.goals.clone(),
            walls: input.walls.clone(),
            internal_walls: input.internal_walls.clone(),
            wind_norm: Normal::<f64>::new(0.0, input.wind_std).unwrap(),
            visited: vec![false; input.goals.len()],
        }
    }

    fn update_from_measure(&mut self, direction: &Point, measured_distance: f64) {
        for particle_id in 0..self.particles.len() {
            self.particles[particle_id].likelihood *= self.measure_likelihood(&self.particles[particle_id].position, direction, measured_distance);
        }
    }

    fn measure_likelihood(&self, position: &Point, direction: &Point, measured_distance: f64) -> f64 {
        let ray = get_ray(position, direction);
        let collision_point = calc_collision_point(&self.walls, &ray);
        debug_assert_ne!(collision_point, ray.b);
        let distance = (collision_point - position).norm().max(EPS);
        self.measure_norm_pdf(measured_distance / distance).max(EPS)
    }

    fn measure_norm_pdf(&self, x: f64) -> f64 {
        // You can ignore the constant multiplication because you use relative likelihood.
        let y = (x - 1.0) / (self.measure_std + EPS);
        (-0.5 * y * y).exp()
    }

    fn accelerate(&mut self, accel: &Point) {
        for particle in &mut self.particles {
            particle.velocity += accel;
        }
    }

    fn update_from_move(&mut self, move_result: &MoveResult, turn: usize) {
        let weights: Vec<f64> = self.particles.iter().map(|particle| particle.likelihood).collect();
        let distribution = WeightedAliasIndex::new(weights).unwrap();
        self.particles_buffer.clear();
        while self.particles_buffer.len() < self.num_particles {
            for _ in 0..self.num_particles {
                let particle_id = distribution.sample(&mut self.rng);
                let mut particle = self.particles[particle_id].clone();
                particle.velocity.re += self.wind_norm.sample(&mut self.rng);
                particle.velocity.im += self.wind_norm.sample(&mut self.rng);
                if self.consistent_with_move_result(&mut particle, move_result) {
                    self.particles_buffer.push(particle);
                    if self.particles_buffer.len() == self.num_particles {
                        break;
                    }
                }
            }
            if get_time() > (turn as f64 / MAX_TURNS as f64) * TIME_LIMIT {
                if !self.particles_buffer.is_empty() {
                    break;
                } else if !move_result.visited_goals.is_empty() {
                    let goal = self.goals[move_result.visited_goals[0]];
                    for particle in &mut self.particles {
                        particle.position = goal;
                        self.particles_buffer.push(particle.clone());
                    }
                    break;
                } else if move_result.collision {
                    for particle in &mut self.particles {
                        particle.velocity = Point::new(0.0, 0.0);
                        self.particles_buffer.push(particle.clone());
                    }
                    break;
                } else {
                    // All particles died :(
                    return;
                }
            }
        }
        swap(&mut self.particles, &mut self.particles_buffer);
        let likelihood = 1.0 / self.particles.len() as f64;
        for particle in &mut self.particles {
            particle.likelihood = likelihood;
        }
        for goal_id in move_result.visited_goals.iter() {
            self.visited[*goal_id] = true;
        }
    }

    // judge consistency, then update the particle
    fn consistent_with_move_result(&self, particle: &mut Particle, move_result: &MoveResult) -> bool {
        let path = Segment {
            a: particle.position,
            b: particle.position + particle.velocity,
        };
        if path.b.re.abs() < AREA_SIZE && path.b.im.abs() < AREA_SIZE && can_move_linearly(&self.internal_walls, &path.a, &path.b) {
            particle.position = path.b;
            !move_result.collision && self.calc_visited_goals(&path) == move_result.visited_goals
        } else {
            particle.velocity = Point::new(0.0, 0.0);
            move_result.collision
        }
    }

    fn calc_visited_goals(&self, path: &Segment) -> Vec<usize> {
        let mut ret = Vec::new();
        for goal_id in 0..self.goals.len() {
            if !self.visited[goal_id] && calc_distance(path, &self.goals[goal_id]) <= GOAL_RADIUS {
                ret.push(goal_id);
            }
        }
        ret
    }

    fn calc_average(&self) -> Particle {
        let mut position = Point::new(0.0, 0.0);
        let mut velocity = Point::new(0.0, 0.0);
        for particle in self.particles.iter() {
            position += particle.likelihood * particle.position;
            velocity += particle.likelihood * particle.velocity;
        }
        Particle {
            position,
            velocity,
            likelihood: 1.0,
        }
    }
}

fn can_move_linearly(walls: &Vec<Segment>, p: &Point, q: &Point) -> bool {
    let path = Segment {
        a: p + Point::from_polar(1.0, (q - p).arg()),
        b: q - Point::from_polar(1.0, (q - p).arg()),
    };
    for wall in walls {
        if intersect(&path, wall) {
            return false;
        }
    }
    true
}

fn calc_wall_distance(walls: &Vec<Segment>, path: &Segment) -> f64 {
    let mut ret = f64::INFINITY;
    for wall in walls {
        ret.chmin(calc_distance(wall, &path.a));
        ret.chmin(calc_distance(wall, &path.b));
        ret.chmin(calc_distance(path, &wall.a));
        ret.chmin(calc_distance(path, &wall.b));
    }
    ret
}

fn calc_collision_point(walls: &Vec<Segment>, path: &Segment) -> Point {
    let mut max_distance = (path.a - path.b).norm();
    let mut ret = path.b;
    for segment in walls {
        if intersect(segment, path) {
            let collision_point = cross_point(segment, path);
            let distance = (collision_point - path.a).norm();
            if max_distance.chmin(distance) {
                ret = collision_point;
            }
        }
    }
    ret
}

fn get_ray(position: &Point, direction: &Point) -> Segment {
    Segment {
        a: *position,
        b: position + direction.scale(4.0 * AREA_SIZE / direction.norm()),
    }
}

// ------------------
// geometry library

pub type Point = Complex<f64>;

pub const EPS: f64 = 1e-9;

pub fn radian_to_degree(theta: f64) -> f64 {
    (180.0 / PI) * theta
}

pub fn degree_to_radian(degree: f64) -> f64 {
    (PI / 180.0) * degree
}

// almost equal
pub fn eq(a: f64, b: f64) -> bool {
    (b - a).abs() < EPS
}

// angle of b-a-c
pub fn angle(a: &Point, b: &Point, c: &Point) -> f64 {
    (c - a).arg() - (b - a).arg()
}

pub fn rot(p: &Point, theta: f64) -> Point {
    Point::from_polar(1.0, theta) * p
}

pub fn cross_product(a: &Point, b: &Point) -> f64 {
    a.re * b.im - a.im * b.re
}

pub fn dot(a: &Point, b: &Point) -> f64 {
    a.re * b.re + a.im * b.im
}

// https://judge.u-aizu.ac.jp/onlinejudge/description.jsp?id=CGL_1_C
// positional relationship between b and c from a
pub fn ccw(a: &Point, b: &Point, c: &Point) -> i32 {
    let ba = b - a;
    let ca = c - a;
    if cross_product(&ba, &ca) > EPS {
        1 // counter-clockwise
    } else if cross_product(&ba, &ca) < -EPS {
        -1 // clockwise
    } else if dot(&ba, &ca) < 0.0 {
        2 // online back (c-a-b)
    } else if ba.norm_sqr() < ca.norm_sqr() {
        -2 // online front (a-b-c)
    } else {
        0 // on segment (a-c-b)
    }
}

#[derive(Clone, PartialEq)]
pub struct Segment {
    a: Point,
    b: Point,
}

pub fn projection(s: &Segment, p: &Point) -> Point {
    let t = dot(&(p - s.a), &(s.b - s.a)) / (s.b - s.a).norm_sqr();
    s.a + (s.b - s.a).scale(t)
}

pub fn on_segment(s: &Segment, p: &Point) -> bool {
    ccw(&s.a, &s.b, p) == 0
}

pub fn intersect(s: &Segment, t: &Segment) -> bool {
    ccw(&s.a, &s.b, &t.a) * ccw(&s.a, &s.b, &t.b) <= 0 && ccw(&t.a, &t.b, &s.a) * ccw(&t.a, &t.b, &s.b) <= 0
}

pub fn calc_distance(s: &Segment, p: &Point) -> f64 {
    let r = projection(s, p);
    if dot(&(s.a - r), &(s.b - r)) < 0.0 {
        (r - p).norm()
    } else {
        (s.a - p).norm().min((s.b - p).norm())
    }
}

pub fn calc_nearest_point(s: &Segment, p: &Point) -> Point {
    let r = projection(s, p);
    if dot(&(s.a - r), &(s.b - r)) < 0.0 {
        r
    } else if (s.a - p).norm_sqr() < (s.b - p).norm_sqr() {
        s.a
    } else {
        s.b
    }
}

pub fn cross_point(s: &Segment, t: &Segment) -> Point {
    let a = cross_product(&(s.b - s.a), &(t.b - t.a));
    let b = cross_product(&(s.b - s.a), &(s.b - t.a));
    if eq(a.abs(), 0.0) && eq(b.abs(), 0.0) {
        t.a
    } else {
        t.a + (b / a) * (t.b - t.a)
    }
}

// ------------------

// calculate the TSP optimal solution
// start from the last vertex (and end to the last vertex)
pub fn solve_tsp(cost_matrix: &Vec<Vec<i64>>, back_to_the_start_point: bool) -> Vec<usize> {
    if cost_matrix.len() == 1 {
        return vec![0];
    }
    let n = cost_matrix.len() - 1;
    let inf = i64::max_value() / 2;

    // dp
    let mut dp = vec![vec![inf; 1 << n]; n];
    for v in 0..n {
        dp[v][1 << v] = cost_matrix[n][v];
    }
    for s in 1..(1 << n) {
        for u in 0..n {
            if ((s >> u) & 1) == 0 {
                continue;
            }
            for v in 0..n {
                if ((s >> v) & 1) == 0 {
                    let cost = dp[u][s] + cost_matrix[u][v];
                    dp[v][s | (1 << v)].chmin(cost);
                }
            }
        }
    }
    let mut last = n;
    let mut cost = inf;
    let mut s = (1 << n) - 1;
    for v in 0..n {
        if back_to_the_start_point {
            if cost.chmin(dp[v][s] + cost_matrix[v][n]) {
                last = v;
            }
        } else {
            if cost.chmin(dp[v][s]) {
                last = v;
            }
        }
    }
    let mut ret = vec![n; n + 1 + back_to_the_start_point as usize];
    ret[n] = last;
    s ^= 1 << last;
    for i in (1..n).rev() {
        for v in 0..n {
            if (s & (1 << v)) > 0 && dp[v][s] + cost_matrix[v][last] == dp[last][s | (1 << last)] {
                ret[i] = v;
                s ^= 1 << v;
                break;
            }
        }
        debug_assert_ne!(ret[i], n);
    }
    ret
}

pub fn get_time() -> f64 {
    static mut STIME: f64 = -1.0;
    let t = std::time::SystemTime::now().duration_since(std::time::UNIX_EPOCH).unwrap();
    let ms = t.as_secs() as f64 + t.subsec_nanos() as f64 * 1e-9;
    unsafe {
        if STIME < 0.0 {
            STIME = ms;
        }
        ms - STIME
    }
}

Submission Info

Judge Result