#224915 (C++11) No.620 ぐるぐるぐるりん

提出ソース

結果

問題	No.620 ぐるぐるぐるりん
ユーザー	はむこ
提出日時	2017-12-19 01:57:41
言語	C++11 (gcc 11.4.0)
結果	WA
実行時間	-
コード長	25,542 bytes
コンパイル時間	2,356 ms
コンパイル使用メモリ	201,116 KB
実行使用メモリ	5,504 KB
最終ジャッジ日時	2024-05-10 01:03:56
合計ジャッジ時間	9,103 ms
ジャッジサーバーID （参考情報）	judge4 / judge1

このコードへのチャレンジ
（要ログイン）

テストケース

テストケース表示

入力	結果	実行時間実行使用メモリ
testcase_00	WA	-
testcase_01	WA	-
testcase_02	WA	-
testcase_03	RE	-
testcase_04	RE	-
testcase_05	RE	-
testcase_06	RE	-
testcase_07	RE	-
testcase_08	WA	-
testcase_09	WA	-
testcase_10	WA	-
testcase_11	WA	-
testcase_12	WA	-
testcase_13	WA	-
testcase_14	WA	-
testcase_15	WA	-
testcase_16	WA	-
testcase_17	WA	-
testcase_18	WA	-
testcase_19	WA	-
testcase_20	WA	-
testcase_21	WA	-
testcase_22	WA	-
testcase_23	WA	-
testcase_24	WA	-
testcase_25	WA	-
testcase_26	RE	-
testcase_27	WA	-
testcase_28	RE	-
testcase_29	WA	-
testcase_30	RE	-

権限があれば一括ダウンロードができます

コンパイルメッセージ

main.cpp: In function ‘void vizGraph(vvll&, int, std::string)’:
main.cpp:42:425: warning: ignoring return value of ‘int system(const char*)’ declared with attribute ‘warn_unused_result’ [-Wunused-result]
   42 | void vizGraph(vvll& g, int mode = 0, string filename = "out.png") { ofstream ofs("./out.dot"); ofs << "digraph graph_name {" << endl; set<P> memo; rep(i, g.size())  rep(j, g[i].size()) { if (mode && (memo.count(P(i, g[i][j])) || memo.count(P(g[i][j], i)))) continue; memo.insert(P(i, g[i][j])); ofs << "    " << i << " -> " << g[i][j] << (mode ? " [arrowhead = none]" : "")<< endl;  } ofs << "}" << endl; ofs.close(); system(((string)"dot -T png out.dot >" + filename).c_str()); }
      |                                                                                                                                                                                                                                                                                                                                                                                                                                   ~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

ソースコード

raw source code

#include <bits/stdc++.h>
#include <sys/time.h>
using namespace std;

#define rep(i,n) for(long long i = 0; i < (long long)(n); i++)
#define repi(i,a,b) for(long long i = (long long)(a); i < (long long)(b); i++)
#define pb push_back
#define all(x) (x).begin(), (x).end()
#define fi first
#define se second
#define mt make_tuple
#define mp make_pair
#define ZERO(a) memset(a,0,sizeof(a))
template<class T1, class T2> bool chmin(T1 &a, T2 b) { return b < a && (a = b, true); }
template<class T1, class T2> bool chmax(T1 &a, T2 b) { return a < b && (a = b, true); }
#define exists find_if
#define forall all_of

using ll = long long; using vll = vector<ll>; using vvll = vector<vll>; using P = pair<ll, ll>;
using vi = vector<int>; using vvi = vector<vi>; vll conv(vi& v) { vll r(v.size()); rep(i, v.size()) r[i] = v[i]; return r; }

inline void input(int &v){ v=0;char c=0;int p=1; while(c<'0' || c>'9'){if(c=='-')p=-1;c=getchar();} while(c>='0' && c<='9'){v=(v<<3)+(v<<1)+c-'0';c=getchar();} v*=p; }
template <typename T, typename U> ostream &operator<<(ostream &o, const pair<T, U> &v) {  o << "(" << v.first << ", " << v.second << ")"; return o; }
template<size_t...> struct seq{}; template<size_t N, size_t... Is> struct gen_seq : gen_seq<N-1, N-1, Is...>{}; template<size_t... Is> struct gen_seq<0, Is...> : seq<Is...>{};
template<class Ch, class Tr, class Tuple, size_t... Is>
void print_tuple(basic_ostream<Ch,Tr>& os, Tuple const& t, seq<Is...>){ using s = int[]; (void)s{0, (void(os << (Is == 0? "" : ", ") << get<Is>(t)), 0)...}; }
template<class Ch, class Tr, class... Args> 
auto operator<<(basic_ostream<Ch, Tr>& os, tuple<Args...> const& t) -> basic_ostream<Ch, Tr>& { os << "("; print_tuple(os, t, gen_seq<sizeof...(Args)>()); return os << ")"; }
ostream &operator<<(ostream &o, const vvll &v) { rep(i, v.size()) { rep(j, v[i].size()) o << v[i][j] << " "; o << endl; } return o; }
template <typename T> ostream &operator<<(ostream &o, const vector<T> &v) { o << '['; rep(i, v.size()) o << v[i] << (i != v.size()-1 ? ", " : ""); o << "]";  return o; }
template <typename T>  ostream &operator<<(ostream &o, const set<T> &m) { o << '['; for (auto it = m.begin(); it != m.end(); it++) o << *it << (next(it) != m.end() ? ", " : ""); o << "]";  return o; }
template <typename T>  ostream &operator<<(ostream &o, const unordered_set<T> &m) { o << '['; for (auto it = m.begin(); it != m.end(); it++) o << *it << (next(it) != m.end() ? ", " : ""); o << "]";  return o; }
template <typename T, typename U>  ostream &operator<<(ostream &o, const map<T, U> &m) { o << '['; for (auto it = m.begin(); it != m.end(); it++) o << *it << (next(it) != m.end() ? ", " : ""); o << "]";  return o; }
template <typename T, typename U, typename V>  ostream &operator<<(ostream &o, const unordered_map<T, U, V> &m) { o << '['; for (auto it = m.begin(); it != m.end(); it++) o << *it; o << "]";  return o; }
vector<int> range(const int x, const int y) { vector<int> v(y - x + 1); iota(v.begin(), v.end(), x); return v; }
template <typename T> istream& operator>>(istream& i, vector<T>& o) { rep(j, o.size()) i >> o[j]; return i;}
string bits_to_string(ll input, ll n=64) { string s; rep(i, n) s += '0' + !!(input & (1ll << i)); reverse(all(s)); return s; }
template <typename T> ostream &operator<<(ostream &o, const priority_queue<T> &v) { auto tmp = v; while (tmp.size()) { auto x = tmp.top(); tmp.pop(); o << x << " ";} o << endl; return o; }

template <typename T> unordered_map<T, ll> counter(vector<T> vec){unordered_map<T, ll> ret; for (auto&& x : vec) ret[x]++; return ret;};
string substr(string s, P x) {return s.substr(x.fi, x.se - x.fi); }
void vizGraph(vvll& g, int mode = 0, string filename = "out.png") { ofstream ofs("./out.dot"); ofs << "digraph graph_name {" << endl; set<P> memo; rep(i, g.size())  rep(j, g[i].size()) { if (mode && (memo.count(P(i, g[i][j])) || memo.count(P(g[i][j], i)))) continue; memo.insert(P(i, g[i][j])); ofs << "    " << i << " -> " << g[i][j] << (mode ? " [arrowhead = none]" : "")<< endl;  } ofs << "}" << endl; ofs.close(); system(((string)"dot -T png out.dot >" + filename).c_str()); }

size_t random_seed; namespace std { using argument_type = P; template<> struct hash<argument_type> { size_t operator()(argument_type const& x) const { size_t seed = random_seed; seed ^= hash<ll>{}(x.fi); seed ^= (hash<ll>{}(x.se) << 1); return seed; } }; }; // hash for various class
namespace myhash{ const int Bsizes[]={3,9,13,17,21,25,29,33,37,41,45,49,53,57,61,65,69,73,77,81}; const int xor_nums[]={0x100007d1,0x5ff049c9,0x14560859,0x07087fef,0x3e277d49,0x4dba1f17,0x709c5988,0x05904258,0x1aa71872,0x238819b3,0x7b002bb7,0x1cf91302,0x0012290a,0x1083576b,0x76473e49,0x3d86295b,0x20536814,0x08634f4d,0x115405e8,0x0e6359f2}; const int hash_key=xor_nums[rand()%20]; const int mod_key=xor_nums[rand()%20]; template <typename T> struct myhash{ std::size_t operator()(const T& val) const { return (hash<T>{}(val)%mod_key)^hash_key; } }; };
template <typename T> class uset:public std::unordered_set<T,myhash::myhash<T>> { using SET=std::unordered_set<T,myhash::myhash<T>>; public: uset():SET(){SET::rehash(myhash::Bsizes[rand()%20]);} };
uint32_t randxor() { static uint32_t x=1+(uint32_t)random_seed,y=362436069,z=521288629,w=88675123; uint32_t t; t=(x^(x<<11));x=y;y=z;z=w; return( w=(w^(w>>19))^(t^(t>>8)) ); }
struct timeval start; long double sec() { struct timeval tv; gettimeofday(&tv, NULL); return (tv.tv_sec - start.tv_sec) + (tv.tv_usec - start.tv_usec) * 1e-6; }
struct init_{init_(){ gettimeofday(&start, NULL); ios::sync_with_stdio(false); cin.tie(0); struct timeval myTime; struct tm *time_st; gettimeofday(&myTime, NULL); time_st = localtime(&myTime.tv_sec); srand(myTime.tv_usec); random_seed = RAND_MAX / 2 + rand() / 2; }} init__;
#define ldout fixed << setprecision(40) 

// Double.cpp
// 多倍長小数クラス
//============================================================================//
// 更新：02/12/26(木)
// 概要：なし。
// 補足：なし。
//============================================================================//

#define MDouble long double
class Double
{
	MDouble	dblMantissa ;	// 仮数
	int	intExponent ;	// 指数

public:
// コンストラクタおよびデストラクタ
	Double( MDouble = 0) ;
	Double( const Double&) ;
	~Double() ;

// 設定
	void SetDouble( MDouble) ;
private:
	void Validate( MDouble) ;


// 演算
	friend ostream &operator<<(ostream &, const Double &);
	friend istream &operator>>(istream &, Double &);
	friend const Double operator+(const Double &, const Double &) ;
	friend const Double operator-(const Double &, const Double &) ;
	friend const Double operator*(const Double &, const Double &) ;
	friend const Double operator/(const Double &, const Double &) ;
	friend bool operator==(const Double &, const Double &);
	friend bool operator==(const Double &, MDouble);
	friend bool operator==(MDouble, const Double &);
	friend bool operator!=(const Double &, const Double &);
	friend bool operator!=(const Double &, MDouble);
	friend bool operator!=(MDouble, const Double &);

public:
	Double& operator=( const Double&) ;
	Double& operator+=(const Double&);
	Double& operator-=(const Double&);
	Double& operator*=(const Double&);
	Double& operator/=(const Double&);
	operator MDouble() const ;
} ;



/******************************************************************************/
//		コンストラクタおよびデストラクタ
/******************************************************************************/
// コンストラクタ
//============================================================================//
// 更新：02/12/25(水)
// 概要：なし。
// 補足：なし。
//============================================================================//

Double::Double( MDouble d) 
{
	SetDouble( d) ;
}


/******************************************************************************/
// コピーコンストラクタ
//============================================================================//
// 更新：02/12/26(木)
// 概要：なし。
// 補足：なし。
//============================================================================//

Double::Double( const Double& d)
: dblMantissa( d.dblMantissa), intExponent( d.intExponent)
{
}


/******************************************************************************/
// デストラクタ
//============================================================================//
// 更新：02/12/25(水)
// 概要：なし。
// 補足：なし。
//============================================================================//

Double::~Double() 
{
}



/******************************************************************************/
//		設定
/******************************************************************************/
// MDouble から設定
//============================================================================//
// 更新：02/12/25(水)
// 概要：なし。
// 補足：なし。
//============================================================================//

void Double::SetDouble( MDouble d)
{
	intExponent = 0 ;
	Validate( d) ;
}


/******************************************************************************/
// 仮数部を正当な値にする
//============================================================================//
// 更新：02/12/26(木)
// 概要：1.0 <= 仮数部 < 10.0 を満たすようにする。
// 補足：なし。
//============================================================================//

void Double::Validate( MDouble d)
{
	// d が 0 の時は特別扱い
	if( d == 0)
	{
		dblMantissa = 0 ;
		intExponent = 0 ;
		return ;
	}

	MDouble dblAbs = ( d > 0 ? d : -d) ;
	if( dblAbs >= 10.0)
	{
		while( dblAbs > 10.0)
		{
			dblAbs /= 10.0 ;
			intExponent++ ;
		}
	}
	else if( dblAbs < 1.0)
	{
		while( dblAbs < 1.0)
		{
			dblAbs *= 10.0 ;
			intExponent-- ;
		}
	}

	dblMantissa = ( d > 0 ? dblAbs : -dblAbs) ;
}


/******************************************************************************/
//		演算子
/******************************************************************************/
// 出力
//============================================================================//
// 更新：02/12/25(水)
// 概要：なし。
// 補足：なし。
//============================================================================//

ostream &operator<<(ostream& stream, const Double& d)
{
	ios_base::fmtflags iosOldSetting = cout.flags() ;
	cout.setf( ios::showpoint) ;
	cout.setf(ios::fixed) ;
	cout.precision(40) ;
	cout.width( 8) ;
	stream << d.dblMantissa << "e" << d.intExponent ;
	cout.flags( iosOldSetting) ;
	return stream ;
}


/******************************************************************************/
// 入力
//============================================================================//
// 更新：02/12/25(水)
// 概要：なし。
// 補足：とりあえずは一端 MDouble で取り込む。
//============================================================================//

istream &operator>>(istream& stream, Double& dbl)
{
	MDouble d ;
	stream >> d ;
	dbl.SetDouble( d) ;
	return stream ;
}


/******************************************************************************/
// 加算
//============================================================================//
// 更新：02/12/26(木)
// 概要：なし。
// 補足：なし。
//============================================================================//

const Double operator+(const Double& left, const Double& right)
{
	Double d = left ;
	d += right ;

	return d ;
}


/******************************************************************************/
// 減算
//============================================================================//
// 更新：02/12/26(木)
// 概要：なし。
// 補足：なし。
//============================================================================//

const Double operator-(const Double& left, const Double& right)
{
	Double d = left ;
	d -= right ;

	return d ;
}


/******************************************************************************/
// 乗算
//============================================================================//
// 更新：02/12/25(水)
// 概要：なし。
// 補足：*= を使って実装。
//============================================================================//

const Double operator*(const Double& left, const Double& right)
{
	Double dblRet = left ;
	dblRet *= right ;
	return dblRet ;
}


/******************************************************************************/
// 除算
//============================================================================//
// 更新：02/12/25(水)
// 概要：なし。
// 補足：*= を使って実装。
//============================================================================//

const Double operator/(const Double& left, const Double& right)
{
	Double dblRet = left ;
	dblRet /= right ;
	return dblRet ;
}


/******************************************************************************/
// 等しいか
//============================================================================//
// 更新：02/12/26(木)
// 概要：なし。
// 補足：なし。
//============================================================================//

bool operator==( const Double& left, const Double& right)
{
	if( left.dblMantissa == 0)
	{
		return right.dblMantissa == 0 ;
	}
	else
	{
		return left.intExponent == right.intExponent && left.dblMantissa == right.dblMantissa ;
	}
}

bool operator==( const Double& left, MDouble right)
{
	return left == Double( right) ;
}

bool operator==( MDouble& left, const Double& right)
{
	return Double( left) == right ;
}


/******************************************************************************/
// 等しくないか
//============================================================================//
// 更新：02/12/26(木)
// 概要：なし。
// 補足：なし。
//============================================================================//

bool operator!=( const Double& left, const Double& right)
{
	return !( left == right) ;
}

bool operator!=( const Double& left, MDouble right)
{
	return left != Double( right) ;
}

bool operator!=( MDouble left, const Double& right)
{
	return Double( left) != right ;
}


/******************************************************************************/
//		メンバ関数の演算子オーバーロード
/******************************************************************************/
// 代入演算子
//============================================================================//
// 更新：02/12/26(木)
// 概要：なし。
// 補足：なし。
//============================================================================//

Double& Double::operator=( const Double& d)
{
	dblMantissa = d.dblMantissa ;
	intExponent = d.intExponent ;

	return *this ;
}

/******************************************************************************/
// 加算代入
//============================================================================//
// 更新：02/12/26(木)
// 概要：なし。
// 補足：加算時は大きい方にあわせるべし。
//============================================================================//

Double& Double::operator+=( const Double& right)
{
	// どちらかが 0 の時は特別
	if( dblMantissa == 0)
	{
		intExponent = right.intExponent ;
		dblMantissa = right.dblMantissa ;
		return *this ;
	}
	else if( right.dblMantissa == 0)
	{
		return *this ;
	}

	// 左辺が大きいとき
	if( intExponent >= right.intExponent)
	{
		MDouble dblRightMantissa = right.dblMantissa ;
		for( int i = 0; i < intExponent - right.intExponent; i++)
		{
			dblRightMantissa /= 10.0 ;
		}

		dblMantissa += dblRightMantissa ;
	}
	// 右辺が大きいとき
	else
	{
		for( int i = 0; i < right.intExponent - intExponent; i++)
		{
			dblMantissa /= 10.0 ;
		}
		intExponent = right.intExponent ;
		dblMantissa += right.dblMantissa ;
	}

	Validate( dblMantissa) ;

	return *this ;
}


/******************************************************************************/
// 減算代入
//============================================================================//
// 更新：02/12/26(木)
// 概要：なし。
// 補足：なし。
//============================================================================//

Double& Double::operator-=(const Double& d)
{
	dblMantissa *= -1.0 ;
	*this += d ;
	dblMantissa *= -1.0 ;

	return *this ;
}


/******************************************************************************/
// 乗算代入
//============================================================================//
// 更新：02/12/26(木)
// 概要：なし。
// 補足：なし。
//============================================================================//

Double& Double::operator*=( const Double& right)
{
	intExponent += right.intExponent ;
	Validate( dblMantissa * right.dblMantissa) ;

	return *this ;
}


/******************************************************************************/
// 除算代入
//============================================================================//
// 更新：02/12/26(木)
// 概要：なし。
// 補足：ごめんやる気ない（藁。
//============================================================================//

Double& Double::operator/=( const Double& right)
{
	if( right.dblMantissa == 0)
	{
		cout << "Double : 0 divide exception.\n" ;
		abort() ;
	}

	intExponent -= right.intExponent ;
	Validate( dblMantissa / right.dblMantissa) ;

	return *this ;
}


/******************************************************************************/
// キャスト演算子
//============================================================================//
// 更新：02/12/26(木)
// 概要：なし。
// 補足：なし。
//============================================================================//

Double::operator MDouble() const
{
	MDouble d = dblMantissa ;
	int i = intExponent ;

	while( i != 0)
	{
		if( i > 0)
		{
			d *= 10.0 ;
			i-- ;
		}
		else
		{
			d /= 10.0 ;
			i++ ;
		}
	}

	return d ;
}



//typedef Double number;
//typedef double number;
typedef long double number;
const number eps = 1e-8;
using vec = vector<number>;
using mat = vector<vec>;

ostream &operator<<(ostream &o, const vec &v) { 
    for (int i = 0; i < v.size(); i++) { cout << v[i] << " "; } cout << endl;
    return o; 
}
ostream &operator<<(ostream &o, const mat &v) { 
    for (int i = 0; i < v.size(); i++) { cout << v[i]; } 
    return o; 
}

// O( n^2 )
vec zero(int n) {
    return vec(n);
}
// O( n m )
mat zero(int n, int m) {
    mat A(n, vec(m, 0));
    return A;
}

mat operator*(const mat& t1, const mat& t2) {
    assert(t1[0].size() == t2.size());
    mat ret = zero(t1.size(), t2[0].size()); 
    rep(i, t1.size()) {
        rep(j, t2[0].size()) {
            rep(k, t1[0].size()) {
                ret[i][j] += t1[i][k] * t2[k][j];
            }
        }
    }
    return ret;
}
vec operator*(const mat& t1, const vec& t2) {
    assert(t1[0].size() == t2.size());
    vec ret(t1.size());
    rep(i, t1.size()) {
        rep(j, t2.size()) {
            ret[i] += t1[i][j] * t2[j];
        }
    }
    return ret;
}
vec operator+(const vec& t1, const vec& t2) {
    assert(t1.size() == t2.size());
    vec ret(t1.size());
    rep(i, t1.size()) {
        ret[i] = t1[i] + t2[i];
    }
    return ret;
}
mat operator+(const mat& t1, const mat& t2) {
    assert(t1.size() == t2.size() && t1[0].size() == t2[0].size());
    mat ret = zero(t1.size(), t1[0].size()); 
    rep(i, t1.size()) {
        rep(j, t1[0].size()) {
            ret[i][j] = t1[i][j] + t2[i][j];
        }
    }
    return ret;
}
vec operator-(const vec& t1, const vec& t2) {
    assert(t1.size() == t2.size());
    vec ret(t1.size());
    rep(i, t1.size()) {
        ret[i] = t1[i] - t2[i];
    }
    return ret;
}
mat operator-(const mat& t1, const mat& t2) {
    assert(t1.size() == t2.size() && t1[0].size() == t2[0].size());
    mat ret = zero(t1.size(), t1[0].size()); 
    rep(i, t1.size()) {
        rep(j, t1[0].size()) {
            ret[i][j] = t1[i][j] - t2[i][j];
        }
    }
    return ret;
}
mat operator-(const mat& t1) {
    mat ret = t1;
    rep(i, ret.size()) {
        rep(j, ret[0].size()) {
            ret[i][j] *= -1;
        }
    }
    return ret;
}

mat inverse(const mat& t) {
    assert(t.size() == t[0].size());
    assert(t.size() == 2); // TODO
    mat ret = zero(2, 2);
    ret[0][0] = t[1][1];
    ret[0][1] = -t[1][0];
    ret[1][0] = -t[0][1];
    ret[1][1] = t[0][0];
    rep(i, 2) rep(j, 2) {
        ret[i][j] /= (t[0][0] * t[1][1] - t[0][1] * t[1][0]);
    }

    return ret;
}
mat transpose(const mat& t) {
    mat ret = zero(t[0].size(), t.size());
    rep(i, t.size()) rep(j, t[0].size()) {
        ret[j][i] = t[i][j];
    }
    return ret;
}
mat block(const mat& t, ll si, ll sj, ll i0, ll j0) {
    assert(i0+si<=t.size() && j0+sj <= t[0].size());
    mat ret = zero(si, sj);
    rep(i, si) rep(j, sj) {
        ret[i][j] = t[i0+i][j0+j];
    }
    return ret;
}


number omega = 0.5;
number vv = -0.5;
vec f(vec x, vec u) {
    mat F_x = zero(2, 2);
    F_x[0][0] = vv+(number)1, F_x[0][1] = -omega;
    F_x[1][0] = omega, F_x[1][1] = vv+(number)1;
    /*
    cerr << "x" << endl;
    cerr << x << endl;
    cerr << "u"<<endl;
    cerr << u << endl;
    cerr << "F_x"<<endl;
    cerr << F_x << endl;
    cerr << "omega vv" << endl;
    cerr << omega << " " << vv << endl;
    cerr << "Next" << endl;
    cerr << F_x * x + u  << endl;
    */
    return F_x * x + u;
}

// Sx(S+U)
mat getF(void) {
    mat F = zero(2, 4);
    F[0][0] = vv+(number)1; F[0][1] = -omega; F[0][2] = (number)1; F[0][3] = 0;
    F[1][0] = omega; F[1][1] = vv+(number)1; F[1][2] = 0; F[1][3] = 1;
    return F;
}
// Sx1
vec getf(void) {
    return zero(2);
}

pair<number, number> evaluate(number g_x, number g_y, number output_coeff, ll T) {
    // X=U=2
    vector<mat> C(T+1, zero(4, 4));
    vector<vec> c(T+1, zero(4));
    C[T][0][0] = 1, C[T][1][1] = 1;
    c[T][0] = -g_x, c[T][1] = -g_y;
    rep(t, T+1) C[t][2][2] = output_coeff, C[t][3][3] = output_coeff;

    vector<mat> Q(T+1, zero(4, 4)); // (S+U)x(S+U)
    vector<vec> q(T+1, zero(4)); // (S+U)x1
    vector<mat> V(T+2, zero(2, 2)); // SxS
    vector<vec> v(T+2, zero(2)); // Sx1
    vector<mat> K(T+2, zero(2, 2)); // UxS
    vector<vec> k(T+2, zero(2)); // Ux1
//    cout <<"HOGE"<<endl;

    for (int t = T; t >= 0; t--) {
//    cout <<t<<" "<<"HOGE"<<endl;
        mat F = getF(); // Sx(S+U)
        vec f = getf(); // Sx1
        Q[t] = C[t] + transpose(F) * V[t+1] * F;
        q[t] = c[t] + transpose(F) * V[t+1] * f + transpose(F) * v[t+1];
        mat Qxx = block(Q[t], 2, 2, 0, 0);
        mat Quu = block(Q[t], 2, 2, 2, 2);
        mat Qxu = block(Q[t], 2, 2, 0, 2);
        mat Qux = block(Q[t], 2, 2, 2, 0);
        vec qx = zero(2); qx[0] = q[t][0], qx[1] = q[t][1];
        vec qu = zero(2); qu[0] = q[t][2], qu[1] = q[t][3];
        K[t] = -inverse(Quu) * Qux;
        k[t] = -inverse(Quu) * qu;
        V[t] = Qxx + Qxu * K[t] + transpose(K[t]) * Qux + transpose(K[t]) * Quu * K[t];
        v[t] = qx + Qxu * k[t] + transpose(K[t]) * qu + transpose(K[t]) * Quu * k[t];
    }
    vector<vec> x(T+2, zero(2));
    vector<number> x_brutal(T+2);
    vector<number> y_brutal(T+2);
    vector<vec> u(T+2, zero(2));
    x[0][0] = 1, x[0][1] = 0;
    x_brutal[0] = 1;
    rep(t, T) {
        vec u_tmp = K[t] * x[t] + k[t];
        u[t] = u_tmp;
        x[t+1] = f(x[t], u[t]);

        number ux = u[t][0];
        number uy = u[t][1];
        x_brutal[t+1] = x_brutal[t] - y_brutal[t] * omega + x_brutal[t] * vv + ux;
        y_brutal[t+1] = y_brutal[t] + x_brutal[t] * omega + y_brutal[t] * vv + uy;
    }
    
    /*
    rep(i, T+1) cout << " " << x[i][0] << " " << x[i][1] << endl;
    */
    rep(i, T) cerr << ldout << u[i][0] << " " << u[i][1] << endl;
//    rep(i, T+1) cerr << " " << x[i][0] << " (" << x_brutal[i]<< ")" << x[i][1] << " (" <<y_brutal[i]<<")"<< " " << u[i][0] << " " << u[i][1] << endl;
//    rep(i, T+1) cerr << " " << x[i][0] << " " << x_brutal[i]<< " " << x[i][1] << " " <<y_brutal[i]<<" "<< " " << u[i][0] << " " << u[i][1] << endl;

    number usum = 0;
    rep(i, T) usum += u[i][0] * u[i][0] + u[i][1] * u[i][1];
    return mp(usum, sqrt(pow(x_brutal[T] - g_x, 2) + pow(y_brutal[T] - g_y, 2)));
}

ll randr(ll i, ll j) { if (i > j) return i; else return (ll)rand() % (j - i + 1) + i; }
number randrf(number i, number j) { 
    number ret = ((number)rand() / (number)RAND_MAX) * ((number)j - (number)i) + (number)i; 
    return ret;
}
int main(int argc, char** argv) {
    cout << ldout;
    number g_x, g_y, output_coeff = 1e-9;
    number k;
    ll T;
#if 0
    T = randr(1, 15);
    k = 1e9;
//    k = randrf(0, 1e9);
    omega = randrf(-10, 10);
    vv = randrf(-10, 10);
    g_x = randrf(-100, 100);
    g_y = randrf(-100, 100);
    auto err = evaluate(g_x, g_y, output_coeff, T);
    assert(err.se <= 1e-4);
    assert(err.fi <= k);
    cout << ldout << T << " " << err.fi * 1.01 << endl;
    cout << omega << " " << vv << endl;
    cout << g_x << " " << g_y << endl;
#else
    cin >> T >> k >> omega >> vv >> g_x>> g_y;
    auto err = evaluate(g_x, g_y, output_coeff, T);
//    cout << err << endl;
//    cout << err.fi << " " << err.se << " " << T << " " << k << " " << vv << " " << g_x << " " << g_y << endl;
    assert(err.se <= 1e-4);
#endif

    return 0;
}

yukicoder

結果

テストケース

コンパイルメッセージ

ソースコード