Submission #66147796

---

Source Code Expand

#include <iostream>
#include <string>
#include <vector>
#include <tuple>
#include <chrono>
#include <algorithm>
#include <random>
#include <map>
#include <set>
#include <queue>
#include <random>
#include <chrono>
#include <cmath>
#include <climits>
#include <bitset>
#include <time.h>
// eigen
#include <Eigen/Dense>

using namespace std;
using Ps = pair<short, short>;
using vec_int = vector<int>;
using P = pair<int, int>;
using P2 = pair<P, P>;
using P3 = pair<float, int>;
using T = tuple<int, int, int>;
using T2 = tuple<float, int, int>;
using T3 = tuple<float, int, int, int, int>;
using T4 = tuple<int, int, int, int>;
using T5 = tuple<int, int, int, int, int>;
using TT = tuple<int, int, int>;
using ll = long long;
using uc = unsigned char;
#define rep(i, n) for (int i = 0; i < (int)(n); i++)

int N, M;
double eps, delta;
int sx, sy;
vector<int> px, py;
vector<int> lx, ly, rx, ry;

// #pragma GCC optimize("Ofast")

std::mt19937 mt{12345};
int RAND_ARR_LEN = 100000;
int RAND_RANGE = 1000000000;
float TIME_LIMIT = 5000;
float INV_TIME_LIMIT = 1. / 1000.;
const int JOB_MAX_LEN = 1003;
std::uniform_int_distribution<int> dist(1, RAND_RANGE);

int WAITING_TASK = 1001;

void remove_val_and_pop_from_last(vec_int &A, int val)
{
    for (int i = 0; i < A.size(); i++)
    {
        if (A.at(i) == val)
        {
            A.at(i) = A.at(A.size() - 1);
            A.pop_back();
            return;
        }
    }
    throw runtime_error("trying to remove non-existing value");
}

class Rand
{
private:
    int count = 0;
    vec_int rand_arr;

public:
    Rand(){
        rep(i, RAND_ARR_LEN){
            rand_arr.push_back(dist(mt));
}
}
;
int get();
int get_rand(int from, int to);
float get_float();
}
;

int Rand::get()
{
    int num = rand_arr.at(count);
    count += 1;
    count %= RAND_ARR_LEN;
    return num;
}

int Rand::get_rand(int from, int to)
{
    int diff = to - from;
    int num = get() % diff;
    return num + from;
}

float Rand::get_float()
{
    // 0~1の間の一様乱数
    return (float)get() / (float)RAND_RANGE;
}

Rand ro;

class PseudoSet
{
private:
    vec_int index_arr;

public:
    vec_int data;
    PseudoSet() {};
    PseudoSet(int value_range)
    {
        index_arr = vec_int(value_range, -1);
    }

    bool count(int value);
    void insert(int value);
    void erase(int value);
    vec_int get_data() { return data; };
    int size() { return data.size(); };
    int get_random() { return data.at(ro.get_rand(0, size())); };
};

bool PseudoSet::count(int value)
{
    return index_arr[value] != -1;
}

void PseudoSet::insert(int value)
{
    if (count(value))
        return;
    index_arr[value] = data.size();
    data.push_back(value);
}

void PseudoSet::erase(int value)
{
    if (!count(value))
    {
        // throw runtime_error("no existing value:"+to_string(value));
        return;
    }
    int tail_value = data[data.size() - 1];
    if (value == tail_value)
    {
        data.pop_back();
        index_arr[value] = -1;
    }
    else
    {
        index_arr[tail_value] = index_arr[value];
        index_arr[value] = -1;
        data[index_arr[tail_value]] = tail_value;
        data.pop_back();
    }
}

float get_time(bool init)
{
    static std::chrono::system_clock::time_point start;
    if (init)
    {
        start = std::chrono::system_clock::now();
    }
    std::chrono::system_clock::time_point end = std::chrono::system_clock::now();
    return std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count(); // 処理に要した時間をミリ秒に変換
}

double current_tmp(double current_time, double T0, double T1)
{
    return T1 + (T0 - T1) * (TIME_LIMIT * 0.92 - current_time) / (TIME_LIMIT * 0.92);
    // double start_time = TIME_LIMIT * 0.6;
    // double x = (current_time - start_time) / (TIME_LIMIT * 0.32);
    // return pow(T1, 1 - x) + (T0, x);
}

bool is_valid_transition(int diff, double current_time, double T0, float T1)
{
    float t = current_tmp(current_time, T0, T1);
    float rand = ro.get_float();
    // diffが正だったら常にアクセぷと
    return rand < exp(((float)diff) / t);
}

/**
 * @brief カルマンフィルタの状態を表すクラス
 *
 * @tparam StateSize 状態ベクトルのサイズ
 */
template <int StateSize>
struct KalmanState
{
    Eigen::Matrix<double, StateSize, 1> x;         // 状態ベクトル
    Eigen::Matrix<double, StateSize, StateSize> P; // 共分散行列

    KalmanState()
    {
        x.setZero();
        P.setIdentity();
    }

    KalmanState(const Eigen::Matrix<double, StateSize, 1> &state,
                const Eigen::Matrix<double, StateSize, StateSize> &covariance)
        : x(state), P(covariance) {}
};

/**
 * @brief 観測値を表すクラス
 *
 * @tparam ObservationSize 観測ベクトルのサイズ
 */
template <int ObservationSize>
struct Observation
{
    Eigen::Matrix<double, ObservationSize, 1> y; // 観測ベクトル

    Observation()
    {
        y.setZero();
    }

    explicit Observation(const Eigen::Matrix<double, ObservationSize, 1> &measurement)
        : y(measurement) {}
};

/**
 * @brief カルマンフィルタクラス
 *
 * 状態方程式: x(k+1) = A x(k) + b1 u(k) + b0 v(k)
 * 観測方程式: y(k) = C x(k) + w(k)
 *
 * @tparam StateSize 状態ベクトルのサイズ
 * @tparam InputSize 制御入力ベクトルのサイズ
 * @tparam ObservationSize 観測ベクトルのサイズ
 */
template <int StateSize, int InputSize, int ObservationSize>
class KalmanFilter
{
public:
    using StateVector = Eigen::Matrix<double, StateSize, 1>;
    using StateMatrix = Eigen::Matrix<double, StateSize, StateSize>;
    using InputVector = Eigen::Matrix<double, InputSize, 1>;
    using InputMatrix = Eigen::Matrix<double, StateSize, InputSize>;
    using ObservationVector = Eigen::Matrix<double, ObservationSize, 1>;
    using ObservationMatrix = Eigen::Matrix<double, ObservationSize, StateSize>;
    using KalmanGain = Eigen::Matrix<double, StateSize, ObservationSize>;

    /**
     * @brief コンストラクタ
     */
    KalmanFilter()
    {
        A.setIdentity();
        B1.setZero();
        B0.setZero();
        C.setZero();
    }

    /**
     * @brief パラメータ付きコンストラクタ
     *
     * @param state_transition 状態遷移行列 A
     * @param control_input 制御入力行列 B1
     * @param process_noise プロセスノイズ入力行列 B0
     * @param observation 観測行列 C
     */
    KalmanFilter(
        const StateMatrix &state_transition,
        const InputMatrix &control_input,
        const StateMatrix &process_noise,
        const ObservationMatrix &observation)
        : A(state_transition), B1(control_input), B0(process_noise),
          C(observation)
    {
    }

    /**
     * @brief システム行列の設定
     *
     * @param state_transition 状態遷移行列 A
     * @param control_input 制御入力行列 B1
     * @param process_noise プロセスノイズ入力行列 B0
     * @param observation 観測行列 C
     */
    void setSystemMatrices(
        const StateMatrix &state_transition,
        const InputMatrix &control_input,
        const StateMatrix &process_noise,
        const ObservationMatrix &observation)
    {
        A = state_transition;
        B1 = control_input;
        B0 = process_noise;
        C = observation;
    }

    /**
     * @brief 初期状態の設定
     *
     * @param initial_state 初期状態ベクトル
     * @param initial_covariance 初期共分散行列
     * @return KalmanState<StateSize> 初期化された状態
     */
    KalmanState<StateSize> initialize(
        const StateVector &initial_state,
        const StateMatrix &initial_covariance)
    {
        return KalmanState<StateSize>(initial_state, initial_covariance);
    }

    /**
     * @brief 予測ステップ（時間更新）
     *
     * @param current_state 現在の状態
     * @param control_input 制御入力 u(k)
     * @param process_noise_cov プロセスノイズ共分散行列
     * @return KalmanState<StateSize> 予測された次の状態
     */
    KalmanState<StateSize> predict(
        const KalmanState<StateSize> &current_state,
        const InputVector &control_input,
        const StateMatrix &process_noise_cov)
    {
        // 状態の予測
        StateVector predicted_x = A * current_state.x + B1 * control_input;

        // 共分散行列の予測
        StateMatrix predicted_P = A * current_state.P * A.transpose() + B0 * process_noise_cov * B0.transpose();

        return KalmanState<StateSize>(predicted_x, predicted_P);
    }

    /**
     * @brief 更新ステップ（観測更新）
     *
     * @param predicted_state 予測された状態
     * @param observation 観測値
     * @param observation_noise_cov 観測ノイズ共分散行列
     * @return KalmanState<StateSize> 更新された状態
     */
    KalmanState<StateSize> update(
        const KalmanState<StateSize> &predicted_state,
        const Observation<ObservationSize> &observation,
        const Eigen::Matrix<double, ObservationSize, ObservationSize> &observation_noise_cov)
    {
        // イノベーション（予測誤差）の計算
        ObservationVector innovation = observation.y - C * predicted_state.x;

        // イノベーション共分散行列
        Eigen::Matrix<double, ObservationSize, ObservationSize> S =
            C * predicted_state.P * C.transpose() + observation_noise_cov;

        // カルマンゲインの計算
        KalmanGain K = predicted_state.P * C.transpose() * S.inverse();

        // 状態の更新
        StateVector updated_x = predicted_state.x + K * innovation;

        // 共分散行列の更新
        StateMatrix updated_P = (StateMatrix::Identity() - K * C) * predicted_state.P;

        return KalmanState<StateSize>(updated_x, updated_P);
    }

    /**
     * @brief 予測と更新を一度に行う
     *
     * @param current_state 現在の状態
     * @param control_input 制御入力
     * @param observation 観測値
     * @param process_noise_cov プロセスノイズ共分散行列
     * @param observation_noise_cov 観測ノイズ共分散行列
     * @return KalmanState<StateSize> 更新された状態
     */
    KalmanState<StateSize> step(
        const KalmanState<StateSize> &current_state,
        const InputVector &control_input,
        const Observation<ObservationSize> &observation,
        const StateMatrix &process_noise_cov,
        const Eigen::Matrix<double, ObservationSize, ObservationSize> &observation_noise_cov)
    {
        // 予測ステップ
        KalmanState<StateSize> predicted_state = predict(current_state, control_input, process_noise_cov);

        // 更新ステップ
        return update(predicted_state, observation, observation_noise_cov);
    }

private:
    StateMatrix A;       // 状態遷移行列
    InputMatrix B1;      // 制御入力行列
    StateMatrix B0;      // プロセスノイズ入力行列
    ObservationMatrix C; // 観測行列
};

void handle_input()
{
    cin >> N >> M >> eps >> delta;
    cin >> sx >> sy;
    px.resize(N);
    py.resize(N);
    rep(i, N) cin >> px[i] >> py[i];
    rep(i, N) cerr << px[i] << " " << py[i] << endl;
    lx.resize(M);
    ly.resize(M);
    rx.resize(M);
    ry.resize(M);
    rep(i, M) cin >> lx[i] >> ly[i] >> rx[i] >> ry[i];
}

int main(int argc, char *argv[])
{
#ifdef OPTUNA_STUDY
    cerr << "optuna study, override parameters" << endl;
    T0 = stod(argv[1]);
    T1 = stod(argv[2]);
#endif
    get_time(true);
    handle_input();
    cerr << N << " " << M << " " << eps << " " << delta << endl;
    cerr << sx << " " << sy << endl;

    // カルマンフィルタの状態を作る
    // 初期の状態ベクトルは(sx, sy, 0, 0)とする
    Eigen::Matrix<double, 4, 1> initial_state = Eigen::Matrix<double, 4, 1>::Zero();
    initial_state(0) = sx;
    initial_state(1) = sy;
    // 初期の共分散行列は対角成分が1の行列にする
    Eigen::Matrix<double, 4, 4> initial_covariance = Eigen::Matrix<double, 4, 4>::Identity();
    auto ks = KalmanState<4>(initial_state, initial_covariance);

    // 状態遷移行列(A)
    Eigen::Matrix<double, 4, 4> A = Eigen::Matrix<double, 4, 4>::Identity();
    A(0, 2) = 1;
    A(1, 3) = 1;

    // 制御入力行列(B1)は、単位行列にする
    Eigen::Matrix<double, 4, 4> B1 = Eigen::Matrix<double, 4, 4>::Identity();
    B1(0, 2) = 1;
    B1(1, 3) = 1;

    // プロセスノイズ入力行列は単位行列にする
    Eigen::Matrix<double, 4, 4> B0 = Eigen::Matrix<double, 4, 4>::Identity();

    // 観測行列
    Eigen::Matrix<double, 2, 4> C = Eigen::Matrix<double, 2, 4>::Zero();
    C(0, 0) = 1;
    C(1, 1) = 1;

    // 観測のノイズは毎回壁との距離d * (delta)の標準偏差を持つ正規分布に従うことになっている
    // 毎回変わるため、updateの際に渡す

    // これでカルマンフィルタの状態を作る
    KalmanFilter<4, 4, 2> kf(A, B1, B0, C);

    // 基本的な戦略は、目標地点を最も近いところに定めて、見つかったら次のところに行くことを繰り返す
    // 3つの行動を1セットで扱い、(加速) -> x観測 -> y観測 を1セットにする
    // 3ターン目にKalmanFilterの状態を更新することにする

    // プロセスノイズとして使用する共分散行列
    Eigen::Matrix<double, 4, 4> process_noise_cov = Eigen::Matrix<double, 4, 4>::Zero();
    // process_noise_cov(0, 0) = eps * eps * 3;
    //  process_noise_cov(1, 1) = eps * eps * 3;
    process_noise_cov(0, 0) = eps * eps;
    process_noise_cov(1, 1) = eps * eps;
    process_noise_cov(2, 2) = eps * eps;
    process_noise_cov(3, 3) = eps * eps;

    int turn = 0;
    int found = 0;
    vector<bool> found_flag(N, false);

    // 最も良い訪問順序を求める
    // 10!は間に合う
    double best_distance = 1e19;
    vector<int> perm = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
    vector<int> best_perm(N);
    int cnt = 0;
    do
    {
        cnt++;
        double dist = sqrt((double)(ks.x(0) - px[perm[0]]) * (double)(ks.x(0) - px[perm[0]]) + (double)(ks.x(1) - py[perm[0]]) * (double)(ks.x(1) - py[perm[0]]));
        rep(i, N - 1)
        {
            dist += sqrt((double)(px[perm[i]] - px[perm[i + 1]]) * (double)(px[perm[i]] - px[perm[i + 1]]) + (double)(py[perm[i]] - py[perm[i + 1]]) * (double)(py[perm[i]] - py[perm[i + 1]]));
        }
        if (dist < best_distance)
        {
            best_distance = dist;
            best_perm = perm;
        }
    } while (next_permutation(perm.begin(), perm.end()));
    cerr << "best_distance: " << best_distance << endl;
    double check_distance = 0;
    check_distance += sqrt((double)(ks.x(0) - px[best_perm[0]]) * (double)(ks.x(0) - px[best_perm[0]]) + (double)(ks.x(1) - py[best_perm[0]]) * (double)(ks.x(1) - py[best_perm[0]]));
    rep(i, N - 1)
    {
        double dist = sqrt((double)(px[best_perm[i]] - px[best_perm[i + 1]]) * (double)(px[best_perm[i]] - px[best_perm[i + 1]]) + (double)(py[best_perm[i]] - py[best_perm[i + 1]]) * (double)(py[best_perm[i]] - py[best_perm[i + 1]]));
        check_distance += sqrt((double)(px[best_perm[i]] - px[best_perm[i + 1]]) * (double)(px[best_perm[i]] - px[best_perm[i + 1]]) + (double)(py[best_perm[i]] - py[best_perm[i + 1]]) * (double)(py[best_perm[i]] - py[best_perm[i + 1]]));
        cerr << px[best_perm[i]] << " " << py[best_perm[i]] << " " << px[best_perm[i + 1]] << " " << py[best_perm[i + 1]] << " check_distance: " << check_distance << endl;
    }
    cerr << "check_distance: " << check_distance << endl;
    for (auto city : best_perm)
    {
        cerr << city << " ";
    }
    cerr << endl;

    while (turn < 5000 && found < N)
    {
        // 最も近いx, yを探す
        int target_x = -1;
        int target_y = -1;
        double min_dist = 1e9;
        rep(i, N)
        {
            int city = best_perm[i];
            if (found_flag[city])
                continue;
            /*
        double dist = sqrt((ks.x(0) - px[i]) * (ks.x(0) - px[i]) + (ks.x(1) - py[i]) * (ks.x(1) - py[i]));
        if (dist < min_dist)
        {
            min_dist = dist;
            target_x = px[i];
            target_y = py[i];
        }
        */
            target_x = px[city];
            target_y = py[city];
            cerr << "city:" << city << " target_x: " << target_x << " target_y: " << target_y << endl;
            break;
        }

        // 現在地点からtarget_x, target_yに向かう

        while (turn < 5000)
        {
            cout << "#"
                 << " turn:" << turn << " (x,y):" << ks.x(0) << " " << ks.x(1) << " (vx,vy):" << ks.x(2) << " " << ks.x(3) << endl;
            cout << "#"
                 << " sigma (x, y):" << sqrt(ks.P(0, 0)) << " " << sqrt(ks.P(1, 1)) << " sigma (vx, vy):" << sqrt(ks.P(2, 2)) << " " << sqrt(ks.P(3, 3)) << endl;

            int c, h;
            // まず、加速をする
            // 現在の速度が、目的地までのベクトルを(dx, dy)として、(dx/10, dy/10)になるように調整する
            // ただし、早さが1000よりは小さくならないようにする
            double vx = ks.x(2);
            double vy = ks.x(3);
            double dx = target_x - ks.x(0);
            double dy = target_y - ks.x(1);
            double distance = sqrt(dx * dx + dy * dy);

            int accel_x, accel_y;

            // まず、現在のvx, vyについて、(dx, dy)の方向と垂直方向の成分を計算する
            double unit_dx = dx / distance;
            double unit_dy = dy / distance;

            double product = vx * unit_dx + vy * unit_dy;
            double vx_perp = vx - product * unit_dx;
            double vy_perp = vy - product * unit_dy;

            // double vx_perp = vx * unit_dy - vy * unit_dx;
            // double vy_perp = vy * unit_dx - vx * unit_dy;
            double vx_perp_size = sqrt(vx_perp * vx_perp + vy_perp * vy_perp);
            // 垂直方向の成分が0になるように加速をする
            accel_x = -vx_perp;
            accel_y = -vy_perp;
            double accel_size = sqrt(accel_x * accel_x + accel_y * accel_y);
            if (accel_size >= 400)
            {
                accel_x *= 400. / accel_size;
                accel_y *= 400. / accel_size;
            }
            // 残りの分は理想的な方向に近づくように加速をする
            // 残りの分は dx, dyの方向の理想の早さに近づくように加速をする
            int res_speed = sqrt(500 * 500 - accel_x * accel_x - accel_y * accel_y);

            int ideal_speed = max(sqrt(distance * 500. * 2. / 3.) * 0.9, 1200.);
            int current_speed = vx * unit_dx + vy * unit_dy;

            int adjusted_speed = min(res_speed, abs(ideal_speed - current_speed));
            if (ideal_speed > current_speed)
            {
                accel_x += adjusted_speed * dx / distance;
                accel_y += adjusted_speed * dy / distance;
            }
            else
            {
                accel_x -= adjusted_speed * dx / distance;
                accel_y -= adjusted_speed * dy / distance;
            }
            double tot_accel = sqrt(accel_x * accel_x + accel_y * accel_y);
            accel_x *= 498. / tot_accel;
            accel_y *= 498. / tot_accel;

            turn++;
            cout << "A"
                 << " " << accel_x << " " << accel_y << endl;
            cin >> c >> h;

            if (c == 1)
            {
                // この場合には壁に当たったので、速度を0に直してやり直し
                ks.x(2) = 0;
                ks.x(3) = 0;
                ks.P(2, 2) = 0;
                ks.P(3, 3) = 0;
                // ks.P = initial_covariance;
                // continue;
            }

            // 制御入力の用意
            Eigen::Matrix<double, 4, 1> control_input = Eigen::Matrix<double, 4, 1>::Zero();
            control_input(2) = accel_x;
            control_input(3) = accel_y;

            // 観測の用意
            Eigen::Matrix<double, 2, 1> observation = Eigen::Matrix<double, 2, 1>::Zero();
            observation(0) = ks.x(0);
            observation(1) = ks.x(1);

            // 観測ノイズの共分散行列
            Eigen::Matrix<double, 2, 2> observation_noise_cov = Eigen::Matrix<double, 2, 2>::Identity();
            observation_noise_cov(0, 0) = pow(10, 15);
            observation_noise_cov(1, 1) = pow(10, 15);

            ks = kf.step(ks, control_input, Observation<2>(observation), process_noise_cov, observation_noise_cov);

            if (h > 0)
            {
                vec_int q(h);
                rep(i, h) cin >> q[i];
                rep(i, h)
                {
                    found_flag[q[i]] = true;
                    found += 1;
                    /*
                    ks.x(0) = px[q[i]];
                    ks.x(1) = py[q[i]];
                    ks.P(0, 0) = ks.x(2) * ks.x(2);
                    ks.P(1, 1) = ks.x(3) * ks.x(3);
                    */
                }
                break;
            }

            if (delta < 0.1)
            {
                double vx = ks.x(2);
                double vy = ks.x(3);
                double dx = target_x - ks.x(0);
                double dy = target_y - ks.x(1);
                double distance = sqrt(dx * dx + dy * dy);

                int accel_x, accel_y;

                // まず、現在のvx, vyについて、(dx, dy)の方向と垂直方向の成分を計算する
                double unit_dx = dx / distance;
                double unit_dy = dy / distance;

                double product = vx * unit_dx + vy * unit_dy;
                double vx_perp = vx - product * unit_dx;
                double vy_perp = vy - product * unit_dy;

                // double vx_perp = vx * unit_dy - vy * unit_dx;
                // double vy_perp = vy * unit_dx - vx * unit_dy;
                double vx_perp_size = sqrt(vx_perp * vx_perp + vy_perp * vy_perp);
                // 垂直方向の成分が0になるように加速をする
                accel_x = -vx_perp;
                accel_y = -vy_perp;
                double accel_size = sqrt(accel_x * accel_x + accel_y * accel_y);
                if (accel_size >= 420)
                {
                    accel_x *= 420. / accel_size;
                    accel_y *= 420. / accel_size;
                }
                // 残りの分は理想的な方向に近づくように加速をする
                // 残りの分は dx, dyの方向の理想の早さに近づくように加速をする
                int res_speed = sqrt(500 * 500 - accel_x * accel_x - accel_y * accel_y);

                int ideal_speed = max(sqrt(distance * 500. * 2. / 3.) * 0.9, 800.);
                int current_speed = vx * unit_dx + vy * unit_dy;

                int adjusted_speed = min(res_speed, abs(ideal_speed - current_speed));
                if (ideal_speed > current_speed)
                {
                    accel_x += adjusted_speed * dx / distance;
                    accel_y += adjusted_speed * dy / distance;
                }
                else
                {
                    accel_x -= adjusted_speed * dx / distance;
                    accel_y -= adjusted_speed * dy / distance;
                }
                double tot_accel = sqrt(accel_x * accel_x + accel_y * accel_y);
                accel_x *= 498. / tot_accel;
                accel_y *= 498. / tot_accel;

                turn++;
                cout << "A"
                     << " " << accel_x << " " << accel_y << endl;
                int c, h;
                cin >> c >> h;

                if (c == 1)
                {
                    // この場合には壁に当たったので、速度を0に直してやり直し
                    ks.x(2) = 0;
                    ks.x(3) = 0;
                    ks.P(2, 2) = 0;
                    ks.P(3, 3) = 0;
                    // ks.P = initial_covariance;
                    // continue;
                }

                // 制御入力の用意
                Eigen::Matrix<double, 4, 1> control_input = Eigen::Matrix<double, 4, 1>::Zero();
                control_input(2) = accel_x;
                control_input(3) = accel_y;

                // 観測の用意
                Eigen::Matrix<double, 2, 1> observation = Eigen::Matrix<double, 2, 1>::Zero();
                observation(0) = ks.x(0);
                observation(1) = ks.x(1);

                // 観測ノイズの共分散行列
                Eigen::Matrix<double, 2, 2> observation_noise_cov = Eigen::Matrix<double, 2, 2>::Identity();
                observation_noise_cov(0, 0) = pow(10, 15);
                observation_noise_cov(1, 1) = pow(10, 15);

                ks = kf.step(ks, control_input, Observation<2>(observation), process_noise_cov, observation_noise_cov);

                if (h > 0)
                {
                    vec_int q(h);
                    rep(i, h) cin >> q[i];
                    rep(i, h)
                    {
                        found_flag[q[i]] = true;
                        found += 1;
                        /*
                        ks.x(0) = px[q[i]];
                        ks.x(1) = py[q[i]];
                        ks.P(0, 0) = ks.x(2) * ks.x(2);
                        ks.P(1, 1) = ks.x(3) * ks.x(3);
                        */
                    }
                    break;
                }
            }

            // 次にx方向の測定をする
            // 現在地が左右の壁のどちらかに近いかで、その壁との距離を観測する
            double distance_x;
            double predicted_x;
            bool is_left = ks.x(0) < 0;
            turn++;
            if (is_left)
            {
                // 左側の壁に近い場合
                cout << "S -1 0" << endl;
                cin >> distance_x;
                predicted_x = -100000 + distance_x;
            }
            else
            {
                cout << "S 1 0" << endl;
                cin >> distance_x;
                predicted_x = 100000 - distance_x;
            }

            cin >> c >> h;
            if (c == 1)
            {
                // この場合には壁に当たったので、速度を0に直してやり直し
                ks.x(0) += ks.x(2);
                ks.x(1) += ks.x(3);
                ks.x(2) = 0;
                ks.x(3) = 0;

                ks.P(2, 2) = 0;
                ks.P(3, 3) = 0;
                // continue;
            }

            // 制御入力の用意
            control_input = Eigen::Matrix<double, 4, 1>::Zero();

            // 観測の用意
            observation = Eigen::Matrix<double, 2, 1>::Zero();
            observation(0) = predicted_x + ks.x(2);
            observation(1) = ks.x(1);

            // 観測ノイズの共分散行列
            observation_noise_cov = Eigen::Matrix<double, 2, 2>::Identity();
            observation_noise_cov(0, 0) = distance_x * distance_x * delta * delta;
            observation_noise_cov(1, 1) = pow(10, 15);

            ks = kf.step(ks, control_input, Observation<2>(observation), process_noise_cov, observation_noise_cov);

            if (h > 0)
            {
                vec_int q(h);
                rep(i, h) cin >> q[i];
                rep(i, h)
                {
                    found_flag[q[i]] = true;
                    found += 1;

                    /*
                    ks.x(0) = px[q[i]];
                    ks.x(1) = py[q[i]];
                    ks.P(0, 0) = ks.x(2) * ks.x(2);
                    ks.P(1, 1) = ks.x(3) * ks.x(3);
                    */
                }
                break;
            }

            // 次にy方向の測定をする
            // 現在地が上下の壁のどちらかに近いかで、その壁との距離を観測する
            double distance_y;
            double predicted_y;
            bool is_bottom = ks.x(1) < 0;
            turn++;
            if (is_bottom)
            {
                cout << "S 0 -1" << endl;
                cin >> distance_y;
                predicted_y = -100000 + distance_y;
            }
            else
            {
                cout << "S 0 1" << endl;
                cin >> distance_y;
                predicted_y = 100000 - distance_y;
            }

            cin >> c >> h;

            if (c == 1)
            {
                // この場合には壁に当たったので、速度を0に直してやり直し
                ks.x(0) += ks.x(2) * 2;
                ks.x(1) += ks.x(3) * 2;
                ks.x(2) = 0;
                ks.x(3) = 0;

                ks.P(2, 2) = 0;
                ks.P(3, 3) = 0;
                // continue;
            }
            // 制御入力の用意
            control_input = Eigen::Matrix<double, 4, 1>::Zero();

            // 観測の用意
            observation = Eigen::Matrix<double, 2, 1>::Zero();
            observation(0) = ks.x(0);
            observation(1) = predicted_y + ks.x(3);

            // 観測ノイズの共分散行列
            observation_noise_cov = Eigen::Matrix<double, 2, 2>::Identity();
            observation_noise_cov(0, 0) = pow(10, 15);
            observation_noise_cov(1, 1) = distance_y * distance_y * delta * delta;

            ks = kf.step(ks, control_input, Observation<2>(observation), process_noise_cov, observation_noise_cov);

            if (h > 0)
            {
                vec_int q(h);
                rep(i, h) cin >> q[i];
                rep(i, h)
                {
                    found_flag[q[i]] = true;
                    found += 1;

                    /*
                    ks.x(0) = px[q[i]];
                    ks.x(1) = py[q[i]];

                    ks.P(0, 0) = ks.x(2) * ks.x(2);
                    ks.P(1, 1) = ks.x(3) * ks.x(3);
                    */
                }
                break;
            }
        }
    }

    return 0;
}

Submission Info

Compile Error

Main.cpp: In function ‘void remove_val_and_pop_from_last(vec_int&, int)’:
Main.cpp:57:23: warning: comparison of integer expressions of different signedness: ‘int’ and ‘std::vector<int>::size_type’ {aka ‘long unsigned int’} [-Wsign-compare]
   57 |     for (int i = 0; i < A.size(); i++)
      |                     ~~^~~~~~~~~~
Main.cpp: In function ‘int main(int, char**)’:
Main.cpp:507:16: warning: unused variable ‘dist’ [-Wunused-variable]
  507 |         double dist = sqrt((double)(px[best_perm[i]] - px[best_perm[i + 1]]) * (double)(px[best_perm[i]] - px[best_perm[i + 1]]) + (double)(py[best_perm[i]] - py[best_perm[i + 1]]) * (double)(py[best_perm[i]] - py[best_perm[i + 1]]));
      |                ^~~~
Main.cpp:678:24: warning: unused variable ‘vx_perp_size’ [-Wunused-variable]
  678 |                 double vx_perp_size = sqrt(vx_perp * vx_perp + vy_perp * vy_perp);
      |                        ^~~~~~~~~~~~
Main.cpp:575:20: warning: unused variable ‘vx_perp_size’ [-Wunused-variable]
  575 |             double vx_perp_size = sqrt(vx_perp * vx_perp + vy_perp * vy_perp);
      |                    ^~~~~~~~~~~~
Main.cpp:523:16: warning: unused variable ‘min_dist’ [-Wunused-variable]
  523 |         double min_dist = 1e9;
      |                ^~~~~~~~
Main.cpp:420:14: warning: unused parameter ‘argc’ [-Wunused-parameter]
  420 | int main(int argc, char *argv[])
      |          ~~~~^~~~
Main.cpp:420:26: warning: unused parameter ‘argv’ [-Wunused-parameter]
  420 | int main(int argc, char *argv[])
      |                    ~~~~~~^~~~~~

Judge Result