罗德里格斯公式

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

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

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

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

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

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

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

；

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

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

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

，

则旋转矩阵。

以下给出代码实现：

 1     void GetRotateMatrix(Matrix<float>& Q, Vector<float>& axis, float theta)
 2     {
 3         // 使用罗德里格斯公式（Rodriguez formula）
 4 
 5         // 构造单位向量
 6         float len = sqrt(axis.data[0] * axis.data[0] +
 7             axis.data[1] * axis.data[1] + axis.data[2] * axis.data[2]);
 8         axis.data[0] = axis.data[0] / len;
 9         axis.data[1] = axis.data[1] / len;
10         axis.data[2] = axis.data[2] / len;
11         
12         // 构造叉积矩阵K
13         Matrix<float> K(3, 3);
14         *(K.data + 0) = 0.;
15         *(K.data + 1) = -axis.data[2];
16         *(K.data + 2) = axis.data[1];
17         *(K.data + K.step * 1 + 0) = axis.data[2];
18         *(K.data + K.step * 1 + 1) = 0.;
19         *(K.data + K.step * 1 + 2) = -axis.data[0];
20         *(K.data + K.step * 2 + 0) = -axis.data[1];
21         *(K.data + K.step * 2 + 1) = axis.data[0];
22         *(K.data + K.step * 2 + 2) = 0.;
23 
24         // 求K^2
25         Matrix<float> K_Temp; K_Temp = K;
26         Matrix<float> K_2; K_2 = K;
27         K_2.Multiply(K, K_Temp);
28 
29         // 求变换矩阵Q(Q = I + (1 - cos(theta))K^2 + sin(theta)K)
30         K.ScalarMultiply(sin(theta));
31         K_2.ScalarMultiply(1. - cos(theta));
32         K_Temp.SetIdentity();
33 
34         Q.SetZero();
35         Q.Add(K);
36         Q.Add(K_2);
37         Q.Add(K_Temp);
38 
39         // 得到旋转矩阵后，使用旋转矩阵Q将任意点沿axis轴旋转theta幅度
40     }

  1     template<typename TYPE> struct  Matrix
  2     {
  3         int rows;
  4         int cols;
  5         int step;
  6 
  7         TYPE* data;
  8 
  9         Matrix()
 10         {
 11             rows = cols = step = 0;
 12             data = 0;
 13         }
 14 
 15         Matrix(int _rows, int _cols)
 16         {
 17             rows = _rows; 
 18             cols = _cols;
 19             step = _cols;
 20             data = (TYPE*)malloc(sizeof(TYPE) * rows * cols);
 21             memset(data, 0, sizeof(TYPE) * rows * cols);
 22         }
 23         
 24         ~Matrix()
 25         {
 26             if(!data) free(data);
 27         }
 28 
 29         void operator = (Matrix<TYPE>& _mat)
 30         {
 31             rows = _mat.rows;
 32             cols = _mat.cols;
 33             step = _mat.step;
 34 
 35             if(data) free(data);
 36             data = (TYPE*)malloc(sizeof(TYPE) * rows * cols);
 37             memcpy(data, _mat.data, sizeof(TYPE) * rows * cols);
 38         }
 39 
 40         bool Add(Matrix<TYPE>& src1)
 41         {
 42             if (src1.rows != rows || src1.cols != cols)
 43                 return false;
 44 
 45             for (int r = 0; r < rows; ++r)
 46                 for (int c = 0; c < cols; ++c)
 47                 {
 48                     *(data + r * step + c) += *(src1.data + r * src1.step + c);
 49                 }
 50         }
 51 
 52         void ScalarMultiply(double scalar)
 53         {
 54             for (int r = 0; r < rows; ++r)
 55                 for (int c = 0; c < cols; ++c)
 56                 {
 57                     *(data + r * step + c) = *(data + r * step + c) * scalar;
 58                 }
 59         }
 60 
 61         void SetZero()
 62         {
 63             memset(data, 0, sizeof(TYPE) * rows * cols);
 64         }
 65 
 66         void SetIdentity()
 67         {
 68             memset(data, 0, sizeof(TYPE) * rows * cols);
 69 
 70             int m = cols < rows ? cols : rows;
 71             for(int i = 0; i < m; ++i)
 72                 *(data + i * step + i) = 1;
 73         }
 74 
 75         void ExchangeRows(int i, int j)
 76         {
 77             if(i < rows && j < rows)
 78             {
 79                 TYPE* temp = (TYPE*)malloc(sizeof(TYPE) * rows);
 80                 memcpy(temp, data + i * step, sizeof(TYPE) * rows);
 81                 memcpy(data + i * step , data + j * step, sizeof(TYPE) * rows);
 82                 memcpy(data + j * step, temp, sizeof(TYPE) * rows);
 83                 free(temp);
 84             }
 85         }
 86 
 87         bool Transpose(Matrix<TYPE>& dst)
 88         {
 89             if(rows != dst.cols || cols != dst.rows || !data || !dst.data)
 90                 return false;
 91 
 92             for(int r = 0; r < rows; ++r)
 93             {
 94                 TYPE* src_data = data + r * step;
 95                 TYPE* dst_data = dst.data + r;
 96 
 97                 for(int c = 0; c < cols; ++c)
 98                 {
 99                     *dst_data = *src_data;
100 
101                     ++src_data;
102                     dst_data += dst.step;
103                 }
104             }
105 
106             return true;
107         }
108 
109     
110         bool Multiply(Matrix<TYPE>& src1, Matrix<TYPE>& src2)
111         {
112             if(src1.cols != src2.rows || rows != src1.rows || cols != src2.cols) 
113                 return false;
114 
115             for(int r = 0; r < rows; ++r)
116             {
117                 for(int c = 0; c < cols; ++c)
118                 {
119                     double value = 0.;
120                     for (int i = 0; i < src1.cols; ++i)
121                     {
122                         value += (*(src1.data + r * src1.step + i)) * (*(src2.data + i * src2.step + c));
123                     }
124                     *(data + r * step + c) = value;
125                 }
126             }
127 
128             return true;
129 
130         }
131 
132     };

猜你喜欢