EulerAngles-未测试-实现文件

#include "stdafx.h"
#include <math.h>
#include <assert.h>
#include "Common.h"
#include "EularAngles.h"
#include "Quaternion.h"
#include "Matrix4x4.h"

EulerAngles ::EulerAngles()
{
}

EulerAngles ::EulerAngles(float heading, float pitch, float bank):
heading(heading), pitch(pitch), bank(bank)
{
}

void EulerAngles ::Indentity()
{
	heading = pitch = bank = 0 ;
}

void EulerAngles ::Canonize()
{
	//		理解这个方法大概用了一个小时,我都怀疑我看书的时候脑袋想什么了...
	pitch = WrapPI(pitch) ;
	if (pitch < -PIOver2)
	{
		pitch = -PI - pitch ;
		heading += PI ;
		bank += PI ;
	}
	else if (pitch > PIOver2)
	{
		pitch = PI - pitch ;
		heading += PI ;
		bank+= PI ;
	}

	if (fabs(pitch) > PIOver2 - 1e-4)
	{
		//		万向锁
		heading += bank ;
		bank = 0 ;
	}

	heading = WrapPI(heading) ;
	bank = WrapPI(bank) ;
}

void EulerAngles ::MatrixToEulerAngles(const Matrix4x4 & m)
{
	//		依据由欧拉角构造的旋转矩阵,推到回欧拉角形式
	float sinPitch = -m.m23 ;

	if (fabs(sinPitch) > One)
	{
		//		万向锁

		pitch = sinPitch > 0 ? PI / 2 : -PI / 2 ;
		bank = 0 ;
		//		cos(pitch) = 0 ;
		//		bank = 0 ;
		//		sin(bank) = 0 ;
		//		cos(bank) = 1 ;
		//		进而,将用欧拉角构造的旋转矩阵从
		//		cos(pitch)*cos(bank) + sin(heading)*sin(pitch)*sin(bank)			|	-cos(heading)*sin(bank) + sin(heading)*sin(pitch)*cos(bank)		|		sin(heading)*cos(pitch)
		//		sin(bank)*cos(pitch)																				|	cos(bank)*cos(pitch)																					|		-sin(pitch)
		//		-sin(heading)*cos(bank) + cos(heading)*sin(pitch)*sin(bank)	|	sin(bank)*sin(heading) + cos(heading)*sin(pitch)*cos(bank)			|		cos(heading)*cos(pitch)
		//		化简为
		//		cos(heading)		|	sin(heading)*sin(pitch)		|		0
		//		0								|	0												|		-sin(pitch)
		//		-sin(heading)		|	cos(heading)*sin(pitch)		|		0
		//		依据此矩阵可计算出bank累加到heading后heading的值
		heading = atan2(-m.m31, m.m11) ;
	}
	else
	{
		pitch = asin(sinPitch) ;
		bank = acos(m.m22 / cos(pitch)) ;
		heading = atan2(m.m13 / cos(pitch), m.m33 / cos(pitch)) ;
	}
}

void EulerAngles ::QuaternionToEulerAngles(const Quaternion& q)
{
	//		利用 四元数->矩阵 & 矩阵->欧拉角 建立从四元数到欧拉角的转换

	//		四元数 -> 矩阵
	//		1 - 2 * y * y - 2 * z * z		|		2 * x * y + 2 * w * z		|		2 * x * z - 2 * w * y
	//		2 * x * y - 2 * w * z			|		1 - 2 * x * x - 2 * z * z	|		2 * y * z + 2 * w * x
	//		2 * x * z + 2 * w * y			|		2 * y * z - 2 * w * x		|		1 - 2 * x * x - 2 * y * y

	//		矩阵 -> 欧拉角
	//		cos(pitch)*cos(bank) + sin(heading)*sin(pitch)*sin(bank)			|	-cos(heading)*sin(bank) + sin(heading)*sin(pitch)*cos(bank)		|		sin(heading)*cos(pitch)
	//		sin(bank)*cos(pitch)																				|	cos(bank)*cos(pitch)																					|		-sin(pitch)
	//		-sin(heading)*cos(bank) + cos(heading)*sin(pitch)*sin(bank)	|	sin(bank)*sin(heading) + cos(heading)*sin(pitch)*cos(bank)			|		cos(heading)*cos(pitch)

	//		建立好以上联系之后,就同从矩阵转换到欧拉角一样了

	//		m23 = 2*y*z + 2*w*x
	float sinPitch = -(2 * q.y * q.z + 2 * q.w + q.x) ;
	
	if (fabs(sinPitch) > One)
	{
		pitch = sinPitch > 0 ? PI / 2 : -PI / 2 ;
		bank = 0 ;
		//		atan2(-m.m31, m.m11)
		heading = atan2(-(2 * q.x * q.z + 2 * q.w * q.y), 1 - 2 * q.y * q.y - 2 * q.z * q.z) ;
	}
	else
	{
		pitch = asin(sinPitch) ;
		//		m22 = 1 - 2*x*x - 2*z*z
		bank = acos((1 - 2 * q.x * q.x - 2 * q.z * q.z) / cos(pitch)) ;
		//		m13 = 2*x*z - 2*w*y		m33 = 1 - 2*x*x - 2*y*y
		heading = atan2((2 * q.x * q.z - 2 * q.w * q.y) / cos(pitch), (1 - 2 * q.x * q.x - 2 * q.y * q.y) / cos(pitch)) ;
	}
}