HDU 4087 ALetter to Programmers (三维坐标旋转 矩阵 + 矩阵快速幂)

题目：传送门

思路：

出处 - > 戳

平移：

1 0 0 tx    x      x+tx
0 1 0 ty * y  =  y+ty
0 0 1 tz    z      z+tz
0 0 0 1    1     1

缩放：

scale kx ky kz

kx 0  0  0

0  ky 0  0

0  0  kz 0

0  0  0  1

 

旋转：

绕任意轴（过原点）旋转（注意要把轴向量归一化，不然会在“点在轴上”这个情况下出问题）

rotate x y z d

(1-cos(d))*x*x+cos(d)     (1-cos(d))*x*y-sin(d)*z   (1-cos(d))*x*z+sin(d)*y   0

(1-cos(d))*y*x+sin(d)*z   (1-cos(d))*y*y+cos(d)     (1-cos(d))*y*z-sin(d)*x   0

(1-cos(d))*z*x-sin(d)*y   (1-cos(d))*z*y+sin(d)*x   (1-cos(d))*z*z+cos(d)     0

                 0                                     0                                      0                   1

 

 

#include <bits/stdc++.h>
#define LL long long
#define ULL unsigned long long
#define mem(i, j) memset(i, j, sizeof(i))
#define rep(i, j, k) for(int i = j; i <= k; i++)
#define dep(i, j, k) for(int i = k; i >= j; i--)
#define pb push_back
#define make make_pair
#define INF INT_MAX
#define inf LLONG_MAX
#define PI acos(-1)
#define fir first
#define sec second
using namespace std;

const int N = 1010;
const double eps = 1e-6;
char s[20];
double a[10];

struct Mat {
    double a[5][5];

    Mat() { mem(a, 0); }

};

struct Point {
    double x, y, z;
} p[N];

Mat Mul(Mat A, Mat B) {
    Mat tmp;
    rep(i, 0, 3) rep(j, 0, 3) rep(k, 0, 3) tmp.a[i][j] += A.a[i][k] * B.a[k][j];
    return tmp;
}

Mat ksm(Mat A, int b) {
    Mat ans;
    rep(i, 0, 3) ans.a[i][i] = 1;
    while(b) {
        if(b & 1) ans = Mul(ans, A);
        A = Mul(A, A);
        b >>= 1;
    }
    return ans;
}

Mat solve(int k) {
    Mat tmp;
    rep(i, 0, 3) tmp.a[i][i] = 1;
    while(scanf("%s", s)) {
        if(s[0] == 'e') break;
        Mat c;
        rep(i, 0, 3) c.a[i][i] = 1;

        if(s[0] == 't') {
            rep(i, 0, 2) scanf("%lf", &c.a[i][3]);
        }
        else if(s[0] == 's') {
            rep(i, 0, 2) scanf("%lf", &c.a[i][i]);
        }
        else if(s[0] == 'r' && s[1] == 'o') {
            rep(i, 0, 3) scanf("%lf", &a[i]);
            a[3] = a[3] / 180 * PI;
            double dis = sqrt(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
            a[0] /= dis; a[1] /= dis; a[2] /= dis;

            c.a[0][0] = (1-cos(a[3]))*a[0]*a[0]+cos(a[3]);
            c.a[0][1] = (1-cos(a[3]))*a[0]*a[1]-sin(a[3])*a[2];
            c.a[0][2] = (1-cos(a[3]))*a[0]*a[2]+sin(a[3])*a[1];
            c.a[1][0] = (1-cos(a[3]))*a[0]*a[1]+sin(a[3])*a[2];
            c.a[1][1] = (1-cos(a[3]))*a[1]*a[1]+cos(a[3]);
            c.a[1][2] = (1-cos(a[3]))*a[1]*a[2]-sin(a[3])*a[0];
            c.a[2][0] = (1-cos(a[3]))*a[0]*a[2]-sin(a[3])*a[1];
            c.a[2][1] = (1-cos(a[3]))*a[1]*a[2]+sin(a[3])*a[0];
            c.a[2][2] = (1-cos(a[3]))*a[2]*a[2]+cos(a[3]);
        }
        else if(s[0] == 'r') {
            int T;
            scanf("%d", &T);
            c = solve(T);
        }
        tmp = Mul(c, tmp);
    }
    return ksm(tmp, k);
}

int main() {

    int n;
    while(scanf("%d", &n)) {
        if(n == 0) return 0;
        Mat tmp;
        rep(i, 0, 3) tmp.a[i][i] = 1;

        tmp = Mul(solve(1), tmp);

        rep(i, 1, n) {
            Mat x, y;
            rep(j, 0, 2) scanf("%lf", &y.a[j][3]);
            y.a[3][3] = 1;
            x = Mul(tmp, y);
            p[i].x = x.a[0][3], p[i].y = x.a[1][3], p[i].z = x.a[2][3];
        }
        rep(i, 1, n) printf("%.2f %.2f %.2f
", p[i].x + eps, p[i].y + eps, p[i].z + eps);
        puts("");
    }

    return 0;
}