xyz的向量旋轉模板

#include<iostream>
#include<algorithm>
#include<stdio.h>
#include<math.h>
#include<cstring>
#include<string>
 
#define inf 0x3f3f3f3f
#define pi acos(-1.0)
#define eps 10e-6
using namespace std;
struct point
{
    double x,y;
    point(double a = 0,double b = 0):x(a),y(b){ }
};
typedef point vector;
vector operator -(point a,point b)
{
    return vector(a.x-b.x,a.y-b.y);
}
vector rotation(vector A,double rad)//向量A旋轉rad個弧度
{
    return vector(A.x*cos(rad)-A.y*sin(rad), A.x*sin(rad)+A.y*cos(rad) );
}
int main()
{
    int t;
    scanf("%d",&t);
    while(t--)
    {
         int n,i;
         scanf("%d",&n);
         point a(0,0);
         double sum = 0;
         for(i = 0; i < n; i++)
         {
                point b;
                double rad;
                scanf("%lf%lf%lf",&b.x,&b.y,&rad);
                sum += rad;
                vector A = b-a;
                vector B = rotation(A,rad);
                a = b-B;
         }
         while(sum-2*pi > eps) sum-=2*pi;
         double c = a.x - a.y*sin(sum)/(1-cos(sum));//假設旋轉中心爲A,通過聯立dis(A,a點初始座標)和dis(A,a點旋轉後坐標)。與dis(A,b點初始座標)和  dis（A，b點旋轉後坐標）。

         double d = 1 - cos(sum) + sin(sum)*sin(sum)/(1-cos(sum));
         double resx = c/d;
         double resy = (resx*sin(sum) + a.y)/(1-cos(sum));
         printf("%.9lf %.9lf %.9f\n",resx,resy,sum);
    }
    return 0;
}