前言
在https://blog.csdn.net/b1049112625/article/details/140848588?spm=1001.2014.3001.5501
这篇文章中介绍了三维旋转的概念,本篇文章介绍一种更高效的方法,四元数旋转
复数
复数的表示方法
复数
z
z
z的一种表示方法是使用有序实数对来表示
z
=
(
x
,
y
)
z = (x,y)
z=(x,y)
其中,
x
x
x为
z
z
z的实部,
y
y
y为
z
z
z的虚部,
x
x
x和
y
y
y都是实数
(
0
,
y
)
(0,y)
(0,y)表示纯虚数
(
x
,
0
)
(x,0)
(x,0)表示实数
另一种表示方法是使用
z
=
x
+
y
i
z = x+yi
z=x+yi
表示,
i
i
i是虚数单位
注意:
i
∗
i
=
−
1
i*i = -1
i∗i=−1
复数的运算
复数的加法、减法和标量乘法是使用与二维向量 一样的规则进行的
z
1
+
z
2
=
(
x
1
,
y
z
)
+
(
x
2
,
y
2
)
=
(
x
1
+
x
2
,
y
1
+
y
2
)
z
1
−
z
2
=
(
x
1
,
y
z
)
−
(
x
2
,
y
2
)
=
(
x
1
−
x
2
,
y
1
−
y
2
)
z_1+ z_2 = (x_1,y_z)+(x_2,y_2)=(x_1+x_2,y_1+y_2)\\ z_1- z_2 = (x_1,y_z)-(x_2,y_2)=(x_1-x_2,y_1-y_2)\\
z1+z2=(x1,yz)+(x2,y2)=(x1+x2,y1+y2)z1−z2=(x1,yz)−(x2,y2)=(x1−x2,y1−y2)
或者
z
1
+
z
2
=
x
1
+
y
1
i
+
x
2
+
y
2
i
=
(
x
1
+
x
2
)
+
(
y
1
+
y
2
)
i
z
1
−
z
2
=
x
1
+
y
1
i
−
x
2
−
y
2
i
=
(
x
1
−
x
2
)
+
(
y
1
−
y
2
)
i
z_1+ z_2 = x_1+y_1i + x_2+y_2i = (x_1+x_2)+ (y_1+y_2)i\\ z_1- z_2 = x_1+y_1i - x_2-y_2i = (x_1-x_2)+ (y_1-y_2)i
z1+z2=x1+y1i+x2+y2i=(x1+x2)+(y1+y2)iz1−z2=x1+y1i−x2−y2i=(x1−x2)+(y1−y2)i
复数的乘法公式如下:
z
1
z
2
=
(
x
1
+
y
1
i
)
(
x
2
+
y
2
i
)
=
x
1
x
2
+
x
1
y
2
i
+
y
1
i
x
2
+
y
1
i
y
2
i
=
x
1
x
2
−
y
1
y
2
+
(
x
1
y
2
+
x
2
y
1
)
i
\begin{aligned} z_1z_2&=(x_1+y_1i)(x_2+y_2i)\\ &=x_1x_2 + x_1y_2i + y_1ix_2 + y_1iy_2i\\ &=x_1x_2-y_1y_2 + (x_1y_2+x_2y_1)i \end{aligned}
z1z2=(x1+y1i)(x2+y2i)=x1x2+x1y2i+y1ix2+y1iy2i=x1x2−y1y2+(x1y2+x2y1)i
共轭复数
对于复数
z
=
x
+
y
i
z = x+yi
z=x+yi,称
z
‾
\overline z
z为
z
z
z的共轭复数,其中
z
‾
=
x
−
y
i
\overline z = x-yi
z=x−yi
复数的模
复数的模或者绝对值定义为:
∣
z
∣
=
z
z
‾
=
x
2
+
y
2
|z| = \sqrt{z\overline z} = \sqrt{x^2+y^2}
∣z∣=zz=x2+y2
四元数(quaternion)
四元数的定义
复数概念由四元数扩充到高维,四元数使用一个实部和三个虚部来表示
q
=
s
+
a
i
+
b
j
+
c
k
q = s + ai + bj + ck
q=s+ai+bj+ck
其中,
s
s
s、
a
a
a、
b
b
b、
c
c
c都是实数,
i
i
i、
j
j
j、
k
k
k是虚数单位
i i i、 j j j、 k k k满足下面的关系:
- i 2 = j 2 = k 2 = − 1 i^2=j^2=k^2=-1 i2=j2=k2=−1
- i j = − j i = k ij = -ji = k ij=−ji=k
- j k = − k j = i jk = -kj = i jk=−kj=i
- k i = − i k = j ki = -ik = j ki=−ik=j
四元数的运算
四元数的加减如下:
q
1
+
q
2
=
(
s
1
+
s
2
)
+
(
a
1
+
a
2
)
i
+
(
b
1
+
b
2
)
j
+
(
c
1
+
c
2
)
k
q
1
−
q
2
=
(
s
1
−
s
2
)
+
(
a
1
−
a
2
)
i
+
(
b
1
−
b
2
)
j
+
(
c
1
−
c
2
)
k
q_1+q_2 = (s_1+s_2) + (a_1+a_2)i + (b_1+b_2)j + (c_1+c_2)k q_1-q_2 = (s_1-s_2) + (a_1-a_2)i + (b_1-b_2)j + (c_1-c_2)k
q1+q2=(s1+s2)+(a1+a2)i+(b1+b2)j+(c1+c2)kq1−q2=(s1−s2)+(a1−a2)i+(b1−b2)j+(c1−c2)k
四元数的乘法如下:
q
1
q
2
=
(
s
1
+
a
1
i
+
b
1
j
+
c
1
k
)
(
s
2
+
a
2
i
+
b
2
j
+
c
2
k
)
=
s
1
s
2
+
s
1
a
2
i
+
s
1
b
2
j
+
s
1
c
2
k
+
a
1
i
s
2
+
a
1
i
a
2
i
+
a
1
i
b
2
j
+
a
1
i
+
c
2
k
+
b
1
j
s
2
+
b
1
j
a
2
i
+
b
1
j
b
2
j
+
b
1
j
+
c
2
k
+
c
1
k
s
2
+
c
1
k
a
2
i
+
c
1
k
b
2
j
+
c
1
k
+
c
2
k
=
s
1
s
2
+
s
1
a
2
i
+
s
1
b
2
j
+
s
1
c
2
k
+
a
1
s
2
i
−
a
1
a
2
+
a
1
b
2
k
−
a
1
c
2
j
+
b
1
s
2
j
−
b
1
a
2
k
−
b
1
b
2
+
b
1
c
2
i
+
c
1
s
2
k
+
c
1
a
2
j
−
c
1
b
2
i
−
c
1
c
2
=
(
s
1
s
2
−
a
1
a
2
−
b
1
b
2
−
c
1
c
2
)
+
(
s
1
a
2
+
a
1
s
2
+
b
1
c
2
−
c
1
b
2
)
i
+
(
s
1
b
2
+
b
1
s
2
+
c
1
a
2
−
a
1
c
2
)
j
+
(
s
1
c
2
+
c
1
s
2
+
a
1
b
2
−
b
1
a
2
)
k
\begin{aligned} q_1q_2 &= (s_1 + a_1i + b_1j + c_1k)(s_2 + a_2i + b_2j + c_2k)\\ &= s_1s_2 + s_1a_2i +s_1b_2j +s_1c_2k +a_1is_2 +a_1ia_2i +a_1ib_2j +a_1i +c_2k+b_1js_2 +b_1ja_2i +b_1jb_2j +b_1j +c_2k+c_1ks_2 +c_1ka_2i +c_1kb_2j +c_1k+c_2k\\ &= s_1s_2 + s_1a_2i +s_1b_2j +s_1c_2k +a_1s_2i -a_1a_2 +a_1b_2k -a_1c_2j+b_1s_2j-b_1a_2k -b_1b_2 +b_1c_2i+c_1s_2k +c_1a_2j -c_1b_2i-c_1c_2\\ &= (s_1s_2-a_1a_2 - b_1b_2-c_1c_2) + (s_1a_2+a_1s_2+b_1c_2-c_1b_2)i+ (s_1b_2+b_1s_2+c_1a_2-a_1c_2)j+ (s_1c_2+c_1s_2+a_1b_2-b_1a_2)k \end{aligned}
q1q2=(s1+a1i+b1j+c1k)(s2+a2i+b2j+c2k)=s1s2+s1a2i+s1b2j+s1c2k+a1is2+a1ia2i+a1ib2j+a1i+c2k+b1js2+b1ja2i+b1jb2j+b1j+c2k+c1ks2+c1ka2i+c1kb2j+c1k+c2k=s1s2+s1a2i+s1b2j+s1c2k+a1s2i−a1a2+a1b2k−a1c2j+b1s2j−b1a2k−b1b2+b1c2i+c1s2k+c1a2j−c1b2i−c1c2=(s1s2−a1a2−b1b2−c1c2)+(s1a2+a1s2+b1c2−c1b2)i+(s1b2+b1s2+c1a2−a1c2)j+(s1c2+c1s2+a1b2−b1a2)k
我们分别把四个部分写出来:
- 实部: ( s 1 s 2 − a 1 a 2 − b 1 b 2 − c 1 c 2 ) (s_1s_2-a_1a_2 - b_1b_2-c_1c_2) (s1s2−a1a2−b1b2−c1c2)
- i i i虚部: ( s 1 a 2 + a 1 s 2 + b 1 c 2 − c 1 b 2 ) (s_1a_2+a_1s_2+b_1c_2-c_1b_2) (s1a2+a1s2+b1c2−c1b2)
- j j j虚部: ( s 1 b 2 + b 1 s 2 + c 1 a 2 − a 1 c 2 ) (s_1b_2+b_1s_2+c_1a_2-a_1c_2) (s1b2+b1s2+c1a2−a1c2)
- k k k虚部: ( s 1 c 2 + c 1 s 2 + a 1 b 2 − b 1 a 2 ) (s_1c_2+c_1s_2+a_1b_2-b_1a_2) (s1c2+c1s2+a1b2−b1a2)
我们如果定义向量
v
1
=
(
a
1
,
b
1
,
c
1
)
,
v
2
=
(
a
2
,
b
2
,
c
2
)
v_1=(a_1,b_1,c_1), v_2=(a_2,b_2,c_2)
v1=(a1,b1,c1),v2=(a2,b2,c2)
那么
v
1
⋅
v
2
=
(
a
1
a
2
+
b
1
b
2
+
c
1
c
2
)
v_1\cdot v_2 = (a_1a_2 + b_1b_2+c_1c_2)
v1⋅v2=(a1a2+b1b2+c1c2)
v
1
×
v
2
=
(
b
1
c
2
−
c
1
b
2
,
c
1
a
2
−
a
1
c
2
,
a
1
b
2
−
b
1
a
2
)
v_1\times v_2 = (b_1c_2-c_1b_2, c_1a_2-a_1c_2, a_1b_2-b_1a_2)
v1×v2=(b1c2−c1b2,c1a2−a1c2,a1b2−b1a2)
观察一下,上面的四部分正好可以写成:
- 实部: ( s 1 s 2 − v 1 ⋅ v 2 ) (s_1s_2-v_1\cdot v_2) (s1s2−v1⋅v2)
- i i i虚部: ( s 1 v 2 + s 2 v 2 + v 1 × v 2 ) (s_1v_2+s_2v_2 + v_1\times v_2) (s1v2+s2v2+v1×v2)的 x x x部分
- j j j虚部: ( s 1 v 2 + s 2 v 2 + v 1 × v 2 ) (s_1v_2+s_2v_2 + v_1\times v_2) (s1v2+s2v2+v1×v2)的 y y y部分
- k k k虚部: ( s 1 v 2 + s 2 v 2 + v 1 × v 2 ) (s_1v_2+s_2v_2 + v_1\times v_2) (s1v2+s2v2+v1×v2)的 z z z部分
所以,四元数的乘法公式如下:
q
1
=
(
s
1
,
v
1
)
q
2
=
(
s
2
,
v
2
)
q
1
q
2
=
(
s
1
s
2
−
v
1
⋅
v
2
,
s
1
v
2
+
s
2
v
2
+
v
1
×
v
2
)
q_1 = (s_1,v_1)\\ q_2 = (s_2,v_2)\\ q_1q_2 = (s_1s_2-v_1\cdot v_2,s_1v_2+s_2v_2 + v_1\times v_2)
q1=(s1,v1)q2=(s2,v2)q1q2=(s1s2−v1⋅v2,s1v2+s2v2+v1×v2)
四元数的共轭
类似复数共轭的定义,四元数的共轭定义如下:
q
‾
=
(
s
,
−
v
)
\overline q = (s,-v)
q=(s,−v)
四元数的模表示为:
∣
q
∣
=
q
q
‾
=
s
2
+
v
⋅
v
|q|=\sqrt{q\overline q} = \sqrt{s^2+v\cdot v}
∣q∣=qq=s2+v⋅v
∣
q
∣
2
=
s
2
+
v
⋅
v
|q|^2 = s^2+v\cdot v
∣q∣2=s2+v⋅v
四元数的逆表示为:
q
−
1
=
(
s
,
−
v
)
∣
q
∣
2
q^{-1} = \frac{(s,-v)}{|q|^2}
q−1=∣q∣2(s,−v)
所以:
q
q
−
1
=
(
s
,
v
)
(
s
,
−
v
)
∣
q
∣
2
=
(
s
2
+
v
⋅
v
,
0
)
∣
q
∣
2
=
(
1
,
0
)
=
q
−
1
q
qq^{-1} = \frac{(s,v)(s,-v)}{|q|^2} = \frac{(s^2+v\cdot v,0)}{|q|^2} = (1,0) = q^{-1}q
qq−1=∣q∣2(s,v)(s,−v)=∣q∣2(s2+v⋅v,0)=(1,0)=q−1q
使用四元数进行旋转
使用四元数对任意轴进行旋转计算有两个好处:
- 四元数占用内存小
- 方便对旋转进行插值
第一步,定义四元数
假设我们需要计算围绕经过原点的一个轴进行旋转,旋转弧度值为
θ
\theta
θ,轴的单位向量为
u
=
(
x
0
,
y
0
,
z
0
)
u=(x_0,y_0,z_0)
u=(x0,y0,z0)
那么,我们应该这样定义四元数
q
=
(
s
,
v
)
,
s
=
c
o
s
(
θ
2
)
,
v
=
u
s
i
n
(
θ
2
)
q=(s,v),s = cos(\frac{\theta}{2}),\ v = usin(\frac{\theta}{2})
q=(s,v),s=cos(2θ), v=usin(2θ)
第二步,旋转点四元数
比如现在进行旋转的顶点坐标为
p
=
(
x
,
y
,
z
)
p=(x,y,z)
p=(x,y,z),将顶点表示成四元数形式
P
=
(
0
,
p
)
P= (0,p)
P=(0,p)
第三步,进行旋转
旋转后的四元数定义为
P
′
=
q
P
q
−
1
P^{'}= qPq^{-1}
P′=qPq−1
∵
∣
q
∣
2
=
s
2
+
v
⋅
v
\because |q|^2 = s^2+v\cdot v
∵∣q∣2=s2+v⋅v
∵
u
⋅
u
=
1
\because u\cdot u =1
∵u⋅u=1
∴
∣
q
∣
2
=
=
c
o
s
(
θ
2
)
2
+
s
i
n
(
θ
2
)
2
=
1
\therefore |q|^2 = = cos(\frac{\theta}{2})^2 +sin(\frac{\theta}{2})^2 = 1
∴∣q∣2==cos(2θ)2+sin(2θ)2=1
∴
q
−
1
=
(
s
,
−
v
)
\therefore q^{-1} = (s,-v)
∴q−1=(s,−v)
第四步,获取旋转后的坐标
再次写上前面的公式
q
1
q
2
=
(
s
1
s
2
−
v
1
⋅
v
2
,
s
1
v
2
+
s
2
v
2
+
v
1
×
v
2
)
q_1q_2 = (s_1s_2-v_1\cdot v_2,s_1v_2+s_2v_2 + v_1\times v_2)
q1q2=(s1s2−v1⋅v2,s1v2+s2v2+v1×v2)
开始计算旋转后的坐标
P
′
=
q
P
q
−
1
=
(
s
,
v
)
(
0
,
p
)
(
s
,
−
v
)
=
(
−
v
⋅
p
,
s
p
+
v
×
p
)
(
s
,
−
v
)
=
(
−
(
v
⋅
p
)
s
+
s
p
⋅
v
+
v
×
p
⋅
v
,
s
2
p
+
v
(
v
⋅
p
)
+
2
s
(
v
×
p
)
+
v
×
(
v
×
p
)
)
=
(
0
,
s
2
p
+
v
(
v
⋅
p
)
+
2
s
(
v
×
p
)
+
v
×
(
v
×
p
)
)
\begin{aligned} P^{'}&= qPq^{-1}\\ &= (s,v)(0,p)(s,-v)\\ &= (-v\cdot p,sp+v\times p)(s,-v)\\ &= (-(v\cdot p)s + sp\cdot v + v\times p\cdot v, s^2p + v(v\cdot p) + 2s(v\times p)+ v \times (v\times p))\\ &= (0,s^2p + v(v\cdot p) + 2s(v\times p)+ v \times (v\times p)) \end{aligned}
P′=qPq−1=(s,v)(0,p)(s,−v)=(−v⋅p,sp+v×p)(s,−v)=(−(v⋅p)s+sp⋅v+v×p⋅v,s2p+v(v⋅p)+2s(v×p)+v×(v×p))=(0,s2p+v(v⋅p)+2s(v×p)+v×(v×p))
所以,旋转后的顶点坐标为
s
2
p
+
v
(
v
⋅
p
)
+
2
s
(
v
×
p
)
+
v
×
(
v
×
p
)
s^2p + v(v\cdot p) + 2s(v\times p)+ v \times (v\times p)
s2p+v(v⋅p)+2s(v×p)+v×(v×p)
带入之前定义的值
假设
a
=
s
i
n
(
θ
2
)
x
0
,
b
=
s
i
n
(
θ
2
)
y
0
,
c
=
s
i
n
(
θ
2
)
z
0
a = sin(\frac{\theta}{2})x_0,b = sin(\frac{\theta}{2})y_0,c = sin(\frac{\theta}{2})z_0
a=sin(2θ)x0,b=sin(2θ)y0,c=sin(2θ)z0
s
2
p
+
v
(
v
⋅
p
)
+
2
s
(
v
×
p
)
+
v
×
(
v
×
p
)
=
(
1
−
2
b
2
−
2
c
2
2
a
b
−
2
s
c
2
a
c
+
2
s
b
2
a
b
+
2
s
c
1
−
2
a
2
−
2
c
2
2
b
c
−
2
s
a
2
a
c
−
2
s
b
2
b
c
+
2
s
a
1
−
2
a
2
−
2
b
2
)
p
=
M
r
p
\begin{aligned} &s^2p + v(v\cdot p) + 2s(v\times p)+ v \times (v\times p)\\ =& \begin{pmatrix} 1-2b^2-2c^2&2ab-2sc&2ac+2sb\\ 2ab+2sc&1-2a^2-2c^2& 2bc-2sa\\ 2ac-2sb&2bc+2sa&1-2a^2-2b^2 \end{pmatrix}p\\ =&M_rp \end{aligned}
==s2p+v(v⋅p)+2s(v×p)+v×(v×p)
1−2b2−2c22ab+2sc2ac−2sb2ab−2sc1−2a2−2c22bc+2sa2ac+2sb2bc−2sa1−2a2−2b2
pMrp
M
r
M_r
Mr矩阵即我们要使用的旋转矩阵
下面给出 M r M_r Mr矩阵计算的代码
void GetMatrix(matrix4d<float>& matrix,const vector3d<float>& rotateLine,const float angle)
{
float temp = angle * 0.5;
const float sr = sin(temp);
const float cr = cos(temp);
rotateLine = rotateLine.normalize();
X =rotateLine.X*sr;
Y =rotateLine.Y*sr;
Z =rotateLine.Z*sr;
W =cr;
matrix[0] = 1.0f - 2.0f*Y*Y - 2.0f*Z*Z;
matrix[1] = 2.0f*X*Y + 2.0f*Z*W;
matrix[2] = 2.0f*X*Z - 2.0f*Y*W;
matrix[3] = 0.0f;
matrix[4] = 2.0f*X*Y - 2.0f*Z*W;
matrix[5] = 1.0f - 2.0f*X*X - 2.0f*Z*Z;
matrix[6] = 2.0f*Z*Y + 2.0f*X*W;
matrix[7] = 0.0f;
matrix[8] = 2.0f*X*Z + 2.0f*Y*W;
matrix[9] = 2.0f*Z*Y - 2.0f*X*W;
matrix[10] = 1.0f - 2.0f*X*X - 2.0f*Y*Y;
matrix[11] = 0.0f;
matrix[12] = 0.0f;
matrix[13] = 0.0f;
matrix[14] = 0.0f;
matrix[15] = 1.f;
}