mat_operations.hpp 82 KB

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