3D数学之四元组应用及实现

第一次接触四元组是在使用OGRE引擎的时候，那个时候非常好奇，为什么4个数字就能表示一个旋转，另外为什么要用四元组来表示旋转，用旋转矩阵不是挺好的吗？经过一段的时间的学习，算是对四元组有了基础的认识，下面跟大家一起走进神奇的四元组之旅。

四元组，英文名字，Quaternion

先说明下，为什么我们要用四元组

1.它能节省存储空间

2.它的连接（concatenation）运算需要的运算量较少

3.四元组容易插值产生平滑的动画

四元组<w,x,y,z>又被称为4D中的复数，它等价于表达式w+xi+yj+zk,如果让v代表向量<x,y,z>,s代表标量w,那么四元组也可以简写为<s;v>;

其中i,j,k有如下关系：

Two quaternions are defined to be equal if and only if they have exactly the same four
components. (Two unequal quaternions may still represent the same rotation.)

四元组被定义为相等只有当它的四部分都相等时才相等，不能说代表相同的旋转四元组就相等，因为不同的四元组可能代表相同的旋转比如四元组q和-q（为什么？后面会解释）

Addition of quaternions is defined component-wise, namely
(w + xi + yj + zk) + (w' + x'i + y' j + z'k) = (w + w') + (x + x')i+(y + y') j + (z + z')k.

The product of a scalar and quaternion is defined also as expected, namely,
α(w + xi + yj + zk) = (αw) + (αx)i + (αy) j + (αz)k,where α ∈ R.

The product of two quaternions q1 and q2 is denoted q1q2 (we reserve the dot product notation q1 · q2 for the usual dot product).

所以(<s_1;v_1>)(<s_2;v_2>)=s_1s_2-v_1·v_2+s_1v_2+s_2v_1+v_1×v_2

其中标量部分是s_1s_2-v_1·v_2

向量部分是s_1v_2+s_2v_1+v_1×v_2   (由于编辑器的问题不能打出上标和下标,用^代替上标,用_代替下标)

Quaternion addition is commutative and associative.

四元组加法满足交换律和结合律

Quaternion multiplication is associative.The left and right distributive laws hold for quaternions, that is, for all
quaternions q, r, s,q(r + s) = qr + qs and (r + s)q = rq + sq.

四元组乘法满足结合律和交换律

可以通过简单的运算验证上面两个命题

由于向量叉乘不满足交换律,所以四元组乘法也不满足交换律

The norm, or magnitude, of a quaternion q = <w, x, y, z> is defined to equal

||q|| =sqrt(w^2 + x^2 + y^2 + z^2)

We define the conjugate, q*, of q to equal q* = <w,−x,−y,−z>

if q=<s;v>,then ||q||=sqrt(s^2+v^2),where v^2=v·v.Also,q*=<s;-v>

容易验证qq*=q*q=<s^2+v^2;0>=||q||^2

A unit quaternion is a quaternion with norm equal to 1.

A quaternion <w; 0> with zero vector component will be identified with the scalar w ∈ R.
This is compatible with the earlier definitions since the product wq is the same whether w is
interpreted as a quaternion or as a scalar. Similarly, a vector v ∈ R3 will be identified with
the quaternion <0; v> with zero scalar component. 

Care should be taken when vectors are interpreted as quaternions because a vector cross product is not the same as a quaternion product. 

(Indeed, they could not be the same, for quaternion products are associative whereas vector cross products are not.) 

As an example, we have
(i + j) × j = k,
but,
(i + j ) j = (<0, 1, 1, 0>)(<0, 0, 1, 0>) = <−1, 0, 0, 1> ≠< 0, 0, 0, 1> = k.

注意:如果q=<s;v>是一个单位四元组,那他的逆可以很容易计算得到

Fix a unit vector u and a rotation angle θ. Recall that Rθ,u is the rotation around axis u through angle θ
with the rotation direction given by the right-hand rule. 

Let c = cos(θ/2) and s = sin(θ/2),and let q be the quaternion

                                                                   

用任何非0标量a乘以四元组q,并不影响qvq^(-1)的值,也就是说并影响qvq^(-1)对应的旋转.

q与-q可以看作q乘以标量-1,所以用它们来表示旋转(即通过qvq^(-1)来表示旋转)是一样的.

In abstract algebra, a mapping of the form
v → qvq^(−1) 

computed by multiplying on the left by a fixed element and on the right by its inverse is called
an inner automorphism.(内自同构)

We now prove Theorem XII.3.

  

这就证明了假设有四元组q< cos(θ/2) ;sin(θ/2)u>(其中u是单位向量),那么对于向量v,表达式qvq^(-1)得到的结果就是v绕定轴u按右手法则旋转θ度.

Quaternions provide a method of representing rotations by 4-tuples.

 An arbitrary, rigid, orientation-preserving linear transformation can
be expressed as a rotation around some fixed axis. Such a rotation is specified by a rotation
angle θ and a rotation axis u, where u is a unit vector: recall that this rotation was denoted
Rθ,u. To represent Rθ,u with a quaternion, let c = cos(θ/2), s = sin(θ/2), and u = <u_1, u_2, u_3>.
Then the 4-tuple q defined by
q = <c, su_1, su_2, su_3> is called a quaternion

 qVq^(-1)就代表了Rθ,u V                                                       

也可说四元组r代表了旋转Rθ,u因为r与公式v → rvr^(−1) 一一对应(实际上我们知道是 rVr^(-1)才代表Rθ,u V ) .

好了,下面解释为什么q和-q代表相同的旋转,虽然前面已经解释了对一个四元组乘以一个标量并不影响它所代表的旋转.但是这里我们从几何角度去理解,可能会更加形象.

接下来,我们用第二种方法证明pvp^(-1)代表的是一个旋转变换

Let q = s + v be a unit quaternion. Then q^(-1) = s- v,and given a vector u(注意向量可以看成标量为0的特殊四元组),we have

A(u)=quq^(-1)=(s+v)u(s-v)=(s;v)(0;u)(s;-v)   (通过四元组乘积公式(<s_1;v_1>)(<s_2;v_2>)=s_1s_2-v_1·v_2+s_1v_2+s_2v_1+v_1×v_2)

       =(-v·u+su+v×u)(s;-v)=(-v·u;su+v×u)(s;-v)=-(v·u)s-(su+v×u)·(-v)+(-v·u)(-v)+s(su+v×u)+(su+v×u)×(-v)

       =-(v·u)s+su·v+(v·u)v+s^2u+s(v×u)-s(u×v)-v×u×v

       =s^2u+2s(v×u)+(v·u)v-v×u×v

利用公式P × (Q× P) = P ×Q× P = P^2Q − (P ⋅Q)P

A(u)=s^2u+2s(v×u)+(v·u)v-(v^2u-(v⋅u)v)

       =(s^2-v^2)u+2s(v×u)+2(v·u)v

令v=tv',v'是一个单位向量(注意q是单位四元组,并不能满足v是单位向量)

则A(u)=quq^(-1)=(s^2-t^2)u+2st(v'×u)+2t^2(v'·u)v'

回忆<<各种变换的原理>>里面讲的绕任意轴旋转的公式

我们假设

s^2-t^2=cosθ

2st=sinθ

2t^2=1-cosθ

我们可以求出

The first and third equalities together tell us that s^2 + t^2 =1, so we must have
s = cos(θ/2).

A(u)=quq^(-1)=ucosθ+(v'×u)sinθ+(v'·u)v'(1-cosθ)

这里证明了,A变换表示旋转

四元组q=cos(θ/2)+sin(θ/2)v'

如果用3x3矩阵代表旋转,当表示2个旋转时,需要两个矩阵相乘,用到9*(3+2)操作(9个元素,每个元素需要3次乘法和2次加法)

然而用四元组代表旋转,当表示2个旋转时,需要两个四元组相乘,用到4*4+(4*4-1)次操作(4*4次乘法,4*4-1次加法)

接着我们再从概念上证明四元组代表旋转

在我的那篇<<各种变换的原理>>里面讲过了,

Every rigid, orientation-preserving, linear transformation is a rotation.

A transformation on R^2 is any mapping A : R^2 → R^2. That is, each point x ∈ R^2 is mapped
to a unique point, A(x), also in R^2.
Definition Let A be a transformation. A is a linear transformation provided the following two
conditions hold:
1. For all α ∈ R and all x ∈ R^2, A(αx) = αA(x).
2. For all x, y ∈ R^2, A(x + y) = A(x) + A(y).

For linear transformations, an equivalent definition of rigid transformation is that a linear
transformation A is rigid if and only if it preserves dot products. That is to say, if and only if, for
all x, y ∈ R^2, x · y = A(x) · A(y). 

下面我们来证明v -> qvq(-1)变换拥有这几个性质

A rotation in three dimensions can be thought of as a function A that maps R^3
onto itself. For A to represent a rotation, it must preserve lengths, angles, and handedness(也就是我们说的朝向保持).

The set of quaternions, known by mathematicians as the ring of Hamiltonian quaternions
and denoted by H, can be thought of as a four-dimensional vector space
for which an element q has the form 

q = <w, x, y, z> = w+ xi + yj + zk.

A quaternion is often written as q = s + v, where s represents the scalar part corresponding
to the w-component of q, and v represents the vector part corresponding
to the x, y, and z components of q.

Extending the function A to a mapping from H onto itself by requiring that A(s + v) = s + A(v) 

所以我们只需要证明A(v)变换满足长度保持,角度保持,朝向保持以及线性变换

这里为了与前面一致,我们将其中的v换成u,即证明A(u)变换满足长度保持,角度保持,朝向保持以及线性变换

先证明其满足线性变换

由前面可知

A(u)=(s^2-v^2)u+2s(v×u)+2(v·u)v

A(au)=(s^2-v^2)au+2s(v×au)+2(v·au)v=a((s^2-v^2)u+2s(v×u)+2(v·u)v)=aA(u)

A(u_1+u_2)=(s^2-v^2)(u_1+u_2)+2s(v×(u_1+u_2))+2(v·(u_1+u_2))v

=(s^2-v^2)u_1+(s^2-v^2)u_2+2s(v×u_1)+2s(v×u_2)+2(v·u_1)v+2(v·u_2)v

=(s^2-v^2)u_1+2s(v×u_1)+2(v·u_1)v+(s^2-v^2)u_2+2s(v×u_2)+2(v·u_2)v

=A(u_1)+A(u_2)

这就证明了A满足线性变换

下面我们证它满足长度保持

||A(u)||=||quq^(-1)||=||q|| ||u|| ||q^(-1)|| = ||u||

下面我们需要证明它满足角度保持和朝向保持

由于长度保持已证,所以

角度保持可以通过A(u_1)·A(u_2)=u_1·u_2来证

朝向保持可以通过A(u_1)×A(u_2)=A(u_1×u_2)来证

证角度保持和朝向保持  等价与  证A(u_1)A(u_2)=A(u_1u_2)

为什么呢?请看下面解释

把u_1,u_2,A(u_1),A(u_2)看成特殊的四元组(标量部分为0的四元组)

A(u_1)A(u_2)=-A(u_1)·A(u_2)+A(u_1)×A(u_2)

A(u_1u_2)=A(-u_1·u_2+u_1×u_2)=-u_1·u_2+A(u_1×u_2)   (注意标量部分相等,向量部分相等)

如果证得A(u_1)A(u_2)=A(u_1u_2),那么我们就得到了

A(u_1)·A(u_2)=u_1·u_2和A(u_1)×A(u_2)=A(u_1×u_2)

即角度保持和朝向保持

下面我们证明A满足A(u_1)A(u_2)=A(u_1u_2)

A(u_1)A(u_2)=q(u_1)q^(-1)q(u_2)q^(-1)=q(u_1)(u_2)q^(-1)=A(u_1u_2)

即证明了A满足了角度保持和朝向保持,加上前面的线性变换,我们可知A表示一个旋转变换

///////////////////////至此 四元组能表示旋转的问题被阐明了,如果你只对四元组表示的旋转部分感兴趣可以跳过下面这段内容  如果你想把四元组搞清楚,请你继续////////////

这段我们会说明为什么是θ/2而不是θ,以及为什么旋转变换是表达式quq^(-1)

前面我们说了

四元组q< cos(θ/2) ;sin(θ/2)u>(其中u是单位向量),那么对于向量v,表达式qvq^(-1)得到的结果就是v绕定轴u按右手法则旋转θ度.

从公式上我们知道为什么是cos(θ/2)和sin(θ/2).下面从数学的角度更形象的阐述为什么是θ/2,而不是θ

We begin by considering how we can embed the set of real numbers in
the set of 2×2 matrices. For any given scalar a, we associate it with exactly
one 2 × 2 matrix, namely the matrix that has a on both of the diagonal
elements:

We have chosen a subset of the 2×2 matrices, and established a one-to-one
correspondence between this smaller set of matrices and the set of all real
numbers. We could have established this one-to-one relationship in other
ways, but this particular way of doing it is important because it preserves
all the ordinary algebra laws of addition, subtraction, and multiplication:
the associative property, distributive property, nonfactorability of zero, and
so on. (We can even include division if we treat division as multiplication
by the inverse.) For example,

Now let’s see if we can create a similar mapping for the set of complex
numbers. You probably already have been introduced to complex numbers;
if so, you should remember that the complex pair (a, b) defines the number
a+bi. The number i is a special number such that i^2 = −1. It’s often called
the imaginary number because no ordinary scalar (a “real” number) can
have this property. The word “imaginary” gives one the impression that
the number doesn’t really exist; we’re going avoid this term and instead
stick with the more descriptive one: “complex.”

Complex numbers can be added, subtracted, and multiplied. All we
need to do is follow the ordinary rules for arithmetic, and replace i^2 with
−1 whenever it appears. This results in the following identities:

Now, how can we extend our system of embedding numbers in the space
of 2 × 2 matrices to include complex numbers? Before, we only had one
degree of freedom, a, and now we have two, a and b. The mapping we use is

We can easily verify that the complex number on the left behaves exactly
the same as the matrix on the right. In a certain sense, they are just two
notations for writing the same quantity:

We also verify that the equation i2 = 1 still holds:

我们知道,

矩阵  实际上代表了逆时针旋转90度.

We can interpret multiplication by i as a 90°rotation.

There’s nothing “imaginary” about this. Instead of thinking of i as the
square root of .1, think instead of the complex number a+bi as a mathematical
entity with two degrees of freedom that behaves in particular ways
when multiplied. The part we usually call the “real” part, a, is the main
degree of freedom, and b measures some secondary degree of freedom. The
two degrees of freedom are in some sense “orthogonal” to one another.
Continuing this further, we see that we can represent rotations by any
arbitrary angle θ using this scheme. The basic 2×2 rotation matrix derived
in Section 5.1.1 happens to be in this special set of matrices that we are
mapping to the complex numbers. It maps to the complex number cos θ +
i sin θ:

Notice how complex conjugation (negating the complex part) corresponds
to matrix transposition. This is particularly pleasing. Remember
that the conjugate of a quaternion expresses the inverse angular displacement.
A corresponding fact is true for transposing rotation matrices: since
they are orthogonal, their transpose is equal to their inverse.

How do ordinary 2D vectors fit into this scheme? We interpret the
vector [x, y] as the complex number x + iy, and then we can interpret the multiplication of two complex numbers

as performing a rotation. This is equivalent to the matrix multiplication

While this a not much more that mathematical trivia so far, our goal is to
build up some parallels that we can carry forward to quaternions, so let’s
repeat the key result.

In 2D, we can interpret the vector [x, y] as a complex number x + yi and
rotate it by using the complex multiplication (cos θ + i sin θ)(x + iy).

A similar conversion from ordinary vectors to complex numbers is necessary
in order to multiply quaternions and 3D vectors.

Before we leave 2D, let’s summarize what we’ve learned so far. Complex
numbers are mathematical objects with two degrees of freedom that obey
certain rules when we multiply them. These objects are usually written as
a + bi, but can equivalently be written as a 2 × 2 matrix. When we write
complex numbers as matrices, it begs the geometric interpretation of multiplication
by i as a 90° rotation. The rule i^2 = −1 has the interpretation
that combining two 90° rotations yields a 180° rotation, and that leaves us
with a warm fuzzy feeling. More generally, any complex number with unit
length can be written as cos θ + i sin θ and interpreted as a rotation by the
angle θ. If we convert a 2D vector into complex form and multiply it by
cos θ + i sin θ, it has the effect of performing the rotation.

It’s very tempting to extend this trick from 2D into 3D.

那么这种新复数应该怎样表示呢?The Irish mathematician William Hamilton (1805–1865),也就是发明四元组这位牛人,最开始以为这种复数有一个实部和两个虚部

但在1843,在他去爱尔兰皇家学院的路上,他突然灵感一现,认识到这种复数要有三个虚部,他在Broom Bridge刻下了描述这种新型复数的方程.4元组因此也就产生

那么为什么不用3D复数来表示3D中的旋转呢?

A 3D complex number would have two
complex parts, i and j, with the properties i^2 = j^2 = −1. (We would
also need to define the values of the products ij and ji. Exactly what
these rules should be, we’re not sure; perhaps Hamilton realized this was
a dead end. In any case, it doesn’t matter for the present discussion.)

The number 1 must obviously map to the 3D identity matrix I_3. The number −1 should map
to its negative, −I_3, which has −1s on the diagonal.

现在的问题是我们需要找到i,j能映射到的3x3矩阵.我们马上就会发现,我们找不到这样的映射.为什么呢?

因为−I_3行列式的值为-1.如果找到这样的矩阵,那么这个矩阵行列式的值的平方为-1,

The only way this can work is for i and
j to contain entries that are complex. In short, there doesn’t seem to be
a coherent system of 3D complex numbers; certainly there isn’t one that
maps elegantly to rotations analogously to standard complex numbers and
2D rotations. For that, we need quaternions.

Quaternions extend the complex number system by having three imaginary
numbers, i, j, and k, which are related by Hamilton’s famous equations:

The quaternion we have been denoting <w;(x, y, z)> corresponds to the complex
number w + xi + yj + zk.

Now we return to matrices. Can we embed the set of quaternions into
the set of matrices such that Hamilton’s rules in Equation still hold?

Yes, we can, although, as you might expect, we map them to 4×4 matrices.
Real numbers are mapped to a matrix with the number on each entry of
the diagonal as before,

and the complex quantities are mapped to the matrices

(8.11)

(自己去验证下)

Combining the above associations, we can map an arbitrary quaternion
to a 4 × 4 matrix as

Once again, we notice how complex conjugation (negating x, y, and z)
corresponds to matrix transposition.

Notice how the upper left 2×2 portion of the
k matrix is the same as the very first 2×2 matrix for i; in other words, part
of k is a 90o rotation about z. By analogy with the 2D case, we might reasonably
expect the quaternion cos θ + k sin θ to represent a rotation about
the z-axis by an arbitrary angle θ. Let’s multiply it by the vector [1, 0, 0]
and see what happens.

As in the 2D case, we need to “promote” the vector
into the complex domain; what’s different here is that quaternions have an
extra number. We’ll map [x, y, z] to the complex number 0 + xi + yj + zk,
so the vector [1, 0, 0] is simply i. Expanding the multiplication, we have

(cos θ + k sin θ)i = i cos θ + ki sin θ,

= i cos θ + j sin θ,

which corresponds to [cos θ, sin θ, 0], exactly what we would expect when
rotating the x-axis about the z-axis. So far, all is good. Let’s try a slightly

more general vector [1, 0, 1], which is represented in the complex domain as
i + k:

(cos θ + k sin θ)(i + k) = i cos θ + k cos θ + ki sin θ + k^2 sin θ
= i cos θ + j sin θ + k cos θ -sin θ.                                                              (8.13)

This result does not correspond to a vector at all, since it has a nonzero
value for w. The rotation in the xy-plane worked as expected, but unfortunately,
the z component did not come out right. There is unwanted
rotation in the zw-hyperplane. This is made perfectly clear by looking at
how (cos θ + k sin θ) is represented as a 4 × 4 matrix:

The upper-left 2 × 2 rotation matrix is the one we want; the lower-right
2 × 2 rotation matrix is not wanted.

Now we are left wondering if maybe we did something wrong.

Perhaps, instead, our problem is that we did the multiplication in the
wrong order. (After all, multiplication of i, j, and k is not commutative.)
Let’s try putting the vector on the left and the rotation quaternion on the
right:

(i + k)(cos θ + k sin θ) = i cos θ + ik sin θ + k cos θ + k^2 sin θ
= i cos θ − j sin θ + k cos θ − sin θ.

Comparing this to Equation (8.13), when the operands were in the opposite
order, we see that the only difference is the sign of the y-coordinate. At first
glance, it looks like this is actually worse. The rotation in the xz-plane that
we want got inverted; now we have a rotation by −θ. Meanwhile, the extra
rotation we didn’t want is exactly the same as before. But perhaps you can
already see the solution. If we use the opposite rotation, which corresponds
to using the conjugate of the quaternion, we fix both problems:

(i + k)(cos θ − k sin θ) = i cos θ + j sin θ − k cos θ + sin θ.

So, multiplying on the left by (cos θ + k sin θ) produced the rotation we
wanted, plus some extra rotation we didn’t want, and multiplication on

the right by the conjugate achieved the same rotation that was desired,
with the opposite undesired rotation. If we combine these two steps, the
unwanted rotation is canceled out, and we are left with only the rotation
we want. Well, not quite, we are left with twice the rotation we want, but
this is easily fixed by using θ/2 instead of θ. Of course, we knew that θ/2
would appear somewhere, but now we see the reason. Let’s summarize our
findings from the preceding paragraphs.

注意我们的变换公式是qvq^(-1),而这里是qvq*,有没有区别呢?

如果q是单位四元组,没有影响因为q^(-1)=q*.

如果q不是单位四元组,那么可以把q表示为q=au,其中u为单位四元组

那么qvq^(-1)=auvu^(-1)(1/a)=uvu^(-1)   

而qvq*=auvau*=auvu^(-1)a=a^2uvu^(-1)

说明qvq^(-1),无论q的长度如何变化,他所表示的始终是一个旋转

而qvq*,如果q的长度增加a倍,那么qvq*所表示的不光是一个旋转,而且还把v的长度增加了a^2倍

////////////////////////////////////////////////////////////////////////////////本段结束///////////////////////////////////////////////////////////////////

如果你想绕(xx,yy,zz)逆时针旋转dAngle度,可以用下面这个函数产生四元组,进而进行旋转

		Quaternion FromAngleAxis(const double dAngle,double xx,double yy,double zz)
		{
			double dHalfAngle=dAngle/2*3.14159265358979323846/180;
			w=cos(dHalfAngle);
			double dSinHalfAngle=sin(dHalfAngle);
			x=xx*dSinHalfAngle;
			y=yy*dSinHalfAngle;
			z=zz*dSinHalfAngle;
			return Quaternion(w,x,y,z);
		}

下面介绍四元组的插值运算

Because quaternions are represented by vectors, they are well suited for interpolation.
When an object is being animated, interpolation is useful for generating
intermediate orientations that fall between precalculated key frames.

The simplest type of interpolation is linear interpolation. For two unit quaternions
q_1 and q_2, the linearly interpolated quaternion q(t) is given by

The function q(t) changes smoothly along the line segment connecting q_1 and q_2
as t varies from 0 to 1. As shown in Figure 4.7, q(t ) does not maintain the unit
length of q_1 and q_2, but we can renormalize at each point by instead using the
function

There are two slight complications. First, the two quaternions q and
−q represent the same orientation, but may produce different results when

used as an argument to slerp. This problem doesn’t happen in 2D or 3D,
but the surface of a 4D hypersphere has a different topology than Euclidian
space. The solution is to choose the signs of q1and q2 such that the dot
product q1 · q2 is nonnegative. This has the effect of always selecting the
shortest rotational arc from q1 to q2. The second complication is that if
q1 and q2 are very close, then θ is very small, and thus sinθ  is also very
small, which will cause problems with the division. To avoid this, if sinθ
is very small, we will use simple linear interpolation. 

下面解释为什么q1·q2 >= 0时,q1到q2是最短路径

we considered the difference quaternion d = ba*, which describes the angular displacement
from orientation a to orientation b. (We assume unit quaternions and replace
the quaternion inverse with the conjugate.) If we expand the product
and examine the contents of d, we find that the w component is equal to
the dot product a · b

设b=<s_b;v_b>,a=<s_a;v_a>

则a*=<s_a;-v_a>

ba*=<s_b;v_b><s_a;-v_a>

     =s_bs_a+v_a·v_b-s_bv_a+s_av_b-v_b×v_a

What does this mean geometrically? Remember Euler’s rotation theorem:
we can rotate from the orientation a into the orientation b via a
single rotation about a carefully chosen axis. This uniquely determined
(up to a reversal of sign) axis and angle are precisely the ones encoded in
d. Remembering the relationship between the w component and the rotation
angle θ, we see that a·b = cos(θ/2), where θ is the amount of rotation
needed to go from the orientation a to the orientation b.

In summary, the quaternion dot product has an interpretation similar to
the vector dot product. The larger the absolute value of the quaternion dot
product a·b, the more “similar” are the angular displacements represented
by a and b. While the vector dot product gives the cosine of the angle
between vectors, the quaternion dot product gives the cosine of half of the
angle needed to rotate one quaternion into the other. For the purpose of
measuring similarity, usually we are interested only in the absolute value
of a · b, since a · b = −(a · −b), even though b and −b represent the same
angular displacement.

这就说明了为什么q1·q2 >= 0时,q1到q2是最短路径

下面给出四元组球形插值的函数

Quaternion Slerp(double dt,const Quaternion &q1,const Quaternion &q2,bool shortestPath)
	{
		double dCos=q1.Dot(q2);
		Quaternion q3;
		if(dCos<0 && shortestPath)
		{
			dCos=-dCos;
			q3=-q2;
		}	
		else
		{
			q3=q2;
		}

		if(abs(dCos)< 1 - 1e-6)
		{
			double dSin=sqrt(1-dCos*dCos);
			double dAngle=atan2(dSin,dCos);//这样写后面的结果为1，无需将结果单位化
			//double dAngle=atan(dSin/dCos);//不知道为何这样写的话，后面的插值结果长度不为1，需要将结果单位化
			double dSinInv=1/dSin;
			double dCoeff0=sin((1-dt)*dAngle)*dSinInv;
			double dCoeff1=sin(dt*dAngle)*dSinInv;
			Quaternion	q= dCoeff0*q1+dCoeff1*q3;
			//return q.Normalize();
			return q;
		}
		else//采用线性插值
		{
			Quaternion t=(1.0 - dt)*q1 + dt*q3;
			t=t.Normalize();
			return t;
		}


	}

下面介绍四元组到3x3矩阵的相互转换

Because a quaternion represents a rotation, its action on R^3 can be represented by a 3 × 3
matrix. We now show how to convert a quaternion q into a 3 × 3 matrix. 

a transformation A(v) is represented by the matrix (w_1 w_2 w_3),

where the columns wi are equal to A(i), A(j), and A(k). Thus, to represent a quaternion q by
a matrix, we set
w_1 = qiq^(−1), w_2 = qjq^(−1), and w3 = qkq^(−1)

四元组转3x3旋转矩阵的函数如下:

		operator Matrix3x3()
		{
			(*this)=(*this).Normalize();
			Matrix3x3 m;
			double ww=w*w;
			double xx=x*x;
			double yy=y*y;
			double zz=z*z;
			double xy=x*y;
			double xz=x*z;
			double xw=x*w;
			double yz=y*z;
			double yw=y*w;
			double zw=z*w;
			m(0,0)=ww+xx-yy-zz;
			m(0,1)=2*xy-2*zw;
			m(0,2)=2*xz+2*yw;
			m(1,0)=2*xy+2*zw;
			m(1,1)=ww-xx+yy-zz;
			m(1,2)=2*yz-2*xw;
			m(2,0)=2*xz-2*yw;
			m(2,1)=2*yz+2*xw;
			m(2,2)=ww-xx-yy+zz;
			return m;
		}

3x3旋转矩阵转四元组

Quaternion(Matrix3x3& m)
		{
			double d=m(0,0)+m(1,1)+m(2,2),max;
			max= m(0,0)>m(1,1) ? (m(0,0)>m(2,2) ? m(0,0) : m(2,2)) : (m(1,1)>m(2,2)?m(1,1):m(2,2) );
			if(d>max)
			{
				w=0.5*sqrt(d+1);
				x=0.25*(m(2,1)-m(1,2))/w;
				y=0.25*(m(0,2)-m(2,0))/w;
				z=0.25*(m(1,0)-m(0,1))/w;
			}
			else if(max == m(0,0))
			{
				x=0.5*sqrt(2*m(0,0)-d+1);
				w=0.25*(m(2,1)-m(1,2))/x;
				y=0.25*(m(1,0)+m(0,1))/x;
				z=0.25*(m(0,2)+m(2,0))/x;
			}
			else if(max== m(1,1))
			{
				y=0.5*sqrt(2*m(1,1)-d+1);
				w=0.25*(m(0,2)-m(2,0))/y;
				x=0.25*(m(1,0)+m(0,1))/y;
				z=0.25*(m(2,1)+m(1,2))/y;
			}
			else if(max == m(2,2))
			{
				z=0.5*sqrt(2*m(2,2)-d+1);
				w=0.25*(m(1,0)-m(0,1))/z;
				x=0.25*(m(0,2)+m(2,0))/z;
				y=0.25*(m(2,1)+m(1,2))/z;
			}
			
		}

main函数它们相互转换如下:

	Seamanj::Quaternion q;
	q.FromAngleAxis(90,1,1,0);
	q.Normalize();
	cout<<q.x<<' '<<q.y<<' '<<q.z<<' '<<q.w<<endl;
	Seamanj::Matrix3x3 m=(Seamanj::Matrix3x3)q;
	std::cout<<m<<std::endl;
	Seamanj::Matrix3x3 m1(m);
	Seamanj::Quaternion q1(m1);
	cout<<q1.x<<' '<<q1.y<<' '<<q1.z<<' '<<q1.w<<endl;

结果如下:

显示函数里调用四元组插值部分代码:

	Seamanj::Quaternion qRotateToBlueLine,qRotateToMagentaLine,qRotateToGreenLine,qRotateToRedLine,qRotateToCyanLine;
	Seamanj::Quaternion q_OriginOrientation(0,2,1,0),q_BlueLineOrientation,q_MagentaLineOrientation,q_GreenLineOrientation,q_RedLineOrientation,q_CyanLineOrientation;
	q_OriginOrientation=q_OriginOrientation.Normalize();
	qRotateToBlueLine.FromAngleAxis(dMyAngle,0,1,0);
	//cout<<"qRotateToBlueLine len:"<<qRotateToBlueLine.Norm()<<endl;
	qRotateToMagentaLine.FromAngleAxis(dMyAngle+90,0,1,0);
	//cout<<"qRotateToMagentaLine len:"<<qRotateToBlueLine.Norm()<<endl;
	qRotateToGreenLine.FromAngleAxis(dMyAngle,1,0,0);
	//cout<<"qRotateToGreenLine len:"<<qRotateToGreenLine.Norm()<<endl;
	qRotateToRedLine=Slerp(dt_Red_between_Blue_and_Green,qRotateToBlueLine,qRotateToGreenLine);
	//cout<<"qRotateToRedLine len:"<<qRotateToRedLine.Norm()<<endl;
	qRotateToCyanLine=Slerp(dt_Cyan_between_Magenta_and_Greeen,qRotateToMagentaLine,qRotateToGreenLine,false);//不用最路径，以看插值的连续性
	//cout<<"qRotateToCyanLine len:"<<qRotateToCyanLine.Norm()<<endl;

	q_BlueLineOrientation=qRotateToBlueLine*q_OriginOrientation*qRotateToBlueLine.UnitInverse();//BLUE LINE
	q_MagentaLineOrientation=qRotateToMagentaLine*q_OriginOrientation*qRotateToMagentaLine.UnitInverse();//MAGENTA LINE
	q_GreenLineOrientation=qRotateToGreenLine*q_OriginOrientation*qRotateToGreenLine.UnitInverse();//GREEN LINE
	q_RedLineOrientation=qRotateToRedLine*q_OriginOrientation*qRotateToRedLine.UnitInverse();//RED LINE
	q_CyanLineOrientation=qRotateToCyanLine*q_OriginOrientation*qRotateToCyanLine.UnitInverse();//CYAN LINE
	



	glBegin(GL_LINES);

	//用白色的线画坐标轴
	glColor3f(1,1,1);
	glVertex3f(0,0,0);
	glVertex3f(10,0,0);
	glVertex3f(0,0,0);
	glVertex3f(0,10,0);
	glVertex3f(0,0,0);
	glVertex3f(0,0,10);
	
	//画蓝线
	glColor3f(0,0,1);
	glVertex3f(0,0,0);
	glVertex3f(3*q_BlueLineOrientation.x,3*q_BlueLineOrientation.y,3*q_BlueLineOrientation.z);

	//画洋红色线
	glColor3f(1,0,1);
	glVertex3f(0,0,0);
	glVertex3f(3*q_MagentaLineOrientation.x,3*q_MagentaLineOrientation.y,3*q_MagentaLineOrientation.z);

	//画绿线
	glColor3f(0,1,0);
	glVertex3f(0,0,0);
	glVertex3f(3*q_GreenLineOrientation.x,3*q_GreenLineOrientation.y,3*q_GreenLineOrientation.z);

	//画红线
	glColor3f(1,0,0);
	glVertex3f(0,0,0);
	glVertex3f(3*q_RedLineOrientation.x,3*q_RedLineOrientation.y,3*q_RedLineOrientation.z);
	
	//画青色线
	glColor3f(0,1,1);
	glVertex3f(0,0,0);
	glVertex3f(3*q_CyanLineOrientation.x,3*q_CyanLineOrientation.y,3*q_CyanLineOrientation.z);

	glEnd();

运行结果如下:

红色线在蓝色和绿色之间按0到1再到0,如此循环进行球形插值,即红色线将会在蓝色线和绿色线之间摆动

青色线0.5的比例在洋红色线和绿色线之间进行球形插值

最后给源代码:

Quaternion.h头文件

#ifndef _QUATERNION
#define _QUATERNION
#include <iostream>
#include <iomanip>
namespace Seamanj
{
	class Matrix3x3
	{
	public:
		Matrix3x3()
		{
			for(int i=0;i<3;i++)
				for(int j=0;j<3;j++)
					data[i][j]=0.0;
		}
		Matrix3x3(double* pd)
		{
			for(int i=0;i<3;i++)
				for(int j=0;j<3;j++)
					data[i][j]=pd[i*3+j];
		}
		Matrix3x3(const Matrix3x3& m)
		{
			for(int i=0;i<3;i++)
				for(int j=0;j<3;j++)
					data[i][j]=m(i,j);
		}

		double data[3][3];
		double& operator()(int i,int j)
		{
			return data[i][j];
		}
		double operator()(int i,int j) const
		{
			return data[i][j];
		}
		friend std::ostream& operator<<(std::ostream& os,Matrix3x3& m);
	};
	std::ostream& operator<<(std::ostream& os,Matrix3x3& m)
	{
		for(int i=0;i<3;i++)
		{
			for(int j=0;j<3;j++)
			{
				os<<std::setw(10)<<((m(i,j)<1e-6 && m(i,j)>-1e-6)?0.0:m(i,j))<<'\t';
			}
			os<<std::endl;
		}
		return os;
	}
	class Quaternion
	{
	public:
		double w,x,y,z;
		Quaternion(double ww=1,double xx=0,double yy=0,double zz=0)
		{
			w=ww;
			x=xx;
			y=yy;
			z=zz;
		}
		Quaternion(Matrix3x3& m)
		{
			double d=m(0,0)+m(1,1)+m(2,2),max;
			max= m(0,0)>m(1,1) ? (m(0,0)>m(2,2) ? m(0,0) : m(2,2)) : (m(1,1)>m(2,2)?m(1,1):m(2,2) );
			if(d>max)
			{
				w=0.5*sqrt(d+1);
				x=0.25*(m(2,1)-m(1,2))/w;
				y=0.25*(m(0,2)-m(2,0))/w;
				z=0.25*(m(1,0)-m(0,1))/w;
			}
			else if(max == m(0,0))
			{
				x=0.5*sqrt(2*m(0,0)-d+1);
				w=0.25*(m(2,1)-m(1,2))/x;
				y=0.25*(m(1,0)+m(0,1))/x;
				z=0.25*(m(0,2)+m(2,0))/x;
			}
			else if(max== m(1,1))
			{
				y=0.5*sqrt(2*m(1,1)-d+1);
				w=0.25*(m(0,2)-m(2,0))/y;
				x=0.25*(m(1,0)+m(0,1))/y;
				z=0.25*(m(2,1)+m(1,2))/y;
			}
			else if(max == m(2,2))
			{
				z=0.5*sqrt(2*m(2,2)-d+1);
				w=0.25*(m(1,0)-m(0,1))/z;
				x=0.25*(m(0,2)+m(2,0))/z;
				y=0.25*(m(2,1)+m(1,2))/z;
			}
			
		}
		operator Matrix3x3()
		{
			(*this)=(*this).Normalize();
			Matrix3x3 m;
			double ww=w*w;
			double xx=x*x;
			double yy=y*y;
			double zz=z*z;
			double xy=x*y;
			double xz=x*z;
			double xw=x*w;
			double yz=y*z;
			double yw=y*w;
			double zw=z*w;
			m(0,0)=ww+xx-yy-zz;
			m(0,1)=2*xy-2*zw;
			m(0,2)=2*xz+2*yw;
			m(1,0)=2*xy+2*zw;
			m(1,1)=ww-xx+yy-zz;
			m(1,2)=2*yz-2*xw;
			m(2,0)=2*xz-2*yw;
			m(2,1)=2*yz+2*xw;
			m(2,2)=ww-xx-yy+zz;
			return m;
		}


		Quaternion operator*(Quaternion q)
		{
			return Quaternion(w*q.w-x*q.x-y*q.y-z*q.z,w*q.x+x*q.w+y*q.z-z*q.y,
				w*q.y-x*q.z+y*q.w+z*q.x,w*q.z+x*q.y-y*q.x+z*q.w);
		}
		friend Quaternion operator*(double dScalar,const Quaternion& q);
		Quaternion operator*(const double d)
		{
			return Quaternion(w*d,x*d,y*d,z*d);
		}
		Quaternion operator+ (const Quaternion& q) const
		{
			return Quaternion(w+q.w,x+q.x,y+q.y,z+q.z);
		}
		Quaternion operator- (const Quaternion& q) const
		{
			return Quaternion(w-q.w,x-q.x,y-q.y,z-q.z);
		}

		Quaternion operator- () const
		{
			return Quaternion(-w,-x,-y,-z);
		}
		Quaternion UnitInverse() const
		{
			// assert:  'this' is unit length
			return Quaternion(w,-x,-y,-z);
		}
		double Norm() const
		{
			return w*w+x*x+y*y+z*z;
		}
		Quaternion Normalize()
		{
			double len=Norm();
			double factor=1/sqrt(len);
			*this=*this * factor;
			return *this;
		}
		Quaternion FromAngleAxis(const double dAngle,double xx,double yy,double zz)
		{
			double dHalfAngle=dAngle/2*3.14159265358979323846/180;
			w=cos(dHalfAngle);
			double dSinHalfAngle=sin(dHalfAngle);
			x=xx*dSinHalfAngle;
			y=yy*dSinHalfAngle;
			z=zz*dSinHalfAngle;
			return Quaternion(w,x,y,z);
		}
		double Dot(const Quaternion& q) const  // dot product
		{
			return w*q.w+x*q.x+y*q.y+z*q.z;
		}
		friend Quaternion Slerp(double dt,const Quaternion &q1,const Quaternion &q2,bool shortestPath=true);
	};
	Quaternion operator*(double d,const Quaternion& q)
	{
		return Quaternion(q.w*d,q.x*d,q.y*d,q.z*d);
	}
	Quaternion Slerp(double dt,const Quaternion &q1,const Quaternion &q2,bool shortestPath)
	{
		double dCos=q1.Dot(q2);
		Quaternion q3;
		if(dCos<0 && shortestPath)
		{
			dCos=-dCos;
			q3=-q2;
		}	
		else
		{
			q3=q2;
		}

		if(abs(dCos)< 1 - 1e-6)
		{
			double dSin=sqrt(1-dCos*dCos);
			double dAngle=atan2(dSin,dCos);//这样写后面的结果为1，无需将结果单位化
			//double dAngle=atan(dSin/dCos);//不知道为何这样写的话，后面的插值结果长度不为1，需要将结果单位化
			double dSinInv=1/dSin;
			double dCoeff0=sin((1-dt)*dAngle)*dSinInv;
			double dCoeff1=sin(dt*dAngle)*dSinInv;
			Quaternion	q= dCoeff0*q1+dCoeff1*q3;
			//return q.Normalize();
			return q;
		}
		else//采用线性插值
		{
			Quaternion t=(1.0 - dt)*q1 + dt*q3;
			t=t.Normalize();
			return t;
		}


	}

}

#endif _QUATERNION

main.cpp头文件

//1 add Cg Frame
//2 add fragment program
//3 get and set variable in vertex shader and fragment shader
//4 add Matrix<--->Quaternion 
#include <stdio.h>
#include <stdlib.h>
#include <cg/cg.h>
#include <cg/cgGL.h>
#include <gl/glut.h>
#include <math.h>
#include <assert.h>
#include <iostream>
#include "Quaternion.h"
using std::cout;
using std::endl;
static CGcontext myCgContext;
static CGprofile myCgVertexProfile;
static CGprogram myCgVertexProgram;
//2 begin
static CGprofile myCgFragmentProfile;
static CGprogram myCgFragmentProgram;
//2 end
static const char* myProgramName="Quaternion";
static const char* myVertexProgramFileName="MyVertex.cg";
static const char* myVertexProgramEntryFunctionName="MyVertexEntry";

//3 begin
static CGparameter myCgVertexParam_modelViewProj;      
static double dMyAngle=0;
static double dt_Red_between_Blue_and_Green=0;
static double dt_Cyan_between_Magenta_and_Greeen=0;
static float myProjectionMatrix[16];
//3 end

static void checkForCgError(const char *situation)
{
	CGerror error;
	const char *string = cgGetLastErrorString(&error);

	if (error != CG_NO_ERROR) 
	{
		printf("%s: %s: %s\n",myProgramName, situation, string);
		if (error == CG_COMPILER_ERROR) 
			printf("%s\n", cgGetLastListing(myCgContext));
		exit(1);
	}
}
static void display(void);
static void keyboard(unsigned char c,int x,int y);
static void reshape(int width, int height);
int main(int argc,char **argv)
{
	glutInitWindowSize(400,400);
	glutInitDisplayMode(GLUT_RGB | GLUT_DOUBLE | GLUT_DEPTH);
	glutInit(&argc,argv);
	glutCreateWindow(myProgramName);//窗口名字为我们的程序名字
	glutDisplayFunc(display);
	glutKeyboardFunc(keyboard);
	glutReshapeFunc(reshape);
	glClearColor(0.1,0.1,0.1,0.0);
	glEnable(GL_DEPTH_TEST);
	


	myCgContext=cgCreateContext();
	checkForCgError("creating context");
	cgGLSetDebugMode(CG_FALSE);
	cgSetParameterSettingMode(myCgContext,CG_DEFERRED_PARAMETER_SETTING);
	myCgVertexProfile=cgGLGetLatestProfile(CG_GL_VERTEX);
	cgGLSetOptimalOptions(myCgVertexProfile);
	checkForCgError("selecting vertex profile");
	myCgVertexProgram=cgCreateProgramFromFile(
		myCgContext,CG_SOURCE,myVertexProgramFileName,myCgVertexProfile,myVertexProgramEntryFunctionName,NULL);
	checkForCgError("creating vertex program from file");
	cgGLLoadProgram(myCgVertexProgram);
	checkForCgError("loading vertex program");
	//3 begin
	myCgVertexParam_modelViewProj =
	cgGetNamedParameter(myCgVertexProgram, "modelViewProj");
	checkForCgError("could not get modelViewProj parameter");
	//3 end
	//2 begin
	myCgFragmentProfile=cgGLGetLatestProfile(CG_GL_FRAGMENT);
	cgGLSetOptimalOptions(myCgFragmentProfile);
	checkForCgError("selecting fragment profile");
	myCgFragmentProgram=cgCreateProgram(
		myCgContext,CG_SOURCE, "float4 main( float4 c:COLOR) : COLOR { return c; }",myCgFragmentProfile,"main",NULL);
	checkForCgError("creating fragment program from string");
	cgGLLoadProgram(myCgFragmentProgram);
	checkForCgError("loading fragment program");
	//2 end
	//4 begin
	Seamanj::Quaternion q;
	q.FromAngleAxis(90,1,1,0);
	q.Normalize();
	cout<<q.x<<' '<<q.y<<' '<<q.z<<' '<<q.w<<endl;
	Seamanj::Matrix3x3 m=(Seamanj::Matrix3x3)q;
	std::cout<<m<<std::endl;
	Seamanj::Matrix3x3 m1(m);
	Seamanj::Quaternion q1(m1);
	cout<<q1.x<<' '<<q1.y<<' '<<q1.z<<' '<<q1.w<<endl;
	//4 end
	glutMainLoop();
	return 0;
}
//3 begin
static void buildPerspectiveMatrix(double, double,
                                   double, double, float m[16]);

static void reshape(int width, int height)
{
  double aspectRatio = (float) width / (float) height;
  double fieldOfView = 40.0; /* Degrees */

  /* Build projection matrix once. */
  buildPerspectiveMatrix(fieldOfView, aspectRatio,
                         1.0, 20.0,  /* Znear and Zfar */
                         myProjectionMatrix);
  glViewport(0, 0, width, height);
}

static const double myPi = 3.14159265358979323846;
static void buildPerspectiveMatrix(double fieldOfView,
                                   double aspectRatio,
                                   double zNear, double zFar,
                                   float m[16])
{
  double sine, cotangent, deltaZ;
  double radians = fieldOfView / 2.0 * myPi / 180.0;
  
  deltaZ = zFar - zNear;
  sine = sin(radians);
  /* Should be non-zero to avoid division by zero. */
  assert(deltaZ);
  assert(sine);
  assert(aspectRatio);
  cotangent = cos(radians) / sine;

  m[0*4+0] = cotangent / aspectRatio;
  m[0*4+1] = 0.0;
  m[0*4+2] = 0.0;
  m[0*4+3] = 0.0;
  
  m[1*4+0] = 0.0;
  m[1*4+1] = cotangent;
  m[1*4+2] = 0.0;
  m[1*4+3] = 0.0;
  
  m[2*4+0] = 0.0;
  m[2*4+1] = 0.0;
  m[2*4+2] = -(zFar + zNear) / deltaZ;
  m[2*4+3] = -2 * zNear * zFar / deltaZ;
  
  m[3*4+0] = 0.0;
  m[3*4+1] = 0.0;
  m[3*4+2] = -1;
  m[3*4+3] = 0;
}

/* Build a row-major (C-style) 4x4 matrix transform based on the
   parameters for gluLookAt. */
static void buildLookAtMatrix(double eyex, double eyey, double eyez,
                              double centerx, double centery, double centerz,
                              double upx, double upy, double upz,
                              float m[16])
{
  double x[3], y[3], z[3], mag;

  /* Difference eye and center vectors to make Z vector. */
  z[0] = eyex - centerx;
  z[1] = eyey - centery;
  z[2] = eyez - centerz;
  /* Normalize Z. */
  mag = sqrt(z[0]*z[0] + z[1]*z[1] + z[2]*z[2]);
  if (mag) {
    z[0] /= mag;
    z[1] /= mag;
    z[2] /= mag;
  }

  /* Up vector makes Y vector. */
  y[0] = upx;
  y[1] = upy;
  y[2] = upz;

  /* X vector = Y cross Z. */
  x[0] =  y[1]*z[2] - y[2]*z[1];
  x[1] = -y[0]*z[2] + y[2]*z[0];
  x[2] =  y[0]*z[1] - y[1]*z[0];

  /* Recompute Y = Z cross X. */
  y[0] =  z[1]*x[2] - z[2]*x[1];
  y[1] = -z[0]*x[2] + z[2]*x[0];
  y[2] =  z[0]*x[1] - z[1]*x[0];

  /* Normalize X. */
  mag = sqrt(x[0]*x[0] + x[1]*x[1] + x[2]*x[2]);
  if (mag) {
    x[0] /= mag;
    x[1] /= mag;
    x[2] /= mag;
  }

  /* Normalize Y. */
  mag = sqrt(y[0]*y[0] + y[1]*y[1] + y[2]*y[2]);
  if (mag) {
    y[0] /= mag;
    y[1] /= mag;
    y[2] /= mag;
  }

  /* Build resulting view matrix. */
  m[0*4+0] = x[0];  m[0*4+1] = x[1];
  m[0*4+2] = x[2];  m[0*4+3] = -x[0]*eyex + -x[1]*eyey + -x[2]*eyez;

  m[1*4+0] = y[0];  m[1*4+1] = y[1];
  m[1*4+2] = y[2];  m[1*4+3] = -y[0]*eyex + -y[1]*eyey + -y[2]*eyez;

  m[2*4+0] = z[0];  m[2*4+1] = z[1];
  m[2*4+2] = z[2];  m[2*4+3] = -z[0]*eyex + -z[1]*eyey + -z[2]*eyez;

  m[3*4+0] = 0.0;   m[3*4+1] = 0.0;  m[3*4+2] = 0.0;  m[3*4+3] = 1.0;
}



static void makeRotateMatrix(float angle,
                             float ax, float ay, float az,
                             float m[16])
{
  float radians, sine, cosine, ab, bc, ca, tx, ty, tz;
  float axis[3];
  float mag;

  axis[0] = ax;
  axis[1] = ay;
  axis[2] = az;
  mag = sqrt(axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]);
  if (mag) {
    axis[0] /= mag;
    axis[1] /= mag;
    axis[2] /= mag;
  }

  radians = angle * myPi / 180.0;
  sine = sin(radians);
  cosine = cos(radians);
  ab = axis[0] * axis[1] * (1 - cosine);
  bc = axis[1] * axis[2] * (1 - cosine);
  ca = axis[2] * axis[0] * (1 - cosine);
  tx = axis[0] * axis[0];
  ty = axis[1] * axis[1];
  tz = axis[2] * axis[2];

  m[0]  = cosine + (1 - cosine) * tx;
  m[1]  = ab - axis[2] * sine;
  m[2]  = ca + axis[1] * sine;
  m[3]  = 0.0f;
  m[4]  = ab + axis[2] * sine;
  m[5]  = cosine + (1 - cosine) * ty;
  m[6]  = bc - axis[0] * sine;
  m[7]  = 0.0f;
  m[8]  = ca - axis[1] * sine;
  m[9]  = bc + axis[0] * sine;
  m[10] = cosine + (1 - cosine) * tz;
  m[11] = 0;
  m[12] = 0;
  m[13] = 0;
  m[14] = 0;
  m[15] = 1;
}

static void makeTranslateMatrix(float x, float y, float z, float m[16])
{
  m[0]  = 1;  m[1]  = 0;  m[2]  = 0;  m[3]  = x;
  m[4]  = 0;  m[5]  = 1;  m[6]  = 0;  m[7]  = y;
  m[8]  = 0;  m[9]  = 0;  m[10] = 1;  m[11] = z;
  m[12] = 0;  m[13] = 0;  m[14] = 0;  m[15] = 1;
}

/* Simple 4x4 matrix by 4x4 matrix multiply. */
static void multMatrix(float dst[16],
                       const float src1[16], const float src2[16])
{
  float tmp[16];
  int i, j;

  for (i=0; i<4; i++) {
    for (j=0; j<4; j++) {
      tmp[i*4+j] = src1[i*4+0] * src2[0*4+j] +
                   src1[i*4+1] * src2[1*4+j] +
                   src1[i*4+2] * src2[2*4+j] +
                   src1[i*4+3] * src2[3*4+j];
    }
  }
  /* Copy result to dst (so dst can also be src1 or src2). */
  for (i=0; i<16; i++)
    dst[i] = tmp[i];
}

//3 end
static void display(void)
{
	//3 begin
	  float translateMatrix[16], rotateMatrix[16],
        modelMatrix[16], viewMatrix[16],
        modelViewMatrix[16], modelViewProjMatrix[16];
	 buildLookAtMatrix(0,0, 13,  /* eye position */
					   0, 0, 0, /* view center */
					   0, 1, 0, /* up vector */
                       viewMatrix);
	//3 end
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	cgGLBindProgram(myCgVertexProgram);
	checkForCgError("binding vertex program");
	cgGLEnableProfile(myCgVertexProfile);
	checkForCgError("enabling vertex profile");
	//2 begin
	cgGLBindProgram(myCgFragmentProgram);
	checkForCgError("binding fragment program");
	cgGLEnableProfile(myCgFragmentProfile);
	checkForCgError("enabling fragment profile");
	//2 end
	//3 begin
	/*** Render green wireframe sphere ***/

	/* modelView = rotateMatrix * translateMatrix */
	makeRotateMatrix(0, 0, 0, 1, rotateMatrix);
	makeTranslateMatrix(0, 0, 0, translateMatrix);
	multMatrix(modelMatrix, translateMatrix, rotateMatrix);

	/* modelViewMatrix = viewMatrix * modelMatrix */
	multMatrix(modelViewMatrix, viewMatrix, modelMatrix);

	/* modelViewProj = projectionMatrix * modelViewMatrix */
	multMatrix(modelViewProjMatrix, myProjectionMatrix, modelViewMatrix);

	/* Set matrix parameter with row-major matrix. */
	cgSetMatrixParameterfr(myCgVertexParam_modelViewProj, modelViewProjMatrix);
	cgUpdateProgramParameters(myCgVertexProgram);
	cgUpdateProgramParameters(myCgFragmentProgram);
	

	Seamanj::Quaternion qRotateToBlueLine,qRotateToMagentaLine,qRotateToGreenLine,qRotateToRedLine,qRotateToCyanLine;
	Seamanj::Quaternion q_OriginOrientation(0,2,1,0),q_BlueLineOrientation,q_MagentaLineOrientation,q_GreenLineOrientation,q_RedLineOrientation,q_CyanLineOrientation;
	q_OriginOrientation=q_OriginOrientation.Normalize();
	qRotateToBlueLine.FromAngleAxis(dMyAngle,0,1,0);
	//cout<<"qRotateToBlueLine len:"<<qRotateToBlueLine.Norm()<<endl;
	qRotateToMagentaLine.FromAngleAxis(dMyAngle+90,0,1,0);
	//cout<<"qRotateToMagentaLine len:"<<qRotateToBlueLine.Norm()<<endl;
	qRotateToGreenLine.FromAngleAxis(dMyAngle,1,0,0);
	//cout<<"qRotateToGreenLine len:"<<qRotateToGreenLine.Norm()<<endl;
	qRotateToRedLine=Slerp(dt_Red_between_Blue_and_Green,qRotateToBlueLine,qRotateToGreenLine);
	//cout<<"qRotateToRedLine len:"<<qRotateToRedLine.Norm()<<endl;
	qRotateToCyanLine=Slerp(dt_Cyan_between_Magenta_and_Greeen,qRotateToMagentaLine,qRotateToGreenLine,false);//不用最路径，以看插值的连续性
	//cout<<"qRotateToCyanLine len:"<<qRotateToCyanLine.Norm()<<endl;

	q_BlueLineOrientation=qRotateToBlueLine*q_OriginOrientation*qRotateToBlueLine.UnitInverse();//BLUE LINE
	q_MagentaLineOrientation=qRotateToMagentaLine*q_OriginOrientation*qRotateToMagentaLine.UnitInverse();//MAGENTA LINE
	q_GreenLineOrientation=qRotateToGreenLine*q_OriginOrientation*qRotateToGreenLine.UnitInverse();//GREEN LINE
	q_RedLineOrientation=qRotateToRedLine*q_OriginOrientation*qRotateToRedLine.UnitInverse();//RED LINE
	q_CyanLineOrientation=qRotateToCyanLine*q_OriginOrientation*qRotateToCyanLine.UnitInverse();//CYAN LINE
	



	glBegin(GL_LINES);

	//用白色的线画坐标轴
	glColor3f(1,1,1);
	glVertex3f(0,0,0);
	glVertex3f(10,0,0);
	glVertex3f(0,0,0);
	glVertex3f(0,10,0);
	glVertex3f(0,0,0);
	glVertex3f(0,0,10);
	
	//画蓝线
	glColor3f(0,0,1);
	glVertex3f(0,0,0);
	glVertex3f(3*q_BlueLineOrientation.x,3*q_BlueLineOrientation.y,3*q_BlueLineOrientation.z);

	//画洋红色线
	glColor3f(1,0,1);
	glVertex3f(0,0,0);
	glVertex3f(3*q_MagentaLineOrientation.x,3*q_MagentaLineOrientation.y,3*q_MagentaLineOrientation.z);

	//画绿线
	glColor3f(0,1,0);
	glVertex3f(0,0,0);
	glVertex3f(3*q_GreenLineOrientation.x,3*q_GreenLineOrientation.y,3*q_GreenLineOrientation.z);

	//画红线
	glColor3f(1,0,0);
	glVertex3f(0,0,0);
	glVertex3f(3*q_RedLineOrientation.x,3*q_RedLineOrientation.y,3*q_RedLineOrientation.z);
	
	//画青色线
	glColor3f(0,1,1);
	glVertex3f(0,0,0);
	glVertex3f(3*q_CyanLineOrientation.x,3*q_CyanLineOrientation.y,3*q_CyanLineOrientation.z);

	glEnd();



	//3 end
	cgGLDisableProfile(myCgVertexProfile);
	checkForCgError("disabling vertex profile");
	//2 begin
	cgGLDisableProfile(myCgFragmentProfile);
	checkForCgError("disabling fragment profile");
	//2 end
	glutSwapBuffers();
}
//3 begin
static int direction=1;
static double vel=1;
static void idle(void)
{
	dMyAngle+=vel;
	dt_Red_between_Blue_and_Green+=direction*0.05;
	if(dt_Red_between_Blue_and_Green>1)
		direction=-1;
	else if(dt_Red_between_Blue_and_Green < 0)
		direction=1;
  glutPostRedisplay();
}
//3 end
static void keyboard(unsigned char c,int x,int y)
{
	//3 begin
	static int animating = 0;
	//3 end
	switch(c)
	{
	//3 begin
	case ' ':
		animating = !animating; /* Toggle */
		if (animating) 
		{
		  glutIdleFunc(idle);
		}
		else
		{
		  glutIdleFunc(NULL);
	    }
		break;
	case 'w':
		dt_Cyan_between_Magenta_and_Greeen+=0.01;
		std::cout<<dt_Cyan_between_Magenta_and_Greeen<<std::endl;
		break;
		
	case 's':
		dt_Cyan_between_Magenta_and_Greeen-=0.01;
		std::cout<<dt_Cyan_between_Magenta_and_Greeen<<std::endl;
		break;
	case 'a':
		vel+=0.1;
		cout<<"vel:"<<vel<<endl;
		break;
	case 'd':
		vel-=0.1;
		cout<<"vel:"<<vel<<endl;
		break;
	//3 end
	case 27:
		cgDestroyProgram(myCgVertexProgram);
		cgDestroyContext(myCgContext);
		exit(0);
		break;
	}
}

MyVertex.cg

void MyVertexEntry(float4 position : POSITION,
				   float4 color:COLOR,
                 out float4 oPosition : POSITION,
                 out float4 oColor:COLOR,
             uniform float4x4 modelViewProj)
{
  // Transform position from object space to clip space
  oPosition = mul(modelViewProj, position);
  oColor=color;
}

对应的可执行文件以及源文件请点这里下载

四元组还有对数和指数的概念,我现在也没有研究,等有时间补上.OK大家下个专题再见了