四元数在OSG中的应用分析

最近要深入使用模型变换的东西，其中有个非常重要的就是旋转，旋转用经常用四元数来做，但是这个原理一直没有弄懂，今天看资料，总结下：

（一）四元数:

   看源码中可以了解，网上别人总结的很好：

四元数：Q = [w,(x,y,z)]被定义为一个四元数,w为一个实数，(x,y,z)是一个三维矢量，四元数的基底为(1,i,j,k),则
Q = w + xi + yj + zk;四元数是复数在四维空间的推广，于是可以认为i,j,k是四元数的虚单位。
     欧拉证明了任何旋转都可以通过对一个轴的单一旋转来表示，因此方向可以分解为一个角度(θ)+轴(n)，可能有人和我一样以为osg::
Quat(x,y,z,w)就是沿着轴(x,y,z)正向看去逆时针旋转w角度(正向是从原点指向点(x,y,z)的矢量的方向)但是，这是不对的。欧拉认为这个角度和轴不是简单的放在四元数中，他们的关系应该如下：
                              q
= [ cos(θ/2) , sin(θ/2)n ] = [ cos(θ/2), ( sin(θ/2)x, sin(θ/2)y, sin(θ/2)z ) ]
    四元数的标量部分为cos(θ/2)，即把旋转角的一半的余弦值作为四元数的标量部分；把旋转角的一半的正弦值和这个旋转轴数乘以后得到的矢量作为四元数的矢量部分。而osg::
Quat在处理时将标量部分放在了最后，上面q用在osg中用osg:: Quat表示为：
                              q
= [ sin(θ/2)n , cos(θ/2) ] = [ ( sin(θ/2)x, sin(θ/2)y, sin(θ/2)z ) ,cos(θ/2) ]
即在osg:: Quat中，它的成员变量_v[3]表示的是四元数的标量部分，因为余弦函数的值域是有界闭区间[-1,1]，所以看来_v[3]的值是介于[-1,1]区间上的，看osg:: Quat的一个成员函数makeRotate：

void Quat::makeRotate( value_type angle, value_type x, value_type y, value_type z )
{
const value_type epsilon = 0.0000001;

value_type length = sqrt( x*x + y*y + z*z );
if (length < epsilon)
{
// ~zero length axis, so reset rotation to zero.
*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;
}

函数最后一句 "_v[3] = coshalfangle;" 说明了osg:: Quat对实部处理的这个事实，而同时，旋转轴被单位化，再乘以一个值域有界为[-1,1]的正弦值，所以虚部每一个分量都介于[-1,1]之间。通过上述处理，可以将一个旋转角，一个旋转轴转化为一个四元数。

再看看getRotate这个函数，怎样从一个四元数得到它的旋转角度和转轴？

void Quat::getRotate( value_type& angle, value_type& x, value_type& y, value_type& z ) const
{
value_type sinhalfangle = sqrt( _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] );

angle = 2.0 * atan2( sinhalfangle, _v[3] );
if(sinhalfangle)
{
x = _v[0] / sinhalfangle;
y = _v[1] / sinhalfangle;
z = _v[2] / sinhalfangle;
}
else
{
x = 0.0;
y = 0.0;
z = 1.0;
}

}

代码中第一句 "sqrt( _v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] )" 为什么会是旋转角一半的正弦值，看看刚才makeRotate的时候是怎么处理过的：
    _v[0]
= x * sinhalfangle * inversenorm;
    _v[1]
= y * sinhalfangle * inversenorm;
    _v[2]
= z * sinhalfangle * inversenorm;

代入上式，提取公因子sinhalfangle，可以发现正确。因为向量部分已经单位化了，所以可以直接得到时sin(angle/2)。事实上，_v[3]就是cos(angle/2),所以也可以
sin(angle/2) = sqrt(1 - _v[3]*_v[3]),_v[3]就是cos(angle/2),所以angle = 2 * acos(_v[3]),osg:: Quat中旋转角angle的得到比较迂回，它求得coshalfangle/sinhalfangle的反正切，即得angle的一半。求旋转轴的时候分别给_v[0],_v[1],_v[2]除以sinhalfangle得到单位化后的旋转轴。

四元数的运算法则：i*i = j*j = k*k = i*j*k = -1;
                  ij
= k; ji = -k;
                  jk
= i; kj = -i;
                  ki
= j; ik = -j;
大家可以用上面的法则推倒一下osg:: Quat中四元数的乘法，发现结果是正确的^^,乘法的时候，osg:: Quat是将实部放在最后一个分量_v[3]上的，有四元数的共轭：
inline Quat conj () const   {  return
Quat( -_v[0], -_v[1], -_v[2], _v[3] ); }可以进一步看出第四个分量是实部，欢迎讨论，批评指正。

另一个函数：

void Quat::makeRotate( const Vec3d& from, const Vec3d& to )
{
Vec3d sourceVector = from;
Vec3d targetVector = to;

value_type fromLen2 = from.length2();
value_type fromLen;
// normalize only when necessary, epsilon test
if ((fromLen2 < 1.0-1e-7) || (fromLen2 > 1.0+1e-7)) {
fromLen = sqrt(fromLen2);
sourceVector /= fromLen;
} else fromLen = 1.0;

value_type toLen2 = to.length2();
// normalize only when necessary, epsilon test
if ((toLen2 < 1.0-1e-7) || (toLen2 > 1.0+1e-7)) {
value_type toLen;
// re-use fromLen for case of mapping 2 vectors of the same length
if ((toLen2 > fromLen2-1e-7) && (toLen2 < fromLen2+1e-7)) {
toLen = fromLen;
}
else toLen = sqrt(toLen2);
targetVector /= toLen;
}

//现在让我们进入真正的东西使用“点积+ 1”ε测试,因为它可以重复使用    double dotProdPlus1 = 1.0 + sourceVector * targetVector;

// 检查退化的全面转变。用ε来检测
if (dotProdPlus1 < 1e-7) {

// 得到一个正交向量给定向量与最大平面向量的坐标。然后用四元数与π轴角
// Trick is to realize one value at least is >0.6 for a normalized vector.至少一个值> 0.6
if (fabs(sourceVector.x()) < 0.6) {
const double norm = sqrt(1.0 - sourceVector.x() * sourceVector.x());
_v[0] = 0.0;
_v[1] = sourceVector.z() / norm;
_v[2] = -sourceVector.y() / norm;
_v[3] = 0.0;
} else if (fabs(sourceVector.y()) < 0.6) {
const double norm = sqrt(1.0 - sourceVector.y() * sourceVector.y());
_v[0] = -sourceVector.z() / norm;
_v[1] = 0.0;
_v[2] = sourceVector.x() / norm;
_v[3] = 0.0;
} else {
const double norm = sqrt(1.0 - sourceVector.z() * sourceVector.z());
_v[0] = sourceVector.y() / norm;
_v[1] = -sourceVector.x() / norm;
_v[2] = 0.0;
_v[3] = 0.0;
}
}

else {
//找到最短的角度将归一化向量转换成另一个四元数。当向量共线的公式仍然有效
const double s = sqrt(0.5 * dotProdPlus1);
const Vec3d tmp = sourceVector ^ targetVector / (2.0*s);

//等到对应的四元数
_v[0] = tmp.x();
_v[1] = tmp.y();
_v[2] = tmp.z();
_v[3] = s;
}
}

（二）四元数在旋转中使用

后面就是对四元数的应用，主要还是应用与矩阵变换中，OSG中，矩阵直接对其使用Matrix（quat）

void Matrix_implementation::setRotate(const Quat& q)
{
double length2 = q.length2();//四元数长度的平方（_v[0]*_v[0] + _v[1]*_v[1] + _v[2]*_v[2] + _v[3]*_v[3]）不能太小
if (fabs(length2) <= std::numeric_limits<double>::min())
{
_mat[0][0] = 0.0; _mat[1][0] = 0.0; _mat[2][0] = 0.0;
_mat[0][1] = 0.0; _mat[1][1] = 0.0; _mat[2][1] = 0.0;
_mat[0][2] = 0.0; _mat[1][2] = 0.0; _mat[2][2] = 0.0;
}
else
{
double rlength2;
// 如果需要将四元数单位化
// We can avoid the expensive sqrt in this case since all 'coefficients' below are products of two q components.
// That is a square of a square root, so it is possible to avoid that 这是一个平方的平方根,所以可以避免这样做
if (length2 != 1.0)
{
rlength2 = 2.0/length2;
}
else
{
rlength2 = 2.0;
}

// Source: Gamasutra, Rotating Objects Using Quaternions
//使用四元数旋转的算法
//http://www.gamasutra.com/features/19980703/quaternions_01.htm

double wx, wy, wz, xx, yy, yz, xy, xz, zz, x2, y2, z2;

// calculate coefficients
x2 = rlength2*QX;
y2 = rlength2*QY;
z2 = rlength2*QZ;

xx = QX * x2;
xy = QX * y2;
xz = QX * z2;

yy = QY * y2;
yz = QY * z2;
zz = QZ * z2;

wx = QW * x2;
wy = QW * y2;
wz = QW * z2;

// Note.  Gamasutra gets the matrix assignments inverted, resulting
// in left-handed rotations, which is contrary to OpenGL and OSG's
// methodology.  The matrix assignment has been altered in the next
// few lines of code to do the right thing.
// Don Burns - Oct 13, 2001
_mat[0][0] = 1.0 - (yy + zz);
_mat[1][0] = xy - wz;
_mat[2][0] = xz + wy;

_mat[0][1] = xy + wz;
_mat[1][1] = 1.0 - (xx + zz);
_mat[2][1] = yz - wx;

_mat[0][2] = xz - wy;
_mat[1][2] = yz + wx;
_mat[2][2] = 1.0 - (xx + yy);
}

#if 0
_mat[0][3] = 0.0;
_mat[1][3] = 0.0;
_mat[2][3] = 0.0;

_mat[3][0] = 0.0;
_mat[3][1] = 0.0;
_mat[3][2] = 0.0;
_mat[3][3] = 1.0;
#endif
}

总之就是将四元数QUAT转化为矩阵matrix了，然后在使用matrix来对模型进行变换。