旋轉-歐拉角

矩陣表示旋轉賦值操作比較麻煩，使用歐拉角代替矩陣表示頂點旋轉

1.歐拉角轉變矩陣

2.矩陣轉歐拉角

3.萬向死鎖避免以及角度限制

#pragma once
#include "RotationMatrix.h"
#include "Matrix4X3.h"


class EulerAngles
{
public :
	float heading;
	float pitch;
	float bank;

	EulerAngles(){}

	EulerAngles(float h,float p,float b):heading(h),pitch(p),bank(b){}

	void FromObjectToWorldMatrix(const Matrix4X3 &m);
	void FromWorldToObjectMatrix(const Matrix4X3 &m);
	void FromRotationMatrix(const RotationMatrix &r);

	void canonize(); // 三個角度限制 一定程度避免萬向死鎖問題
};

#include "pch.h"
#include "MathUtil.h"
#include "EulerAngles.h"

void EulerAngles::FromObjectToWorldMatrix(const Matrix4X3 &m)
{
	//pitch正負九十度發生萬向死鎖
	float sp = -m.m32;
	if (fabs(sp) > 0.999999f) //檢測萬像死鎖
	{
		pitch = PiOver2 * sp;
		bank = 0.0f;
		heading = atan2(-m.m23, m.m11);
	}
	else
	{
		heading = atan2(m.m31, m.m33);
		pitch = asin(sp);
		bank = atan2(m.m12, m.m22);
	}
}

void EulerAngles::FromWorldToObjectMatrix(const Matrix4X3 &m)
{
	//pitch正負九十度發生萬向死鎖
	float sp = -m.m23;
	if (fabs(sp) > 0.999999f) //檢測萬像死鎖
	{
		pitch = PiOver2 * sp;
		bank = 0.0f;
		heading = atan2(-m.m31, m.m11);
	}
	else
	{
		heading = atan2(m.m13, m.m33);
		pitch = asin(sp);
		bank = atan2(m.m21, m.m22);
	}
}

void EulerAngles::FromRotationMatrix(const RotationMatrix &m)
{
	//pitch正負九十度發生萬向死鎖
	float sp = -m.m23;
	if (fabs(sp) > 0.999999f) //檢測萬像死鎖
	{
		pitch = PiOver2 * sp;
		bank = 0.0f;
		heading = atan2(-m.m31, m.m11);
	}
	else
	{
		heading = atan2(m.m13, m.m33);
		pitch = asin(sp);
		bank = atan2(m.m21, m.m22);
	}
}

void EulerAngles::canonize()
{
	pitch = wrapPi(pitch);
	if (pitch < -k1Over2Pi)
	{
		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.0f;
	}
	else
	{
		bank = wrapPi(bank);
	}
	heading = wrapPi(heading);
}

#include "pch.h"
#include "MathUtil.h"
#include <math.h>

float wrapPi(float theta)
{
	theta = theta + PI;
	theta -= floor(theta*k1Over2Pi)*Pi2;
	theta -= PI;
	return theta;
}

// 3DMath.cpp : 此文件包含 "main" 函數。程序執行將在此處開始並結束。
//

#include "pch.h"
#include <iostream>
#include "Vector3.h"
#include "Matrix4X3.h"
#include "MathUtil.h"
#include <math.h>
#include "RotationMatrix.h"
#include "EulerAngles.h"
using namespace std;

float to_zero(float n)
{
	if (abs(n) < 0.0001)
	{
		return 0;
	}
	return n;
}

void print_v(Vector3 &v)
{
	cout << "[" << to_zero(v.x) << "," << to_zero(v.y) << "," << to_zero(v.z) << "]" << endl;
}

void print_m(Matrix4X3 &m)
{
	cout << to_zero(m.m11) << "\t" << to_zero(m.m12) << "\t" << to_zero(m.m13) << endl;
	cout << to_zero(m.m21) << "\t" << to_zero(m.m22) << "\t" << to_zero(m.m23) << endl;
	cout << to_zero(m.m31) << "\t" << to_zero(m.m32) << "\t" << to_zero(m.m33) << endl;
}

int main()
{
	cout << "Hello Rotation Matrix !\n";

	RotationMatrix rm;
	rm.m11 = 0.866f; rm.m12 = 0; rm.m13 =-0.5f;
	rm.m21 = 0.0f; rm.m22 = 1.0f; rm.m23 = 0.0f;
	rm.m31 = 0.5f; rm.m32 = 0.0f; rm.m33 = 0.866f;

	Vector3 v(10, 20, 30);

	Vector3 r;

	r = rm.intertialToObject(v);
	print_v(r);
	RotationMatrix erm;
	EulerAngles ea(30*PI/180,0,0);
	erm.Setup(ea);
	Vector3 er;
	er = erm.intertialToObject(v);
	print_v(er);
	system("pause");
	return 0;
}

項目地址