旋转变换（三）四元数
四元数是另一种描述三维旋转的方式，四元数使用4个分量来描述旋转，四元数的描述方式如下：
q=s+xi+yj+zk,(s,x,y,z∈R）i2=j2=k2=ijk=−1q=s+xi+yj+zk,(s,x,y,z∈R）i2=j2=k2=ijk=−1
四元数的由来和复数很很大的关系，因此首先讨论一下关于复数的内容。
1.1 复数
复数可以定义如下：
z=a+bia,b∈Ri2=−1z=a+bia,b∈Ri2=−1
复数常用的基本运算如下：
复数中一个比较重要的概念是共轭复数，将复数的虚部取相反数，得到它的共轭复数：
z=a+biz∗=a−biz=a+biz∗=a−bi
复数的模，定义为：
复数还可以使用复平面来表示，复平面分为实轴和虚轴（类似于二维直角坐标系中的x轴和y轴），如下图所示：
当我们使用i去乘以一个复数时，当我们把得到的结果绘制在复平面上时，发现得到的位置正好是绕原点旋转90度的效果。
于是可以猜测，复数的乘法和旋转之间应该有某些关系。
我们可以通过定义一个复数
q=cosθ+isinθq=cosθ+isinθ
角之后的位置。
2. 四元数
既然使用复数的乘法可以描述二维的旋转，那么拓展一个维度是否能表示三维旋转呢，这个也正是四元数发明者William Hamilton最初的想法，也就是说使用
z=a+ib+jci2=j2=−1z=a+ib+jci2=j2=−1
但是很遗憾 “三维的复数”（这仅仅是我按概念杜撰的一个词，并不存在）的乘法并不是闭合的。也就是说有可能两个值相乘得到的结果并不是三维的复数。
William Hamilton经历了无数个日日夜夜，他绞尽脑汁也没想明白这个问题。终于有一天（1843年的一天），他意识到自己所需要的运算在三维空间中是不可能实现的，但在四维空间中是可以的，他是如此的兴奋，以至于把四元数的公式刻在了爱尔兰的一座桥上。
四元数可以写成下面的方式：
q=[s,v]s∈Rv∈R3q=[s,v]s∈Rv∈R3
2.1 四元数的运算
- 两四元数相加：
A（a+bi+cj+dk） + B(e + fi + gj + hk) = C【 (a+e) + (b+f)i + (c+g)j + (d+h)k 】，实现代码：
// Quat的成员_v[4]代表四元数的（x,y,z,w)
inline Quat operator + (const Quat& rhs) const
{
return Quat(
_v[0] + rhs._v[0],
_v[1] + rhs._v[1],
_v[2] + rhs._v[2],
_v[3] + rhs._v[3]
);
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 两个四元数相减
(sa,va) - (sb,vb) = (sa-sb,va-vb)
inline Quat operator - (const Quat& rhs) const
{
return Quat
(
_v[0] - rhs._v[0],
_v[1] - rhs._v[1],
_v[2] - rhs._v[2],
_v[3] - rhs._v[3]
);
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 两个四元数相乘
两个四元数相乘的规则和多项式乘法一样，
(a + i b + j c + k d)*(e + i f + j g + k h)
当有i，j，k参与时，规则如下：
i*i = j*j = k*k = -1
i*j = k,
j*i = -k
j*k = i,
k*j = -i
k*i = j,
i*k = -j
使用多项式乘法展开，可以得到：
a*e - b*f - c*g - d*h
+ i (b*e + a*f + c*h- d*g)
+ j (a*g - b*h+ c*e + d*f)
+ k (a*h + b*g - c*f + d*e)
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
实现代码：
inline const Quat operator*(const Quat& rhs) const
{
return Quat( rhs._v[3]*_v[0] + rhs._v[0]*_v[3] + rhs._v[1]*_v[2] - rhs._v[2]*_v[1],
rhs._v[3]*_v[1] - rhs._v[0]*_v[2] + rhs._v[1]*_v[3] + rhs._v[2]*_v[0],
rhs._v[3]*_v[2] + rhs._v[0]*_v[1] - rhs._v[1]*_v[0] + rhs._v[2]*_v[3],
rhs._v[3]*_v[3] - rhs._v[0]*_v[0] - rhs._v[1]*_v[1] - rhs._v[2]*_v[2] );
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 两四元数相除
四元数一般来说不定义除法，因为四元数的乘法运算并不满足交换律。一般有四元数的类定义除法是，其实定义的是q1∗q2−1q1∗q2−1，这个结论很容易推导出来。conj(q)称为q的共轭表达式，
con(q) = w - xi - yj -zk，只需要四元数向量部分取负即可
实现如下：
inline const Quat operator/(const Quat& denom) const
{
return ( (*this) * denom.inverse() );
}
/// Conjugate
inline Quat conj () const
{
return Quat( -_v[0], -_v[1], -_v[2], _v[3] );
}
/// Multiplicative inverse method: q^(-1) = q^*/(q.q^*)
inline const Quat inverse () const
{
return conj() / length2();
}
value_type length2() const
{
return _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] + _v[3]*_v[3];
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
2.2 四元数的旋转
四元数在三维图形学领域的一个重要应用是用它来描述三维旋转，四元数从某种意义上来说是四维空间的旋转，难以想象，了解它的结论和使用场景更加重要。欧拉定理告诉我们任意三维旋转都可以使用一个旋转向量和旋转角度来描述。因此四元数往往是使用旋转轴和旋转角来构造的，构造它的方法如下：
2.2.1 绕向量u旋转角度θθ构造四元数
可以用下面的四元数来表示：
u⃗ =(ux,uy,uz)=uxi+uyj+uzku→=(ux,uy,uz)=uxi+uyj+uzk
实现代码如下：
void makeRotate(value_type angle, value_type x, value_type y, value_type z)
{
const value_type epsilon = 1e-7;
value_type length = sqrt(x*x + y*y + z*z);
if (length < epsilon)
{
*this = Quat();
return;
}
value_type inversenorm = 1.0 / length;
value_type coshalfangle = cos(0.5*angle);
value_type sinhalfangle = sin(0.5*angle);
_v[0] = x * sinhalfangle * inversenorm;
_v[1] = y * sinhalfangle * inversenorm;
_v[2] = z * sinhalfangle * inversenorm;
_v[3] = coshalfangle;
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
四元数是另一种描述三维旋转的方式，四元数使用4个分量来描述旋转，四元数的描述方式如下：
q=s+xi+yj+zk,(s,x,y,z∈R）i2=j2=k2=ijk=−1q=s+xi+yj+zk,(s,x,y,z∈R）i2=j2=k2=ijk=−1
四元数的由来和复数很很大的关系，因此首先讨论一下关于复数的内容。
1.1 复数
复数可以定义如下：
z=a+bia,b∈Ri2=−1z=a+bia,b∈Ri2=−1
复数常用的基本运算如下：
复数中一个比较重要的概念是共轭复数，将复数的虚部取相反数，得到它的共轭复数：
z=a+biz∗=a−biz=a+biz∗=a−bi
复数的模，定义为：
复数还可以使用复平面来表示，复平面分为实轴和虚轴（类似于二维直角坐标系中的x轴和y轴），如下图所示：
当我们使用i去乘以一个复数时，当我们把得到的结果绘制在复平面上时，发现得到的位置正好是绕原点旋转90度的效果。
于是可以猜测，复数的乘法和旋转之间应该有某些关系。
我们可以通过定义一个复数
q=cosθ+isinθq=cosθ+isinθ
角之后的位置。
2. 四元数
既然使用复数的乘法可以描述二维的旋转，那么拓展一个维度是否能表示三维旋转呢，这个也正是四元数发明者William Hamilton最初的想法，也就是说使用
z=a+ib+jci2=j2=−1z=a+ib+jci2=j2=−1
但是很遗憾 “三维的复数”（这仅仅是我按概念杜撰的一个词，并不存在）的乘法并不是闭合的。也就是说有可能两个值相乘得到的结果并不是三维的复数。
William Hamilton经历了无数个日日夜夜，他绞尽脑汁也没想明白这个问题。终于有一天（1843年的一天），他意识到自己所需要的运算在三维空间中是不可能实现的，但在四维空间中是可以的，他是如此的兴奋，以至于把四元数的公式刻在了爱尔兰的一座桥上。
四元数可以写成下面的方式：
q=[s,v]s∈Rv∈R3q=[s,v]s∈Rv∈R3
2.1 四元数的运算
- 两四元数相加：
A（a+bi+cj+dk） + B(e + fi + gj + hk) = C【 (a+e) + (b+f)i + (c+g)j + (d+h)k 】，实现代码：
// Quat的成员_v[4]代表四元数的（x,y,z,w)
inline Quat operator + (const Quat& rhs) const
{
return Quat(
_v[0] + rhs._v[0],
_v[1] + rhs._v[1],
_v[2] + rhs._v[2],
_v[3] + rhs._v[3]
);
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 两个四元数相减
(sa,va) - (sb,vb) = (sa-sb,va-vb)
inline Quat operator - (const Quat& rhs) const
{
return Quat
(
_v[0] - rhs._v[0],
_v[1] - rhs._v[1],
_v[2] - rhs._v[2],
_v[3] - rhs._v[3]
);
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 两个四元数相乘
两个四元数相乘的规则和多项式乘法一样，
(a + i b + j c + k d)*(e + i f + j g + k h)
当有i，j，k参与时，规则如下：
i*i = j*j = k*k = -1
i*j = k,
j*i = -k
j*k = i,
k*j = -i
k*i = j,
i*k = -j
使用多项式乘法展开，可以得到：
a*e - b*f - c*g - d*h
+ i (b*e + a*f + c*h- d*g)
+ j (a*g - b*h+ c*e + d*f)
+ k (a*h + b*g - c*f + d*e)
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
实现代码：
inline const Quat operator*(const Quat& rhs) const
{
return Quat( rhs._v[3]*_v[0] + rhs._v[0]*_v[3] + rhs._v[1]*_v[2] - rhs._v[2]*_v[1],
rhs._v[3]*_v[1] - rhs._v[0]*_v[2] + rhs._v[1]*_v[3] + rhs._v[2]*_v[0],
rhs._v[3]*_v[2] + rhs._v[0]*_v[1] - rhs._v[1]*_v[0] + rhs._v[2]*_v[3],
rhs._v[3]*_v[3] - rhs._v[0]*_v[0] - rhs._v[1]*_v[1] - rhs._v[2]*_v[2] );
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 两四元数相除
四元数一般来说不定义除法，因为四元数的乘法运算并不满足交换律。一般有四元数的类定义除法是，其实定义的是q1∗q2−1q1∗q2−1，这个结论很容易推导出来。conj(q)称为q的共轭表达式，
con(q) = w - xi - yj -zk，只需要四元数向量部分取负即可
实现如下：
inline const Quat operator/(const Quat& denom) const
{
return ( (*this) * denom.inverse() );
}
/// Conjugate
inline Quat conj () const
{
return Quat( -_v[0], -_v[1], -_v[2], _v[3] );
}
/// Multiplicative inverse method: q^(-1) = q^*/(q.q^*)
inline const Quat inverse () const
{
return conj() / length2();
}
value_type length2() const
{
return _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] + _v[3]*_v[3];
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
2.2 四元数的旋转
四元数在三维图形学领域的一个重要应用是用它来描述三维旋转，四元数从某种意义上来说是四维空间的旋转，难以想象，了解它的结论和使用场景更加重要。欧拉定理告诉我们任意三维旋转都可以使用一个旋转向量和旋转角度来描述。因此四元数往往是使用旋转轴和旋转角来构造的，构造它的方法如下：
2.2.1 绕向量u旋转角度θθ构造四元数
可以用下面的四元数来表示：
u⃗ =(ux,uy,uz)=uxi+uyj+uzku→=(ux,uy,uz)=uxi+uyj+uzk
实现代码如下：
void makeRotate(value_type angle, value_type x, value_type y, value_type z)
{
const value_type epsilon = 1e-7;
value_type length = sqrt(x*x + y*y + z*z);
if (length < epsilon)
{
*this = Quat();
return;
}
value_type inversenorm = 1.0 / length;
value_type coshalfangle = cos(0.5*angle);
value_type sinhalfangle = sin(0.5*angle);
_v[0] = x * sinhalfangle * inversenorm;
_v[1] = y * sinhalfangle * inversenorm;
_v[2] = z * sinhalfangle * inversenorm;
_v[3] = coshalfangle;
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
2.2.2 从一个向量旋转到另一个向量构造四元数
按照最原始的想法，从一个向量旋转到另一个向量，那么旋转轴可以通过两个向量的叉乘得到，旋转角度可以通过两个向量间的夹角得到。（向量间的夹角的余弦可以通过两向量点乘去除以它们的模，再通过反余弦函数计算），得到旋转轴和旋转角度之后就转换成2.2.1中的情形了。
也就是说最初的代码如下：
void makeRotate(Vec3& u, Vec3& v)
{
u.normalize();
v.normalize();
double costheta = u*v;
double angle = acos(costheta);
Vec3 w = u^v;
w.normalize();
makeRotate(angle, w.x(), w.y(), w.z());
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
有一种特殊情况需要考虑：两向量共线（包括方向相同和方向相反，也就是夹角是0度和180度的情形）
void makeRotate(const Vec3& from, const Vec3& to)
{
const value_type epsilon = 1e-7;
value_type length1 = from.length();
value_type length2 = to.length();
value_type cosangle = from*to / (length1*length2);
if (fabs(cosangle - 1) < epsilon)
{
makeRotate(0.0, 0.0, 0.0, 1.0);
}
else if (fabs(cosangle + 1.0) < epsilon)
{
Vec3 tmp;
if ((fabs(from.x())) < fabs(from.y()))
{
if (fabs(from.x()) < fabs(from.z()))
{
tmp.set(1.0, 0.0, 0.0);
}
else
{
tmp.set(0.0, 0.0, 1.0);
}
}
else if (fabs(from.y()) < fabs(from.z()))
{
tmp.set(0.0, 1.0, 0.0);
}
else
{
tmp.set(0.0, 0.0, 1.0);
}
Vec3 fromd(from.x(), from.y(), from.z());
Vec3 axis(fromd^tmp);
axis.normalize();
_v[0] = axis[0];
_v[1] = axis[1];
_v[2] = axis[2];
_v[3] = 0.0;
}
else
{
Vec3 axis(from^to);
value_type angle = acos(cosangle);
makeRotate(angle, axis.x(), axis.y(), axis.z());
}
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
上述的代码改进了之前代码，但是在计算过程中使用了反三角函数（相对比较耗时），可以通过三角函数公式，简化，不需要调用反三角函数：
sinθ2cosθ2=1−cosθ2−−−−−−−−√=1+cosθ2−−−−−−−−√sinθ2=1−cosθ2cosθ2=1+cosθ2
代码可以修改为：
//省略部分相同的代码
else
{
Vec3 axis(from^to);
//替换成下面几行
//value_type angle = acos(cosangle);
//makeRotate(angle, axis.x(), axis.y(), axis.z());
axis.normalize();
value_type half_cos = sqrt(0.5*(1+cosangle));
value_type half_sin = sqrt(0.5*(1-cosangle));
_v[0] = axis[0] * half_sin;
_v[1] = axis[1] * half_sin;
_v[2] = axis[2] * half_sin;
_v[3] = half_cos;
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
这样修改之后，去掉了算法中复杂的三角函数运算，事实上还可以进一步改进计算过程，考虑到代码中多次的归一化（normalize）的操作，需要进行多次开方运算，为了简化，可以考虑：
||u