Line data    Source code

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