三维空间任意一点绕任意轴线旋转
参考链接三维空间任意一点绕任意轴线旋转_Marc Pony-CSDN博客_三维坐标旋转公式绕任意轴
对三维空间任意一点 \(P(p_x,p_y,p_z)\)，求绕任意轴线旋转角度 \(\alpha\) 得到新的点 \(P^{'}(p^{'}_{x},p^{'}_{y},p^{'}_{z})\)
设轴线的方程为
\(\left\{\matrix{x=x_0+n_xt\\y=y_0+n_yt\\z=z_0+n_zt}\right.\)
\(PP^{'}\)所在平面在坐标系 \(XYZ\) 下的平面方程为：
\(n_x(x-p_x)+n_y(y-p_y)+n_z(z-p_z)=0\)
结合两式可以求得
\(t_0=n_x(x-p_x)+n_y(y-p_y)+n_z(z-p_z)\)
平面与轴线相交的圆心坐标为
\(\left\{\matrix{x_c=x_0+n_xt_0\\y_c=y_0+n_yt_0\\z_c=z_0+n_zt_0}\right.\)
圆半径
\(r=\sqrt{(p_x-x_c)^2+(p_y-y_c)^2+(p_z-z_c)^2}\)
向量OP
\(\textbf{OP}=[p_x-x_c\ p_y-y_c\ p_z-z_c]^T/r\)
沿轴线建立新坐标系，根据右手法则
\(\textbf{y}^{'}=[n_x\ n_y\ n_z]^{T}\times\textbf{OP}\)
新的坐标系与旧坐标系之间的旋转变换矩阵为
\(R_{3\times3}=[\textbf{OP}\ \textbf{y}^{'}\ \hat{n}]\)
点 \(P^{'}\) 在新坐标系下的坐标为
\(\left\{\matrix{x_{temp}=rcos(\alpha)\\y_{temp}=rsin(\alpha)\\z_{temp}=0}\right.\)
利用齐次变换，将点 \(P^{'}\) 在新坐标系变回原坐标系
\(\left[\matrix{p^{'}_x\\p^{'}_y\\p^{'}_z\\1}\right]=\left[\matrix{R_{11}&R_{12}&R_{13}&x_c\\R_{21}&R_{22}&R_{23}&y_c\\R_{31}&R_{32}&R_{33}&z_c\\0&0&0&1}\right]\left[\matrix{x_{temp}\\y_{temp}\\z_{temp}\\1}\right]\)
\(\left[\matrix{p^{'}_x\\p^{'}_y\\p^{'}_z\\1}\right]=T_{4\times4}\left[\matrix{p_x\\p_y\\p_z\\1}\right]\)
\(T_{4\times4}=\left[\matrix{{n^2_x}K+cos(\alpha)&n_xn_yK-n_zsin(\alpha)&n_xn_zK+n_ysin(\alpha)&(x_0-n_zM)K+(n_zy_0-n_yz_0)sin(\alpha)\\n_xn_yK+n_zsin(\alpha)&n_y^2K+cos(\alpha)&n_yn_zK-n_xsin(\alpha)&(y_0-n_yM)K+(n_xz_0-n_zx_0)sin(\alpha)\\n_xn_zK-n_ysin(\alpha)&n_yn_zK+n_xsin(\alpha)&n_z^2K+cos(\alpha)&(z_0-n_zM)K+(n_yx_0-n_xy_0)sin(\alpha)\\0&0&0&1}\right]\)
其中 \(K=1-cos(\alpha),M=n_xx_0+n_yy_0+n_zz_0\)
代码
经场上测试，轴过原点时适用
#include<bits/stdc++.h>
using namespace std;
const double pi=acos(-1.0);
struct tnode{
double x,y,z;
};
tnode xz(tnode l,tnode r,tnode from,double dp)
{
tnode to;
tnode n;
n.x=r.x-l.x;
n.y=r.y-l.y;
n.z=r.z-l.z;
double cosa=cos(pi*dp/180);
double sina=sin(pi*dp/180);
double K=1-cosa;
double M=0;
double p=sqrt(n.x*n.x+n.y*n.y+n.z*n.z);
n.x/=p;n.y/=p;n.z/=p;
double t00=n.x*n.x*K+cosa;
double t01=n.x*n.y*K-n.z*sina;
double t02=n.x*n.z*K+n.y*sina;
double t03=0;
double t10=n.x*n.y*K+n.z*sina;
double t11=n.y*n.y*K+cosa;
double t12=n.y*n.z*K-n.x*sina;
double t13=0;
double t20=n.z*n.x*K-n.y*sina;
double t21=n.y*n.z*K+n.x*sina;
double t22=n.z*n.z*K+cosa;
double t23=0;
to.x=from.x*t00+from.y*t01+from.z*t02;
to.y=from.x*t10+from.y*t11+from.z*t12;
to.z=from.x*t20+from.y*t21+from.z*t22;
return to;
}
int main()
{
tnode l;tnode r;double dq;
int t;
scanf("%d",&t);
while(t--)
{
scanf("%lf%lf%lf%lf%lf%lf%lf",&l.x,&l.y,&l.z,&r.x,&r.y,&r.z,&dq);
tnode o;
o.x=0;o.y=0;o.z=0;
tnode pl=xz(o,l,r,-dq);
tnode pr=xz(o,l,r,dq);
if(pr.z>pl.z)
{
printf("%.9lf %.9lf %.9lf\n",pr.x,pr.y,pr.z);
}
else
{
printf("%.9lf %.9lf %.9lf\n",pl.x,pl.y,pl.z);
}
}
return 0;
}
未经测试，理论上没错
#include<bits/stdc++.h>
using namespace std;
const double pi=acos(-1.0);
struct tnode{
double x,y,z;
};
tnode xz(tnode l,tnode r,tnode from,double dp)
{
//l为起始点，r为中止点，from为目标点，dp为旋转角度
tnode to;
tnode n;
n.x=r.x-l.x;
n.y=r.y-l.y;
n.z=r.z-l.z;
double x0=l.x,y0=l.y,z0=l.z;
double cosa=cos(pi*dp/180);
double sina=sin(pi*dp/180);
double K=1-cosa;
double M=n.x*x0+n.y*y0+n.z*z0;
double p=sqrt(n.x*n.x+n.y*n.y+n.z*n.z);
n.x/=p;n.y/=p;n.z/=p;
double t00=n.x*n.x*K+cosa;
double t01=n.x*n.y*K-n.z*sina;
double t02=n.x*n.z*K+n.y*sina;
double t03=(x0-n.x*M)*K+(n.z*y0-n.y*z0)*sina;
double t10=n.x*n.y*K+n.z*sina;
double t11=n.y*n.y*K+cosa;
double t12=n.y*n.z*K-n.x*sina;
double t13=(y0-n.y*M)*K+(n.x*z0-n.z*x0)*sina;
double t20=n.z*n.x*K-n.y*sina;
double t21=n.y*n.z*K+n.x*sina;
double t22=n.z*n.z*K+cosa;
double t23=(z0-n.z*M)*K+(n.y*x0-n.x*y0)*sina;;
to.x=from.x*t00+from.y*t01+from.z*t02+t03;
to.y=from.x*t10+from.y*t11+from.z*t12+t13;
to.z=from.x*t20+from.y*t21+from.z*t22+t23;
return to;
}
int main()
{
tnode l;tnode r;double dq;
int t;
scanf("%d",&t);
while(t--)
{
scanf("%lf%lf%lf%lf%lf%lf%lf",&l.x,&l.y,&l.z,&r.x,&r.y,&r.z,&dq);
tnode o;
o.x=0;o.y=0;o.z=0;
tnode pl=xz(o,l,r,-dq);
tnode pr=xz(o,l,r,dq);
if(pr.z>pl.z)
{
printf("%.9lf %.9lf %.9lf\n",pr.x,pr.y,pr.z);
}
else
{
printf("%.9lf %.9lf %.9lf\n",pl.x,pl.y,pl.z);
}
}
return 0;
}