g4tools  5.4.0
qrot
Go to the documentation of this file.
1 // Copyright (C) 2010, Guy Barrand. All rights reserved.
2 // See the file tools.license for terms.
3 
4 #ifndef tools_qrot
5 #define tools_qrot
6 
7 // rotation done with quaternion.
8 
9 namespace tools {
10 
11 template <class VEC3,class VEC4>
12 class qrot {
13 protected:
14 //typedef typename VEC3::elem_t T3;
15  typedef typename VEC4::elem_t T; //we assume = T3
16 public:
17  qrot()
18  :m_quat(0,0,0,1) //zero rotation around the positive Z axis.
19  {}
20  qrot(const VEC3& a_axis,T a_radians,T(*a_sin)(T),T(*a_cos)(T)){
21  if(!set_value(a_axis,a_radians,a_sin,a_cos)) {} //FIXME : throw
22  }
23  qrot(const VEC3& a_from,const VEC3& a_to,T(*a_sqrt)(T),T(*a_fabs)(T)){set_value(a_from,a_to,a_sqrt,a_fabs);}
24  virtual ~qrot(){}
25 public:
26  qrot(const qrot& a_from)
27  :m_quat(a_from.m_quat)
28  {}
29  qrot& operator=(const qrot& a_from){
30  m_quat = a_from.m_quat;
31  return *this;
32  }
33 protected:
34  qrot(T a_q0,T a_q1,T a_q2,T a_q3)
35  :m_quat(a_q0,a_q1,a_q2,a_q3)
36  {
37  if(!m_quat.normalize()) {} //FIXME throw
38  }
39 
40 public:
41  qrot& operator*=(const qrot& a_q) {
42  //Multiplies the quaternions.
43  //Note that order is important when combining quaternions with the
44  //multiplication operator.
45  // Formula from <http://www.lboro.ac.uk/departments/ma/gallery/quat/>
46 
47  T tx = m_quat.v0();
48  T ty = m_quat.v1();
49  T tz = m_quat.v2();
50  T tw = m_quat.v3();
51 
52  T qx = a_q.m_quat.v0();
53  T qy = a_q.m_quat.v1();
54  T qz = a_q.m_quat.v2();
55  T qw = a_q.m_quat.v3();
56 
57  m_quat.set_value(qw*tx + qx*tw + qy*tz - qz*ty,
58  qw*ty - qx*tz + qy*tw + qz*tx,
59  qw*tz + qx*ty - qy*tx + qz*tw,
60  qw*tw - qx*tx - qy*ty - qz*tz);
61  m_quat.normalize();
62  return *this;
63  }
64 
65  bool operator==(const qrot& a_r) const {
66  return m_quat.equal(a_r.m_quat);
67  }
68  bool operator!=(const qrot& a_r) const {
69  return !operator==(a_r);
70  }
71  qrot operator*(const qrot& a_r) const {
72  qrot tmp(*this);
73  tmp *= a_r;
74  return tmp;
75  }
76 
77  bool invert(){
78  T length = m_quat.length();
79  if(length==T()) return false;
80 
81  // Optimize by doing 1 div and 4 muls instead of 4 divs.
82  T inv = one() / length;
83 
84  m_quat.set_value(-m_quat.v0() * inv,
85  -m_quat.v1() * inv,
86  -m_quat.v2() * inv,
87  m_quat.v3() * inv);
88 
89  return true;
90  }
91 
92  bool inverse(qrot& a_r) const {
93  //Non-destructively inverses the rotation and returns the result.
94  T length = m_quat.length();
95  if(length==T()) return false;
96 
97  // Optimize by doing 1 div and 4 muls instead of 4 divs.
98  T inv = one() / length;
99 
100  a_r.m_quat.set_value(-m_quat.v0() * inv,
101  -m_quat.v1() * inv,
102  -m_quat.v2() * inv,
103  m_quat.v3() * inv);
104 
105  return true;
106  }
107 
108 
109  bool set_value(const VEC3& a_axis,T a_radians,T(*a_sin)(T),T(*a_cos)(T)) {
110  // Reset rotation with the given axis-of-rotation and rotation angle.
111  // Make sure axis is not the null vector when calling this method.
112  // From <http://www.automation.hut.fi/~jaro/thesis/hyper/node9.html>.
113  if(a_axis.length()==T()) return false;
114  m_quat.v3(a_cos(a_radians/2));
115  T sineval = a_sin(a_radians/2);
116  VEC3 a = a_axis;
117  a.normalize();
118  m_quat.v0(a.v0() * sineval);
119  m_quat.v1(a.v1() * sineval);
120  m_quat.v2(a.v2() * sineval);
121  return true;
122  }
123 
124  bool set_value(const VEC3& a_from,const VEC3& a_to,T(*a_sqrt)(T),T(*a_fabs)(T)) {
125  // code taken from coin3d/SbRotation.
126  //NOTE : coin3d/SbMatrix logic is transposed relative to us.
127 
128  VEC3 from(a_from);
129  if(from.normalize()==T()) return false;
130  VEC3 to(a_to);
131  if(to.normalize()==T()) return false;
132 
133  T dot = from.dot(to);
134  VEC3 crossvec;from.cross(to,crossvec);
135  T crosslen = crossvec.normalize();
136 
137  if(crosslen == T()) { // Parallel vectors
138  // Check if they are pointing in the same direction.
139  if (dot > T()) {
140  m_quat.set_value(0,0,0,1);
141  } else {
142  // Ok, so they are parallel and pointing in the opposite direction
143  // of each other.
144  // Try crossing with x axis.
145  VEC3 t;from.cross(VEC3(1,0,0),t);
146  // If not ok, cross with y axis.
147  if(t.normalize() == T()) {
148  from.cross(VEC3(0,1,0),t);
149  t.normalize();
150  }
151  m_quat.set_value(t[0],t[1],t[2],0);
152  }
153  } else { // Vectors are not parallel
154  // The fabs() wrapping is to avoid problems when `dot' "overflows"
155  // a tiny wee bit, which can lead to sqrt() returning NaN.
156  crossvec *= (T)a_sqrt(half() * a_fabs(one() - dot));
157  // The fabs() wrapping is to avoid problems when `dot' "underflows"
158  // a tiny wee bit, which can lead to sqrt() returning NaN.
159  m_quat.set_value(crossvec[0], crossvec[1], crossvec[2],(T)a_sqrt(half()*a_fabs(one()+dot)));
160  }
161 
162  return true;
163  }
164 
165 
166  bool value(VEC3& a_axis,T& a_radians,T(*a_sin)(T),T(*a_acos)(T)) const { //WARNING a_acos and NOT a_cos
167  //WARNING : can fail.
168  if( (m_quat.v3()<minus_one()) || (m_quat.v3()> one()) ) {
169  a_axis.set_value(0,0,1);
170  a_radians = 0;
171  return false;
172  }
173 
174  a_radians = a_acos(m_quat.v3()) * 2; //in [0,2*pi]
175  T sineval = a_sin(a_radians/2);
176 
177  if(sineval==T()) { //a_radian = 0 or 2*pi.
178  a_axis.set_value(0,0,1);
179  a_radians = 0;
180  return false;
181  }
182  a_axis.set_value(m_quat.v0()/sineval,
183  m_quat.v1()/sineval,
184  m_quat.v2()/sineval);
185  return true;
186  }
187 
188  template <class MAT4>
189  void set_value(const MAT4& a_m,T(*a_sqrt)(T)) {
190  // See tests/qrot.icc.
191  // code taken from coin3d/SbRotation.
192 
193  //Set the rotation from the components of the given matrix.
194 
195  T scalerow = a_m.v00() + a_m.v11() + a_m.v22();
196  if (scalerow > T()) {
197  T _s = a_sqrt(scalerow + a_m.v33());
198  m_quat.v3(_s * half());
199  _s = half() / _s;
200 
201  m_quat.v0((a_m.v21() - a_m.v12()) * _s);
202  m_quat.v1((a_m.v02() - a_m.v20()) * _s);
203  m_quat.v2((a_m.v10() - a_m.v01()) * _s);
204  } else {
205  unsigned int i = 0;
206  if (a_m.v11() > a_m.v00()) i = 1;
207  if (a_m.v22() > a_m.value(i,i)) i = 2;
208 
209  unsigned int j = (i+1)%3;
210  unsigned int k = (j+1)%3;
211 
212  T _s = a_sqrt((a_m.value(i,i) - (a_m.value(j,j) + a_m.value(k,k))) + a_m.v33());
213 
214  m_quat.set_value(i,_s * half());
215  _s = half() / _s;
216 
217  m_quat.v3((a_m.value(k,j) - a_m.value(j,k)) * _s);
218  m_quat.set_value(j,(a_m.value(j,i) + a_m.value(i,j)) * _s);
219  m_quat.set_value(k,(a_m.value(k,i) + a_m.value(i,k)) * _s);
220  }
221 
222  if (a_m.v33()!=one()) {
223  m_quat.multiply(one()/a_sqrt(a_m.v33()));
224  }
225  }
226 
227  template <class MAT4>
228  void value(MAT4& a_m) const {
229  //Return this rotation in the form of a matrix.
230  //NOTE : in coin3d/SbRotation, it looks as if "w <-> -w", but coin3d/SbMatrix logic is transposed relative to us.
231 
232  const T x = m_quat.v0();
233  const T y = m_quat.v1();
234  const T z = m_quat.v2();
235  const T w = m_quat.v3();
236  // q = w + x * i + y * j + z * k
237 
238  // first row :
239  a_m.v00(w*w + x*x - y*y - z*z);
240  a_m.v01(2*x*y - 2*w*z);
241  a_m.v02(2*x*z + 2*w*y);
242  a_m.v03(0);
243 
244  // second row :
245  a_m.v10(2*x*y + 2*w*z);
246  a_m.v11(w*w - x*x + y*y - z*z);
247  a_m.v12(2*y*z - 2*w*x);
248  a_m.v13(0);
249 
250  // third row :
251  a_m.v20(2*x*z - 2*w*y);
252  a_m.v21(2*y*z + 2*w*x);
253  a_m.v22(w*w - x*x - y*y + z*z);
254  a_m.v23(0);
255 
256  // fourth row :
257  a_m.v30(0);
258  a_m.v31(0);
259  a_m.v32(0);
260  a_m.v33(w*w + x*x + y*y + z*z);
261  }
262 
263  template <class MAT3>
264  T value_3(MAT3& a_m) const {
265  //Return this rotation in the form of a 3D matrix.
266  //NOTE : in coin3d/SbRotation, it looks as if "w <-> -w", but coin3d/SbMatrix logic is transposed relative to us.
267 
268  const T x = m_quat.v0();
269  const T y = m_quat.v1();
270  const T z = m_quat.v2();
271  const T w = m_quat.v3();
272  // q = w + x * i + y * j + z * k
273 
274  // first row :
275  a_m.v00(w*w + x*x - y*y - z*z);
276  a_m.v01(2*x*y - 2*w*z);
277  a_m.v02(2*x*z + 2*w*y);
278 
279  // second row :
280  a_m.v10(2*x*y + 2*w*z);
281  a_m.v11(w*w - x*x + y*y - z*z);
282  a_m.v12(2*y*z - 2*w*x);
283 
284  // third row :
285  a_m.v20(2*x*z - 2*w*y);
286  a_m.v21(2*y*z + 2*w*x);
287  a_m.v22(w*w - x*x - y*y + z*z);
288 
289  // fourth row :
290  //a_m.v30(0);
291  //a_m.v31(0);
292  //a_m.v32(0);
293  //a_m.v33(w*w + x*x + y*y + z*z);
294  return w*w + x*x + y*y + z*z; //should be 1.
295  }
296 
297  void mul_vec(const VEC3& a_in,VEC3& a_out) const {
298  const T x = m_quat.v0();
299  const T y = m_quat.v1();
300  const T z = m_quat.v2();
301  const T w = m_quat.v3();
302 
303  // first row :
304  T v0 = (w*w + x*x - y*y - z*z) * a_in.v0()
305  + (2*x*y - 2*w*z) * a_in.v1()
306  + (2*x*z + 2*w*y) * a_in.v2();
307 
308  T v1 = (2*x*y + 2*w*z) * a_in.v0()
309  +(w*w - x*x + y*y - z*z) * a_in.v1()
310  + (2*y*z - 2*w*x) * a_in.v2();
311 
312  T v2 = (2*x*z - 2*w*y) * a_in.v0()
313  + (2*y*z + 2*w*x) * a_in.v1()
314  +(w*w - x*x - y*y + z*z) * a_in.v2();
315 
316  a_out.set_value(v0,v1,v2);
317  }
318 
319  void mul_vec(VEC3& a_v) const {
320  const T x = m_quat.v0();
321  const T y = m_quat.v1();
322  const T z = m_quat.v2();
323  const T w = m_quat.v3();
324 
325  // first row :
326  T v0 = (w*w + x*x - y*y - z*z) * a_v.v0()
327  + (2*x*y - 2*w*z) * a_v.v1()
328  + (2*x*z + 2*w*y) * a_v.v2();
329 
330  T v1 = (2*x*y + 2*w*z) * a_v.v0()
331  +(w*w - x*x + y*y - z*z) * a_v.v1()
332  + (2*y*z - 2*w*x) * a_v.v2();
333 
334  T v2 = (2*x*z - 2*w*y) * a_v.v0()
335  + (2*y*z + 2*w*x) * a_v.v1()
336  +(w*w - x*x - y*y + z*z) * a_v.v2();
337 
338  a_v.set_value(v0,v1,v2);
339  }
340 
341  void mul_3(T& a_x,T& a_y,T& a_z) const {
342  const T x = m_quat.v0();
343  const T y = m_quat.v1();
344  const T z = m_quat.v2();
345  const T w = m_quat.v3();
346 
347  // first row :
348  T v0 = (w*w + x*x - y*y - z*z) * a_x
349  + (2*x*y - 2*w*z) * a_y
350  + (2*x*z + 2*w*y) * a_z;
351 
352  T v1 = (2*x*y + 2*w*z) * a_x
353  +(w*w - x*x + y*y - z*z) * a_y
354  + (2*y*z - 2*w*x) * a_z;
355 
356  T v2 = (2*x*z - 2*w*y) * a_x
357  + (2*y*z + 2*w*x) * a_y
358  +(w*w - x*x - y*y + z*z) * a_z;
359 
360  a_x = v0;
361  a_y = v1;
362  a_z = v2;
363  }
364 
365 public: //for io::streamer
366  const VEC4& quat() const {return m_quat;}
367  VEC4& quat() {return m_quat;}
368 
369 protected:
370  static T one() {return T(1);}
371  static T minus_one() {return T(-1);}
372  static T half() {return T(0.5);}
373 
374 protected:
375  VEC4 m_quat;
376 
377 public:
378  //NOTE : don't handle a static object because of mem balance.
379  //static const qrot<double>& identity() {
380  // static const qrot<double> s_v(0,0,0,1);
381  // return s_v;
382  //}
383 
384 //private:static void check_instantiation() {qrot<float> v;}
385 };
386 
387 }
388 
389 #endif
tools::qrot::set_value
void set_value(const MAT4 &a_m, T(*a_sqrt)(T))
Definition: qrot:189
tools::qrot::qrot
qrot(T a_q0, T a_q1, T a_q2, T a_q3)
Definition: qrot:34
tools::qrot::qrot
qrot()
Definition: qrot:17
tools::qrot::set_value
bool set_value(const VEC3 &a_from, const VEC3 &a_to, T(*a_sqrt)(T), T(*a_fabs)(T))
Definition: qrot:124
tools::qrot::mul_vec
void mul_vec(const VEC3 &a_in, VEC3 &a_out) const
Definition: qrot:297
tools::qrot::value
void value(MAT4 &a_m) const
Definition: qrot:228
tools::qrot::m_quat
VEC4 m_quat
Definition: qrot:375
tools::qrot::value_3
T value_3(MAT3 &a_m) const
Definition: qrot:264
tools::qrot::mul_vec
void mul_vec(VEC3 &a_v) const
Definition: qrot:319
tools::qrot::quat
const VEC4 & quat() const
Definition: qrot:366
tools::qrot::qrot
qrot(const VEC3 &a_axis, T a_radians, T(*a_sin)(T), T(*a_cos)(T))
Definition: qrot:20
tools::qrot::half
static T half()
Definition: qrot:372
tools::qrot::mul_3
void mul_3(T &a_x, T &a_y, T &a_z) const
Definition: qrot:341
tools::qrot::qrot
qrot(const qrot &a_from)
Definition: qrot:26
tools::qrot::minus_one
static T minus_one()
Definition: qrot:371
tools::qrot::operator*
qrot operator*(const qrot &a_r) const
Definition: qrot:71
tools::qrot::one
static T one()
Definition: qrot:370
tools::qrot::invert
bool invert()
Definition: qrot:77
tools::qrot::operator!=
bool operator!=(const qrot &a_r) const
Definition: qrot:68
tools::qrot::operator==
bool operator==(const qrot &a_r) const
Definition: qrot:65
tools::to
std::vector< std::string > to(int a_argc, char **a_argv)
Definition: args:507
tools::qrot::~qrot
virtual ~qrot()
Definition: qrot:24
tools::qrot::set_value
bool set_value(const VEC3 &a_axis, T a_radians, T(*a_sin)(T), T(*a_cos)(T))
Definition: qrot:109
tools
inlined C code : ///////////////////////////////////
Definition: aida_ntuple:26
tools::qrot::T
VEC4::elem_t T
Definition: qrot:15
tools::qrot::quat
VEC4 & quat()
Definition: qrot:367
tools::qrot
Definition: qrot:12
tools::qrot::operator=
qrot & operator=(const qrot &a_from)
Definition: qrot:29
tools::qrot::inverse
bool inverse(qrot &a_r) const
Definition: qrot:92
tools::qrot::value
bool value(VEC3 &a_axis, T &a_radians, T(*a_sin)(T), T(*a_acos)(T)) const
Definition: qrot:166
tools::qrot::operator*=
qrot & operator*=(const qrot &a_q)
Definition: qrot:41
tools::qrot::qrot
qrot(const VEC3 &a_from, const VEC3 &a_to, T(*a_sqrt)(T), T(*a_fabs)(T))
Definition: qrot:23
Generated by doxygen 1.8.20