hdu4087ALetter to Programmers(三维旋转矩阵)

  1 #include <iostream>
  2 #include<cstdio>
  3 #include<cstring>
  4 #include<algorithm>
  5 #include<stdlib.h>
  6 #include<vector>
  7 #include<cmath>
  8 #include<queue>
  9 #include<set>
 10 using namespace std;
 11 #define N 1010
 12 #define LL long long
 13 #define INF 0xfffffff
 14 #define forn(i,n) for(int i=0; i<(int)(n); i++)
 15 const double eps = 1e-6;
 16 const double pi = acos(-1.0);
 17 const double inf = ~0u>>2;
 18 
 19 struct point3
 20 {
 21     double x,y,z;
 22 } p[N];
 23 struct Mat
 24 {
 25     double mat[5][5];
 26 };
 27 int n,m;
 28 char str[20];
 29 Mat operator * (Mat a,Mat b)
 30 {
 31     Mat c;
 32     memset(c.mat,0,sizeof(c.mat));
 33     int i,j,k;
 34     for(i = 0 ; i < n ; i++)
 35         for(j = 0 ; j < n ; j++)
 36             for(k =0 ; k < n ; k++)
 37                 c.mat[i][j] +=a.mat[i][k]*b.mat[k][j];
 38     return c;
 39 }
 40 Mat operator ^(Mat a,int k)
 41 {
 42     Mat c;
 43 
 44     int i,j;
 45     for(i =0 ; i < n ; i++)
 46         for(j = 0; j < n ; j++)
 47             c.mat[i][j] = (i==j);
 48     for(; k ; k >>= 1)
 49     {
 50         if(k&1) c = c*a;
 51         a = a*a;
 52     }
 53     return c;
 54 }
 55 Mat solve(int k)
 56 {
 57 
 58     Mat cc;
 59     double a[10];
 60     int i;
 61     memset(cc.mat,0,sizeof(cc.mat));
 62     for(i = 0 ; i < 4 ; i++) cc.mat[i][i] = 1;
 63     while(~scanf("%s",str))
 64     {
 65         if(str[0]==‘e‘)
 66         {
 67             break;
 68         }
 69         Mat c;
 70         memset(c.mat,0,sizeof(c.mat));
 71         for(i = 0 ; i < 4 ; i++) c.mat[i][i] = 1;
 72         if(str[0]==‘t‘)
 73         {
 74             forn(i,3) scanf("%lf", &a[i]);
 75             forn(i,3) c.mat[i][3]=a[i];
 76         }
 77         else if(str[0]==‘s‘)
 78         {
 79             for(i = 0 ; i < 3 ; i++)
 80                 scanf("%lf",&a[i]);
 81             for(i = 0 ; i < 3; i++)
 82                 c.mat[i][i] = a[i];
 83         }
 84         else if(str[0]==‘r‘&&str[1]==‘o‘)
 85         {
 86             for(i =0  ; i < 4; i++) scanf("%lf",&a[i]);
 87             a[3] = a[3]/180*pi;
 88             double mag=sqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);
 89             a[0]/=mag;
 90             a[1]/=mag;
 91             a[2]/=mag;
 92             c.mat[0][0] = (1-cos(a[3]))*a[0]*a[0]+cos(a[3]);
 93             c.mat[0][1] = (1-cos(a[3]))*a[0]*a[1]-sin(a[3])*a[2];
 94             c.mat[0][2] = (1-cos(a[3]))*a[0]*a[2]+sin(a[3])*a[1];
 95             c.mat[1][0] = (1-cos(a[3]))*a[0]*a[1]+sin(a[3])*a[2];
 96             c.mat[1][1] = (1-cos(a[3]))*a[1]*a[1]+cos(a[3]);
 97             c.mat[1][2] = (1-cos(a[3]))*a[1]*a[2]-sin(a[3])*a[0];
 98             c.mat[2][0] = (1-cos(a[3]))*a[0]*a[2]-sin(a[3])*a[1];
 99             c.mat[2][1] = (1-cos(a[3]))*a[1]*a[2]+sin(a[3])*a[0];
100             c.mat[2][2] = (1-cos(a[3]))*a[2]*a[2]+cos(a[3]);
101         }
102         else if(str[0]==‘r‘)
103         {
104             int kk ;
105             scanf("%d",&kk);
106             c = solve(kk);
107         }
108         cc = c*cc;
109     }
110     return cc^k;
111 }
112 int dcmp(double x)
113 {
114     if(fabs(x)<eps) return 0;
115     return x<0?-1:1;
116 }
117 int main()
118 {
119     int i,j;
120     n = 4;
121     while(scanf("%d",&m)&&m)
122     {
123         Mat c;
124         memset(c.mat,0,sizeof(c.mat));
125         for(i = 0; i < n; i++) c.mat[i][i] = 1;
126         c = solve(1)*c;
127         for(i = 1; i <= m; i++)
128         {
129             Mat x,y;
130             memset(y.mat,0,sizeof(y.mat));
131             for(j = 0 ; j < 3 ; j++)
132                 scanf("%lf",&y.mat[j][3]);
133             y.mat[3][3] = 1;
134             x = c*y;
135             p[i].x = x.mat[0][3],p[i].y = x.mat[1][3],p[i].z = x.mat[2][3];
136         }
137         for(i = 1; i <= m; i++)
138         {
139             printf("%.2f %.2f %.2f\n",p[i].x+eps,p[i].y+eps,p[i].z+eps);
140         }
141         puts("");
142     }
143     return 0;
144 }