MVE - Multi-View Environment mve-devel
Loading...
Searching...
No Matches
quaternion.h
Go to the documentation of this file.
1/*
2 * Copyright (C) 2015, Simon Fuhrmann
3 * TU Darmstadt - Graphics, Capture and Massively Parallel Computing
4 * All rights reserved.
5 *
6 * This software may be modified and distributed under the terms
7 * of the BSD 3-Clause license. See the LICENSE.txt file for details.
8 */
9
10#ifndef MATH_QUATERNION_HEADER
11#define MATH_QUATERNION_HEADER
12
13#include "math/defines.h"
14#include "math/vector.h"
15
17
18template <typename T> class Quaternion;
25
29template <class T>
30class Quaternion : public Vector<T,4>
31{
32public:
34 Quaternion (void);
35
37 Quaternion (T const& value);
38
40 Quaternion (T const& v1, T const& v2, T const& v3, T const& v4);
41
43 Quaternion (Vector<T,3> const& axis, T const& angle);
44
46 void set (Vector<T,3> const& axis, T const& angle);
47
49 void set_from_rotation_matrix (T const* matrix);
50
52 void get_axis_angle (T* axis, T& angle);
53
55 void to_rotation_matrix (T* matrix) const;
56
57 /* ---------------------- Unary operators --------------------- */
58
60 Quaternion<T>& conjugate (void);
61
63 Quaternion<T> conjugated (void) const;
64
65 /* --------------------- Binary operators --------------------- */
66
68 Vector<T,3> rotate (Vector<T,3> const& v) const;
69
70 /* Binary operators. */
71 //Quaternion<T> plus (T const& f) const
72 //{ return Quaternion<T>(r + f, x, y, z); }
73 //Quaternion<T> minus (T const& f) const
74 //{ return Quaternion<T>(r - f, x, y, z); }
75
76 /* --------------------- Object operators --------------------- */
77
79 Quaternion<T> operator* (Quaternion<T> const& rhs) const;
80
82 Quaternion<T>& operator*= (Quaternion<T> const& rhs);
83};
84
86
87/* ------------------------ Implementation ------------------------ */
88
89#include <cmath>
90#include <algorithm>
91#include <stdexcept>
92#include <numeric>
93
95
96template <typename T>
97inline
101
102template <typename T>
103inline
105 : Vector<T,4>(value)
106{
107}
108
109template <typename T>
110inline
111Quaternion<T>::Quaternion (T const& v1, T const& v2, T const& v3, T const& v4)
112{
113 this->v[0] = v1;
114 this->v[1] = v2;
115 this->v[2] = v3;
116 this->v[3] = v4;
117}
118
119template <typename T>
120inline
121Quaternion<T>::Quaternion (Vector<T,3> const& axis, T const& angle)
122{
123 this->set(axis, angle);
124}
125
126template <typename T>
127inline void
128Quaternion<T>::set (Vector<T,3> const& axis, T const& angle)
129{
130 this->v[0] = std::cos(angle / T(2));
131 T sa = std::sin(angle / T(2));
132 this->v[1] = axis[0] * sa;
133 this->v[2] = axis[1] * sa;
134 this->v[3] = axis[2] * sa;
135}
136
137template <typename T>
138inline void
140{
141 float trace = rot[0] + rot[4] + rot[8];
142 if (trace > 0)
143 {
144 float s = T(0.5) / std::sqrt(trace + T(1.0));
145 // w x y z
146 this->v[0] = T(0.25) / s;
147 this->v[1] = (rot[7] - rot[5]) * s;
148 this->v[2] = (rot[2] - rot[6]) * s;
149 this->v[3] = (rot[3] - rot[1]) * s;
150 }
151 else
152 {
153 if (rot[0] > rot[4] && rot[0] > rot[8])
154 {
155 float s = T(2.0) * std::sqrt(T(1.0) + rot[0] - rot[4] - rot[8]);
156 this->v[0] = (rot[7] - rot[5]) / s;
157 this->v[1] = T(0.25) * s;
158 this->v[2] = (rot[1] + rot[3]) / s;
159 this->v[3] = (rot[2] + rot[6]) / s;
160 }
161 else if (rot[4] > rot[8])
162 {
163 float s = T(2.0) * std::sqrt(T(1.0) + rot[4] - rot[0] - rot[8]);
164 this->v[0] = (rot[2] - rot[6]) / s;
165 this->v[1] = (rot[1] + rot[3]) / s;
166 this->v[2] = T(0.25) * s;
167 this->v[3] = (rot[5] + rot[7]) / s;
168 }
169 else
170 {
171 float s = T(2.0) * std::sqrt(T(1.0) + rot[8] - rot[0] - rot[4]);
172 this->v[0] = (rot[3] - rot[1]) / s;
173 this->v[1] = (rot[2] + rot[6]) / s;
174 this->v[2] = (rot[5] + rot[7]) / s;
175 this->v[3] = T(0.25) * s;
176 }
177 }
178}
179
180template <typename T>
181void
183{
184 const T len = std::sqrt(MATH_POW2(this->v[1])
185 + MATH_POW2(this->v[2]) + MATH_POW2(this->v[3]));
186 if (len == T(0))
187 {
188 axis[0] = T(1);
189 axis[1] = T(0);
190 axis[2] = T(0);
191 angle = T(0);
192 return;
193 }
194
195 axis[0] = this->v[1] / len;
196 axis[1] = this->v[2] / len;
197 axis[2] = this->v[3] / len;
198 angle = T(2) * std::acos(this->v[0]);
199}
200
201template <typename T>
202void
204{
205 T const xxzz = this->v[1] * this->v[1] - this->v[3] * this->v[3];
206 T const rryy = this->v[0] * this->v[0] - this->v[2] * this->v[2];
207 T const yyrrxxzz = this->v[2] * this->v[2] + this->v[0] * this->v[0]
208 - this->v[1] * this->v[1] - this->v[3] * this->v[3];
209
210 T const xr2 = this->v[1] * this->v[0] * T(2);
211 T const xy2 = this->v[1] * this->v[2] * T(2);
212 T const xz2 = this->v[1] * this->v[3] * T(2);
213 T const yr2 = this->v[2] * this->v[0] * T(2);
214 T const yz2 = this->v[2] * this->v[3] * T(2);
215 T const zr2 = this->v[3] * this->v[0] * T(2);
216
217 matrix[0] = xxzz + rryy;
218 matrix[1] = xy2 - zr2;
219 matrix[2] = xz2 + yr2;
220 matrix[3] = xy2 + zr2;
221 matrix[4] = yyrrxxzz;
222 matrix[5] = yz2 - xr2;
223 matrix[6] = xz2 - yr2;
224 matrix[7] = yz2 + xr2;
225 matrix[8] = rryy - xxzz;
226}
227
228template <typename T>
231{
232 T rot[9];
233 this->to_rotation_matrix(rot);
234
235 Vector<T,3> ret;
236 ret[0] = rot[0] * vec[0] + rot[1] * vec[1] + rot[2] * vec[2];
237 ret[1] = rot[3] * vec[0] + rot[4] * vec[1] + rot[5] * vec[2];
238 ret[2] = rot[6] * vec[0] + rot[7] * vec[1] + rot[8] * vec[2];
239 return ret;
240}
241
242template <typename T>
243inline Quaternion<T>
245{
246 return Quaternion<T>(
247 this->v[0] * rhs.v[0] - this->v[1] * rhs.v[1]
248 - this->v[2] * rhs.v[2] - this->v[3] * rhs.v[3],
249 this->v[0] * rhs.v[1] + this->v[1] * rhs.v[0]
250 + this->v[2] * rhs.v[3] - this->v[3] * rhs.v[2],
251 this->v[0] * rhs.v[2] - this->v[1] * rhs.v[3]
252 + this->v[2] * rhs.v[0] + this->v[3] * rhs.v[1],
253 this->v[0] * rhs.v[3] + this->v[1] * rhs.v[2]
254 - this->v[2] * rhs.v[1] + this->v[3] * rhs.v[0]);
255}
256
257template <typename T>
258inline Quaternion<T>&
260{
261 *this = *this * rhs;
262 return *this;
263}
264
265template <typename T>
266inline Quaternion<T>&
268{
269 this->v[1] = -this->v[1];
270 this->v[2] = -this->v[2];
271 this->v[3] = -this->v[3];
272 return *this;
273}
274
275template <typename T>
276inline Quaternion<T>
278{
279 return Quaternion<T>(*this).conjugate();
280}
281
283
284/* --------------------- Ouput stream adapter --------------------- */
285
286#include <ostream>
287
289
291template <typename T, int N>
292inline std::ostream&
293operator<< (std::ostream& os, Quaternion<T> const& v)
294{
295 for (int i = 0; i < 4; ++i)
296 os << v[i] << ( i < 3 ? " " : "");
297 return os;
298}
299
301
302#endif /* MATH_QUATERNION_HEADER */
Quaternion class for arbitrary types (WORK IN PROGRESS).
Definition quaternion.h:31
Quaternion< T > & operator*=(Quaternion< T > const &rhs)
Quaternion self-multiplication.
Definition quaternion.h:259
Quaternion< T > & conjugate(void)
Conjugates self and returns reference to self.
Definition quaternion.h:267
Quaternion(void)
Creates a new, uninitialized quaternion.
Definition quaternion.h:98
Quaternion< T > conjugated(void) const
Returns a conjugated copy of self.
Definition quaternion.h:277
void to_rotation_matrix(T *matrix) const
Conversion to a 3x3 rotation matrix.
Definition quaternion.h:203
void set_from_rotation_matrix(T const *matrix)
Sets the quaternion from an orthonormal rotation matrix.
Definition quaternion.h:139
void set(Vector< T, 3 > const &axis, T const &angle)
Sets the quaternion from axis and angle.
Definition quaternion.h:128
Vector< T, 3 > rotate(Vector< T, 3 > const &v) const
Rotates a vector using the quaternion.
Definition quaternion.h:230
void get_axis_angle(T *axis, T &angle)
Provides the axis and angle of the quaternion.
Definition quaternion.h:182
Vector class for arbitrary dimensions and types.
Definition vector.h:87
T * operator*(void)
Dereference operator to value array.
Definition vector.h:613
#define MATH_NAMESPACE_BEGIN
Definition defines.h:15
#define MATH_NAMESPACE_END
Definition defines.h:16
#define MATH_POW2(x)
Definition defines.h:68
std::ostream & operator<<(std::ostream &os, Matrix< T, N, M > const &m)
Serializing a vector to an output stream.
Definition matrix.h:729