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();}/*1R 0.0 0.0 1.0 2.032.0 3.0 4.0*/