/src/FreeImage/Source/OpenEXR/Imath/ImathQuat.h

https://bitbucket.org/cabalistic/ogredeps/ · C++ Header · 963 lines · 549 code · 202 blank · 212 comment · 30 complexity · d69c9a00bb641eeba2ed520f02d8d1ac MD5 · raw file

  1. ///////////////////////////////////////////////////////////////////////////
  2. //
  3. // Copyright (c) 2002, Industrial Light & Magic, a division of Lucas
  4. // Digital Ltd. LLC
  5. //
  6. // All rights reserved.
  7. //
  8. // Redistribution and use in source and binary forms, with or without
  9. // modification, are permitted provided that the following conditions are
  10. // met:
  11. // * Redistributions of source code must retain the above copyright
  12. // notice, this list of conditions and the following disclaimer.
  13. // * Redistributions in binary form must reproduce the above
  14. // copyright notice, this list of conditions and the following disclaimer
  15. // in the documentation and/or other materials provided with the
  16. // distribution.
  17. // * Neither the name of Industrial Light & Magic nor the names of
  18. // its contributors may be used to endorse or promote products derived
  19. // from this software without specific prior written permission.
  20. //
  21. // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  22. // "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  23. // LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
  24. // A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
  25. // OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
  26. // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  27. // LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
  28. // DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
  29. // THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  30. // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
  31. // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  32. //
  33. ///////////////////////////////////////////////////////////////////////////
  34. #ifndef INCLUDED_IMATHQUAT_H
  35. #define INCLUDED_IMATHQUAT_H
  36. //----------------------------------------------------------------------
  37. //
  38. // template class Quat<T>
  39. //
  40. // "Quaternions came from Hamilton ... and have been an unmixed
  41. // evil to those who have touched them in any way. Vector is a
  42. // useless survival ... and has never been of the slightest use
  43. // to any creature."
  44. //
  45. // - Lord Kelvin
  46. //
  47. // This class implements the quaternion numerical type -- you
  48. // will probably want to use this class to represent orientations
  49. // in R3 and to convert between various euler angle reps. You
  50. // should probably use Imath::Euler<> for that.
  51. //
  52. //----------------------------------------------------------------------
  53. #include "ImathExc.h"
  54. #include "ImathMatrix.h"
  55. #include <iostream>
  56. namespace Imath {
  57. #if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
  58. // Disable MS VC++ warnings about conversion from double to float
  59. #pragma warning(disable:4244)
  60. #endif
  61. template <class T>
  62. class Quat
  63. {
  64. public:
  65. T r; // real part
  66. Vec3<T> v; // imaginary vector
  67. //-----------------------------------------------------
  68. // Constructors - default constructor is identity quat
  69. //-----------------------------------------------------
  70. Quat ();
  71. template <class S>
  72. Quat (const Quat<S> &q);
  73. Quat (T s, T i, T j, T k);
  74. Quat (T s, Vec3<T> d);
  75. static Quat<T> identity ();
  76. //-------------------------------------------------
  77. // Basic Algebra - Operators and Methods
  78. // The operator return values are *NOT* normalized
  79. //
  80. // operator^ and euclideanInnnerProduct() both
  81. // implement the 4D dot product
  82. //
  83. // operator/ uses the inverse() quaternion
  84. //
  85. // operator~ is conjugate -- if (S+V) is quat then
  86. // the conjugate (S+V)* == (S-V)
  87. //
  88. // some operators (*,/,*=,/=) treat the quat as
  89. // a 4D vector when one of the operands is scalar
  90. //-------------------------------------------------
  91. const Quat<T> & operator = (const Quat<T> &q);
  92. const Quat<T> & operator *= (const Quat<T> &q);
  93. const Quat<T> & operator *= (T t);
  94. const Quat<T> & operator /= (const Quat<T> &q);
  95. const Quat<T> & operator /= (T t);
  96. const Quat<T> & operator += (const Quat<T> &q);
  97. const Quat<T> & operator -= (const Quat<T> &q);
  98. T & operator [] (int index); // as 4D vector
  99. T operator [] (int index) const;
  100. template <class S> bool operator == (const Quat<S> &q) const;
  101. template <class S> bool operator != (const Quat<S> &q) const;
  102. Quat<T> & invert (); // this -> 1 / this
  103. Quat<T> inverse () const;
  104. Quat<T> & normalize (); // returns this
  105. Quat<T> normalized () const;
  106. T length () const; // in R4
  107. Vec3<T> rotateVector(const Vec3<T> &original) const;
  108. T euclideanInnerProduct(const Quat<T> &q) const;
  109. //-----------------------
  110. // Rotation conversion
  111. //-----------------------
  112. Quat<T> & setAxisAngle (const Vec3<T> &axis, T radians);
  113. Quat<T> & setRotation (const Vec3<T> &fromDirection,
  114. const Vec3<T> &toDirection);
  115. T angle () const;
  116. Vec3<T> axis () const;
  117. Matrix33<T> toMatrix33 () const;
  118. Matrix44<T> toMatrix44 () const;
  119. Quat<T> log () const;
  120. Quat<T> exp () const;
  121. private:
  122. void setRotationInternal (const Vec3<T> &f0,
  123. const Vec3<T> &t0,
  124. Quat<T> &q);
  125. };
  126. template<class T>
  127. Quat<T> slerp (const Quat<T> &q1, const Quat<T> &q2, T t);
  128. template<class T>
  129. Quat<T> slerpShortestArc
  130. (const Quat<T> &q1, const Quat<T> &q2, T t);
  131. template<class T>
  132. Quat<T> squad (const Quat<T> &q1, const Quat<T> &q2,
  133. const Quat<T> &qa, const Quat<T> &qb, T t);
  134. template<class T>
  135. void intermediate (const Quat<T> &q0, const Quat<T> &q1,
  136. const Quat<T> &q2, const Quat<T> &q3,
  137. Quat<T> &qa, Quat<T> &qb);
  138. template<class T>
  139. Matrix33<T> operator * (const Matrix33<T> &M, const Quat<T> &q);
  140. template<class T>
  141. Matrix33<T> operator * (const Quat<T> &q, const Matrix33<T> &M);
  142. template<class T>
  143. std::ostream & operator << (std::ostream &o, const Quat<T> &q);
  144. template<class T>
  145. Quat<T> operator * (const Quat<T> &q1, const Quat<T> &q2);
  146. template<class T>
  147. Quat<T> operator / (const Quat<T> &q1, const Quat<T> &q2);
  148. template<class T>
  149. Quat<T> operator / (const Quat<T> &q, T t);
  150. template<class T>
  151. Quat<T> operator * (const Quat<T> &q, T t);
  152. template<class T>
  153. Quat<T> operator * (T t, const Quat<T> &q);
  154. template<class T>
  155. Quat<T> operator + (const Quat<T> &q1, const Quat<T> &q2);
  156. template<class T>
  157. Quat<T> operator - (const Quat<T> &q1, const Quat<T> &q2);
  158. template<class T>
  159. Quat<T> operator ~ (const Quat<T> &q);
  160. template<class T>
  161. Quat<T> operator - (const Quat<T> &q);
  162. template<class T>
  163. Vec3<T> operator * (const Vec3<T> &v, const Quat<T> &q);
  164. //--------------------
  165. // Convenient typedefs
  166. //--------------------
  167. typedef Quat<float> Quatf;
  168. typedef Quat<double> Quatd;
  169. //---------------
  170. // Implementation
  171. //---------------
  172. template<class T>
  173. inline
  174. Quat<T>::Quat (): r (1), v (0, 0, 0)
  175. {
  176. // empty
  177. }
  178. template<class T>
  179. template <class S>
  180. inline
  181. Quat<T>::Quat (const Quat<S> &q): r (q.r), v (q.v)
  182. {
  183. // empty
  184. }
  185. template<class T>
  186. inline
  187. Quat<T>::Quat (T s, T i, T j, T k): r (s), v (i, j, k)
  188. {
  189. // empty
  190. }
  191. template<class T>
  192. inline
  193. Quat<T>::Quat (T s, Vec3<T> d): r (s), v (d)
  194. {
  195. // empty
  196. }
  197. template<class T>
  198. inline Quat<T>
  199. Quat<T>::identity ()
  200. {
  201. return Quat<T>();
  202. }
  203. template<class T>
  204. inline const Quat<T> &
  205. Quat<T>::operator = (const Quat<T> &q)
  206. {
  207. r = q.r;
  208. v = q.v;
  209. return *this;
  210. }
  211. template<class T>
  212. inline const Quat<T> &
  213. Quat<T>::operator *= (const Quat<T> &q)
  214. {
  215. T rtmp = r * q.r - (v ^ q.v);
  216. v = r * q.v + v * q.r + v % q.v;
  217. r = rtmp;
  218. return *this;
  219. }
  220. template<class T>
  221. inline const Quat<T> &
  222. Quat<T>::operator *= (T t)
  223. {
  224. r *= t;
  225. v *= t;
  226. return *this;
  227. }
  228. template<class T>
  229. inline const Quat<T> &
  230. Quat<T>::operator /= (const Quat<T> &q)
  231. {
  232. *this = *this * q.inverse();
  233. return *this;
  234. }
  235. template<class T>
  236. inline const Quat<T> &
  237. Quat<T>::operator /= (T t)
  238. {
  239. r /= t;
  240. v /= t;
  241. return *this;
  242. }
  243. template<class T>
  244. inline const Quat<T> &
  245. Quat<T>::operator += (const Quat<T> &q)
  246. {
  247. r += q.r;
  248. v += q.v;
  249. return *this;
  250. }
  251. template<class T>
  252. inline const Quat<T> &
  253. Quat<T>::operator -= (const Quat<T> &q)
  254. {
  255. r -= q.r;
  256. v -= q.v;
  257. return *this;
  258. }
  259. template<class T>
  260. inline T &
  261. Quat<T>::operator [] (int index)
  262. {
  263. return index ? v[index - 1] : r;
  264. }
  265. template<class T>
  266. inline T
  267. Quat<T>::operator [] (int index) const
  268. {
  269. return index ? v[index - 1] : r;
  270. }
  271. template <class T>
  272. template <class S>
  273. inline bool
  274. Quat<T>::operator == (const Quat<S> &q) const
  275. {
  276. return r == q.r && v == q.v;
  277. }
  278. template <class T>
  279. template <class S>
  280. inline bool
  281. Quat<T>::operator != (const Quat<S> &q) const
  282. {
  283. return r != q.r || v != q.v;
  284. }
  285. template<class T>
  286. inline T
  287. operator ^ (const Quat<T>& q1 ,const Quat<T>& q2)
  288. {
  289. return q1.r * q2.r + (q1.v ^ q2.v);
  290. }
  291. template <class T>
  292. inline T
  293. Quat<T>::length () const
  294. {
  295. return Math<T>::sqrt (r * r + (v ^ v));
  296. }
  297. template <class T>
  298. inline Quat<T> &
  299. Quat<T>::normalize ()
  300. {
  301. if (T l = length())
  302. {
  303. r /= l;
  304. v /= l;
  305. }
  306. else
  307. {
  308. r = 1;
  309. v = Vec3<T> (0);
  310. }
  311. return *this;
  312. }
  313. template <class T>
  314. inline Quat<T>
  315. Quat<T>::normalized () const
  316. {
  317. if (T l = length())
  318. return Quat (r / l, v / l);
  319. return Quat();
  320. }
  321. template<class T>
  322. inline Quat<T>
  323. Quat<T>::inverse () const
  324. {
  325. //
  326. // 1 Q*
  327. // - = ---- where Q* is conjugate (operator~)
  328. // Q Q* Q and (Q* Q) == Q ^ Q (4D dot)
  329. //
  330. T qdot = *this ^ *this;
  331. return Quat (r / qdot, -v / qdot);
  332. }
  333. template<class T>
  334. inline Quat<T> &
  335. Quat<T>::invert ()
  336. {
  337. T qdot = (*this) ^ (*this);
  338. r /= qdot;
  339. v = -v / qdot;
  340. return *this;
  341. }
  342. template<class T>
  343. inline Vec3<T>
  344. Quat<T>::rotateVector(const Vec3<T>& original) const
  345. {
  346. //
  347. // Given a vector p and a quaternion q (aka this),
  348. // calculate p' = qpq*
  349. //
  350. // Assumes unit quaternions (because non-unit
  351. // quaternions cannot be used to rotate vectors
  352. // anyway).
  353. //
  354. Quat<T> vec (0, original); // temporarily promote grade of original
  355. Quat<T> inv (*this);
  356. inv.v *= -1; // unit multiplicative inverse
  357. Quat<T> result = *this * vec * inv;
  358. return result.v;
  359. }
  360. template<class T>
  361. inline T
  362. Quat<T>::euclideanInnerProduct (const Quat<T> &q) const
  363. {
  364. return r * q.r + v.x * q.v.x + v.y * q.v.y + v.z * q.v.z;
  365. }
  366. template<class T>
  367. T
  368. angle4D (const Quat<T> &q1, const Quat<T> &q2)
  369. {
  370. //
  371. // Compute the angle between two quaternions,
  372. // interpreting the quaternions as 4D vectors.
  373. //
  374. Quat<T> d = q1 - q2;
  375. T lengthD = Math<T>::sqrt (d ^ d);
  376. Quat<T> s = q1 + q2;
  377. T lengthS = Math<T>::sqrt (s ^ s);
  378. return 2 * Math<T>::atan2 (lengthD, lengthS);
  379. }
  380. template<class T>
  381. Quat<T>
  382. slerp (const Quat<T> &q1, const Quat<T> &q2, T t)
  383. {
  384. //
  385. // Spherical linear interpolation.
  386. // Assumes q1 and q2 are normalized and that q1 != -q2.
  387. //
  388. // This method does *not* interpolate along the shortest
  389. // arc between q1 and q2. If you desire interpolation
  390. // along the shortest arc, and q1^q2 is negative, then
  391. // consider calling slerpShortestArc(), below, or flipping
  392. // the second quaternion explicitly.
  393. //
  394. // The implementation of squad() depends on a slerp()
  395. // that interpolates as is, without the automatic
  396. // flipping.
  397. //
  398. // Don Hatch explains the method we use here on his
  399. // web page, The Right Way to Calculate Stuff, at
  400. // http://www.plunk.org/~hatch/rightway.php
  401. //
  402. T a = angle4D (q1, q2);
  403. T s = 1 - t;
  404. Quat<T> q = sinx_over_x (s * a) / sinx_over_x (a) * s * q1 +
  405. sinx_over_x (t * a) / sinx_over_x (a) * t * q2;
  406. return q.normalized();
  407. }
  408. template<class T>
  409. Quat<T>
  410. slerpShortestArc (const Quat<T> &q1, const Quat<T> &q2, T t)
  411. {
  412. //
  413. // Spherical linear interpolation along the shortest
  414. // arc from q1 to either q2 or -q2, whichever is closer.
  415. // Assumes q1 and q2 are unit quaternions.
  416. //
  417. if ((q1 ^ q2) >= 0)
  418. return slerp (q1, q2, t);
  419. else
  420. return slerp (q1, -q2, t);
  421. }
  422. template<class T>
  423. Quat<T>
  424. spline (const Quat<T> &q0, const Quat<T> &q1,
  425. const Quat<T> &q2, const Quat<T> &q3,
  426. T t)
  427. {
  428. //
  429. // Spherical Cubic Spline Interpolation -
  430. // from Advanced Animation and Rendering
  431. // Techniques by Watt and Watt, Page 366:
  432. // A spherical curve is constructed using three
  433. // spherical linear interpolations of a quadrangle
  434. // of unit quaternions: q1, qa, qb, q2.
  435. // Given a set of quaternion keys: q0, q1, q2, q3,
  436. // this routine does the interpolation between
  437. // q1 and q2 by constructing two intermediate
  438. // quaternions: qa and qb. The qa and qb are
  439. // computed by the intermediate function to
  440. // guarantee the continuity of tangents across
  441. // adjacent cubic segments. The qa represents in-tangent
  442. // for q1 and the qb represents the out-tangent for q2.
  443. //
  444. // The q1 q2 is the cubic segment being interpolated.
  445. // The q0 is from the previous adjacent segment and q3 is
  446. // from the next adjacent segment. The q0 and q3 are used
  447. // in computing qa and qb.
  448. //
  449. Quat<T> qa = intermediate (q0, q1, q2);
  450. Quat<T> qb = intermediate (q1, q2, q3);
  451. Quat<T> result = squad (q1, qa, qb, q2, t);
  452. return result;
  453. }
  454. template<class T>
  455. Quat<T>
  456. squad (const Quat<T> &q1, const Quat<T> &qa,
  457. const Quat<T> &qb, const Quat<T> &q2,
  458. T t)
  459. {
  460. //
  461. // Spherical Quadrangle Interpolation -
  462. // from Advanced Animation and Rendering
  463. // Techniques by Watt and Watt, Page 366:
  464. // It constructs a spherical cubic interpolation as
  465. // a series of three spherical linear interpolations
  466. // of a quadrangle of unit quaternions.
  467. //
  468. Quat<T> r1 = slerp (q1, q2, t);
  469. Quat<T> r2 = slerp (qa, qb, t);
  470. Quat<T> result = slerp (r1, r2, 2 * t * (1 - t));
  471. return result;
  472. }
  473. template<class T>
  474. Quat<T>
  475. intermediate (const Quat<T> &q0, const Quat<T> &q1, const Quat<T> &q2)
  476. {
  477. //
  478. // From advanced Animation and Rendering
  479. // Techniques by Watt and Watt, Page 366:
  480. // computing the inner quadrangle
  481. // points (qa and qb) to guarantee tangent
  482. // continuity.
  483. //
  484. Quat<T> q1inv = q1.inverse();
  485. Quat<T> c1 = q1inv * q2;
  486. Quat<T> c2 = q1inv * q0;
  487. Quat<T> c3 = (T) (-0.25) * (c2.log() + c1.log());
  488. Quat<T> qa = q1 * c3.exp();
  489. qa.normalize();
  490. return qa;
  491. }
  492. template <class T>
  493. inline Quat<T>
  494. Quat<T>::log () const
  495. {
  496. //
  497. // For unit quaternion, from Advanced Animation and
  498. // Rendering Techniques by Watt and Watt, Page 366:
  499. //
  500. T theta = Math<T>::acos (std::min (r, (T) 1.0));
  501. if (theta == 0)
  502. return Quat<T> (0, v);
  503. T sintheta = Math<T>::sin (theta);
  504. T k;
  505. if (abs (sintheta) < 1 && abs (theta) >= limits<T>::max() * abs (sintheta))
  506. k = 1;
  507. else
  508. k = theta / sintheta;
  509. return Quat<T> ((T) 0, v.x * k, v.y * k, v.z * k);
  510. }
  511. template <class T>
  512. inline Quat<T>
  513. Quat<T>::exp () const
  514. {
  515. //
  516. // For pure quaternion (zero scalar part):
  517. // from Advanced Animation and Rendering
  518. // Techniques by Watt and Watt, Page 366:
  519. //
  520. T theta = v.length();
  521. T sintheta = Math<T>::sin (theta);
  522. T k;
  523. if (abs (theta) < 1 && abs (sintheta) >= limits<T>::max() * abs (theta))
  524. k = 1;
  525. else
  526. k = sintheta / theta;
  527. T costheta = Math<T>::cos (theta);
  528. return Quat<T> (costheta, v.x * k, v.y * k, v.z * k);
  529. }
  530. template <class T>
  531. inline T
  532. Quat<T>::angle () const
  533. {
  534. return 2 * Math<T>::atan2 (v.length(), r);
  535. }
  536. template <class T>
  537. inline Vec3<T>
  538. Quat<T>::axis () const
  539. {
  540. return v.normalized();
  541. }
  542. template <class T>
  543. inline Quat<T> &
  544. Quat<T>::setAxisAngle (const Vec3<T> &axis, T radians)
  545. {
  546. r = Math<T>::cos (radians / 2);
  547. v = axis.normalized() * Math<T>::sin (radians / 2);
  548. return *this;
  549. }
  550. template <class T>
  551. Quat<T> &
  552. Quat<T>::setRotation (const Vec3<T> &from, const Vec3<T> &to)
  553. {
  554. //
  555. // Create a quaternion that rotates vector from into vector to,
  556. // such that the rotation is around an axis that is the cross
  557. // product of from and to.
  558. //
  559. // This function calls function setRotationInternal(), which is
  560. // numerically accurate only for rotation angles that are not much
  561. // greater than pi/2. In order to achieve good accuracy for angles
  562. // greater than pi/2, we split large angles in half, and rotate in
  563. // two steps.
  564. //
  565. //
  566. // Normalize from and to, yielding f0 and t0.
  567. //
  568. Vec3<T> f0 = from.normalized();
  569. Vec3<T> t0 = to.normalized();
  570. if ((f0 ^ t0) >= 0)
  571. {
  572. //
  573. // The rotation angle is less than or equal to pi/2.
  574. //
  575. setRotationInternal (f0, t0, *this);
  576. }
  577. else
  578. {
  579. //
  580. // The angle is greater than pi/2. After computing h0,
  581. // which is halfway between f0 and t0, we rotate first
  582. // from f0 to h0, then from h0 to t0.
  583. //
  584. Vec3<T> h0 = (f0 + t0).normalized();
  585. if ((h0 ^ h0) != 0)
  586. {
  587. setRotationInternal (f0, h0, *this);
  588. Quat<T> q;
  589. setRotationInternal (h0, t0, q);
  590. *this *= q;
  591. }
  592. else
  593. {
  594. //
  595. // f0 and t0 point in exactly opposite directions.
  596. // Pick an arbitrary axis that is orthogonal to f0,
  597. // and rotate by pi.
  598. //
  599. r = T (0);
  600. Vec3<T> f02 = f0 * f0;
  601. if (f02.x <= f02.y && f02.x <= f02.z)
  602. v = (f0 % Vec3<T> (1, 0, 0)).normalized();
  603. else if (f02.y <= f02.z)
  604. v = (f0 % Vec3<T> (0, 1, 0)).normalized();
  605. else
  606. v = (f0 % Vec3<T> (0, 0, 1)).normalized();
  607. }
  608. }
  609. return *this;
  610. }
  611. template <class T>
  612. void
  613. Quat<T>::setRotationInternal (const Vec3<T> &f0, const Vec3<T> &t0, Quat<T> &q)
  614. {
  615. //
  616. // The following is equivalent to setAxisAngle(n,2*phi),
  617. // where the rotation axis, n, is orthogonal to the f0 and
  618. // t0 vectors, and 2*phi is the angle between f0 and t0.
  619. //
  620. // This function is called by setRotation(), above; it assumes
  621. // that f0 and t0 are normalized and that the angle between
  622. // them is not much greater than pi/2. This function becomes
  623. // numerically inaccurate if f0 and t0 point into nearly
  624. // opposite directions.
  625. //
  626. //
  627. // Find a normalized vector, h0, that is halfway between f0 and t0.
  628. // The angle between f0 and h0 is phi.
  629. //
  630. Vec3<T> h0 = (f0 + t0).normalized();
  631. //
  632. // Store the rotation axis and rotation angle.
  633. //
  634. q.r = f0 ^ h0; // f0 ^ h0 == cos (phi)
  635. q.v = f0 % h0; // (f0 % h0).length() == sin (phi)
  636. }
  637. template<class T>
  638. Matrix33<T>
  639. Quat<T>::toMatrix33() const
  640. {
  641. return Matrix33<T> (1 - 2 * (v.y * v.y + v.z * v.z),
  642. 2 * (v.x * v.y + v.z * r),
  643. 2 * (v.z * v.x - v.y * r),
  644. 2 * (v.x * v.y - v.z * r),
  645. 1 - 2 * (v.z * v.z + v.x * v.x),
  646. 2 * (v.y * v.z + v.x * r),
  647. 2 * (v.z * v.x + v.y * r),
  648. 2 * (v.y * v.z - v.x * r),
  649. 1 - 2 * (v.y * v.y + v.x * v.x));
  650. }
  651. template<class T>
  652. Matrix44<T>
  653. Quat<T>::toMatrix44() const
  654. {
  655. return Matrix44<T> (1 - 2 * (v.y * v.y + v.z * v.z),
  656. 2 * (v.x * v.y + v.z * r),
  657. 2 * (v.z * v.x - v.y * r),
  658. 0,
  659. 2 * (v.x * v.y - v.z * r),
  660. 1 - 2 * (v.z * v.z + v.x * v.x),
  661. 2 * (v.y * v.z + v.x * r),
  662. 0,
  663. 2 * (v.z * v.x + v.y * r),
  664. 2 * (v.y * v.z - v.x * r),
  665. 1 - 2 * (v.y * v.y + v.x * v.x),
  666. 0,
  667. 0,
  668. 0,
  669. 0,
  670. 1);
  671. }
  672. template<class T>
  673. inline Matrix33<T>
  674. operator * (const Matrix33<T> &M, const Quat<T> &q)
  675. {
  676. return M * q.toMatrix33();
  677. }
  678. template<class T>
  679. inline Matrix33<T>
  680. operator * (const Quat<T> &q, const Matrix33<T> &M)
  681. {
  682. return q.toMatrix33() * M;
  683. }
  684. template<class T>
  685. std::ostream &
  686. operator << (std::ostream &o, const Quat<T> &q)
  687. {
  688. return o << "(" << q.r
  689. << " " << q.v.x
  690. << " " << q.v.y
  691. << " " << q.v.z
  692. << ")";
  693. }
  694. template<class T>
  695. inline Quat<T>
  696. operator * (const Quat<T> &q1, const Quat<T> &q2)
  697. {
  698. return Quat<T> (q1.r * q2.r - (q1.v ^ q2.v),
  699. q1.r * q2.v + q1.v * q2.r + q1.v % q2.v);
  700. }
  701. template<class T>
  702. inline Quat<T>
  703. operator / (const Quat<T> &q1, const Quat<T> &q2)
  704. {
  705. return q1 * q2.inverse();
  706. }
  707. template<class T>
  708. inline Quat<T>
  709. operator / (const Quat<T> &q, T t)
  710. {
  711. return Quat<T> (q.r / t, q.v / t);
  712. }
  713. template<class T>
  714. inline Quat<T>
  715. operator * (const Quat<T> &q, T t)
  716. {
  717. return Quat<T> (q.r * t, q.v * t);
  718. }
  719. template<class T>
  720. inline Quat<T>
  721. operator * (T t, const Quat<T> &q)
  722. {
  723. return Quat<T> (q.r * t, q.v * t);
  724. }
  725. template<class T>
  726. inline Quat<T>
  727. operator + (const Quat<T> &q1, const Quat<T> &q2)
  728. {
  729. return Quat<T> (q1.r + q2.r, q1.v + q2.v);
  730. }
  731. template<class T>
  732. inline Quat<T>
  733. operator - (const Quat<T> &q1, const Quat<T> &q2)
  734. {
  735. return Quat<T> (q1.r - q2.r, q1.v - q2.v);
  736. }
  737. template<class T>
  738. inline Quat<T>
  739. operator ~ (const Quat<T> &q)
  740. {
  741. return Quat<T> (q.r, -q.v);
  742. }
  743. template<class T>
  744. inline Quat<T>
  745. operator - (const Quat<T> &q)
  746. {
  747. return Quat<T> (-q.r, -q.v);
  748. }
  749. template<class T>
  750. inline Vec3<T>
  751. operator * (const Vec3<T> &v, const Quat<T> &q)
  752. {
  753. Vec3<T> a = q.v % v;
  754. Vec3<T> b = q.v % a;
  755. return v + T (2) * (q.r * a + b);
  756. }
  757. #if (defined _WIN32 || defined _WIN64) && defined _MSC_VER
  758. #pragma warning(default:4244)
  759. #endif
  760. } // namespace Imath
  761. #endif