四元数与矩阵欧拉角之间的相互转换

 四元数转矩阵

void fromInertialToObjectQuaternion(const Quaternion &q);
	void fromObjectToInertialQuaternion(const Quaternion &q);

void RotationMatrix::fromInertialToObjectQuaternion(const Quaternion &q)
{
	m11 = 1.0f - 2.0f*q.y*q.y - 2.0f*q.z*q.z;
	m12 = 2.0f * q.x*q.y + 2.0f * q.w*q.z;
	m13 = 2.0f * q.x*q.z - 2.0f * q.w*q.y;
	m21 = 2.0f * q.x*q.y - 2.0f * q.w*q.z;
	m22 = 1.0f - 2.0f * q.x*q.x - 2.0f * q.z*q.z;
	m23 = 2.0f * q.y*q.z + 2.0f * q.w*q.x;
	m31 = 2.0f * q.x*q.z + 2.0f * q.w*q.y;
	m32 = 2.0f * q.y*q.z - 2.0f * q.w*q.z;
	m33 = 1.0f - 2.0f * q.x*q.x - 2.0f * q.y*q.y;
}

//正交矩阵 逆=转置
void RotationMatrix::fromObjectToInertialQuaternion(const Quaternion &q)
{
	m11 = 1.0f - 2.0f*q.y*q.y - 2.0f*q.z*q.z;
	m21 = 2.0f * q.x*q.y + 2.0f * q.w*q.z;
	m31 = 2.0f * q.x*q.z - 2.0f * q.w*q.y;
	m12 = 2.0f * q.x*q.y - 2.0f * q.w*q.z;
	m22 = 1.0f - 2.0f * q.x*q.x - 2.0f * q.z*q.z;
	m32 = 2.0f * q.y*q.z + 2.0f * q.w*q.x;
	m13 = 2.0f * q.x*q.z + 2.0f * q.w*q.y;
	m23 = 2.0f * q.y*q.z - 2.0f * q.w*q.z;
	m33 = 1.0f - 2.0f * q.x*q.x - 2.0f * q.y*q.y;
}

 矩阵转四元数 

void fromRotationMatrix(const RotationMatrix &m);

void Quaternion::fromRotationMatrix(const RotationMatrix &m)
{
	float m11 = m.m11;
	float m12 = m.m12;
	float m13 = m.m13;

	float m21 = m.m21;
	float m22 = m.m22;
	float m23 = m.m23;

	float m31 = m.m31;
	float m32 = m.m32;
	float m33 = m.m33;

	//计算 四元数中 w x y z 谁最大 经验算法 哪一个大 然后用这个值用另外一组公式去计算其他三个数值
	float fourWsqrtMinus1 = m11 + m22 + m33;
	float fourXsqrtMinus1 = m11 - m22 - m33;
	float fourYsqrtMinus1 = m22 - m11 - m33;
	float fourZsqrtMinus1 = m33 - m11 - m22;

	int biggest = fourWsqrtMinus1;
	int index = 0;
	if (fourXsqrtMinus1 > biggest)
	{
		biggest = fourXsqrtMinus1;
		index = 1;
	}
	if (fourYsqrtMinus1 > biggest)
	{
		biggest = fourYsqrtMinus1;
		index = 2;
	}
	if (fourZsqrtMinus1 > biggest)
	{
		biggest = fourZsqrtMinus1;
		index = 3;
	}
	
	float biggestVal = sqrt(biggest + 1.0f)*0.5f;
	float mult = 0.25  / biggestVal;
	switch (index)
	{
	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;
	default:
		break;
	}
}

 

四元数转欧拉角

void fromObjectToIneritialQuaternion(const Quaternion&q);
	void fromIneritialToObjectQuaternion(const Quaternion&q);

void EulerAngles::fromObjectToIneritialQuaternion(const Quaternion&q)
{
	float sp = -2.0f*(q.y*q.z - q.w*q.x);
	//检查是否会发生万向锁
	if (fabs(sp) > 0.99999f)
	{
		pitch = k1Over2Pi * sp;
		heading = atan2(-q.x*q.z + q.w*q.y,0.5f-q.y*q.y-q.z*q.z);
		bank = 0.0f;
	}
	else
	{
		pitch = asin(sp);
		heading = atan2(q.x*q.z + q.w*q.y, 0.5f - q.x*q.x - q.y*q.y);
		bank = atan2(q.x*q.y + q.w*q.z, 0.5 - q.x*q.x - q.z*q.z);
	}
}

void EulerAngles::fromIneritialToObjectQuaternion(const Quaternion&q)
{
	//相反变换 四元数共轭 取xyz 相应的负数
	float sp = -2.0f*(q.y*q.z + q.w*q.x);
	//检查是否会发生万向锁
	if (fabs(sp) > 0.99999f)
	{
		pitch = k1Over2Pi * sp;
		heading = atan2(-q.x*q.z - q.w*q.y, 0.5f - q.y*q.y - q.z*q.z);
		bank = 0.0f;
	}
	else
	{
		pitch = asin(sp);
		heading = atan2(q.x*q.z - q.w*q.y, 0.5f - q.x*q.x - q.y*q.y);
		bank = atan2(q.x*q.y -q.w*q.z, 0.5 - q.x*q.x - q.z*q.z);
	}
}

 欧拉角转四元数

void setToRatoteObjectToInertial(const EulerAngles &orientation);  //欧拉角转四元数 物体坐标系到惯性坐标系

	void setToRatoteInertialToObject(const EulerAngles &orientation); //欧拉角转四元数 惯性坐标系到物体坐标系

void Quaternion::setToRatoteObjectToInertial(const EulerAngles &orientation)
{
	float sp, sb, sh;
	float cp, cb, ch;

	SinCos(&sp, &cp, orientation.pitch*0.5f);
	SinCos(&sb, &cb, orientation.bank*0.5f);
	SinCos(&sh, &ch, orientation.heading*0.5f);

	w = ch * cp*cb + sh * sp*sb;
	x = ch * sp*cb*+sh * cp*sb;
	y = sh * cp*cb - ch * sp*sb;
	z = ch * cp*sb - sh * sp*cb;
}

void Quaternion::setToRatoteInertialToObject(const EulerAngles &orientation)
{
	//上面的共轭
	float sp, sb, sh;
	float cp, cb, ch;

	SinCos(&sp, &cp, orientation.pitch*0.5f);
	SinCos(&sb, &cb, orientation.bank*0.5f);
	SinCos(&sh, &ch, orientation.heading*0.5f);

	w = ch * cp*cb + sh * sp*sb;
	x = -ch * sp*cb*-sh * cp*sb;
	y = -sh * cp*cb + ch * sp*sb;
	z = -ch * cp*sb + sh * sp*cb;
}