一些立体几何

点绕轴旋转

const double PI=acos(-1.0);
struct point3{double x,y,z;};
 
//求三维空间上一点q绕 向量（x0,y0,z0）正向旋转ang 弧度的点，向量起点在原点
point3 rotate3(point3 q,double x0,double y0,double z0,double ang)
{
    //ang=ang*PI/180;//如果是角度制则不用注释
	double x2=x0*x0;
	double y2=y0*y0;
	double z2=z0*z0;
	double d2=x2+y2+z2;
	double d=sqrt(d2);
	double sina=sin(ang);
	double cosa=cos(ang);
	point3 ans;
	ans.x=(x2+(y2+z2)*cosa)/d2*q.x
		+ (x0*y0*(1-cosa)/d2 - z0*sina/d )* q.y
		+ (x0*z0*(1-cosa)/d2+y0*sina/d)*q.z;
	ans.y=(y0*x0*(1-cosa)/d2+z0*sina/d)*q.x 
		+ (y2+(x2+z2)*cosa)/d2* q.y
		+ (y0*z0*(1-cosa)/d2-x0*sina/d)*q.z;
	ans.z=(z0*x0*(1-cosa)/d2 - y0*sina/d)*q.x
		+ (z0*y0*(1-cosa)/d2+x0*sina/d)*q.y 
		+ (z2+(x2+y2)*cosa)/d2*q.z;
	return ans;
}