三维坐标点绕任意轴旋转的新坐标计算

http://blog.csdn.net/sniffer_wang/article/details/6456183

任意轴可以用一个起点一个方向向量来表示。那么绕任意轴旋转就可以先将此轴移到通过原点，然后再旋转，再将旋转完的新坐标做反向平移。

则问题化为 计算绕通过原点的向量旋转任意角度后的新点。假设单位向量为(rx,ry,rz)，那么旋转矩阵如下：

写成函数如下：

void Rotate_Point3D(float theta, float nx, float ny, float nz, float (&ptIn)[3], float (&ptOut)[3])
{
 float len = sqrtf(nx * nx + ny * ny + nz * nz); //单位化
 nx /= len;        ny /= len;            nz /= len;   
 ptOut[0] =  ptIn[0] * (cosf(theta) + nx * nx * (1 - cosf(theta))) +    //transform by matrix
  ptIn[1] * (nx * ny * (1 - cosf(theta)) - nz * sinf(theta)) + 
  ptIn[2] * (nx * nz * (1 - cosf(theta) + ny * sinf(theta)));
 ptOut[1] = ptIn[0] * (nx * ny * (1 - cosf(theta)) + nz * sinf(theta)) +  
  ptIn[1] * (ny * ny * (1 - cosf(theta)) + cosf(theta)) + 
  ptIn[2] * (ny * nz * (1 - cosf(theta)) - nx * sinf(theta));
 ptOut[2] = ptIn[0] * (nx * nz * (1 - cosf(theta) - ny * sinf(theta))) + 
  ptIn[1] * (ny * nz * (1 -cosf(theta)) + nx * sinf(theta)) + 
  ptIn[2] * (nz * nz * (1 - cosf(theta)) + cosf(theta));
}

注意 单位向量、逆时针