[hdu][4087][ALetter to Programmers]

题目：http://acm.hdu.edu.cn/showproblem.php?pid=4087

三维旋转矩阵 + 矩阵加速

这个还要用到仿射变换。

平移

translate tx ty tz

1 0 0 tx

0 1 0 ty

0 0 1 tz

0 0 0 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

 代码：

View Code

#include <cstdio>

#include <cstring>

#include <cmath>

#include <algorithm>



#define forn(i,n) for(int i=0; i<(int)(n); i++)

using namespace std;



const int N = 4;

const double eps = 1e-6;

const double pi = acos(-1.0);



struct matrix {

    int n;

    double a[N][N];

    void clear() {forn(i,n)forn(j,n)a[i][j]=0;}

    matrix(){}

    matrix(int z):n(z){clear();}

    matrix operator * (const matrix& u){

        matrix ans(n);

        forn(i,n) forn(k,n) forn(j,n)

            ans.a[i][j] += a[i][k] * u.a[k][j];

        return ans;

    }

    matrix pow(int k){

        matrix r(n), t=*this;

        forn(i,n) r.a[i][i]=1.0;

        while (k){

            if (k&1) r = t*r;

            t = t*t;

            k >>= 1;

        } return r;

    }

};

char op[20];



matrix dfs(int tim){

    matrix res(4);

    forn(i,4)res.a[i][i]=1;

    double r[5];

    while (~scanf("%s", op)){

        if (op[0] == 'e') break;        // end

        matrix tmp(4);

        forn(i,4)tmp.a[i][i]=1;

        if (op[0] == 't'){                    // translate

            forn(i,3) scanf("%lf", &r[i]);

            forn(i,3) tmp.a[i][3]=r[i];

        }

        else if (op[0] == 's'){              // scale

            forn(i,3) scanf("%lf", &r[i]);

            forn(i,3) tmp.a[i][i]=r[i];

        }

        else if (op[0] == 'r' && op[1] == 'o'){     //rotate

            double t=0.0, c, s;

            forn(i,4) scanf("%lf", &r[i]);

            forn(i,3) t += r[i]*r[i];

            t = sqrt(t);

            forn(i,3) r[i] /= t;

            r[3] = r[3]/180*pi;

            c = cos(r[3]), s = sin(r[3]);

            forn(i,3){

                forn(j,3){

                    bool f = (i+1)%3==j;

                    if (i == j) tmp.a[i][j]=(1-c)*r[i]*r[j]+c;

                    else tmp.a[i][j]=(1-c)*r[i]*r[j]+s*(f?-r[3^i^j]:r[3^i^j]);

                }

            }

        }

        else {              // repeat

            int tim2;

            scanf("%d", &tim2);

            tmp = dfs(tim2);

        }

        res = tmp*res;

    }

    return res.pow(tim);

}

double p[5], q[5];

int main()

{

    //freopen("D:/a.txt", "r", stdin);

    int n;

    while (~scanf("%d", &n) && n){

        matrix tmp(4);

        forn(i,4)tmp.a[i][i]=1;

        tmp = dfs(1)*tmp;

        forn(i,n){

            forn(j,3) scanf("%lf",&p[j]);

            p[3] = 1.0;

            forn(j,3){

                q[j] = 0.0;

                forn(k,4) q[j]+=tmp.a[j][k]*p[k];

            }

            forn(j,3) printf("%.2f%c",q[j]+eps,j==2?'\n':' ');

        }

        puts("");

    }

    return 0;

}