quaternion.hpp 48 KB

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