四元数与复数：
在搞清楚四元数之前首先要知道什么是复数以及复数的运算，详情:http://www.cnblogs.com/ThreeThousandBigWorld/archive/2012/07/21/2602588.html
四元数是对复数的扩充，它使用三个虚部i,j,k它们的关系如下:
i² = j² = k² = –1
四元数形式:
[w v]或[ w (x y z) ].
四元数定义了一个复数
w + xi + yj + zk
四元数只能用来代替矩阵保存旋转信息，平移无法代替，旋转我们有 矩阵，欧拉角，四元数，
四元数是里面最复杂的一个但它可以在保证效率的同时减小矩阵1/4的内存占有量，同时又能避免欧拉角的万向锁问题，
无法用语言表达清楚万向锁的现象只有碰到过才能理解。。。
四元数和轴——角对:
设O为旋转角度，N为旋转轴(绕任意轴n旋转O度):
Q = [cos(O/2) sin(O/2)N] = [ cos(O/2) ( sin(O/2)Nx sin(O/2)Ny sin(O/2)Nz ) ]
负四元数:
-Q = –[w (x y z)] = [-w (-x -y -z)] = –[w v] = [-w -v]
Q和-Q代表的是相同的角位移，如果我们将O加上360°的倍数，不会改变Q代表的角位移，但它使Q的四个分量都变负了。
因此，3D中的任意角位移都有两种不同的四元数表示方法，它们互为负数。
单位四元数:
几何上存在两个“单位”四元数，它们没有角位移， [1, 0]和[-1,0] 0代表0向量，让我们看看原因：
当O为360°的偶数倍时，有第一种形式，cos(O/2) = 1;
当O为360°的奇数倍时，有第二种形式，cos(0/2) =-1;
在这两种形势下都有sin(O/2) = 0,所以和旋转轴N是无关的,他的意义是，当旋转角O是360°的整数倍时，方位没有改变，并且旋转轴也是关紧要的.
注意:
数学上只有一个单位四元数[1, 0]，用四元数q乘以单位四元数[1,0]，结果认识q。任意四元数q乘以另一个“几何单位”四元数[-1,0]时得到-q。几何上，因为q和-q代表的角位移相同，可
认为结果是相同。但在数学上，q和-q不相等，所以[-1,0]斌不是“真正”的单位四元数。
四元数的模:
公式:
|| q || = || [w (x y z)] || = √(w² + x² + y² + z²)
所以
|| q || = √( cos²(O/2) + ( sin(O/2) ||n||)² )
当n为单位向量时:
= √( cos²(O/2) + sin²(O/2) * 1 )
应用三角形公式 sin²(x) + cos²(x) = 1
= √( cos²(O/2) + sin²(O/2) )
= √1 = 1
如果为了用四元数来表示方位，我们仅仅使用符合这个规则的单位四元数。非单位四元数要查阅其他资料
四元数共轨和逆:
共轨:
四元数共轨记做 q*可以通过让四元数向量部分变负来获得:
q* = [w v]* = [w -v]
= [w (-x –y -z)]
逆:
用q^-1 表示逆
q^-1 = q* / || q ||
四元数乘以自己的逆等于“单位四元数”[1 0]
如果我们使用单位四元数是单位四元数那么就可以得到 q^-1 = q*. 所以单位四元数的共轨等于他的逆
四元数叉乘:
四元数乘法的标准定义:
[W1 V1] *[W2 V2]叉乘后还是四元数
(W1+X1i + Y1j + Z1k)(W2 + X2i + y2j + Z2K)
= [W1W2 - V1·V2 W1V2+W2V1+V2*V1]
叉乘满足结合律，但不满足交换律.
(a*b)*c = a*(b*c)
ab ≠ ba
叉乘的模:
||Q1 * Q2|| = ||Q1|| * ||Q2||
这个结论保证了两个单位四元数相乘还是单位四元数.
四元数逆的乘积:
(a*b)^1 = b^-1 * a^-1
四元数和点：
把一个标准3D点(x,y,z)扩展到四元数空间:P = [0, (x,y,z)]即可，一般情况下它不会是单位四元数
设Q为旋转四元数Q=[cos(O/2),nsin(O/2)],n为单位向量，O为旋转角度,那么P绕n旋转O度就是：
P` = Q * P * Q^-1
四元数的乘法和3D向量旋转的对应关系更多是理论上的，实际上，它几乎和把四元数转换成矩阵形式然后在用矩阵乘以向量时间是一样的.
多次旋转:
P` = B * (A * P * A^-1)B^-1
= (B * A) * P * (A * B)^-1
这个形式是标准定义但我们不经常使用因为不好看,下面是经常使用的这个影响到了叉乘本身:
红色部分是改变后所影响的
[W1 V1] *[W2 V2]
(W1+X1i + Y1j + Z1k)(W2 + X2i + y2j + Z2K)
= [W1W2 - V1·V2 W1V2+W2V1+V2*V1]
相应的 四元数与3D向量的对应关系也随着改变:
P` = Q^-1 * P * Q
p` = B^-1 * (A^-1 * P * A) * B
= (A * B)^-1 * P * (A * B)
四元数的差:
我们看：
A * D = B
两边都乘以 A^-1
A^-1 * A = A^-1 * B
因为 A^-1 * A = 1
所以 D = A^-1 * B
这就所谓的 “差”,被定义为一个方位到另一个方位的角位移,顺序是不能错的,从左向右.
四元数的点乘:
点乘那个很简单: Q1 · Q2 = [w1 v1] ·[w2 v2] = w1 · w2 + v1 · v2
点乘结是一个标量这个和向量点乘一样,对于单位四元数 a 和 b 有-1≤ a · b ≤1。
因为 a·b = –( a · –b)，多以b和-b代表的角位移是一样的。
几何解释，四元数点乘a · b的绝对值越大，a 和 b代表的角位移越相似，这一点和 向量点乘类似.
四元数的对数，指数，标量乘运算:
对数：
指数:
标量相乘:
Kq = k[w v] = [kw kv]
四元数求冥:
四元数差值:
球面差值—— Slerp:
样条——Squad:
Slerp提供了两个方位间的插值，当有多于两个的方位序列（它描述了我们想要经过的插值“路径”）时怎么办？我们可以在”控制点“之间使用slerp。类似于基本几何学中的线性插值，控制点之间是以直线连接的。显然，控制点上会有不连续性 ---- 这是我们想要避免的，我们给出squad(Spherical and Quadrangle)的公式，用来描绘控制点间的路径。
设控制点由四元数序列所定义：
q1，q2，q3，…qn-2，qn-1，qn
另外，引进一个辅助四元数si，将它作为临时控制点：
注意，qi-1至qi+1计算出si，所以s1和sn是未定义的。换句话说，曲线从q2延伸到qn-1，第一个和最后一个控制点仅用于控制中间的曲线。如果曲线一定要经过这两点，必须在头部和尾部增加虚控制点，一个显而易见的方法就是复制这两个控制点。
给定四个相邻的控制点，squad用于计算中间两点间的插值，这点非常像三次样条。
设四个控制点为： qi-1，qi，qi+1，qi+2
还要引入一个插值变量h，h从0变化到1时，squad描绘qi到qi+1之间的曲线。
整条插值曲线能够分段应用squad方法来获得，如公式10.12所示：
表达时间的转换:
从欧拉角到矩阵:
欧拉角描述了一个旋转序列。分别计算出每个旋转的矩阵再将它们连接成一个矩阵，这个矩阵就代表了整个角位移。当然，它和我们是想要物体到惯性坐标的变换矩阵还是惯性到物体坐标的变换矩阵是相关的。
我们对欧拉角的定义是一个旋转序列，该旋转序列将物体（和它的坐标空间）从惯性坐标空间转换到物体坐标空间。因此，可以用欧拉角定义的直接转换来直接产生惯性 ---- 物体旋转矩阵的一般形式：
M惯性 --> 物体 = HPB
H、P、B分别为heading、pitch、bank的旋转矩阵，它们分别绕y、x、z轴旋转，仅仅旋转"坐标空间"就是旋转"点"的严格相反操作。可以想象这些旋转发生时点是固定在空间中不变的，例如，pitch使坐标空间向下，点实际上关于坐标空间向上。欧拉角公式明确指明是物体和它的坐标空间旋转，但我们需要的是变换"点"的矩阵，所以计算矩阵H、P、B时，用相反的旋转量来旋转。设heading、pitch、bank的旋转角分别为变量h、p、b:
以适当的顺序连接这些矩阵得到公式10.21：
如果要从物体坐标空间变换到惯性坐标空间，应该使用惯性----物体旋转矩阵的逆。因为旋转矩阵是正交的，所以求它的逆就是求它的转置，下面验证这一点。
为了从物体坐标空间变换到惯性坐标空间，顺序应该为"un-bank"、"un-pitch"、"un-heading"，公式表示为：
M物体->惯性 = （M惯性->物体）-1 = （HPB）-1 = B-1P-1H-1
注意，可以认为旋转矩阵B-1、P-1、H-1为它们对应矩阵的逆，或者是使用相反旋转角b、p、h的一般旋转矩阵。（惯性 --- 物体矩阵中，使用负的旋转角，因此这里的角不用变负。）
以适当的顺序连接这些矩阵得到公式10.22:
比较公式10.21和公式10.22，可以看到物体---惯性矩阵确实惯性---物体矩阵的转置。
从矩阵到欧拉角:
将角位移从矩阵形式转换到欧拉角需要考虑以下几点：
（1）必须清楚矩阵代表什么旋转：物体 -- 惯性还是 惯性 -- 物体，这里讨论使用惯性 -- 物体矩阵的技术，物体 -- 惯性矩阵转换成欧拉角的过程与之类似。
（2）对任意给定角位移，存在无穷多个欧拉角可以用来表示它。因为"别名"问题，这里讨论的技术总是返回"限制欧拉角"，heading和bank的范围±180°，pitch的范围为±90°。
（3）矩阵可能是病态的，我们必须忍受浮点数精度的误差。有些矩阵还包括除旋转外的变换，如缩放、镜像等。这里只讨论工作在旋转矩阵上的变换。
考虑这些因素后，我们尝试从公式10.21直接解得欧拉角：
If cos p = 0, then we cannot use the above trick since all the matrix elements involved are zero.
However, notice that when cos p = 0, then p = 90°, which means we are either looking straight
up or straight down. This is the Gimbal lock situation, where heading and bank effectively rotate
about the same physical axis (the vertical axis). In this case, we will arbitrarily assign all rotation
about the vertical axis to heading and set bank equal to zero. Now we know the value of pitch and
bank, and all we have left is to solve for heading. Armed with the following simplifying
assumptions:
cosp = 0
b = 0
sinb = 0
cosb = 1
将它们代入公式10.21:
现在，能够通过-m31和m11计算h，它们分别包含了h的sin和cos值。
让我们看看使用上面的技术从惯性 ---- 物体旋转矩阵中抽取欧拉角的代码，为了使示例简单，假设输入输出为全局变量。
//Listing 10.3: Extracting Euler angles from an inertial-to-object rotation matrix
// Assume the matrix is stored in these variables:
float m11,m12,m13;
float m21,m22,m23;
float m31,m32,m33;
// We will compute the Euler angle values in radians and store them here:
float h,p,b;
// Extract pitch from m23, being careful for domain errors with asin(). We could have
// values slightly out of range due to floating point arithmetic.
float sp = –m23;
if (sp <= –1.0f) {
p = –1.570796f; // –pi/2
} else if (sp >= 1.0) {
p = 1.570796; // pi/2
} else {
p = asin(sp);
}
// Check for the Gimbal lock case, giving a slight tolerance
// for numerical imprecision
if (sp > 0.9999f) {
// We are looking straight up or down.
// Slam bank to zero and just set heading
b = 0.0f;
h = atan2(–m31, m11);
} else {
// Compute heading from m13 and m33
h = atan2(m13, m33);
// Compute bank from m21 and m22
b = atan2(m21, m22);
}从四元数到矩阵:
为了将角位移从四元数转换到矩阵形式，可以利用旋转矩阵，它能计算绕任意轴的旋转：
这个矩阵是用n和θ表示的，但四元数的分量是：
w = cos(θ/2)
x = nx sin(θ/2)
y = ny sin(θ/2)
z = nz sin(θ/2)
让我们看看能否将矩阵变形以代入w、x、y、z，矩阵的9个元素都必须这样做。幸运的是，这个矩阵的结构非常好，一旦解出对角线上的一个元素，其他元素就能用同样的方法求出。同样，非对角线元素之间也是彼此类似的。
考虑矩阵对角线上的元素，我们将完整地解出m11，m22和m33解法与之类似：
m11 = nx2(1 - cosθ) + cosθ
我们将从上式的变形开始，变形方法看起来像是在绕圈子，但你马上就能理解这样做的目的：
现在需要消去cosθ项，而代之以包含cosθ/2或sinθ/2的项，因为四元数的元素都是用它们表示的，像以前那样，设α=θ/2，先用α写出cos的倍角公式，再代入θ:
上式是正确的，但它和其他的标准形式不同，即:
m11 = 1 - 2y2 - 2z2
实际上，还有其他的形式存在。最著名的一个形式是m11 = w2 + x2 - y2- z2，因为w2 + x2 + y2 + z2 = 1，所以这三种形式是等价的。现在回过头来看看能不能直接导出其他标准形式，第一步，n是单位向量，nx2+ny2 + nz2 = 1，则1 - nx2 = ny2 + nz2。
m11 = 1 - (1 - nx2)(2sin2(θ/2))
= 1 - (ny2 +nz2)(2sin2(θ/2))
= 1 - 2ny2sin2(θ/2) - 2nz2sin2(θ/2)
= 1 - 2y2 - 2z2
元素m22和m33可以用同样的方法求得。
让我们来看看非对角线元素，它们比对角线元素简单一些，以m12为例子：
m12 = nxny(1 - cosθ) + nzsinθ
其他非对角线元素可用同样的方法导出。
最后，给出从四元素构造的完整旋转矩阵，如公式10.23所示：
从矩阵到四元数:
为了从旋转矩阵中抽出相应的四元数，可以直接利用公式 10.23，检查对角线元素的和（也称作矩阵的轨迹）得到：
通过使轨迹中三个元素中的两个为负，可以用类似的方法求得其他三个元素:
不幸的是，这种方法并不总是能正确工作，因为平方根的结果总是正值。（更加准确地说，没有选择正根还是负根的依据。）但是，q和-q代表相同的方位，我们能任意选择用非负根作为4个分量中的一个并仍能得到正确的四元数，只是不能对四元数的所有4个数都用这种方法。
另一个技巧是检查相对于对角线的对称位置上元素的和与差：
那么应选用四种方法中的哪个呢？似乎最简单的策略是总是先计算同一个分量，如w，然后再计算x、y、z。这伴随着问题，如果w=0，除法就没有定义；如果w非常小，将会出现数值不稳定。Shoemake建议先判断w、x、y、z中哪个最大（不用做平方根运算），根据上面的表，用矩阵对角线计算该元素，再用它计算其他三个。
下面的代码用一种非常直接的方式实现了这个方法。
// Listing 10.4: Converting a rotation matrix to a quaternion
// Input matrix:
float m11,m12,m13;
float m21,m22,m23;
float m31,m32,m33;
// Output quaternion
float w,x,y,z;
// Determine which of w, x, y, or z has the largest absolute value
float fourWSquaredMinus1 = m11 + m22 + m33;
float fourXSquaredMinus1 = m11 – m22 – m33;
float fourYSquaredMinus1 = m22 – m11 – m33;
float fourZSquaredMinus1 = m33 – m11 – m22;
int biggestIndex = 0;
float fourBiggestSquaredMinus1 = fourWSquaredMinus1;
if (fourXSquaredMinus1 > fourBiggestSquaredMinus1) {
fourBiggestSquaredMinus1 = fourXSquaredMinus1;
biggestIndex = 1;
}
if (fourYSquaredMinus1 > fourBiggestSquaredMinus1) {
fourBiggestSquaredMinus1 = fourYSquaredMinus1;
biggestIndex = 2;
}
if (fourZSquaredMinus1 > fourBiggestSquaredMinus1) {
fourBiggestSquaredMinus1 = fourZSquaredMinus1;
biggestIndex = 3;
}
// Perform square root and division
float biggestVal = sqrt(fourBiggestSquaredMinus1 + 1.0f) * 0.5f;
float mult = 0.25f / biggestVal;
// Apply table to compute quaternion values
switch (biggestIndex) {
case 0:
w = biggestVal;
x = (m23 – m32) * mult;
y = (m31 – m13) * mult;
z = (m12 – m21) * mult;
break;
case 1:
x = biggestVal;
w = (m23 – m32) * mult;
y = (m12 + m21) * mult;
z = (m31 + m13) * mult;
break;
case 2:
y = biggestVal;
w = (m31 – m13) * mult;
x = (m12 + m21) * mult;
z = (m23 + m32) * mult;
break;
case 3:
z = biggestVal;
w = (m12 – m21) * mult;
x = (m31 + m13) * mult;
y = (m23 + m32) * mult;
break;
} 从欧拉角到四元数:
为了将角位移从欧拉角转换到四元数，可以使用从欧拉角构造矩阵类似的方法。先将这三个旋转分别转换为四元数，这是一个简单的运算。再将这三个四元数连接成一个四元数。和矩阵一样，有两种情况需要考虑，第一种是惯性 -- 物体四元数，第二种是物体-- 惯性四元数。因为它们互为共轭关系，所以我们只推导惯性--物体四元数。
设欧拉角为变量h、p、b，设h、p、b分别绕轴y、x、z旋转的四元数。记住，使用负旋转量，因为它们指定坐标系中的旋转角度。
用正确的顺序连接它们得到公式10.24：
（记住，四元数乘法定义是按旋转的顺序从左向右乘。）
物体--惯性四元数是惯性--物体四元数的共轭，见公式10.25：
从四元数到欧拉角:
根据前面的公式发现：
现在可以将它直接转换到代码中，如程序清单10.5所示，它能把惯性--物体四元数转换成欧拉角。
//Listing 10.5: Converting an inertial-to-object quaternion to Euler angles // Use global variables for input and output
float w,x,y,z;
float h,p,b;
// Extract sin(pitch)
float sp = –2.0f * (y*z + w*x);
// Check for Gimbal lock, giving slight tolerance for numerical imprecision
if (fabs(sp) > 0.9999f) {
// Looking straight up or down
p = 1.570796f * sp; // pi/2
// Compute heading, slam bank to zero
h = atan2(–x*z – w*y, 0.5f – y*y – z*z);
b = 0.0f;
} else {
// Compute angles
p = asin(sp);
h = atan2(x*z – w*y, 0.5f – x*x – y*y);
b = atan2(x*y – w*z, 0.5f – x*x – z*z);
}
// --惯性四元数转换到欧拉角，所用的代码和上面非常类似。只是将x、y、z值变负，因为物体--惯性四元数是惯性--物体四元数的共轭。
//Listing 10.6: Converting an object-to-inertial quaternion to Euler angles
// Extract sin(pitch)
float sp = –2.0f * (y*z – w*x);
// Check for Gimbal lock, giving slight tolerance for numerical imprecision
if (fabs(sp) > 0.9999f) {
// Looking straight up or down
p = 1.570796f * sp; // pi/2
// Compute heading, slam bank to zero
h = atan2(–x*z + w*y, 0.5f – y*y – z*z);
b = 0.0f;
} else {
// Compute angles
p = asin(sp);
h = atan2(x*z + w*y, 0.5f – x*x – y*y);
b = atan2(x*y + w*z, 0.5f – x*x – z*z);
}
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
