License Public Domain
Lines 178
Included in this Library
Permissions
Viewable by Everyone
Editable by Michael Sutherland

quaternion.cpp Atom Feed 0

In Brief Implements quaternion functions in header quaternion.h
# '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}
99
100Quaternion& 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}
108
109Quaternion& Quaternion::conjugate()
110{
111 m_i = -m_i;
112 m_j = -m_j;
113 m_k = -m_k;
114 return *this;
115}
116
117Quaternion& 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}
126
127Quaternion& 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}
136
137Quaternion& 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}
141
142Quaternion& 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}
146
147Quaternion& Quaternion::operator*=(const float b)
148{
149 m_r*=b; m_i*=b; m_j*=b; m_k*=b; return *this;
150}
151
152Quaternion& Quaternion::operator/=(const float b)
153{
154 m_r/=b; m_i/=b; m_j/=b; m_k/=b; return *this;
155}
156
157Quaternion& 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}
175
176Quaternion& Quaternion::operator/=(const Quaternion& b)
177{
178 Quaternion temp = b;
179 return (*this)*=(temp.invert()); // Ugh!
180}
181
182int 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}
186
187int operator!=(const Quaternion& a, const Quaternion& b)
188{
189 return !(a==b); // derived operator
190}
191
192Quaternion operator-(Quaternion& x)
193{
194 return x.negate();
195}
196
197Quaternion 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}
202
203std::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