从旋转矩阵到四元数

在文章：从四元数计算旋转矩阵的基础上，现在来考虑从旋转矩阵到四元数的计算。

从四元数(w,x,y,z)计算旋转矩阵

---

从旋转矩阵计算四元数

为了从旋转矩阵中求出相应的四元数，可以直接利用上述已经求得的矩阵。

计算方法一：

计算对角线元素之和即可求的 w 值：

同理可以计算 x,y,z 的值：



！！！重点提醒！！！
上述的计算方式是存在不完整性的，因为四元数所有分量的计算都是通过开方所得，所有值都是非负数，这与实际是不相符的。没有准确的依据来确定是选择正根还是负根。

计算方式二：

计算相对于对角线堆成位置上的元素和与差：

我们可以发现，一旦对角线元素和/差的平方根解的了4个值中的一个，就嫩用以下方法计算其他的三个：




那么问题来了，应该选以上四种的哪一种呢？

似乎最简单的策略就是总是先计算同一个分量，比如 w，然后再计算 x,y,z 。这样的方式可能存在如下错误：如果 w=0，除法就没有意义(分母不能为0)；如有w非常小，将会出现数值不稳定。有学者建议首先判断 w,x,y,z 中哪一个最大，就用对角线元素计算钙元素，然后再通过计算方式二中公式计算其他三个分量。

NOTE： 你可以能有一个疑惑，为什么方法二中可以不用考虑开方取正根还是负根的问题。此处用到了负四元数的性质，那就是 q(w,x,y,z) 和 -q(-w,-x,-y,-z) 代表相同的方位。所以只要保证他们的符号是相对一致即可，同时负号并不影响结果。

基于计算方式二的代码实现：

以下是C++版本的核心代码

float m11,m12,m13;
float m21,m22,m23;
float m31,m32,m33;

float w,x,y,z;


//探测四元数中最大的项 
float fourWSquaredMinusl = m11+m22+m33;
float fourXSquaredMinusl = m11-m22-m33;
float fourYSquaredMinusl = m22-m11-m33;
float fourZSquaredMinusl = m33-m11-m22;

int biggestIndex = 0;
float fourBiggestSqureMinus1 = fourWSquaredMinusl;
if(fourXSquaredMinusl>fourBiggestSqureMinus1){
	fourBiggestSqureMinus1 = fourXSquaredMinusl;
	biggestIndex =1;
} 
if(fourYSquaredMinusl>fourBiggestSqureMinus1){
	fourBiggestSqureMinus1 = fourYSquaredMinusl;
	biggestIndex =2;
} 
if(fourZSquaredMinusl>fourBiggestSqureMinus1){
	fourBiggestSqureMinus1 = fourZSquaredMinusl;
	biggestIndex =3;
} 

//计算平方根和除法 
float biggestVal = sqrt(fourBiggestSqureMinus1+1.0f)*0.5f;
float mult = 0.25f/biggestVal;

//计算四元数的值
switch(biggestIndex){
	case 0:
		w=biggestVal;
		x=(m23-m32)*mult;
		y=(m31-m13)*mult;
		z=(m12-m21)*mult;
		break;
	case 1:
		x = biggestVal;
		w =(m23-m32)*mult;
		y =(m12+m21)*mult;
		z =(m31+m13)*mult;
		break;
	case 2:
		y =biggestVal;
		w =(m31-m13)*mult;
		x =(m12+m21)*mult;
		z =(m23+m32)*mult;
		break;
	case 3:
		z =biggestVal;
		w =(m12-m21)*mult;
		x =(m31+m13)*mult;
		y =(m23+m32)*mult;
		break;
} 

注意：以上所有的计算都是在左手坐标系中进行的，都是基于左手法则的。如果是存在坐标系等的变化结果可能与上述不同。