Marble

Quaternion.cpp
1 // SPDX-License-Identifier: LGPL-2.1-or-later
2 //
3 // SPDX-FileCopyrightText: 2006-2007 Torsten Rahn <[email protected]>
4 // SPDX-FileCopyrightText: 2007 Inge Wallin <[email protected]>
5 // SPDX-FileCopyrightText: 2011 Bernhard Beschow <[email protected]>
6 //
7 
8 #include "Quaternion.h"
9 
10 using namespace std;
11 
12 #include <QString>
13 #include <QDebug>
14 
15 
16 using namespace Marble;
17 
18 Quaternion::Quaternion()
19 {
20 // like in libeigen we keep the quaternion uninitialized
21 // set( 1.0, 0.0, 0.0, 0.0 );
22 }
23 
24 Quaternion::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 
32 Quaternion 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 
42 void 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 
58 void Quaternion::normalize()
59 {
60  (*this) *= 1.0 / length();
61 }
62 
63 qreal 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 
68 Quaternion& Quaternion::operator*=(qreal mult)
69 {
70  (*this) = (*this) * mult;
71 
72  return *this;
73 }
74 
75 Quaternion 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 
83 Quaternion 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 
91 Quaternion 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 
99 Quaternion 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 
117 qreal 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 
123 qreal 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 
129 qreal 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
135 QDebug 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 
146 Quaternion& Quaternion::operator*=(const Quaternion &q)
147 {
148  (*this) = (*this) * q;
149 
150  return *this;
151 }
152 
153 bool 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 
162 Quaternion 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 
172 Quaternion 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 
180 Quaternion 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 
185 void 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 
195 Quaternion 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 
225 Quaternion 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 
240 void 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 
267 void 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 }
KCALENDARCORE_EXPORT QDataStream & operator<<(QDataStream &out, const KCalendarCore::Alarm::Ptr &)
Binds a QML item to a specific geodetic location in screen coordinates.
QString arg(qlonglong a, int fieldWidth, int base, QChar fillChar) const const
This file is part of the KDE documentation.
Documentation copyright © 1996-2023 The KDE developers.
Generated on Wed Oct 4 2023 04:09:43 by doxygen 1.8.17 written by Dimitri van Heesch, © 1997-2006

KDE's Doxygen guidelines are available online.