目标
定义四元数，并实现四元数的运算。
源代码：
实现
关于四元数的意义、四元数与复数、四元数的共轭等概念，网上有太资料来学习，本文就不再多写了。我在实现四元数时，主要参考《3D数学基础：图形与游戏开发》第10章的内容。
四元数记法
四元数包含一个标量分量和一个3D向量分量。经常记标量分量为 w，记向量分量为单一 v 或分开的 x, y ,z。两种记法的分别如下：
- [w, v]
- [w, x, y, z]
在代码中，我把 w 写在 x, y, z 的后面，只要不影响计算即可。
/**
* 四元数
*
* @author yanmaoyuan
*
*/
public class Quaternion {
public float x, y, z, w;
public Quaternion() {
x = y = z = 0;
w = 1;
}
public Quaternion(float x, float y, float z, float w) {
this.x = x;
this.y = y;
this.z = z;
this.w = w;
}
public Quaternion(Quaternion q) {
this.x = q.x;
this.y = q.y;
this.z = q.z;
this.w = q.w;
}
}
角轴旋转
已知由单位向量 n 定义的旋转轴，以及旋转的弧度 θ，用四元数来记录这个旋转如下：
q = [cos(θ/2), sin(θ/2)n]
把单位向量n展开为 x, y, z 三个分量，记为：
q = [cos(θ/2), sin(θ/2) * x, sin(θ/2) * y, sin(θ/2) * z]
在右手坐标系中，可以使用右手定则来确定 n 和 θ 的正方向：右手握拳，让大拇指伸出朝向 n 的正方向；此时其余四指的方向（逆时针）就是 θ 的正方向。
代码实现如下：
/**
* 使用角轴对来计算四元数
*
* @param axis 由单位向量定义的旋转轴
* @param angle 旋转角（弧度制）
*/
public Quaternion(Vector3f axis, float angle) {
axis.normalizeLocal();
float cosHalfAngle = (float) Math.cos(angle * 0.5);
float sinHalfAngle = (float) Math.sin(angle * 0.5);
w = cosHalfAngle;
x = axis.x * sinHalfAngle;
y = axis.y * sinHalfAngle;
z = axis.z * sinHalfAngle;
}
/**
* 使用角轴对来计算四元数
* @param axis 由单位向量定义的旋转轴
* @param angle 旋转角（弧度制）
* @return
*/
public Quaternion fromAxisAngle(Vector3f axis, float angle) {
axis.normalizeLocal();
float sinHalfAngle = (float) Math.sin(angle * 0.5);
float cosHalfAngle = (float) Math.cos(angle * 0.5);
w = cosHalfAngle;
x = axis.x * sinHalfAngle;
y = axis.y * sinHalfAngle;
z = axis.z * sinHalfAngle;
return this;
}
如果是绕X、Y、Z轴旋转，实现还会更简单一点。
/**
* 绕X轴旋转
* @param angle
* @return
*/
public Quaternion rotateX(float angle) {
float sinHalfAngle = (float) Math.sin(angle * 0.5);
float cosHalfAngle = (float) Math.cos(angle * 0.5);
w = cosHalfAngle;
x = sinHalfAngle;
y = 0;
z = 0;
return this;
}
/**
* 绕Y轴旋转
* @param angle
* @return
*/
public Quaternion rotateY(float angle) {
float sinHalfAngle = (float) Math.sin(angle * 0.5);
float cosHalfAngle = (float) Math.cos(angle * 0.5);
w = cosHalfAngle;
x = 0;
y = sinHalfAngle;
z = 0;
return this;
}
/**
* 绕Z轴旋转
* @param angle
* @return
*/
public Quaternion rotateZ(float angle) {
float sinHalfAngle = (float) Math.sin(angle * 0.5);
float cosHalfAngle = (float) Math.cos(angle * 0.5);
w = cosHalfAngle;
x = 0;
y = 0;
z = sinHalfAngle;
return this;
}
记住，q 的 w 分量与 θ 有关，但它们不是同一回事。同样，v与n也有关系但不完全相同。
负四元数
四元数能求负：将每个分量都变负即可。
/**
* 负四元数
*
* @return
*/
public Quaternion negate() {
return new Quaternion(-x, -y, -z, -w);
}
/**
* 负四元数
*
* @return
*/
public Quaternion negateLocal() {
x = -x;
y = -y;
z = -z;
w = -w;
return this;
}
q 和 -q 代表的实际角位移是相同，很奇怪吧！如果我们将 θ 加上360°的倍数，不会改变 q 代表的角位移，但它使 q 的四个分量都变负了。因此，3D中的任意角位移都有两种不同的四元数表示方法，它们互相为负。
单位四元数
几何上，存在两个“单位”四元数，它们代表没有角位移：[1, 0] 和 [-1, 0]（注意这里的 0 代表零向量）。
- 当 θ 是360°的偶数倍时，有第一种形式，cos(θ/2)=1；
- 当 θ 是360°的奇数倍时，有第二种形式，cos(θ/2)=-1。
这两种情况下，都有 sin(θ/2)=0，所以 n 的值无关紧要。它的意义在于：当旋转角 θ 是360°的整数倍时，方位并没有改变，并且旋转轴也是无关紧要的。
数学上，实际只有一个单位四元数：[1, 0]。用任意四元数q乘以单位四元数 [1, 0]，结果任是 q。任意四元数 q 乘以另一个“几何单位”四元数 [-1, 0] 时得到 -q。几何上，因为 q 和 -q 代表的角位移相同，可认为结果是相同的。但在数学上，q 和 -q 不相等，所以 [-1, 0] 并不是“真正”的单位四元数。
代码如下：
// 单位四元数
public final static Quaternion IDENTITY = new Quaternion(0, 0, 0, 1);
/**
* 将四元数设为 (0, 0, 0, 1)
*/
public void loadIdentity() {
x = y = z = 0;
w = 1;
}
/**
* 判断是否为单位四元数
*
* @return true
*/
public boolean isIdentity() {
if (x == 0 && y == 0 && z == 0 && w == 1) {
return true;
} else {
return false;
}
}
四元数的模
四元数求模公式与向量求模是类似的。
/**
* 求长度的平方
*
* @return
*/
public float lengthSquared() {
return x * x + y * y + z * z + w * w;
}
/**
* 求四元数的模，或者叫长度。
*
* @return
*/
public float length() {
return (float) Math.sqrt(x * x + y * y + z * z + w * w);
}
若旋转轴n是单位向量，就有 |q| = 1。如果为了使用四元数来表示方位，我们仅使用符合这个规则的单位四元数。
规范化四元数
规范化四元数的定义与向量的规范化类似。
/**
* 规范化四元数
*
* @return
*/
public Quaternion normalize() {
float length = x * x + y * y + z * z + w * w;
if (length != 1f && length != 0f) {
length = (float) (1.0 / Math.sqrt(length));
return new Quaternion(length * x, length * y, length * z, length * w);
}
return new Quaternion(x, y, z, w);
}
/**
* 规范化四元数
*
* @return
*/
public Quaternion normalizeLocal() {
float length = x * x + y * y + z * z + w * w;
if (length != 1f && length != 0f) {
length = (float) (1.0 / Math.sqrt(length));
x *= length;
y *= length;
z *= length;
w *= length;
}
return this;
}
四元数的共轭
四元数的共轭记作q*，可以通过让四元数的向量部分变负来获得。
q* = [w, v]* = [w, -v]
= [w, -x, -y, -z]
代码实现如下：
/**
* 求共轭四元数
*
* @return
*/
public Quaternion conjugate() {
return new Quaternion(-x, -y, -z, w);
}
/**
* 求共轭四元数
*
* @return
*/
public Quaternion conjugateLocal() {
x = -x;
y = -y;
z = -z;
return this;
}
它的几何意义是绕旋转轴 n 的反方向旋转 θ 角。
四元数的逆
四元数的逆记作 q^-1，定义为四元数的共轭除以它的模。
q^-1 = q* / |q|
代码实现如下：
/**
* 求四元数的逆。 四元数的逆 = 四元数的共轭 / 四元数的模
*
* @return
*/
public Quaternion inverse() {
float length = x * x + y * y + z * z + w * w;
if (length != 1f && length != 0f) {
length = (float) (1.0 / Math.sqrt(length));
return new Quaternion(-x * length, -y * length, -z * length, w * length);
}
// 单位四元数的逆和共轭是相等的。
return new Quaternion(-x, -y, -z, w);
}
/**
* 求四元数的逆。 四元数的逆 = 四元数的共轭 / 四元数的模
*
* @return
*/
public Quaternion inverseLocal() {
float length = x * x + y * y + z * z + w * w;
if (length != 1f && length != 0f) {
length = (float) (1.0 / Math.sqrt(length));
x *= length;
y *= length;
z *= length;
}
x = -x;
y = -y;
z = -z;
return this;
}
一个四元数 q 乘以它的逆 q^-1，即可得到单位四元数[1, 0]。
上面的公式是四元数求逆的正式定义，但是由于我们只使用单位四元数，所以四元数的逆和共轭是相等的。
四元数乘法
四元数的乘法是根据其复数解释来相乘的，推导过程就不写了，直接上代码。
/**
* 四元数乘法，或者叫叉乘。
*
* @param q
* @return
*/
public Quaternion mult(Quaternion q) {
float qw = q.w, qx = q.x, qy = q.y, qz = q.z;
float rw = w * qw - x * qx - y * qy - z * qz;
float rx = w * qx + x * qw + y * qz - z * qy;
float ry = w * qy + y * qw + z * qx - x * qz;
float rz = w * qz + z * qw + x * qy - y * qx;
return new Quaternion(rx, ry, rz, rw);
}
/**
* 四元数乘法，或者叫叉乘。
*
* @param q
* @return
*/
public Quaternion multLocal(Quaternion q) {
float qw = q.w, qx = q.x, qy = q.y, qz = q.z;
float rw = w * qw - x * qx - y * qy - z * qz;
float rx = w * qx + x * qw + y * qz - z * qy;
float ry = w * qy + y * qw + z * qx - x * qz;
float rz = w * qz + z * qw + x * qy - y * qx;
x = rx;
y = ry;
z = rz;
w = rw;
return this;
}
四元数的乘法满足结合律，但不满足交换律。
a(bc) = (ab)c
ab =/= ba
两个四元数乘积的模，等于模的乘积。
|q1×q2| = |q1||q2|
这个结论非常重要，因为它保证了两个单位四元数相乘的结果还是单位四元数。
四元数乘积的逆，等于各个四元数的逆以相反的顺序相乘。
(a × b)^-1 = b^-1 x a^-1
旋转三维向量
若将一个三维空间中的点v(x, y, z) “扩展” 到四元数空间，定义为四元数 p = [0, v]。设 q 为我们讨论的旋转四元数形式 q = [cos(θ/2), sin(θ/2)*n]。执行下面的乘法，可以使三维点 p 绕 n 旋转：
p' = q x p x q^-1
由于 q 是单位四元数，可以用 q 的共轭来代替 q 的逆，写作：
p' = q x p x q*
若 p = (0, x, y, z)，q = (w', x', y', z')，q* = (w', -x', -y', -z')就有：
p' = (w', x', y', z') × (0, x, y, z) × (w', -x', -y', -z')
三个四元数连续叉乘，真让人头大。。
我试着在纸上推导结果，算了一整页，才推导出来p'.w = 0。
直接上代码吧。
/**
* 使用这个四元数来旋转一个三维向量。
*
* @param v
* @return
*/
public Vector3f mult(Vector3f v) {
if (v.x == 0 && v.y == 0 && v.z == 0) {
return new Vector3f(0, 0, 0);
} else {
float vx = v.x, vy = v.y, vz = v.z;
float rx = w * w * vx + 2 * y * w * vz - 2 * z * w * vy + x * x * vx + 2 * y * x * vy + 2 * z * x * vz - z * z * vx - y * y * vx;
float ry = 2 * x * y * vx + y * y * vy + 2 * z * y * vz + 2 * w * z * vx - z * z * vy + w * w * vy - 2 * x * w * vz - x * x * vy;
float rz = 2 * x * z * vx + 2 * y * z * vy + z * z * vz - 2 * w * y * vx - y * y * vz + 2 * w * x * vy - x * x * vz + w * w * vz;
return new Vector3f(rx, ry, rz);
}
}
如果要进行多次旋转，情况如下：
p' = b(apa*)b*
= (ba)p(a*b*) // 结合律
= (ba)p(ba)* // 四元数乘积的逆，等于各个四元数的逆以相反的顺序相乘。
注意，先进性a旋转再进行b旋转，等价于执行乘积ba代表的单一旋转。因此，四元数乘法能用来连接多次旋转，这个矩阵乘法的效果一样。
根据四元数的标准定义，这个旋转式以从右向左的顺序发生的。这非常不幸，因为它迫使我们以“由里向外”的顺序连接多次旋转，这和以矩阵形式作同样的运算是不同的（至少在使用行向量时时是不同的）。
乘法的变种
针对这个公式导致的“顺序颠倒”问题，在本书文本和代码中，我们将违背标准定义，以相反的顺序来定义四元数乘法。注意，仅仅叉乘部分收到了影响。
——《3D数学基础：图形与游戏开发》，第10.4.8节，P 149。
按照这本书上的做法，四元数乘法改成了：
[w1, x1, y1, z1] [w2, x2, y2, z2]
= [w1, v1] [w2, v2]
= [w1w2 - v1.v2 w1v2 + w2v1 + v1 x v2] // <--注意最右叉乘的顺序
w1w2 - x1x2 - y1y2 - z1z2
= w1x2 + x1w2 + y1z2 - z1y2
w1y2 + y1w2 + z1x2 - x1z2
w1z2 + z1w2 + x1y2 - y1x2
而标准定义是：
[w1, x1, y1, z1] [w2, x2, y2, z2]
= [w1, v1] [w2, v2]
= [w1w2 - v1.v2 w1v2 + w2v1 + v2 x v1]
w1w2 - x1x2 - y1y2 - z1z2
= w1x2 + x1w2 + z1y2 - y1z2
w1y2 + y1w2 + x1z2 - z1x2
w1z2 + z1w2 + y1x2 - x1y2
经过这么一改，四元数的几何性质没变，但是乘法方式改变了：
p' = q^-1 x p x q
= q* x p x q
多个旋转连续相乘变成了自左向右，与旋转发生的顺序一致：
p' = b*(a*pa)b
= (b*a*)p(ab)
= (ab)* p (ab)
在我的程序中，依然按照四元数的标准方法来进行计算。
说句题外话，我似乎有点明白为什么解析某些游戏的3D动画旋转数据时总是出错了。。也许在别人的程序中，是使用这种方式计算的四元数，而jMonkeyEngine使用的是标准方式。
四元数的“差”
利用四元数的乘法和逆，就能够计算两个四元数的“差”。“差”被定义为一个方位到另一个方位的角位移。换句话说，给定方位a和b，能够计算从a旋转到b的角位移d。用四元数等式更加紧凑地表示为：ad=b。
现在来求d。
ad = b
(a^-1)ad = (a^-1)b
[1 0] d = (a^-1)b
d = a* b
这样就有了求的代表一个方位到另一个方位角位移的四元数方法。在四元数插值时会用到这种方法。
/**
* 计算两个四元数之间的“差”。
*
* 已知四元数a和b，计算从a旋转到b的角位移d。
* a * d = b
* d = a的逆 * b
*
* @param q
* @return
*/
public Quaternion delta(Quaternion q) {
return this.inverse().mult(q);
}
四元数点乘
四元数的点乘与向量点乘非常相似。
/**
* 四元数点乘。这个值越大，说明两个四元数的旋转角度越接近。
* 当两个四元数都是单位四元数时，返回值是它们之间夹角的余弦值。
*
* @param q
* @return
*/
public float dot(Quaternion q) {
return x * q.x + y * q.y + z * q.z + w * q.w;
}
注意，和向量点乘一样，其结果是标量。对于单位四元数a和b，有 -1 <= a.b <= 1。通常我们只关心 a.b 的绝对值，因为 a.b = -(a.-b)，所以 b 和 -b 代表相同的角位移。
四元数点乘的几何解释类似于向量点乘的几何解释。四元数点乘 a.b 的绝对值越大，a 和 b代表的角位移越“相似”。
四元数插值
我跳过了四元数的对数、指数和标量乘法运算，直接到了这里。因为slerp运算实在是太重要了，因为它可以在两个四元数间平滑插值。slerp运算避免了欧拉角插值的所有问题。
slerp 运算是一个三元操作。前两个操作数是两个四元数，将在它们中间插值。设这两个“开始”和“结束”四元数分别为q0和q1。插值参数设为变量t，t在0到1之间变化。slerp函数：slerp(q0, q1, t)
代码如下：
/**
* 球面线性插值(Spherical Linear intERPolation)
*
* @param src
* @param dest
* @param t
*/
public Quaternion slerp(Quaternion src, Quaternion dest, float t) {
if (src.x == dest.x && src.y == dest.y && src.z == dest.z && src.w == dest.w) {
return new Quaternion(src);
}
// 用点乘计算两四元数夹角的 cos 值
float cos = src.dot(dest);
// 如果点乘为负，则反转一个四元数以取得短的4D“弧”
if (cos < 0.0f) {
cos = -cos;
dest = dest.negate();
}
// 计算两个四元数的插值系数
float srcFactor = 1 - t;
float destFactor = t;
// 检查它们是否过于接近以避免除零
if (cos > 0.999f) {
// 非常接近 -- 即线性插值
srcFactor = 1 - t;
destFactor = t;
} else {
// 计算两个四元数之间的夹角
float angle = (float) Math.acos(cos);
// 计算分母的倒数，这样就只需要一次除法
float invSin = 1f / (float) Math.sin(angle);
// 计算插值系数
srcFactor = (float) Math.sin((1 - t) * angle) * invSin;
destFactor = (float) Math.sin((t * angle)) * invSin;
}
// 插值
float rx = (srcFactor * src.x) + (destFactor * dest.x);
float ry = (srcFactor * src.y) + (destFactor * dest.y);
float rz = (srcFactor * src.z) + (destFactor * dest.z);
float rw = (srcFactor * src.w) + (destFactor * dest.w);
// 返回插值结果
return new Quaternion(rx, ry, rz, rw);
}
总结
目标达成。
虽然四元数很难理解，但毕竟我还是实现了四元数的常用运算。具体代码写得对不对，还得看以后的运行结果。