]> git.proxmox.com Git - ceph.git/blame - ceph/src/boost/boost/qvm/quat_operations.hpp
projects / ceph.git / blame
? search:
summary | shortlog | log | commit | commitdiff | tree
blob | blame (incremental) | history | HEAD
update source to Ceph Pacific 16.2.2
[ceph.git] / ceph / src / boost / boost / qvm / quat_operations.hpp
CommitLineData
92f5a8d4
TL		 1//Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.
2
3//Distributed under the Boost Software License, Version 1.0. (See accompanying
4//file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
5
f67539c2
TL		 6#ifndef BOOST_QVM_E6519754D19211DFB8405F74DFD72085
7#define BOOST_QVM_E6519754D19211DFB8405F74DFD72085
92f5a8d4
TL		 8
9#include <boost/qvm/detail/quat_assign.hpp>
10#include <boost/qvm/deduce_quat.hpp>
11#include <boost/qvm/mat_traits.hpp>
12#include <boost/qvm/scalar_traits.hpp>
13#include <boost/qvm/math.hpp>
14#include <boost/qvm/assert.hpp>
15#include <boost/qvm/error.hpp>
16#include <boost/qvm/throw_exception.hpp>
17#include <string>
18
19namespace
20boost
21 {
22 namespace
23 qvm
24 {
25 namespace
26 qvm_detail
27 {
28 BOOST_QVM_INLINE_CRITICAL
29 void const *
30 get_valid_ptr_quat_operations()
31 {
32 static int const obj=0;
33 return &obj;
34 }
35 }
36
37 ////////////////////////////////////////////////
38
39 namespace
40 msvc_parse_bug_workaround
41 {
42 template <class A,class B>
43 struct
44 quats
45 {
46 static bool const value=is_quat<A>::value && is_quat<B>::value;
47 };
48 }
49
50 namespace
51 qvm_to_string_detail
52 {
53 template <class T>
54 std::string to_string( T const & x );
55 }
56
57 template <class A>
58 inline
59 typename boost::enable_if_c<
60 is_quat<A>::value,
61 std::string>::type
62 to_string( A const & a )
63 {
64 using namespace qvm_to_string_detail;
65 return '('+
66 to_string(quat_traits<A>::template read_element<0>(a))+','+
67 to_string(quat_traits<A>::template read_element<1>(a))+','+
68 to_string(quat_traits<A>::template read_element<2>(a))+','+
69 to_string(quat_traits<A>::template read_element<3>(a))+')';
70 }
71
72 ////////////////////////////////////////////////
73
74 template <class A,class B,class Cmp>
75 BOOST_QVM_INLINE_OPERATIONS
76 typename enable_if_c<
77 is_quat<A>::value && is_quat<B>::value,
78 bool>::type
79 cmp( A const & a, B const & b, Cmp f )
80 {
f67539c2
TL		 81 typedef typename quat_traits<A>::scalar_type T;
82 typedef typename quat_traits<B>::scalar_type U;
92f5a8d4
TL		 83 T q1[4] =
84 {
85 quat_traits<A>::template read_element<0>(a),
86 quat_traits<A>::template read_element<1>(a),
87 quat_traits<A>::template read_element<2>(a),
88 quat_traits<A>::template read_element<3>(a)
89 };
f67539c2 90 U q2[4] =
92f5a8d4
TL		 91 {
92 quat_traits<B>::template read_element<0>(b),
93 quat_traits<B>::template read_element<1>(b),
94 quat_traits<B>::template read_element<2>(b),
95 quat_traits<B>::template read_element<3>(b)
96 };
97 int i;
98 for( i=0; i!=4; ++i )
99 if( !f(q1[i],q2[i]) )
100 break;
101 if( i==4 )
102 return true;
103 for( i=0; i!=4; ++i )
104 if( !f(q1[i],-q2[i]) )
105 return false;
106 return true;
107 }
108
109 ////////////////////////////////////////////////
110
111 template <class R,class A>
112 BOOST_QVM_INLINE_TRIVIAL
113 typename enable_if_c<
114 is_quat<R>::value && is_quat<A>::value,
115 R>::type
116 convert_to( A const & a )
117 {
118 R r;
119 quat_traits<R>::template write_element<0>(r) = quat_traits<A>::template read_element<0>(a);
120 quat_traits<R>::template write_element<1>(r) = quat_traits<A>::template read_element<1>(a);
121 quat_traits<R>::template write_element<2>(r) = quat_traits<A>::template read_element<2>(a);
122 quat_traits<R>::template write_element<3>(r) = quat_traits<A>::template read_element<3>(a);
123 return r;
124 }
125
126 template <class R,class A>
127 BOOST_QVM_INLINE_OPERATIONS
128 typename enable_if_c<
129 is_quat<R>::value && is_mat<A>::value &&
130 mat_traits<A>::rows==3 && mat_traits<A>::cols==3,
131 R>::type
132 convert_to( A const & a )
133 {
134 typedef typename mat_traits<A>::scalar_type T;
135 T const mat[3][3] =
136 {
137 { mat_traits<A>::template read_element<0,0>(a), mat_traits<A>::template read_element<0,1>(a), mat_traits<A>::template read_element<0,2>(a) },
138 { mat_traits<A>::template read_element<1,0>(a), mat_traits<A>::template read_element<1,1>(a), mat_traits<A>::template read_element<1,2>(a) },
139 { mat_traits<A>::template read_element<2,0>(a), mat_traits<A>::template read_element<2,1>(a), mat_traits<A>::template read_element<2,2>(a) }
140 };
141 R r;
142 if( mat[0][0]+mat[1][1]+mat[2][2] > scalar_traits<T>::value(0) )
143 {
144 T t = mat[0][0] + mat[1][1] + mat[2][2] + scalar_traits<T>::value(1);
145 T s = (scalar_traits<T>::value(1)/sqrt<T>(t))/2;
146 quat_traits<R>::template write_element<0>(r)=s*t;
147 quat_traits<R>::template write_element<1>(r)=(mat[2][1]-mat[1][2])*s;
148 quat_traits<R>::template write_element<2>(r)=(mat[0][2]-mat[2][0])*s;
149 quat_traits<R>::template write_element<3>(r)=(mat[1][0]-mat[0][1])*s;
150 }
151 else if( mat[0][0]>mat[1][1] && mat[0][0]>mat[2][2] )
152 {
153 T t = mat[0][0] - mat[1][1] - mat[2][2] + scalar_traits<T>::value(1);
154 T s = (scalar_traits<T>::value(1)/sqrt<T>(t))/2;
155 quat_traits<R>::template write_element<0>(r)=(mat[2][1]-mat[1][2])*s;
156 quat_traits<R>::template write_element<1>(r)=s*t;
157 quat_traits<R>::template write_element<2>(r)=(mat[1][0]+mat[0][1])*s;
158 quat_traits<R>::template write_element<3>(r)=(mat[0][2]+mat[2][0])*s;
159 }
160 else if( mat[1][1]>mat[2][2] )
161 {
162 T t = - mat[0][0] + mat[1][1] - mat[2][2] + scalar_traits<T>::value(1);
163 T s = (scalar_traits<T>::value(1)/sqrt<T>(t))/2;
164 quat_traits<R>::template write_element<0>(r)=(mat[0][2]-mat[2][0])*s;
165 quat_traits<R>::template write_element<1>(r)=(mat[1][0]+mat[0][1])*s;
166 quat_traits<R>::template write_element<2>(r)=s*t;
167 quat_traits<R>::template write_element<3>(r)=(mat[2][1]+mat[1][2])*s;
168 }
169 else
170 {
171 T t = - mat[0][0] - mat[1][1] + mat[2][2] + scalar_traits<T>::value(1);
172 T s = (scalar_traits<T>::value(1)/sqrt<T>(t))/2;
173 quat_traits<R>::template write_element<0>(r)=(mat[1][0]-mat[0][1])*s;
174 quat_traits<R>::template write_element<1>(r)=(mat[0][2]+mat[2][0])*s;
175 quat_traits<R>::template write_element<2>(r)=(mat[2][1]+mat[1][2])*s;
176 quat_traits<R>::template write_element<3>(r)=s*t;
177 }
178 return r;
179 }
180
181 ////////////////////////////////////////////////
182
183 template <class A>
184 BOOST_QVM_INLINE_OPERATIONS
185 typename lazy_enable_if_c<
186 is_quat<A>::value,
187 deduce_quat<A> >::type
188 conjugate( A const & a )
189 {
190 typedef typename deduce_quat<A>::type R;
191 R r;
192 quat_traits<R>::template write_element<0>(r)=quat_traits<A>::template read_element<0>(a);
193 quat_traits<R>::template write_element<1>(r)=-quat_traits<A>::template read_element<1>(a);
194 quat_traits<R>::template write_element<2>(r)=-quat_traits<A>::template read_element<2>(a);
195 quat_traits<R>::template write_element<3>(r)=-quat_traits<A>::template read_element<3>(a);
196 return r;
197 }
198
199 ////////////////////////////////////////////////
200
201 namespace
202 qvm_detail
203 {
204 template <class T>
205 class
206 identity_quat_
207 {
208 identity_quat_( identity_quat_ const & );
209 identity_quat_ & operator=( identity_quat_ const & );
210 ~identity_quat_();
211
212 public:
213
214 template <class R>
215 BOOST_QVM_INLINE_TRIVIAL
216 operator R() const
217 {
218 R r;
219 assign(r,*this);
220 return r;
221 }
222 };
223 }
224
225 template <class T>
226 struct
227 quat_traits< qvm_detail::identity_quat_<T> >
228 {
229 typedef qvm_detail::identity_quat_<T> this_quaternion;
230 typedef T scalar_type;
231
232 template <int I>
233 static
234 BOOST_QVM_INLINE_CRITICAL
235 scalar_type
236 read_element( this_quaternion const & x )
237 {
238 BOOST_QVM_STATIC_ASSERT(I>=0);
239 BOOST_QVM_STATIC_ASSERT(I<4);
240 return scalar_traits<T>::value(I==0);
241 }
242
243 static
244 BOOST_QVM_INLINE_CRITICAL
245 scalar_type
246 read_element_idx( int i, this_quaternion const & x )
247 {
248 BOOST_QVM_ASSERT(i>=0);
249 BOOST_QVM_ASSERT(i<4);
250 return scalar_traits<T>::value(i==0);
251 }
252 };
253
254 template <class T>
255 struct
256 deduce_quat< qvm_detail::identity_quat_<T> >
257 {
258 typedef quat<T> type;
259 };
260
261 template <class T>
262 struct
263 deduce_quat2< qvm_detail::identity_quat_<T>, qvm_detail::identity_quat_<T> >
264 {
265 typedef quat<T> type;
266 };
267
268 template <class T>
269 BOOST_QVM_INLINE_TRIVIAL
270 qvm_detail::identity_quat_<T> const &
271 identity_quat()
272 {
273 return *(qvm_detail::identity_quat_<T> const *)qvm_detail::get_valid_ptr_quat_operations();
274 }
275
276 template <class A>
277 BOOST_QVM_INLINE_OPERATIONS
278 typename enable_if_c<
279 is_quat<A>::value,
280 void>::type
281 set_identity( A & a )
282 {
283 typedef typename quat_traits<A>::scalar_type T;
284 T const zero=scalar_traits<T>::value(0);
285 T const one=scalar_traits<T>::value(1);
286 quat_traits<A>::template write_element<0>(a) = one;
287 quat_traits<A>::template write_element<1>(a) = zero;
288 quat_traits<A>::template write_element<2>(a) = zero;
289 quat_traits<A>::template write_element<3>(a) = zero;
290 }
291
292 ////////////////////////////////////////////////
293
294 namespace
295 qvm_detail
296 {
297 template <class OriginalType,class Scalar>
298 class
299 quaternion_scalar_cast_
300 {
301 quaternion_scalar_cast_( quaternion_scalar_cast_ const & );
302 quaternion_scalar_cast_ & operator=( quaternion_scalar_cast_ const & );
303 ~quaternion_scalar_cast_();
304
305 public:
306
307 template <class T>
308 BOOST_QVM_INLINE_TRIVIAL
309 quaternion_scalar_cast_ &
310 operator=( T const & x )
311 {
312 assign(*this,x);
313 return *this;
314 }
315
316 template <class R>
317 BOOST_QVM_INLINE_TRIVIAL
318 operator R() const
319 {
320 R r;
321 assign(r,*this);
322 return r;
323 }
324 };
325
326 template <bool> struct scalar_cast_quaternion_filter { };
327 template <> struct scalar_cast_quaternion_filter<true> { typedef int type; };
328 }
329
330 template <class OriginalType,class Scalar>
331 struct
332 quat_traits< qvm_detail::quaternion_scalar_cast_<OriginalType,Scalar> >
333 {
334 typedef Scalar scalar_type;
335 typedef qvm_detail::quaternion_scalar_cast_<OriginalType,Scalar> this_quaternion;
336
337 template <int I>
338 static
339 BOOST_QVM_INLINE_CRITICAL
340 scalar_type
341 read_element( this_quaternion const & x )
342 {
343 BOOST_QVM_STATIC_ASSERT(I>=0);
344 BOOST_QVM_STATIC_ASSERT(I<4);
345 return scalar_type(quat_traits<OriginalType>::template read_element<I>(reinterpret_cast<OriginalType const &>(x)));
346 }
347
348 static
349 BOOST_QVM_INLINE_CRITICAL
350 scalar_type
351 read_element_idx( int i, this_quaternion const & x )
352 {
353 BOOST_QVM_ASSERT(i>=0);
354 BOOST_QVM_ASSERT(i<4);
355 return scalar_type(quat_traits<OriginalType>::read_element_idx(i,reinterpret_cast<OriginalType const &>(x)));
356 }
357 };
358
359 template <class Scalar,class T>
360 BOOST_QVM_INLINE_TRIVIAL
361 qvm_detail::quaternion_scalar_cast_<T,Scalar> const &
362 scalar_cast( T const & x, typename qvm_detail::scalar_cast_quaternion_filter<is_quat<T>::value>::type=0 )
363 {
364 return reinterpret_cast<qvm_detail::quaternion_scalar_cast_<T,Scalar> const &>(x);
365 }
366
367 ////////////////////////////////////////////////
368
369 template <class A,class B>
370 BOOST_QVM_INLINE_OPERATIONS
371 typename enable_if_c<
372 is_quat<A>::value && is_scalar<B>::value,
373 A &>::type
374 operator/=( A & a, B b )
375 {
376 quat_traits<A>::template write_element<0>(a)/=b;
377 quat_traits<A>::template write_element<1>(a)/=b;
378 quat_traits<A>::template write_element<2>(a)/=b;
379 quat_traits<A>::template write_element<3>(a)/=b;
380 return a;
381 }
382
383 template <class A,class B>
384 BOOST_QVM_INLINE_OPERATIONS
385 typename lazy_enable_if_c<
386 is_quat<A>::value && is_scalar<B>::value,
387 deduce_quat<A> >::type
388 operator/( A const & a, B b )
389 {
390 typedef typename deduce_quat<A>::type R;
391 R r;
392 quat_traits<R>::template write_element<0>(r) = quat_traits<A>::template read_element<0>(a)/b;
393 quat_traits<R>::template write_element<1>(r) = quat_traits<A>::template read_element<1>(a)/b;
394 quat_traits<R>::template write_element<2>(r) = quat_traits<A>::template read_element<2>(a)/b;
395 quat_traits<R>::template write_element<3>(r) = quat_traits<A>::template read_element<3>(a)/b;
396 return r;
397 }
398
399 template <class A,class B>
400 BOOST_QVM_INLINE_OPERATIONS
401 typename lazy_enable_if_c<
402 is_quat<A>::value && is_quat<B>::value,
403 deduce_scalar<typename quat_traits<A>::scalar_type,typename quat_traits<B>::scalar_type> >::type
404 dot( A const & a, B const & b )
405 {
406 typedef typename quat_traits<A>::scalar_type Ta;
407 typedef typename quat_traits<B>::scalar_type Tb;
408 typedef typename deduce_scalar<Ta,Tb>::type Tr;
409 Ta const a0=quat_traits<A>::template read_element<0>(a);
410 Ta const a1=quat_traits<A>::template read_element<1>(a);
411 Ta const a2=quat_traits<A>::template read_element<2>(a);
412 Ta const a3=quat_traits<A>::template read_element<3>(a);
413 Tb const b0=quat_traits<B>::template read_element<0>(b);
414 Tb const b1=quat_traits<B>::template read_element<1>(b);
415 Tb const b2=quat_traits<B>::template read_element<2>(b);
416 Tb const b3=quat_traits<B>::template read_element<3>(b);
417 Tr const dp=a0*b0+a1*b1+a2*b2+a3*b3;
418 return dp;
419 }
420
421 template <class A,class B>
422 BOOST_QVM_INLINE_OPERATIONS
423 typename enable_if_c<
424 is_quat<A>::value && is_quat<B>::value,
425 bool>::type
426 operator==( A const & a, B const & b )
427 {
428 return
429 quat_traits<A>::template read_element<0>(a)==quat_traits<B>::template read_element<0>(b) &&
430 quat_traits<A>::template read_element<1>(a)==quat_traits<B>::template read_element<1>(b) &&
431 quat_traits<A>::template read_element<2>(a)==quat_traits<B>::template read_element<2>(b) &&
432 quat_traits<A>::template read_element<3>(a)==quat_traits<B>::template read_element<3>(b);
433 }
434
435 template <class A>
436 BOOST_QVM_INLINE_OPERATIONS
437 typename lazy_enable_if_c<
438 is_quat<A>::value,
439 deduce_quat<A> >::type
440 inverse( A const & a )
441 {
442 typedef typename deduce_quat<A>::type R;
443 typedef typename quat_traits<A>::scalar_type TA;
444 TA aa = quat_traits<A>::template read_element<0>(a);
445 TA ab = quat_traits<A>::template read_element<1>(a);
446 TA ac = quat_traits<A>::template read_element<2>(a);
447 TA ad = quat_traits<A>::template read_element<3>(a);
448 TA m2 = ab*ab + ac*ac + ad*ad + aa*aa;
449 if( m2==scalar_traits<TA>::value(0) )
450 BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
451 TA rm=scalar_traits<TA>::value(1)/m2;
452 R r;
453 quat_traits<R>::template write_element<0>(r) = aa*rm;
454 quat_traits<R>::template write_element<1>(r) = -ab*rm;
455 quat_traits<R>::template write_element<2>(r) = -ac*rm;
456 quat_traits<R>::template write_element<3>(r) = -ad*rm;
457 return r;
458 }
459
460 template <class A>
461 BOOST_QVM_INLINE_OPERATIONS
462 typename enable_if_c<
463 is_quat<A>::value,
464 typename quat_traits<A>::scalar_type>::type
465 mag_sqr( A const & a )
466 {
467 typedef typename quat_traits<A>::scalar_type T;
468 T x=quat_traits<A>::template read_element<0>(a);
469 T y=quat_traits<A>::template read_element<1>(a);
470 T z=quat_traits<A>::template read_element<2>(a);
471 T w=quat_traits<A>::template read_element<3>(a);
472 return x*x+y*y+z*z+w*w;
473 }
474
475 template <class A>
476 BOOST_QVM_INLINE_OPERATIONS
477 typename enable_if_c<
478 is_quat<A>::value,
479 typename quat_traits<A>::scalar_type>::type
480 mag( A const & a )
481 {
482 typedef typename quat_traits<A>::scalar_type T;
483 T x=quat_traits<A>::template read_element<0>(a);
484 T y=quat_traits<A>::template read_element<1>(a);
485 T z=quat_traits<A>::template read_element<2>(a);
486 T w=quat_traits<A>::template read_element<3>(a);
487 return sqrt<T>(x*x+y*y+z*z+w*w);
488 }
489
490 template <class A,class B>
491 BOOST_QVM_INLINE_OPERATIONS
492 typename enable_if<
493 msvc_parse_bug_workaround::quats<A,B>,
494 A &>::type
495 operator-=( A & a, B const & b )
496 {
497 quat_traits<A>::template write_element<0>(a)-=quat_traits<B>::template read_element<0>(b);
498 quat_traits<A>::template write_element<1>(a)-=quat_traits<B>::template read_element<1>(b);
499 quat_traits<A>::template write_element<2>(a)-=quat_traits<B>::template read_element<2>(b);
500 quat_traits<A>::template write_element<3>(a)-=quat_traits<B>::template read_element<3>(b);
501 return a;
502 }
503
504 template <class A,class B>
505 BOOST_QVM_INLINE_OPERATIONS
506 typename lazy_enable_if_c<
507 is_quat<A>::value && is_quat<B>::value,
508 deduce_quat2<A,B> >::type
509 operator-( A const & a, B const & b )
510 {
511 typedef typename deduce_quat2<A,B>::type R;
512 R r;
513 quat_traits<R>::template write_element<0>(r)=quat_traits<A>::template read_element<0>(a)-quat_traits<B>::template read_element<0>(b);
514 quat_traits<R>::template write_element<1>(r)=quat_traits<A>::template read_element<1>(a)-quat_traits<B>::template read_element<1>(b);
515 quat_traits<R>::template write_element<2>(r)=quat_traits<A>::template read_element<2>(a)-quat_traits<B>::template read_element<2>(b);
516 quat_traits<R>::template write_element<3>(r)=quat_traits<A>::template read_element<3>(a)-quat_traits<B>::template read_element<3>(b);
517 return r;
518 }
519
520 template <class A>
521 BOOST_QVM_INLINE_OPERATIONS
522 typename lazy_enable_if_c<
523 is_quat<A>::value,
524 deduce_quat<A> >::type
525 operator-( A const & a )
526 {
527 typedef typename deduce_quat<A>::type R;
528 R r;
529 quat_traits<R>::template write_element<0>(r)=-quat_traits<A>::template read_element<0>(a);
530 quat_traits<R>::template write_element<1>(r)=-quat_traits<A>::template read_element<1>(a);
531 quat_traits<R>::template write_element<2>(r)=-quat_traits<A>::template read_element<2>(a);
532 quat_traits<R>::template write_element<3>(r)=-quat_traits<A>::template read_element<3>(a);
533 return r;
534 }
535
536 template <class A,class B>
537 BOOST_QVM_INLINE_OPERATIONS
538 typename enable_if<
539 msvc_parse_bug_workaround::quats<A,B>,
540 A &>::type
541 operator*=( A & a, B const & b )
542 {
543 typedef typename quat_traits<A>::scalar_type TA;
544 typedef typename quat_traits<B>::scalar_type TB;
545 TA const aa=quat_traits<A>::template read_element<0>(a);
546 TA const ab=quat_traits<A>::template read_element<1>(a);
547 TA const ac=quat_traits<A>::template read_element<2>(a);
548 TA const ad=quat_traits<A>::template read_element<3>(a);
549 TB const ba=quat_traits<B>::template read_element<0>(b);
550 TB const bb=quat_traits<B>::template read_element<1>(b);
551 TB const bc=quat_traits<B>::template read_element<2>(b);
552 TB const bd=quat_traits<B>::template read_element<3>(b);
553 quat_traits<A>::template write_element<0>(a) = aa*ba - ab*bb - ac*bc - ad*bd;
554 quat_traits<A>::template write_element<1>(a) = aa*bb + ab*ba + ac*bd - ad*bc;
555 quat_traits<A>::template write_element<2>(a) = aa*bc + ac*ba + ad*bb - ab*bd;
556 quat_traits<A>::template write_element<3>(a) = aa*bd + ad*ba + ab*bc - ac*bb;
557 return a;
558 }
559
560 template <class A,class B>
561 BOOST_QVM_INLINE_OPERATIONS
562 typename enable_if_c<
563 is_quat<A>::value && is_scalar<B>::value,
564 A &>::type
565 operator*=( A & a, B b )
566 {
567 quat_traits<A>::template write_element<0>(a)*=b;
568 quat_traits<A>::template write_element<1>(a)*=b;
569 quat_traits<A>::template write_element<2>(a)*=b;
570 quat_traits<A>::template write_element<3>(a)*=b;
571 return a;
572 }
573
574 template <class A,class B>
575 BOOST_QVM_INLINE_OPERATIONS
576 typename lazy_enable_if_c<
577 is_quat<A>::value && is_quat<B>::value,
578 deduce_quat2<A,B> >::type
579 operator*( A const & a, B const & b )
580 {
581 typedef typename deduce_quat2<A,B>::type R;
582 typedef typename quat_traits<A>::scalar_type TA;
583 typedef typename quat_traits<B>::scalar_type TB;
584 TA const aa=quat_traits<A>::template read_element<0>(a);
585 TA const ab=quat_traits<A>::template read_element<1>(a);
586 TA const ac=quat_traits<A>::template read_element<2>(a);
587 TA const ad=quat_traits<A>::template read_element<3>(a);
588 TB const ba=quat_traits<B>::template read_element<0>(b);
589 TB const bb=quat_traits<B>::template read_element<1>(b);
590 TB const bc=quat_traits<B>::template read_element<2>(b);
591 TB const bd=quat_traits<B>::template read_element<3>(b);
592 R r;
593 quat_traits<R>::template write_element<0>(r) = aa*ba - ab*bb - ac*bc - ad*bd;
594 quat_traits<R>::template write_element<1>(r) = aa*bb + ab*ba + ac*bd - ad*bc;
595 quat_traits<R>::template write_element<2>(r) = aa*bc + ac*ba + ad*bb - ab*bd;
596 quat_traits<R>::template write_element<3>(r) = aa*bd + ad*ba + ab*bc - ac*bb;
597 return r;
598 }
599
600 template <class A,class B>
601 BOOST_QVM_INLINE_OPERATIONS
602 typename lazy_enable_if_c<
603 is_quat<A>::value && is_scalar<B>::value,
604 deduce_quat<A> >::type
605 operator*( A const & a, B b )
606 {
607 typedef typename deduce_quat<A>::type R;
608 R r;
609 quat_traits<R>::template write_element<0>(r)=quat_traits<A>::template read_element<0>(a)*b;
610 quat_traits<R>::template write_element<1>(r)=quat_traits<A>::template read_element<1>(a)*b;
611 quat_traits<R>::template write_element<2>(r)=quat_traits<A>::template read_element<2>(a)*b;
612 quat_traits<R>::template write_element<3>(r)=quat_traits<A>::template read_element<3>(a)*b;
613 return r;
614 }
615
616 template <class A,class B>
617 BOOST_QVM_INLINE_OPERATIONS
618 typename enable_if_c<
619 is_quat<A>::value && is_quat<B>::value,
620 bool>::type
621 operator!=( A const & a, B const & b )
622 {
623 return
624 quat_traits<A>::template read_element<0>(a)!=quat_traits<B>::template read_element<0>(b) ||
625 quat_traits<A>::template read_element<1>(a)!=quat_traits<B>::template read_element<1>(b) ||
626 quat_traits<A>::template read_element<2>(a)!=quat_traits<B>::template read_element<2>(b) ||
627 quat_traits<A>::template read_element<3>(a)!=quat_traits<B>::template read_element<3>(b);
628 }
629
630 template <class A>
631 BOOST_QVM_INLINE_OPERATIONS
632 typename lazy_enable_if_c<
633 is_quat<A>::value,
634 deduce_quat<A> >::type
635 normalized( A const & a )
636 {
637 typedef typename quat_traits<A>::scalar_type T;
638 T const a0=quat_traits<A>::template read_element<0>(a);
639 T const a1=quat_traits<A>::template read_element<1>(a);
640 T const a2=quat_traits<A>::template read_element<2>(a);
641 T const a3=quat_traits<A>::template read_element<3>(a);
642 T const m2=a0*a0+a1*a1+a2*a2+a3*a3;
643 if( m2==scalar_traits<typename quat_traits<A>::scalar_type>::value(0) )
644 BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
645 T const rm=scalar_traits<T>::value(1)/sqrt<T>(m2);
646 typedef typename deduce_quat<A>::type R;
647 R r;
648 quat_traits<R>::template write_element<0>(r)=a0*rm;
649 quat_traits<R>::template write_element<1>(r)=a1*rm;
650 quat_traits<R>::template write_element<2>(r)=a2*rm;
651 quat_traits<R>::template write_element<3>(r)=a3*rm;
652 return r;
653 }
654
655 template <class A>
656 BOOST_QVM_INLINE_OPERATIONS
657 typename enable_if_c<
658 is_quat<A>::value,
659 void>::type
660 normalize( A & a )
661 {
662 typedef typename quat_traits<A>::scalar_type T;
663 T const a0=quat_traits<A>::template read_element<0>(a);
664 T const a1=quat_traits<A>::template read_element<1>(a);
665 T const a2=quat_traits<A>::template read_element<2>(a);
666 T const a3=quat_traits<A>::template read_element<3>(a);
667 T const m2=a0*a0+a1*a1+a2*a2+a3*a3;
668 if( m2==scalar_traits<typename quat_traits<A>::scalar_type>::value(0) )
669 BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
670 T const rm=scalar_traits<T>::value(1)/sqrt<T>(m2);
671 quat_traits<A>::template write_element<0>(a)*=rm;
672 quat_traits<A>::template write_element<1>(a)*=rm;
673 quat_traits<A>::template write_element<2>(a)*=rm;
674 quat_traits<A>::template write_element<3>(a)*=rm;
675 }
676
677 template <class A,class B>
678 BOOST_QVM_INLINE_OPERATIONS
679 typename enable_if<
680 msvc_parse_bug_workaround::quats<A,B>,
681 A &>::type
682 operator+=( A & a, B const & b )
683 {
684 quat_traits<A>::template write_element<0>(a)+=quat_traits<B>::template read_element<0>(b);
685 quat_traits<A>::template write_element<1>(a)+=quat_traits<B>::template read_element<1>(b);
686 quat_traits<A>::template write_element<2>(a)+=quat_traits<B>::template read_element<2>(b);
687 quat_traits<A>::template write_element<3>(a)+=quat_traits<B>::template read_element<3>(b);
688 return a;
689 }
690
691 template <class A,class B>
692 BOOST_QVM_INLINE_OPERATIONS
693 typename lazy_enable_if_c<
694 is_quat<A>::value && is_quat<B>::value,
695 deduce_quat2<A,B> >::type
696 operator+( A const & a, B const & b )
697 {
698 typedef typename deduce_quat2<A,B>::type R;
699 R r;
700 quat_traits<R>::template write_element<0>(r)=quat_traits<A>::template read_element<0>(a)+quat_traits<B>::template read_element<0>(b);
701 quat_traits<R>::template write_element<1>(r)=quat_traits<A>::template read_element<1>(a)+quat_traits<B>::template read_element<1>(b);
702 quat_traits<R>::template write_element<2>(r)=quat_traits<A>::template read_element<2>(a)+quat_traits<B>::template read_element<2>(b);
703 quat_traits<R>::template write_element<3>(r)=quat_traits<A>::template read_element<3>(a)+quat_traits<B>::template read_element<3>(b);
704 return r;
705 }
706
707 template <class A,class B,class C>
708 BOOST_QVM_INLINE_OPERATIONS
709 typename lazy_enable_if_c<
710 is_quat<A>::value && is_quat<B>::value && is_scalar<C>::value,
711 deduce_quat2<A,B> >::type
712 slerp( A const & a, B const & b, C t )
713 {
714 typedef typename deduce_quat2<A,B>::type R;
715 typedef typename quat_traits<R>::scalar_type TR;
716 TR const one = scalar_traits<TR>::value(1);
717 TR dp = dot(a,b);
718 TR sc=one;
719 if( dp < one )
720 {
721 TR const theta = acos<TR>(dp);
722 TR const invsintheta = one/sin<TR>(theta);
723 TR const scale = sin<TR>(theta*(one-t)) * invsintheta;
724 TR const invscale = sin<TR>(theta*t) * invsintheta * sc;
725 return a*scale + b*invscale;
726 }
727 else
728 return normalized(a+(b-a)*t);
729 }
730
731 ////////////////////////////////////////////////
732
733 namespace
734 qvm_detail
735 {
736 template <class T>
737 class
738 qref_
739 {
740 qref_( qref_ const & );
741 qref_ & operator=( qref_ const & );
742 ~qref_();
743
744 public:
745
746 template <class R>
747 BOOST_QVM_INLINE_TRIVIAL
748 qref_ &
749 operator=( R const & x )
750 {
751 assign(*this,x);
752 return *this;
753 }
754
755 template <class R>
756 BOOST_QVM_INLINE_TRIVIAL
757 operator R() const
758 {
759 R r;
760 assign(r,*this);
761 return r;
762 }
763 };
764 }
765
766 template <class Q>
767 struct quat_traits;
768
769 template <class Q>
770 struct
771 quat_traits< qvm_detail::qref_<Q> >
772 {
773 typedef typename quat_traits<Q>::scalar_type scalar_type;
774 typedef qvm_detail::qref_<Q> this_quaternion;
775
776 template <int I>
777 static
778 BOOST_QVM_INLINE_CRITICAL
779 scalar_type
780 read_element( this_quaternion const & x )
781 {
782 BOOST_QVM_STATIC_ASSERT(I>=0);
783 BOOST_QVM_STATIC_ASSERT(I<4);
784 return quat_traits<Q>::template read_element<I>(reinterpret_cast<Q const &>(x));
785 }
786
787 template <int I>
788 static
789 BOOST_QVM_INLINE_CRITICAL
790 scalar_type &
791 write_element( this_quaternion & x )
792 {
793 BOOST_QVM_STATIC_ASSERT(I>=0);
794 BOOST_QVM_STATIC_ASSERT(I<4);
795 return quat_traits<Q>::template write_element<I>(reinterpret_cast<Q &>(x));
796 }
797 };
798
799 template <class Q>
800 struct
801 deduce_quat< qvm_detail::qref_<Q> >
802 {
803 typedef quat<typename quat_traits<Q>::scalar_type> type;
804 };
805
806 template <class Q>
807 BOOST_QVM_INLINE_TRIVIAL
808 typename enable_if_c<
809 is_quat<Q>::value,
810 qvm_detail::qref_<Q> const &>::type
811 qref( Q const & a )
812 {
813 return reinterpret_cast<qvm_detail::qref_<Q> const &>(a);
814 }
815
816 template <class Q>
817 BOOST_QVM_INLINE_TRIVIAL
818 typename enable_if_c<
819 is_quat<Q>::value,
820 qvm_detail::qref_<Q> &>::type
821 qref( Q & a )
822 {
823 return reinterpret_cast<qvm_detail::qref_<Q> &>(a);
824 }
825
826 ////////////////////////////////////////////////
827
828 namespace
829 qvm_detail
830 {
831 template <class T>
832 class
833 zero_q_
834 {
835 zero_q_( zero_q_ const & );
836 zero_q_ & operator=( zero_q_ const & );
837 ~zero_q_();
838
839 public:
840
841 template <class R>
842 BOOST_QVM_INLINE_TRIVIAL
843 operator R() const
844 {
845 R r;
846 assign(r,*this);
847 return r;
848 }
849 };
850 }
851
852 template <class T>
853 struct
854 quat_traits< qvm_detail::zero_q_<T> >
855 {
856 typedef qvm_detail::zero_q_<T> this_quaternion;
857 typedef T scalar_type;
858
859 template <int I>
860 static
861 BOOST_QVM_INLINE_CRITICAL
862 scalar_type
863 read_element( this_quaternion const & x )
864 {
865 BOOST_QVM_STATIC_ASSERT(I>=0);
866 BOOST_QVM_STATIC_ASSERT(I<4);
867 return scalar_traits<scalar_type>::value(0);
868 }
869
870 static
871 BOOST_QVM_INLINE_CRITICAL
872 scalar_type
873 read_element_idx( int i, this_quaternion const & x )
874 {
875 BOOST_QVM_ASSERT(i>=0);
876 BOOST_QVM_ASSERT(i<4);
877 return scalar_traits<scalar_type>::value(0);
878 }
879 };
880
881 template <class T>
882 BOOST_QVM_INLINE_TRIVIAL
883 qvm_detail::zero_q_<T> const &
884 zero_quat()
885 {
886 return *(qvm_detail::zero_q_<T> const *)qvm_detail::get_valid_ptr_quat_operations();
887 }
888
889 template <class A>
890 BOOST_QVM_INLINE_OPERATIONS
891 typename enable_if_c<
892 is_quat<A>::value,
893 void>::type
894 set_zero( A & a )
895 {
896 typedef typename quat_traits<A>::scalar_type T;
897 T const zero=scalar_traits<T>::value(0);
898 quat_traits<A>::template write_element<0>(a) = zero;
899 quat_traits<A>::template write_element<1>(a) = zero;
900 quat_traits<A>::template write_element<2>(a) = zero;
901 quat_traits<A>::template write_element<3>(a) = zero;
902 }
903
904 ////////////////////////////////////////////////
905
906 namespace
907 qvm_detail
908 {
909 template <class V>
910 struct
911 rot_quat_
912 {
913 typedef typename vec_traits<V>::scalar_type scalar_type;
914 scalar_type a[4];
915
916 template <class Angle>
917 BOOST_QVM_INLINE
918 rot_quat_( V const & axis, Angle angle )
919 {
920 scalar_type const x=vec_traits<V>::template read_element<0>(axis);
921 scalar_type const y=vec_traits<V>::template read_element<1>(axis);
922 scalar_type const z=vec_traits<V>::template read_element<2>(axis);
923 scalar_type const m2=x*x+y*y+z*z;
924 if( m2==scalar_traits<scalar_type>::value(0) )
925 BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
926 scalar_type const rm=scalar_traits<scalar_type>::value(1)/sqrt<scalar_type>(m2);
927 angle/=2;
928 scalar_type const s=sin<Angle>(angle);
929 a[0] = cos<Angle>(angle);
930 a[1] = rm*x*s;
931 a[2] = rm*y*s;
932 a[3] = rm*z*s;
933 }
934
935 template <class R>
936 BOOST_QVM_INLINE_TRIVIAL
937 operator R() const
938 {
939 R r;
940 assign(r,*this);
941 return r;
942 }
943 };
944 }
945
946 template <class V>
947 struct
948 quat_traits< qvm_detail::rot_quat_<V> >
949 {
950 typedef qvm_detail::rot_quat_<V> this_quaternion;
951 typedef typename this_quaternion::scalar_type scalar_type;
952
953 template <int I>
954 static
955 BOOST_QVM_INLINE_CRITICAL
956 scalar_type
957 read_element( this_quaternion const & x )
958 {
959 BOOST_QVM_STATIC_ASSERT(I>=0);
960 BOOST_QVM_STATIC_ASSERT(I<4);
961 return x.a[I];
962 }
963 };
964
965 template <class V>
966 struct
967 deduce_quat< qvm_detail::rot_quat_<V> >
968 {
969 typedef quat<typename vec_traits<V>::scalar_type> type;
970 };
971
972 template <class A,class Angle>
973 BOOST_QVM_INLINE
974 typename enable_if_c<
975 is_vec<A>::value && vec_traits<A>::dim==3,
976 qvm_detail::rot_quat_<A> >::type
977 rot_quat( A const & axis, Angle angle )
978 {
979 return qvm_detail::rot_quat_<A>(axis,angle);
980 }
981
982 template <class A,class B,class Angle>
983 BOOST_QVM_INLINE_OPERATIONS
984 typename enable_if_c<
985 is_quat<A>::value &&
986 is_vec<B>::value && vec_traits<B>::dim==3,
987 void>::type
988 set_rot( A & a, B const & axis, Angle angle )
989 {
990 assign(a,rot_quat(axis,angle));
991 }
992
993 template <class A,class B,class Angle>
994 BOOST_QVM_INLINE_OPERATIONS
995 typename enable_if_c<
996 is_quat<A>::value &&
997 is_vec<B>::value && vec_traits<B>::dim==3,
998 void>::type
999 rotate( A & a, B const & axis, Angle angle )
1000 {
1001 a *= rot_quat(axis,angle);
1002 }
1003
1004 ////////////////////////////////////////////////
1005
1006 namespace
1007 qvm_detail
1008 {
1009 template <class T>
1010 struct
1011 rotx_quat_
1012 {
1013 BOOST_QVM_INLINE_TRIVIAL
1014 rotx_quat_()
1015 {
1016 }
1017
1018 template <class R>
1019 BOOST_QVM_INLINE_TRIVIAL
1020 operator R() const
1021 {
1022 R r;
1023 assign(r,*this);
1024 return r;
1025 }
1026
1027 private:
1028
1029 rotx_quat_( rotx_quat_ const & );
1030 rotx_quat_ & operator=( rotx_quat_ const & );
1031 ~rotx_quat_();
1032 };
1033
1034 template <int I>
1035 struct
1036 rotx_q_get
1037 {
1038 template <class T>
1039 static
1040 BOOST_QVM_INLINE_CRITICAL
1041 T
1042 get( T const & )
1043 {
1044 return scalar_traits<T>::value(0);
1045 }
1046 };
1047
1048 template <>
1049 struct
1050 rotx_q_get<1>
1051 {
1052 template <class T>
1053 static
1054 BOOST_QVM_INLINE_CRITICAL
1055 T
1056 get( T const & angle )
1057 {
1058 return sin<T>(angle/2);
1059 }
1060 };
1061
1062 template <>
1063 struct
1064 rotx_q_get<0>
1065 {
1066 template <class T>
1067 static
1068 BOOST_QVM_INLINE_CRITICAL
1069 T
1070 get( T const & angle )
1071 {
1072 return cos<T>(angle/2);
1073 }
1074 };
1075 }
1076
1077 template <class Angle>
1078 struct
1079 quat_traits< qvm_detail::rotx_quat_<Angle> >
1080 {
1081 typedef qvm_detail::rotx_quat_<Angle> this_quaternion;
1082 typedef Angle scalar_type;
1083
1084 template <int I>
1085 static
1086 BOOST_QVM_INLINE_CRITICAL
1087 scalar_type
1088 read_element( this_quaternion const & x )
1089 {
1090 BOOST_QVM_STATIC_ASSERT(I>=0);
1091 BOOST_QVM_STATIC_ASSERT(I<4);
1092 return qvm_detail::rotx_q_get<I>::get(reinterpret_cast<Angle const &>(x));
1093 }
1094 };
1095
1096 template <class Angle>
1097 struct
1098 deduce_quat< qvm_detail::rotx_quat_<Angle> >
1099 {
1100 typedef quat<Angle> type;
1101 };
1102
1103 template <class Angle>
1104 struct
1105 deduce_quat2< qvm_detail::rotx_quat_<Angle>, qvm_detail::rotx_quat_<Angle> >
1106 {
1107 typedef quat<Angle> type;
1108 };
1109
1110 template <class Angle>
1111 BOOST_QVM_INLINE_TRIVIAL
1112 qvm_detail::rotx_quat_<Angle> const &
1113 rotx_quat( Angle const & angle )
1114 {
1115 return reinterpret_cast<qvm_detail::rotx_quat_<Angle> const &>(angle);
1116 }
1117
1118 template <class A,class Angle>
1119 BOOST_QVM_INLINE_OPERATIONS
1120 typename enable_if_c<
1121 is_quat<A>::value,
1122 void>::type
1123 set_rotx( A & a, Angle angle )
1124 {
1125 assign(a,rotx_quat(angle));
1126 }
1127
1128 template <class A,class Angle>
1129 BOOST_QVM_INLINE_OPERATIONS
1130 typename enable_if_c<
1131 is_quat<A>::value,
1132 void>::type
1133 rotate_x( A & a, Angle angle )
1134 {
1135 a *= rotx_quat(angle);
1136 }
1137
1138 ////////////////////////////////////////////////
1139
1140 namespace
1141 qvm_detail
1142 {
1143 template <class T>
1144 struct
1145 roty_quat_
1146 {
1147 BOOST_QVM_INLINE_TRIVIAL
1148 roty_quat_()
1149 {
1150 }
1151
1152 template <class R>
1153 BOOST_QVM_INLINE_TRIVIAL
1154 operator R() const
1155 {
1156 R r;
1157 assign(r,*this);
1158 return r;
1159 }
1160
1161 private:
1162
1163 roty_quat_( roty_quat_ const & );
1164 roty_quat_ & operator=( roty_quat_ const & );
1165 ~roty_quat_();
1166 };
1167
1168 template <int I>
1169 struct
1170 roty_q_get
1171 {
1172 template <class T>
1173 static
1174 BOOST_QVM_INLINE_CRITICAL
1175 T
1176 get( T const & )
1177 {
1178 return scalar_traits<T>::value(0);
1179 }
1180 };
1181
1182 template <>
1183 struct
1184 roty_q_get<2>
1185 {
1186 template <class T>
1187 static
1188 BOOST_QVM_INLINE_CRITICAL
1189 T
1190 get( T const & angle )
1191 {
1192 return sin<T>(angle/2);
1193 }
1194 };
1195
1196 template <>
1197 struct
1198 roty_q_get<0>
1199 {
1200 template <class T>
1201 static
1202 BOOST_QVM_INLINE_CRITICAL
1203 T
1204 get( T const & angle )
1205 {
1206 return cos<T>(angle/2);
1207 }
1208 };
1209 }
1210
1211 template <class Angle>
1212 struct
1213 quat_traits< qvm_detail::roty_quat_<Angle> >
1214 {
1215 typedef qvm_detail::roty_quat_<Angle> this_quaternion;
1216 typedef Angle scalar_type;
1217
1218 template <int I>
1219 static
1220 BOOST_QVM_INLINE_CRITICAL
1221 scalar_type
1222 read_element( this_quaternion const & x )
1223 {
1224 BOOST_QVM_STATIC_ASSERT(I>=0);
1225 BOOST_QVM_STATIC_ASSERT(I<4);
1226 return qvm_detail::roty_q_get<I>::get(reinterpret_cast<Angle const &>(x));
1227 }
1228 };
1229
1230 template <class Angle>
1231 struct
1232 deduce_quat< qvm_detail::roty_quat_<Angle> >
1233 {
1234 typedef quat<Angle> type;
1235 };
1236
1237 template <class Angle>
1238 struct
1239 deduce_quat2< qvm_detail::roty_quat_<Angle>, qvm_detail::roty_quat_<Angle> >
1240 {
1241 typedef quat<Angle> type;
1242 };
1243
1244 template <class Angle>
1245 BOOST_QVM_INLINE_TRIVIAL
1246 qvm_detail::roty_quat_<Angle> const &
1247 roty_quat( Angle const & angle )
1248 {
1249 return reinterpret_cast<qvm_detail::roty_quat_<Angle> const &>(angle);
1250 }
1251
1252 template <class A,class Angle>
1253 BOOST_QVM_INLINE_OPERATIONS
1254 typename enable_if_c<
1255 is_quat<A>::value,
1256 void>::type
1257 set_roty( A & a, Angle angle )
1258 {
1259 assign(a,roty_quat(angle));
1260 }
1261
1262 template <class A,class Angle>
1263 BOOST_QVM_INLINE_OPERATIONS
1264 typename enable_if_c<
1265 is_quat<A>::value,
1266 void>::type
1267 rotate_y( A & a, Angle angle )
1268 {
1269 a *= roty_quat(angle);
1270 }
1271
1272 ////////////////////////////////////////////////
1273
1274 namespace
1275 qvm_detail
1276 {
1277 template <class T>
1278 struct
1279 rotz_quat_
1280 {
1281 BOOST_QVM_INLINE_TRIVIAL
1282 rotz_quat_()
1283 {
1284 }
1285
1286 template <class R>
1287 BOOST_QVM_INLINE_TRIVIAL
1288 operator R() const
1289 {
1290 R r;
1291 assign(r,*this);
1292 return r;
1293 }
1294
1295 private:
1296
1297 rotz_quat_( rotz_quat_ const & );
1298 rotz_quat_ & operator=( rotz_quat_ const & );
1299 ~rotz_quat_();
1300 };
1301
1302 template <int I>
1303 struct
1304 rotz_q_get
1305 {
1306 template <class T>
1307 static
1308 BOOST_QVM_INLINE_CRITICAL
1309 T
1310 get( T const & )
1311 {
1312 return scalar_traits<T>::value(0);
1313 }
1314 };
1315
1316 template <>
1317 struct
1318 rotz_q_get<3>
1319 {
1320 template <class T>
1321 static
1322 BOOST_QVM_INLINE_CRITICAL
1323 T
1324 get( T const & angle )
1325 {
1326 return sin<T>(angle/2);
1327 }
1328 };
1329
1330 template <>
1331 struct
1332 rotz_q_get<0>
1333 {
1334 template <class T>
1335 static
1336 BOOST_QVM_INLINE_CRITICAL
1337 T
1338 get( T const & angle )
1339 {
1340 return cos<T>(angle/2);
1341 }
1342 };
1343 }
1344
1345 template <class Angle>
1346 struct
1347 quat_traits< qvm_detail::rotz_quat_<Angle> >
1348 {
1349 typedef qvm_detail::rotz_quat_<Angle> this_quaternion;
1350 typedef Angle scalar_type;
1351
1352 template <int I>
1353 static
1354 BOOST_QVM_INLINE_CRITICAL
1355 scalar_type
1356 read_element( this_quaternion const & x )
1357 {
1358 BOOST_QVM_STATIC_ASSERT(I>=0);
1359 BOOST_QVM_STATIC_ASSERT(I<4);
1360 return qvm_detail::rotz_q_get<I>::get(reinterpret_cast<Angle const &>(x));
1361 }
1362 };
1363
1364 template <class Angle>
1365 struct
1366 deduce_quat< qvm_detail::rotz_quat_<Angle> >
1367 {
1368 typedef quat<Angle> type;
1369 };
1370
1371 template <class Angle>
1372 struct
1373 deduce_quat2< qvm_detail::rotz_quat_<Angle>, qvm_detail::rotz_quat_<Angle> >
1374 {
1375 typedef quat<Angle> type;
1376 };
1377
1378 template <class Angle>
1379 BOOST_QVM_INLINE_TRIVIAL
1380 qvm_detail::rotz_quat_<Angle> const &
1381 rotz_quat( Angle const & angle )
1382 {
1383 return reinterpret_cast<qvm_detail::rotz_quat_<Angle> const &>(angle);
1384 }
1385
1386 template <class A,class Angle>
1387 BOOST_QVM_INLINE_OPERATIONS
1388 typename enable_if_c<
1389 is_quat<A>::value,
1390 void>::type
1391 set_rotz( A & a, Angle angle )
1392 {
1393 assign(a,rotz_quat(angle));
1394 }
1395
1396 template <class A,class Angle>
1397 BOOST_QVM_INLINE_OPERATIONS
1398 typename enable_if_c<
1399 is_quat<A>::value,
1400 void>::type
1401 rotate_z( A & a, Angle angle )
1402 {
1403 a *= rotz_quat(angle);
1404 }
1405
1406 template <class A,class B>
1407 BOOST_QVM_INLINE_OPERATIONS
1408 typename enable_if_c<
1409 is_quat<A>::value && is_vec<B>::value && vec_traits<B>::dim==3,
1410 typename quat_traits<A>::scalar_type>::type
1411 axis_angle( A const & a, B & b )
1412 {
1413 typedef typename quat_traits<A>::scalar_type T;
1414 T a0=quat_traits<A>::template read_element<0>(a);
1415 T a1=quat_traits<A>::template read_element<1>(a);
1416 T a2=quat_traits<A>::template read_element<2>(a);
1417 T a3=quat_traits<A>::template read_element<3>(a);
1418 if( a0>1 )
1419 {
1420 T const m2=a0*a0+a1*a1+a2*a2+a3*a3;
1421 if( m2==scalar_traits<T>::value(0) )
1422 BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
1423 T const s=sqrt<T>(m2);
1424 a0/=s;
1425 a1/=s;
1426 a2/=s;
1427 a3/=s;
1428 }
1429 if( T s=sqrt<T>(1-a0*a0) )
1430 {
1431 vec_traits<B>::template write_element<0>(b) = a1/s;
1432 vec_traits<B>::template write_element<1>(b) = a2/s;
1433 vec_traits<B>::template write_element<2>(b) = a3/s;
1434 }
1435 else
1436 {
1437 typedef typename vec_traits<B>::scalar_type U;
1438 vec_traits<B>::template write_element<0>(b) = scalar_traits<U>::value(1);
1439 vec_traits<B>::template write_element<1>(b) = vec_traits<B>::template write_element<2>(b) = scalar_traits<U>::value(0);
1440 }
1441 return scalar_traits<T>::value(2) * qvm::acos(a0);
1442 }
1443
1444 ////////////////////////////////////////////////
1445
1446 namespace
1447 sfinae
1448 {
1449 using ::boost::qvm::assign;
1450 using ::boost::qvm::cmp;
1451 using ::boost::qvm::convert_to;
1452 using ::boost::qvm::conjugate;
1453 using ::boost::qvm::set_identity;
1454 using ::boost::qvm::set_zero;
1455 using ::boost::qvm::scalar_cast;
1456 using ::boost::qvm::operator/=;
1457 using ::boost::qvm::operator/;
1458 using ::boost::qvm::dot;
1459 using ::boost::qvm::operator==;
1460 using ::boost::qvm::inverse;
1461 using ::boost::qvm::mag_sqr;
1462 using ::boost::qvm::mag;
1463 using ::boost::qvm::slerp;
1464 using ::boost::qvm::operator-=;
1465 using ::boost::qvm::operator-;
1466 using ::boost::qvm::operator*=;
1467 using ::boost::qvm::operator*;
1468 using ::boost::qvm::operator!=;
1469 using ::boost::qvm::normalized;
1470 using ::boost::qvm::normalize;
1471 using ::boost::qvm::operator+=;
1472 using ::boost::qvm::operator+;
1473 using ::boost::qvm::qref;
1474 using ::boost::qvm::rot_quat;
1475 using ::boost::qvm::set_rot;
1476 using ::boost::qvm::rotate;
1477 using ::boost::qvm::rotx_quat;
1478 using ::boost::qvm::set_rotx;
1479 using ::boost::qvm::rotate_x;
1480 using ::boost::qvm::roty_quat;
1481 using ::boost::qvm::set_roty;
1482 using ::boost::qvm::rotate_y;
1483 using ::boost::qvm::rotz_quat;
1484 using ::boost::qvm::set_rotz;
1485 using ::boost::qvm::rotate_z;
1486 }
1487
1488 ////////////////////////////////////////////////
1489 }
1490 }
1491
1492#endif
Ceph