Rotationmatrix-not tested-implementation file

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