mat_operations.hpp 69 KB

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