cgv
quaternion.h
1 #pragma once
2 
3 #include "fvec.h"
4 #include "fmat.h"
5 #include "functions.h"
6 
7 namespace cgv {
8  namespace math {
9 
10 #define EPSILON 1e-6
11 
13 template <typename T>
14 class quaternion : public fvec<T,4>
15 {
16 public:
19  typedef fvec<T,4> base_type;
22  typedef T coord_type;
24  enum AxisEnum { X_AXIS, Y_AXIS, Z_AXIS };
26  typedef fvec<T,3> vec_type;
28  typedef fmat<T, 3, 3> mat_type;
30  typedef fmat<T, 4, 4> hmat_type;
32 
35  quaternion() : fvec<T,4>(T(0),T(0),T(0),T(1)) {}
38  quaternion(const quaternion<T>& quat) : fvec<T,4>(quat) {}
40  template <typename S>
41  quaternion(const quaternion<S>& q) : fvec<T,4>(q) {}
43  quaternion<T>& operator=(const quaternion<T>& quat) {
44  base_type::operator = (quat);
45  return *this;
46  }
48  quaternion(AxisEnum axis, coord_type angle) { set(axis, angle); }
50  quaternion(const vec_type& axis, coord_type angle) { set(axis, angle); }
52  quaternion(const mat_type& matrix) { set(matrix); }
54  quaternion(coord_type w,coord_type x, coord_type y,coord_type z) { set(w,x,y,z); }
56  quaternion(coord_type re, const vec_type& im) { set(re, im); }
57  base_type& vec() { return *this; }
58  const base_type& vec() const { return *this; }
60 
62  void set(AxisEnum axis, coord_type angle)
64  {
65  vec_type v(0,0,0);
66  v((int)axis) = 1;
67  set(v, angle);
68  }
70  void set(const vec_type& axis, coord_type angle)
71  {
72  angle *= (coord_type)0.5;
73  set(cos(angle), coord_type(sin(angle))*axis);
74  }
76  void set(const quaternion<T>& quat) { *this = quat; }
78  void set(const mat_type& M)
79  {
80  this->w() = sqrt(plus(T(0.25)*( M(0, 0) + M(1, 1) + M(2, 2) + T(1))));
81  this->x() = sqrt(plus(T(0.25)*( M(0, 0) - M(1, 1) - M(2, 2) + T(1))));
82  this->y() = sqrt(plus(T(0.25)*(-M(0, 0) + M(1, 1) - M(2, 2) + T(1))));
83  this->z() = sqrt(plus(T(0.25)*(-M(0, 0) - M(1, 1) + M(2, 2) + T(1))));
84  if (this->w() >= this->x() && this->w() >= this->y() && this->w() >= this->z()) {
85  this->x() *= sign(M(2, 1) - M(1, 2));
86  this->y() *= sign(M(0, 2) - M(2, 0));
87  this->z() *= sign(M(1, 0) - M(0, 1));
88  }
89  else if (this->x() >= this->y() && this->x() >= this->z()) {
90  this->w() *= sign(M(2, 1) - M(1, 2));
91  this->y() *= sign(M(0, 1) + M(1, 0));
92  this->z() *= sign(M(2, 0) + M(0, 2));
93  }
94  else if (this->y() >= this->z()) {
95  this->w() *= sign(M(0, 2) - M(2, 0));
96  this->x() *= sign(M(0, 1) + M(1, 0));
97  this->z() *= sign(M(1, 2) + M(2, 1));
98  }
99  else {
100  this->w() *= sign(M(1, 0) - M(0, 1));
101  this->x() *= sign(M(0, 2) + M(2, 0));
102  this->y() *= sign(M(1, 2) + M(2, 1));
103  }
104  }
106  void set(coord_type re, coord_type ix, coord_type iy, coord_type iz)
107  {
108  this->x() = ix;
109  this->y() = iy;
110  this->z() = iz;
111  this->w() = re;
112  }
114  void set(coord_type re, const vec_type& im)
115  {
116  set(re, im.x(), im.y(), im.z());
117  }
119 
121  void put_matrix(mat_type& M) const
123  {
124  M(0, 0) = 1 - 2 * this->y()*this->y() - 2 * this->z()*this->z();
125  M(0, 1) = 2 * this->x()*this->y() - 2 * this->w()*this->z();
126  M(0, 2) = 2 * this->x()*this->z() + 2 * this->w()*this->y();
127  M(1, 0) = 2 * this->x()*this->y() + 2 * this->w()*this->z();
128  M(1, 1) = 1 - 2 * this->x()*this->x() - 2 * this->z()*this->z();
129  M(1, 2) = 2 * this->y()*this->z() - 2 * this->w()*this->x();
130  M(2, 0) = 2 * this->x()*this->z() - 2 * this->w()*this->y();
131  M(2, 1) = 2 * this->y()*this->z() + 2 * this->w()*this->x();
132  M(2, 2) = 1-2*this->x()*this->x()-2*this->y()*this->y();
133  }
135  mat_type get_matrix() const { mat_type M; put_matrix(M); return M; }
137  void put_homogeneous_matrix(hmat_type& M) const
138  {
139  M(0, 0) = 1 - 2 * this->y()*this->y() - 2 * this->z()*this->z();
140  M(0, 1) = 2 * this->x()*this->y() - 2 * this->w()*this->z();
141  M(0, 2) = 2 * this->x()*this->z() + 2 * this->w()*this->y();
142  M(1, 0) = 2 * this->x()*this->y() + 2 * this->w()*this->z();
143  M(1, 1) = 1 - 2 * this->x()*this->x() - 2 * this->z()*this->z();
144  M(1, 2) = 2 * this->y()*this->z() - 2 * this->w()*this->x();
145  M(2, 0) = 2 * this->x()*this->z() - 2 * this->w()*this->y();
146  M(2, 1) = 2 * this->y()*this->z() + 2 * this->w()*this->x();
147  M(2, 2) = 1 - 2 * this->x()*this->x() - 2 * this->y()*this->y();
148  M(3,0) = M(3,1) = M(3,2) = M(0,3) = M(1, 3) = M(2, 3) = M(3, 3) = 0;
149  M(3,3) = 1;
150  }
152  hmat_type get_homogeneous_matrix() const { hmat_type M; put_homogeneous_matrix(M); return M; }
154  void set_normal(const vec_type& n)
155  {
156  coord_type cosfac = 1-n.x();
157  if (fabs(cosfac)<EPSILON)
158  set(1,0,0,0);
159  else {
160  cosfac = sqrt(cosfac);
161  coord_type sinfac = sqrt(n.y()*n.y() + n.z()*n.z());
162  static coord_type fac = (coord_type)(1/sqrt(2.0));
163  coord_type tmp = fac*cosfac/sinfac;
164  set(fac*sinfac/cosfac, 0, -n.z()*tmp, n.y()*tmp);
165  }
166  }
168  void put_normal(coord_type* n)
169  {
170  n[0] = 1-2*(this->y()*this->y()+ this->z()*this->z());
171  n[1] = 2*(this->w()*this->z() + this->x()*this->y());
172  n[2] = 2*(this->x()*this->z() - this->w()*this->y());
173  }
175  void put_image(const vec_type& preimage, vec_type& image) const
176  {
177  image = cross(im(), preimage);
178  image = dot(preimage,im())*im() + re()*(re()*preimage + (coord_type)2*image) + cross(im(),image);
179  }
181  void rotate(vec_type& v) const { vec_type tmp; put_image(v, tmp); v = tmp; }
183  vec_type get_rotated(const vec_type& v) const { vec_type tmp; put_image(v, tmp); return tmp; }
185  void rotate(mat_type& m) const
186  {
187  m.set_col(0, get_rotated(m.col(0)));
188  m.set_col(1, get_rotated(m.col(1)));
189  m.set_col(2, get_rotated(m.col(2)));
190  }
192  mat_type get_rotated(const mat_type& M) const { mat_type tmp = M; rotate(tmp); return tmp; }
194  void put_preimage(const vec_type& image, vec_type& preimage) const
195  {
196  preimage = cross(-im(), image);
197  preimage = dot(image,-im())*(-im()) + re()*(re()*image + (coord_type)2*preimage) + cross(-im(),preimage);
198  }
200  void inverse_rotate(vec_type& image) const { vec_type tmp; put_preimage(image, tmp); image = tmp; }
202  vec_type apply(const vec_type& v) const { vec_type tmp; put_image(v, tmp); return tmp; }
204 
206  quaternion<T> conj() const { return quaternion<T>(re(), -im()); }
209  quaternion<T> negated() const { return quaternion<T>(-re(), -im()); }
211  void negate() { conjugate(); re() = -re(); }
213  void conjugate() { this->x() = -this->x(); this->y() = -this->y(); this->z() = -this->z(); }
215  quaternion<T> inverse() const { quaternion<T> tmp(*this); tmp.invert(); return tmp; }
217  void invert()
218  {
219  conjugate();
220  coord_type sn = this->sqr_length();
221  if (sn < EPSILON*EPSILON)
222  base_type::operator *= ((coord_type) 1e14);
223  else
224  base_type::operator /= (sn);
225  }
227  void affin(const quaternion<T>& p, coord_type t, const quaternion<T>& q)
228  {
229  coord_type omega, cosom, sinom, sclp, sclq;
230  *this = q*p;
231  cosom = this->sqr_length();
232  if ( ( 1 + cosom) > EPSILON ) {
233  if ( ( 1 - cosom) > EPSILON ) {
234  omega = acos( cosom );
235  sinom = sin ( omega );
236  sclp = sin ( (1-t) * omega ) / sinom;
237  sclq = sin ( t*omega ) / sinom;
238  }
239  else
240  {
241  sclp = 1 - t;
242  sclq = t;
243  }
244  set(sclp*p.w() + sclq*q.w(), sclp*p.x() + sclq*q.x(), sclp*p.y() + sclq*q.y(), sclp*p.z() + sclq*q.z());
245  }
246  else {
247  sclp = (T)sin ( (1-t)*M_PI/2 );
248  sclq = (T)sin ( t * M_PI/2 );
249  set(p.z(), sclp*p.x() - sclq*p.y(), sclp*p.y() + sclq*p.x(), sclp*p.z() - sclq*p.w());
250  }
251  }
253  void affin(coord_type s, const quaternion<T>& q) { quaternion<T> tmp; tmp.affin(*this, s, q); *this = tmp; }
255  quaternion<T> operator - () const { return negated(); }
257  quaternion<T>& operator*=(const quaternion<T>& q)
258  {
259  // Fastmul-Alg. siehe Seidel-Paper p.4
260  coord_type s[9], t;
261  s[0] = (this->z()-this->y())*(q.y()-q.z());
262  s[1] = (this->w()+this->x())*(q.w()+q.x());
263  s[2] = (this->w()-this->x())*(q.y()+q.z());
264  s[3] = (this->z()+this->y())*(q.w()-q.x());
265  s[4] = (this->z()-this->x())*(q.x()-q.y());
266  s[5] = (this->z()+this->x())*(q.x()+q.y());
267  s[6] = (this->w()+this->y())*(q.w()-q.z());
268  s[7] = (this->w()-this->y())*(q.w()+q.z());
269  s[8] = s[5]+s[6]+s[7];
270  t = (s[4] +s[8])/2;
271  set(s[0]+t-s[5], s[1]+t-s[8], s[2]+t-s[7], s[3]+t-s[6]);
272  return *this;
273  }
275  quaternion<T> operator * (const quaternion<T>& q) const { quaternion<T> tmp(*this); tmp*=q; return tmp; }
277  quaternion<T>& operator *= (const T& s) { for (unsigned i=0;i<4;++i) this->v[i] *= s; return *this; }
279  quaternion<T> operator * (const T& s) const { quaternion<T> r = *this; r *= s; return r; }
281 
283  coord_type re() const { return this->w(); }
286  coord_type& re() { return this->w(); }
288  void put_im(vec_type& vector) const { vector.x() = this->x(); vector.y() = this->y(); vector.z() = this->z(); }
290  vec_type im() const { return vec_type(this->x(),this->y(),this->z()); }
292  coord_type put_axis(vec_type& v) const
293  {
294  if (re() > 1) {
295  quaternion<T> q(*this);
296  q.normalize();
297  return q.put_axis(v);
298  }
299  coord_type angle = 2 * acos(re());
300  coord_type s = sqrt(1 - re()*re());
301  if (s < (coord_type)EPSILON) {
302  v = vec_type(0,0,1);
303  return 0;
304  }
305  v = im()/s;
306 /* if (2*angle > M_PI) {
307  angle -= (coord_type)(2*M_PI);
308  v = -v;
309  }*/
310  return angle;
311  }
313  quaternion<T> exp() const
314  {
315  T m = im().length();
316  quaternion<T> quat(cos(m), im()*(sin(m) / m));
317  (typename quaternion<T>::base_type&) quat *= exp(re());
318  return quat;
319  }
321  quaternion<T> log() const
322  {
323  T R = this->length();
324  typename quaternion<T>::vec_type v(im());
325  T sinTimesR = v.length();
326  if (sinTimesR < EPSILON)
327  v /= sinTimesR;
328  else {
329  v = typename quaternion<T>::vec_type(0, 0, 0);
330  sinTimesR = 0;
331  }
332  return quaternion<T>(::log(R), atan2(sinTimesR, re()*v));
333  }
335 };
336 
338 template <typename T>
339 quaternion<T> operator * (const T& s, const quaternion<T>& v)
340 {
341  quaternion<T> r = v; r *= s; return r;
342 }
343 
344 
345  }
346 }
cgv::math::quaternion::negated
quaternion< T > negated() const
return the negated quaternion
Definition: quaternion.h:209
cgv::math::quaternion::operator=
quaternion< T > & operator=(const quaternion< T > &quat)
assignement operator
Definition: quaternion.h:43
cgv::math::quaternion< float >::AxisEnum
AxisEnum
enumeration of the three coordinate axes
Definition: quaternion.h:24
cgv::math::fvec< T, 4 >::normalize
double normalize()
normalize the vector using the L2-Norm and return the length
Definition: fvec.h:260
cgv::math::fmat
matrix of fixed size dimensions
Definition: fmat.h:23
cgv::math::quaternion::quaternion
quaternion(coord_type w, coord_type x, coord_type y, coord_type z)
construct quaternion directly
Definition: quaternion.h:54
cgv::math::quaternion::set
void set(const mat_type &M)
initialize quaternion from 3x3 rotation matrix
Definition: quaternion.h:78
cgv::math::sign
T sign(const T &a, const T &b)
returns the abs(a)*sign(b)
Definition: functions.h:15
cgv::math::quaternion::inverse_rotate
void inverse_rotate(vec_type &image) const
rotate vector according to the inverse quaternion
Definition: quaternion.h:200
cgv::math::quaternion::negate
void negate()
negate the quaternion
Definition: quaternion.h:211
cgv::math::quaternion::quaternion
quaternion(const mat_type &matrix)
construct quaternion from 3x3 rotation matrix
Definition: quaternion.h:52
cgv::math::quaternion::quaternion
quaternion(AxisEnum axis, coord_type angle)
construct quaternion from coordinate axis and rotation angle
Definition: quaternion.h:48
cgv::math::fvec::length
T length() const
length of the vector L2-Norm
Definition: fvec.h:219
cgv::math::quaternion::set
void set(const vec_type &axis, coord_type angle)
initialize quaternion from axis and rotation angle
Definition: quaternion.h:70
cgv::math::fvec< T, 4 >::y
T & y()
second element
Definition: fvec.h:133
cgv::math::quaternion::get_rotated
mat_type get_rotated(const mat_type &M) const
Rotate source frame s into destination frame d.
Definition: quaternion.h:192
cgv::math::cross
fvec< T, N > cross(const fvec< T, N > &v, const fvec< T, N > &w)
returns the cross product of vector v and w
Definition: fvec.h:338
cgv::math::quaternion::get_rotated
vec_type get_rotated(const vec_type &v) const
return rotated vector
Definition: quaternion.h:183
cgv::math::quaternion::put_image
void put_image(const vec_type &preimage, vec_type &image) const
rotate preimage according to quaternion into image
Definition: quaternion.h:175
cgv::math::quaternion::put_im
void put_im(vec_type &vector) const
put imaginary part
Definition: quaternion.h:288
cgv::math::quaternion::operator*
quaternion< T > operator*(const quaternion< T > &q) const
field multiplication
Definition: quaternion.h:275
cgv::math::fvec
Definition: fvec.h:18
cgv::math::quaternion::apply
vec_type apply(const vec_type &v) const
rotate vector according to quaternion
Definition: quaternion.h:202
cgv::math::quaternion
Definition: quaternion.h:15
cgv::math::quaternion::conjugate
void conjugate()
compute conjugate
Definition: quaternion.h:213
cgv::math::vec
A column vector class.
Definition: fvec.h:13
cgv::math::quaternion::put_preimage
void put_preimage(const vec_type &image, vec_type &preimage) const
rotate image according to quaternion into preimage
Definition: quaternion.h:194
cgv::math::fvec< T, 4 >::w
T & w()
fourth element
Definition: fvec.h:141
cgv::math::quaternion::im
vec_type im() const
return this as vector
Definition: quaternion.h:290
cgv::math::fvec< T, 4 >::operator*=
fvec< T, N > & operator*=(const T &s)
in place multiplication with s
Definition: fvec.h:163
cgv::math::plus
T plus(const T &v)
if v >= 0 returns v or 0 otherwise
Definition: functions.h:21
cgv::math::quaternion::put_axis
coord_type put_axis(vec_type &v) const
put rotation axis and return rotation angle
Definition: quaternion.h:292
cgv::math::quaternion::operator*=
quaternion< T > & operator*=(const quaternion< T > &q)
field multiplication
Definition: quaternion.h:257
cgv::math::quaternion::put_homogeneous_matrix
void put_homogeneous_matrix(hmat_type &M) const
compute equivalent homogeneous 4x4 rotation matrix
Definition: quaternion.h:137
cgv::math::quaternion::inverse
quaternion< T > inverse() const
return the inverse
Definition: quaternion.h:215
cgv::math::quaternion::coord_type
T coord_type
coordinate type
Definition: quaternion.h:22
cgv::math::quaternion::exp
quaternion< T > exp() const
exponential map
Definition: quaternion.h:313
cgv::math::fvec< T, 4 >::sqr_length
T sqr_length() const
square length of vector
Definition: fvec.h:252
cgv::math::quaternion::hmat_type
fmat< T, 4, 4 > hmat_type
type of 4x4 matrix
Definition: quaternion.h:30
cgv::math::fvec< T, 4 >::x
T & x()
first element
Definition: fvec.h:129
cgv::math::quaternion::re
coord_type & re()
return reference to real part
Definition: quaternion.h:286
cgv::math::quaternion::get_homogeneous_matrix
hmat_type get_homogeneous_matrix() const
return equivalent 4x4 rotation matrix
Definition: quaternion.h:152
cgv::math::quaternion::set
void set(coord_type re, const vec_type &im)
initialize quaternion from real part and vector
Definition: quaternion.h:114
cgv::math::quaternion::quaternion
quaternion(const quaternion< S > &q)
copy constructor with type conversion
Definition: quaternion.h:41
cgv::math::quaternion::vec_type
fvec< T, 3 > vec_type
type of 3d axis
Definition: quaternion.h:26
cgv::math::quaternion::operator-
quaternion< T > operator-() const
negation operator
Definition: quaternion.h:255
cgv::math::quaternion::quaternion
quaternion(const quaternion< T > &quat)
copy constructor
Definition: quaternion.h:38
cgv::math::quaternion::set
void set(coord_type re, coord_type ix, coord_type iy, coord_type iz)
initialize quaternion directly
Definition: quaternion.h:106
cgv::math::fvec< T, 4 >::operator=
fvec & operator=(const fvec< T, N > &rhs)
assign vector rhs, if vector and rhs have different sizes, vector has been resized to match the size ...
Definition: fvec.h:103
cgv::math::dot
T dot(const fvec< T, N > &v, const fvec< T, N > &w)
returns the dot product of vector v and w
Definition: fvec.h:300
cgv::math::quaternion::base_type
fvec< T, 4 > base_type
base class type
Definition: quaternion.h:20
cgv::math::quaternion::rotate
void rotate(mat_type &m) const
Rotate a frame according to quaternion.
Definition: quaternion.h:185
cgv::math::quaternion::quaternion
quaternion()
standard constructor initializes to unit quaternion
Definition: quaternion.h:36
cgv::math::quaternion::set
void set(const quaternion< T > &quat)
setter from quaternion
Definition: quaternion.h:76
cgv::math::quaternion::invert
void invert()
compute inverse
Definition: quaternion.h:217
cgv::math::fvec< T, 4 >::z
T & z()
third element
Definition: fvec.h:137
cgv::math::quaternion::log
quaternion< T > log() const
logarithmic map
Definition: quaternion.h:321
cgv::math::quaternion::put_normal
void put_normal(coord_type *n)
extract normal vector
Definition: quaternion.h:168
cgv::math::quaternion::put_matrix
void put_matrix(mat_type &M) const
compute equivalent 3x3 rotation matrix
Definition: quaternion.h:122
cgv::math::quaternion::set
void set(AxisEnum axis, coord_type angle)
initialize quaternion from coordinate axis and rotation angle
Definition: quaternion.h:63
cgv::math::quaternion::get_matrix
mat_type get_matrix() const
return equivalent 3x3 rotation matrix
Definition: quaternion.h:135
cgv::math::quaternion::quaternion
quaternion(const vec_type &axis, coord_type angle)
construct quaternion from axis and rotation angle
Definition: quaternion.h:50
cgv::math::quaternion::mat_type
fmat< T, 3, 3 > mat_type
type of 3x3 matrix
Definition: quaternion.h:28
cgv::math::quaternion::re
coord_type re() const
return real part
Definition: quaternion.h:284
cgv::math::quaternion::quaternion
quaternion(coord_type re, const vec_type &im)
construct quaternion from real part and vector
Definition: quaternion.h:56
cgv::math::quaternion::affin
void affin(const quaternion< T > &p, coord_type t, const quaternion< T > &q)
compute affin combination with angular interpolation
Definition: quaternion.h:227
cgv::math::quaternion::rotate
void rotate(vec_type &v) const
rotate vector according to quaternion
Definition: quaternion.h:181
cgv::math::quaternion::conj
quaternion< T > conj() const
return the conjugate
Definition: quaternion.h:207
cgv
the cgv namespace
Definition: vr_calib.cxx:9
cgv::math::quaternion::set_normal
void set_normal(const vec_type &n)
initialize quaternion from normal vector
Definition: quaternion.h:154
cgv::math::quaternion::affin
void affin(coord_type s, const quaternion< T > &q)
compute affin combination with angular interpolation
Definition: quaternion.h:253
cgv::math::fvec< T, 4 >::operator/=
fvec< T, N > & operator/=(const T &s)
in place division by scalar s
Definition: fvec.h:165