Line data    Source code

       1             : /* -*- Mode: C++; tab-width: 20; 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             : #include "Quaternion.h"
       7             : #include "Matrix.h"
       8             : #include "Tools.h"
       9             : #include <algorithm>
      10             : #include <ostream>
      11             : #include <math.h>
      12             : 
      13             : using namespace std;
      14             : 
      15             : namespace mozilla {
      16             : namespace gfx {
      17             : 
      18             : std::ostream&
      19           0 : operator<<(std::ostream& aStream, const Quaternion& aQuat)
      20             : {
      21           0 :   return aStream << "< " << aQuat.x << " "  << aQuat.y << " " << aQuat.z << " " << aQuat.w << ">";
      22             : }
      23             : 
      24             : void
      25           0 : Quaternion::SetFromRotationMatrix(const Matrix4x4& m)
      26             : {
      27             :   // see http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/index.htm
      28           0 :   const Float trace = m._11 + m._22 + m._33;
      29           0 :   if (trace > 0.0) {
      30           0 :     const Float s = 0.5f / sqrt(trace + 1.0f);
      31           0 :     w = 0.25f / s;
      32           0 :     x = (m._32 - m._23) * s;
      33           0 :     y = (m._13 - m._31) * s;
      34           0 :     z = (m._21 - m._12) * s;
      35           0 :   } else if (m._11 > m._22 && m._11 > m._33) {
      36           0 :     const Float s = 2.0f * sqrt(1.0f + m._11 - m._22 - m._33);
      37           0 :     w = (m._32 - m._23) / s;
      38           0 :     x = 0.25f * s;
      39           0 :     y = (m._12 + m._21) / s;
      40           0 :     z = (m._13 + m._31) / s;
      41           0 :   } else if (m._22 > m._33) {
      42           0 :     const Float s = 2.0 * sqrt(1.0f + m._22 - m._11 - m._33);
      43           0 :     w = (m._13 - m._31) / s;
      44           0 :     x = (m._12 + m._21) / s;
      45           0 :     y = 0.25f * s;
      46           0 :     z = (m._23 + m._32) / s;
      47             :   } else {
      48           0 :     const Float s = 2.0 * sqrt(1.0f + m._33 - m._11 - m._22);
      49           0 :     w = (m._21 - m._12) / s;
      50           0 :     x = (m._13 + m._31) / s;
      51           0 :     y = (m._23 + m._32) / s;
      52           0 :     z = 0.25f * s;
      53             :   }
      54           0 : }
      55             : 
      56             : } // namespace gfx
      57             : } // namespace mozilla