1 /****************************************************************************
2 **
3 ** Copyright (C) 2016 The Qt Company Ltd.
4 ** Contact: https://www.qt.io/licensing/
5 **
6 ** This file is part of the QtGui module of the Qt Toolkit.
7 **
8 ** $QT_BEGIN_LICENSE:LGPL$
9 ** Commercial License Usage
10 ** Licensees holding valid commercial Qt licenses may use this file in
11 ** accordance with the commercial license agreement provided with the
12 ** Software or, alternatively, in accordance with the terms contained in
13 ** a written agreement between you and The Qt Company. For licensing terms
14 ** and conditions see https://www.qt.io/terms-conditions. For further
15 ** information use the contact form at https://www.qt.io/contact-us.
16 **
17 ** GNU Lesser General Public License Usage
18 ** Alternatively, this file may be used under the terms of the GNU Lesser
19 ** General Public License version 3 as published by the Free Software
20 ** Foundation and appearing in the file LICENSE.LGPL3 included in the
21 ** packaging of this file. Please review the following information to
22 ** ensure the GNU Lesser General Public License version 3 requirements
23 ** will be met: https://www.gnu.org/licenses/lgpl-3.0.html.
24 **
25 ** GNU General Public License Usage
26 ** Alternatively, this file may be used under the terms of the GNU
27 ** General Public License version 2.0 or (at your option) the GNU General
28 ** Public license version 3 or any later version approved by the KDE Free
29 ** Qt Foundation. The licenses are as published by the Free Software
30 ** Foundation and appearing in the file LICENSE.GPL2 and LICENSE.GPL3
31 ** included in the packaging of this file. Please review the following
32 ** information to ensure the GNU General Public License requirements will
33 ** be met: https://www.gnu.org/licenses/gpl-2.0.html and
34 ** https://www.gnu.org/licenses/gpl-3.0.html.
35 **
36 ** $QT_END_LICENSE$
37 **
38 ****************************************************************************/
39
40 #include "qquaternion.h"
41 #include <QtCore/qdatastream.h>
42 #include <QtCore/qmath.h>
43 #include <QtCore/qvariant.h>
44 #include <QtCore/qdebug.h>
45
46 #include <cmath>
47
48 QT_BEGIN_NAMESPACE
49
50 #ifndef QT_NO_QUATERNION
51
52 /*!
53 \class QQuaternion
54 \brief The QQuaternion class represents a quaternion consisting of a vector and scalar.
55 \since 4.6
56 \ingroup painting-3D
57 \inmodule QtGui
58
59 Quaternions are used to represent rotations in 3D space, and
60 consist of a 3D rotation axis specified by the x, y, and z
61 coordinates, and a scalar representing the rotation angle.
62 */
63
64 /*!
65 \fn QQuaternion::QQuaternion()
66
67 Constructs an identity quaternion (1, 0, 0, 0), i.e. with the vector (0, 0, 0)
68 and scalar 1.
69 */
70
71 /*!
72 \fn QQuaternion::QQuaternion(Qt::Initialization)
73 \since 5.5
74 \internal
75
76 Constructs a quaternion without initializing the contents.
77 */
78
79 /*!
80 \fn QQuaternion::QQuaternion(float scalar, float xpos, float ypos, float zpos)
81
82 Constructs a quaternion with the vector (\a xpos, \a ypos, \a zpos)
83 and \a scalar.
84 */
85
86 #ifndef QT_NO_VECTOR3D
87
88 /*!
89 \fn QQuaternion::QQuaternion(float scalar, const QVector3D& vector)
90
91 Constructs a quaternion vector from the specified \a vector and
92 \a scalar.
93
94 \sa vector(), scalar()
95 */
96
97 /*!
98 \fn QVector3D QQuaternion::vector() const
99
100 Returns the vector component of this quaternion.
101
102 \sa setVector(), scalar()
103 */
104
105 /*!
106 \fn void QQuaternion::setVector(const QVector3D& vector)
107
108 Sets the vector component of this quaternion to \a vector.
109
110 \sa vector(), setScalar()
111 */
112
113 #endif
114
115 /*!
116 \fn void QQuaternion::setVector(float x, float y, float z)
117
118 Sets the vector component of this quaternion to (\a x, \a y, \a z).
119
120 \sa vector(), setScalar()
121 */
122
123 #ifndef QT_NO_VECTOR4D
124
125 /*!
126 \fn QQuaternion::QQuaternion(const QVector4D& vector)
127
128 Constructs a quaternion from the components of \a vector.
129 */
130
131 /*!
132 \fn QVector4D QQuaternion::toVector4D() const
133
134 Returns this quaternion as a 4D vector.
135 */
136
137 #endif
138
139 /*!
140 \fn bool QQuaternion::isNull() const
141
142 Returns \c true if the x, y, z, and scalar components of this
143 quaternion are set to 0.0; otherwise returns \c false.
144 */
145
146 /*!
147 \fn bool QQuaternion::isIdentity() const
148
149 Returns \c true if the x, y, and z components of this
150 quaternion are set to 0.0, and the scalar component is set
151 to 1.0; otherwise returns \c false.
152 */
153
154 /*!
155 \fn float QQuaternion::x() const
156
157 Returns the x coordinate of this quaternion's vector.
158
159 \sa setX(), y(), z(), scalar()
160 */
161
162 /*!
163 \fn float QQuaternion::y() const
164
165 Returns the y coordinate of this quaternion's vector.
166
167 \sa setY(), x(), z(), scalar()
168 */
169
170 /*!
171 \fn float QQuaternion::z() const
172
173 Returns the z coordinate of this quaternion's vector.
174
175 \sa setZ(), x(), y(), scalar()
176 */
177
178 /*!
179 \fn float QQuaternion::scalar() const
180
181 Returns the scalar component of this quaternion.
182
183 \sa setScalar(), x(), y(), z()
184 */
185
186 /*!
187 \fn void QQuaternion::setX(float x)
188
189 Sets the x coordinate of this quaternion's vector to the given
190 \a x coordinate.
191
192 \sa x(), setY(), setZ(), setScalar()
193 */
194
195 /*!
196 \fn void QQuaternion::setY(float y)
197
198 Sets the y coordinate of this quaternion's vector to the given
199 \a y coordinate.
200
201 \sa y(), setX(), setZ(), setScalar()
202 */
203
204 /*!
205 \fn void QQuaternion::setZ(float z)
206
207 Sets the z coordinate of this quaternion's vector to the given
208 \a z coordinate.
209
210 \sa z(), setX(), setY(), setScalar()
211 */
212
213 /*!
214 \fn void QQuaternion::setScalar(float scalar)
215
216 Sets the scalar component of this quaternion to \a scalar.
217
218 \sa scalar(), setX(), setY(), setZ()
219 */
220
221 /*!
222 \fn float QQuaternion::dotProduct(const QQuaternion &q1, const QQuaternion &q2)
223 \since 5.5
224
225 Returns the dot product of \a q1 and \a q2.
226
227 \sa length()
228 */
229
230 /*!
231 Returns the length of the quaternion. This is also called the "norm".
232
233 \sa lengthSquared(), normalized(), dotProduct()
234 */
length() const235 float QQuaternion::length() const
236 {
237 return std::sqrt(xp * xp + yp * yp + zp * zp + wp * wp);
238 }
239
240 /*!
241 Returns the squared length of the quaternion.
242
243 \sa length(), dotProduct()
244 */
lengthSquared() const245 float QQuaternion::lengthSquared() const
246 {
247 return xp * xp + yp * yp + zp * zp + wp * wp;
248 }
249
250 /*!
251 Returns the normalized unit form of this quaternion.
252
253 If this quaternion is null, then a null quaternion is returned.
254 If the length of the quaternion is very close to 1, then the quaternion
255 will be returned as-is. Otherwise the normalized form of the
256 quaternion of length 1 will be returned.
257
258 \sa normalize(), length(), dotProduct()
259 */
normalized() const260 QQuaternion QQuaternion::normalized() const
261 {
262 // Need some extra precision if the length is very small.
263 double len = double(xp) * double(xp) +
264 double(yp) * double(yp) +
265 double(zp) * double(zp) +
266 double(wp) * double(wp);
267 if (qFuzzyIsNull(len - 1.0f))
268 return *this;
269 else if (!qFuzzyIsNull(len))
270 return *this / std::sqrt(len);
271 else
272 return QQuaternion(0.0f, 0.0f, 0.0f, 0.0f);
273 }
274
275 /*!
276 Normalizes the current quaternion in place. Nothing happens if this
277 is a null quaternion or the length of the quaternion is very close to 1.
278
279 \sa length(), normalized()
280 */
normalize()281 void QQuaternion::normalize()
282 {
283 // Need some extra precision if the length is very small.
284 double len = double(xp) * double(xp) +
285 double(yp) * double(yp) +
286 double(zp) * double(zp) +
287 double(wp) * double(wp);
288 if (qFuzzyIsNull(len - 1.0f) || qFuzzyIsNull(len))
289 return;
290
291 len = std::sqrt(len);
292
293 xp /= len;
294 yp /= len;
295 zp /= len;
296 wp /= len;
297 }
298
299 /*!
300 \fn QQuaternion QQuaternion::inverted() const
301 \since 5.5
302
303 Returns the inverse of this quaternion.
304 If this quaternion is null, then a null quaternion is returned.
305
306 \sa isNull(), length()
307 */
308
309 /*!
310 \fn QQuaternion QQuaternion::conjugated() const
311 \since 5.5
312
313 Returns the conjugate of this quaternion, which is
314 (-x, -y, -z, scalar).
315 */
316
317 /*!
318 \fn QQuaternion QQuaternion::conjugate() const
319 \obsolete
320
321 Use conjugated() instead.
322 */
323
324 /*!
325 Rotates \a vector with this quaternion to produce a new vector
326 in 3D space. The following code:
327
328 \snippet code/src_gui_math3d_qquaternion.cpp 0
329
330 is equivalent to the following:
331
332 \snippet code/src_gui_math3d_qquaternion.cpp 1
333 */
rotatedVector(const QVector3D & vector) const334 QVector3D QQuaternion::rotatedVector(const QVector3D& vector) const
335 {
336 return (*this * QQuaternion(0, vector) * conjugated()).vector();
337 }
338
339 /*!
340 \fn QQuaternion &QQuaternion::operator+=(const QQuaternion &quaternion)
341
342 Adds the given \a quaternion to this quaternion and returns a reference to
343 this quaternion.
344
345 \sa operator-=()
346 */
347
348 /*!
349 \fn QQuaternion &QQuaternion::operator-=(const QQuaternion &quaternion)
350
351 Subtracts the given \a quaternion from this quaternion and returns a
352 reference to this quaternion.
353
354 \sa operator+=()
355 */
356
357 /*!
358 \fn QQuaternion &QQuaternion::operator*=(float factor)
359
360 Multiplies this quaternion's components by the given \a factor, and
361 returns a reference to this quaternion.
362
363 \sa operator/=()
364 */
365
366 /*!
367 \fn QQuaternion &QQuaternion::operator*=(const QQuaternion &quaternion)
368
369 Multiplies this quaternion by \a quaternion and returns a reference
370 to this quaternion.
371 */
372
373 /*!
374 \fn QQuaternion &QQuaternion::operator/=(float divisor)
375
376 Divides this quaternion's components by the given \a divisor, and
377 returns a reference to this quaternion.
378
379 \sa operator*=()
380 */
381
382 #ifndef QT_NO_VECTOR3D
383
384 /*!
385 \fn void QQuaternion::getAxisAndAngle(QVector3D *axis, float *angle) const
386 \since 5.5
387 \overload
388
389 Extracts a 3D axis \a axis and a rotating angle \a angle (in degrees)
390 that corresponds to this quaternion.
391
392 \sa fromAxisAndAngle()
393 */
394
395 /*!
396 Creates a normalized quaternion that corresponds to rotating through
397 \a angle degrees about the specified 3D \a axis.
398
399 \sa getAxisAndAngle()
400 */
fromAxisAndAngle(const QVector3D & axis,float angle)401 QQuaternion QQuaternion::fromAxisAndAngle(const QVector3D& axis, float angle)
402 {
403 // Algorithm from:
404 // http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q56
405 // We normalize the result just in case the values are close
406 // to zero, as suggested in the above FAQ.
407 float a = qDegreesToRadians(angle / 2.0f);
408 float s = std::sin(a);
409 float c = std::cos(a);
410 QVector3D ax = axis.normalized();
411 return QQuaternion(c, ax.x() * s, ax.y() * s, ax.z() * s).normalized();
412 }
413
414 #endif
415
416 /*!
417 \since 5.5
418
419 Extracts a 3D axis (\a x, \a y, \a z) and a rotating angle \a angle (in degrees)
420 that corresponds to this quaternion.
421
422 \sa fromAxisAndAngle()
423 */
getAxisAndAngle(float * x,float * y,float * z,float * angle) const424 void QQuaternion::getAxisAndAngle(float *x, float *y, float *z, float *angle) const
425 {
426 Q_ASSERT(x && y && z && angle);
427
428 // The quaternion representing the rotation is
429 // q = cos(A/2)+sin(A/2)*(x*i+y*j+z*k)
430
431 float length = xp * xp + yp * yp + zp * zp;
432 if (!qFuzzyIsNull(length)) {
433 *x = xp;
434 *y = yp;
435 *z = zp;
436 if (!qFuzzyIsNull(length - 1.0f)) {
437 length = std::sqrt(length);
438 *x /= length;
439 *y /= length;
440 *z /= length;
441 }
442 *angle = 2.0f * std::acos(wp);
443 } else {
444 // angle is 0 (mod 2*pi), so any axis will fit
445 *x = *y = *z = *angle = 0.0f;
446 }
447
448 *angle = qRadiansToDegrees(*angle);
449 }
450
451 /*!
452 Creates a normalized quaternion that corresponds to rotating through
453 \a angle degrees about the 3D axis (\a x, \a y, \a z).
454
455 \sa getAxisAndAngle()
456 */
fromAxisAndAngle(float x,float y,float z,float angle)457 QQuaternion QQuaternion::fromAxisAndAngle
458 (float x, float y, float z, float angle)
459 {
460 float length = std::sqrt(x * x + y * y + z * z);
461 if (!qFuzzyIsNull(length - 1.0f) && !qFuzzyIsNull(length)) {
462 x /= length;
463 y /= length;
464 z /= length;
465 }
466 float a = qDegreesToRadians(angle / 2.0f);
467 float s = std::sin(a);
468 float c = std::cos(a);
469 return QQuaternion(c, x * s, y * s, z * s).normalized();
470 }
471
472 #ifndef QT_NO_VECTOR3D
473
474 /*!
475 \fn QVector3D QQuaternion::toEulerAngles() const
476 \since 5.5
477 \overload
478
479 Calculates roll, pitch, and yaw Euler angles (in degrees)
480 that corresponds to this quaternion.
481
482 \sa fromEulerAngles()
483 */
484
485 /*!
486 \fn QQuaternion QQuaternion::fromEulerAngles(const QVector3D &eulerAngles)
487 \since 5.5
488 \overload
489
490 Creates a quaternion that corresponds to a rotation of \a eulerAngles:
491 eulerAngles.z() degrees around the z axis, eulerAngles.x() degrees around the x axis,
492 and eulerAngles.y() degrees around the y axis (in that order).
493
494 \sa toEulerAngles()
495 */
496
497 #endif // QT_NO_VECTOR3D
498
499 /*!
500 \since 5.5
501
502 Calculates \a roll, \a pitch, and \a yaw Euler angles (in degrees)
503 that corresponds to this quaternion.
504
505 \sa fromEulerAngles()
506 */
getEulerAngles(float * pitch,float * yaw,float * roll) const507 void QQuaternion::getEulerAngles(float *pitch, float *yaw, float *roll) const
508 {
509 Q_ASSERT(pitch && yaw && roll);
510
511 // Algorithm from:
512 // http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q37
513
514 float xx = xp * xp;
515 float xy = xp * yp;
516 float xz = xp * zp;
517 float xw = xp * wp;
518 float yy = yp * yp;
519 float yz = yp * zp;
520 float yw = yp * wp;
521 float zz = zp * zp;
522 float zw = zp * wp;
523
524 const float lengthSquared = xx + yy + zz + wp * wp;
525 if (!qFuzzyIsNull(lengthSquared - 1.0f) && !qFuzzyIsNull(lengthSquared)) {
526 xx /= lengthSquared;
527 xy /= lengthSquared; // same as (xp / length) * (yp / length)
528 xz /= lengthSquared;
529 xw /= lengthSquared;
530 yy /= lengthSquared;
531 yz /= lengthSquared;
532 yw /= lengthSquared;
533 zz /= lengthSquared;
534 zw /= lengthSquared;
535 }
536
537 *pitch = std::asin(-2.0f * (yz - xw));
538 if (*pitch < M_PI_2) {
539 if (*pitch > -M_PI_2) {
540 *yaw = std::atan2(2.0f * (xz + yw), 1.0f - 2.0f * (xx + yy));
541 *roll = std::atan2(2.0f * (xy + zw), 1.0f - 2.0f * (xx + zz));
542 } else {
543 // not a unique solution
544 *roll = 0.0f;
545 *yaw = -std::atan2(-2.0f * (xy - zw), 1.0f - 2.0f * (yy + zz));
546 }
547 } else {
548 // not a unique solution
549 *roll = 0.0f;
550 *yaw = std::atan2(-2.0f * (xy - zw), 1.0f - 2.0f * (yy + zz));
551 }
552
553 *pitch = qRadiansToDegrees(*pitch);
554 *yaw = qRadiansToDegrees(*yaw);
555 *roll = qRadiansToDegrees(*roll);
556 }
557
558 /*!
559 \since 5.5
560
561 Creates a quaternion that corresponds to a rotation of
562 \a roll degrees around the z axis, \a pitch degrees around the x axis,
563 and \a yaw degrees around the y axis (in that order).
564
565 \sa getEulerAngles()
566 */
fromEulerAngles(float pitch,float yaw,float roll)567 QQuaternion QQuaternion::fromEulerAngles(float pitch, float yaw, float roll)
568 {
569 // Algorithm from:
570 // http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q60
571
572 pitch = qDegreesToRadians(pitch);
573 yaw = qDegreesToRadians(yaw);
574 roll = qDegreesToRadians(roll);
575
576 pitch *= 0.5f;
577 yaw *= 0.5f;
578 roll *= 0.5f;
579
580 const float c1 = std::cos(yaw);
581 const float s1 = std::sin(yaw);
582 const float c2 = std::cos(roll);
583 const float s2 = std::sin(roll);
584 const float c3 = std::cos(pitch);
585 const float s3 = std::sin(pitch);
586 const float c1c2 = c1 * c2;
587 const float s1s2 = s1 * s2;
588
589 const float w = c1c2 * c3 + s1s2 * s3;
590 const float x = c1c2 * s3 + s1s2 * c3;
591 const float y = s1 * c2 * c3 - c1 * s2 * s3;
592 const float z = c1 * s2 * c3 - s1 * c2 * s3;
593
594 return QQuaternion(w, x, y, z);
595 }
596
597 /*!
598 \since 5.5
599
600 Creates a rotation matrix that corresponds to this quaternion.
601
602 \note If this quaternion is not normalized,
603 the resulting rotation matrix will contain scaling information.
604
605 \sa fromRotationMatrix(), getAxes()
606 */
toRotationMatrix() const607 QMatrix3x3 QQuaternion::toRotationMatrix() const
608 {
609 // Algorithm from:
610 // http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q54
611
612 QMatrix3x3 rot3x3(Qt::Uninitialized);
613
614 const float f2x = xp + xp;
615 const float f2y = yp + yp;
616 const float f2z = zp + zp;
617 const float f2xw = f2x * wp;
618 const float f2yw = f2y * wp;
619 const float f2zw = f2z * wp;
620 const float f2xx = f2x * xp;
621 const float f2xy = f2x * yp;
622 const float f2xz = f2x * zp;
623 const float f2yy = f2y * yp;
624 const float f2yz = f2y * zp;
625 const float f2zz = f2z * zp;
626
627 rot3x3(0, 0) = 1.0f - (f2yy + f2zz);
628 rot3x3(0, 1) = f2xy - f2zw;
629 rot3x3(0, 2) = f2xz + f2yw;
630 rot3x3(1, 0) = f2xy + f2zw;
631 rot3x3(1, 1) = 1.0f - (f2xx + f2zz);
632 rot3x3(1, 2) = f2yz - f2xw;
633 rot3x3(2, 0) = f2xz - f2yw;
634 rot3x3(2, 1) = f2yz + f2xw;
635 rot3x3(2, 2) = 1.0f - (f2xx + f2yy);
636
637 return rot3x3;
638 }
639
640 /*!
641 \since 5.5
642
643 Creates a quaternion that corresponds to a rotation matrix \a rot3x3.
644
645 \note If a given rotation matrix is not normalized,
646 the resulting quaternion will contain scaling information.
647
648 \sa toRotationMatrix(), fromAxes()
649 */
fromRotationMatrix(const QMatrix3x3 & rot3x3)650 QQuaternion QQuaternion::fromRotationMatrix(const QMatrix3x3 &rot3x3)
651 {
652 // Algorithm from:
653 // http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q55
654
655 float scalar;
656 float axis[3];
657
658 const float trace = rot3x3(0, 0) + rot3x3(1, 1) + rot3x3(2, 2);
659 if (trace > 0.00000001f) {
660 const float s = 2.0f * std::sqrt(trace + 1.0f);
661 scalar = 0.25f * s;
662 axis[0] = (rot3x3(2, 1) - rot3x3(1, 2)) / s;
663 axis[1] = (rot3x3(0, 2) - rot3x3(2, 0)) / s;
664 axis[2] = (rot3x3(1, 0) - rot3x3(0, 1)) / s;
665 } else {
666 static int s_next[3] = { 1, 2, 0 };
667 int i = 0;
668 if (rot3x3(1, 1) > rot3x3(0, 0))
669 i = 1;
670 if (rot3x3(2, 2) > rot3x3(i, i))
671 i = 2;
672 int j = s_next[i];
673 int k = s_next[j];
674
675 const float s = 2.0f * std::sqrt(rot3x3(i, i) - rot3x3(j, j) - rot3x3(k, k) + 1.0f);
676 axis[i] = 0.25f * s;
677 scalar = (rot3x3(k, j) - rot3x3(j, k)) / s;
678 axis[j] = (rot3x3(j, i) + rot3x3(i, j)) / s;
679 axis[k] = (rot3x3(k, i) + rot3x3(i, k)) / s;
680 }
681
682 return QQuaternion(scalar, axis[0], axis[1], axis[2]);
683 }
684
685 #ifndef QT_NO_VECTOR3D
686
687 /*!
688 \since 5.5
689
690 Returns the 3 orthonormal axes (\a xAxis, \a yAxis, \a zAxis) defining the quaternion.
691
692 \sa fromAxes(), toRotationMatrix()
693 */
getAxes(QVector3D * xAxis,QVector3D * yAxis,QVector3D * zAxis) const694 void QQuaternion::getAxes(QVector3D *xAxis, QVector3D *yAxis, QVector3D *zAxis) const
695 {
696 Q_ASSERT(xAxis && yAxis && zAxis);
697
698 const QMatrix3x3 rot3x3(toRotationMatrix());
699
700 *xAxis = QVector3D(rot3x3(0, 0), rot3x3(1, 0), rot3x3(2, 0));
701 *yAxis = QVector3D(rot3x3(0, 1), rot3x3(1, 1), rot3x3(2, 1));
702 *zAxis = QVector3D(rot3x3(0, 2), rot3x3(1, 2), rot3x3(2, 2));
703 }
704
705 /*!
706 \since 5.5
707
708 Constructs the quaternion using 3 axes (\a xAxis, \a yAxis, \a zAxis).
709
710 \note The axes are assumed to be orthonormal.
711
712 \sa getAxes(), fromRotationMatrix()
713 */
fromAxes(const QVector3D & xAxis,const QVector3D & yAxis,const QVector3D & zAxis)714 QQuaternion QQuaternion::fromAxes(const QVector3D &xAxis, const QVector3D &yAxis, const QVector3D &zAxis)
715 {
716 QMatrix3x3 rot3x3(Qt::Uninitialized);
717 rot3x3(0, 0) = xAxis.x();
718 rot3x3(1, 0) = xAxis.y();
719 rot3x3(2, 0) = xAxis.z();
720 rot3x3(0, 1) = yAxis.x();
721 rot3x3(1, 1) = yAxis.y();
722 rot3x3(2, 1) = yAxis.z();
723 rot3x3(0, 2) = zAxis.x();
724 rot3x3(1, 2) = zAxis.y();
725 rot3x3(2, 2) = zAxis.z();
726
727 return QQuaternion::fromRotationMatrix(rot3x3);
728 }
729
730 /*!
731 \since 5.5
732
733 Constructs the quaternion using specified forward direction \a direction
734 and upward direction \a up.
735 If the upward direction was not specified or the forward and upward
736 vectors are collinear, a new orthonormal upward direction will be generated.
737
738 \sa fromAxes(), rotationTo()
739 */
fromDirection(const QVector3D & direction,const QVector3D & up)740 QQuaternion QQuaternion::fromDirection(const QVector3D &direction, const QVector3D &up)
741 {
742 if (qFuzzyIsNull(direction.x()) && qFuzzyIsNull(direction.y()) && qFuzzyIsNull(direction.z()))
743 return QQuaternion();
744
745 const QVector3D zAxis(direction.normalized());
746 QVector3D xAxis(QVector3D::crossProduct(up, zAxis));
747 if (qFuzzyIsNull(xAxis.lengthSquared())) {
748 // collinear or invalid up vector; derive shortest arc to new direction
749 return QQuaternion::rotationTo(QVector3D(0.0f, 0.0f, 1.0f), zAxis);
750 }
751
752 xAxis.normalize();
753 const QVector3D yAxis(QVector3D::crossProduct(zAxis, xAxis));
754
755 return QQuaternion::fromAxes(xAxis, yAxis, zAxis);
756 }
757
758 /*!
759 \since 5.5
760
761 Returns the shortest arc quaternion to rotate from the direction described by the vector \a from
762 to the direction described by the vector \a to.
763
764 \sa fromDirection()
765 */
rotationTo(const QVector3D & from,const QVector3D & to)766 QQuaternion QQuaternion::rotationTo(const QVector3D &from, const QVector3D &to)
767 {
768 // Based on Stan Melax's article in Game Programming Gems
769
770 const QVector3D v0(from.normalized());
771 const QVector3D v1(to.normalized());
772
773 float d = QVector3D::dotProduct(v0, v1) + 1.0f;
774
775 // if dest vector is close to the inverse of source vector, ANY axis of rotation is valid
776 if (qFuzzyIsNull(d)) {
777 QVector3D axis = QVector3D::crossProduct(QVector3D(1.0f, 0.0f, 0.0f), v0);
778 if (qFuzzyIsNull(axis.lengthSquared()))
779 axis = QVector3D::crossProduct(QVector3D(0.0f, 1.0f, 0.0f), v0);
780 axis.normalize();
781
782 // same as QQuaternion::fromAxisAndAngle(axis, 180.0f)
783 return QQuaternion(0.0f, axis.x(), axis.y(), axis.z());
784 }
785
786 d = std::sqrt(2.0f * d);
787 const QVector3D axis(QVector3D::crossProduct(v0, v1) / d);
788
789 return QQuaternion(d * 0.5f, axis).normalized();
790 }
791
792 #endif // QT_NO_VECTOR3D
793
794 /*!
795 \fn bool operator==(const QQuaternion &q1, const QQuaternion &q2)
796 \relates QQuaternion
797
798 Returns \c true if \a q1 is equal to \a q2; otherwise returns \c false.
799 This operator uses an exact floating-point comparison.
800 */
801
802 /*!
803 \fn bool operator!=(const QQuaternion &q1, const QQuaternion &q2)
804 \relates QQuaternion
805
806 Returns \c true if \a q1 is not equal to \a q2; otherwise returns \c false.
807 This operator uses an exact floating-point comparison.
808 */
809
810 /*!
811 \fn const QQuaternion operator+(const QQuaternion &q1, const QQuaternion &q2)
812 \relates QQuaternion
813
814 Returns a QQuaternion object that is the sum of the given quaternions,
815 \a q1 and \a q2; each component is added separately.
816
817 \sa QQuaternion::operator+=()
818 */
819
820 /*!
821 \fn const QQuaternion operator-(const QQuaternion &q1, const QQuaternion &q2)
822 \relates QQuaternion
823
824 Returns a QQuaternion object that is formed by subtracting
825 \a q2 from \a q1; each component is subtracted separately.
826
827 \sa QQuaternion::operator-=()
828 */
829
830 /*!
831 \fn const QQuaternion operator*(float factor, const QQuaternion &quaternion)
832 \relates QQuaternion
833
834 Returns a copy of the given \a quaternion, multiplied by the
835 given \a factor.
836
837 \sa QQuaternion::operator*=()
838 */
839
840 /*!
841 \fn const QQuaternion operator*(const QQuaternion &quaternion, float factor)
842 \relates QQuaternion
843
844 Returns a copy of the given \a quaternion, multiplied by the
845 given \a factor.
846
847 \sa QQuaternion::operator*=()
848 */
849
850 /*!
851 \fn const QQuaternion operator*(const QQuaternion &q1, const QQuaternion& q2)
852 \relates QQuaternion
853
854 Multiplies \a q1 and \a q2 using quaternion multiplication.
855 The result corresponds to applying both of the rotations specified
856 by \a q1 and \a q2.
857
858 \sa QQuaternion::operator*=()
859 */
860
861 /*!
862 \fn const QQuaternion operator-(const QQuaternion &quaternion)
863 \relates QQuaternion
864 \overload
865
866 Returns a QQuaternion object that is formed by changing the sign of
867 all three components of the given \a quaternion.
868
869 Equivalent to \c {QQuaternion(0,0,0,0) - quaternion}.
870 */
871
872 /*!
873 \fn const QQuaternion operator/(const QQuaternion &quaternion, float divisor)
874 \relates QQuaternion
875
876 Returns the QQuaternion object formed by dividing all components of
877 the given \a quaternion by the given \a divisor.
878
879 \sa QQuaternion::operator/=()
880 */
881
882 #ifndef QT_NO_VECTOR3D
883
884 /*!
885 \fn QVector3D operator*(const QQuaternion &quaternion, const QVector3D &vec)
886 \since 5.5
887 \relates QQuaternion
888
889 Rotates a vector \a vec with a quaternion \a quaternion to produce a new vector in 3D space.
890 */
891
892 #endif
893
894 /*!
895 \fn bool qFuzzyCompare(const QQuaternion& q1, const QQuaternion& q2)
896 \relates QQuaternion
897
898 Returns \c true if \a q1 and \a q2 are equal, allowing for a small
899 fuzziness factor for floating-point comparisons; false otherwise.
900 */
901
902 /*!
903 Interpolates along the shortest spherical path between the
904 rotational positions \a q1 and \a q2. The value \a t should
905 be between 0 and 1, indicating the spherical distance to travel
906 between \a q1 and \a q2.
907
908 If \a t is less than or equal to 0, then \a q1 will be returned.
909 If \a t is greater than or equal to 1, then \a q2 will be returned.
910
911 \sa nlerp()
912 */
slerp(const QQuaternion & q1,const QQuaternion & q2,float t)913 QQuaternion QQuaternion::slerp
914 (const QQuaternion& q1, const QQuaternion& q2, float t)
915 {
916 // Handle the easy cases first.
917 if (t <= 0.0f)
918 return q1;
919 else if (t >= 1.0f)
920 return q2;
921
922 // Determine the angle between the two quaternions.
923 QQuaternion q2b(q2);
924 float dot = QQuaternion::dotProduct(q1, q2);
925 if (dot < 0.0f) {
926 q2b = -q2b;
927 dot = -dot;
928 }
929
930 // Get the scale factors. If they are too small,
931 // then revert to simple linear interpolation.
932 float factor1 = 1.0f - t;
933 float factor2 = t;
934 if ((1.0f - dot) > 0.0000001) {
935 float angle = std::acos(dot);
936 float sinOfAngle = std::sin(angle);
937 if (sinOfAngle > 0.0000001) {
938 factor1 = std::sin((1.0f - t) * angle) / sinOfAngle;
939 factor2 = std::sin(t * angle) / sinOfAngle;
940 }
941 }
942
943 // Construct the result quaternion.
944 return q1 * factor1 + q2b * factor2;
945 }
946
947 /*!
948 Interpolates along the shortest linear path between the rotational
949 positions \a q1 and \a q2. The value \a t should be between 0 and 1,
950 indicating the distance to travel between \a q1 and \a q2.
951 The result will be normalized().
952
953 If \a t is less than or equal to 0, then \a q1 will be returned.
954 If \a t is greater than or equal to 1, then \a q2 will be returned.
955
956 The nlerp() function is typically faster than slerp() and will
957 give approximate results to spherical interpolation that are
958 good enough for some applications.
959
960 \sa slerp()
961 */
nlerp(const QQuaternion & q1,const QQuaternion & q2,float t)962 QQuaternion QQuaternion::nlerp
963 (const QQuaternion& q1, const QQuaternion& q2, float t)
964 {
965 // Handle the easy cases first.
966 if (t <= 0.0f)
967 return q1;
968 else if (t >= 1.0f)
969 return q2;
970
971 // Determine the angle between the two quaternions.
972 QQuaternion q2b(q2);
973 float dot = QQuaternion::dotProduct(q1, q2);
974 if (dot < 0.0f)
975 q2b = -q2b;
976
977 // Perform the linear interpolation.
978 return (q1 * (1.0f - t) + q2b * t).normalized();
979 }
980
981 /*!
982 Returns the quaternion as a QVariant.
983 */
operator QVariant() const984 QQuaternion::operator QVariant() const
985 {
986 return QVariant(QMetaType::QQuaternion, this);
987 }
988
989 #ifndef QT_NO_DEBUG_STREAM
990
operator <<(QDebug dbg,const QQuaternion & q)991 QDebug operator<<(QDebug dbg, const QQuaternion &q)
992 {
993 QDebugStateSaver saver(dbg);
994 dbg.nospace() << "QQuaternion(scalar:" << q.scalar()
995 << ", vector:(" << q.x() << ", "
996 << q.y() << ", " << q.z() << "))";
997 return dbg;
998 }
999
1000 #endif
1001
1002 #ifndef QT_NO_DATASTREAM
1003
1004 /*!
1005 \fn QDataStream &operator<<(QDataStream &stream, const QQuaternion &quaternion)
1006 \relates QQuaternion
1007
1008 Writes the given \a quaternion to the given \a stream and returns a
1009 reference to the stream.
1010
1011 \sa {Serializing Qt Data Types}
1012 */
1013
operator <<(QDataStream & stream,const QQuaternion & quaternion)1014 QDataStream &operator<<(QDataStream &stream, const QQuaternion &quaternion)
1015 {
1016 stream << quaternion.scalar() << quaternion.x()
1017 << quaternion.y() << quaternion.z();
1018 return stream;
1019 }
1020
1021 /*!
1022 \fn QDataStream &operator>>(QDataStream &stream, QQuaternion &quaternion)
1023 \relates QQuaternion
1024
1025 Reads a quaternion from the given \a stream into the given \a quaternion
1026 and returns a reference to the stream.
1027
1028 \sa {Serializing Qt Data Types}
1029 */
1030
operator >>(QDataStream & stream,QQuaternion & quaternion)1031 QDataStream &operator>>(QDataStream &stream, QQuaternion &quaternion)
1032 {
1033 float scalar, x, y, z;
1034 stream >> scalar;
1035 stream >> x;
1036 stream >> y;
1037 stream >> z;
1038 quaternion.setScalar(scalar);
1039 quaternion.setX(x);
1040 quaternion.setY(y);
1041 quaternion.setZ(z);
1042 return stream;
1043 }
1044
1045 #endif // QT_NO_DATASTREAM
1046
1047 #endif
1048
1049 QT_END_NAMESPACE
1050