Libs/Math3D/Quaternion.cpp

   1 /*
   2 Cafu Engine, http://www.cafu.de/
   3 Copyright (c) Carsten Fuchs and other contributors.
   4 This project is licensed under the terms of the MIT license.
   5 */
   6
   7 #include "Quaternion.hpp"
   8 #include "Angles.hpp"
   9 #include "Matrix3x3.hpp"
  10
  11
  12 using namespace cf::math;
  13
  14
  15 template<class T> QuaternionT<T>::QuaternionT(const AnglesT<T>& Angles)
  16 {
  17     // See http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q60 for details.
  18     const T SinYaw   = sin(T(0.5) * AnglesT<T>::DegToRad(Angles.yaw()));
  19     const T CosYaw   = cos(T(0.5) * AnglesT<T>::DegToRad(Angles.yaw()));
  20     const T SinPitch = sin(T(0.5) * AnglesT<T>::DegToRad(Angles.pitch()));
  21     const T CosPitch = cos(T(0.5) * AnglesT<T>::DegToRad(Angles.pitch()));
  22     const T SinRoll  = sin(T(0.5) * AnglesT<T>::DegToRad(Angles.roll()));
  23     const T CosRoll  = cos(T(0.5) * AnglesT<T>::DegToRad(Angles.roll()));
  24
  25     x = SinRoll*CosPitch*CosYaw - CosRoll*SinPitch*SinYaw;
  26     y = CosRoll*SinPitch*CosYaw + SinRoll*CosPitch*SinYaw;
  27     z = CosRoll*CosPitch*SinYaw - SinRoll*SinPitch*CosYaw;
  28     w = CosRoll*CosPitch*CosYaw + SinRoll*SinPitch*SinYaw;
  29 }
  30
  31
  32 template<class T> QuaternionT<T>::QuaternionT(const Matrix3x3T<T>& Mat)
  33     : x(0), y(0), z(0), w(1)
  34 {
  35     // The idea is from <http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q55>,
  36     // but we use our own approach for finding the operative factor.
  37     const T t[]={ 0,
  38                   T(1.0) + Mat[0][0] + Mat[1][1] + Mat[2][2],
  39                   T(1.0) + Mat[0][0] - Mat[1][1] - Mat[2][2],
  40                   T(1.0) + Mat[1][1] - Mat[0][0] - Mat[2][2],
  41                   T(1.0) + Mat[2][2] - Mat[0][0] - Mat[1][1] };
  42
  43     unsigned int Max=0;
  44
  45     for (unsigned int i=1; i<5; i++)
  46         if (t[i]>t[Max]) Max=i;
  47
  48     const T s=T(2.0) * sqrt(t[Max]);
  49
  50     if (Max==1)
  51     {
  52         x = (Mat[2][1] - Mat[1][2]) / s;
  53         y = (Mat[0][2] - Mat[2][0]) / s;
  54         z = (Mat[1][0] - Mat[0][1]) / s;
  55         w = T(0.25) * s;
  56     }
  57     else if (Max==2)
  58     {
  59         x = T(0.25) * s;
  60         y = (Mat[1][0] + Mat[0][1]) / s;
  61         z = (Mat[0][2] + Mat[2][0]) / s;
  62         w = (Mat[2][1] - Mat[1][2]) / s;
  63     }
  64     else if (Max==3)
  65     {
  66         x = (Mat[1][0] + Mat[0][1]) / s;
  67         y = T(0.25) * s;
  68         z = (Mat[2][1] + Mat[1][2]) / s;
  69         w = (Mat[0][2] - Mat[2][0]) / s;
  70     }
  71     else if (Max==4)
  72     {
  73         x = (Mat[0][2] + Mat[2][0]) / s;
  74         y = (Mat[2][1] + Mat[1][2]) / s;
  75         z = T(0.25) * s;
  76         w = (Mat[1][0] - Mat[0][1]) / s;
  77     }
  78 }
  79
  80
  81 template<class T> QuaternionT<T>::QuaternionT(const Vector3T<T>& Axis, const T Angle)
  82 {
  83     // Convert the rotation axis and angle to a quaternion as described at
  84     // http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q56
  85     assert(::length(Axis) > T(0.99) && ::length(Axis) < T(1.01));
  86
  87     const T sin_a = sin(T(0.5) * Angle);
  88     const T cos_a = cos(T(0.5) * Angle);
  89
  90     x = Axis.x * sin_a;
  91     y = Axis.y * sin_a;
  92     z = Axis.z * sin_a;
  93     w = cos_a;
  94
  95     assert(IsNormal(T(0.01)));
  96 }
  97
  98
  99 template<class T> QuaternionT<T> QuaternionT<T>::Euler(const T Pitch, const T Yaw, const T Roll)
 100 {
 101     // See <http://www.j3d.org/matrix_faq/matrfaq_latest.html#Q60> for details.
 102     const T SinPitch=sin(T(0.5)*Pitch);
 103     const T CosPitch=cos(T(0.5)*Pitch);
 104     const T SinYaw  =sin(T(0.5)*Yaw);
 105     const T CosYaw  =cos(T(0.5)*Yaw);
 106     const T SinRoll =sin(T(0.5)*Roll);
 107     const T CosRoll =cos(T(0.5)*Roll);
 108
 109     return QuaternionT(
 110         SinRoll*CosPitch*CosYaw - CosRoll*SinPitch*SinYaw,
 111         CosRoll*SinPitch*CosYaw + SinRoll*CosPitch*SinYaw,
 112         CosRoll*CosPitch*SinYaw - SinRoll*SinPitch*CosYaw,
 113         CosRoll*CosPitch*CosYaw + SinRoll*SinPitch*SinYaw);
 114 }
 115
 116
 117 template class QuaternionT<float>;
 118 template class QuaternionT<double>;