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

KDE's Doxygen guidelines are available online.