00001
00009 #include "rotationmatrix3.h"
00010 #include <argos3/core/utility/math/quaternion.h>
00011 #include <argos3/core/utility/math/vector3.h>
00012
00013 namespace argos {
00014
00015 CQuaternion CRotationMatrix3::ToQuaternion() const
00016 {
00017 const Real trace = 1.0f + m_pfValues[0] + m_pfValues[4] + m_pfValues[8];
00018
00019 if (trace > 0.00001f) {
00020 const Real s = Sqrt(trace) * 2.0f;
00021 return CQuaternion(
00022 s / 4.0f,
00023 (m_pfValues[7] - m_pfValues[5]) / s,
00024 (m_pfValues[2] - m_pfValues[6]) / s,
00025 (m_pfValues[3] - m_pfValues[1]) / s);
00026 }
00027 else if (m_pfValues[0] > m_pfValues[4] && m_pfValues[0] > m_pfValues[8]) {
00028 const Real s = Sqrt(1.0f + m_pfValues[0] - m_pfValues[4] - m_pfValues[8]) * 2.0f;
00029 return CQuaternion(
00030 (m_pfValues[7] - m_pfValues[5]) / s,
00031 s / 4.0f,
00032 (m_pfValues[3] + m_pfValues[1]) / s,
00033 (m_pfValues[2] + m_pfValues[6]) / s);
00034 }
00035 else if (m_pfValues[4] > m_pfValues[8]) {
00036 const Real s = Sqrt(1.0f + m_pfValues[4] - m_pfValues[0] - m_pfValues[8]) * 2.0f;
00037 return CQuaternion(
00038 (m_pfValues[2] - m_pfValues[6]) / s,
00039 (m_pfValues[3] + m_pfValues[1]) / s,
00040 s / 4.0f,
00041 (m_pfValues[7] + m_pfValues[5]) / s);
00042 }
00043 else {
00044 const Real s = Sqrt(1.0f + m_pfValues[8] - m_pfValues[0] - m_pfValues[4]) * 2.0f;
00045 return CQuaternion(
00046 (m_pfValues[3] - m_pfValues[1]) / s,
00047 (m_pfValues[2] + m_pfValues[6]) / s,
00048 (m_pfValues[7] + m_pfValues[5]) / s,
00049 s / 4.0f);
00050 }
00051 }
00052
00053
00054
00055
00056 void CRotationMatrix3::SetFromMatrix(const CMatrix<3,3>& c_matrix) {
00057 m_pfValues[0] = c_matrix.m_pfValues[0];
00058 m_pfValues[1] = c_matrix.m_pfValues[1];
00059 m_pfValues[2] = c_matrix.m_pfValues[2];
00060 m_pfValues[3] = c_matrix.m_pfValues[3];
00061 m_pfValues[4] = c_matrix.m_pfValues[4];
00062 m_pfValues[5] = c_matrix.m_pfValues[5];
00063 m_pfValues[6] = c_matrix.m_pfValues[6];
00064 m_pfValues[7] = c_matrix.m_pfValues[7];
00065 m_pfValues[8] = c_matrix.m_pfValues[8];
00066 }
00067
00068
00069
00070
00071 void CRotationMatrix3::SetFromQuaternion(const CQuaternion& c_quaternion) {
00072
00073 Real d = c_quaternion.SquareLength();
00074 Real s = 2.0f / d;
00075 Real xs = c_quaternion.GetX() * s, ys = c_quaternion.GetY() * s, zs = c_quaternion.GetZ() * s;
00076 Real wx = c_quaternion.GetW() * xs, wy = c_quaternion.GetW() * ys, wz = c_quaternion.GetW() * zs;
00077 Real xx = c_quaternion.GetX() * xs, xy = c_quaternion.GetX() * ys, xz = c_quaternion.GetX() * zs;
00078 Real yy = c_quaternion.GetY() * ys, yz = c_quaternion.GetY() * zs, zz = c_quaternion.GetZ() * zs;
00079
00080
00081 m_pfValues[0] = 1.0f - (yy + zz);
00082 m_pfValues[1] = xy - wz;
00083 m_pfValues[2] = xz + wy;
00084 m_pfValues[3] = xy + wz;
00085 m_pfValues[4] = 1.0f - (xx + zz);
00086 m_pfValues[5] = yz - wx;
00087 m_pfValues[6] = xz - wy;
00088 m_pfValues[7] = yz + wx;
00089 m_pfValues[8] = 1.0f - (xx + yy);
00090 }
00091
00092
00093
00094
00095 void CRotationMatrix3::SetFromAngles(const CRadians& c_z_angle, const CRadians& c_y_angle, const CRadians& c_x_angle) {
00096 Real fSinX = Sin(c_x_angle);
00097 Real fCosX = Cos(c_x_angle);
00098 Real fSinY = Sin(c_y_angle);
00099 Real fCosY = Cos(c_y_angle);
00100 Real fSinZ = Sin(c_z_angle);
00101 Real fCosZ = Cos(c_z_angle);
00102
00103 m_pfValues[0] = (fCosZ * fCosY);
00104 m_pfValues[1] = (fCosZ * fSinY * fSinX) - (fCosX * fSinZ);
00105 m_pfValues[2] = (fSinZ * fSinX) + (fCosZ * fCosX * fSinY);
00106 m_pfValues[3] = (fCosY * fSinZ);
00107 m_pfValues[4] = (fCosZ * fCosX) + (fSinZ * fSinY * fSinX);
00108 m_pfValues[5] = (fCosX * fSinZ * fSinY) - (fCosZ * fSinX);
00109 m_pfValues[6] = -fSinY;
00110 m_pfValues[7] = (fCosY * fSinX);
00111 m_pfValues[8] = (fCosY * fCosX);
00112 }
00113
00114
00115
00116
00117 void CRotationMatrix3::SetFromValues(Real f_value0, Real f_value1, Real f_value2,
00118 Real f_value3, Real f_value4, Real f_value5,
00119 Real f_value6, Real f_value7, Real f_value8) {
00120 m_pfValues[0] = f_value0;
00121 m_pfValues[1] = f_value1;
00122 m_pfValues[2] = f_value2;
00123 m_pfValues[3] = f_value3;
00124 m_pfValues[4] = f_value4;
00125 m_pfValues[5] = f_value5;
00126 m_pfValues[6] = f_value6;
00127 m_pfValues[7] = f_value7;
00128 m_pfValues[8] = f_value8;
00129 }
00130
00131
00132
00133
00134 CVector3 CRotationMatrix3::operator*(const CVector3& c_vector) const {
00135 return CVector3(m_pfValues[0]*c_vector.m_fX + m_pfValues[1]*c_vector.m_fY + m_pfValues[2]*c_vector.m_fZ,
00136 m_pfValues[3]*c_vector.m_fX + m_pfValues[4]*c_vector.m_fY + m_pfValues[5]*c_vector.m_fZ,
00137 m_pfValues[6]*c_vector.m_fX + m_pfValues[7]*c_vector.m_fY + m_pfValues[8]*c_vector.m_fZ);
00138 }
00139
00140
00141
00142 }