Theoretica
A C++ numerical and automatic mathematical library
Loading...
Searching...
No Matches
quat.h
Go to the documentation of this file.
1
5
6#ifndef THEORETICA_QUATERNION_H
7#define THEORETICA_QUATERNION_H
8
9#ifndef THEORETICA_NO_PRINT
10#include <sstream>
11#include <ostream>
12#endif
13
14#include "../core/real_analysis.h"
15#include "../algebra/algebra.h"
16
17
18namespace theoretica {
19
22 template<typename Type = real>
23 class quat {
24 public:
25
28
30 Type b, c, d;
31
32
34 quat() : a(0), b(0), c(0), d(0) {}
35
36
38 quat(Type r) : a(r), b(0), c(0), d(0) {}
39
40
43 : a(a), b(b), c(c), d(d) {}
44
45
47 quat(const quat& q) : a(q.a), b(q.b), c(q.c), d(q.d) {}
48
49
51 inline quat& operator=(const quat& q) {
52 a = q.a;
53 b = q.b;
54 c = q.c;
55 d = q.d;
56 return *this;
57 }
58
59
61 template<typename T>
62 inline quat& operator=(const std::array<T, 4>& v) {
63 a = v[0];
64 b = v[1];
65 c = v[2];
66 d = v[3];
67 return *this;
68 }
69
70
72 inline Type Re() const {
73 return a;
74 }
75
76
78 inline friend Type Re(const quat& q) {
79 return q.a;
80 }
81
82
84 inline Type Im1() const {
85 return b;
86 }
87
88
90 inline friend Type Im1(const quat& q) {
91 return q.b;
92 }
93
94
96 inline Type Im2() const {
97 return c;
98 }
99
100
102 inline friend Type Im2(const quat& q) {
103 return q.c;
104 }
105
106
108 inline Type Im3() const {
109 return d;
110 }
111
112
114 inline friend Type Im3(const quat& q) {
115 return q.d;
116 }
117
118
120 inline quat conjugate() const {
121 return quat(a, -b, -c, -d);
122 }
123
124
126 inline Type sqr_norm() const {
127 return a * a + b * b + c * c + d * d;
128 }
129
130
132 inline Type norm() const {
133 return sqrt(sqr_norm());
134 }
135
136
138 inline quat inverse() const {
139
140 const Type n = sqr_norm();
141
142 if(n < MACH_EPSILON) {
143 TH_MATH_ERROR("quat::inverse", n, DIV_BY_ZERO);
144 return quat((Type) nan());
145 }
146
147 return conjugate() / sqr_norm();
148 }
149
150
152 inline quat& invert() {
153
154 const Type n = sqr_norm();
155
156 if(n < MACH_EPSILON) {
157 TH_MATH_ERROR("quat::invert", n, DIV_BY_ZERO);
158 return quat((Type) nan());
159 }
160
161 return (*this = quat(
162 a / n,
163 b / n * -1,
164 c / n * -1,
165 d / n * -1
166 ));
167 }
168
169
171 inline quat operator+() const {
172 return *this;
173 }
174
175
177 inline quat operator-() const {
178 return quat(-a, -b, -c, -d);
179 }
180
181
183 inline quat operator+(Type k) const {
184 return quat(a + k, b, c, d);
185 }
186
187
189 inline quat operator-(Type k) const {
190 return quat(a - k, b, c, d);
191 }
192
193
195 inline quat operator*(Type k) const {
196 return quat(a * k, b * k, c * k, d * k);
197 }
198
199
201 inline quat operator/(Type k) const {
202
203 if(abs(k) < MACH_EPSILON) {
204 TH_MATH_ERROR("quat::operator/", k, DIV_BY_ZERO);
205 return quat(nan());
206 }
207
208 return quat(a / k, b / k, c / k, d / k);
209 }
210
211
213 inline quat operator+(const quat& other) const {
214 return quat(a + other.a, b + other.b, c + other.c, d + other.d);
215 }
216
217
219 inline quat operator-(const quat& other) const {
220 return quat(a - other.a, b - other.b, c - other.c, d - other.d);
221 }
222
223
225 inline quat operator*(const quat& q) const {
226
227 quat r;
228
229 r.a = a * q.a - b * q.b - c * q.c - d * q.d;
230 r.b = a * q.b + b * q.a + c * q.d - d * q.c;
231 r.c = a * q.c - b * q.d + c * q.a + d * q.b;
232 r.d = a * q.d + b * q.c - c * q.b + d * q.a;
233
234 return r;
235 }
236
237
239 inline quat operator/(const quat& other) const {
240 return operator*(other.inverse());
241 }
242
243
246 return (*this = operator*(k));
247 }
248
249
252
253 if(abs(k) < MACH_EPSILON) {
254 TH_MATH_ERROR("quat::operator/=", k, DIV_BY_ZERO);
255 return (*this = quat(nan()));
256 }
257
258 a /= k;
259 b /= k;
260 c /= k;
261 d /= k;
262 return *this;
263 }
264
265
267 inline quat& operator+=(const quat& other) {
268 a += other.a;
269 b += other.b;
270 c += other.c;
271 d += other.d;
272 return *this;
273 }
274
275
277 inline quat& operator-=(const quat& other) {
278 a -= other.a;
279 b -= other.b;
280 c -= other.c;
281 d -= other.d;
282 return *this;
283 }
284
285
287 inline quat& operator*=(const quat& other) {
288 return (*this = operator*(other));
289 }
290
291
293 inline quat& operator/=(const quat& other) {
294 return operator*=(other.inverse());
295 }
296
297
299 template<typename Vector>
300 inline Vector transform(const Vector& v) const {
301
302 Vector res;
303 res.resize(3);
304
305 if(v.size() != 3) {
306 TH_MATH_ERROR("quat::transform", v.size(), INVALID_ARGUMENT);
308 return res;
309 }
310
311 const quat q = quat(0, v.get(0), v.get(1), v.get(2));
312 const quat r = (*this * q) * inverse();
313
314 res[0] = r.b;
315 res[1] = r.c;
316 res[2] = r.d;
317
318 return res;
319 }
320
321
324 template<typename Vector>
325 inline static quat rotation(Type rad, const Vector& axis) {
326 return quat(
327 cos(rad / 2.0),
329 );
330 }
331
332
335 template<typename Vector>
336 inline static Vector rotate(const Vector& v, Type rad, const Vector& axis) {
337
338 Vector res;
339 res.resize(3);
340
341 if(axis.size() != 3) {
342 TH_MATH_ERROR("quat::rotate", axis.size(), INVALID_ARGUMENT);
344 return res;
345 }
346
347 if(v.size() != 3) {
348 TH_MATH_ERROR("quat::rotate", v.size(), INVALID_ARGUMENT);
350 return res;
351 }
352
354
355 const Type s = sin(rad / 2.0);
356 const Type c = cos(rad / 2.0);
357
358 const quat q = quat(
359 c, n_axis.get(0) * s, n_axis.get(1) * s, n_axis.get(2) * s);
360
361 const quat q_inv = q.conjugate();
362 const quat p = quat(0, v.get(0), v.get(1), v.get(2));
363
364 const quat r = q * p * q_inv;
365 res[0] = r.b;
366 res[1] = r.c;
367 res[2] = r.d;
368
369 return res;
370 }
371
372
373 // Friend operators to enable equations of the form
374 // (real) op. (quat)
375
376 inline friend quat operator+(Type r, const quat& z) {
377 return z + quat(r, 0, 0, 0);
378 }
379
380 inline friend quat operator-(Type r, const quat& z) {
381 return (z * -1) + quat(r, 0, 0, 0);
382 }
383
384 inline friend quat operator*(Type r, const quat& z) {
385 return z * r;
386 }
387
388 inline friend quat operator/(Type r, const quat& z) {
389 return quat(r, 0, 0, 0) / z;
390 }
391
392
393
394#ifndef THEORETICA_NO_PRINT
395
397 inline std::string to_string() const {
398
399 std::stringstream res;
400
401 res << a;
402 res << (b >= 0 ? " + " : " - ") << abs(b) << "i";
403 res << (c >= 0 ? " + " : " - ") << abs(c) << "j";
404 res << (d >= 0 ? " + " : " - ") << abs(d) << "k";
405
406 return res.str();
407 }
408
409
411 inline operator std::string() {
412 return to_string();
413 }
414
415
418 inline friend std::ostream& operator<<(std::ostream& out, const quat& obj) {
419 return out << obj.to_string();
420 }
421
422#endif
423
424 };
425
426}
427
428#endif
Quaternion class in the form .
Definition quat.h:23
quat operator-() const
Get the opposite of the quaternion.
Definition quat.h:177
static quat rotation(Type rad, const Vector &axis)
Construct a quaternion which represents a rotation of <rad> radians around the <axis> arbitrary axis.
Definition quat.h:325
quat & operator+=(const quat &other)
Add a quaternion to this one.
Definition quat.h:267
quat()
Construct a quaternion with all zero components.
Definition quat.h:34
quat operator/(Type k) const
Divide a quaternion by a scalar value.
Definition quat.h:201
friend Type Im2(const quat &q)
Extract the second imaginary part of the quaternion.
Definition quat.h:102
Type Im1() const
Get the first imaginary part of the quaternion.
Definition quat.h:84
friend Type Re(const quat &q)
Extract the real part of the quaternion.
Definition quat.h:78
quat(const quat &q)
Copy constructor.
Definition quat.h:47
quat operator-(const quat &other) const
Subtract two quaternions.
Definition quat.h:219
friend Type Im3(const quat &q)
Extract the third imaginary part of the quaternion.
Definition quat.h:114
quat & invert()
Invert the quaternion.
Definition quat.h:152
quat operator+(Type k) const
Sum a quaternion and a scalar value.
Definition quat.h:183
Type b
Imaginary parts.
Definition quat.h:30
static Vector rotate(const Vector &v, Type rad, const Vector &axis)
Rotate a 3D vector <v> by <rad> radians around the <axis> arbitrary axis.
Definition quat.h:336
quat & operator=(const quat &q)
Assignment operator.
Definition quat.h:51
quat operator+() const
Identity (for consistency)
Definition quat.h:171
quat inverse() const
Compute the inverse of the quaternion.
Definition quat.h:138
quat & operator*=(Type k)
Multiply this quaternion by a scalar value.
Definition quat.h:245
quat conjugate() const
Compute the conjugate of the quaternion.
Definition quat.h:120
quat & operator=(const std::array< T, 4 > &v)
Assignment operator from a 4D array.
Definition quat.h:62
quat operator+(const quat &other) const
Add two quaternions.
Definition quat.h:213
quat operator*(const quat &q) const
Multiply two quaternions.
Definition quat.h:225
quat(Type a, Type b, Type c, Type d)
Construct a quaternion from its components.
Definition quat.h:42
Type Re() const
Get the real part of the quaternion.
Definition quat.h:72
quat(Type r)
Construct a quaternion from a real number.
Definition quat.h:38
quat operator*(Type k) const
Multiply a quaternion by a scalar value.
Definition quat.h:195
quat operator/(const quat &other) const
Divide two quaternions.
Definition quat.h:239
std::string to_string() const
Convert the quaternion to string representation.
Definition quat.h:397
quat & operator/=(Type k)
Divide this quaternion by a scalar value.
Definition quat.h:251
Type Im3() const
Get the third imaginary part of the quaternion.
Definition quat.h:108
Type norm() const
Compute the norm of the quaternion.
Definition quat.h:132
quat & operator*=(const quat &other)
Multiply this quaternion by another one.
Definition quat.h:287
quat & operator/=(const quat &other)
Divide this quaternion by another one.
Definition quat.h:293
Type sqr_norm() const
Compute the square norm of the quaternion.
Definition quat.h:126
friend std::ostream & operator<<(std::ostream &out, const quat &obj)
Stream the quaternion in string representation to an output stream (std::ostream)
Definition quat.h:418
Type Im2() const
Get the second imaginary part of the quaternion.
Definition quat.h:96
friend Type Im1(const quat &q)
Extract the first imaginary part of the quaternion.
Definition quat.h:90
quat & operator-=(const quat &other)
Subtract a quaternion to this one.
Definition quat.h:277
Type a
Real part.
Definition quat.h:27
quat operator-(Type k) const
Subtract a quaternion and a scalar value.
Definition quat.h:189
Vector transform(const Vector &v) const
Transform a 3D vector.
Definition quat.h:300
#define TH_MATH_ERROR(F_NAME, VALUE, EXCEPTION)
TH_MATH_ERROR is a macro which throws exceptions or modifies errno (depending on which compiling opti...
Definition error.h:219
Vector & vec_error(Vector &v)
Overwrite the given vector with the error vector with NaN values.
Definition algebra.h:62
Vector normalize(const Vector &v)
Returns the normalized vector.
Definition algebra.h:302
Main namespace of the library which contains all functions and objects.
Definition algebra.h:27
dual2 sqrt(dual2 x)
Compute the square root of a second order dual number.
Definition dual2_functions.h:54
dual2 abs(dual2 x)
Compute the absolute value of a second order dual number.
Definition dual2_functions.h:198
std::remove_reference_t< decltype(std::declval< Structure >()[0])> vector_element_t
Extract the type of a vector (or any indexable container) from its operator[].
Definition core_traits.h:134
dual2 cos(dual2 x)
Compute the cosine of a second order dual number.
Definition dual2_functions.h:86
constexpr real MACH_EPSILON
Machine epsilon for the real type.
Definition constants.h:207
dual2 sin(dual2 x)
Compute the sine of a second order dual number.
Definition dual2_functions.h:72
real nan()
Return a quiet NaN number in floating point representation.
Definition error.h:54