File indexing completed on 2024-04-14 03:48:02
0001 // SPDX-License-Identifier: LGPL-2.1-or-later 0002 // 0003 // SPDX-FileCopyrightText: 2006-2007 Torsten Rahn <tackat@kde.org> 0004 // SPDX-FileCopyrightText: 2007 Inge Wallin <ingwa@kde.org> 0005 // SPDX-FileCopyrightText: 2011 Bernhard Beschow <bbeschow@cs.tu-berlin.de> 0006 // 0007 0008 #include "Quaternion.h" 0009 0010 using namespace std; 0011 0012 #include <QString> 0013 #include <QDebug> 0014 0015 0016 using namespace Marble; 0017 0018 Quaternion::Quaternion() 0019 { 0020 // like in libeigen we keep the quaternion uninitialized 0021 // set( 1.0, 0.0, 0.0, 0.0 ); 0022 } 0023 0024 Quaternion::Quaternion(qreal w, qreal x, qreal y, qreal z) 0025 { 0026 v[Q_W] = w; 0027 v[Q_X] = x; 0028 v[Q_Y] = y; 0029 v[Q_Z] = z; 0030 } 0031 0032 Quaternion Quaternion::fromSpherical(qreal lon, qreal lat) 0033 { 0034 const qreal w = 0.0; 0035 const qreal x = cos(lat) * sin(lon); 0036 const qreal y = sin(lat); 0037 const qreal z = cos(lat) * cos(lon); 0038 0039 return Quaternion( w, x, y, z ); 0040 } 0041 0042 void Quaternion::getSpherical(qreal &lon, qreal &lat) const 0043 { 0044 qreal y = v[Q_Y]; 0045 if ( y > 1.0 ) 0046 y = 1.0; 0047 else if ( y < -1.0 ) 0048 y = -1.0; 0049 0050 lat = asin( y ); 0051 0052 if(v[Q_X] * v[Q_X] + v[Q_Z] * v[Q_Z] > 0.00005) 0053 lon = atan2(v[Q_X], v[Q_Z]); 0054 else 0055 lon = 0.0; 0056 } 0057 0058 void Quaternion::normalize() 0059 { 0060 (*this) *= 1.0 / length(); 0061 } 0062 0063 qreal Quaternion::length() const 0064 { 0065 return sqrt(v[Q_W] * v[Q_W] + v[Q_X] * v[Q_X] + v[Q_Y] * v[Q_Y] + v[Q_Z] * v[Q_Z]); 0066 } 0067 0068 Quaternion& Quaternion::operator*=(qreal mult) 0069 { 0070 (*this) = (*this) * mult; 0071 0072 return *this; 0073 } 0074 0075 Quaternion Quaternion::inverse() const 0076 { 0077 Quaternion inverse( v[Q_W], -v[Q_X], -v[Q_Y], -v[Q_Z] ); 0078 inverse.normalize(); 0079 0080 return inverse; 0081 } 0082 0083 Quaternion Quaternion::log() const 0084 { 0085 double const qlen = length(); 0086 double const vlen = sqrt(v[Q_X]*v[Q_X] + v[Q_Y]*v[Q_Y] + v[Q_Z]*v[Q_Z]); 0087 double const a = acos(v[Q_W]/qlen) / vlen; 0088 return Quaternion(std::log(qlen), v[Q_X] * a, v[Q_Y] * a, v[Q_Z] * a); 0089 } 0090 0091 Quaternion Quaternion::exp() const 0092 { 0093 double const vlen = sqrt(v[Q_X]*v[Q_X] + v[Q_Y]*v[Q_Y] + v[Q_Z]*v[Q_Z]); 0094 double const s = std::exp(v[Q_W]); 0095 double const a = s * sin(vlen) / vlen; 0096 return Quaternion(s * cos(vlen), v[Q_X] * a, v[Q_Y] * a, v[Q_Z] * a); 0097 } 0098 0099 Quaternion Quaternion::fromEuler(qreal pitch, qreal yaw, qreal roll) 0100 { 0101 const qreal cPhi = cos(0.5 * pitch); // also: "heading" 0102 const qreal cThe = cos(0.5 * yaw); // also: "attitude" 0103 const qreal cPsi = cos(0.5 * roll); // also: "bank" 0104 0105 const qreal sPhi = sin(0.5 * pitch); 0106 const qreal sThe = sin(0.5 * yaw); 0107 const qreal sPsi = sin(0.5 * roll); 0108 0109 const qreal w = cPhi * cThe * cPsi + sPhi * sThe * sPsi; 0110 const qreal x = sPhi * cThe * cPsi - cPhi * sThe * sPsi; 0111 const qreal y = cPhi * sThe * cPsi + sPhi * cThe * sPsi; 0112 const qreal z = cPhi * cThe * sPsi - sPhi * sThe * cPsi; 0113 0114 return Quaternion( w, x, y, z ); 0115 } 0116 0117 qreal Quaternion::pitch() const // "heading", phi 0118 { 0119 return atan2( 2.0*(v[Q_X]*v[Q_W]-v[Q_Y]*v[Q_Z]), 0120 ( 1.0 - 2.0*(v[Q_X]*v[Q_X]+v[Q_Z]*v[Q_Z]) ) ); 0121 } 0122 0123 qreal Quaternion::yaw() const // "attitude", theta 0124 { 0125 return atan2( 2.0*(v[Q_Y]*v[Q_W]-v[Q_X]*v[Q_Z]), 0126 ( 1.0 - 2.0*(v[Q_Y]*v[Q_Y]+v[Q_Z]*v[Q_Z]) ) ); 0127 } 0128 0129 qreal Quaternion::roll() const // "bank", psi 0130 { 0131 return asin(2.0*(v[Q_X]*v[Q_Y]+v[Q_Z]*v[Q_W])); 0132 } 0133 0134 #ifndef QT_NO_DEBUG_STREAM 0135 QDebug operator<<(QDebug debug, const Quaternion &q) 0136 { 0137 QString quatdisplay = QString("Quaternion: w= %1, x= %2, y= %3, z= %4, |q|= %5" ) 0138 .arg(q.v[Q_W]).arg(q.v[Q_X]).arg(q.v[Q_Y]).arg(q.v[Q_Z]).arg(q.length()); 0139 0140 debug << quatdisplay; 0141 0142 return debug; 0143 } 0144 #endif 0145 0146 Quaternion& Quaternion::operator*=(const Quaternion &q) 0147 { 0148 (*this) = (*this) * q; 0149 0150 return *this; 0151 } 0152 0153 bool Quaternion::operator==(const Quaternion &q) const 0154 { 0155 0156 return ( v[Q_W] == q.v[Q_W] 0157 && v[Q_X] == q.v[Q_X] 0158 && v[Q_Y] == q.v[Q_Y] 0159 && v[Q_Z] == q.v[Q_Z] ); 0160 } 0161 0162 Quaternion Quaternion::operator*(const Quaternion &q) const 0163 { 0164 const qreal w = v[Q_W] * q.v[Q_W] - v[Q_X] * q.v[Q_X] - v[Q_Y] * q.v[Q_Y] - v[Q_Z] * q.v[Q_Z]; 0165 const qreal x = v[Q_W] * q.v[Q_X] + v[Q_X] * q.v[Q_W] + v[Q_Y] * q.v[Q_Z] - v[Q_Z] * q.v[Q_Y]; 0166 const qreal y = v[Q_W] * q.v[Q_Y] - v[Q_X] * q.v[Q_Z] + v[Q_Y] * q.v[Q_W] + v[Q_Z] * q.v[Q_X]; 0167 const qreal z = v[Q_W] * q.v[Q_Z] + v[Q_X] * q.v[Q_Y] - v[Q_Y] * q.v[Q_X] + v[Q_Z] * q.v[Q_W]; 0168 0169 return Quaternion( w, x, y, z ); 0170 } 0171 0172 Quaternion Quaternion::operator+(const Quaternion &q) const 0173 { 0174 return Quaternion(v[Q_W] + q.v[Q_W], 0175 v[Q_X] + q.v[Q_X], 0176 v[Q_Y] + q.v[Q_Y], 0177 v[Q_Z] + q.v[Q_Z]); 0178 } 0179 0180 Quaternion Quaternion::operator*(qreal factor) const 0181 { 0182 return Quaternion( v[Q_W] * factor, v[Q_X] * factor, v[Q_Y] * factor, v[Q_Z] * factor ); 0183 } 0184 0185 void Quaternion::rotateAroundAxis(const Quaternion &q) 0186 { 0187 const qreal w = + v[Q_X] * q.v[Q_X] + v[Q_Y] * q.v[Q_Y] + v[Q_Z] * q.v[Q_Z]; 0188 const qreal x = + v[Q_X] * q.v[Q_W] - v[Q_Y] * q.v[Q_Z] + v[Q_Z] * q.v[Q_Y]; 0189 const qreal y = + v[Q_X] * q.v[Q_Z] + v[Q_Y] * q.v[Q_W] - v[Q_Z] * q.v[Q_X]; 0190 const qreal z = - v[Q_X] * q.v[Q_Y] + v[Q_Y] * q.v[Q_X] + v[Q_Z] * q.v[Q_W]; 0191 0192 (*this) = q * Quaternion( w, x, y, z ); 0193 } 0194 0195 Quaternion Quaternion::slerp(const Quaternion &q1, const Quaternion &q2, qreal t) 0196 { 0197 qreal p1; 0198 qreal p2; 0199 0200 // Let alpha be the angle between the two quaternions. 0201 qreal cosAlpha = ( q1.v[Q_X] * q2.v[Q_X] 0202 + q1.v[Q_Y] * q2.v[Q_Y] 0203 + q1.v[Q_Z] * q2.v[Q_Z] 0204 + q1.v[Q_W] * q2.v[Q_W] ); 0205 qreal alpha = acos( cosAlpha ); 0206 qreal sinAlpha = sin( alpha ); 0207 0208 if ( sinAlpha > 0.0 ) { 0209 p1 = sin( ( 1.0 - t ) * alpha ) / sinAlpha; 0210 p2 = sin( t * alpha ) / sinAlpha; 0211 } else { 0212 // both Quaternions are equal 0213 p1 = 1.0; 0214 p2 = 0.0; 0215 } 0216 0217 const qreal w = p1 * q1.v[Q_W] + p2 * q2.v[Q_W]; 0218 const qreal x = p1 * q1.v[Q_X] + p2 * q2.v[Q_X]; 0219 const qreal y = p1 * q1.v[Q_Y] + p2 * q2.v[Q_Y]; 0220 const qreal z = p1 * q1.v[Q_Z] + p2 * q2.v[Q_Z]; 0221 0222 return Quaternion( w, x, y, z ); 0223 } 0224 0225 Quaternion Quaternion::nlerp(const Quaternion &q1, const Quaternion &q2, qreal t) 0226 { 0227 const qreal p1 = 1.0 - t; 0228 0229 const qreal w = p1 * q1.v[Q_W] + t * q2.v[Q_W]; 0230 const qreal x = p1 * q1.v[Q_X] + t * q2.v[Q_X]; 0231 const qreal y = p1 * q1.v[Q_Y] + t * q2.v[Q_Y]; 0232 const qreal z = p1 * q1.v[Q_Z] + t * q2.v[Q_Z]; 0233 0234 Quaternion result( w, x, y, z ); 0235 result.normalize(); 0236 0237 return result; 0238 } 0239 0240 void Quaternion::toMatrix(matrix &m) const 0241 { 0242 0243 const qreal xy = v[Q_X] * v[Q_Y], xz = v[Q_X] * v[Q_Z]; 0244 const qreal yy = v[Q_Y] * v[Q_Y], yw = v[Q_Y] * v[Q_W]; 0245 const qreal zw = v[Q_Z] * v[Q_W], zz = v[Q_Z] * v[Q_Z]; 0246 0247 m[0][0] = 1.0 - 2.0 * (yy + zz); 0248 m[0][1] = 2.0 * (xy + zw); 0249 m[0][2] = 2.0 * (xz - yw); 0250 m[0][3] = 0.0; 0251 0252 const qreal xx = v[Q_X] * v[Q_X]; 0253 const qreal xw = v[Q_X] * v[Q_W]; 0254 const qreal yz = v[Q_Y] * v[Q_Z]; 0255 0256 m[1][0] = 2.0 * (xy - zw); 0257 m[1][1] = 1.0 - 2.0 * (xx + zz); 0258 m[1][2] = 2.0 * (yz + xw); 0259 m[1][3] = 0.0; 0260 0261 m[2][0] = 2.0 * (xz + yw); 0262 m[2][1] = 2.0 * (yz - xw); 0263 m[2][2] = 1.0 - 2.0 * (xx + yy); 0264 m[2][3] = 0.0; 0265 } 0266 0267 void Quaternion::rotateAroundAxis(const matrix &m) 0268 { 0269 const qreal x = m[0][0] * v[Q_X] + m[1][0] * v[Q_Y] + m[2][0] * v[Q_Z]; 0270 const qreal y = m[0][1] * v[Q_X] + m[1][1] * v[Q_Y] + m[2][1] * v[Q_Z]; 0271 const qreal z = m[0][2] * v[Q_X] + m[1][2] * v[Q_Y] + m[2][2] * v[Q_Z]; 0272 0273 v[Q_W] = 1.0; 0274 v[Q_X] = x; 0275 v[Q_Y] = y; 0276 v[Q_Z] = z; 0277 }