Lines 178
##### Permissions
Viewable by Everyone
Editable by Michael Sutherland

# quaternion.cpp 0

 In Brief Implements quaternion functions in header quaternion.h
 Language C++
# 's
`  1#include "quaternion.h"  2#include "vector3.h"  3  4#include     <iostream>       // stream library  5#include     <math.h>           // math routines  6#include     <cstdlib>         // standard lib for exit() function  7  8//code modified from:  9//http://www.lboro.ac.uk/departments/ma/gallery/quat/src.html  10//by Andy Burbanks. 11 12using namespace std; 13 14Quaternion::Quaternion() 15{ 16   //(*this) = Quaternion::axisRot2Quat(0.25, 1, 0, 0); 17   m_r = 1; 18   m_i = 0; 19   m_j = 0; 20   m_k = 0; 21} 22 23Quaternion::Quaternion( float rr, float ii, float jj, float kk ) 24   : m_r(rr),m_i(ii),m_j(jj),m_k(kk) 25{} 26 27float Quaternion::norm() const 28{ 29   return m_r*m_r+m_i*m_i+m_j*m_j+m_k*m_k; 30} 31 32float Quaternion::abs() const 33{ 34   return sqrt(norm()); 35} 36 37void Quaternion::quat2AxisRot(float& angle, float& x, float& y, float& z) 38{ 39   if (!m_i && !m_j && !m_k) 40   { 41      angle = 0; 42      y = 1; 43      return; 44   } // there is no rotation... 45   //cout << "quat before" << *this << endl; 46   //cout << "abs = " << abs() << endl; 47   (*this) /= abs(); // normalize the quaternion 48   float s = m_i*m_i + m_j*m_j + m_k*m_k; 49   x = m_i / s; 50   y = m_j / s; 51   z = m_k / s; 52   //cout << "quat after = " << *this << endl; 53   //cout << "quat2AxisRot r = " << m_r << endl; 54   angle = 2*acos(m_r) * 180. / Math::kPi; // want the angle in degrees 55} 56 57Quaternion Quaternion::axisRot2Quat(float angle, float x, float y, float z) 58{ 59   float s = sin(angle / 2. * (Math::kPi / 180.)); 60   Quaternion quat(cos(angle/2. * (Math::kPi / 180.)), s*x, s*y, s*z); 61   return (quat /= quat.abs()); 62} 63 64void Quaternion::setQuatAxisRot(float angle, float x, float y, float z) 65{ 66   (*this) = Quaternion::axisRot2Quat(angle, x, y, z); 67} 68 69void Quaternion::addRot(float angle, float x, float y, float z) 70{ 71   // can't use *= because multiplication doesn't commute... 72   //cout << " abs = " << this->abs() << endl; 73   //(*this) = this->abs(); 74   (*this) = axisRot2Quat(angle, x, y, z) * (*this); 75   //cout << "addRot quat = " << *this << endl; 76   //cout << " abs = " << this->abs() << endl; 77   //(*this) /= this->abs(); 78} 79 80Vector3 Quaternion::transVec(const Vector3& pos) 81{ 82   Quaternion posQuat(0, pos[X], pos[Y], pos[Z]); 83   Quaternion conjugate = (*this); 84   conjugate.conjugate(); 85   Quaternion posTrans = (*this) * posQuat * conjugate; 86   return Vector3(posTrans.I(), posTrans.J(), posTrans.K()); 87} 88 89Vector3 Quaternion::invTransVec(const Vector3& pos) 90{ 91   Quaternion posQuat(0, pos[X], pos[Y], pos[Z]); 92   Quaternion temp = (*this); 93   temp.setR(-temp.R()); 94   Quaternion conjugate = temp; 95   conjugate.conjugate(); 96   Quaternion posTrans = temp * posQuat * conjugate; 97   return Vector3(posTrans.I(), posTrans.J(), posTrans.K()); 98} 99100Quaternion& Quaternion::negate()101{102   m_r = -m_r;103   m_i = -m_i;104   m_j = -m_j;105   m_k = -m_k;106   return *this;107}108109Quaternion& Quaternion::conjugate()110{111   m_i = -m_i;112   m_j = -m_j;113   m_k = -m_k;114   return *this;115}116117Quaternion& Quaternion::square()118{119   float temp = 2*m_r;120   m_r = m_r*m_r-(m_i*m_i+m_j*m_j+m_k*m_k);121   m_i *= temp;122   m_j *= temp;123   m_k *= temp;124   return *this;125}126127Quaternion& Quaternion::invert()128{129   float temp = abs();130   m_r /= temp;131   m_i /= -temp;132   m_j /= -temp;133   m_k /= -temp;134   return (*this);       // Okay, same norm.135}136137Quaternion& Quaternion::operator+=(const Quaternion& b)138{139   m_r+=b.m_r; m_i+=b.m_i; m_j+=b.m_j; m_k+=b.m_k; return *this;140}141142Quaternion& Quaternion::operator-=(const Quaternion& b)143{144   m_r-=b.m_r; m_i-=b.m_i; m_j-=b.m_j; m_k-=b.m_k; return *this;145}146147Quaternion& Quaternion::operator*=(const float b)148{149   m_r*=b; m_i*=b; m_j*=b; m_k*=b; return *this;150}151152Quaternion& Quaternion::operator/=(const float b)153{154   m_r/=b; m_i/=b; m_j/=b; m_k/=b; return *this;155}156157Quaternion& Quaternion::operator*=(const Quaternion& b)158{159   float t0 = (m_k-m_j)*(b.m_j-b.m_k);160   float t1 = (m_r+m_i)*(b.m_r+b.m_i);161   float t2 = (m_r-m_i)*(b.m_j+b.m_k);162   float t3 = (m_k+m_j)*(b.m_r-b.m_i);163   float t4 = (m_k-m_i)*(b.m_i-b.m_j);164   float t5 = (m_k+m_i)*(b.m_i+b.m_j);165   float t6 = (m_r+m_j)*(b.m_r-b.m_k);166   float t7 = (m_r-m_j)*(b.m_r+b.m_k);167   float t8 = t5+t6+t7;168   float t9 = (t4+t8)/2;169   m_r = t0+t9-t5;170   m_i = t1+t9-t8;171   m_j = t2+t9-t7;172   m_k = t3+t9-t6;173   return *this;174}175176Quaternion& Quaternion::operator/=(const Quaternion& b)177{178   Quaternion temp = b;179   return (*this)*=(temp.invert());      // Ugh!180}181182int operator==(const Quaternion& a, const Quaternion& b)183{184   return (a.m_r==b.m_r && a.m_i==b.m_i && a.m_j==b.m_j && a.m_k==b.m_k);185}186187int operator!=(const Quaternion& a, const Quaternion& b)188{189   return !(a==b); // derived operator190}191192Quaternion operator-(Quaternion& x)193{194   return x.negate();195}196197Quaternion sqrt(const Quaternion& x)198{199   cerr<<"sqrt(const Quaternion&) undefined because Quaternion::sqrt() undefined."<<endl;200   exit(1);      // Should be derived from member function.201}202203std::ostream& operator<<(std::ostream& ost, const Quaternion& x)204{205   return ost<<"( "<<x.m_r<<", "<<x.m_i<<", "<<x.m_j<<", "<<x.m_k<<" )";206}`

Implements quaternion functions in header quaternion.h