RotationMatrix-未测试-实现文件

最新推荐文章于 2025-09-14 00:15:00 发布

原创最新推荐文章于 2025-09-14 00:15:00 发布 · 842 阅读

0 ·

CC 4.0 BY-SA版权

图形学专栏收录该内容

40 篇文章

订阅专栏

本文详细介绍了如何使用旋转矩阵实现不同的坐标转换，包括基于欧拉角设置旋转矩阵、从惯性坐标系到物体坐标系及反之的四元数转换，以及向量在不同坐标系间的转换。

#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) ;
}