yukicoder : No.760 Where am I moved to?

そこまで難しい知識は要らなかったので助かった。
https://yukicoder.me/problems/no/760

問題

今ある２次元の世界座標系で船の位置P0、および向きθと、２つの杭の位置P1,P2が与えられる。

ここで、船の位置や向きが変化してしまったとする。
もし船の位置・向きが変化していないと仮定した場合、現在の杭の位置Q1、Q2が与えられる。
これらの情報から、元の世界座標系における現在の船の位置・向きを求めよ。

解法

まず、移動後の座標における元の船の位置Q0を求めよう。
これはP0,P1,P2とQ0,Q1,Q2は合同なことから、ベクトルP1→P2がQ1→Q2となる回転角よりベクトルP1→P0を同様に回転させてQ0を求めることができる。

Q0→P0と船が移動してしまったことになるので、Q0がP0と一致するよう座標全体を平行移動させつつ、先ほど求めた回転角を逆回転させるようP0を動かすと、世界座標における現在の船の位置が求まる。

double X[5],Y[5];
double T;

void solve() {
	int i,j,k,l,r,x,y; string s;
	
	cin>>X[0]>>Y[0]>>T;
	FOR(i,4) cin>>X[i+1]>>Y[i+1];
	
	double dif=atan2(Y[4]-Y[3],X[4]-X[3])-atan2(Y[2]-Y[1],X[2]-X[1]);
	
	double xx=X[3]+cos(dif)*(X[0]-X[1])-sin(dif)*(Y[0]-Y[1]);
	double yy=Y[3]+sin(dif)*(X[0]-X[1])+cos(dif)*(Y[0]-Y[1]);
	double x2=X[0]+cos(-dif)*(X[0]-xx)-sin(-dif)*(Y[0]-yy);
	double y2=Y[0]+sin(-dif)*(X[0]-xx)+cos(-dif)*(Y[0]-yy);
	
	double a=fmod(360+T-dif*180/(atan(1)*4),360);
	_P("%.12lf %.12lf %.12lf\n",x2,y2,a);
}

まとめ

こういう問題、うまく行列で書けずついベクトルや三角関数でゴリ押ししちゃうんだよな。