根据点、旋转轴、旋转角度，计算点旋转之后的位置

Point Util::Rotate(Point p, double angle, double x, double y, double z) //注意这里的（x,y,z）是单位化的旋转轴向量
{
    double m[3][3];
    //D3DXVec3Normalize();
    //angle = 1.57;
    double u = x;
    double v = y;
    double w = z;

    m[0][0] = cosf(angle) + (u * u) * (1 - cosf(angle));
    m[0][1] = u * v * (1 - cosf(angle)) + w * sinf(angle);
    m[0][2] = u * w * (1 - cosf(angle)) - v * sinf(angle);
    //m[0][3] = 0;

    m[1][0] = u * v * (1 - cosf(angle)) - w * sinf(angle);
    m[1][1] = cosf(angle) + v * v * (1 - cosf(angle));
    m[1][2] = w * v * (1 - cosf(angle)) + u * sinf(angle);
    //m[1][3] = 0;

    m[2][0] = u * w * (1 - cosf(angle)) + v * sinf(angle);
    m[2][1] = v * w * (1 - cosf(angle)) - u * sinf(angle);
    m[2][2] = cosf(angle) + w * w * (1 - cosf(angle));
    //m[2][3] = 0;

    /*m[3][0] = 0;
    m[3][1] = 0;
    m[3][2] = 0;
    m[3][3] = 1;*/

    Vector3d temp(p.x, p.y, p.z);
    Matrix3d mt(3,3);
    mt(0, 0) = m[0][0];
    mt(0, 1) = m[0][1];
    mt(0, 2) = m[0][2];
    mt(1, 0) = m[1][0];
    mt(1, 1) = m[1][1];
    mt(1, 2) = m[1][2];
    mt(2, 0) = m[2][0];
    mt(2, 1) = m[2][1];
    mt(2, 2) = m[2][2];

    Vector3d temp1 = mt * temp;

    Point pp;
    pp.x = temp1[0];
    pp.y = temp1[1];
    pp.z = temp1[2];

    return pp;
}