License Public Domain
Lines 178
Included in this Library
Permissions
Viewable by Everyone
Editable by Michael Sutherland
Hide
Bored? Check out the Recent Activity on Siafoo Join Siafoo Now or Learn More

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