mat_operations.hpp 82 KB

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