quaternion.hpp 48 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254
  1. // boost quaternion.hpp header file
  2. // (C) Copyright Hubert Holin 2001.
  3. // Distributed under the Boost Software License, Version 1.0. (See
  4. // accompanying file LICENSE_1_0.txt or copy at
  5. // http://www.boost.org/LICENSE_1_0.txt)
  6. // See http://www.boost.org for updates, documentation, and revision history.
  7. #ifndef BOOST_QUATERNION_HPP
  8. #define BOOST_QUATERNION_HPP
  9. #include <boost/config.hpp> // for BOOST_NO_STD_LOCALE
  10. #include <boost/math_fwd.hpp>
  11. #include <boost/detail/workaround.hpp>
  12. #include <boost/type_traits/is_convertible.hpp>
  13. #include <boost/utility/enable_if.hpp>
  14. #ifndef BOOST_NO_STD_LOCALE
  15. # include <locale> // for the "<<" operator
  16. #endif /* BOOST_NO_STD_LOCALE */
  17. #include <complex>
  18. #include <iosfwd> // for the "<<" and ">>" operators
  19. #include <sstream> // for the "<<" operator
  20. #include <boost/math/special_functions/sinc.hpp> // for the Sinus cardinal
  21. #include <boost/math/special_functions/sinhc.hpp> // for the Hyperbolic Sinus cardinal
  22. #include <boost/math/tools/cxx03_warn.hpp>
  23. #if defined(BOOST_NO_CXX11_NOEXCEPT) || defined(BOOST_NO_CXX11_RVALUE_REFERENCES) || defined(BOOST_NO_SFINAE_EXPR)
  24. #include <boost/type_traits/is_pod.hpp>
  25. #endif
  26. namespace boost
  27. {
  28. namespace math
  29. {
  30. namespace detail {
  31. #if !defined(BOOST_NO_CXX11_NOEXCEPT) && !defined(BOOST_NO_CXX11_RVALUE_REFERENCES) && !defined(BOOST_NO_SFINAE_EXPR)
  32. template <class T>
  33. struct is_trivial_arithmetic_type_imp
  34. {
  35. typedef boost::integral_constant<bool,
  36. noexcept(std::declval<T&>() += std::declval<T>())
  37. && noexcept(std::declval<T&>() -= std::declval<T>())
  38. && noexcept(std::declval<T&>() *= std::declval<T>())
  39. && noexcept(std::declval<T&>() /= std::declval<T>())
  40. > type;
  41. };
  42. template <class T>
  43. struct is_trivial_arithmetic_type : public is_trivial_arithmetic_type_imp<T>::type {};
  44. #else
  45. template <class T>
  46. struct is_trivial_arithmetic_type : public boost::is_pod<T> {};
  47. #endif
  48. }
  49. #ifndef BOOST_NO_CXX14_CONSTEXPR
  50. namespace constexpr_detail
  51. {
  52. template <class T>
  53. constexpr void swap(T& a, T& b)
  54. {
  55. T t(a);
  56. a = b;
  57. b = t;
  58. }
  59. }
  60. #endif
  61. template<typename T>
  62. class quaternion
  63. {
  64. public:
  65. typedef T value_type;
  66. // constructor for H seen as R^4
  67. // (also default constructor)
  68. BOOST_CONSTEXPR explicit quaternion( T const & requested_a = T(),
  69. T const & requested_b = T(),
  70. T const & requested_c = T(),
  71. T const & requested_d = T())
  72. : a(requested_a),
  73. b(requested_b),
  74. c(requested_c),
  75. d(requested_d)
  76. {
  77. // nothing to do!
  78. }
  79. // constructor for H seen as C^2
  80. BOOST_CONSTEXPR explicit quaternion( ::std::complex<T> const & z0,
  81. ::std::complex<T> const & z1 = ::std::complex<T>())
  82. : a(z0.real()),
  83. b(z0.imag()),
  84. c(z1.real()),
  85. d(z1.imag())
  86. {
  87. // nothing to do!
  88. }
  89. // UNtemplated copy constructor
  90. BOOST_CONSTEXPR quaternion(quaternion const & a_recopier)
  91. : a(a_recopier.R_component_1()),
  92. b(a_recopier.R_component_2()),
  93. c(a_recopier.R_component_3()),
  94. d(a_recopier.R_component_4()) {}
  95. #ifndef BOOST_NO_CXX11_RVALUE_REFERENCES
  96. BOOST_CONSTEXPR quaternion(quaternion && a_recopier)
  97. : a(std::move(a_recopier.R_component_1())),
  98. b(std::move(a_recopier.R_component_2())),
  99. c(std::move(a_recopier.R_component_3())),
  100. d(std::move(a_recopier.R_component_4())) {}
  101. #endif
  102. // templated copy constructor
  103. template<typename X>
  104. BOOST_CONSTEXPR explicit quaternion(quaternion<X> const & a_recopier)
  105. : a(static_cast<T>(a_recopier.R_component_1())),
  106. b(static_cast<T>(a_recopier.R_component_2())),
  107. c(static_cast<T>(a_recopier.R_component_3())),
  108. d(static_cast<T>(a_recopier.R_component_4()))
  109. {
  110. // nothing to do!
  111. }
  112. // destructor
  113. // (this is taken care of by the compiler itself)
  114. // accessors
  115. //
  116. // Note: Like complex number, quaternions do have a meaningful notion of "real part",
  117. // but unlike them there is no meaningful notion of "imaginary part".
  118. // Instead there is an "unreal part" which itself is a quaternion, and usually
  119. // nothing simpler (as opposed to the complex number case).
  120. // However, for practicality, there are accessors for the other components
  121. // (these are necessary for the templated copy constructor, for instance).
  122. BOOST_CONSTEXPR T real() const
  123. {
  124. return(a);
  125. }
  126. BOOST_CONSTEXPR quaternion<T> unreal() const
  127. {
  128. return(quaternion<T>(static_cast<T>(0), b, c, d));
  129. }
  130. BOOST_CONSTEXPR T R_component_1() const
  131. {
  132. return(a);
  133. }
  134. BOOST_CONSTEXPR T R_component_2() const
  135. {
  136. return(b);
  137. }
  138. BOOST_CONSTEXPR T R_component_3() const
  139. {
  140. return(c);
  141. }
  142. BOOST_CONSTEXPR T R_component_4() const
  143. {
  144. return(d);
  145. }
  146. BOOST_CONSTEXPR ::std::complex<T> C_component_1() const
  147. {
  148. return(::std::complex<T>(a, b));
  149. }
  150. BOOST_CONSTEXPR ::std::complex<T> C_component_2() const
  151. {
  152. return(::std::complex<T>(c, d));
  153. }
  154. BOOST_CXX14_CONSTEXPR void swap(quaternion& o)
  155. {
  156. #ifndef BOOST_NO_CXX14_CONSTEXPR
  157. using constexpr_detail::swap;
  158. #else
  159. using std::swap;
  160. #endif
  161. swap(a, o.a);
  162. swap(b, o.b);
  163. swap(c, o.c);
  164. swap(d, o.d);
  165. }
  166. // assignment operators
  167. template<typename X>
  168. BOOST_CXX14_CONSTEXPR quaternion<T> & operator = (quaternion<X> const & a_affecter)
  169. {
  170. a = static_cast<T>(a_affecter.R_component_1());
  171. b = static_cast<T>(a_affecter.R_component_2());
  172. c = static_cast<T>(a_affecter.R_component_3());
  173. d = static_cast<T>(a_affecter.R_component_4());
  174. return(*this);
  175. }
  176. BOOST_CXX14_CONSTEXPR quaternion<T> & operator = (quaternion<T> const & a_affecter)
  177. {
  178. a = a_affecter.a;
  179. b = a_affecter.b;
  180. c = a_affecter.c;
  181. d = a_affecter.d;
  182. return(*this);
  183. }
  184. #ifndef BOOST_NO_CXX11_RVALUE_REFERENCES
  185. BOOST_CXX14_CONSTEXPR quaternion<T> & operator = (quaternion<T> && a_affecter)
  186. {
  187. a = std::move(a_affecter.a);
  188. b = std::move(a_affecter.b);
  189. c = std::move(a_affecter.c);
  190. d = std::move(a_affecter.d);
  191. return(*this);
  192. }
  193. #endif
  194. BOOST_CXX14_CONSTEXPR quaternion<T> & operator = (T const & a_affecter)
  195. {
  196. a = a_affecter;
  197. b = c = d = static_cast<T>(0);
  198. return(*this);
  199. }
  200. BOOST_CXX14_CONSTEXPR quaternion<T> & operator = (::std::complex<T> const & a_affecter)
  201. {
  202. a = a_affecter.real();
  203. b = a_affecter.imag();
  204. c = d = static_cast<T>(0);
  205. return(*this);
  206. }
  207. // other assignment-related operators
  208. //
  209. // NOTE: Quaternion multiplication is *NOT* commutative;
  210. // symbolically, "q *= rhs;" means "q = q * rhs;"
  211. // and "q /= rhs;" means "q = q * inverse_of(rhs);"
  212. //
  213. // Note2: Each operator comes in 2 forms - one for the simple case where
  214. // type T throws no exceptions, and one exception-safe version
  215. // for the case where it might.
  216. private:
  217. BOOST_CXX14_CONSTEXPR quaternion<T> & do_add(T const & rhs, const boost::true_type&)
  218. {
  219. a += rhs;
  220. return *this;
  221. }
  222. BOOST_CXX14_CONSTEXPR quaternion<T> & do_add(T const & rhs, const boost::false_type&)
  223. {
  224. quaternion<T> result(a + rhs, b, c, d); // exception guard
  225. swap(result);
  226. return *this;
  227. }
  228. BOOST_CXX14_CONSTEXPR quaternion<T> & do_add(std::complex<T> const & rhs, const boost::true_type&)
  229. {
  230. a += std::real(rhs);
  231. b += std::imag(rhs);
  232. return *this;
  233. }
  234. BOOST_CXX14_CONSTEXPR quaternion<T> & do_add(std::complex<T> const & rhs, const boost::false_type&)
  235. {
  236. quaternion<T> result(a + std::real(rhs), b + std::imag(rhs), c, d); // exception guard
  237. swap(result);
  238. return *this;
  239. }
  240. template <class X>
  241. BOOST_CXX14_CONSTEXPR quaternion<T> & do_add(quaternion<X> const & rhs, const boost::true_type&)
  242. {
  243. a += rhs.R_component_1();
  244. b += rhs.R_component_2();
  245. c += rhs.R_component_3();
  246. d += rhs.R_component_4();
  247. return *this;
  248. }
  249. template <class X>
  250. BOOST_CXX14_CONSTEXPR quaternion<T> & do_add(quaternion<X> const & rhs, const boost::false_type&)
  251. {
  252. quaternion<T> result(a + rhs.R_component_1(), b + rhs.R_component_2(), c + rhs.R_component_3(), d + rhs.R_component_4()); // exception guard
  253. swap(result);
  254. return *this;
  255. }
  256. BOOST_CXX14_CONSTEXPR quaternion<T> & do_subtract(T const & rhs, const boost::true_type&)
  257. {
  258. a -= rhs;
  259. return *this;
  260. }
  261. BOOST_CXX14_CONSTEXPR quaternion<T> & do_subtract(T const & rhs, const boost::false_type&)
  262. {
  263. quaternion<T> result(a - rhs, b, c, d); // exception guard
  264. swap(result);
  265. return *this;
  266. }
  267. BOOST_CXX14_CONSTEXPR quaternion<T> & do_subtract(std::complex<T> const & rhs, const boost::true_type&)
  268. {
  269. a -= std::real(rhs);
  270. b -= std::imag(rhs);
  271. return *this;
  272. }
  273. BOOST_CXX14_CONSTEXPR quaternion<T> & do_subtract(std::complex<T> const & rhs, const boost::false_type&)
  274. {
  275. quaternion<T> result(a - std::real(rhs), b - std::imag(rhs), c, d); // exception guard
  276. swap(result);
  277. return *this;
  278. }
  279. template <class X>
  280. BOOST_CXX14_CONSTEXPR quaternion<T> & do_subtract(quaternion<X> const & rhs, const boost::true_type&)
  281. {
  282. a -= rhs.R_component_1();
  283. b -= rhs.R_component_2();
  284. c -= rhs.R_component_3();
  285. d -= rhs.R_component_4();
  286. return *this;
  287. }
  288. template <class X>
  289. BOOST_CXX14_CONSTEXPR quaternion<T> & do_subtract(quaternion<X> const & rhs, const boost::false_type&)
  290. {
  291. quaternion<T> result(a - rhs.R_component_1(), b - rhs.R_component_2(), c - rhs.R_component_3(), d - rhs.R_component_4()); // exception guard
  292. swap(result);
  293. return *this;
  294. }
  295. BOOST_CXX14_CONSTEXPR quaternion<T> & do_multiply(T const & rhs, const boost::true_type&)
  296. {
  297. a *= rhs;
  298. b *= rhs;
  299. c *= rhs;
  300. d *= rhs;
  301. return *this;
  302. }
  303. BOOST_CXX14_CONSTEXPR quaternion<T> & do_multiply(T const & rhs, const boost::false_type&)
  304. {
  305. quaternion<T> result(a * rhs, b * rhs, c * rhs, d * rhs); // exception guard
  306. swap(result);
  307. return *this;
  308. }
  309. BOOST_CXX14_CONSTEXPR quaternion<T> & do_divide(T const & rhs, const boost::true_type&)
  310. {
  311. a /= rhs;
  312. b /= rhs;
  313. c /= rhs;
  314. d /= rhs;
  315. return *this;
  316. }
  317. BOOST_CXX14_CONSTEXPR quaternion<T> & do_divide(T const & rhs, const boost::false_type&)
  318. {
  319. quaternion<T> result(a / rhs, b / rhs, c / rhs, d / rhs); // exception guard
  320. swap(result);
  321. return *this;
  322. }
  323. public:
  324. BOOST_CXX14_CONSTEXPR quaternion<T> & operator += (T const & rhs) { return do_add(rhs, detail::is_trivial_arithmetic_type<T>()); }
  325. BOOST_CXX14_CONSTEXPR quaternion<T> & operator += (::std::complex<T> const & rhs) { return do_add(rhs, detail::is_trivial_arithmetic_type<T>()); }
  326. template<typename X> BOOST_CXX14_CONSTEXPR quaternion<T> & operator += (quaternion<X> const & rhs) { return do_add(rhs, detail::is_trivial_arithmetic_type<T>()); }
  327. BOOST_CXX14_CONSTEXPR quaternion<T> & operator -= (T const & rhs) { return do_subtract(rhs, detail::is_trivial_arithmetic_type<T>()); }
  328. BOOST_CXX14_CONSTEXPR quaternion<T> & operator -= (::std::complex<T> const & rhs) { return do_subtract(rhs, detail::is_trivial_arithmetic_type<T>()); }
  329. template<typename X> BOOST_CXX14_CONSTEXPR quaternion<T> & operator -= (quaternion<X> const & rhs) { return do_subtract(rhs, detail::is_trivial_arithmetic_type<T>()); }
  330. BOOST_CXX14_CONSTEXPR quaternion<T> & operator *= (T const & rhs) { return do_multiply(rhs, detail::is_trivial_arithmetic_type<T>()); }
  331. BOOST_CXX14_CONSTEXPR quaternion<T> & operator *= (::std::complex<T> const & rhs)
  332. {
  333. T ar = rhs.real();
  334. T br = rhs.imag();
  335. quaternion<T> result(a*ar - b*br, a*br + b*ar, c*ar + d*br, -c*br+d*ar);
  336. swap(result);
  337. return(*this);
  338. }
  339. template<typename X>
  340. BOOST_CXX14_CONSTEXPR quaternion<T> & operator *= (quaternion<X> const & rhs)
  341. {
  342. T ar = static_cast<T>(rhs.R_component_1());
  343. T br = static_cast<T>(rhs.R_component_2());
  344. T cr = static_cast<T>(rhs.R_component_3());
  345. T dr = static_cast<T>(rhs.R_component_4());
  346. quaternion<T> result(a*ar - b*br - c*cr - d*dr, a*br + b*ar + c*dr - d*cr, a*cr - b*dr + c*ar + d*br, a*dr + b*cr - c*br + d*ar);
  347. swap(result);
  348. return(*this);
  349. }
  350. BOOST_CXX14_CONSTEXPR quaternion<T> & operator /= (T const & rhs) { return do_divide(rhs, detail::is_trivial_arithmetic_type<T>()); }
  351. BOOST_CXX14_CONSTEXPR quaternion<T> & operator /= (::std::complex<T> const & rhs)
  352. {
  353. T ar = rhs.real();
  354. T br = rhs.imag();
  355. T denominator = ar*ar+br*br;
  356. quaternion<T> result((+a*ar + b*br) / denominator, (-a*br + b*ar) / denominator, (+c*ar - d*br) / denominator, (+c*br + d*ar) / denominator);
  357. swap(result);
  358. return(*this);
  359. }
  360. template<typename X>
  361. BOOST_CXX14_CONSTEXPR quaternion<T> & operator /= (quaternion<X> const & rhs)
  362. {
  363. T ar = static_cast<T>(rhs.R_component_1());
  364. T br = static_cast<T>(rhs.R_component_2());
  365. T cr = static_cast<T>(rhs.R_component_3());
  366. T dr = static_cast<T>(rhs.R_component_4());
  367. T denominator = ar*ar+br*br+cr*cr+dr*dr;
  368. quaternion<T> result((+a*ar+b*br+c*cr+d*dr)/denominator, (-a*br+b*ar-c*dr+d*cr)/denominator, (-a*cr+b*dr+c*ar-d*br)/denominator, (-a*dr-b*cr+c*br+d*ar)/denominator);
  369. swap(result);
  370. return(*this);
  371. }
  372. private:
  373. T a, b, c, d;
  374. };
  375. // swap:
  376. template <class T>
  377. BOOST_CXX14_CONSTEXPR void swap(quaternion<T>& a, quaternion<T>& b) { a.swap(b); }
  378. // operator+
  379. template <class T1, class T2>
  380. inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
  381. operator + (const quaternion<T1>& a, const T2& b)
  382. {
  383. return quaternion<T1>(static_cast<T1>(a.R_component_1() + b), a.R_component_2(), a.R_component_3(), a.R_component_4());
  384. }
  385. template <class T1, class T2>
  386. inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
  387. operator + (const T1& a, const quaternion<T2>& b)
  388. {
  389. return quaternion<T2>(static_cast<T2>(b.R_component_1() + a), b.R_component_2(), b.R_component_3(), b.R_component_4());
  390. }
  391. template <class T1, class T2>
  392. inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
  393. operator + (const quaternion<T1>& a, const std::complex<T2>& b)
  394. {
  395. return quaternion<T1>(a.R_component_1() + std::real(b), a.R_component_2() + std::imag(b), a.R_component_3(), a.R_component_4());
  396. }
  397. template <class T1, class T2>
  398. inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
  399. operator + (const std::complex<T1>& a, const quaternion<T2>& b)
  400. {
  401. return quaternion<T1>(b.R_component_1() + real(a), b.R_component_2() + imag(a), b.R_component_3(), b.R_component_4());
  402. }
  403. template <class T>
  404. inline BOOST_CONSTEXPR quaternion<T> operator + (const quaternion<T>& a, const quaternion<T>& b)
  405. {
  406. return quaternion<T>(a.R_component_1() + b.R_component_1(), a.R_component_2() + b.R_component_2(), a.R_component_3() + b.R_component_3(), a.R_component_4() + b.R_component_4());
  407. }
  408. // operator-
  409. template <class T1, class T2>
  410. inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
  411. operator - (const quaternion<T1>& a, const T2& b)
  412. {
  413. return quaternion<T1>(static_cast<T1>(a.R_component_1() - b), a.R_component_2(), a.R_component_3(), a.R_component_4());
  414. }
  415. template <class T1, class T2>
  416. inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
  417. operator - (const T1& a, const quaternion<T2>& b)
  418. {
  419. return quaternion<T2>(static_cast<T2>(a - b.R_component_1()), -b.R_component_2(), -b.R_component_3(), -b.R_component_4());
  420. }
  421. template <class T1, class T2>
  422. inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
  423. operator - (const quaternion<T1>& a, const std::complex<T2>& b)
  424. {
  425. return quaternion<T1>(a.R_component_1() - std::real(b), a.R_component_2() - std::imag(b), a.R_component_3(), a.R_component_4());
  426. }
  427. template <class T1, class T2>
  428. inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
  429. operator - (const std::complex<T1>& a, const quaternion<T2>& b)
  430. {
  431. return quaternion<T1>(real(a) - b.R_component_1(), imag(a) - b.R_component_2(), -b.R_component_3(), -b.R_component_4());
  432. }
  433. template <class T>
  434. inline BOOST_CONSTEXPR quaternion<T> operator - (const quaternion<T>& a, const quaternion<T>& b)
  435. {
  436. return quaternion<T>(a.R_component_1() - b.R_component_1(), a.R_component_2() - b.R_component_2(), a.R_component_3() - b.R_component_3(), a.R_component_4() - b.R_component_4());
  437. }
  438. // operator*
  439. template <class T1, class T2>
  440. inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
  441. operator * (const quaternion<T1>& a, const T2& b)
  442. {
  443. return quaternion<T1>(static_cast<T1>(a.R_component_1() * b), a.R_component_2() * b, a.R_component_3() * b, a.R_component_4() * b);
  444. }
  445. template <class T1, class T2>
  446. inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
  447. operator * (const T1& a, const quaternion<T2>& b)
  448. {
  449. return quaternion<T2>(static_cast<T2>(a * b.R_component_1()), a * b.R_component_2(), a * b.R_component_3(), a * b.R_component_4());
  450. }
  451. template <class T1, class T2>
  452. inline BOOST_CXX14_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
  453. operator * (const quaternion<T1>& a, const std::complex<T2>& b)
  454. {
  455. quaternion<T1> result(a);
  456. result *= b;
  457. return result;
  458. }
  459. template <class T1, class T2>
  460. inline BOOST_CXX14_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
  461. operator * (const std::complex<T1>& a, const quaternion<T2>& b)
  462. {
  463. quaternion<T1> result(a);
  464. result *= b;
  465. return result;
  466. }
  467. template <class T>
  468. inline BOOST_CXX14_CONSTEXPR quaternion<T> operator * (const quaternion<T>& a, const quaternion<T>& b)
  469. {
  470. quaternion<T> result(a);
  471. result *= b;
  472. return result;
  473. }
  474. // operator/
  475. template <class T1, class T2>
  476. inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
  477. operator / (const quaternion<T1>& a, const T2& b)
  478. {
  479. return quaternion<T1>(a.R_component_1() / b, a.R_component_2() / b, a.R_component_3() / b, a.R_component_4() / b);
  480. }
  481. template <class T1, class T2>
  482. inline BOOST_CXX14_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
  483. operator / (const T1& a, const quaternion<T2>& b)
  484. {
  485. quaternion<T2> result(a);
  486. result /= b;
  487. return result;
  488. }
  489. template <class T1, class T2>
  490. inline BOOST_CXX14_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T2, T1>::value, quaternion<T1> >::type
  491. operator / (const quaternion<T1>& a, const std::complex<T2>& b)
  492. {
  493. quaternion<T1> result(a);
  494. result /= b;
  495. return result;
  496. }
  497. template <class T1, class T2>
  498. inline BOOST_CXX14_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<T1, T2>::value, quaternion<T2> >::type
  499. operator / (const std::complex<T1>& a, const quaternion<T2>& b)
  500. {
  501. quaternion<T2> result(a);
  502. result /= b;
  503. return result;
  504. }
  505. template <class T>
  506. inline BOOST_CXX14_CONSTEXPR quaternion<T> operator / (const quaternion<T>& a, const quaternion<T>& b)
  507. {
  508. quaternion<T> result(a);
  509. result /= b;
  510. return result;
  511. }
  512. template<typename T>
  513. inline BOOST_CONSTEXPR const quaternion<T>& operator + (quaternion<T> const & q)
  514. {
  515. return q;
  516. }
  517. template<typename T>
  518. inline BOOST_CONSTEXPR quaternion<T> operator - (quaternion<T> const & q)
  519. {
  520. return(quaternion<T>(-q.R_component_1(),-q.R_component_2(),-q.R_component_3(),-q.R_component_4()));
  521. }
  522. template<typename R, typename T>
  523. inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<R, T>::value, bool>::type operator == (R const & lhs, quaternion<T> const & rhs)
  524. {
  525. return (
  526. (rhs.R_component_1() == lhs)&&
  527. (rhs.R_component_2() == static_cast<T>(0))&&
  528. (rhs.R_component_3() == static_cast<T>(0))&&
  529. (rhs.R_component_4() == static_cast<T>(0))
  530. );
  531. }
  532. template<typename T, typename R>
  533. inline BOOST_CONSTEXPR typename boost::enable_if_c<boost::is_convertible<R, T>::value, bool>::type operator == (quaternion<T> const & lhs, R const & rhs)
  534. {
  535. return rhs == lhs;
  536. }
  537. template<typename T>
  538. inline BOOST_CONSTEXPR bool operator == (::std::complex<T> const & lhs, quaternion<T> const & rhs)
  539. {
  540. return (
  541. (rhs.R_component_1() == lhs.real())&&
  542. (rhs.R_component_2() == lhs.imag())&&
  543. (rhs.R_component_3() == static_cast<T>(0))&&
  544. (rhs.R_component_4() == static_cast<T>(0))
  545. );
  546. }
  547. template<typename T>
  548. inline BOOST_CONSTEXPR bool operator == (quaternion<T> const & lhs, ::std::complex<T> const & rhs)
  549. {
  550. return rhs == lhs;
  551. }
  552. template<typename T>
  553. inline BOOST_CONSTEXPR bool operator == (quaternion<T> const & lhs, quaternion<T> const & rhs)
  554. {
  555. return (
  556. (rhs.R_component_1() == lhs.R_component_1())&&
  557. (rhs.R_component_2() == lhs.R_component_2())&&
  558. (rhs.R_component_3() == lhs.R_component_3())&&
  559. (rhs.R_component_4() == lhs.R_component_4())
  560. );
  561. }
  562. template<typename R, typename T> inline BOOST_CONSTEXPR bool operator != (R const & lhs, quaternion<T> const & rhs) { return !(lhs == rhs); }
  563. template<typename T, typename R> inline BOOST_CONSTEXPR bool operator != (quaternion<T> const & lhs, R const & rhs) { return !(lhs == rhs); }
  564. template<typename T> inline BOOST_CONSTEXPR bool operator != (::std::complex<T> const & lhs, quaternion<T> const & rhs) { return !(lhs == rhs); }
  565. template<typename T> inline BOOST_CONSTEXPR bool operator != (quaternion<T> const & lhs, ::std::complex<T> const & rhs) { return !(lhs == rhs); }
  566. template<typename T> inline BOOST_CONSTEXPR bool operator != (quaternion<T> const & lhs, quaternion<T> const & rhs) { return !(lhs == rhs); }
  567. // Note: we allow the following formats, with a, b, c, and d reals
  568. // a
  569. // (a), (a,b), (a,b,c), (a,b,c,d)
  570. // (a,(c)), (a,(c,d)), ((a)), ((a),c), ((a),(c)), ((a),(c,d)), ((a,b)), ((a,b),c), ((a,b),(c)), ((a,b),(c,d))
  571. template<typename T, typename charT, class traits>
  572. ::std::basic_istream<charT,traits> & operator >> ( ::std::basic_istream<charT,traits> & is,
  573. quaternion<T> & q)
  574. {
  575. #ifdef BOOST_NO_STD_LOCALE
  576. #else
  577. const ::std::ctype<charT> & ct = ::std::use_facet< ::std::ctype<charT> >(is.getloc());
  578. #endif /* BOOST_NO_STD_LOCALE */
  579. T a = T();
  580. T b = T();
  581. T c = T();
  582. T d = T();
  583. ::std::complex<T> u = ::std::complex<T>();
  584. ::std::complex<T> v = ::std::complex<T>();
  585. charT ch = charT();
  586. char cc;
  587. is >> ch; // get the first lexeme
  588. if (!is.good()) goto finish;
  589. #ifdef BOOST_NO_STD_LOCALE
  590. cc = ch;
  591. #else
  592. cc = ct.narrow(ch, char());
  593. #endif /* BOOST_NO_STD_LOCALE */
  594. if (cc == '(') // read "(", possible: (a), (a,b), (a,b,c), (a,b,c,d), (a,(c)), (a,(c,d)), ((a)), ((a),c), ((a),(c)), ((a),(c,d)), ((a,b)), ((a,b),c), ((a,b),(c)), ((a,b,),(c,d,))
  595. {
  596. is >> ch; // get the second lexeme
  597. if (!is.good()) goto finish;
  598. #ifdef BOOST_NO_STD_LOCALE
  599. cc = ch;
  600. #else
  601. cc = ct.narrow(ch, char());
  602. #endif /* BOOST_NO_STD_LOCALE */
  603. if (cc == '(') // read "((", possible: ((a)), ((a),c), ((a),(c)), ((a),(c,d)), ((a,b)), ((a,b),c), ((a,b),(c)), ((a,b,),(c,d,))
  604. {
  605. is.putback(ch);
  606. is >> u; // we extract the first and second components
  607. a = u.real();
  608. b = u.imag();
  609. if (!is.good()) goto finish;
  610. is >> ch; // get the next lexeme
  611. if (!is.good()) goto finish;
  612. #ifdef BOOST_NO_STD_LOCALE
  613. cc = ch;
  614. #else
  615. cc = ct.narrow(ch, char());
  616. #endif /* BOOST_NO_STD_LOCALE */
  617. if (cc == ')') // format: ((a)) or ((a,b))
  618. {
  619. q = quaternion<T>(a,b);
  620. }
  621. else if (cc == ',') // read "((a)," or "((a,b),", possible: ((a),c), ((a),(c)), ((a),(c,d)), ((a,b),c), ((a,b),(c)), ((a,b,),(c,d,))
  622. {
  623. is >> v; // we extract the third and fourth components
  624. c = v.real();
  625. d = v.imag();
  626. if (!is.good()) goto finish;
  627. is >> ch; // get the last lexeme
  628. if (!is.good()) goto finish;
  629. #ifdef BOOST_NO_STD_LOCALE
  630. cc = ch;
  631. #else
  632. cc = ct.narrow(ch, char());
  633. #endif /* BOOST_NO_STD_LOCALE */
  634. if (cc == ')') // format: ((a),c), ((a),(c)), ((a),(c,d)), ((a,b),c), ((a,b),(c)) or ((a,b,),(c,d,))
  635. {
  636. q = quaternion<T>(a,b,c,d);
  637. }
  638. else // error
  639. {
  640. is.setstate(::std::ios_base::failbit);
  641. }
  642. }
  643. else // error
  644. {
  645. is.setstate(::std::ios_base::failbit);
  646. }
  647. }
  648. else // read "(a", possible: (a), (a,b), (a,b,c), (a,b,c,d), (a,(c)), (a,(c,d))
  649. {
  650. is.putback(ch);
  651. is >> a; // we extract the first component
  652. if (!is.good()) goto finish;
  653. is >> ch; // get the third lexeme
  654. if (!is.good()) goto finish;
  655. #ifdef BOOST_NO_STD_LOCALE
  656. cc = ch;
  657. #else
  658. cc = ct.narrow(ch, char());
  659. #endif /* BOOST_NO_STD_LOCALE */
  660. if (cc == ')') // format: (a)
  661. {
  662. q = quaternion<T>(a);
  663. }
  664. else if (cc == ',') // read "(a,", possible: (a,b), (a,b,c), (a,b,c,d), (a,(c)), (a,(c,d))
  665. {
  666. is >> ch; // get the fourth lexeme
  667. if (!is.good()) goto finish;
  668. #ifdef BOOST_NO_STD_LOCALE
  669. cc = ch;
  670. #else
  671. cc = ct.narrow(ch, char());
  672. #endif /* BOOST_NO_STD_LOCALE */
  673. if (cc == '(') // read "(a,(", possible: (a,(c)), (a,(c,d))
  674. {
  675. is.putback(ch);
  676. is >> v; // we extract the third and fourth component
  677. c = v.real();
  678. d = v.imag();
  679. if (!is.good()) goto finish;
  680. is >> ch; // get the ninth lexeme
  681. if (!is.good()) goto finish;
  682. #ifdef BOOST_NO_STD_LOCALE
  683. cc = ch;
  684. #else
  685. cc = ct.narrow(ch, char());
  686. #endif /* BOOST_NO_STD_LOCALE */
  687. if (cc == ')') // format: (a,(c)) or (a,(c,d))
  688. {
  689. q = quaternion<T>(a,b,c,d);
  690. }
  691. else // error
  692. {
  693. is.setstate(::std::ios_base::failbit);
  694. }
  695. }
  696. else // read "(a,b", possible: (a,b), (a,b,c), (a,b,c,d)
  697. {
  698. is.putback(ch);
  699. is >> b; // we extract the second component
  700. if (!is.good()) goto finish;
  701. is >> ch; // get the fifth lexeme
  702. if (!is.good()) goto finish;
  703. #ifdef BOOST_NO_STD_LOCALE
  704. cc = ch;
  705. #else
  706. cc = ct.narrow(ch, char());
  707. #endif /* BOOST_NO_STD_LOCALE */
  708. if (cc == ')') // format: (a,b)
  709. {
  710. q = quaternion<T>(a,b);
  711. }
  712. else if (cc == ',') // read "(a,b,", possible: (a,b,c), (a,b,c,d)
  713. {
  714. is >> c; // we extract the third component
  715. if (!is.good()) goto finish;
  716. is >> ch; // get the seventh lexeme
  717. if (!is.good()) goto finish;
  718. #ifdef BOOST_NO_STD_LOCALE
  719. cc = ch;
  720. #else
  721. cc = ct.narrow(ch, char());
  722. #endif /* BOOST_NO_STD_LOCALE */
  723. if (cc == ')') // format: (a,b,c)
  724. {
  725. q = quaternion<T>(a,b,c);
  726. }
  727. else if (cc == ',') // read "(a,b,c,", possible: (a,b,c,d)
  728. {
  729. is >> d; // we extract the fourth component
  730. if (!is.good()) goto finish;
  731. is >> ch; // get the ninth lexeme
  732. if (!is.good()) goto finish;
  733. #ifdef BOOST_NO_STD_LOCALE
  734. cc = ch;
  735. #else
  736. cc = ct.narrow(ch, char());
  737. #endif /* BOOST_NO_STD_LOCALE */
  738. if (cc == ')') // format: (a,b,c,d)
  739. {
  740. q = quaternion<T>(a,b,c,d);
  741. }
  742. else // error
  743. {
  744. is.setstate(::std::ios_base::failbit);
  745. }
  746. }
  747. else // error
  748. {
  749. is.setstate(::std::ios_base::failbit);
  750. }
  751. }
  752. else // error
  753. {
  754. is.setstate(::std::ios_base::failbit);
  755. }
  756. }
  757. }
  758. else // error
  759. {
  760. is.setstate(::std::ios_base::failbit);
  761. }
  762. }
  763. }
  764. else // format: a
  765. {
  766. is.putback(ch);
  767. is >> a; // we extract the first component
  768. if (!is.good()) goto finish;
  769. q = quaternion<T>(a);
  770. }
  771. finish:
  772. return(is);
  773. }
  774. template<typename T, typename charT, class traits>
  775. ::std::basic_ostream<charT,traits> & operator << ( ::std::basic_ostream<charT,traits> & os,
  776. quaternion<T> const & q)
  777. {
  778. ::std::basic_ostringstream<charT,traits> s;
  779. s.flags(os.flags());
  780. #ifdef BOOST_NO_STD_LOCALE
  781. #else
  782. s.imbue(os.getloc());
  783. #endif /* BOOST_NO_STD_LOCALE */
  784. s.precision(os.precision());
  785. s << '(' << q.R_component_1() << ','
  786. << q.R_component_2() << ','
  787. << q.R_component_3() << ','
  788. << q.R_component_4() << ')';
  789. return os << s.str();
  790. }
  791. // values
  792. template<typename T>
  793. inline BOOST_CONSTEXPR T real(quaternion<T> const & q)
  794. {
  795. return(q.real());
  796. }
  797. template<typename T>
  798. inline BOOST_CONSTEXPR quaternion<T> unreal(quaternion<T> const & q)
  799. {
  800. return(q.unreal());
  801. }
  802. template<typename T>
  803. inline T sup(quaternion<T> const & q)
  804. {
  805. using ::std::abs;
  806. return (std::max)((std::max)(abs(q.R_component_1()), abs(q.R_component_2())), (std::max)(abs(q.R_component_3()), abs(q.R_component_4())));
  807. }
  808. template<typename T>
  809. inline T l1(quaternion<T> const & q)
  810. {
  811. using ::std::abs;
  812. return abs(q.R_component_1()) + abs(q.R_component_2()) + abs(q.R_component_3()) + abs(q.R_component_4());
  813. }
  814. template<typename T>
  815. inline T abs(quaternion<T> const & q)
  816. {
  817. using ::std::abs;
  818. using ::std::sqrt;
  819. T maxim = sup(q); // overflow protection
  820. if (maxim == static_cast<T>(0))
  821. {
  822. return(maxim);
  823. }
  824. else
  825. {
  826. T mixam = static_cast<T>(1)/maxim; // prefer multiplications over divisions
  827. T a = q.R_component_1() * mixam;
  828. T b = q.R_component_2() * mixam;
  829. T c = q.R_component_3() * mixam;
  830. T d = q.R_component_4() * mixam;
  831. a *= a;
  832. b *= b;
  833. c *= c;
  834. d *= d;
  835. return(maxim * sqrt(a + b + c + d));
  836. }
  837. //return(sqrt(norm(q)));
  838. }
  839. // Note: This is the Cayley norm, not the Euclidean norm...
  840. template<typename T>
  841. inline BOOST_CXX14_CONSTEXPR T norm(quaternion<T>const & q)
  842. {
  843. return(real(q*conj(q)));
  844. }
  845. template<typename T>
  846. inline BOOST_CONSTEXPR quaternion<T> conj(quaternion<T> const & q)
  847. {
  848. return(quaternion<T>( +q.R_component_1(),
  849. -q.R_component_2(),
  850. -q.R_component_3(),
  851. -q.R_component_4()));
  852. }
  853. template<typename T>
  854. inline quaternion<T> spherical( T const & rho,
  855. T const & theta,
  856. T const & phi1,
  857. T const & phi2)
  858. {
  859. using ::std::cos;
  860. using ::std::sin;
  861. //T a = cos(theta)*cos(phi1)*cos(phi2);
  862. //T b = sin(theta)*cos(phi1)*cos(phi2);
  863. //T c = sin(phi1)*cos(phi2);
  864. //T d = sin(phi2);
  865. T courrant = static_cast<T>(1);
  866. T d = sin(phi2);
  867. courrant *= cos(phi2);
  868. T c = sin(phi1)*courrant;
  869. courrant *= cos(phi1);
  870. T b = sin(theta)*courrant;
  871. T a = cos(theta)*courrant;
  872. return(rho*quaternion<T>(a,b,c,d));
  873. }
  874. template<typename T>
  875. inline quaternion<T> semipolar( T const & rho,
  876. T const & alpha,
  877. T const & theta1,
  878. T const & theta2)
  879. {
  880. using ::std::cos;
  881. using ::std::sin;
  882. T a = cos(alpha)*cos(theta1);
  883. T b = cos(alpha)*sin(theta1);
  884. T c = sin(alpha)*cos(theta2);
  885. T d = sin(alpha)*sin(theta2);
  886. return(rho*quaternion<T>(a,b,c,d));
  887. }
  888. template<typename T>
  889. inline quaternion<T> multipolar( T const & rho1,
  890. T const & theta1,
  891. T const & rho2,
  892. T const & theta2)
  893. {
  894. using ::std::cos;
  895. using ::std::sin;
  896. T a = rho1*cos(theta1);
  897. T b = rho1*sin(theta1);
  898. T c = rho2*cos(theta2);
  899. T d = rho2*sin(theta2);
  900. return(quaternion<T>(a,b,c,d));
  901. }
  902. template<typename T>
  903. inline quaternion<T> cylindrospherical( T const & t,
  904. T const & radius,
  905. T const & longitude,
  906. T const & latitude)
  907. {
  908. using ::std::cos;
  909. using ::std::sin;
  910. T b = radius*cos(longitude)*cos(latitude);
  911. T c = radius*sin(longitude)*cos(latitude);
  912. T d = radius*sin(latitude);
  913. return(quaternion<T>(t,b,c,d));
  914. }
  915. template<typename T>
  916. inline quaternion<T> cylindrical(T const & r,
  917. T const & angle,
  918. T const & h1,
  919. T const & h2)
  920. {
  921. using ::std::cos;
  922. using ::std::sin;
  923. T a = r*cos(angle);
  924. T b = r*sin(angle);
  925. return(quaternion<T>(a,b,h1,h2));
  926. }
  927. // transcendentals
  928. // (please see the documentation)
  929. template<typename T>
  930. inline quaternion<T> exp(quaternion<T> const & q)
  931. {
  932. using ::std::exp;
  933. using ::std::cos;
  934. using ::boost::math::sinc_pi;
  935. T u = exp(real(q));
  936. T z = abs(unreal(q));
  937. T w = sinc_pi(z);
  938. return(u*quaternion<T>(cos(z),
  939. w*q.R_component_2(), w*q.R_component_3(),
  940. w*q.R_component_4()));
  941. }
  942. template<typename T>
  943. inline quaternion<T> cos(quaternion<T> const & q)
  944. {
  945. using ::std::sin;
  946. using ::std::cos;
  947. using ::std::cosh;
  948. using ::boost::math::sinhc_pi;
  949. T z = abs(unreal(q));
  950. T w = -sin(q.real())*sinhc_pi(z);
  951. return(quaternion<T>(cos(q.real())*cosh(z),
  952. w*q.R_component_2(), w*q.R_component_3(),
  953. w*q.R_component_4()));
  954. }
  955. template<typename T>
  956. inline quaternion<T> sin(quaternion<T> const & q)
  957. {
  958. using ::std::sin;
  959. using ::std::cos;
  960. using ::std::cosh;
  961. using ::boost::math::sinhc_pi;
  962. T z = abs(unreal(q));
  963. T w = +cos(q.real())*sinhc_pi(z);
  964. return(quaternion<T>(sin(q.real())*cosh(z),
  965. w*q.R_component_2(), w*q.R_component_3(),
  966. w*q.R_component_4()));
  967. }
  968. template<typename T>
  969. inline quaternion<T> tan(quaternion<T> const & q)
  970. {
  971. return(sin(q)/cos(q));
  972. }
  973. template<typename T>
  974. inline quaternion<T> cosh(quaternion<T> const & q)
  975. {
  976. return((exp(+q)+exp(-q))/static_cast<T>(2));
  977. }
  978. template<typename T>
  979. inline quaternion<T> sinh(quaternion<T> const & q)
  980. {
  981. return((exp(+q)-exp(-q))/static_cast<T>(2));
  982. }
  983. template<typename T>
  984. inline quaternion<T> tanh(quaternion<T> const & q)
  985. {
  986. return(sinh(q)/cosh(q));
  987. }
  988. template<typename T>
  989. quaternion<T> pow(quaternion<T> const & q,
  990. int n)
  991. {
  992. if (n > 1)
  993. {
  994. int m = n>>1;
  995. quaternion<T> result = pow(q, m);
  996. result *= result;
  997. if (n != (m<<1))
  998. {
  999. result *= q; // n odd
  1000. }
  1001. return(result);
  1002. }
  1003. else if (n == 1)
  1004. {
  1005. return(q);
  1006. }
  1007. else if (n == 0)
  1008. {
  1009. return(quaternion<T>(static_cast<T>(1)));
  1010. }
  1011. else /* n < 0 */
  1012. {
  1013. return(pow(quaternion<T>(static_cast<T>(1))/q,-n));
  1014. }
  1015. }
  1016. }
  1017. }
  1018. #endif /* BOOST_QUATERNION_HPP */