Marble

Quaternion.cpp
1// SPDX-License-Identifier: LGPL-2.1-or-later
2//
3// SPDX-FileCopyrightText: 2006-2007 Torsten Rahn <tackat@kde.org>
4// SPDX-FileCopyrightText: 2007 Inge Wallin <ingwa@kde.org>
5// SPDX-FileCopyrightText: 2011 Bernhard Beschow <bbeschow@cs.tu-berlin.de>
6//
7
8#include "Quaternion.h"
9
10using namespace std;
11
12#include <QString>
13#include <QDebug>
14
15
16using namespace Marble;
17
18Quaternion::Quaternion()
19{
20// like in libeigen we keep the quaternion uninitialized
21// set( 1.0, 0.0, 0.0, 0.0 );
22}
23
24Quaternion::Quaternion(qreal w, qreal x, qreal y, qreal z)
25{
26 v[Q_W] = w;
27 v[Q_X] = x;
28 v[Q_Y] = y;
29 v[Q_Z] = z;
30}
31
32Quaternion Quaternion::fromSpherical(qreal lon, qreal lat)
33{
34 const qreal w = 0.0;
35 const qreal x = cos(lat) * sin(lon);
36 const qreal y = sin(lat);
37 const qreal z = cos(lat) * cos(lon);
38
39 return Quaternion( w, x, y, z );
40}
41
42void Quaternion::getSpherical(qreal &lon, qreal &lat) const
43{
44 qreal y = v[Q_Y];
45 if ( y > 1.0 )
46 y = 1.0;
47 else if ( y < -1.0 )
48 y = -1.0;
49
50 lat = asin( y );
51
52 if(v[Q_X] * v[Q_X] + v[Q_Z] * v[Q_Z] > 0.00005)
53 lon = atan2(v[Q_X], v[Q_Z]);
54 else
55 lon = 0.0;
56}
57
58void Quaternion::normalize()
59{
60 (*this) *= 1.0 / length();
61}
62
63qreal Quaternion::length() const
64{
65 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]);
66}
67
68Quaternion& Quaternion::operator*=(qreal mult)
69{
70 (*this) = (*this) * mult;
71
72 return *this;
73}
74
75Quaternion Quaternion::inverse() const
76{
77 Quaternion inverse( v[Q_W], -v[Q_X], -v[Q_Y], -v[Q_Z] );
78 inverse.normalize();
79
80 return inverse;
81}
82
83Quaternion Quaternion::log() const
84{
85 double const qlen = length();
86 double const vlen = sqrt(v[Q_X]*v[Q_X] + v[Q_Y]*v[Q_Y] + v[Q_Z]*v[Q_Z]);
87 double const a = acos(v[Q_W]/qlen) / vlen;
88 return Quaternion(std::log(qlen), v[Q_X] * a, v[Q_Y] * a, v[Q_Z] * a);
89}
90
91Quaternion Quaternion::exp() const
92{
93 double const vlen = sqrt(v[Q_X]*v[Q_X] + v[Q_Y]*v[Q_Y] + v[Q_Z]*v[Q_Z]);
94 double const s = std::exp(v[Q_W]);
95 double const a = s * sin(vlen) / vlen;
96 return Quaternion(s * cos(vlen), v[Q_X] * a, v[Q_Y] * a, v[Q_Z] * a);
97}
98
99Quaternion Quaternion::fromEuler(qreal pitch, qreal yaw, qreal roll)
100{
101 const qreal cPhi = cos(0.5 * pitch); // also: "heading"
102 const qreal cThe = cos(0.5 * yaw); // also: "attitude"
103 const qreal cPsi = cos(0.5 * roll); // also: "bank"
104
105 const qreal sPhi = sin(0.5 * pitch);
106 const qreal sThe = sin(0.5 * yaw);
107 const qreal sPsi = sin(0.5 * roll);
108
109 const qreal w = cPhi * cThe * cPsi + sPhi * sThe * sPsi;
110 const qreal x = sPhi * cThe * cPsi - cPhi * sThe * sPsi;
111 const qreal y = cPhi * sThe * cPsi + sPhi * cThe * sPsi;
112 const qreal z = cPhi * cThe * sPsi - sPhi * sThe * cPsi;
113
114 return Quaternion( w, x, y, z );
115}
116
117qreal Quaternion::pitch() const // "heading", phi
118{
119 return atan2( 2.0*(v[Q_X]*v[Q_W]-v[Q_Y]*v[Q_Z]),
120 ( 1.0 - 2.0*(v[Q_X]*v[Q_X]+v[Q_Z]*v[Q_Z]) ) );
121}
122
123qreal Quaternion::yaw() const // "attitude", theta
124{
125 return atan2( 2.0*(v[Q_Y]*v[Q_W]-v[Q_X]*v[Q_Z]),
126 ( 1.0 - 2.0*(v[Q_Y]*v[Q_Y]+v[Q_Z]*v[Q_Z]) ) );
127}
128
129qreal Quaternion::roll() const // "bank", psi
130{
131 return asin(2.0*(v[Q_X]*v[Q_Y]+v[Q_Z]*v[Q_W]));
132}
133
134#ifndef QT_NO_DEBUG_STREAM
135QDebug operator<<(QDebug debug, const Quaternion &q)
136{
137 QString quatdisplay = QString("Quaternion: w= %1, x= %2, y= %3, z= %4, |q|= %5" )
138 .arg(q.v[Q_W]).arg(q.v[Q_X]).arg(q.v[Q_Y]).arg(q.v[Q_Z]).arg(q.length());
139
140 debug << quatdisplay;
141
142 return debug;
143}
144#endif
145
146Quaternion& Quaternion::operator*=(const Quaternion &q)
147{
148 (*this) = (*this) * q;
149
150 return *this;
151}
152
153bool Quaternion::operator==(const Quaternion &q) const
154{
155
156 return ( v[Q_W] == q.v[Q_W]
157 && v[Q_X] == q.v[Q_X]
158 && v[Q_Y] == q.v[Q_Y]
159 && v[Q_Z] == q.v[Q_Z] );
160}
161
162Quaternion Quaternion::operator*(const Quaternion &q) const
163{
164 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];
165 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];
166 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];
167 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];
168
169 return Quaternion( w, x, y, z );
170}
171
172Quaternion Quaternion::operator+(const Quaternion &q) const
173{
174 return Quaternion(v[Q_W] + q.v[Q_W],
175 v[Q_X] + q.v[Q_X],
176 v[Q_Y] + q.v[Q_Y],
177 v[Q_Z] + q.v[Q_Z]);
178}
179
180Quaternion Quaternion::operator*(qreal factor) const
181{
182 return Quaternion( v[Q_W] * factor, v[Q_X] * factor, v[Q_Y] * factor, v[Q_Z] * factor );
183}
184
185void Quaternion::rotateAroundAxis(const Quaternion &q)
186{
187 const qreal w = + v[Q_X] * q.v[Q_X] + v[Q_Y] * q.v[Q_Y] + v[Q_Z] * q.v[Q_Z];
188 const qreal x = + v[Q_X] * q.v[Q_W] - v[Q_Y] * q.v[Q_Z] + v[Q_Z] * q.v[Q_Y];
189 const qreal y = + v[Q_X] * q.v[Q_Z] + v[Q_Y] * q.v[Q_W] - v[Q_Z] * q.v[Q_X];
190 const qreal z = - v[Q_X] * q.v[Q_Y] + v[Q_Y] * q.v[Q_X] + v[Q_Z] * q.v[Q_W];
191
192 (*this) = q * Quaternion( w, x, y, z );
193}
194
195Quaternion Quaternion::slerp(const Quaternion &q1, const Quaternion &q2, qreal t)
196{
197 qreal p1;
198 qreal p2;
199
200 // Let alpha be the angle between the two quaternions.
201 qreal cosAlpha = ( q1.v[Q_X] * q2.v[Q_X]
202 + q1.v[Q_Y] * q2.v[Q_Y]
203 + q1.v[Q_Z] * q2.v[Q_Z]
204 + q1.v[Q_W] * q2.v[Q_W] );
205 qreal alpha = acos( cosAlpha );
206 qreal sinAlpha = sin( alpha );
207
208 if ( sinAlpha > 0.0 ) {
209 p1 = sin( ( 1.0 - t ) * alpha ) / sinAlpha;
210 p2 = sin( t * alpha ) / sinAlpha;
211 } else {
212 // both Quaternions are equal
213 p1 = 1.0;
214 p2 = 0.0;
215 }
216
217 const qreal w = p1 * q1.v[Q_W] + p2 * q2.v[Q_W];
218 const qreal x = p1 * q1.v[Q_X] + p2 * q2.v[Q_X];
219 const qreal y = p1 * q1.v[Q_Y] + p2 * q2.v[Q_Y];
220 const qreal z = p1 * q1.v[Q_Z] + p2 * q2.v[Q_Z];
221
222 return Quaternion( w, x, y, z );
223}
224
225Quaternion Quaternion::nlerp(const Quaternion &q1, const Quaternion &q2, qreal t)
226{
227 const qreal p1 = 1.0 - t;
228
229 const qreal w = p1 * q1.v[Q_W] + t * q2.v[Q_W];
230 const qreal x = p1 * q1.v[Q_X] + t * q2.v[Q_X];
231 const qreal y = p1 * q1.v[Q_Y] + t * q2.v[Q_Y];
232 const qreal z = p1 * q1.v[Q_Z] + t * q2.v[Q_Z];
233
234 Quaternion result( w, x, y, z );
235 result.normalize();
236
237 return result;
238}
239
240void Quaternion::toMatrix(matrix &m) const
241{
242
243 const qreal xy = v[Q_X] * v[Q_Y], xz = v[Q_X] * v[Q_Z];
244 const qreal yy = v[Q_Y] * v[Q_Y], yw = v[Q_Y] * v[Q_W];
245 const qreal zw = v[Q_Z] * v[Q_W], zz = v[Q_Z] * v[Q_Z];
246
247 m[0][0] = 1.0 - 2.0 * (yy + zz);
248 m[0][1] = 2.0 * (xy + zw);
249 m[0][2] = 2.0 * (xz - yw);
250 m[0][3] = 0.0;
251
252 const qreal xx = v[Q_X] * v[Q_X];
253 const qreal xw = v[Q_X] * v[Q_W];
254 const qreal yz = v[Q_Y] * v[Q_Z];
255
256 m[1][0] = 2.0 * (xy - zw);
257 m[1][1] = 1.0 - 2.0 * (xx + zz);
258 m[1][2] = 2.0 * (yz + xw);
259 m[1][3] = 0.0;
260
261 m[2][0] = 2.0 * (xz + yw);
262 m[2][1] = 2.0 * (yz - xw);
263 m[2][2] = 1.0 - 2.0 * (xx + yy);
264 m[2][3] = 0.0;
265}
266
267void Quaternion::rotateAroundAxis(const matrix &m)
268{
269 const qreal x = m[0][0] * v[Q_X] + m[1][0] * v[Q_Y] + m[2][0] * v[Q_Z];
270 const qreal y = m[0][1] * v[Q_X] + m[1][1] * v[Q_Y] + m[2][1] * v[Q_Z];
271 const qreal z = m[0][2] * v[Q_X] + m[1][2] * v[Q_Y] + m[2][2] * v[Q_Z];
272
273 v[Q_W] = 1.0;
274 v[Q_X] = x;
275 v[Q_Y] = y;
276 v[Q_Z] = z;
277}
Marble
Binds a QML item to a specific geodetic location in screen coordinates.
Definition AbstractDataPlugin.cpp:23
PerceptualColor::operator<<
QDebug operator<<(QDebug dbg, const PerceptualColor::LchaDouble &value)
QString::arg
QString arg(Args &&... args) const const
This file is part of the KDE documentation.
Documentation copyright © 1996-2024 The KDE developers.
Generated on Tue Mar 26 2024 11:18:17 by doxygen 1.10.0 written by Dimitri van Heesch, © 1997-2006

KDE's Doxygen guidelines are available online.