HDU 4087 三维上的平移缩放旋转矩阵变化

 题目大意：

就是根据它给的程序的要求，不断平移，缩放，旋转三维的点，最后计算出点的位置

 

这里主要是要列出三种转换方式的齐次矩阵描述

平移
translate tx ty tz
1 0 0 0
0 1 0 0
0 0 1 0
tx ty tz 1
缩放
scale a b c
a  0  0  0
0  b  0  0
0  0  c  0
0  0  0  1
绕任意轴(过原点)旋转(注意要把轴向量归一化，否则点在旋转轴上时有问题)

这里是以(x,y,z)向量指向我们人的方向逆时针旋转 d 的弧度
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

然后这里因为循环的问题，所以用矩阵快速幂加速

而循环是可能出现嵌套的

我没用递归，而是判断当前循环属于第几个矩阵下的计算，每次进入一个新的循环都会给当前新的循环设置一个编号，以便找到它的矩阵

每次退出循环，都会将当前矩阵和循环外那个矩阵相乘即可

最后输出+eps，防止 -0.0 , -0.0 , -0.0 的情况发生

#include <cstdio>
#include <cstring>
#include <cmath>
#include <algorithm>
#include <iostream>
using namespace std;
#define N 1005
#define eps 1e-6
const double PI = acos(-1.0);
char s[20];
int rec[N];

struct Matrix{
    double m[4][4];
    Matrix operator*(const Matrix &t) const {
        Matrix ans;
        for(int i=0 ; i<4 ; i++){
            for(int j=0 ; j<4 ; j++){
                ans.m[i][j] = 0;
                for(int k=0 ; k<4 ; k++){
                    ans.m[i][j]+=m[i][k]*t.m[k][j];
                }
            }
        }
        return ans;
    }
    Matrix(){}
    Matrix(double p[][4])
    {
        for(int i=0 ; i<4 ; i++)
            for(int j=0 ; j<4 ; j++)
                m[i][j] = p[i][j];
    }
    void init(){
        memset( m , 0 , sizeof(m));
        for(int i=0 ; i<4 ; i++) m[i][i] = 1;
    }
    void out(){
        for(int i=0 ; i<4 ; i++){
            for(int j=0 ; j<4 ; j++)
                cout<<m[i][j]<<" ";
            cout<<endl;
        }
    }
}mat[1005] , tmp;

Matrix q_pow(Matrix a , int k)
{
    Matrix ret;
    ret.init();
    while(k)
    {
        if(k&1) ret = ret*a;
        a = a*a;
        k>>=1;
    }
    return ret;
}
//当向量方向指向自己的时候逆时针旋转A的弧度 
void Rotate(Matrix &p , double a , double b , double c , double A)
{
    double len = sqrt(a*a+b*b+c*c);
    double x = a/len , y = b/len , z = c/len;
    double sine = sin(A) , cosine = cos(A);
    double m[][4] = {
        {cosine+(1-cosine)*x*x, x*y*(1-cosine)-z*sine, x*z*(1-cosine)+y*sine, 0},
        {y*x*(1-cosine)+z*sine, cosine+y*y*(1-cosine), y*z*(1-cosine)-x*sine, 0},
        {z*x*(1-cosine)-y*sine, z*y*(1-cosine)+x*sine, cosine+z*z*(1-cosine), 0},
        {0                    , 0                    , 0                    , 1}
    };
    p = Matrix(m);
}

void Trans(Matrix &p , double a , double b , double c)
{
    p.init();
    p.m[3][0]=a , p.m[3][1]=b , p.m[3][2]=c;
}

void Scale(Matrix &p , double a , double b , double c)
{
    p.init();
    p.m[0][0] = a , p.m[1][1] = b , p.m[2][2] = c;
}

int main()
{
   // freopen("in.txt" , "r" , stdin);
    int n;
    double a,b,c,d;

    while(scanf("%d" , &n) , n){
        mat[0].init();
        int cnt = 0;//repeat次数
        while(scanf("%s" , s)){
            if(s[0]=='e' && cnt==0) break;
            if(s[0]=='e'){
              //  cout<<"id: "<<cnt<<": "<<endl;
              //  mat[cnt].out();
                mat[cnt] = q_pow(mat[cnt] , rec[cnt]);
              //  cout<<"id: "<<cnt<<": "<<endl;
              //  mat[cnt].out();
                mat[cnt-1] = mat[cnt-1]*mat[cnt];
             //   cout<<"id: "<<cnt<<": "<<endl;
              //  mat[cnt-1].out();
                cnt--;
            }
            if(s[0]=='r' && s[1]=='e') {
                scanf("%d" , &rec[++cnt]);
                mat[cnt].init();
            }
            else if(s[0]=='t'){
                scanf("%lf%lf%lf" , &a , &b , &c);
                Trans(tmp , a , b , c);
             //   tmp.out();
                mat[cnt] = mat[cnt]*tmp;
            }
            else if(s[0]=='s'){
                scanf("%lf%lf%lf" , &a , &b , &c);
                Scale(tmp , a , b , c);
                mat[cnt] = mat[cnt]*tmp;
            }
            else if(s[0]=='r' && s[1]=='o'){
                scanf("%lf%lf%lf%lf" , &a , &b , &c , &d);
                Rotate(tmp , a , b , c , -d/180*PI);
                mat[cnt] = mat[cnt]*tmp;
            }
        }
      //  mat[0].out();
        for(int i=0 ; i<n ; i++){
            double x , y , z , xx , yy , zz;
            scanf("%lf%lf%lf" , &x , &y , &z);
            xx = x*mat[0].m[0][0]+y*mat[0].m[1][0]+z*mat[0].m[2][0]+mat[0].m[3][0];
            yy = x*mat[0].m[0][1]+y*mat[0].m[1][1]+z*mat[0].m[2][1]+mat[0].m[3][1];
            zz = x*mat[0].m[0][2]+y*mat[0].m[1][2]+z*mat[0].m[2][2]+mat[0].m[3][2];
            printf("%.2f %.2f %.2f\n" , xx+eps , yy+eps , zz+eps);
        }
        puts("");
    }
    return 0;
}