PCK 2011 本選 - はじめてのロボット

問題文 : はじめてのロボット

解法 :

問題文にかかれているように回転行列$R$ を使って回転を行う.
下腕については, 上腕と連動して動くので上腕の回転も一緒にかけてやれば良い. これで12 点.

下腕の回転を最後にまとめてやるというミスで4 WA. もっとしっかり考察すべき.

#include <cstdio>
#include <cmath>

using namespace std;

const double EPS = 1e-9;

void rotate(double *bef, double theta, double a, double b, double c)
{
	double l = sqrt(a * a + b * b + c * c);
	double x = a / l * sin(theta),
			y = b / l * sin(theta),
			z = c / l * sin(theta),
			w = cos(theta);
	double R[3][3] = {{1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (z * x + y * w)},
					   {2 * (x * y + z * w), 1 - 2 * (z * z + x * x), 2 * (y * z - x * w)},
					   {2 * (z * x - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)}};
	
	double aft[3];
	for (int i = 0; i < 3; i++)
		aft[i] = R[i][0] * bef[0] + R[i][1] * bef[1] + R[i][2] * bef[2];
	for (int i = 0; i < 3; i++)
		bef[i] = aft[i];
}

int main()
{
	int up, lo, n;
	
	while (scanf("%d %d %d", &up, &lo, &n) && (up != 0 || lo != 0 || n != 0)){
		double hi[3] = {up, 0, 0}, low[3] = {lo, 0, 0};
		
		for (int i = 0; i < n; i++){
			double theta, a, b, c;
			scanf("%lf %lf %lf %lf", &theta, &a, &b, &c);
			rotate(hi, theta * M_PI / 360, a, b, c);
			rotate(low, theta * M_PI / 360, a, b, c);
			
			scanf("%lf %lf %lf %lf", &theta, &a, &b, &c);
			rotate(low, theta * M_PI / 360, a, b, c);
		}
		
		for (int i = 0; i < 3; i++){
			hi[i] += EPS; low[i] += EPS;
		}
		
		printf("%.6lf %.6lf %.6lf %.6lf %.6lf %.6lf\n", hi[0], hi[1], hi[2], hi[0] + low[0], hi[1] + low[1], hi[2] + low[2]);
		
	}
	
	return (0);
}