Advertisement
Guest User

Untitled

a guest
Jan 23rd, 2018
85
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 3.55 KB | None | 0 0
  1. #include "vector.h"
  2. #include "matrix.h"
  3.  
  4. class Quaternion : public vec4
  5. {
  6. public:
  7.     Quaternion()
  8.         : vec4{ 0.0f, 0.0f, 0.0f, 1.0f }
  9.     {
  10.  
  11.     }
  12.  
  13.     Quaternion(float x, float y, float z, float w)
  14.         : vec4{ x, y, z, w }
  15.     {
  16.  
  17.     }
  18.  
  19.     Quaternion(vec3 axis, float angle)
  20.         : vec4{ axis.getX() * std::sin(angle / 2.0f), axis.getY() * std::sin(angle / 2.0f), axis.getZ() * std::sin(angle / 2.0f), std::cos(angle / 2.0f) }
  21.     {
  22.     }
  23.  
  24.     Quaternion(const mat4& rotationMatrix)
  25.     {
  26.         const float t = rotationMatrix.get(0, 0) + rotationMatrix.get(1, 1) + rotationMatrix.get(2, 2);
  27.         if (t > 0.0f)
  28.         {
  29.             const float r = std::sqrt(1 + t);
  30.             const float s = 1.0f / (2.0f * r);
  31.             setW(1.0f / 2.0f * r);
  32.             setX((rotationMatrix.get(2, 1) - rotationMatrix.get(1, 2)) * s);
  33.             setY((rotationMatrix.get(0, 2) - rotationMatrix.get(2, 0)) * s);
  34.             setZ((rotationMatrix.get(1, 0) - rotationMatrix.get(0, 1)) * s);
  35.         }
  36.         else if (rotationMatrix.get(0, 0) > rotationMatrix.get(1, 1) && rotationMatrix.get(0, 0) > rotationMatrix.get(2, 2))
  37.         {
  38.             const float r = std::sqrt(1 + rotationMatrix.get(0, 0) - rotationMatrix.get(1, 1) - rotationMatrix.get(2, 2));
  39.             const float s = 1.0f / (2.0f * r);
  40.             setW((rotationMatrix.get(2, 1) - rotationMatrix.get(1, 2)) * s);
  41.             setX(1.0f / 2.0f * r);
  42.             setY((rotationMatrix.get(0, 1) + rotationMatrix.get(1, 0)) * s);
  43.             setZ((rotationMatrix.get(2, 0) + rotationMatrix.get(0, 2)) * s);
  44.         }
  45.         else if (rotationMatrix.get(1, 1) > rotationMatrix.get(0, 0) && rotationMatrix.get(1, 1) > rotationMatrix.get(2, 2))
  46.         {
  47.             const float r = std::sqrt(1 - rotationMatrix.get(0, 0) + rotationMatrix.get(1, 1) - rotationMatrix.get(2, 2));
  48.             const float s = 1.0f / (2.0f * r);
  49.             setW((rotationMatrix.get(0, 2) - rotationMatrix.get(2, 0)) * s);
  50.             setX((rotationMatrix.get(0, 1) + rotationMatrix.get(1, 0)) * s);
  51.             setY(1.0f / 2.0f * r);
  52.             setZ((rotationMatrix.get(2, 1) + rotationMatrix.get(1, 2)) * s);
  53.         }
  54.         else
  55.         {
  56.             const float r = std::sqrt(1 - rotationMatrix.get(0, 0) - rotationMatrix.get(1, 1) + rotationMatrix.get(2, 2));
  57.             const float s = 1.0f / (2.0f * r);
  58.             setW((rotationMatrix.get(1, 0) - rotationMatrix.get(0, 1)) * s);
  59.             setX((rotationMatrix.get(0, 2) + rotationMatrix.get(2, 0)) * s);
  60.             setY((rotationMatrix.get(2, 1) + rotationMatrix.get(1, 2)) * s);
  61.             setZ(1.0f / 2.0f * r);
  62.         }
  63.     }
  64.  
  65.     vec3 rotate(const vec3& vector) const
  66.     {
  67.         Quaternion result = *this * vector * this->conjugate();
  68.  
  69.         return vec3{ result.getX(), result.getY(), result.getZ() };
  70.     }
  71.  
  72.     Quaternion conjugate() const
  73.     {
  74.         return Quaternion(-getX(), -getY(), -getZ(), getW());
  75.     }
  76.  
  77.     Quaternion operator*(const Quaternion& rhs) const
  78.     {
  79.         return Quaternion(
  80.             getW() * rhs.getX() + getX() * rhs.getW() + getY() * rhs.getZ() - getZ() * rhs.getY(),
  81.             getW() * rhs.getY() - getX() * rhs.getZ() + getY() * rhs.getW() + getZ() * rhs.getX(),
  82.             getW() * rhs.getZ() + getX() * rhs.getY() - getY() * rhs.getX() + getZ() * rhs.getW(),
  83.             getW() * rhs.getW() - getX() * rhs.getX() - getY() * rhs.getY() - getZ() * rhs.getZ()
  84.         );
  85.     }
  86.  
  87.     Quaternion operator*(const vec3& rhs) const
  88.     {
  89.         return *this * Quaternion(rhs.getX(), rhs.getY(), rhs.getZ(), 1.0f);
  90.     }
  91.  
  92.     mat4 getRotationMatrix() const
  93.     {
  94.         const float x = getX();
  95.         const float y = getY();
  96.         const float z = getZ();
  97.         const float w = getW();
  98.  
  99.         return {
  100.             { 1 - 2 * (y * y + z * z), 2 * (x * y - z * w)    , 2 * (x * z + y * w)    , 0.0f },
  101.             { 2 * (x * y + z * w)    , 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)    , 0.0f },
  102.             { 2 * (x * z - y * w)    , 2 * (y * z + x * w)    , 1 - 2 * (x * x + y * y), 0.0f },
  103.             { 0.0f                   , 0.0f,                    0.0f,                    1.0f }
  104.         };
  105.     }
  106. };
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement