四个月前刚研究了四元数，这周正好就在项目中用上了。看来深入钻研相关的知识点还是有莫大的好处的。
项目中的应用场景是，已知一个起始的用户坐标系UCS1，和一个终止的用户坐标系UCS2，如何求出一系列的中间坐标系，使得它们在UCS1和UCS2直接平滑的过渡？
（UCS1在这里是机器人手部的起始位置，UCS2是手部的目标位置，因为目前我只使用简单的反向动力学算法来求机器人的关节位置，所以需要计算初始和终止位置的过渡坐标系。将来用上运动规划的模块，这个就不再需要了。不过，这仍然是一个具有普适意义的好题目。例如UCS1可以看成是摄像机的初始坐标系，UCS2是摄像机的终止坐标系，我们可以求一个平滑的相机过渡路径。）
最简单的做法，坐标系的原点可以逐步的平移过去。那坐标系的旋转如何逐步过渡呢？答案是使用四元数的插值(interpolation)。
从前文得知，坐标系的旋转矩阵表达可以和四元数表达互相转换。所以我们有如下解决问题的伪代码：
1 UCS GetInterpolationMatrix(UCS& ucs1, UCS& ucs2, double t) 2 { 3 Matrix m1 = UCS1.GetMatrix(); 4 Matrix m2 = UCS2.GetMatrix(); 5 Vector p1 = m1.GetT(); 6 Vector p2 = m2.GetT(); 7 Quaternion q1 = m1.GetQ(); 8 Quaternion q2 = m2.GetQ(); 9 10 Matrix m; 11 Vector p = p1 * (1 - t) + p2 * t; 12 Quaternion q = q1 * (1 - t) + q2 * t; 13 m.SetT(p); 14 m.SetQ(q); 15 16 return UCS(m); 17 }
这个函数接受两个待插值UCS和一个数值t，将t在0和1之间变化，就可以得到一系列过渡的UCS。
值得注意的是，两个四元数的插值还有很多种方法，本文只是展示了最简单并且可用的一个方法。