mat_operations.hpp 82 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658165916601661166216631664166516661667166816691670167116721673167416751676167716781679168016811682168316841685168616871688168916901691169216931694169516961697169816991700170117021703170417051706170717081709171017111712171317141715171617171718171917201721172217231724172517261727172817291730173117321733173417351736173717381739174017411742174317441745174617471748174917501751175217531754175517561757175817591760176117621763176417651766176717681769177017711772177317741775177617771778177917801781178217831784178517861787178817891790179117921793179417951796179717981799180018011802180318041805180618071808180918101811181218131814181518161817181818191820182118221823182418251826182718281829183018311832183318341835183618371838183918401841184218431844184518461847184818491850185118521853185418551856185718581859186018611862186318641865186618671868186918701871187218731874187518761877187818791880188118821883188418851886188718881889189018911892189318941895189618971898189919001901190219031904190519061907190819091910191119121913191419151916191719181919192019211922192319241925192619271928192919301931193219331934193519361937193819391940194119421943194419451946194719481949195019511952195319541955195619571958195919601961196219631964196519661967196819691970197119721973197419751976197719781979198019811982198319841985198619871988198919901991199219931994199519961997199819992000200120022003200420052006200720082009201020112012201320142015201620172018201920202021202220232024202520262027202820292030203120322033203420352036203720382039204020412042204320442045204620472048204920502051205220532054205520562057205820592060206120622063206420652066206720682069207020712072207320742075207620772078207920802081208220832084208520862087208820892090209120922093209420952096209720982099210021012102210321042105210621072108210921102111211221132114211521162117211821192120212121222123212421252126212721282129213021312132213321342135213621372138213921402141214221432144214521462147214821492150215121522153215421552156215721582159216021612162216321642165216621672168216921702171217221732174217521762177217821792180218121822183218421852186218721882189219021912192219321942195219621972198219922002201220222032204220522062207220822092210221122122213221422152216221722182219222022212222222322242225222622272228222922302231223222332234223522362237223822392240224122422243224422452246224722482249225022512252225322542255225622572258225922602261226222632264226522662267226822692270227122722273227422752276227722782279228022812282228322842285228622872288228922902291229222932294229522962297229822992300230123022303230423052306230723082309231023112312231323142315231623172318231923202321232223232324232523262327232823292330233123322333233423352336233723382339234023412342234323442345234623472348234923502351235223532354235523562357235823592360236123622363236423652366236723682369237023712372237323742375237623772378237923802381238223832384238523862387238823892390239123922393239423952396239723982399240024012402240324042405240624072408240924102411241224132414241524162417241824192420242124222423242424252426242724282429243024312432243324342435243624372438243924402441244224432444244524462447244824492450245124522453245424552456245724582459246024612462246324642465246624672468246924702471
  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