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

source code of boost/libs/qvm/include/boost/qvm/quat_operations.hpp