Introduce QQuaternion::inverted()

Change-Id: I6de77082bb7c32e48fb7f7d765a58fdbe68db1fd
Reviewed-by: Sean Harmer <sean.harmer@kdab.com>

2 files changed, 24 insertions, 0 deletions

diff --git a/src/gui/math3d/qquaternion.cpp b/src/gui/math3d/qquaternion.cpp
index f1af8922ca..23409189ac 100644
--- a/src/gui/math3d/qquaternion.cpp
+++ b/src/gui/math3d/qquaternion.cpp
@@ -274,6 +274,16 @@ void QQuaternion::normalize()
 }
 
 /*!
+    \fn QQuaternion QQuaternion::inverted() const
+    \since 5.5
+
+    Returns the inverse of this quaternion.
+    If this quaternion is null, then a null quaternion is returned.
+
+    \sa isNull(), length()
+*/
+
+/*!
     \fn QQuaternion QQuaternion::conjugate() const
 
     Returns the conjugate of this quaternion, which is
diff --git a/src/gui/math3d/qquaternion.h b/src/gui/math3d/qquaternion.h
index e0e52cefdd..2bef926b76 100644
--- a/src/gui/math3d/qquaternion.h
+++ b/src/gui/math3d/qquaternion.h
@@ -82,6 +82,8 @@ public:
     QQuaternion normalized() const;
     void normalize();
 
+    inline QQuaternion inverted() const;
+
     QQuaternion conjugate() const;
 
     QVector3D rotatedVector(const QVector3D& vector) const;
@@ -152,6 +154,18 @@ inline void QQuaternion::setY(float aY) { yp = aY; }
 inline void QQuaternion::setZ(float aZ) { zp = aZ; }
 inline void QQuaternion::setScalar(float aScalar) { wp = aScalar; }
 
+inline QQuaternion QQuaternion::inverted() const
+{
+    // Need some extra precision if the length is very small.
+    double len = double(xp) * double(xp) +
+                 double(yp) * double(yp) +
+                 double(zp) * double(zp) +
+                 double(wp) * double(wp);
+    if (!qFuzzyIsNull(len))
+        return QQuaternion(wp / len, -xp / len, -yp / len, -zp / len);
+    return QQuaternion(0.0f, 0.0f, 0.0f, 0.0f);
+}
+
 inline QQuaternion QQuaternion::conjugate() const
 {
     return QQuaternion(wp, -xp, -yp, -zp);