罗德里格斯公式

    在三维空间中，给定一固定旋转轴，任意初始向量绕旋转轴旋转任意角度，可表示为 ，其中，v 表示旋转前向量， 表示旋转后向量，R 表示旋转矩阵，该矩阵参数与固定旋转轴坐标，旋转角度有关。下面使用图示推导旋转矩阵 R：

    

    如上图所示，单位向量 为固定旋转轴，原向量 v 旋转  后为 ；

    分解原向量 ，向量 v 与向量 k（或者 ） 的夹角为 ，该值已知；

    向量  构成直角三角形，则有： ，；

    向量叉乘 ，其方向垂直于 构成的平面，w 的模长为  ，则有  ，且两向量正交；

    以上向量  构成三维空间已知正交向量基， 可表示：，在 向量构成平面上， 旋转  后为：

    ， ，由于 ，进一步化简为：

    ；

    定义向量 ，其方向与 平行，其模长为 ，则 ；

    引入向量 的叉积矩阵 ，叉积运算可转换为矩阵运算 ；

    改写  线性组合 ，引入叉积矩阵 K得：

    ，

    则旋转矩阵 。

    以下给出代码实现：

    void GetRotateMatrix(Matrix<float>& Q, Vector<float>& axis, float theta)
    {
        // 使用罗德里格斯公式（Rodriguez formula）

        // 构造单位向量
        float len = sqrt(axis.data[0] * axis.data[0] +
            axis.data[1] * axis.data[1] + axis.data[2] * axis.data[2]);
        axis.data[0] = axis.data[0] / len;
        axis.data[1] = axis.data[1] / len;
        axis.data[2] = axis.data[2] / len;
        
        // 构造叉积矩阵K
        Matrix<float> K(3, 3);
        *(K.data + 0) = 0.;
        *(K.data + 1) = -axis.data[2];
        *(K.data + 2) = axis.data[1];
        *(K.data + K.step * 1 + 0) = axis.data[2];
        *(K.data + K.step * 1 + 1) = 0.;
        *(K.data + K.step * 1 + 2) = -axis.data[0];
        *(K.data + K.step * 2 + 0) = -axis.data[1];
        *(K.data + K.step * 2 + 1) = axis.data[0];
        *(K.data + K.step * 2 + 2) = 0.;

        // 求K^2
        Matrix<float> K_Temp; K_Temp = K;
        Matrix<float> K_2; K_2 = K;
        K_2.Multiply(K, K_Temp);

        // 求变换矩阵Q(Q = I + (1 - cos(theta))K^2 + sin(theta)K)
        K.ScalarMultiply(sin(theta));
        K_2.ScalarMultiply(1. - cos(theta));
        K_Temp.SetIdentity();

        Q.SetZero();
        Q.Add(K);
        Q.Add(K_2);
        Q.Add(K_Temp);

        // 得到旋转矩阵后，使用旋转矩阵Q将任意点沿axis轴旋转theta幅度
    }

    

    template<typename TYPE> struct  Matrix
    {
        int rows;
        int cols;
        int step;

        TYPE* data;

        Matrix()
        {
            rows = cols = step = 0;
            data = 0;
        }

        Matrix(int _rows, int _cols)
        {
            rows = _rows; 
            cols = _cols;
            step = _cols;
            data = (TYPE*)malloc(sizeof(TYPE) * rows * cols);
            memset(data, 0, sizeof(TYPE) * rows * cols);
        }
        
        ~Matrix()
        {
            if(!data) free(data);
        }

        void operator = (Matrix<TYPE>& _mat)
        {
            rows = _mat.rows;
            cols = _mat.cols;
            step = _mat.step;

            if(data) free(data);
            data = (TYPE*)malloc(sizeof(TYPE) * rows * cols);
            memcpy(data, _mat.data, sizeof(TYPE) * rows * cols);
        }

        bool Add(Matrix<TYPE>& src1)
        {
            if (src1.rows != rows || src1.cols != cols)
                return false;

            for (int r = 0; r < rows; ++r)
                for (int c = 0; c < cols; ++c)
                {
                    *(data + r * step + c) += *(src1.data + r * src1.step + c);
                }
        }

        void ScalarMultiply(double scalar)
        {
            for (int r = 0; r < rows; ++r)
                for (int c = 0; c < cols; ++c)
                {
                    *(data + r * step + c) = *(data + r * step + c) * scalar;
                }
        }

        void SetZero()
        {
            memset(data, 0, sizeof(TYPE) * rows * cols);
        }

        void SetIdentity()
        {
            memset(data, 0, sizeof(TYPE) * rows * cols);

            int m = cols < rows ? cols : rows;
            for(int i = 0; i < m; ++i)
                *(data + i * step + i) = 1;
        }

        void ExchangeRows(int i, int j)
        {
            if(i < rows && j < rows)
            {
                TYPE* temp = (TYPE*)malloc(sizeof(TYPE) * rows);
                memcpy(temp, data + i * step, sizeof(TYPE) * rows);
                memcpy(data + i * step , data + j * step, sizeof(TYPE) * rows);
                memcpy(data + j * step, temp, sizeof(TYPE) * rows);
                free(temp);
            }
        }

        bool Transpose(Matrix<TYPE>& dst)
        {
            if(rows != dst.cols || cols != dst.rows || !data || !dst.data)
                return false;

            for(int r = 0; r < rows; ++r)
            {
                TYPE* src_data = data + r * step;
                TYPE* dst_data = dst.data + r;

                for(int c = 0; c < cols; ++c)
                {
                    *dst_data = *src_data;

                    ++src_data;
                    dst_data += dst.step;
                }
            }

            return true;
        }

    
        bool Multiply(Matrix<TYPE>& src1, Matrix<TYPE>& src2)
        {
            if(src1.cols != src2.rows || rows != src1.rows || cols != src2.cols) 
                return false;

            for(int r = 0; r < rows; ++r)
            {
                for(int c = 0; c < cols; ++c)
                {
                    double value = 0.;
                    for (int i = 0; i < src1.cols; ++i)
                    {
                        value += (*(src1.data + r * src1.step + i)) * (*(src2.data + i * src2.step + c));
                    }
                    *(data + r * step + c) = value;
                }
            }

            return true;

        }

    };