]> git.proxmox.com Git - ceph.git/blame - ceph/src/boost/boost/qvm/mat_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 / mat_operations.hpp
CommitLineData
92f5a8d4 1//Copyright (c) 2008-2016 Emil Dotchevski and Reverge Studios, Inc.
f67539c2 2//Copyright (c) 2019 agate-pris
92f5a8d4
TL		 3
4//Distributed under the Boost Software License, Version 1.0. (See accompanying
5//file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
6
f67539c2
TL		 7#ifndef BOOST_QVM_4F915D9ED30A11DF962186E3DFD72085
8#define BOOST_QVM_4F915D9ED30A11DF962186E3DFD72085
92f5a8d4
TL		 9
10#include <boost/qvm/detail/mat_assign.hpp>
11#include <boost/qvm/mat_operations2.hpp>
12#include <boost/qvm/mat_operations3.hpp>
13#include <boost/qvm/mat_operations4.hpp>
14#include <boost/qvm/math.hpp>
15#include <boost/qvm/detail/determinant_impl.hpp>
16#include <boost/qvm/detail/cofactor_impl.hpp>
17#include <boost/qvm/detail/transp_impl.hpp>
18#include <boost/qvm/scalar_traits.hpp>
19#include <string>
20
21namespace
22boost
23 {
24 namespace
25 qvm
26 {
27 namespace
28 qvm_detail
29 {
30 BOOST_QVM_INLINE_CRITICAL
31 void const *
32 get_valid_ptr_mat_operations()
33 {
34 static int const obj=0;
35 return &obj;
36 }
37 }
38
39 ////////////////////////////////////////////////
40
41 namespace
42 qvm_to_string_detail
43 {
44 template <class T>
45 std::string to_string( T const & x );
46 }
47
48 namespace
49 qvm_detail
50 {
51 template <int R,int C>
52 struct
53 to_string_m_defined
54 {
55 static bool const value=false;
56 };
57
58 template <int I,int SizeMinusOne>
59 struct
60 to_string_matrix_elements
61 {
62 template <class A>
63 static
64 std::string
65 f( A const & a )
66 {
67 using namespace qvm_to_string_detail;
68 return
69 ( (I%mat_traits<A>::cols)==0 ? '(' : ',' ) +
70 to_string(mat_traits<A>::template read_element<I/mat_traits<A>::cols,I%mat_traits<A>::cols>(a)) +
71 ( (I%mat_traits<A>::cols)==mat_traits<A>::cols-1 ? ")" : "" ) +
72 to_string_matrix_elements<I+1,SizeMinusOne>::f(a);
73 }
74 };
75
76 template <int SizeMinusOne>
77 struct
78 to_string_matrix_elements<SizeMinusOne,SizeMinusOne>
79 {
80 template <class A>
81 static
82 std::string
83 f( A const & a )
84 {
85 using namespace qvm_to_string_detail;
86 return
87 ( (SizeMinusOne%mat_traits<A>::cols)==0 ? '(' : ',' ) +
88 to_string(mat_traits<A>::template read_element<SizeMinusOne/mat_traits<A>::cols,SizeMinusOne%mat_traits<A>::cols>(a)) +
89 ')';
90 }
91 };
92 }
93
94 template <class A>
95 inline
96 typename boost::enable_if_c<
97 is_mat<A>::value &&
98 !qvm_detail::to_string_m_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value,
99 std::string>::type
100 to_string( A const & a )
101 {
102 return "("+qvm_detail::to_string_matrix_elements<0,mat_traits<A>::rows*mat_traits<A>::cols-1>::f(a)+')';
103 }
104
105 ////////////////////////////////////////////////
106
107 template <class A,class B,class Cmp>
108 BOOST_QVM_INLINE_OPERATIONS
109 typename enable_if_c<
110 is_mat<A>::value && is_mat<B>::value &&
111 mat_traits<A>::rows==mat_traits<B>::rows &&
112 mat_traits<A>::cols==mat_traits<B>::cols,
113 bool>::type
114 cmp( A const & a, B const & b, Cmp f )
115 {
f67539c2
TL		 116 for( int i=0; i!=mat_traits<A>::rows; ++i )
117 for( int j=0; j!=mat_traits<A>::cols; ++j )
118 if( !f(
119 mat_traits<A>::read_element_idx(i, j, a),
120 mat_traits<B>::read_element_idx(i, j, b)) )
92f5a8d4
TL		 121 return false;
122 return true;
123 }
124
125 ////////////////////////////////////////////////
126
127 namespace
128 qvm_detail
129 {
130 template <int M,int N>
131 struct
132 convert_to_m_defined
133 {
134 static bool const value=false;
135 };
136 }
137
138 template <class R,class A>
139 BOOST_QVM_INLINE_TRIVIAL
140 typename enable_if_c<
141 is_mat<R>::value && is_mat<A>::value &&
142 mat_traits<R>::rows==mat_traits<A>::rows &&
143 mat_traits<R>::cols==mat_traits<A>::cols &&
144 !qvm_detail::convert_to_m_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value,
145 R>::type
146 convert_to( A const & a )
147 {
148 R r; assign(r,a);
149 return r;
150 }
151
152 ////////////////////////////////////////////////
153
154 namespace
155 qvm_detail
156 {
157 template <int D>
158 struct
159 determinant_defined
160 {
161 static bool const value=false;
162 };
163 }
164
165 template <class A>
166 BOOST_QVM_INLINE_TRIVIAL
167 typename enable_if_c<
168 is_mat<A>::value &&
169 mat_traits<A>::rows==mat_traits<A>::cols &&
170 !qvm_detail::determinant_defined<mat_traits<A>::rows>::value,
171 typename mat_traits<A>::scalar_type>::type
172 determinant( A const & a )
173 {
174 return qvm_detail::determinant_impl(a);
175 }
176
177 ////////////////////////////////////////////////
178
179 namespace
180 qvm_detail
181 {
182 template <class T,int Dim>
183 class
184 identity_mat_
185 {
186 identity_mat_( identity_mat_ const & );
187 identity_mat_ & operator=( identity_mat_ const & );
188 ~identity_mat_();
189
190 public:
191
192 template <class R>
193 BOOST_QVM_INLINE_TRIVIAL
194 operator R() const
195 {
196 R r;
197 assign(r,*this);
198 return r;
199 }
200 };
201 }
202
203 template <class T,int Dim>
204 struct
205 mat_traits< qvm_detail::identity_mat_<T,Dim> >
206 {
207 typedef qvm_detail::identity_mat_<T,Dim> this_matrix;
208 typedef T scalar_type;
209 static int const rows=Dim;
210 static int const cols=Dim;
211
212 template <int Row,int Col>
213 static
214 BOOST_QVM_INLINE_CRITICAL
215 scalar_type
216 read_element( this_matrix const & /*x*/ )
217 {
218 BOOST_QVM_STATIC_ASSERT(Row>=0);
219 BOOST_QVM_STATIC_ASSERT(Row<Dim);
220 BOOST_QVM_STATIC_ASSERT(Col>=0);
221 BOOST_QVM_STATIC_ASSERT(Col<Dim);
222 return scalar_traits<scalar_type>::value(Row==Col);
223 }
224
225 static
226 BOOST_QVM_INLINE_CRITICAL
227 scalar_type
228 read_element_idx( int row, int col, this_matrix const & /*x*/ )
229 {
230 BOOST_QVM_ASSERT(row>=0);
231 BOOST_QVM_ASSERT(row<Dim);
232 BOOST_QVM_ASSERT(col>=0);
233 BOOST_QVM_ASSERT(col<Dim);
234 return scalar_traits<scalar_type>::value(row==col);
235 }
236 };
237
238 template <class T,int Dim>
239 BOOST_QVM_INLINE_TRIVIAL
240 qvm_detail::identity_mat_<T,Dim> const &
241 identity_mat()
242 {
243 return *(qvm_detail::identity_mat_<T,Dim> const *)qvm_detail::get_valid_ptr_mat_operations();
244 }
245
246 template <class A>
247 BOOST_QVM_INLINE_OPERATIONS
248 typename enable_if_c<
249 is_mat<A>::value &&
250 mat_traits<A>::rows==mat_traits<A>::cols,
251 void>::type
252 set_identity( A & a )
253 {
254 assign(a,identity_mat<typename mat_traits<A>::scalar_type,mat_traits<A>::rows>());
255 }
256
257 ////////////////////////////////////////////////
258
259 namespace
260 qvm_detail
261 {
262 template <class T>
263 struct
264 projection_
265 {
266 T const _00;
267 T const _11;
268 T const _22;
269 T const _23;
270 T const _32;
271
272 BOOST_QVM_INLINE_TRIVIAL
273 projection_( T _00, T _11, T _22, T _23, T _32 ):
274 _00(_00),
275 _11(_11),
276 _22(_22),
277 _23(_23),
278 _32(_32)
279 {
280 }
281
282 template <class R>
283 BOOST_QVM_INLINE_TRIVIAL
284 operator R() const
285 {
286 R r;
287 assign(r,*this);
288 return r;
289 }
290 };
291
292 template <int Row,int Col>
293 struct
294 projection_get
295 {
296 template <class T>
297 static
298 BOOST_QVM_INLINE_CRITICAL
299 T
300 get( projection_<T> const & )
301 {
302 return scalar_traits<T>::value(0);
303 }
304 };
305
306 template <> struct projection_get<0,0> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( projection_<T> const & m ) { return m._00; } };
307 template <> struct projection_get<1,1> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( projection_<T> const & m ) { return m._11; } };
308 template <> struct projection_get<2,2> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( projection_<T> const & m ) { return m._22; } };
309 template <> struct projection_get<2,3> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( projection_<T> const & m ) { return m._23; } };
310 template <> struct projection_get<3,2> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( projection_<T> const & m ) { return m._32; } };
311 }
312
313 template <class T>
314 struct
315 mat_traits< qvm_detail::projection_<T> >
316 {
317 typedef qvm_detail::projection_<T> this_matrix;
318 typedef T scalar_type;
319 static int const rows=4;
320 static int const cols=4;
321
322 template <int Row,int Col>
323 static
324 BOOST_QVM_INLINE_CRITICAL
325 scalar_type
326 read_element( this_matrix const & x )
327 {
328 BOOST_QVM_STATIC_ASSERT(Row>=0);
329 BOOST_QVM_STATIC_ASSERT(Row<rows);
330 BOOST_QVM_STATIC_ASSERT(Col>=0);
331 BOOST_QVM_STATIC_ASSERT(Col<cols);
332 return qvm_detail::projection_get<Row,Col>::get(x);
333 }
334 };
335
336 template <class T>
337 qvm_detail::projection_<T>
338 BOOST_QVM_INLINE_OPERATIONS
339 perspective_lh( T fov_y, T aspect_ratio, T z_near, T z_far )
340 {
341 T const one = scalar_traits<T>::value(1);
342 T const ys = one/tan<T>(fov_y/scalar_traits<T>::value(2));
343 T const xs = ys/aspect_ratio;
344 T const zd = z_far-z_near;
345 T const z1 = z_far/zd;
346 T const z2 = -z_near*z1;
347 return qvm_detail::projection_<T>(xs,ys,z1,z2,one);
348 }
349
350 template <class T>
351 qvm_detail::projection_<T>
352 BOOST_QVM_INLINE_OPERATIONS
353 perspective_rh( T fov_y, T aspect_ratio, T z_near, T z_far )
354 {
355 T const one = scalar_traits<T>::value(1);
356 T const ys = one/tan<T>(fov_y/scalar_traits<T>::value(2));
357 T const xs = ys/aspect_ratio;
358 T const zd = z_near-z_far;
359 T const z1 = z_far/zd;
360 T const z2 = z_near*z1;
361 return qvm_detail::projection_<T>(xs,ys,z1,z2,-one);
362 }
363
364 ////////////////////////////////////////////////
365
366 namespace
367 qvm_detail
368 {
369 template <class OriginalType,class Scalar>
370 class
371 matrix_scalar_cast_
372 {
373 matrix_scalar_cast_( matrix_scalar_cast_ const & );
374 matrix_scalar_cast_ & operator=( matrix_scalar_cast_ const & );
375 ~matrix_scalar_cast_();
376
377 public:
378
379 template <class T>
380 BOOST_QVM_INLINE_TRIVIAL
381 matrix_scalar_cast_ &
382 operator=( T const & x )
383 {
384 assign(*this,x);
385 return *this;
386 }
387
388 template <class R>
389 BOOST_QVM_INLINE_TRIVIAL
390 operator R() const
391 {
392 R r;
393 assign(r,*this);
394 return r;
395 }
396 };
397
398 template <bool> struct scalar_cast_matrix_filter { };
399 template <> struct scalar_cast_matrix_filter<true> { typedef int type; };
400 }
401
402 template <class OriginalType,class Scalar>
403 struct
404 mat_traits< qvm_detail::matrix_scalar_cast_<OriginalType,Scalar> >
405 {
406 typedef Scalar scalar_type;
407 typedef qvm_detail::matrix_scalar_cast_<OriginalType,Scalar> this_matrix;
408 static int const rows=mat_traits<OriginalType>::rows;
409 static int const cols=mat_traits<OriginalType>::cols;
410
411 template <int Row,int Col>
412 static
413 BOOST_QVM_INLINE_CRITICAL
414 scalar_type
415 read_element( this_matrix const & x )
416 {
417 BOOST_QVM_STATIC_ASSERT(Row>=0);
418 BOOST_QVM_STATIC_ASSERT(Row<rows);
419 BOOST_QVM_STATIC_ASSERT(Col>=0);
420 BOOST_QVM_STATIC_ASSERT(Col<cols);
421 return scalar_type(mat_traits<OriginalType>::template read_element<Row,Col>(reinterpret_cast<OriginalType const &>(x)));
422 }
423
424 static
425 BOOST_QVM_INLINE_CRITICAL
426 scalar_type
427 read_element_idx( int row, int col, this_matrix const & x )
428 {
429 BOOST_QVM_ASSERT(row>=0);
430 BOOST_QVM_ASSERT(row<rows);
431 BOOST_QVM_ASSERT(col>=0);
432 BOOST_QVM_ASSERT(col<cols);
433 return scalar_type(mat_traits<OriginalType>::read_element_idx(col,row,reinterpret_cast<OriginalType const &>(x)));
434 }
435 };
436
437 template <class OriginalType,class Scalar,int R,int C>
438 struct
439 deduce_mat<qvm_detail::matrix_scalar_cast_<OriginalType,Scalar>,R,C>
440 {
441 typedef mat<Scalar,R,C> type;
442 };
443
444 template <class Scalar,class T>
445 BOOST_QVM_INLINE_TRIVIAL
446 qvm_detail::matrix_scalar_cast_<T,Scalar> const &
447 scalar_cast( T const & x, typename qvm_detail::scalar_cast_matrix_filter<is_mat<T>::value>::type=0 )
448 {
449 return reinterpret_cast<qvm_detail::matrix_scalar_cast_<T,Scalar> const &>(x);
450 }
451
452 ////////////////////////////////////////////////
453
454 namespace
455 qvm_detail
456 {
457 template <int M,int N>
458 struct
459 div_eq_ms_defined
460 {
461 static bool const value=false;
462 };
463 }
464
465 template <class A,class B>
466 BOOST_QVM_INLINE_OPERATIONS
467 typename enable_if_c<
468 is_mat<A>::value && is_scalar<B>::value &&
469 !qvm_detail::div_eq_ms_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value,
470 A &>::type
471 operator/=( A & a, B b )
472 {
473 for( int i=0; i!=mat_traits<A>::rows; ++i )
474 for( int j=0; j!=mat_traits<A>::cols; ++j )
475 mat_traits<A>::write_element_idx(i,j,a)/=b;
476 return a;
477 }
478
479 ////////////////////////////////////////////////
480
481 namespace
482 qvm_detail
483 {
484 template <int M,int N>
485 struct
486 div_ms_defined
487 {
488 static bool const value=false;
489 };
490 }
491
492 template <class A,class B>
493 BOOST_QVM_INLINE_OPERATIONS
494 typename lazy_enable_if_c<
495 is_mat<A>::value && is_scalar<B>::value &&
496 !qvm_detail::div_ms_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value,
497 deduce_mat<A> >::type
498 operator/( A const & a, B b )
499 {
500 typedef typename deduce_mat<A>::type R;
501 R r;
502 for( int i=0; i!=mat_traits<A>::rows; ++i )
503 for( int j=0; j!=mat_traits<A>::cols; ++j )
504 mat_traits<R>::write_element_idx(i,j,r)=mat_traits<A>::read_element_idx(i,j,a)/b;
505 return r;
506 }
507
508 ////////////////////////////////////////////////
509
510 namespace
511 qvm_detail
512 {
513 template <int M,int N>
514 struct
515 eq_mm_defined
516 {
517 static bool const value=false;
518 };
519 }
520
521 template <class A,class B>
522 BOOST_QVM_INLINE_OPERATIONS
523 typename enable_if_c<
524 is_mat<A>::value && is_mat<B>::value &&
525 mat_traits<A>::rows==mat_traits<B>::rows &&
526 mat_traits<A>::cols==mat_traits<B>::cols &&
527 !qvm_detail::eq_mm_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value,
528 bool>::type
529 operator==( A const & a, B const & b )
530 {
531 for( int i=0; i!=mat_traits<A>::rows; ++i )
532 for( int j=0; j!=mat_traits<A>::cols; ++j )
533 if( mat_traits<A>::read_element_idx(i,j,a)!=mat_traits<B>::read_element_idx(i,j,b) )
534 return false;
535 return true;
536 }
537
538 ////////////////////////////////////////////////
539
540 namespace
541 qvm_detail
542 {
543 template <int M,int N>
544 struct
545 minus_eq_mm_defined
546 {
547 static bool const value=false;
548 };
549 }
550
551 template <class A,class B>
552 BOOST_QVM_INLINE_OPERATIONS
553 typename enable_if_c<
554 is_mat<A>::value && is_mat<B>::value &&
555 mat_traits<A>::rows==mat_traits<B>::rows &&
556 mat_traits<A>::cols==mat_traits<B>::cols &&
557 !qvm_detail::minus_eq_mm_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value,
558 A &>::type
559 operator-=( A & a, B const & b )
560 {
561 for( int i=0; i!=mat_traits<A>::rows; ++i )
562 for( int j=0; j!=mat_traits<A>::cols; ++j )
563 mat_traits<A>::write_element_idx(i,j,a)-=mat_traits<B>::read_element_idx(i,j,b);
564 return a;
565 }
566
567 ////////////////////////////////////////////////
568
569 namespace
570 qvm_detail
571 {
572 template <int M,int N>
573 struct
574 minus_m_defined
575 {
576 static bool const value=false;
577 };
578 }
579
580 template <class A>
581 BOOST_QVM_INLINE_OPERATIONS
582 typename lazy_enable_if_c<
583 is_mat<A>::value &&
584 !qvm_detail::minus_m_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value,
585 deduce_mat<A> >::type
586 operator-( A const & a )
587 {
588 typedef typename deduce_mat<A>::type R;
589 R r;
590 for( int i=0; i!=mat_traits<A>::rows; ++i )
591 for( int j=0; j!=mat_traits<A>::cols; ++j )
592 mat_traits<R>::write_element_idx(i,j,r)=-mat_traits<A>::read_element_idx(i,j,a);
593 return r;
594 }
595
596 ////////////////////////////////////////////////
597
598 namespace
599 qvm_detail
600 {
601 template <int M,int N>
602 struct
603 minus_mm_defined
604 {
605 static bool const value=false;
606 };
607 }
608
609 template <class A,class B>
610 BOOST_QVM_INLINE_OPERATIONS
611 typename lazy_enable_if_c<
612 is_mat<A>::value && is_mat<B>::value &&
613 mat_traits<A>::rows==mat_traits<B>::rows &&
614 mat_traits<A>::cols==mat_traits<B>::cols &&
615 !qvm_detail::minus_mm_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value,
616 deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<A>::cols> >::type
617 operator-( A const & a, B const & b )
618 {
619 typedef typename deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<A>::cols>::type R;
620 R r;
621 for( int i=0; i!=mat_traits<A>::rows; ++i )
622 for( int j=0; j!=mat_traits<A>::cols; ++j )
623 mat_traits<R>::write_element_idx(i,j,r)=mat_traits<A>::read_element_idx(i,j,a)-mat_traits<B>::read_element_idx(i,j,b);
624 return r;
625 }
626
627 ////////////////////////////////////////////////
628
629 namespace
630 qvm_detail
631 {
632 template <int D>
633 struct
634 mul_eq_mm_defined
635 {
636 static bool const value=false;
637 };
638 }
639
640 template <class A,class B>
641 BOOST_QVM_INLINE_OPERATIONS
642 typename enable_if_c<
643 is_mat<A>::value &&
644 is_mat<B>::value &&
645 mat_traits<A>::rows==mat_traits<A>::cols &&
646 mat_traits<A>::rows==mat_traits<B>::rows &&
647 mat_traits<A>::cols==mat_traits<B>::cols &&
648 !qvm_detail::mul_eq_mm_defined<mat_traits<A>::rows>::value,
649 A &>::type
650 operator*=( A & r, B const & b )
651 {
652 typedef typename mat_traits<A>::scalar_type Ta;
653 Ta a[mat_traits<A>::rows][mat_traits<A>::cols];
654 for( int i=0; i<mat_traits<A>::rows; ++i )
655 for( int j=0; j<mat_traits<B>::cols; ++j )
656 a[i][j]=mat_traits<A>::read_element_idx(i,j,r);
657 for( int i=0; i<mat_traits<A>::rows; ++i )
658 for( int j=0; j<mat_traits<B>::cols; ++j )
659 {
660 Ta x(scalar_traits<Ta>::value(0));
661 for( int k=0; k<mat_traits<A>::cols; ++k )
662 x += a[i][k]*mat_traits<B>::read_element_idx(k,j,b);
663 mat_traits<A>::write_element_idx(i,j,r) = x;
664 }
665 return r;
666 }
667
668 ////////////////////////////////////////////////
669
670 namespace
671 qvm_detail
672 {
673 template <int M,int N>
674 struct
675 mul_eq_ms_defined
676 {
677 static bool const value=false;
678 };
679 }
680
681 template <class A,class B>
682 BOOST_QVM_INLINE_OPERATIONS
683 typename enable_if_c<
684 is_mat<A>::value && is_scalar<B>::value &&
685 !qvm_detail::mul_eq_ms_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value,
686 A &>::type
687 operator*=( A & a, B b )
688 {
689 for( int i=0; i!=mat_traits<A>::rows; ++i )
690 for( int j=0; j!=mat_traits<A>::cols; ++j )
691 mat_traits<A>::write_element_idx(i,j,a)*=b;
692 return a;
693 }
694
695 ////////////////////////////////////////////////
696
697 namespace
698 qvm_detail
699 {
f67539c2 700 template <int R,int /*CR*/,int C>
92f5a8d4
TL		 701 struct
702 mul_mm_defined
703 {
704 static bool const value=false;
705 };
706 }
707
708 template <class A,class B>
709 BOOST_QVM_INLINE_OPERATIONS
710 typename lazy_enable_if_c<
711 is_mat<A>::value && is_mat<B>::value &&
712 mat_traits<A>::cols==mat_traits<B>::rows &&
713 !qvm_detail::mul_mm_defined<mat_traits<A>::rows,mat_traits<A>::cols,mat_traits<B>::cols>::value,
714 deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<B>::cols> >::type
715 operator*( A const & a, B const & b )
716 {
717 typedef typename deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<B>::cols>::type R;
718 R r;
719 for( int i=0; i<mat_traits<A>::rows; ++i )
720 for( int j=0; j<mat_traits<B>::cols; ++j )
721 {
722 typedef typename mat_traits<A>::scalar_type Ta;
723 Ta x(scalar_traits<Ta>::value(0));
724 for( int k=0; k<mat_traits<A>::cols; ++k )
725 x += mat_traits<A>::read_element_idx(i,k,a)*mat_traits<B>::read_element_idx(k,j,b);
726 mat_traits<R>::write_element_idx(i,j,r) = x;
727 }
728 return r;
729 }
730
731 ////////////////////////////////////////////////
732
733 namespace
734 qvm_detail
735 {
736 template <int M,int N>
737 struct
738 mul_ms_defined
739 {
740 static bool const value=false;
741 };
742 }
743
744 template <class A,class B>
745 BOOST_QVM_INLINE_OPERATIONS
746 typename lazy_enable_if_c<
747 is_mat<A>::value && is_scalar<B>::value &&
748 !qvm_detail::mul_ms_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value,
749 deduce_mat<A> >::type
750 operator*( A const & a, B b )
751 {
752 typedef typename deduce_mat<A>::type R;
753 R r;
754 for( int i=0; i!=mat_traits<A>::rows; ++i )
755 for( int j=0; j!=mat_traits<A>::cols; ++j )
756 mat_traits<R>::write_element_idx(i,j,r)=mat_traits<A>::read_element_idx(i,j,a)*b;
757 return r;
758 }
759
760 ////////////////////////////////////////////////
761
762 namespace
763 qvm_detail
764 {
765 template <int M,int N>
766 struct
767 mul_sm_defined
768 {
769 static bool const value=false;
770 };
771 }
772
773 template <class A,class B>
774 BOOST_QVM_INLINE_OPERATIONS
775 typename lazy_enable_if_c<
776 is_scalar<A>::value && is_mat<B>::value &&
777 !qvm_detail::mul_sm_defined<mat_traits<B>::rows,mat_traits<B>::cols>::value,
778 deduce_mat<B> >::type
779 operator*( A a, B const & b )
780 {
781 typedef typename deduce_mat<B>::type R;
782 R r;
783 for( int i=0; i!=mat_traits<B>::rows; ++i )
784 for( int j=0; j!=mat_traits<B>::cols; ++j )
785 mat_traits<R>::write_element_idx(i,j,r)=a*mat_traits<B>::read_element_idx(i,j,b);
786 return r;
787 }
788
789 ////////////////////////////////////////////////
790
791 namespace
792 qvm_detail
793 {
794 template <int M,int N>
795 struct
796 neq_mm_defined
797 {
798 static bool const value=false;
799 };
800 }
801
802 template <class A,class B>
803 BOOST_QVM_INLINE_OPERATIONS
804 typename enable_if_c<
805 is_mat<A>::value && is_mat<B>::value &&
806 mat_traits<A>::rows==mat_traits<B>::rows &&
807 mat_traits<A>::cols==mat_traits<B>::cols &&
808 !qvm_detail::neq_mm_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value,
809 bool>::type
810 operator!=( A const & a, B const & b )
811 {
812 for( int i=0; i!=mat_traits<A>::rows; ++i )
813 for( int j=0; j!=mat_traits<A>::cols; ++j )
814 if( mat_traits<A>::read_element_idx(i,j,a)!=mat_traits<B>::read_element_idx(i,j,b) )
815 return true;
816 return false;
817 }
818
819 ////////////////////////////////////////////////
820
821 namespace
822 qvm_detail
823 {
824 template <int M,int N>
825 struct
826 plus_eq_mm_defined
827 {
828 static bool const value=false;
829 };
830 }
831
832 template <class A,class B>
833 BOOST_QVM_INLINE_OPERATIONS
834 typename enable_if_c<
835 is_mat<A>::value && is_mat<B>::value &&
836 mat_traits<A>::rows==mat_traits<B>::rows &&
837 mat_traits<A>::cols==mat_traits<B>::cols &&
838 !qvm_detail::plus_eq_mm_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value,
839 A &>::type
840 operator+=( A & a, B const & b )
841 {
842 for( int i=0; i!=mat_traits<A>::rows; ++i )
843 for( int j=0; j!=mat_traits<A>::cols; ++j )
844 mat_traits<A>::write_element_idx(i,j,a)+=mat_traits<B>::read_element_idx(i,j,b);
845 return a;
846 }
847
848 ////////////////////////////////////////////////
849
850 namespace
851 qvm_detail
852 {
853 template <int M,int N>
854 struct
855 plus_mm_defined
856 {
857 static bool const value=false;
858 };
859 }
860
861 template <class A,class B>
862 BOOST_QVM_INLINE_OPERATIONS
863 typename lazy_enable_if_c<
864 is_mat<A>::value && is_mat<B>::value &&
865 mat_traits<A>::rows==mat_traits<B>::rows &&
866 mat_traits<A>::cols==mat_traits<B>::cols &&
867 !qvm_detail::plus_mm_defined<mat_traits<A>::rows,mat_traits<A>::cols>::value,
868 deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<A>::cols> >::type
869 operator+( A const & a, B const & b )
870 {
871 typedef typename deduce_mat2<A,B,mat_traits<A>::rows,mat_traits<A>::cols>::type R;
872 R r;
873 for( int i=0; i!=mat_traits<A>::rows; ++i )
874 for( int j=0; j!=mat_traits<A>::cols; ++j )
875 mat_traits<R>::write_element_idx(i,j,r)=mat_traits<A>::read_element_idx(i,j,a)+mat_traits<B>::read_element_idx(i,j,b);
876 return r;
877 }
878
879 ////////////////////////////////////////////////
880
881 namespace
882 qvm_detail
883 {
884 template <class T>
885 class
886 mref_
887 {
888 mref_( mref_ const & );
889 mref_ & operator=( mref_ const & );
890 ~mref_();
891
892 public:
893
894 template <class R>
895 BOOST_QVM_INLINE_TRIVIAL
896 mref_ &
897 operator=( R const & x )
898 {
899 assign(*this,x);
900 return *this;
901 }
902
903 template <class R>
904 BOOST_QVM_INLINE_TRIVIAL
905 operator R() const
906 {
907 R r;
908 assign(r,*this);
909 return r;
910 }
911 };
912 }
913
914 template <class M>
915 struct
916 mat_traits< qvm_detail::mref_<M> >
917 {
918 typedef typename mat_traits<M>::scalar_type scalar_type;
919 typedef qvm_detail::mref_<M> this_matrix;
920 static int const rows=mat_traits<M>::rows;
921 static int const cols=mat_traits<M>::cols;
922
923 template <int Row,int Col>
924 static
925 BOOST_QVM_INLINE_CRITICAL
926 scalar_type
927 read_element( this_matrix const & x )
928 {
929 BOOST_QVM_STATIC_ASSERT(Row>=0);
930 BOOST_QVM_STATIC_ASSERT(Row<rows);
931 BOOST_QVM_STATIC_ASSERT(Col>=0);
932 BOOST_QVM_STATIC_ASSERT(Col<cols);
933 return mat_traits<M>::template read_element<Row,Col>(reinterpret_cast<M const &>(x));
934 }
935
936 template <int Row,int Col>
937 static
938 BOOST_QVM_INLINE_CRITICAL
939 scalar_type &
940 write_element( this_matrix & x )
941 {
942 BOOST_QVM_STATIC_ASSERT(Row>=0);
943 BOOST_QVM_STATIC_ASSERT(Row<rows);
944 BOOST_QVM_STATIC_ASSERT(Col>=0);
945 BOOST_QVM_STATIC_ASSERT(Col<cols);
946 return mat_traits<M>::template write_element<Row,Col>(reinterpret_cast<M &>(x));
947 }
948
949 static
950 BOOST_QVM_INLINE_CRITICAL
951 scalar_type
952 read_element_idx( int row, int col, this_matrix const & x )
953 {
954 BOOST_QVM_ASSERT(row>=0);
955 BOOST_QVM_ASSERT(row<rows);
956 BOOST_QVM_ASSERT(col>=0);
957 BOOST_QVM_ASSERT(col<cols);
958 return mat_traits<M>::read_element_idx(row,col,reinterpret_cast<M const &>(x));
959 }
960
961 static
962 BOOST_QVM_INLINE_CRITICAL
963 scalar_type &
964 write_element_idx( int row, int col, this_matrix & x )
965 {
966 BOOST_QVM_ASSERT(row>=0);
967 BOOST_QVM_ASSERT(row<rows);
968 BOOST_QVM_ASSERT(col>=0);
969 BOOST_QVM_ASSERT(col<cols);
970 return mat_traits<M>::write_element_idx(row,col,reinterpret_cast<M &>(x));
971 }
972 };
973
974 template <class M,int R,int C>
975 struct
976 deduce_mat<qvm_detail::mref_<M>,R,C>
977 {
978 typedef mat<typename mat_traits<M>::scalar_type,R,C> type;
979 };
980
981 template <class M>
982 BOOST_QVM_INLINE_TRIVIAL
983 typename enable_if_c<
984 is_mat<M>::value,
985 qvm_detail::mref_<M> const &>::type
986 mref( M const & a )
987 {
988 return reinterpret_cast<qvm_detail::mref_<M> const &>(a);
989 }
990
991 template <class M>
992 BOOST_QVM_INLINE_TRIVIAL
993 typename enable_if_c<
994 is_mat<M>::value,
995 qvm_detail::mref_<M> &>::type
996 mref( M & a )
997 {
998 return reinterpret_cast<qvm_detail::mref_<M> &>(a);
999 }
1000
1001 ////////////////////////////////////////////////
1002
1003 namespace
1004 qvm_detail
1005 {
1006 template <class T,int Rows,int Cols>
1007 class
1008 zero_mat_
1009 {
1010 zero_mat_( zero_mat_ const & );
1011 zero_mat_ & operator=( zero_mat_ const & );
1012 ~zero_mat_();
1013
1014 public:
1015
1016 template <class R>
1017 BOOST_QVM_INLINE_TRIVIAL
1018 operator R() const
1019 {
1020 R r;
1021 assign(r,*this);
1022 return r;
1023 }
1024 };
1025 }
1026
1027 template <class T,int Rows,int Cols>
1028 struct
1029 mat_traits< qvm_detail::zero_mat_<T,Rows,Cols> >
1030 {
1031 typedef qvm_detail::zero_mat_<T,Rows,Cols> this_matrix;
1032 typedef T scalar_type;
1033 static int const rows=Rows;
1034 static int const cols=Cols;
1035
1036 template <int Row,int Col>
1037 static
1038 BOOST_QVM_INLINE_CRITICAL
1039 scalar_type
1040 read_element( this_matrix const & )
1041 {
1042 BOOST_QVM_STATIC_ASSERT(Row>=0);
1043 BOOST_QVM_STATIC_ASSERT(Row<Rows);
1044 BOOST_QVM_STATIC_ASSERT(Col>=0);
1045 BOOST_QVM_STATIC_ASSERT(Col<Cols);
1046 return scalar_traits<scalar_type>::value(0);
1047 }
1048
1049 static
1050 BOOST_QVM_INLINE_CRITICAL
1051 scalar_type
1052 read_element_idx( int row, int col, this_matrix const & )
1053 {
1054 BOOST_QVM_ASSERT(row>=0);
1055 BOOST_QVM_ASSERT(row<rows);
1056 BOOST_QVM_ASSERT(col>=0);
1057 BOOST_QVM_ASSERT(col<cols);
1058 return scalar_traits<scalar_type>::value(0);
1059 }
1060 };
1061
1062 template <class T,int Rows,int Cols,int R,int C>
1063 struct
1064 deduce_mat<qvm_detail::zero_mat_<T,Rows,Cols>,R,C>
1065 {
1066 typedef mat<T,R,C> type;
1067 };
1068
1069 template <class T,int Rows,int Cols>
1070 BOOST_QVM_INLINE_TRIVIAL
1071 qvm_detail::zero_mat_<T,Rows,Cols> const &
1072 zero_mat()
1073 {
1074 return *(qvm_detail::zero_mat_<T,Rows,Cols> const *)qvm_detail::get_valid_ptr_mat_operations();
1075 }
1076
1077 template <class T,int Dim>
1078 BOOST_QVM_INLINE_TRIVIAL
1079 qvm_detail::zero_mat_<T,Dim,Dim> const &
1080 zero_mat()
1081 {
1082 return *(qvm_detail::zero_mat_<T,Dim,Dim> const *)qvm_detail::get_valid_ptr_mat_operations();
1083 }
1084
1085 template <class A>
1086 BOOST_QVM_INLINE_OPERATIONS
1087 typename enable_if_c<
1088 is_mat<A>::value,
1089 void>::type
1090 set_zero( A & a )
1091 {
1092 assign(a,zero_mat<typename mat_traits<A>::scalar_type,mat_traits<A>::rows,mat_traits<A>::cols>());
1093 }
1094
1095 ////////////////////////////////////////////////
1096
1097 namespace
1098 qvm_detail
1099 {
1100 template <int D,class S>
1101 struct
1102 rot_mat_
1103 {
1104 typedef S scalar_type;
1105 scalar_type a[3][3];
1106
1107 BOOST_QVM_INLINE
1108 rot_mat_(
1109 scalar_type a00, scalar_type a01, scalar_type a02,
1110 scalar_type a10, scalar_type a11, scalar_type a12,
1111 scalar_type a20, scalar_type a21, scalar_type a22 )
1112 {
1113 a[0][0] = a00;
1114 a[0][1] = a01;
1115 a[0][2] = a02;
1116 a[1][0] = a10;
1117 a[1][1] = a11;
1118 a[1][2] = a12;
1119 a[2][0] = a20;
1120 a[2][1] = a21;
1121 a[2][2] = a22;
1122 }
1123
1124 template <class R>
1125 BOOST_QVM_INLINE_TRIVIAL
1126 operator R() const
1127 {
1128 R r;
1129 assign(r,*this);
1130 return r;
1131 }
1132 };
1133
1134 template <int Row,int Col>
1135 struct
1136 rot_m_get
1137 {
1138 template <class T>
1139 static
1140 BOOST_QVM_INLINE_CRITICAL
1141 T
1142 get( T const (&)[3][3] )
1143 {
1144 return scalar_traits<T>::value(Row==Col);
1145 }
1146 };
1147
1148 template <> struct rot_m_get<0,0> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[0][0]; } };
1149 template <> struct rot_m_get<0,1> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[0][1]; } };
1150 template <> struct rot_m_get<0,2> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[0][2]; } };
1151 template <> struct rot_m_get<1,0> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[1][0]; } };
1152 template <> struct rot_m_get<1,1> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[1][1]; } };
1153 template <> struct rot_m_get<1,2> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[1][2]; } };
1154 template <> struct rot_m_get<2,0> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[2][0]; } };
1155 template <> struct rot_m_get<2,1> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[2][1]; } };
1156 template <> struct rot_m_get<2,2> { template <class T> static BOOST_QVM_INLINE_CRITICAL T get( T const (&a)[3][3] ) { return a[2][2]; } };
1157 }
1158
1159 template <class M>
1160 struct mat_traits;
1161
1162 template <int D,class S>
1163 struct
1164 mat_traits< qvm_detail::rot_mat_<D,S> >
1165 {
1166 typedef qvm_detail::rot_mat_<D,S> this_matrix;
1167 typedef typename this_matrix::scalar_type scalar_type;
1168 static int const rows=D;
1169 static int const cols=D;
1170
1171 template <int Row,int Col>
1172 static
1173 BOOST_QVM_INLINE_CRITICAL
1174 scalar_type
1175 read_element( this_matrix const & x )
1176 {
1177 BOOST_QVM_STATIC_ASSERT(Row>=0);
1178 BOOST_QVM_STATIC_ASSERT(Row<D);
1179 BOOST_QVM_STATIC_ASSERT(Col>=0);
1180 BOOST_QVM_STATIC_ASSERT(Col<D);
1181 return qvm_detail::rot_m_get<Row,Col>::get(x.a);
1182 }
1183
1184 static
1185 BOOST_QVM_INLINE_CRITICAL
1186 scalar_type
1187 read_element_idx( int row, int col, this_matrix const & x )
1188 {
1189 BOOST_QVM_ASSERT(row>=0);
1190 BOOST_QVM_ASSERT(row<D);
1191 BOOST_QVM_ASSERT(col>=0);
1192 BOOST_QVM_ASSERT(col<D);
1193 return row<3 && col<3?
1194 x.a[row][col] :
1195 scalar_traits<scalar_type>::value(row==col);
1196 }
1197 };
1198
1199 template <int Dim,class V,class Angle>
1200 BOOST_QVM_INLINE
1201 typename enable_if_c<
1202 is_vec<V>::value && vec_traits<V>::dim==3,
1203 qvm_detail::rot_mat_<Dim,Angle> >::type
1204 rot_mat( V const & axis, Angle angle )
1205 {
1206 typedef Angle scalar_type;
1207 scalar_type const x=vec_traits<V>::template read_element<0>(axis);
1208 scalar_type const y=vec_traits<V>::template read_element<1>(axis);
1209 scalar_type const z=vec_traits<V>::template read_element<2>(axis);
1210 scalar_type const m2=x*x+y*y+z*z;
1211 if( m2==scalar_traits<scalar_type>::value(0) )
1212 BOOST_QVM_THROW_EXCEPTION(zero_magnitude_error());
1213 scalar_type const s = sin<scalar_type>(angle);
1214 scalar_type const c = cos<scalar_type>(angle);
1215 scalar_type const x2 = x*x;
1216 scalar_type const y2 = y*y;
1217 scalar_type const z2 = z*z;
1218 scalar_type const xy = x*y;
1219 scalar_type const xz = x*z;
1220 scalar_type const yz = y*z;
1221 scalar_type const xs = x*s;
1222 scalar_type const ys = y*s;
1223 scalar_type const zs = z*s;
1224 scalar_type const one = scalar_traits<scalar_type>::value(1);
1225 scalar_type const c1 = one-c;
1226 return qvm_detail::rot_mat_<Dim,Angle>(
1227 x2+(one-x2)*c, xy*c1-zs, xz*(one-c)+ys,
1228 xy*c1+zs, y2+(one-y2)*c, yz*c1-xs,
1229 xz*c1-ys, yz*c1+xs, z2+(one-z2)*c );
1230 }
1231
1232 template <class A,class B,class Angle>
1233 BOOST_QVM_INLINE_OPERATIONS
1234 typename enable_if_c<
1235 is_mat<A>::value &&
1236 mat_traits<A>::rows==mat_traits<A>::cols &&
1237 mat_traits<A>::rows>=3 &&
1238 is_vec<B>::value && vec_traits<B>::dim==3,
1239 void>::type
1240 set_rot( A & a, B const & axis, Angle angle )
1241 {
1242 assign(a,rot_mat<mat_traits<A>::rows>(axis,angle));
1243 }
1244
1245 template <class A,class B,class Angle>
1246 BOOST_QVM_INLINE_OPERATIONS
1247 typename enable_if_c<
1248 is_mat<A>::value &&
1249 mat_traits<A>::rows==mat_traits<A>::cols &&
1250 mat_traits<A>::rows>=3 &&
1251 is_vec<B>::value && vec_traits<B>::dim==3,
1252 void>::type
1253 rotate( A & a, B const & axis, Angle angle )
1254 {
1255 a *= rot_mat<mat_traits<A>::rows>(axis,angle);
1256 }
1257
1258 ////////////////////////////////////////////////
1259
1260 template <int Dim,class Angle>
1261 BOOST_QVM_INLINE
1262 qvm_detail::rot_mat_<Dim,Angle>
1263 rot_mat_xzy( Angle x1, Angle z2, Angle y3 )
1264 {
1265 typedef Angle scalar_type;
1266 scalar_type const c1 = cos<scalar_type>(x1);
1267 scalar_type const s1 = sin<scalar_type>(x1);
1268 scalar_type const c2 = cos<scalar_type>(z2);
1269 scalar_type const s2 = sin<scalar_type>(z2);
1270 scalar_type const c3 = cos<scalar_type>(y3);
1271 scalar_type const s3 = sin<scalar_type>(y3);
1272 return qvm_detail::rot_mat_<Dim,Angle>(
1273 c2*c3, -s2, c2*s3,
1274 s1*s3 + c1*c3*s2, c1*c2, c1*s2*s3 - c3*s1,
1275 c3*s1*s2 - c1*s3, c2*s1, c1*c3 + s1*s2*s3 );
1276 }
1277
1278 template <class A,class Angle>
1279 BOOST_QVM_INLINE_OPERATIONS
1280 typename enable_if_c<
1281 is_mat<A>::value &&
1282 mat_traits<A>::rows==mat_traits<A>::cols &&
1283 mat_traits<A>::rows>=3,
1284 void>::type
1285 set_rot_xzy( A & a, Angle x1, Angle z2, Angle y3 )
1286 {
1287 assign(a,rot_mat_xzy<mat_traits<A>::rows>(x1,z2,y3));
1288 }
1289
1290 template <class A,class Angle>
1291 BOOST_QVM_INLINE_OPERATIONS
1292 typename enable_if_c<
1293 is_mat<A>::value &&
1294 mat_traits<A>::rows==mat_traits<A>::cols &&
1295 mat_traits<A>::rows>=3,
1296 void>::type
1297 rotate_xzy( A & a, Angle x1, Angle z2, Angle y3 )
1298 {
1299 a *= rot_mat_xzy<mat_traits<A>::rows>(x1,z2,y3);
1300 }
1301
1302 ////////////////////////////////////////////////
1303
1304 template <int Dim,class Angle>
1305 BOOST_QVM_INLINE
1306 qvm_detail::rot_mat_<Dim,Angle>
1307 rot_mat_xyz( Angle x1, Angle y2, Angle z3 )
1308 {
1309 typedef Angle scalar_type;
1310 scalar_type const c1 = cos<scalar_type>(x1);
1311 scalar_type const s1 = sin<scalar_type>(x1);
1312 scalar_type const c2 = cos<scalar_type>(y2);
1313 scalar_type const s2 = sin<scalar_type>(y2);
1314 scalar_type const c3 = cos<scalar_type>(z3);
1315 scalar_type const s3 = sin<scalar_type>(z3);
1316 return qvm_detail::rot_mat_<Dim,Angle>(
1317 c2*c3, -c2*s3, s2,
1318 c1*s3 + c3*s1*s2, c1*c3 - s1*s2*s3, -c2*s1,
1319 s1*s3 - c1*c3*s2, c3*s1 + c1*s2*s3, c1*c2 );
1320 }
1321
1322 template <class A,class Angle>
1323 BOOST_QVM_INLINE_OPERATIONS
1324 typename enable_if_c<
1325 is_mat<A>::value &&
1326 mat_traits<A>::rows==mat_traits<A>::cols &&
1327 mat_traits<A>::rows>=3,
1328 void>::type
1329 set_rot_xyz( A & a, Angle x1, Angle y2, Angle z3 )
1330 {
1331 assign(a,rot_mat_xyz<mat_traits<A>::rows>(x1,y2,z3));
1332 }
1333
1334 template <class A,class Angle>
1335 BOOST_QVM_INLINE_OPERATIONS
1336 typename enable_if_c<
1337 is_mat<A>::value &&
1338 mat_traits<A>::rows==mat_traits<A>::cols &&
1339 mat_traits<A>::rows>=3,
1340 void>::type
1341 rotate_xyz( A & a, Angle x1, Angle y2, Angle z3 )
1342 {
1343 a *= rot_mat_xyz<mat_traits<A>::rows>(x1,y2,z3);
1344 }
1345
1346 ////////////////////////////////////////////////
1347
1348 template <int Dim,class Angle>
1349 BOOST_QVM_INLINE
1350 qvm_detail::rot_mat_<Dim,Angle>
1351 rot_mat_yxz( Angle y1, Angle x2, Angle z3 )
1352 {
1353 typedef Angle scalar_type;
1354 scalar_type const c1 = cos<scalar_type>(y1);
1355 scalar_type const s1 = sin<scalar_type>(y1);
1356 scalar_type const c2 = cos<scalar_type>(x2);
1357 scalar_type const s2 = sin<scalar_type>(x2);
1358 scalar_type const c3 = cos<scalar_type>(z3);
1359 scalar_type const s3 = sin<scalar_type>(z3);
1360 return qvm_detail::rot_mat_<Dim,Angle>(
1361 c1*c3 + s1*s2*s3, c3*s1*s2 - c1*s3, c2*s1,
1362 c2*s3, c2*c3, -s2,
1363 c1*s2*s3 - c3*s1, c1*c3*s2 + s1*s3, c1*c2 );
1364 }
1365
1366 template <class A,class Angle>
1367 BOOST_QVM_INLINE_OPERATIONS
1368 typename enable_if_c<
1369 is_mat<A>::value &&
1370 mat_traits<A>::rows==mat_traits<A>::cols &&
1371 mat_traits<A>::rows>=3,
1372 void>::type
1373 set_rot_yxz( A & a, Angle y1, Angle x2, Angle z3 )
1374 {
1375 assign(a,rot_mat_yxz<mat_traits<A>::rows>(y1,x2,z3));
1376 }
1377
1378 template <class A,class Angle>
1379 BOOST_QVM_INLINE_OPERATIONS
1380 typename enable_if_c<
1381 is_mat<A>::value &&
1382 mat_traits<A>::rows==mat_traits<A>::cols &&
1383 mat_traits<A>::rows>=3,
1384 void>::type
1385 rotate_yxz( A & a, Angle y1, Angle x2, Angle z3 )
1386 {
1387 a *= rot_mat_yxz<mat_traits<A>::rows>(y1,x2,z3);
1388 }
1389
1390 ////////////////////////////////////////////////
1391
1392 template <int Dim,class Angle>
1393 BOOST_QVM_INLINE
1394 qvm_detail::rot_mat_<Dim,Angle>
1395 rot_mat_yzx( Angle y1, Angle z2, Angle x3 )
1396 {
1397 typedef Angle scalar_type;
1398 scalar_type const c1 = cos<scalar_type>(y1);
1399 scalar_type const s1 = sin<scalar_type>(y1);
1400 scalar_type const c2 = cos<scalar_type>(z2);
1401 scalar_type const s2 = sin<scalar_type>(z2);
1402 scalar_type const c3 = cos<scalar_type>(x3);
1403 scalar_type const s3 = sin<scalar_type>(x3);
1404 return qvm_detail::rot_mat_<Dim,Angle>(
1405 c1*c2, s1*s3 - c1*c3*s2, c3*s1 + c1*s2*s3,
1406 s2, c2*c3, -c2*s3,
1407 -c2*s1, c1*s3 + c3*s1*s2, c1*c3 - s1*s2*s3 );
1408 }
1409
1410 template <class A,class Angle>
1411 BOOST_QVM_INLINE_OPERATIONS
1412 typename enable_if_c<
1413 is_mat<A>::value &&
1414 mat_traits<A>::rows==mat_traits<A>::cols &&
1415 mat_traits<A>::rows>=3,
1416 void>::type
1417 set_rot_yzx( A & a, Angle y1, Angle z2, Angle x3 )
1418 {
1419 assign(a,rot_mat_yzx<mat_traits<A>::rows>(y1,z2,x3));
1420 }
1421
1422 template <class A,class Angle>
1423 BOOST_QVM_INLINE_OPERATIONS
1424 typename enable_if_c<
1425 is_mat<A>::value &&
1426 mat_traits<A>::rows==mat_traits<A>::cols &&
1427 mat_traits<A>::rows>=3,
1428 void>::type
1429 rotate_yzx( A & a, Angle y1, Angle z2, Angle x3 )
1430 {
1431 a *= rot_mat_yzx<mat_traits<A>::rows>(y1,z2,x3);
1432 }
1433
1434 ////////////////////////////////////////////////
1435
1436 template <int Dim,class Angle>
1437 BOOST_QVM_INLINE
1438 qvm_detail::rot_mat_<Dim,Angle>
1439 rot_mat_zyx( Angle z1, Angle y2, Angle x3 )
1440 {
1441 typedef Angle scalar_type;
1442 scalar_type const c1 = cos<scalar_type>(z1);
1443 scalar_type const s1 = sin<scalar_type>(z1);
1444 scalar_type const c2 = cos<scalar_type>(y2);
1445 scalar_type const s2 = sin<scalar_type>(y2);
1446 scalar_type const c3 = cos<scalar_type>(x3);
1447 scalar_type const s3 = sin<scalar_type>(x3);
1448 return qvm_detail::rot_mat_<Dim,Angle>(
1449 c1*c2, c1*s2*s3 - c3*s1, s1*s3 + c1*c3*s2,
1450 c2*s1, c1*c3 + s1*s2*s3, c3*s1*s2 - c1*s3,
1451 -s2, c2*s3, c2*c3 );
1452 }
1453
1454 template <class A,class Angle>
1455 BOOST_QVM_INLINE_OPERATIONS
1456 typename enable_if_c<
1457 is_mat<A>::value &&
1458 mat_traits<A>::rows==mat_traits<A>::cols &&
1459 mat_traits<A>::rows>=3,
1460 void>::type
1461 set_rot_zyx( A & a, Angle z1, Angle y2, Angle x3 )
1462 {
1463 assign(a,rot_mat_zyx<mat_traits<A>::rows>(z1,y2,x3));
1464 }
1465
1466 template <class A,class Angle>
1467 BOOST_QVM_INLINE_OPERATIONS
1468 typename enable_if_c<
1469 is_mat<A>::value &&
1470 mat_traits<A>::rows==mat_traits<A>::cols &&
1471 mat_traits<A>::rows>=3,
1472 void>::type
1473 rotate_zyx( A & a, Angle z1, Angle y2, Angle x3 )
1474 {
1475 a *= rot_mat_zyx<mat_traits<A>::rows>(z1,y2,x3);
1476 }
1477
1478 ////////////////////////////////////////////////
1479
1480 template <int Dim,class Angle>
1481 BOOST_QVM_INLINE
1482 qvm_detail::rot_mat_<Dim,Angle>
1483 rot_mat_zxy( Angle z1, Angle x2, Angle y3 )
1484 {
1485 typedef Angle scalar_type;
1486 scalar_type const c1 = cos<scalar_type>(z1);
1487 scalar_type const s1 = sin<scalar_type>(z1);
1488 scalar_type const c2 = cos<scalar_type>(x2);
1489 scalar_type const s2 = sin<scalar_type>(x2);
1490 scalar_type const c3 = cos<scalar_type>(y3);
1491 scalar_type const s3 = sin<scalar_type>(y3);
1492 return qvm_detail::rot_mat_<Dim,Angle>(
1493 c1*c3 - s1*s2*s3, -c2*s1, c1*s3 + c3*s1*s2,
1494 c3*s1 + c1*s2*s3, c1*c2, s1*s3 - c1*c3*s2,
1495 -c2*s3, s2, c2*c3 );
1496 }
1497
1498 template <class A,class Angle>
1499 BOOST_QVM_INLINE_OPERATIONS
1500 typename enable_if_c<
1501 is_mat<A>::value &&
1502 mat_traits<A>::rows==mat_traits<A>::cols &&
1503 mat_traits<A>::rows>=3,
1504 void>::type
1505 set_rot_zxy( A & a, Angle z1, Angle x2, Angle y3 )
1506 {
1507 assign(a,rot_mat_zxy<mat_traits<A>::rows>(z1,x2,y3));
1508 }
1509
1510 template <class A,class Angle>
1511 BOOST_QVM_INLINE_OPERATIONS
1512 typename enable_if_c<
1513 is_mat<A>::value &&
1514 mat_traits<A>::rows==mat_traits<A>::cols &&
1515 mat_traits<A>::rows>=3,
1516 void>::type
1517 rotate_zxy( A & a, Angle z1, Angle x2, Angle y3 )
1518 {
1519 a *= rot_mat_zxy<mat_traits<A>::rows>(z1,x2,y3);
1520 }
1521
1522 ////////////////////////////////////////////////
1523
1524 template <int Dim,class Angle>
1525 BOOST_QVM_INLINE
1526 qvm_detail::rot_mat_<Dim,Angle>
1527 rot_mat_xzx( Angle x1, Angle z2, Angle x3 )
1528 {
1529 typedef Angle scalar_type;
1530 scalar_type const c1 = cos<scalar_type>(x1);
1531 scalar_type const s1 = sin<scalar_type>(x1);
1532 scalar_type const c2 = cos<scalar_type>(z2);
1533 scalar_type const s2 = sin<scalar_type>(z2);
1534 scalar_type const c3 = cos<scalar_type>(x3);
1535 scalar_type const s3 = sin<scalar_type>(x3);
1536 return qvm_detail::rot_mat_<Dim,Angle>(
1537 c2, -c3*s2, s2*s3,
1538 c1*s2, c1*c2*c3 - s1*s3, -c3*s1 - c1*c2*s3,
1539 s1*s2, c1*s3 + c2*c3*s1, c1*c3 - c2*s1*s3 );
1540 }
1541
1542 template <class A,class Angle>
1543 BOOST_QVM_INLINE_OPERATIONS
1544 typename enable_if_c<
1545 is_mat<A>::value &&
1546 mat_traits<A>::rows==mat_traits<A>::cols &&
1547 mat_traits<A>::rows>=3,
1548 void>::type
1549 set_rot_xzx( A & a, Angle x1, Angle z2, Angle x3 )
1550 {
1551 assign(a,rot_mat_xzx<mat_traits<A>::rows>(x1,z2,x3));
1552 }
1553
1554 template <class A,class Angle>
1555 BOOST_QVM_INLINE_OPERATIONS
1556 typename enable_if_c<
1557 is_mat<A>::value &&
1558 mat_traits<A>::rows==mat_traits<A>::cols &&
1559 mat_traits<A>::rows>=3,
1560 void>::type
1561 rotate_xzx( A & a, Angle x1, Angle z2, Angle x3 )
1562 {
1563 a *= rot_mat_xzx<mat_traits<A>::rows>(x1,z2,x3);
1564 }
1565
1566 ////////////////////////////////////////////////
1567
1568 template <int Dim,class Angle>
1569 BOOST_QVM_INLINE
1570 qvm_detail::rot_mat_<Dim,Angle>
1571 rot_mat_xyx( Angle x1, Angle y2, Angle x3 )
1572 {
1573 typedef Angle scalar_type;
1574 scalar_type const c1 = cos<scalar_type>(x1);
1575 scalar_type const s1 = sin<scalar_type>(x1);
1576 scalar_type const c2 = cos<scalar_type>(y2);
1577 scalar_type const s2 = sin<scalar_type>(y2);
1578 scalar_type const c3 = cos<scalar_type>(x3);
1579 scalar_type const s3 = sin<scalar_type>(x3);
1580 return qvm_detail::rot_mat_<Dim,Angle>(
1581 c2, s2*s3, c3*s2,
1582 s1*s2, c1*c3 - c2*s1*s3, -c1*s3 - c2*c3*s1,
1583 -c1*s2, c3*s1 + c1*c2*s3, c1*c2*c3 - s1*s3 );
1584 }
1585
1586 template <class A,class Angle>
1587 BOOST_QVM_INLINE_OPERATIONS
1588 typename enable_if_c<
1589 is_mat<A>::value &&
1590 mat_traits<A>::rows==mat_traits<A>::cols &&
1591 mat_traits<A>::rows>=3,
1592 void>::type
1593 set_rot_xyx( A & a, Angle x1, Angle y2, Angle x3 )
1594 {
1595 assign(a,rot_mat_xyx<mat_traits<A>::rows>(x1,y2,x3));
1596 }
1597
1598 template <class A,class Angle>
1599 BOOST_QVM_INLINE_OPERATIONS
1600 typename enable_if_c<
1601 is_mat<A>::value &&
1602 mat_traits<A>::rows==mat_traits<A>::cols &&
1603 mat_traits<A>::rows>=3,
1604 void>::type
1605 rotate_xyx( A & a, Angle x1, Angle y2, Angle x3 )
1606 {
1607 a *= rot_mat_xyx<mat_traits<A>::rows>(x1,y2,x3);
1608 }
1609
1610 ////////////////////////////////////////////////
1611
1612 template <int Dim,class Angle>
1613 BOOST_QVM_INLINE
1614 qvm_detail::rot_mat_<Dim,Angle>
1615 rot_mat_yxy( Angle y1, Angle x2, Angle y3 )
1616 {
1617 typedef Angle scalar_type;
1618 scalar_type const c1 = cos<scalar_type>(y1);
1619 scalar_type const s1 = sin<scalar_type>(y1);
1620 scalar_type const c2 = cos<scalar_type>(x2);
1621 scalar_type const s2 = sin<scalar_type>(x2);
1622 scalar_type const c3 = cos<scalar_type>(y3);
1623 scalar_type const s3 = sin<scalar_type>(y3);
1624 return qvm_detail::rot_mat_<Dim,Angle>(
1625 c1*c3 - c2*s1*s3, s1*s2, c1*s3 + c2*c3*s1,
1626 s2*s3, c2, -c3*s2,
1627 -c3*s1 - c1*c2*s3, c1*s2, c1*c2*c3 - s1*s3 );
1628 }
1629
1630 template <class A,class Angle>
1631 BOOST_QVM_INLINE_OPERATIONS
1632 typename enable_if_c<
1633 is_mat<A>::value &&
1634 mat_traits<A>::rows==mat_traits<A>::cols &&
1635 mat_traits<A>::rows>=3,
1636 void>::type
1637 set_rot_yxy( A & a, Angle y1, Angle x2, Angle y3 )
1638 {
1639 assign(a,rot_mat_yxy<mat_traits<A>::rows>(y1,x2,y3));
1640 }
1641
1642 template <class A,class Angle>
1643 BOOST_QVM_INLINE_OPERATIONS
1644 typename enable_if_c<
1645 is_mat<A>::value &&
1646 mat_traits<A>::rows==mat_traits<A>::cols &&
1647 mat_traits<A>::rows>=3,
1648 void>::type
1649 rotate_yxy( A & a, Angle y1, Angle x2, Angle y3 )
1650 {
1651 a *= rot_mat_yxy<mat_traits<A>::rows>(y1,x2,y3);
1652 }
1653
1654 ////////////////////////////////////////////////
1655
1656 template <int Dim,class Angle>
1657 BOOST_QVM_INLINE
1658 qvm_detail::rot_mat_<Dim,Angle>
1659 rot_mat_yzy( Angle y1, Angle z2, Angle y3 )
1660 {
1661 typedef Angle scalar_type;
1662 scalar_type const c1 = cos<scalar_type>(y1);
1663 scalar_type const s1 = sin<scalar_type>(y1);
1664 scalar_type const c2 = cos<scalar_type>(z2);
1665 scalar_type const s2 = sin<scalar_type>(z2);
1666 scalar_type const c3 = cos<scalar_type>(y3);
1667 scalar_type const s3 = sin<scalar_type>(y3);
1668 return qvm_detail::rot_mat_<Dim,Angle>(
1669 c1*c2*c3 - s1*s3, -c1*s2, c3*s1 + c1*c2*s3,
1670 c3*s2, c2, s2*s3,
1671 -c1*s3 - c2*c3*s1, s1*s2, c1*c3 - c2*s1*s3 );
1672 }
1673
1674 template <class A,class Angle>
1675 BOOST_QVM_INLINE_OPERATIONS
1676 typename enable_if_c<
1677 is_mat<A>::value &&
1678 mat_traits<A>::rows==mat_traits<A>::cols &&
1679 mat_traits<A>::rows>=3,
1680 void>::type
1681 set_rot_yzy( A & a, Angle y1, Angle z2, Angle y3 )
1682 {
1683 assign(a,rot_mat_yzy<mat_traits<A>::rows>(y1,z2,y3));
1684 }
1685
1686 template <class A,class Angle>
1687 BOOST_QVM_INLINE_OPERATIONS
1688 typename enable_if_c<
1689 is_mat<A>::value &&
1690 mat_traits<A>::rows==mat_traits<A>::cols &&
1691 mat_traits<A>::rows>=3,
1692 void>::type
1693 rotate_yzy( A & a, Angle y1, Angle z2, Angle y3 )
1694 {
1695 a *= rot_mat_yzy<mat_traits<A>::rows>(y1,z2,y3);
1696 }
1697
1698 ////////////////////////////////////////////////
1699
1700 template <int Dim,class Angle>
1701 BOOST_QVM_INLINE
1702 qvm_detail::rot_mat_<Dim,Angle>
1703 rot_mat_zyz( Angle z1, Angle y2, Angle z3 )
1704 {
1705 typedef Angle scalar_type;
1706 scalar_type const c1 = cos<scalar_type>(z1);
1707 scalar_type const s1 = sin<scalar_type>(z1);
1708 scalar_type const c2 = cos<scalar_type>(y2);
1709 scalar_type const s2 = sin<scalar_type>(y2);
1710 scalar_type const c3 = cos<scalar_type>(z3);
1711 scalar_type const s3 = sin<scalar_type>(z3);
1712 return qvm_detail::rot_mat_<Dim,Angle>(
1713 c1*c2*c3 - s1*s3, -c3*s1 - c1*c2*s3, c1*s2,
1714 c1*s3 + c2*c3*s1, c1*c3 - c2*s1*s3, s1*s2,
1715 -c3*s2, s2*s3, c2 );
1716 }
1717
1718 template <class A,class Angle>
1719 BOOST_QVM_INLINE_OPERATIONS
1720 typename enable_if_c<
1721 is_mat<A>::value &&
1722 mat_traits<A>::rows==mat_traits<A>::cols &&
1723 mat_traits<A>::rows>=3,
1724 void>::type
1725 set_rot_zyz( A & a, Angle z1, Angle y2, Angle z3 )
1726 {
1727 assign(a,rot_mat_zyz<mat_traits<A>::rows>(z1,y2,z3));
1728 }
1729
1730 template <class A,class Angle>
1731 BOOST_QVM_INLINE_OPERATIONS
1732 typename enable_if_c<
1733 is_mat<A>::value &&
1734 mat_traits<A>::rows==mat_traits<A>::cols &&
1735 mat_traits<A>::rows>=3,
1736 void>::type
1737 rotate_zyz( A & a, Angle z1, Angle y2, Angle z3 )
1738 {
1739 a *= rot_mat_zyz<mat_traits<A>::rows>(z1,y2,z3);
1740 }
1741
1742 ////////////////////////////////////////////////
1743
1744 template <int Dim,class Angle>
1745 BOOST_QVM_INLINE
1746 qvm_detail::rot_mat_<Dim,Angle>
1747 rot_mat_zxz( Angle z1, Angle x2, Angle z3 )
1748 {
1749 typedef Angle scalar_type;
1750 scalar_type const c1 = cos<scalar_type>(z1);
1751 scalar_type const s1 = sin<scalar_type>(z1);
1752 scalar_type const c2 = cos<scalar_type>(x2);
1753 scalar_type const s2 = sin<scalar_type>(x2);
1754 scalar_type const c3 = cos<scalar_type>(z3);
1755 scalar_type const s3 = sin<scalar_type>(z3);
1756 return qvm_detail::rot_mat_<Dim,Angle>(
1757 c1*c3 - c2*s1*s3, -c1*s3 - c2*c3*s1, s1*s2,
1758 c3*s1 + c1*c2*s3, c1*c2*c3 - s1*s3, -c1*s2,
1759 s2*s3, c3*s2, c2 );
1760 }
1761
1762 template <class A,class Angle>
1763 BOOST_QVM_INLINE_OPERATIONS
1764 typename enable_if_c<
1765 is_mat<A>::value &&
1766 mat_traits<A>::rows==mat_traits<A>::cols &&
1767 mat_traits<A>::rows>=3,
1768 void>::type
1769 set_rot_zxz( A & a, Angle z1, Angle x2, Angle z3 )
1770 {
1771 assign(a,rot_mat_zxz<mat_traits<A>::rows>(z1,x2,z3));
1772 }
1773
1774 template <class A,class Angle>
1775 BOOST_QVM_INLINE_OPERATIONS
1776 typename enable_if_c<
1777 is_mat<A>::value &&
1778 mat_traits<A>::rows==mat_traits<A>::cols &&
1779 mat_traits<A>::rows>=3,
1780 void>::type
1781 rotate_zxz( A & a, Angle z1, Angle x2, Angle z3 )
1782 {
1783 a *= rot_mat_zxz<mat_traits<A>::rows>(z1,x2,z3);
1784 }
1785
1786 ////////////////////////////////////////////////
1787
1788 namespace
1789 qvm_detail
1790 {
1791 template <int Dim,class Angle>
1792 struct
1793 rotx_mat_
1794 {
1795 BOOST_QVM_INLINE_TRIVIAL
1796 rotx_mat_()
1797 {
1798 }
1799
1800 template <class R>
1801 BOOST_QVM_INLINE_TRIVIAL
1802 operator R() const
1803 {
1804 R r;
1805 assign(r,*this);
1806 return r;
1807 }
1808
1809 private:
1810
1811 rotx_mat_( rotx_mat_ const & );
1812 rotx_mat_ & operator=( rotx_mat_ const & );
1813 ~rotx_mat_();
1814 };
1815
1816 template <int Row,int Col>
1817 struct
1818 rotx_m_get
1819 {
1820 template <class T>
1821 static
1822 BOOST_QVM_INLINE_CRITICAL
1823 T
1824 get( T const & )
1825 {
1826 return scalar_traits<T>::value(Row==Col);
1827 }
1828 };
1829
1830 template <>
1831 struct
1832 rotx_m_get<1,1>
1833 {
1834 template <class T>
1835 static
1836 BOOST_QVM_INLINE_CRITICAL
1837 T
1838 get( T const & angle )
1839 {
1840 return cos<T>(angle);
1841 }
1842 };
1843
1844 template <>
1845 struct
1846 rotx_m_get<1,2>
1847 {
1848 template <class T>
1849 static
1850 BOOST_QVM_INLINE_CRITICAL
1851 T
1852 get( T const & angle )
1853 {
1854 return -sin<T>(angle);
1855 }
1856 };
1857
1858 template <>
1859 struct
1860 rotx_m_get<2,1>
1861 {
1862 template <class T>
1863 static
1864 BOOST_QVM_INLINE_CRITICAL
1865 T
1866 get( T const & angle )
1867 {
1868 return sin<T>(angle);
1869 }
1870 };
1871
1872 template <>
1873 struct
1874 rotx_m_get<2,2>
1875 {
1876 template <class T>
1877 static
1878 BOOST_QVM_INLINE_CRITICAL
1879 T
1880 get( T const & angle )
1881 {
1882 return cos<T>(angle);
1883 }
1884 };
1885 }
1886
1887 template <int Dim,class Angle>
1888 struct
1889 mat_traits< qvm_detail::rotx_mat_<Dim,Angle> >
1890 {
1891 typedef qvm_detail::rotx_mat_<Dim,Angle> this_matrix;
1892 typedef Angle scalar_type;
1893 static int const rows=Dim;
1894 static int const cols=Dim;
1895
1896 template <int Row,int Col>
1897 static
1898 BOOST_QVM_INLINE_CRITICAL
1899 scalar_type
1900 read_element( this_matrix const & x )
1901 {
1902 BOOST_QVM_STATIC_ASSERT(Row>=0);
1903 BOOST_QVM_STATIC_ASSERT(Col>=0);
1904 BOOST_QVM_STATIC_ASSERT(Row<Dim);
1905 BOOST_QVM_STATIC_ASSERT(Col<Dim);
1906 return qvm_detail::rotx_m_get<Row,Col>::get(reinterpret_cast<Angle const &>(x));
1907 }
1908
1909 static
1910 BOOST_QVM_INLINE_CRITICAL
1911 scalar_type
1912 read_element_idx( int row, int col, this_matrix const & x )
1913 {
1914 BOOST_QVM_ASSERT(row>=0);
1915 BOOST_QVM_ASSERT(col>=0);
1916 BOOST_QVM_ASSERT(row<Dim);
1917 BOOST_QVM_ASSERT(col<Dim);
1918 Angle const & a=reinterpret_cast<Angle const &>(x);
1919 if( row==1 )
1920 {
1921 if( col==1 )
1922 return cos<scalar_type>(a);
1923 if( col==2 )
1924 return -sin<scalar_type>(a);
1925 }
1926 if( row==2 )
1927 {
1928 if( col==1 )
1929 return sin<scalar_type>(a);
1930 if( col==2 )
1931 return cos<scalar_type>(a);
1932 }
1933 return scalar_traits<scalar_type>::value(row==col);
1934 }
1935 };
1936
1937 template <int Dim,class Angle>
1938 struct
1939 deduce_mat<qvm_detail::rotx_mat_<Dim,Angle>,Dim,Dim>
1940 {
1941 typedef mat<Angle,Dim,Dim> type;
1942 };
1943
1944 template <int Dim,class Angle>
1945 struct
1946 deduce_mat2<qvm_detail::rotx_mat_<Dim,Angle>,qvm_detail::rotx_mat_<Dim,Angle>,Dim,Dim>
1947 {
1948 typedef mat<Angle,Dim,Dim> type;
1949 };
1950
1951 template <int Dim,class Angle>
1952 BOOST_QVM_INLINE_TRIVIAL
1953 qvm_detail::rotx_mat_<Dim,Angle> const &
1954 rotx_mat( Angle const & angle )
1955 {
1956 BOOST_QVM_STATIC_ASSERT(Dim>=3);
1957 return reinterpret_cast<qvm_detail::rotx_mat_<Dim,Angle> const &>(angle);
1958 }
1959
1960 template <class A,class Angle>
1961 BOOST_QVM_INLINE_OPERATIONS
1962 typename enable_if_c<
1963 is_mat<A>::value &&
1964 mat_traits<A>::rows>=3 &&
1965 mat_traits<A>::rows==mat_traits<A>::cols,
1966 void>::type
1967 set_rotx( A & a, Angle angle )
1968 {
1969 assign(a,rotx_mat<mat_traits<A>::rows>(angle));
1970 }
1971
1972 template <class A,class Angle>
1973 BOOST_QVM_INLINE_OPERATIONS
1974 typename enable_if_c<
1975 is_mat<A>::value &&
1976 mat_traits<A>::rows>=3 &&
1977 mat_traits<A>::rows==mat_traits<A>::cols,
1978 void>::type
1979 rotate_x( A & a, Angle angle )
1980 {
1981 a *= rotx_mat<mat_traits<A>::rows>(angle);
1982 }
1983
1984 ////////////////////////////////////////////////
1985
1986 namespace
1987 qvm_detail
1988 {
1989 template <int Dim,class Angle>
1990 struct
1991 roty_mat_
1992 {
1993 BOOST_QVM_INLINE_TRIVIAL
1994 roty_mat_()
1995 {
1996 }
1997
1998 template <class R>
1999 BOOST_QVM_INLINE_TRIVIAL
2000 operator R() const
2001 {
2002 R r;
2003 assign(r,*this);
2004 return r;
2005 }
2006
2007 private:
2008
2009 roty_mat_( roty_mat_ const & );
2010 roty_mat_ & operator=( roty_mat_ const & );
2011 ~roty_mat_();
2012 };
2013
2014 template <int Row,int Col>
2015 struct
2016 roty_m_get
2017 {
2018 template <class T>
2019 static
2020 BOOST_QVM_INLINE_CRITICAL
2021 T
2022 get( T const & )
2023 {
2024 return scalar_traits<T>::value(Row==Col);
2025 }
2026 };
2027
2028 template <>
2029 struct
2030 roty_m_get<0,0>
2031 {
2032 template <class T>
2033 static
2034 BOOST_QVM_INLINE_CRITICAL
2035 T
2036 get( T const & angle )
2037 {
2038 return cos<T>(angle);
2039 }
2040 };
2041
2042 template <>
2043 struct
2044 roty_m_get<0,2>
2045 {
2046 template <class T>
2047 static
2048 BOOST_QVM_INLINE_CRITICAL
2049 T
2050 get( T const & angle )
2051 {
2052 return sin<T>(angle);
2053 }
2054 };
2055
2056 template <>
2057 struct
2058 roty_m_get<2,0>
2059 {
2060 template <class T>
2061 static
2062 BOOST_QVM_INLINE_CRITICAL
2063 T
2064 get( T const & angle )
2065 {
2066 return -sin<T>(angle);
2067 }
2068 };
2069
2070 template <>
2071 struct
2072 roty_m_get<2,2>
2073 {
2074 template <class T>
2075 static
2076 BOOST_QVM_INLINE_CRITICAL
2077 T
2078 get( T const & angle )
2079 {
2080 return cos<T>(angle);
2081 }
2082 };
2083 }
2084
2085 template <int Dim,class Angle>
2086 struct
2087 mat_traits< qvm_detail::roty_mat_<Dim,Angle> >
2088 {
2089 typedef qvm_detail::roty_mat_<Dim,Angle> this_matrix;
2090 typedef Angle scalar_type;
2091 static int const rows=Dim;
2092 static int const cols=Dim;
2093
2094 template <int Row,int Col>
2095 static
2096 BOOST_QVM_INLINE_CRITICAL
2097 scalar_type
2098 read_element( this_matrix const & x )
2099 {
2100 BOOST_QVM_STATIC_ASSERT(Row>=0);
2101 BOOST_QVM_STATIC_ASSERT(Col>=0);
2102 BOOST_QVM_STATIC_ASSERT(Row<Dim);
2103 BOOST_QVM_STATIC_ASSERT(Col<Dim);
2104 return qvm_detail::roty_m_get<Row,Col>::get(reinterpret_cast<Angle const &>(x));
2105 }
2106
2107 static
2108 BOOST_QVM_INLINE_CRITICAL
2109 scalar_type
2110 read_element_idx( int row, int col, this_matrix const & x )
2111 {
2112 BOOST_QVM_ASSERT(row>=0);
2113 BOOST_QVM_ASSERT(col>=0);
2114 BOOST_QVM_ASSERT(row<Dim);
2115 BOOST_QVM_ASSERT(col<Dim);
2116 Angle const & a=reinterpret_cast<Angle const &>(x);
2117 if( row==0 )
2118 {
2119 if( col==0 )
2120 return cos<scalar_type>(a);
2121 if( col==2 )
2122 return sin<scalar_type>(a);
2123 }
2124 if( row==2 )
2125 {
2126 if( col==0 )
2127 return -sin<scalar_type>(a);
2128 if( col==2 )
2129 return cos<scalar_type>(a);
2130 }
2131 return scalar_traits<scalar_type>::value(row==col);
2132 }
2133 };
2134
2135 template <int Dim,class Angle>
2136 struct
2137 deduce_mat<qvm_detail::roty_mat_<Dim,Angle>,Dim,Dim>
2138 {
2139 typedef mat<Angle,Dim,Dim> type;
2140 };
2141
2142 template <int Dim,class Angle>
2143 struct
2144 deduce_mat2<qvm_detail::roty_mat_<Dim,Angle>,qvm_detail::roty_mat_<Dim,Angle>,Dim,Dim>
2145 {
2146 typedef mat<Angle,Dim,Dim> type;
2147 };
2148
2149 template <int Dim,class Angle>
2150 BOOST_QVM_INLINE_TRIVIAL
2151 qvm_detail::roty_mat_<Dim,Angle> const &
2152 roty_mat( Angle const & angle )
2153 {
2154 BOOST_QVM_STATIC_ASSERT(Dim>=3);
2155 return reinterpret_cast<qvm_detail::roty_mat_<Dim,Angle> const &>(angle);
2156 }
2157
2158 template <class A,class Angle>
2159 BOOST_QVM_INLINE_OPERATIONS
2160 typename enable_if_c<
2161 is_mat<A>::value &&
2162 mat_traits<A>::rows>=2 &&
2163 mat_traits<A>::rows==mat_traits<A>::cols,
2164 void>::type
2165 set_roty( A & a, Angle angle )
2166 {
2167 assign(a,roty_mat<mat_traits<A>::rows>(angle));
2168 }
2169
2170 template <class A,class Angle>
2171 BOOST_QVM_INLINE_OPERATIONS
2172 typename enable_if_c<
2173 is_mat<A>::value &&
2174 mat_traits<A>::rows>=3 &&
2175 mat_traits<A>::rows==mat_traits<A>::cols,
2176 void>::type
2177 rotate_y( A & a, Angle angle )
2178 {
2179 a *= roty_mat<mat_traits<A>::rows>(angle);
2180 }
2181
2182 ////////////////////////////////////////////////
2183
2184 namespace
2185 qvm_detail
2186 {
2187 template <int Dim,class Angle>
2188 struct
2189 rotz_mat_
2190 {
2191 BOOST_QVM_INLINE_TRIVIAL
2192 rotz_mat_()
2193 {
2194 }
2195
2196 template <class R>
2197 BOOST_QVM_INLINE_TRIVIAL
2198 operator R() const
2199 {
2200 R r;
2201 assign(r,*this);
2202 return r;
2203 }
2204
2205 private:
2206
2207 rotz_mat_( rotz_mat_ const & );
2208 rotz_mat_ & operator=( rotz_mat_ const & );
2209 ~rotz_mat_();
2210 };
2211
2212 template <int Row,int Col>
2213 struct
2214 rotz_m_get
2215 {
2216 template <class T>
2217 static
2218 BOOST_QVM_INLINE_CRITICAL
2219 T
2220 get( T const & )
2221 {
2222 return scalar_traits<T>::value(Row==Col);
2223 }
2224 };
2225
2226 template <>
2227 struct
2228 rotz_m_get<0,0>
2229 {
2230 template <class T>
2231 static
2232 BOOST_QVM_INLINE_CRITICAL
2233 T
2234 get( T const & angle )
2235 {
2236 return cos<T>(angle);
2237 }
2238 };
2239
2240 template <>
2241 struct
2242 rotz_m_get<0,1>
2243 {
2244 template <class T>
2245 static
2246 BOOST_QVM_INLINE_CRITICAL
2247 T
2248 get( T const & angle )
2249 {
2250 return -sin<T>(angle);
2251 }
2252 };
2253
2254 template <>
2255 struct
2256 rotz_m_get<1,0>
2257 {
2258 template <class T>
2259 static
2260 BOOST_QVM_INLINE_CRITICAL
2261 T
2262 get( T const & angle )
2263 {
2264 return sin<T>(angle);
2265 }
2266 };
2267
2268 template <>
2269 struct
2270 rotz_m_get<1,1>
2271 {
2272 template <class T>
2273 static
2274 BOOST_QVM_INLINE_CRITICAL
2275 T
2276 get( T const & angle )
2277 {
2278 return cos<T>(angle);
2279 }
2280 };
2281 }
2282
2283 template <int Dim,class Angle>
2284 struct
2285 mat_traits< qvm_detail::rotz_mat_<Dim,Angle> >
2286 {
2287 typedef qvm_detail::rotz_mat_<Dim,Angle> this_matrix;
2288 typedef Angle scalar_type;
2289 static int const rows=Dim;
2290 static int const cols=Dim;
2291
2292 template <int Row,int Col>
2293 static
2294 BOOST_QVM_INLINE_CRITICAL
2295 scalar_type
2296 read_element( this_matrix const & x )
2297 {
2298 BOOST_QVM_STATIC_ASSERT(Row>=0);
2299 BOOST_QVM_STATIC_ASSERT(Col>=0);
2300 BOOST_QVM_STATIC_ASSERT(Row<Dim);
2301 BOOST_QVM_STATIC_ASSERT(Col<Dim);
2302 return qvm_detail::rotz_m_get<Row,Col>::get(reinterpret_cast<Angle const &>(x));
2303 }
2304
2305 static
2306 BOOST_QVM_INLINE_CRITICAL
2307 scalar_type
2308 read_element_idx( int row, int col, this_matrix const & x )
2309 {
2310 BOOST_QVM_ASSERT(row>=0);
2311 BOOST_QVM_ASSERT(col>=0);
2312 BOOST_QVM_ASSERT(row<Dim);
2313 BOOST_QVM_ASSERT(col<Dim);
2314 Angle const & a=reinterpret_cast<Angle const &>(x);
2315 if( row==0 )
2316 {
2317 if( col==0 )
2318 return cos<scalar_type>(a);
2319 if( col==1 )
2320 return -sin<scalar_type>(a);
2321 }
2322 if( row==1 )
2323 {
2324 if( col==0 )
2325 return sin<scalar_type>(a);
2326 if( col==1 )
2327 return cos<scalar_type>(a);
2328 }
2329 return scalar_traits<scalar_type>::value(row==col);
2330 }
2331 };
2332
2333 template <int Dim,class Angle>
2334 struct
2335 deduce_mat<qvm_detail::rotz_mat_<Dim,Angle>,Dim,Dim>
2336 {
2337 typedef mat<Angle,Dim,Dim> type;
2338 };
2339
2340 template <int Dim,class Angle,int R,int C>
2341 struct
2342 deduce_mat2<qvm_detail::rotz_mat_<Dim,Angle>,qvm_detail::rotz_mat_<Dim,Angle>,R,C>
2343 {
2344 typedef mat<Angle,R,C> type;
2345 };
2346
2347 template <int Dim,class Angle>
2348 BOOST_QVM_INLINE_TRIVIAL
2349 qvm_detail::rotz_mat_<Dim,Angle> const &
2350 rotz_mat( Angle const & angle )
2351 {
2352 BOOST_QVM_STATIC_ASSERT(Dim>=2);
2353 return reinterpret_cast<qvm_detail::rotz_mat_<Dim,Angle> const &>(angle);
2354 }
2355
2356 template <class A,class Angle>
2357 BOOST_QVM_INLINE_OPERATIONS
2358 typename enable_if_c<
2359 is_mat<A>::value &&
2360 mat_traits<A>::rows>=2 &&
2361 mat_traits<A>::rows==mat_traits<A>::cols,
2362 void>::type
2363 set_rotz( A & a, Angle angle )
2364 {
2365 assign(a,rotz_mat<mat_traits<A>::rows>(angle));
2366 }
2367
2368 template <class A,class Angle>
2369 BOOST_QVM_INLINE_OPERATIONS
2370 typename enable_if_c<
2371 is_mat<A>::value &&
2372 mat_traits<A>::rows>=2 &&
2373 mat_traits<A>::rows==mat_traits<A>::cols,
2374 void>::type
2375 rotate_z( A & a, Angle angle )
2376 {
2377 a *= rotz_mat<mat_traits<A>::rows>(angle);
2378 }
2379
2380 ////////////////////////////////////////////////
2381
2382 namespace
2383 qvm_detail
2384 {
2385 template <int D>
2386 struct
2387 inverse_m_defined
2388 {
2389 static bool const value=false;
2390 };
2391 }
2392
2393 template <class A,class B>
2394 BOOST_QVM_INLINE_TRIVIAL
2395 typename lazy_enable_if_c<
2396 is_mat<A>::value && is_scalar<B>::value &&
2397 mat_traits<A>::rows==mat_traits<A>::cols &&
2398 !qvm_detail::inverse_m_defined<mat_traits<A>::rows>::value,
2399 deduce_mat<A> >::type
2400 inverse( A const & a, B det )
2401 {
2402 typedef typename mat_traits<A>::scalar_type T;
2403 BOOST_QVM_ASSERT(det!=scalar_traits<T>::value(0));
2404 T f=scalar_traits<T>::value(1)/det;
2405 typedef typename deduce_mat<A>::type cofactor_return_type;
2406 cofactor_return_type c=qvm_detail::cofactor_impl(a);
2407 return reinterpret_cast<qvm_detail::transposed_<cofactor_return_type> const &>(c) * f;
2408 }
2409
2410 template <class A>
2411 BOOST_QVM_INLINE_TRIVIAL
2412 typename lazy_enable_if_c<
2413 is_mat<A>::value &&
2414 mat_traits<A>::rows==mat_traits<A>::cols &&
2415 !qvm_detail::inverse_m_defined<mat_traits<A>::rows>::value,
2416 deduce_mat<A> >::type
2417 inverse( A const & a )
2418 {
2419 typedef typename mat_traits<A>::scalar_type T;
2420 T det=determinant(a);
2421 if( det==scalar_traits<T>::value(0) )
2422 BOOST_QVM_THROW_EXCEPTION(zero_determinant_error());
2423 return inverse(a,det);
2424 }
2425
2426 ////////////////////////////////////////////////
2427
2428 namespace
2429 sfinae
2430 {
2431 using ::boost::qvm::to_string;
2432 using ::boost::qvm::assign;
2433 using ::boost::qvm::determinant;
2434 using ::boost::qvm::cmp;
2435 using ::boost::qvm::convert_to;
2436 using ::boost::qvm::set_identity;
2437 using ::boost::qvm::set_zero;
2438 using ::boost::qvm::scalar_cast;
2439 using ::boost::qvm::operator/=;
2440 using ::boost::qvm::operator/;
2441 using ::boost::qvm::operator==;
2442 using ::boost::qvm::operator-=;
2443 using ::boost::qvm::operator-;
2444 using ::boost::qvm::operator*=;
2445 using ::boost::qvm::operator*;
2446 using ::boost::qvm::operator!=;
2447 using ::boost::qvm::operator+=;
2448 using ::boost::qvm::operator+;
2449 using ::boost::qvm::mref;
2450 using ::boost::qvm::rot_mat;
2451 using ::boost::qvm::set_rot;
2452 using ::boost::qvm::rotate;
2453 using ::boost::qvm::set_rotx;
2454 using ::boost::qvm::rotate_x;
2455 using ::boost::qvm::set_roty;
2456 using ::boost::qvm::rotate_y;
2457 using ::boost::qvm::set_rotz;
2458 using ::boost::qvm::rotate_z;
2459 using ::boost::qvm::inverse;
2460 }
2461
2462 ////////////////////////////////////////////////
2463 }
2464 }
2465
2466#endif
Ceph