hdu4087ALetter to Programmers(三维旋转矩阵)

参考

三维旋转矩阵 + 矩阵加速

这个还要用到仿射变换。

平移

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\n",p[i].x+eps,p[i].y+eps,p[i].z+eps);

140         }

141         puts("");

142     }

143     return 0;

144 }

View Code