appli/TempAirwaysAppli/MathLib/Quaternion.cpp

   1 //----------------------------------------------------------------------------
   2 // Class definition include
   3 //----------------------------------------------------------------------------
   4
   5 #include "Quaternion.h"
   6
   7 //----------------------------------------------------------------------------
   8 // Class implementation
   9 //----------------------------------------------------------------------------
  10 //----------------------------------------------------------------------------
  11 // Builder
  12 //----------------------------------------------------------------------------
  13
  14 Quaternion::Quaternion( )
  15 {
  16         theta = 0.0;
  17         w = Vec3(0,0,0);
  18         r = cos(theta/2);
  19         double sinHalfTheta = sin(theta/2);
  20         im = w*sinHalfTheta;
  21 }
  22
  23 Quaternion::Quaternion(Vec3 v1, Vec3 v2)
  24 {
  25         // Normalize the vectors
  26         v1.Normalize();
  27         v2.Normalize();
  28
  29         // Dot product
  30         float cosTheta = v1.Dot(v2);
  31         theta = acos(cosTheta);
  32
  33         // Cross product
  34         w = v1.Cross(v2);
  35
  36         // Normalization
  37         w.Normalize();
  38
  39         // Build the quaternion
  40         r = cos(theta/2);
  41         double sinHalfTheta = sin(theta/2);
  42         im = w*sinHalfTheta;
  43 }
  44
  45 Quaternion::Quaternion(float nTheta, Vec3 nW)
  46 {
  47         theta = nTheta;
  48         w = nW;
  49         w.Normalize();
  50
  51         // Build the quaternion
  52         r = cos(theta/2);
  53         double sinHalfTheta = sin(theta/2);
  54         im = w*sinHalfTheta;
  55 }
  56
  57 Quaternion::Quaternion(float nR, float nI, float nJ, float nK)
  58 {
  59         r = nR;
  60         im = Vec3(nI, nJ, nK);
  61
  62         theta = 2*acos(r);
  63         double sinHalfTheta = sin(theta/2);
  64         w = im/sinHalfTheta;
  65 }
  66
  67 //----------------------------------------------------------------------------
  68 // Destructor
  69 //----------------------------------------------------------------------------
  70
  71 Quaternion::~Quaternion( )
  72 {
  73
  74 }
  75 //----------------------------------------------------------------------------
  76 // Methods
  77 //----------------------------------------------------------------------------
  78
  79 float Quaternion::getR()
  80 {
  81         return r;
  82 }
  83
  84 float Quaternion::getI()
  85 {
  86         return w[0];
  87 }
  88
  89 float Quaternion::getJ()
  90 {
  91         return w[1];
  92 }
  93
  94 float Quaternion::getK()
  95 {
  96         return w[2];
  97 }
  98
  99 Vec3 Quaternion::getW()
 100 {
 101         return w;
 102 }
 103
 104 Quaternion Quaternion::conjugate( )
 105 {
 106         return Quaternion(theta, -w);
 107 }
 108
 109 Vec3 Quaternion::rotateVector(Vec3 vector)
 110 {
 111         // Quaterion representing the vector
 112         vector.Normalize();
 113         Quaternion qv(0,vector[0],vector[1],vector[2]);
 114         Quaternion qthis(r,im[0],im[1],im[2]);
 115         Quaternion qthis_conj = qthis.conjugate();
 116
 117
 118         cout << "Qv: [" << qv << "]" << endl;
 119         cout << "Qthis: [" << qthis << "]" << endl;
 120         cout << "QthisConj: [" << qthis_conj << "]" << endl;
 121
 122         // Final quaternion
 123         Quaternion Qtemp = qthis*qv;
 124         cout << "Qtemp: [" << Qtemp << "]" << endl;
 125
 126         Quaternion qf = Qtemp*qthis_conj;
 127
 128         cout << "Qf: [" << qf << "]" << endl;
 129
 130         return qf.im;
 131         //return Vec3(0,0,0);
 132 }
 133
 134 //----------------------------------------------------------------------------
 135 // Operators
 136 //----------------------------------------------------------------------------
 137
 138 const float& Quaternion::operator[](unsigned int i) const
 139 {
 140         return (i == 0 ? r : (im)[i-1]);
 141 }
 142
 143 Quaternion Quaternion::operator*(Quaternion q)
 144 {
 145         float a1 = this->r;
 146         float b1 = this->w[0];
 147         float c1 = this->w[1];
 148         float d1 = this->w[2];
 149
 150         float a2 = q.r;
 151         float b2 = q.w[0];
 152         float c2 = q.w[1];
 153         float d2 = q.w[2];
 154
 155         float r_n = a1*a2 - b1*b2 - c1*c2 - d1*d2;
 156         float i_n = a1*b2 + a2*b1 + c1*d2 - c2*d1;
 157         float j_n = a1*c2 - b1*d2 + a2*c1 + b2*d1;
 158         float k_n = a1*d2 + b1*c2 - b2*c1 + a2*d1;
 159
 160         return Quaternion(r_n, i_n, j_n, k_n);
 161 }
 162
 163 std::ostream& operator<<(std::ostream& os, const Quaternion& q)
 164 {
 165         os << q[0] << " " << q[1] << " " << q[2] << " " << q[3];
 166         os << " " <<q.theta*180/M_PI << " " << q.w;
 167         return os;
 168 }