四元素进行多次运算操作后需要规范化

最近被一个问题困扰了几天，刚才终得解决，在此纪录一下：

问题表现：我的AR算法计算出的矩阵转换成四元数在unity里面使用时，会在某些角度出现大幅度不规律的飘、闪动

问题思路：刚开始以为ICP运算时点的变化太大，导致旋转矩阵变化太大，把点值变化和矩阵变化都输入查看后，发现变化都在很小的范围 0.000X，绝对不足以造成这么大的波动。于是把问题锁定在旋转矩阵转四元素的计算方法上出了问题，但查了很多资料也自己推算了一次算法后发现也没有问题（旋转矩阵转四元素的方法我之前的文章有写）。为了证明我的旋转矩阵没问题而是转四元数出了问题，我用iOS的SceneKit引擎实现了我的AR Demo，因为sceneKit中既可以使用矩阵也可以使用四元数做变换，通过对比发现，单独使用矩阵进行坐标变换后使用不会出现上面的问题，而直接使用四元数变换后就会出现问题，同时将矩阵进行坐标变换后在最后一步转成四元数来用的话会减少上述问题的概率和幅度，现在我确定时四元数转换或者是后面的变换中出了问题，但不知道问题到底出在哪里，没有解决根本问题。

解决办法：最后实在没办法，决定再从最基础的四元素的定义开始找找问题的源头，直到看到这样一句话才恍然大悟。

“

• 四元数、矩阵在多次运算后会积攒误差，需要分别对其做规范化(normalize)和正交化(orthogonalize)，对四元数规范化更容易

”

对！就是积攒误差！因为AR坐标系和unity的坐标系不一致，需要进行多次旋转、Invert、取反等操作，会积攒误差，计算后要对四元素进行规范化，我的问题也得到解决！

方法如下：

void Quaternion::normalise()
{
	
	float mag2 = w * w + x * x + y * y + z * z;
	if (  mag2!=0.f && (fabs(mag2 - 1.0f) > TOLERANCE)) {
		float mag = sqrt(mag2);
		w /= mag;
		x /= mag;
		y /= mag;
		z /= mag;
	}
}