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 | */ |
235 | 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 | */ |
245 | 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 | */ |
260 | 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 | */ |
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 | */ |
334 | 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 | */ |
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 | */ |
424 | 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 | */ |
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 | */ |
507 | 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 | */ |
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 | */ |
607 | 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 | */ |
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 | */ |
694 | 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 | */ |
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 | */ |
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 | */ |
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 | */ |
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 | */ |
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 | */ |
984 | QQuaternion::operator QVariant() const |
985 | { |
986 | return QVariant(QVariant::Quaternion, this); |
987 | } |
988 | |
989 | #ifndef QT_NO_DEBUG_STREAM |
990 | |
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 | |
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 | |
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 | |