从零实现3D图像引擎：(9)四元数函数库

1. 数学分析

1) 四元数是什么东西？

这个东西算盘、矩阵、复数是一类东西，即数学工具，数学家们创造了这个东西来解决一些数学问题。其实四元数是一种超复数，他不是只有一个虚数的复数，而是有三个虚数的复数。我们先回顾一下复数吧。

 

2) 虚数的来源

实数集中没有-1的平方根，因为没有哪个实数的平方等于-1，所以数学家们就创造它——虚数i，并且定义了i * i = -1。

所以我们可以计算sqrt(-4)了，sqrt(-4) = 2*i

 

3) 复数

定义：一个实数与一个虚数的和。 z = a + bi。

a叫实部，b叫虚部。

其中(a,b)为复平面上的点。这也是为什么3D图形运算能和四元数挂上关系了。

运算法则：

z1 = a + b*i

z2 = c + d*i

 

与标量相乘 k*z1 = k*a + K*b*i

复数相加 z1 + z2 = a + c + (b + d) * i

复数相乘 z1 * z2 = (a + b * i) * (c + d * i) = a * c + a * d * i + c * b * i - b * d = a*c - b*d + (a*d+b*c) * i

复数相除 z1 / z2 = (a + b*i) / (c + d*i)

z1的共轭z1* = a - b*i，作用是z×z*为实数

所以复数相除 z1 / z2 = (a + b*i)*(c - d*i) / (c²+d²) = ... = (a*c + b*d)/(a²+b²) + (b*c-a*d)*i)/(a²+b²)

z1的倒数 1/z = 1 / (a+b*i)，同样可以转化为：a/(a²+b²) + b*i/(a²+b²)

z1的范数 |z| = sqrt(a²+b²) = sqrt(z×z*)

 

4) 超复数

q = q₀ + q₁*i + q₂*j + q₃*k

上面就是四元数的表示，其中q₀为实部，而虚部<q₁,q₂,q₃>正好组成了3D复平面中的向量。

简写就是q = q₀ + q_v，其中q_v是向量<q₁, q₂ ,q₃>。

超复数的运算法则与复数相同，这里就不再重复了，但要特别说明四元数的倒数q^-1。

q×q^-1 = 1

两边同时乘以q*：

q×q^-1×q* = q*

因为：q×q* = |q|²

所以q^-1=q* / |q|²

可以注意到，如果q是一个单位四元数的话，那么q的倒数就等于q的共轭。

这个特性非常重要，因为在四元数旋转中要使用。

 

5) 四元数旋转

对于一个3D向量<x, y, z>，我们把他表现为四元数形式，则是：v_q= <0, x, y, z>。

以及给定一个表示旋转轴和旋转角度的单位四元数q，那么向量v_q绕指定轴旋转指定角度的结果四元数v_q'满足如下：

右手坐标系：

  顺时针旋转：v_q' = q^*×v_q×q

  逆时针旋转：v_q' = q×v_q×q^*

左手坐标系：

  顺时针旋转：v_q' = q×v_q×q^*

  逆时针旋转：v_q' = q^*×v_q×q

其中，对于那个用于存储旋转轴和旋转角度的单位四元数q，他与旋转轴v = <x0, y0, z0>和角度theta关系如下：

q = Cos(theta/2) + Sin(theta/2) × v

 

 

 

2. 代码实现

1) 四元数结构体定义

typedef struct QUAT_TYPE // 四元数{union{double M[4];struct{double w, x, y, z;};struct{double q0;VECTOR3D qv;};};} QUAT, *QUAT_PTR;

 

 

2) 四元数常用操作函数实现

void _CPPYIN_Math::QuatCreate(QUAT_PTR q, VECTOR3D_PTR v, double theta) // 创建用于旋转的四元数q, v必须为单位向量{double theta_div_2 = (0.5)*theta;double sin_theta = sin(theta_div_2);q->x = sin_theta * v->x;q->y = sin_theta * v->y;q->z = sin_theta * v->z;q->w = cos( theta_div_2 );}void _CPPYIN_Math::QuatGetVectorAndTheta(QUAT_PTR q, VECTOR3D_PTR v, double *theta){*theta = acos(q->w);double sin_theta_inv = 1.0/sin(*theta);v->x = q->x * sin_theta_inv;v->y = q->y * sin_theta_inv;v->z = q->z * sin_theta_inv;*theta *= 2;}void _CPPYIN_Math::QuatAdd(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qsum){qsum->x = q1->x + q2->x;qsum->y = q1->y + q2->y;qsum->z = q1->z + q2->z;qsum->w = q1->w + q2->w;}void _CPPYIN_Math::QuatSub(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qdiff){qdiff->x = q1->x - q2->x;qdiff->y = q1->y - q2->y;qdiff->z = q1->z - q2->z;qdiff->w = q1->w - q2->w;}void _CPPYIN_Math::QuatConjugate(QUAT_PTR q, QUAT_PTR qconj) // 求共轭{qconj->x = -q->x;qconj->y = -q->y;qconj->z = -q->z;qconj->w = q->w;}void _CPPYIN_Math::QuatScale(QUAT_PTR q, double scale, QUAT_PTR qs){qs->x = scale * q->x;qs->y = scale * q->y;qs->z = scale * q->z;qs->w = scale * q->w;}double _CPPYIN_Math::QuatNorm(QUAT_PTR q){return sqrt(q->w * q->w + q->x * q->x + q->y * q->y + q->z * q->z);}double _CPPYIN_Math::QuatNorm2(QUAT_PTR q){return q->w * q->w + q->x * q->x + q->y * q->y + q->z * q->z;}void _CPPYIN_Math::QuatNormalize(QUAT_PTR q, QUAT_PTR qn){double qlength_inv = 1.0/(sqrt(q->w*q->w + q->x*q->x + q->y*q->y + q->z*q->z));qn->w = q->w * qlength_inv;qn->x = q->x * qlength_inv;qn->y = q->y * qlength_inv;qn->z = q->z * qlength_inv;}void _CPPYIN_Math::QuatUnitInverse(QUAT_PTR q, QUAT_PTR qi) // 单位四元数的逆，等于求共轭{qi->w = q->w;qi->x = -q->x;qi->y = -q->y;qi->z = -q->z;}void _CPPYIN_Math::QuatInverse(QUAT_PTR q, QUAT_PTR qi) // 非单位四元数的逆{double norm2_inv = 1.0 / (q->w * q->w + q->x * q->x + q->y * q->y + q->z * q->z);qi->w = q->w * norm2_inv;qi->x = -q->x * norm2_inv;qi->y = -q->y * norm2_inv;qi->z = -q->z * norm2_inv;}void _CPPYIN_Math::QuatMul(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR qprod){double prd_0 = (q1->z - q1->y) * (q2->y - q2->z);double prd_1 = (q1->w + q1->x) * (q2->w + q2->x);double prd_2 = (q1->w - q1->x) * (q2->y + q2->z);double prd_3 = (q1->y + q1->z) * (q2->w - q2->x);double prd_4 = (q1->z - q1->x) * (q2->x - q2->y);double prd_5 = (q1->z + q1->x) * (q2->x + q2->y);double prd_6 = (q1->w + q1->y) * (q2->w - q2->z);double prd_7 = (q1->w - q1->y) * (q2->w + q2->z);double prd_8 = prd_5 + prd_6 + prd_7;double prd_9 = 0.5 * (prd_4 + prd_8);qprod->w = prd_0 + prd_9 - prd_5;qprod->x = prd_1 + prd_9 - prd_8;qprod->y = prd_2 + prd_9 - prd_7;qprod->z = prd_3 + prd_9 - prd_6;}void _CPPYIN_Math::QuatMul3(QUAT_PTR q1, QUAT_PTR q2, QUAT_PTR q3, QUAT_PTR qprod) // 三个四元数相乘，用于做旋转变换{QUAT qtmp;QuatMul(q1, q2, &qtmp);QuatMul(&qtmp, q3, qprod);}

 

 

 

3. 代码下载

完整项目代码下载：>>点击进入下载页<<