Quat.cpp

   1 #include <math.h>
   2
   3 #include "Quat.h"
   4
   5 Quat::Quat () {}
   6
   7 Quat::Quat(double sc,
   8                 Vector vec)
   9 {
  10   s = sc;
  11   v = vec;
  12 }
  13
  14 Quat::Quat(double sc,
  15            double x,
  16            double y,
  17            double z)
  18 {
  19   Quat (sc, Vector(x,y,z));
  20 }
  21
  22 Quat::Quat(double roll,
  23            double pitch,
  24            double yaw)
  25 {
  26   Quat Qr (cos (roll/2),
  27            sin (roll/2),
  28            0,
  29            0);
  30   Quat Qp (cos (pitch/2),
  31            0,
  32            sin (pitch/2),
  33            0);
  34   Quat Qy (cos (yaw/2),
  35            0,
  36            0,
  37            sin (yaw/2));
  38
  39   Quat res = Qr * Qp * Qy;
  40
  41   s = res.s;
  42   v = res.v;
  43 }
  44
  45 Quat Quat::operator+(const Quat& q2) const
  46 {
  47   return Quat (s + q2.s, v + q2.v);
  48 }
  49
  50 Quat Quat::operator-(const Quat& q2) const
  51 {
  52   return Quat (s - q2.s, v - q2.v);
  53 }
  54
  55 Quat Quat::operator*(const Quat& q2) const
  56 {
  57   return Quat (s * q2.s - v.dot (q2.v),
  58                q2.v.scale (s) + v.scale (q2.s) + v.cross (q2.v)
  59                );
  60 }
  61
  62 Quat Quat::conj () const
  63 {
  64   return Quat (s, v.scale (-1));
  65 }
  66
  67 double Quat::norm () const
  68 {
  69   return s*s + v.dot(v);
  70 }
  71
  72 Quat Quat::scale (double sc) const
  73 {
  74   return Quat (s, v.scale (sc));
  75 }
  76
  77 Quat Quat::inv () const
  78 {
  79   return this->conj ().scale (1.0 / this->norm ());
  80 }