sgu265Wizards(计算几何)

题意：

一个空间直角坐标系内，对一个点有三种操作：

1.平移(x,y,z)⇒(x+a,y+b,z+c)；

2.缩放(x,y,z)⇒(ax,by,cz)；

3.绕一个向量(x,y,z)顺时针旋转α角(角度制)。
对空间内的m个点给出n个操作，求每个点在操作后的坐标。

tip：

每次操作在之前的基础上左乘当前操作的矩阵，最后右乘每次给的初始坐标。

#include <cstdio>
#include <iostream>
#include <algorithm>
#include <cstring>
#include <cmath>
using namespace std;
const double PI = acos(-1.0);
struct Tmatrix{
    double mat[4][4];
    void init(){
        for(int i = 0 ;i < 4; i++)for(int j = 0 ; j < 4 ; j++)  mat[i][j] = 0.0;
    }
    Tmatrix mul(Tmatrix b){
        Tmatrix p;
        p.init();
        for(int i =  0 ;i <= 3 ;i++){
            for(int k = 0 ; k <= 3 ; k++){
                for(int j = 0 ; j <= 3 ; j++){
                    p.mat[i][j] += mat[i][k]*b.mat[k][j];
                }
            }
        }
        return p;
    }
}ans;
struct Tpoint{
    double x,y,z;
    friend Tpoint operator + (const Tpoint &a, const Tpoint &b) {return Tpoint(a.x+b.x, a.y+b.y, a.z+b.z);}
    friend Tpoint operator - (const Tpoint &a, const Tpoint &b) {return Tpoint(a.x-b.x, a.y-b.y, a.z-b.z);}
    friend Tpoint operator * (const Tpoint &a, double k) {return Tpoint(a.x*k, a.y*k, a.z*k);}
    friend Tpoint operator / (const Tpoint &a, double k) {return Tpoint(a.x/k, a.y/k, a.z/k);}
    friend double operator * (const Tpoint &a, const Tpoint &b) {return a.x*b.x+a.y*b.y+a.z*b.z;}
    friend Tpoint operator ^ (const Tpoint &a, const Tpoint &b) {return Tpoint(a.y*b.z-a.z*b.y, a.z*b.x-a.x*b.z, a.x*b.y-a.y*b.x);}
    double len() {return sqrt((*this)*(*this));}

    Tpoint (double x = 0,double y = 0,double z = 0):x(x),y(y),z(z){}
    void read(){
        scanf("%lf%lf%lf",&x,&y,&z);
    }
    void print(){
        printf("%.2f %.2f %.2f\n",x,y,z);
    }

};

Tmatrix trans(double addx,double addy,double addz){
        Tmatrix m;
        m.init();
        for(int i = 0 ; i < 4; i++)
            m.mat[i][i]  = 1;

        m.mat[0][3] = addx;m.mat[1][3] = addy;m.mat[2][3] = addz;
        return m;
}
Tmatrix scal(double tx,double ty,double tz){
        Tmatrix m;
        m.init();
        m.mat[0][0] = tx;m.mat[1][1] = ty;m.mat[2][2] = tz;m.mat[3][3] = 1;
        return m;
}
Tmatrix rot(Tpoint p,Tpoint vv,double t){
        Tmatrix m;
        Tpoint tmp = vv/vv.len();
        //cout <<vv.x<<"  "<<vv.y<<"  "<<vv.z<<endl;
        double u = tmp.x;
        double v = tmp.y;
        double w = tmp.z;
        double a = p.x;
        double b = p.y;
        double c = p.z;
        double mak[4][4] = { {u*u+(v*v+w*w)*cos(t),    u*v*(1-cos(t))-w*sin(t), u*w*(1-cos(t))+v*sin(t), (a*(v*v+w*w)-u*(b*v+c*w))*(1-cos(t))+(b*w-c*v)*sin(t)},
                          {u*v*(1-cos(t))+w*sin(t), v*v+(u*u+w*w)*cos(t),    v*w*(1-cos(t))-u*sin(t), (b*(u*u+w*w)-v*(a*u+c*w))*(1-cos(t))+(c*u-a*w)*sin(t)},
                          {u*w*(1-cos(t))-v*sin(t), v*w*(1-cos(t))+u*sin(t), w*w+(u*u+v*v)*cos(t),    (c*(u*u+v*v)-w*(a*u+b*v))*(1-cos(t))+(a*v-b*u)*sin(t)},
                          { 0.0,                    0.0,                     0.0,                      1.0} };
        for(int i = 0 ; i < 4; i++)
            for(int j = 0 ; j < 4; j++)
                m.mat[i][j] = mak[i][j];
        return m;
}
int n;
char s[3];
void init(){
    scanf("%d",&n);
    ans.init();
    for(int i = 0 ; i < 4 ; i++)    ans.mat[i][i] = 1;

    for(int i = 0 ; i < n ; i++){
        scanf("%s",s);
        //printf("%s\n",s);
        Tpoint a;
        a.read();
        //a.print();
        if(s[0] == 'T'){
            ans = trans(a.x,a.y,a.z).mul(ans);
        }
        else if(s[0] == 'S'){
             ans = scal(a.x,a.y,a.z).mul(ans);
        }
        else{
            double ang;
            scanf("%lf",&ang);
            ang = ang*PI/180;
            ans = rot(Tpoint(0,0,0),a,ang).mul(ans);
        }
        //cout <<ans.mat[0][0]<<"  "<<ans.mat[0][1] <<"  "<<ans.mat[0][2]<<"  "<<ans.mat[0][3]<<endl;
        //cout <<ans.mat[1][0]<<"  "<<ans.mat[1][1] <<"  "<<ans.mat[1][2]<<"  "<<ans.mat[1][3]<<endl;
        //cout <<ans.mat[2][0]<<"  "<<ans.mat[2][1] <<"  "<<ans.mat[2][2]<<"  "<<ans.mat[2][3]<<endl;
        //cout <<ans.mat[3][0]<<"  "<<ans.mat[3][1] <<"  "<<ans.mat[3][2]<<"  "<<ans.mat[3][3]<<endl;
    }

}
void sov(){
    scanf("%d",&n);
    for(int i = 0  ; i < n ; i++){
        Tpoint a,b;
        a.read();
        //cout <<a.x<<"  "<<a.y<<"  "<<a.z<<endl;
        b.x = a.x*ans.mat[0][0]+ans.mat[0][1]*a.y+ans.mat[0][2]*a.z+ans.mat[0][3];
        b.y = a.x*ans.mat[1][0]+ans.mat[1][1]*a.y+ans.mat[1][2]*a.z+ans.mat[1][3];
        b.z = a.x*ans.mat[2][0]+ans.mat[2][1]*a.y+ans.mat[2][2]*a.z+ans.mat[2][3];
        b.print();
    }
}
int main(){
    init();
    sov();
}
/*
1
R 0.0 0.0 1.0 2.0
3
2.0 3.0 4.0
*/