ESyS-Particle  2.3.2
Quaternion.hpp
Go to the documentation of this file.
1 
2 // //
3 // Copyright (c) 2003-2014 by The University of Queensland //
4 // Centre for Geoscience Computing //
5 // http://earth.uq.edu.au/centre-geoscience-computing //
6 // //
7 // Primary Business: Brisbane, Queensland, Australia //
8 // Licensed under the Open Software License version 3.0 //
9 // http://www.opensource.org/licenses/osl-3.0.php //
10 // //
12 
13 
14 #ifndef _QUATERNION_HPP
15 #define _QUATERNION_HPP
16 //
17 // ============================================================
18 //
19 // Quaternion.hpp
20 
21 #include "Foundation/Quaternion.h"
22 #include "Foundation/vec3.h"
23 #include "Foundation/Matrix3.h"
24 
25 //
26 // ============================================================
27 //
28 // CONSTRUCTORS, DESTRUCTORS
29 //
30 // ============================================================
31 //
32 
34  : vector(Vec3::ZERO),
35  scalar(1.0)
36 {
37 }
38 
40  : vector(v),
41  scalar(d)
42 {
43 }
44 
46  : vector(q.vector),
47  scalar(q.scalar)
48 {
49 }
50 
51 //
52 // ============================================================
53 //
54 // ASSIGNMENT
55 //
56 // ============================================================
57 //
58 
60 {
61 #if 0
62  if (&q == this) return *this;
63 #endif
64  vector = q.vector;
65  scalar = q.scalar;
66 
67  return *this;
68 }
69 
70 //
71 // ============================================================
72 //
73 // OUTPUT
74 //
75 // ============================================================
76 //
77 
78 QUATERNION_INLINE std::ostream& operator<<(std::ostream& co, const Quaternion& q)
79 {
80  return q.output(co);
81 }
82 
83 QUATERNION_INLINE std::istream& operator>>(std::istream& ci, Quaternion& q)
84 {
85  return q.input(ci);
86 }
87 
88 QUATERNION_INLINE std::ostream& Quaternion::output(std::ostream& co) const
89 {
90  co
91  << scalar << ' '
92  << vector;
93 
94  return co;
95 }
96 
97 QUATERNION_INLINE std::istream& Quaternion::input(std::istream& ci)
98 {
99  ci
100  >> scalar
101  >> vector;
102 
103  return ci;
104 }
105 
107 {
108  return
109  (
110  (return_sca() == q.return_sca())
111  &&
112  (return_vec() == q.return_vec())
113  );
114 }
115 
117 {
118  return !(*this == q);
119 }
120 
121 //
122 // ============================================================
123 //
124 // ARITHMETIC OPERATIONS
125 //
126 // ============================================================
127 //
128 
130 {
131 #if 0
132  Quaternion qq;
133 
134  qq.scalar = scalar + q2.scalar;
135  qq.vector = vector + q2.vector;
136 #endif
137  return Quaternion(scalar + q2.scalar, vector + q2.vector);
138 }
139 
141 {
142 #if 0
143  Quaternion qq;
144 
145  qq.scalar = scalar - q2.scalar;
146  qq.vector = vector - q2.vector;
147 #endif
148  return Quaternion(scalar-q2.scalar, vector-q2.vector);
149 }
150 
152 {
153 #if 0
154  Quaternion qq;
155 
156  qq.scalar = -scalar;
157  qq.vector = -vector;
158 #endif
159  return Quaternion(-scalar, -vector);
160 }
161 
163 {
164 #if 0
165  Quaternion qq;
166 
167  qq.scalar = c * q.scalar;
168  qq.vector = c * q.vector;
169 #endif
170  return Quaternion(c*q.scalar, c*q.vector);
171 }
172 
174 {
175  return Quaternion(c * scalar, c * vector);
176 }
177 
179 {
180 #if 0
181  Quaternion qq;
182 
183  qq.scalar = scalar * q2.scalar - dot(vector, q2.vector);
184  qq.vector = scalar * q2.vector
185  + q2.scalar * vector
186  + cross(vector, q2.vector);
187 #endif
188  return
189  Quaternion(
190  scalar * q2.scalar - dot(vector, q2.vector),
191  scalar * q2.vector
192  + q2.scalar * vector
193  + cross(vector, q2.vector)
194  );
195 }
196 
198 {
199  return Quaternion(scalar, -vector);
200 }
201 
203 {
204  return *this * q2.inverse();
205 }
206 
208 {
209  scalar += q.scalar;
210  vector += q.vector;
211 
212  return *this;
213 }
214 
216 {
217  scalar -= q.scalar;
218  vector -= q.vector;
219 
220  return *this;
221 }
222 
224 {
225  scalar *= c;
226  vector *= c;
227 
228  return *this;
229 }
230 
232 {
233  const double s = scalar * q.scalar - dot(vector, q.vector);
234  vector = scalar * q.vector
235  + q.scalar * vector
236  + cross(vector, q.vector);
237  scalar = s;
238 
239  return *this;
240 }
241 
243 {
244  Quaternion qq = q.inverse();
245 
246  const double s = scalar * qq.scalar - dot(vector, qq.vector);
247  vector = scalar * qq.vector
248  + qq.scalar * vector
249  + cross(vector, qq.vector);
250  scalar = s;
251 
252  return *this;
253 }
254 
255 
257 {
258  double len = length();
259 
260  if (len > 1.0e-8)
261  {
262  scalar /= len;
263  vector /= len;
264  }
265 }
266 
268 {
269  const double vlen = vector.norm();
270  return sqrt(scalar * scalar + vlen * vlen);
271 }
272 
274 {
275  double m[3][3];
276 
277  m[0][0] = scalar*scalar + vector.X()*vector.X()
278  - vector.Y()*vector.Y() -vector.Z()*vector.Z();
279  m[0][1] = 2*(vector.X()*vector.Y() + scalar*vector.Z());
280  m[0][2] = 2*(vector.X()*vector.Z() - scalar*vector.Y());
281  m[1][0] = 2*(vector.X()*vector.Y() - scalar*vector.Z());
282  m[1][1] = scalar*scalar - vector.X()*vector.X()
283  + vector.Y()*vector.Y() - vector.Z()*vector.Z();
284  m[1][2] = 2*(vector.Y()*vector.Z() + scalar*vector.X());
285  m[2][0] = 2*(vector.X()*vector.Z() + scalar*vector.Y());
286  m[2][1] = 2*(vector.Y()*vector.Z() - scalar*vector.X());
287  m[2][2] = scalar*scalar - vector.X()*vector.X()
288  - vector.Y()*vector.Y() + vector.Z()*vector.Z();
289  // In 2D case m[2][2] is not so accurate!
290 // m[2][2] = 0.0 ;
291 
292 /*
293 
294  m[0][0] = 1.0
295  - 2.0*vector.Y()*vector.Y() -2.0*vector.Z()*vector.Z();
296  m[0][1] = 2*(vector.X()*vector.Y() + scalar*vector.Z());
297  m[0][2] = 2*(vector.X()*vector.Z() - scalar*vector.Y());
298  m[1][0] = 2*(vector.X()*vector.Y() - scalar*vector.Z());
299  m[1][1] = 1.0 -2.0* vector.X()*vector.X()
300  - 2.0*vector.Z()*vector.Z();
301  m[1][2] = 2*(vector.Y()*vector.Z() + scalar*vector.X());
302  m[2][0] = 2*(vector.X()*vector.Z() + scalar*vector.Y());
303  m[2][1] = 2*(vector.Y()*vector.Z() - scalar*vector.X());
304  m[2][2] = 1.0 - 2.0*vector.X()*vector.X()
305  - 2.0*vector.Y()*vector.Y() ;
306 */
307 
308  return Matrix3(m);
309 }
310 
312 {
313  Vec3 v(this->vector);
314  return (v *= (2*acos(this->scalar)/v.norm()));
315 }
316 
318 {
319  return AngleAxisPair(2*acos(this->scalar), this->vector);
320 }
321 
322 #endif // _QUATERNION_HPP