三维矩阵旋转

#include <iostream>
#include <cmath>
using namespace std;
const double PI = acos(-1.0);
const double eps = 1e-16;
struct point3
{
    double x,y,z;
    point3(double a,double b,double c)
    {
        x = a;
        y = b;
        z = c;
    }
    point3() {}
};
struct cube
{
    point3 p[8];
} cb,cb1;
inline double zero(double a)
{
    return fabs(a)<eps ? 0:a;
}
point3 rotate(double ux,double uy,double uz,double angle,point3 ori)
{
    point3 p;
    p.x=(ux*ux+cos(angle)*(1-ux*ux))*ori.x+
        (ux*uy*(1-cos(angle))-uz*sin(angle))*ori.y+
        (uz*ux*(1-cos(angle))+uy*sin(angle))*ori.z;
    p.y=(ux*uy*(1-cos(angle))+uz*sin(angle))*ori.x+
        (uy*uy+cos(angle)*(1-uy*uy))*ori.y+
        (uy*uz*(1-cos(angle))-ux*sin(angle))*ori.z;
    p.z=(uz*ux*(1-cos(angle))-uy*sin(angle))*ori.x+
        (uy*uz*(1-cos(angle))+ux*sin(angle))*ori.y+
        (uz*uz+cos(angle)*(1-uz*uz))*ori.z;
    return p=point3(zero(p.x),zero(p.y),zero(p.z));
}
void getCube(point3 p0,double a)
{
    cb.p[0]=p0;
    cb.p[1]=point3(p0.x,p0.y,p0.z+a);
    cb.p[2]=point3(p0.x,p0.y+a,p0.z);
    cb.p[3]=point3(p0.x+a,p0.y,p0.z);
    cb.p[4]=point3(p0.x+a,p0.y,p0.z+a);
    cb.p[5]=point3(p0.x,p0.y+a,p0.z+a);
    cb.p[6]=point3(p0.x+a,p0.y+a,p0.z);
    cb.p[7]=point3(p0.x+a,p0.y+a,p0.z+a);
}
int main()
{
    int t;
    point3 us,ue,p0,vec;
    double  angle,a;
    scanf("%d",&t);
    while(t--)
    {
        scanf("%lf%lf%lf%lf",&a,&p0.x,&p0.y,&p0.z);
        scanf("%lf%lf%lf%lf%lf%lf",&us.x,&us.y,&us.z,&ue.x,&ue.y,&ue.z);
        scanf("%lf",&angle);
        angle=angle*PI/180;
        getCube(p0,a);
        double tmp=sqrt((ue.x-us.x)*(ue.x-us.x)+(ue.y-us.y)*(ue.y-us.y)+(ue.z-us.z)*(ue.z-us.z));
        vec.x=(ue.x-us.x)/tmp;
        vec.y=(ue.y-us.y)/tmp;
        vec.z=(ue.z-us.z)/tmp;
        for(int i=0; i<8; i++)
            cb1.p[i]=rotate(vec.x,vec.y,vec.z,angle,cb.p[i]);

        for(int i=0; i<8; i++)
            PointSet[i].x=cb1.p[i].x, PointSet[i].y=cb1.p[i].y;

        int vcnt;

        GS(PointSet,ch,8,vcnt);

        printf("%.2lf\n",area(vcnt,ch));
    }
    return 0;
}