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); }