学步园

#include "Vector3.h"
#include "RotationMatrix.h"
#include "EularAngles.h"
#include "Quaternion.h"

void RotationMatrix ::Identity()
{
	m11 = m22 = m33 = 1 ;
	m12 = m13 = m21 = m23 = m31 = m32 = 0 ;
}

void RotationMatrix ::SetUp(const EulerAngles& orientation)
{
	float sinHeading = sin(orientation.heading) ;
	float cosHeading = cos(orientation.heading) ;
	float sinPitch = sin(orientation.pitch) ;
	float cosPitch = cos(orientation.pitch) ;
	float sinBank = sin(orientation.bank) ;
	float cosBank = cos(orientation.bank) ;

	m11 = cosHeading * cosBank + sinHeading * sinPitch * sinBank ;
	m12 = -cosHeading * sinBank + sinHeading * sinPitch * cosBank ;
	m13 = sinHeading * sinPitch ;

	m21 = sinBank * cosPitch ;
	m22 = cosBank * cosPitch ;
	m23 = -sinPitch ;

	m31 = -sinHeading * cosBank + cosHeading * sinPitch * sinBank ;
	m32 = sinBank * sinHeading + cosHeading * sinPitch * cosBank ;
	m33 = cosHeading * cosPitch ;
}

void RotationMatrix ::FromInertialToObjectQuaternion(const Quaternion& q)
{
	m11 = 1 - 2 * (q.y * q.y + q.z * q.z) ;
	m12 = 2 * (q.x * q.y + q.w * q.z) ;
	m13 = 2 * (q.x * q.z - q.w * q.y) ;

	m21 = 2 * (q.x * q.y - q.w * q.z) ;
	m22 = 1 - 2 * (q.x * q.x + q.z * q.z) ;
	m23 = 2 * (q.y + q.z + q.w * q.x) ;

	m31 = 2 * (q.x * q.z + q.w * q.y) ;
	m32 = 2 * (q.y * q.z - q.w * q.x) ;
	m33 = 1 - 2 * (q.x * q.x + q.y * q.y) ;
}

void RotationMatrix ::FromObjectToInertialQuaternion(const Quaternion& q)
{
	m11 = 1 - 2 * (q.y * q.y + q.z * q.z) ;
	m12 = 2 * (q.x * q.y - q.w * q.z) ;
	m13 = 2 * (q.x * q.z + q.w * q.y) ;

	m21 = 2 * (q.x * q.y + q.w * q.z) ;
	m22 = 1 - 2 * (q.x * q.x + q.z * q.z) ;
	m23 = 2 * (q.y * q.z - q.w * q.x) ;

	m31 = 2 * (q.x * q.z - q.w * q.y) ;
	m32 = 2 * (q.y * q.z + q.w * q.x) ;
	m33 = 1 - 2 * (q.x * q.x + q.y * q.y) ;
}

Vector3 RotationMatrix ::InertialToObject(const Vector3& v) const
{
	return Vector3(
		m11 * v.x + m12 * v.y + m13 * v.z,
		m21 * v.x + m22 * v.y + m23 * v.z,
		m31 * v.x + m32 * v.y + m33 * v.z) ;
}

Vector3 RotationMatrix ::ObjectToInertial(const Vector3& v) const
{
	return Vector3(
		m11 * v.x + m32 * v.y + m31 * v.z,
		m12 * v.x + m22 * v.y + m32 * v.z,
		m13 * v.x + m23 * v.y + m33 * v.z) ;
}