void PointRotateByVector(double p1[3], double v1[3], double theta, double p2[3])
{
double r = theta * PI / 180;
double c = cos(r);
double s = sin(r);
p2[0] = (v1[0] * v1[0] * (1 - c) + c) * p1[0] + (v1[0] * v1[1] * (1 - c) - v1[2] * s) * p1[1] + (v1[0] * v1[2] * (1 - c) + v1[1] * s) * p1[2];
p2[1] = (v1[1] * v1[0] * (1 - c) + v1[2] * s) * p1[0] + (v1[1] * v1[1] * (1 - c) + c) * p1[1] + (v1[1] * v1[2] * (1 - c) - v1[0] * s) * p1[2];
p2[2] = (v1[0] * v1[2] * (1 - c) - v1[1] * s) * p1[0] + (v1[1] * v1[2] * (1 - c) + v1[0] * s) * p1[1] + (v1[2] * v1[2] * (1 - c) + c) * p1[2];
}
3D点绕任意轴旋转
最新推荐文章于 2024-07-12 16:34:36 发布