三维旋转矩阵 + 矩阵加速
这个还要用到仿射变换。
平移
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
类似于一种构造矩阵的方式，每一种操作都相当于乘一次矩阵，具体第三个怎么推出来的没看懂。。。
矩阵乘法不支持交换律，一定要看好顺序。。debug了一天。。
据说会有-0.00这种答案不通过的坑，不过下面的代码加了eps..
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 ",p[i].x+eps,p[i].y+eps,p[i].z+eps); 140 } 141 puts(""); 142 } 143 return 0; 144 }