三维点坐标绕任意轴旋转后坐标

//old:旋转前点坐标
//theta：旋转角度
//v1V2 ：旋转轴
//newPoint: 旋转后点坐标
void LithoMath::GetRotatePointByVector(double old[3], double theta, double v1[3], double v2[3], double newPoint[3])
{
	double a = v1[0];
	double b = v1[1];
	double c = v1[2];

	double vector[3];
	for (int i = 0; i < 3; i++) {
		vector[i] = v2[i] - v1[i];
	}
	normalize(vector);

	double u = vector[0];
	double v = vector[1];
	double w = vector[2];

	double uu = u * u;
	double uv = u * v;
	double uw = u * w;
	double vv = v * v;
	double vw = v * w;
	double ww = w * w;
	double au = a * u;
	double av = a * v;
	double aw = a * w;
	double bu = b * u;
	double bv = b * v;
	double bw = b * w;
	double cu = c * u;
	double cv = c * v;
	double cw = c * w;

	float costheta = cosf(theta);
	float sintheta = sinf(theta);


	double matrix_00 = uu + (vv + ww) * costheta;
	double matrix_01 = uv * (1 - costheta) + w * sintheta;
	double matrix_02 = uw * (1 - costheta) - v * sintheta;
	double matrix_03 = 0;
	

	double matrix_10 = uv * (1 - costheta) - w * sintheta;
	double matrix_11 = vv + (uu + ww) * costheta;
	double matrix_12 = vw * (1 - costheta) + u * sintheta;
	double matrix_13 = 0;
	

	double matrix_20 = uw * (1 - costheta) + v * sintheta;
	double matrix_21 = vw * (1 - costheta) - u * sintheta;
	double matrix_22 = ww + (uu + vv) * costheta;
	double matrix_23 = 0;
	

	double matrix_30 = (a * (vv + ww) - u * (bv + cw)) * (1 - costheta) + (bw - cv) * sintheta;
	double matrix_31 = (b * (uu + ww) - v * (au + cw)) * (1 - costheta) + (cu - aw) * sintheta;
	double matrix_32 = (c * (uu + vv) - w * (au + bv)) * (1 - costheta) + (av - bu) * sintheta;
	double matrix_33 = 1;

	double old_x = old[0];
	double old_y = old[1];
	double old_z = old[2];

	newPoint[0] = old_x * matrix_00 + old_y * matrix_10 + old_z * matrix_20 + matrix_30;
	newPoint[1] = old_x * matrix_01 + old_y * matrix_11 + old_z * matrix_21 + matrix_31;
	newPoint[2] = old_x * matrix_02 + old_y * matrix_12 + old_z * matrix_22 + matrix_32;
}