本文共 8231 字,大约阅读时间需要 27 分钟。
转自:http://www.cnblogs.com/kfqcome/archive/2011/08/17/2143289.html
一.四元组基础
Q(x,y,z,w),其中x,y,z用来确定旋转轴,w为旋转的角度
Q=w+xi+yj+zk,i,j,k为三个虚轴的单位分量
I*j=k
J*k=i;
K*i=j;
叉乘:
c=a × b=
| i j k| |a1 b1 c1| |a2 b2 c2| =(b1c2-b2c1,c1a2-a1c2,a1b2-a2b1)
c也为一个向量,且c的长度为|a||b|sin(theta),垂直于a和b所在的平面,方向由右手法则来判定,用右手的四指先表示向量a的方向,然后手指朝着手心的方向摆动到向量b的方向,大拇指所指的方向就是向量c的方向
1. 四元组相乘:
Q1=w1+x1i+y1j+z1k=(w1,v1)
Q2=w2+x2i+y2j+z2k=(w2,v2)
Q1*Q2=(w1*w2-<v1,v2>,w1*v2+w2*v1+v1xv2)
( w1+x1i+y1j+z1k)*( w2+x2i+y2j+z2k)
=w1*w2-x1*x2-y1*y2-z1*z2+
(W1*x2+x1*w2+y1*z2-z1-y2)i+
(y1*w2+w1*y2+z1*x2-x1*z2)j+
(w1*z2+z1*w2+x1*y2-y1*x2)k
对于其中的轴部分,假如v1//v2,则有v1 x v2=0(平行向量的叉乘结果为0)
2. 四元组的点乘,点乘积为数值:
Q1.*Q2=w1*w2+<v1,v2>=w1*w2+x1*x2+y1*y2+z1*z2;
3. 数乘
s为一实数,q为四元组,则有sq=qs
4. 共轭
p=(w,v),则p*=(w,-v)
(pq)*=q*p*
N(q)=w2+x2+y2+z2
q-1=q*/N(q)---------------à显然可得qq-1=(1,0)
二.使用四元数旋转向量
假如有一表示向量的四元组q=(w,v),对其应用旋转量p后的结果为:
q’=pqp-1=(w,v’)
从上可以看出,计算的结果q’的实部和q的实部是相等的,并且有N(v)=N(v’)
如果N(q)=1,则可以令q=(cosa,usina),u也为一个单位向量,则q’是q绕u旋转2a个弧度的结果
假如S(q)表示q的实部,则有2S(q)=q+q*
2S(pqp-1)= pqp-1+( pqp-1)*=pqp*+(pqp*)*=pqp*+pq*p*=p(q+q*)p*=2S(q)
(这里由于p是单位四元数,所以有p-1等于p*)
欧拉角到四元数的转换
定义pitch, yaw, roll分别为绕X轴、Y轴、Z轴的旋转弧度
float p = pitch * PIOVER180 / 2.0;
float y = yaw * PIOVER180 / 2.0;
float r = roll * PIOVER180 / 2.0;
float sinp = sin(p);
float siny = sin(y);
float sinr = sin(r);
float cosp = cos(p);
float cosy = cos(y);
float cosr = cos(r);
this->x = sinr * cosp * cosy - cosr * sinp * siny;
this->y = cosr * sinp * cosy + sinr * cosp * siny;
this->z = cosr * cosp * siny - sinr * sinp * cosy;
this->w = cosr * cosp * cosy + sinr * sinp * siny;
normalise();
三.使用matlab进行相关计算
计算两个向量v1和v2之间的旋转量四元数p,使得v1应用p后到达v2
假如v1转到v2的旋转轴为v,旋转角为theta,则q=[v*cos(theta/2) sin(theta/2)]
Matlab代码:
function q=vector2q(v1,v2)
%..normalize....
len1=sqrt(v1*v1');
len2=sqrt(v2*v2');
v1=v1/len1;
v2=v2/len2;
angle=v1*v2';
axis=cross(v1,v2);
alen=sqrt(axis*axis');
axis=axis/alen;
t=acos(angle);
t=t/2;
q(1)=axis(1)*sin(t);
q(2)=axis(2)*sin(t);
q(3)=axis(3)*sin(t);
q(4)=cos(t);
end
计算出了q之后,可以获得对应的旋转矩阵,旋转矩阵的计算
Matlab里面的矩阵是以列为主顺序的
function r=q2rot(q)
w=q(4);
x=q(1);
y=q(2);
z=q(3);
r=zeros(3,3);
r(1,1)=1-2*y*y-2*z*z;
r(1,2)=2*x*y+2*w*z;
r(1,3)=2*x*z-2*w*y;
r(2,1)=2*x*y-2*w*z;
r(2,2)=1-2*x*x-2*z*z;
r(2,3)=2*z*y+2*w*x;
r(3,1)=2*x*z+2*w*y;
r(3,2)=2*y*z-2*w*x;
r(3,3)=1-2*x*x-2*y*y;
r=r';
end
同时,也可以根据四元数来计算欧拉角
function R=q2euler(q)
w=q(4);
x=q(1);
y=q(2);
z=q(3);
t11=2*(w*x+y*z);
t12=1-2*(x*x+y*y);
R(1)=atan2(t11,t12);
t2=2*(w*y-z*x);
R(2)=asin(t2);
t31=2*(w*z+x*y);
t32=1-2*(y*y+z*z);
R(3)=atan2(t31,t32);
end
计算出来的欧拉角rx,ry,rz,分别为绕X轴、Y轴和Z轴的旋转角,假如有:
Rotq=q2rot(q)
R=q2euler(q)
[rotx roty rotz]=Rotation(R)
可以发现Rotq==rotz*roty*rotx
从这里可以看出,上面使用四元数这样计算出来的旋转矩阵的旋转顺序分别是X轴、Y轴和Z轴的
ra=pi/4;
qz=[0 0 -sin(ra) cos(ra)] %绕z旋转-90度
qy=[0 sin(ra) 0 cos(ra) ] %绕y旋转90度
qyz=qmult(qy,qz)
r=q2euler(qyz)
上面的r得出的结果为
r = -1.5708 0.0000 -1.5708
也就是说其几何意义变成先绕X轴旋转-90度,再绕Z轴旋转-90度,而根据qy和qz的相乘我们实际进行的操作却是先绕Z轴旋转-90度,再绕Y轴旋转90度,但是结果却是这两种操作等价,这说明由四元数到欧拉角可以有多个解
两个四元数,假如它们的方向是相反的,用它们作用于向量得到的新向量的值仍然相等
q1=[0.024666 -0.023954 0.504727 0.862594];
arm=[-8.881719 6.037597 -2.36776];
q2=-q1;
rot1=q2rot(q1);
rot2=q2rot(q2);
v1=rot1*arm'
v2=rot2*arm'
上面计算出来的v1等于v2
四元数的余弦值为它们的内积
假如余弦值小于0,则需要将其中的一个取反,因为上面我们知道一个四元数和它的反方向的四元数对一个向量起相同的作用
四元数的相乘,代表旋转的累积
pq=p*q;
rotp=q2rot(p);
rotq=q2rot(q);
rotpq=q2rot(pq);
rotmul=rotp*rotq;
这里rotpq与rotmul相等
四. OGRE中Quaternion类的几个函数
1. 四元数到旋转向量
void Quaternion::ToRotationMatrix (Matrix3& kRot) const
1 - 2*qy2 - 2*qz2 | 2*qx*qy - 2*qz*qw | 2*qx*qz + 2*qy*qw |
2*qx*qy + 2*qz*qw | 1 - 2*qx2 - 2*qz2 | 2*qy*qz - 2*qx*qw |
2*qx*qz - 2*qy*qw | 2*qy*qz + 2*qx*qw | 1 - 2*qx2 - 2*qy2 |
2. 旋转量到四元数
根据1中的表格,有:
4 *(1-qx2-qy2-qz2) = 1 + m00 + m11 + m22
又qw2=1-qx2-qy2-qz2,可得
4 *qw2= 1 + m00 + m11 + m22
这里解qw必须保证1 + m00 + m11 + m22>=0,如果不是的话,就构造其他的等式来计算,OGRE中分成两种情况,一种是m00 + m11 + m22>=0,就可以直接先解出qw,否则的采用另外的等式计算
3.Local axis
Vector3 xAixs(void) const;
取得旋转矩阵的第一列,旋转矩阵和一个向量相乘的话,第一列的数据均和向量的x分量相乘
Vecotr3 yAxis(void) const;
取得旋转矩阵的第二列,旋转矩阵和一个向量相乘的话,第二列的数据均和向量的y分量相乘
Vecotr3 zAxis(void) const;
取得旋转矩阵的第三列,旋转矩阵和一个向量相乘的话,第三列的数据均和向量的z分量相乘
转自:http://www.cnblogs.com/Mrt-02/archive/2011/10/15/2213656.html
在讨论「四元数」之前,我们来想想对三维直角坐标而言,在物体旋转会有何影响,可以扩充三维直角坐标系统的旋转为三角度系统(Three-angle system),在Game Programming Gems中有提供这么一段:
Quaternions do not suffer from gimbal lock. With a three-angle(roll, pitch, yaw) system, there are always certain orientations in which there is no simple change to the trhee values to represent a simple local roation. You often see this rotation having "pitched up" 90 degree when you are trying to specify a local yaw for right.
简单的说,三角度系统无法表现任意轴的旋转,只要一开始旋转,物体本身即失去对任意轴的自主性。
四元数(Quaternions)为数学家Hamilton于1843年所创造的,您可能学过的是复数,例如:a + b i 这样的数,其中i * i = -1,Hamilton创造了三维的复数,其形式为 w + x i + y j + z k,其中i、j、k的关系如下:
i2 = j2 = k2 = -1
i * j = k = -j * i
j * k = i = -k * j
k * i = j = -i * k
假设有两个四元数:
q1 = w1 + x1 i + y1 j + z1 k
q2 = w2 + x2 i + y2 j + z2 k
四元数的加法定义如下:
q1 + q2 = (w1+w2) + (x1+x2) i + (y1+y2) j + (z1+z2) k
四元数的乘法定义如下,利用简单的分配律就是了:
q1 * q2 =
(w1*w2 - x1*x2 - y1*y2 - z1*z2) +
(w1*x2 + x1*w2 + y1*z2 - z1*y2) i +
(w1*y2 - x1*z2 + y1*w2 + z1*x2) j +
(w1*z2 + x1*y2 - y1*x2 + z1*w2) k
由于q = w + x i + y j + z k中可以分为纯量w与向量x i + y j + z k,所以为了方便表示,将q表示为(S, V),其中S表示纯量w,V表示向量x i + y j + z k,所以四元数乘法又可以表示为:
q1 * q2 = (S1 + V1)*(S2 + V2) = S1*S2 - V1.V2 + V1XV2 + S1*V2 + S2*V1
其中V1.V2表示向量内积,V1XV2表示向量外积。
定义四元数q = w + x i + y j +z k 的norm为:
N(q) = |q| = x2 + y2 + z2 + w2
满足N(q) = 1的四元数集合,称之为单位四元数(Unit quaternions)。
定义四元数定义四元数q = w + x i + y j +zk的共轭(Conjugate)为:
q* = 定义四元数q = w - x i - y j -z k = [S - V]
定义四元数的倒数为:
1/ q = q* / N(q)
说明了一些数学,您所关心的或许是,四元数与旋转究竟有何关系,假设有一任意旋转轴的向量A(Xa, Ya, Za)与一旋转角度θ,如下图所示:
可以将之转换为四元数:
x = s * Xa
y = s * Xb
z = s * Xc
w = cos(θ/2)
s = sin(θ/2)
所以使用四元数来表示的好处是:我们可以简单的取出旋转轴与旋转角度。
那么四元数如何表示三维空间的任意轴旋转?假设有一向量P(X, Y, Z)对着一单位四元数q作旋转,则将P视为无纯量的四元数X i + Y j + Z k,则向量的旋转经导证如下:
Rot(P) = q p q*
四元数具有纯量与向量,为了计算方便,将之以矩阵的方式来表现四元数的乘法,假设将四元数表示如下:
q = [w, x, y, z] = [S, V]
两个四元数相乘q" = q * q'的矩阵表示法如下所示:
若令q = [S, V] = [cosθ, u*sinθ],其中u为单位向量,而令q'= [S', V']为一四元数,则经过导证,可以得出q * q' * q^(-1)会使得q'绕着u轴旋转2θ。
由四元数的矩阵乘法与四元数的旋转,可以导证出上面的旋转公式可以使用以下的矩阵乘法来达成:
讲了这么多,其实就是要引出上面这个矩阵乘法,也就是说如果您要让向量(x', y', z')(w'为0)对某个单位向量轴u(x, y, z)旋转角度2θ,则w = cosθ,代入以上的矩阵乘法,即可得旋转后的(x", y", z"),如果为了方便,转换矩阵的最下列与最右行会省略不写出来,而如下所示:
关于四元数的其它说明,您可以参考 向量外积与四元数 这篇文章。
关于旋转的转换矩阵导证,在Game Programming Gems第二章有详细的说明。
关于 Gimbal lock。
下面是关于四元数的一点程序的表达方法。
1: class CQuaternion
2: {
3: public:
4: CQuaternion(const float fScalar,const Vector3& rVec)
5: {
6: mVector=rVec ;
7: mScalar=fScalar;
8: }
9:
10: void FromAxisAngle (const Vector3& rAxis, const F32 Angle)
11: {
12: F32 fSin, fCos;
13: //取得一个弧度角的SIN COS值
14: SinCos( Angle*0.5f, fSin, fCos);
15: mVector = rAxis*fSin;
16: mScalar = fCos;
17: }
18: private:
19: float mScalar;
20: float mVector;
21: }
22:
23: class CMatrix44
24: {
25: public:
26: enum { _X_,_Y_,_Z_,_W_ };
27: void QuaternionToMatrix(const CQuaternion& q)
28: {
29: F32 s,xs,ys,zs,wx,wy,wz,xx,xy,xz,yy,yz,zz;
30: s = q.Length2();
31: s = (s>0 ? 2.f/s : 0);
32:
33: xs = q.Vect[_X_]*s; ys = q.Vect[_Y_]*s; zs = q.Vect[_Z_]*s;
34: wx = q.Scalar*xs; wy = q.Scalar*ys; wz = q.Scalar*zs;
35: xx = q.Vect[_X_]*xs; xy = q.Vect[_X_]*ys; xz = q.Vect[_X_]*zs;
36: yy = q.Vect[_Y_]*ys; yz = q.Vect[_Y_]*zs; zz = q.Vect[_Z_]*zs;
37:
38: (*this)[0].Set(1.f-(yy+zz),xy+wz, xz-wy, 0.f); // col 0
39: (*this)[1].Set(xy-wz, 1.f-(xx+zz),yz+wx, 0.f); // col 1
40: (*this)[2].Set(xz+wy, yz-wx, 1.f-(xx+yy),0.f); // col 2
41: }
42: //忽略其它无关紧要的
43: //、、、、、、、
44: };
//========================================================
不用多说,肯定有回过神来的,也有没有回过神来的。
正如上面那某位的博客里面讲到的。
对于旋转轴A,绕其旋转一定的角度,则可以表示为
x = s * Xa
y = s * Xb
z = s * Xc
w = cos(θ/2)
s = sin(θ/2)
这正是我们FromAxisAngle 所做的事情。
而QuaternionToMatrix则是对应了
我想说明的是,数学库本身并不在于代码。而是在于数学公式,代码仅是将其用另一种符号表示出来而已。只要仔细去看,定能明白其中的道理。
关于文中介绍的公式推导以及万向锁,可以GOOGLE和百度。
另外,编程中还经常用到欧拉角和矩阵的转换。
这三个的特点。
矩阵运算的数据相对来说比较直观,容易调试和诊断。但数据存储量大,特别是旋转的时候,会浪费很多空间。
欧拉角储存小,但有万向锁,并且插值不够平滑。
四元数据量介于二者之间。但插值容易。
在骨骼动画中,可以在文件中存储欧拉角,加载后将旋转数据转换为四元数。最后动画计算时统一采用矩阵运算。
要说的东西很多,一言难尽。今天就到这里吧。