gfx/thebes/gfxQuaternion.h

   1 /* -*- Mode: C++; tab-width: 2; indent-tabs-mode: nil; c-basic-offset: 2 -*-
   2  * This Source Code Form is subject to the terms of the Mozilla Public
   3  * License, v. 2.0. If a copy of the MPL was not distributed with this
   4  * file, You can obtain one at http://mozilla.org/MPL/2.0/. */
   5
   6 #ifndef GFX_QUATERNION_H
   7 #define GFX_QUATERNION_H
   8
   9 #include "mozilla/gfx/BasePoint4D.h"
  10 #include "mozilla/gfx/Matrix.h"
  11 #include "nsAlgorithm.h"
  12 #include <algorithm>
  13
  14 struct gfxQuaternion
  15     : public mozilla::gfx::BasePoint4D<gfxFloat, gfxQuaternion> {
  16   typedef mozilla::gfx::BasePoint4D<gfxFloat, gfxQuaternion> Super;
  17
  18   gfxQuaternion() : Super() {}
  19   gfxQuaternion(gfxFloat aX, gfxFloat aY, gfxFloat aZ, gfxFloat aW)
  20       : Super(aX, aY, aZ, aW) {}
  21
  22   explicit gfxQuaternion(const mozilla::gfx::Matrix4x4& aMatrix) {
  23     w = 0.5 *
  24         sqrt(std::max(1 + aMatrix[0][0] + aMatrix[1][1] + aMatrix[2][2], 0.0f));
  25     x = 0.5 *
  26         sqrt(std::max(1 + aMatrix[0][0] - aMatrix[1][1] - aMatrix[2][2], 0.0f));
  27     y = 0.5 *
  28         sqrt(std::max(1 - aMatrix[0][0] + aMatrix[1][1] - aMatrix[2][2], 0.0f));
  29     z = 0.5 *
  30         sqrt(std::max(1 - aMatrix[0][0] - aMatrix[1][1] + aMatrix[2][2], 0.0f));
  31
  32     if (aMatrix[2][1] > aMatrix[1][2]) x = -x;
  33     if (aMatrix[0][2] > aMatrix[2][0]) y = -y;
  34     if (aMatrix[1][0] > aMatrix[0][1]) z = -z;
  35   }
  36
  37   // Convert from |direction axis, angle| pair to gfxQuaternion.
  38   //
  39   // Reference:
  40   // https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation
  41   //
  42   // if the direction axis is (x, y, z) = xi + yj + zk,
  43   // and the angle is |theta|, this formula can be done using
  44   // an extension of Euler's formula:
  45   //   q = cos(theta/2) + (xi + yj + zk)(sin(theta/2))
  46   //     = cos(theta/2) +
  47   //       x*sin(theta/2)i + y*sin(theta/2)j + z*sin(theta/2)k
  48   // Note: aDirection should be an unit vector and
  49   //       the unit of aAngle should be Radian.
  50   gfxQuaternion(const mozilla::gfx::Point3D& aDirection, gfxFloat aAngle) {
  51     MOZ_ASSERT(mozilla::gfx::FuzzyEqual(aDirection.Length(), 1.0f),
  52                "aDirection should be an unit vector");
  53     x = aDirection.x * sin(aAngle / 2.0);
  54     y = aDirection.y * sin(aAngle / 2.0);
  55     z = aDirection.z * sin(aAngle / 2.0);
  56     w = cos(aAngle / 2.0);
  57   }
  58
  59   gfxQuaternion Slerp(const gfxQuaternion& aOther, gfxFloat aCoeff) const {
  60     gfxFloat dot = mozilla::clamped(DotProduct(aOther), -1.0, 1.0);
  61     if (dot == 1.0) {
  62       return *this;
  63     }
  64
  65     gfxFloat theta = acos(dot);
  66     gfxFloat rsintheta = 1 / sqrt(1 - dot * dot);
  67     gfxFloat rightWeight = sin(aCoeff * theta) * rsintheta;
  68
  69     gfxQuaternion left = *this;
  70     gfxQuaternion right = aOther;
  71
  72     left *= cos(aCoeff * theta) - dot * rightWeight;
  73     right *= rightWeight;
  74
  75     return left + right;
  76   }
  77
  78   using Super::operator*=;
  79
  80   // Quaternion multiplication
  81   // Reference:
  82   // https://en.wikipedia.org/wiki/Quaternion#Ordered_list_form
  83   //
  84   // (w1, x1, y1, z1)(w2, x2, y2, z2) = (w1w2 - x1x2 - y1y2 - z1z2,
  85   //                                     w1x2 + x1w2 + y1z2 - z1y2,
  86   //                                     w1y2 - x1z2 + y1w2 + z1x2,
  87   //                                     w1z2 + x1y2 - y1x2 + z1w2)
  88   gfxQuaternion operator*(const gfxQuaternion& aOther) const {
  89     return gfxQuaternion(
  90         w * aOther.x + x * aOther.w + y * aOther.z - z * aOther.y,
  91         w * aOther.y - x * aOther.z + y * aOther.w + z * aOther.x,
  92         w * aOther.z + x * aOther.y - y * aOther.x + z * aOther.w,
  93         w * aOther.w - x * aOther.x - y * aOther.y - z * aOther.z);
  94   }
  95   gfxQuaternion& operator*=(const gfxQuaternion& aOther) {
  96     *this = *this * aOther;
  97     return *this;
  98   }
  99
 100   mozilla::gfx::Matrix4x4 ToMatrix() const {
 101     mozilla::gfx::Matrix4x4 temp;
 102
 103     temp[0][0] = 1 - 2 * (y * y + z * z);
 104     temp[0][1] = 2 * (x * y + w * z);
 105     temp[0][2] = 2 * (x * z - w * y);
 106     temp[1][0] = 2 * (x * y - w * z);
 107     temp[1][1] = 1 - 2 * (x * x + z * z);
 108     temp[1][2] = 2 * (y * z + w * x);
 109     temp[2][0] = 2 * (x * z + w * y);
 110     temp[2][1] = 2 * (y * z - w * x);
 111     temp[2][2] = 1 - 2 * (x * x + y * y);
 112
 113     return temp;
 114   }
 115 };
 116
 117 #endif /* GFX_QUATERNION_H */