三維空間任意一點繞任意軸線旋轉
參考鏈接三維空間任意一點繞任意軸線旋轉_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;
}