四元数

四元数（quaternion）：

q = [ qv qs ]，qv为矢量部分，qs为标量部分。qv = αsinθ，qs = cosθ，

α为旋转轴方向的单位向量，注意：θ为旋转角度的一半

也可写成q = [ qx qy qz qw ] = [ axsinθ αysinθ αzsinθ cosθ ]

四元数乘法：

pq = [ (psqv + qspv + pv X qv) (psqs - pv * qv) ]

逆四元数：

q的共轭q* = [ -qv qs ]，而q的逆q^-1 = q* / ( |q| ^2 )，当|q| = 1时，q^-1 = [ -qv qs ]

用四元数旋转向量：

用q旋转v，把v转为四元数，v = [ vx vy vz 0 ]，然后旋转后的v' = qvq^-1

串接旋转：

比如先转q1，然后转q2，最后转q3：v' = q3 q2 q1 v (q1^-1) (q2^-1) (q3^-1)