| Geant4 Cross Reference |
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