罗德里格旋转公式
罗德里格旋转公式(Rodrigues’ rotation formula) 是计算三维空间中，一个向量绕旋转轴旋转给定角度以后得到的新向量的计算公式。被广泛应用于空间解析几何和计算机图形学领域，成为刚体运动的基本计算公式。
1.旋转轴 k ⃗ \vec k k与旋转向量 v ⃗ \vec v v垂直，则
v ⃗ r o t = c o s θ ⋅ v ⃗ + s i n θ ⋅ ( k ⃗ × v ⃗ ) \vec v_{rot}=cos\theta\cdot\vec{v}+sin\theta\cdot(\vec k\times\vec v) vrot=cosθ⋅v+sinθ⋅(k×v)
推导过程：
对旋转后的向量进行正交分解： v ⃗ r o t = v 1 ⃗ + v 2 ⃗ \vec v_{rot}=\vec{v_1}+\vec{v_2} vrot=v1+v2
因为 v 1 ⃗ = ∣ v 1 ⃗ ∣ ⋅ i ⃗ \vec{v_1}=|\vec{v_1}|\cdot \vec i v1=∣v1∣⋅i ，其中 i i i 是 v ⃗ 1 \vec v_1 v1 的单位向量，所以 v 1 ⃗ = ∣ v ⃗ ∣ ⋅ c o s θ ⋅ v ⃗ ∣ v ⃗ ∣ = c o s θ ⋅ v ⃗ \vec{v_1}=|\vec{v}|\cdot cos\theta\cdot\frac{\vec v}{|\vec v|}=cos\theta\cdot{\vec v} v1=∣v∣⋅cosθ⋅∣v∣v=cosθ⋅v
同理 v 2 ⃗ = ∣ v ⃗ ∣ ⋅ s i n θ ⋅ k ⃗ × v ⃗ ∣ k ⃗ ∣ ∣ v ⃗ ∣ s i n < k ⃗ , v ⃗ > = s i n θ ⋅ k ⃗ × v ⃗ \vec{v_2}=|\vec{v}|\cdot sin\theta\cdot\frac{\vec k \times \vec v}{|\vec k||\vec v|sin<\vec k,\vec v>}=sin\theta\cdot{\vec k\times \vec v} v2=∣v∣⋅sinθ⋅∣k∣∣v∣sin<k,v>k×v=sinθ⋅k×v.
2.旋转轴 k ⃗ \vec k k与旋转向量 v ⃗ \vec v v不垂直，则
v ⃗ r o t = c o s θ ⋅ v ⃗ + ( 1 − c o s θ ) ( v ⃗ ⋅ k ⃗ ) k ⃗ + s i n θ ⋅ ( k ⃗ × v ⃗ ) \vec v_{rot}=cos\theta\cdot\vec{v}+(1-cos\theta)(\vec v\cdot\vec k)\vec k+sin\theta\cdot(\vec k\times\vec v) vrot=cosθ⋅v+(1−cosθ)(v⋅k)k+sinθ⋅(k×v)
推导如下：
对 v ⃗ \vec v v做正交分解， v ⃗ = v ∣ ∣ ⃗ + v ⊥ ⃗ \vec v=\vec{v_{||}}+\vec{v_{\bot} } v=v∣∣+v⊥
v ∣ ∣ ⃗ = ∣ v ⃗ ∣ c o s < v ⃗ , k ⃗ > ⋅ k ⃗ = ∣ v ⃗ ∣ v ⃗ ⋅ k ⃗ ∣ v ⃗ ∣ ⋅ ∣ k ⃗ ∣ ⋅ k ⃗ = ( v ⃗ ⋅ k ⃗ ) k ⃗ \vec{v_{||}}=|\vec v|cos<\vec v,\vec k>\cdot \vec{k}=|\vec v|\frac {\vec v \cdot \vec k}{|\vec v| \cdot |\vec k|}\cdot \vec{k}=(\vec v \cdot \vec k)\vec{k} v∣∣=∣v∣cos<v,k>⋅k=∣v∣∣v∣⋅∣k∣v⋅k⋅k=(v⋅k)k ;
v ⃗ ⊥ = v ⃗ − v ∣ ∣ ⃗ \vec v_{\bot}=\vec v-\vec{v_{||}} v⊥=v−v∣∣;
由1得： v ⃗ ⊥ r o t = c o s θ ⋅ v ⃗ ⊥ + s i n θ ⋅ ( k ⃗ × v ⃗ ⊥ ) \vec v_{\bot rot}=cos\theta\cdot\vec{v}_{\bot}+sin\theta\cdot(\vec k\times\vec v_{\bot}) v⊥rot=cosθ⋅v⊥+sinθ⋅(k×v⊥);
对旋转后的向量进行正交分解：
v ⃗ r o t = v ∣ ∣ ⃗ + v ⊥ r o t ⃗ = ( v ⃗ ⋅ k ⃗ ) k ⃗ + c o s θ ⋅ v ⃗ ⊥ + s i n θ ⋅ ( k ⃗ × v ⃗ ⊥ ) = ( v ⃗ ⋅ k ⃗ ) k ⃗ + c o s θ ⋅ ( v ⃗ − ( v ⃗ ⋅ k ⃗ ) k ⃗ ) + s i n θ ⋅ ( k ⃗ × ( v ⃗ − v ∣ ∣ ⃗ ) ) = c o s θ ⋅ v ⃗ + ( 1 − c o s θ ) ⋅ ( v ⃗ ⋅ k ⃗ ) k ⃗ + s i n θ ⋅ ( k ⃗ × v ⃗ ) \begin{aligned} \vec v_{rot}&=\vec{v_{||}}+\vec{v_{\bot rot}}\\ &=(\vec v \cdot \vec k)\vec{k}+cos\theta\cdot\vec{v}_{\bot}+sin\theta\cdot(\vec k\times\vec v_{\bot})\\ &=(\vec v \cdot \vec k)\vec{k}+cos\theta\cdot(\vec v-(\vec v \cdot \vec k)\vec{k} )+sin\theta\cdot(\vec k\times(\vec v-\vec{v_{||}}))\\ &=cos\theta\cdot\vec v+(1-cos\theta)\cdot(\vec v \cdot \vec k)\vec{k}+sin\theta\cdot(\vec k\times\vec v) \end{aligned} vrot=v∣∣+v⊥rot=(v⋅k)k+cosθ⋅v⊥+sinθ⋅(k×v⊥)=(v⋅k)k+cosθ⋅(v−(v⋅k)k)+sinθ⋅(k×(v−v∣∣))=cosθ⋅v+(1−cosθ)⋅(v⋅k)k+sinθ⋅(k×v)
得证。
把 v v v提取出来，写成矩阵形式：
v r o t = c o s θ ⋅ v + ( 1 − c o s θ ) ⋅ ( v T ⋅ k ) k + s i n θ ⋅ ( k × v ) = R ⋅ v \begin{aligned} v_{rot}&=cos\theta\cdot v+(1-cos\theta)\cdot(v^T \cdot k)k+sin\theta\cdot(k\times v)\\ &=R\cdot v \end{aligned} vrot=cosθ⋅v+(1−cosθ)⋅(vT⋅k)k+sinθ⋅(k×v)=R⋅v
因为 ( v T ⋅ k ) k = k ( v T ⋅ k ) = k ( k T ⋅ v ) = k ⋅ k T ⋅ v (v^T \cdot k )k=k(v^T \cdot k)=k(k^T \cdot v)=k\cdot k^T \cdot v (vT⋅k)k=k(vT⋅k)=k(kT⋅v)=k⋅kT⋅v
又因为
k ⃗ × v ⃗ = ( k 2 v 3 − k 3 v 2 ) ⋅ i ⃗ + ( k 3 v 1 − k 1 v 3 ) ⋅ j ⃗ + ( k 1 v 2 − k 2 v 1 ) ⋅ k ⃗ = [ k 2 v 3 − k 3 v 2 k 3 v 1 − k 1 v 3 k 1 v 2 − k 2 v 1 ] = [ 0 , − k 3 , k 2 k 3 , 0 , − k 1 − k 2 , k 1 , 0 ] [ v 1 v 2 v 3 ] = [ 0 , − k 3 , k 2 k 3 , 0 , − k 1 − k 2 , k 1 , 0 ] ⋅ v ⃗ \begin{aligned} \vec k\times \vec v&=(k_2v_3-k_3v_2)\cdot \vec i+(k_3v_1-k_1v_3)\cdot \vec j+(k_1v_2-k_2v_1)\cdot \vec k\\ &=\left[\begin{array}{l} k_2v_3-k_3v_2\\ k_3v_1-k_1v_3\\ k_1v_2-k_2v_1 \end{array}\right]\\ &=\left[\begin{array}{l} 0,-k_3,k_2\\ k_3,0,-k_1\\ -k_2,k_1,0 \end{array}\right] \left[\begin{array}{l} v_1\\ v_2\\ v_3 \end{array}\right]=\left[\begin{array}{l} 0,-k_3,k_2\\ k_3,0,-k_1\\ -k_2,k_1,0 \end{array}\right]\cdot \vec v \end{aligned} k×v=(k2v3−k3v2)⋅i+(k3v1−k1v3)⋅j+(k1v2−k2v1)⋅k=⎣ ⎡k2v3−k3v2k3v1−k1v3k1v2−k2v1⎦ ⎤=⎣ ⎡0,−k3,k2k3,0,−k1−k2,k1,0⎦ ⎤⎣ ⎡v1v2v3⎦ ⎤=⎣ ⎡0,−k3,k2k3,0,−k1−k2,k1,0⎦ ⎤⋅v
即：
R = c o s θ ⋅ I 3 × 3 + ( 1 − c o s θ ) ⋅ ( k ⃗ ⋅ k ⃗ T ) + s i n θ ⋅ [ 0 , − k 3 , k 2 k 3 , 0 , − k 1 − k 2 , k 1 , 0 ] R=cos\theta\cdot I_{3\times3}+(1-cos\theta)\cdot(\vec k \cdot \vec k^T) +sin\theta\cdot\left[\begin{array}{l} 0,-k_3,k_2\\ k_3,0,-k_1\\ -k_2,k_1,0 \end{array}\right] R=cosθ⋅I3×3+(1−cosθ)⋅(k⋅kT)+sinθ⋅⎣ ⎡0,−k3,k2k3,0,−k1−k2,k1,0⎦ ⎤
罗德里格旋转公式给出了轴角->旋转矩阵的转换：
R = c o s θ ⋅ I 3 × 3 + ( 1 − c o s θ ) ⋅ ( k ⃗ ⋅ k ⃗ T ) + s i n θ ⋅ [ 0 , − k 3 , k 2 k 3 , 0 , − k 1 − k 2 , k 1 , 0 ] R=cos\theta\cdot I_{3\times3}+(1-cos\theta)\cdot(\vec k \cdot \vec k^T) +sin\theta\cdot\left[\begin{array}{l} 0,-k_3,k_2\\ k_3,0,-k_1\\ -k_2,k_1,0 \end{array}\right] R=cosθ⋅I3×3+(1−cosθ)⋅(k⋅kT)+sinθ⋅⎣ ⎡0,−k3,k2k3,0,−k1−k2,k1,0⎦ ⎤