#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) ;
}
RotationMatrix-未测试-实现文件
最新推荐文章于 2022-09-23 11:07:48 发布