home *** CD-ROM | disk | FTP | other *** search
/ Hackers Magazine 57 / CdHackersMagazineNr57.iso / Software / Multimedia / k3d-setup-0.7.11.0.exe / include / k3d / k3dsdk / algebra.h next >
Encoding:
C/C++ Source or Header  |  2008-12-16  |  39.2 KB  |  1,294 lines

  1. #ifndef K3DSDK_ALGEBRA_H
  2. #define K3DSDK_ALGEBRA_H
  3.  
  4. // K-3D
  5. // Copyright (c) 1995-2006, Timothy M. Shead
  6. //
  7. // Contact: tshead@k-3d.com
  8. //
  9. // This library is free software; you can redistribute it and/or
  10. // modify it under the terms of the GNU General Public
  11. // License as published by the Free Software Foundation; either
  12. // version 2 of the License, or (at your option) any later version.
  13. //
  14. // This library is distributed in the hope that it will be useful,
  15. // but WITHOUT ANY WARRANTY; without even the implied warranty of
  16. // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  17. // General Public License for more details.
  18. //
  19. // You should have received a copy of the GNU General Public
  20. // License along with this library; if not, write to the Free Software
  21. // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  22.  
  23. /** \file
  24.         \brief Algebra classes
  25.         \author Tim Shead (tshead@k-3d.com)
  26. */
  27.  
  28. #include "basic_math.h"
  29. #include "log.h"
  30. #include "vectors.h"
  31.  
  32. #include <cfloat>
  33. #include <cstring>
  34.  
  35. /****************************************************************
  36. *
  37. * C++ Vector and Matrix Algebra routines
  38. * Author: Jean-Francois DOUE
  39. * Version 3.1 --- October 1993
  40. *
  41. ****************************************************************/
  42.  
  43. //
  44. //    From "Graphics Gems IV / Edited by Paul S. Heckbert
  45. //    Academic Press, 1994, ISBN 0-12-336156-9
  46. //    "You are free to use and modify this code in any way
  47. //    you like." (p. xv)
  48. //
  49. //    Modified by J. Nagle, March 1997
  50. //    -    All functions are inline.
  51. //    -    All functions are const-correct.
  52. //    -    All checking is via the standard "assert" macro.
  53. //
  54.  
  55. // Modified by Tim Shead for use with K-3D, January 1998
  56.  
  57. namespace k3d
  58. {
  59.  
  60. class matrix4;
  61. class quaternion;
  62. class angle_axis;
  63. class euler_angles;
  64.  
  65. /////////////////////////////////////////////////////////////////////////////
  66. // point3
  67.  
  68. /// Matrix multiplication
  69. point3 operator*(const matrix4& a, const point3& v);
  70. /// Matrix multiplication
  71. point3 operator*(const point3& v, const matrix4& a);
  72.  
  73. /////////////////////////////////////////////////////////////////////////////
  74. // point4
  75.  
  76. /// Matrix multiplication
  77. point4 operator*(const matrix4& a, const point4& v);
  78. /// Matrix multiplication
  79. point4 operator*(const point4& v, const matrix4& a);
  80. /// Linear transformation
  81. point3 operator*(const matrix4& a, const point3& v);
  82. /// Matrix multiplication
  83. matrix4 operator*(const matrix4& a, const matrix4& b);
  84.  
  85. /////////////////////////////////////////////////////////////////////////////
  86. // matrix4
  87.  
  88. /// A 4x4 matrix
  89. class matrix4
  90. {
  91. public:
  92.     /// Stores the matrix elements
  93.     vector4 v[4];
  94.     // Constructors
  95.     matrix4();
  96.     matrix4(const vector4& v0, const vector4& v1, const vector4& v2, const vector4& v3);
  97.     matrix4(const double d);
  98.     matrix4(euler_angles Angles);
  99.     /// Copy constructor
  100.     matrix4(const matrix4& m);
  101.     /// Assignment of an matrix4
  102.     matrix4& operator=(const matrix4& m);
  103. /*
  104.     /// Assignment of a C/C++ array
  105.     matrix4& operator=(const double d[4][4]);
  106. */
  107.     /// Assignment of a C/C++ array
  108.     matrix4& operator=(const double d[16]);
  109.     /// Addition
  110.     matrix4& operator+=(const matrix4& m);
  111.     /// Subtraction
  112.     matrix4& operator-=(const matrix4& m);
  113.     /// Multiplication by a constant
  114.     matrix4& operator*=(const double d);
  115.     /// Division by a constant
  116.     matrix4& operator/=(const double d);
  117.     /// Returns a vector by index
  118.     vector4& operator[](int i);
  119.     /// Returns a vector by index
  120.     const vector4& operator[](int i) const;
  121.     /// Copies the matrix contents into a C/C++ style array
  122.     void CopyArray(float m[16]) const;
  123.     /// Copies the matrix contents into a C/C++ style array
  124.     void CopyArray(double m[16]) const;
  125.     /// Casts the matrix to a C/C++ style array
  126.     operator double*() { return &v[0][0]; }
  127. };
  128.  
  129. /// Negation
  130. matrix4 operator-(const matrix4& a);
  131. /// Addition
  132. matrix4 operator+(const matrix4& a, const matrix4& b);
  133. /// Subtraction
  134. matrix4 operator-(const matrix4& a, const matrix4& b);
  135. /// Matrix multiplication
  136. matrix4 operator*(const matrix4& a, const matrix4& b);
  137. /// Multiplication by a constant
  138. matrix4 operator*(const matrix4& a, const double d);
  139. /// Multiplication by a constant
  140. matrix4 operator*(const double d, const matrix4& a);
  141. /// Division by a constant
  142. matrix4 operator/(const matrix4& a, const double d);
  143. /// Equality
  144. bool operator==(const matrix4& a, const matrix4& b);
  145. /// Non-equality
  146. bool operator!=(const matrix4& a, const matrix4& b);
  147. /// Linear transformation
  148. point4 operator*(const matrix4& a, const point4& v);
  149. /// Linear transformation
  150. point3 operator*(const matrix4& a, const point3& v);
  151. /// Returns a three-dimensional identity matrix
  152. inline matrix4 identity3()
  153. {
  154.     return matrix4(
  155.         vector4(1, 0, 0, 0),
  156.         vector4(0, 1, 0, 0),
  157.         vector4(0, 0, 1, 0),
  158.         vector4(0, 0, 0, 1));
  159. }
  160. /// Returns the transposition of a matrix
  161. inline matrix4 transpose(const matrix4& v)
  162. {
  163.     return matrix4(
  164.         vector4(v[0][0], v[1][0], v[2][0], v[3][0]),
  165.         vector4(v[0][1], v[1][1], v[2][1], v[3][1]),
  166.         vector4(v[0][2], v[1][2], v[2][2], v[3][2]),
  167.         vector4(v[0][3], v[1][3], v[2][3], v[3][3]));
  168. }
  169. /// Returns the inverse of a matrix
  170. inline matrix4 inverse(const matrix4& v)
  171. {
  172.     matrix4 a(v),     // As a evolves from original mat into identity
  173.     b(identity3()); // b evolves from identity into inverse(a)
  174.  
  175.     // Loop over cols of a from left to right, eliminating above and below diag
  176.     for(int j=0; j<4; ++j) { // Find largest pivot in column j among rows j..3
  177.         int i1 = j;         // Row with largest pivot candidate
  178.  
  179.         for(int i=j+1; i<4; ++i)
  180.         {
  181.             if (fabs(a.v[i].n[j]) > fabs(a.v[i1].n[j]))
  182.                 i1 = i;
  183.         }
  184.  
  185.         // Swap rows i1 and j in a and b to put pivot on diagonal
  186.         std::swap(a.v[i1], a.v[j]);
  187.         std::swap(b.v[i1], b.v[j]);
  188.  
  189.         // Scale row j to have a unit diagonal
  190.         if(!a.v[j].n[j])
  191.         {
  192.             // Singular matrix - can't invert
  193.             log() << error << "Can't invert singular matrix!" << std::endl;
  194.             return b;
  195.         }
  196.  
  197.         b.v[j] /= a.v[j].n[j];
  198.         a.v[j] /= a.v[j].n[j];
  199.  
  200.         // Eliminate off-diagonal elems in col j of a, doing identical ops to b
  201.         for(int i=0; i<4; ++i)
  202.         {
  203.             if(i!=j)
  204.             {
  205.                 b.v[i] -= a.v[i].n[j]*b.v[j];
  206.                 a.v[i] -= a.v[i].n[j]*a.v[j];
  207.             }
  208.         }
  209.     }
  210.     return b;
  211. }
  212.  
  213. /// Returns true if a matrix contains negative scale factors that will flip an object "inside-out"
  214. inline const bool inside_out(const matrix4& m)
  215. {
  216.     const vector3 a(m[0][0], m[0][1], m[0][2]);
  217.     const vector3 b(m[1][0], m[1][1], m[1][2]);
  218.     const vector3 c(m[2][0], m[2][1], m[2][2]);
  219.  
  220.     return ((a ^ b) * c) < 0 ? true : false;
  221. }
  222.  
  223. /// Linear transformation
  224. inline const vector3 operator*(const matrix4& a, const vector3& v)
  225. {
  226.     const vector4 temp((a * point4(v[0], v[1], v[2], 1)) - (a * point4(0, 0, 0, 1)));
  227.     return vector3(temp.n[0], temp.n[1], temp.n[2]);
  228. }
  229.  
  230. /// Linear transformation
  231. inline const vector3 operator*(const vector3& v, const matrix4& a)
  232. {
  233.     return transpose(a) * v;
  234. }
  235.  
  236. /// Linear transformation
  237. inline const normal3 operator*(const matrix4& a, const normal3& v)
  238. {
  239.     const vector4 temp((a * point4(v[0], v[1], v[2], 1)) - (a * point4(0, 0, 0, 1)));
  240.     return normal3(temp.n[0], temp.n[1], temp.n[2]);
  241. }
  242.  
  243. /// Linear transformation
  244. inline const normal3 operator*(const normal3& v, const matrix4& a)
  245. {
  246.     return transpose(a) * v;
  247. }
  248.  
  249. /// Output inserter
  250. std::ostream& operator<<(std::ostream& Stream, const matrix4& Arg);
  251. /// Input extractor
  252. std::istream& operator>>(std::istream& Stream, matrix4& Arg);
  253.  
  254. /////////////////////////////////////////////////////////////////////////////
  255. // quaternion
  256.  
  257. /// Encapsulates a quaternion
  258. class quaternion
  259. {
  260. public:
  261.     // The scalar+vector representation is used
  262.     double w;
  263.     vector3 v;
  264.  
  265.     // Constructors
  266.     quaternion();
  267.     quaternion(const double u, const point3& t);
  268.     quaternion(const double u, const vector3& t);
  269.     quaternion(const double u, const double x, const double y, const double z);
  270.     quaternion(const angle_axis& AngleAxis);
  271.     quaternion(euler_angles Angles);
  272.     /// Assignment of a quaternion
  273.     quaternion& operator=(const quaternion& q);
  274.     /// Assignment of an axis/angle
  275.     quaternion& operator=(const angle_axis& AngleAxis);
  276.     /// Multiplication by a constant
  277.     quaternion& operator*=(const double d);
  278.     /// Division by a constant
  279.     quaternion& operator/=(const double d);
  280.     /// Addition
  281.     quaternion& operator+=(const quaternion& q);
  282.     /// Substraction
  283.     quaternion& operator-=(const quaternion& q);
  284.     /// Multiplication
  285.     quaternion& operator*=(const quaternion& q);
  286.     /// Division
  287.     quaternion& operator/=(const quaternion& q);
  288.     /// Magnitude
  289.     double Magnitude() const;
  290.     /// Normalization
  291.     quaternion& Normalize();
  292.     /// Conjugate
  293.     quaternion& Conjugate();
  294.     /// Inversion in place
  295.     quaternion& Inverse();
  296.     /// Squares the quaternion in place
  297.     quaternion& Square();
  298.     /// Axis rotation
  299.     quaternion& AxisRotation(const double phi);
  300. };
  301.  
  302. /// Negation
  303. quaternion operator-(quaternion& q);
  304. /// Addition
  305. quaternion operator+(const quaternion& q, const quaternion& r);
  306. /// Substraction
  307. quaternion operator-(const quaternion& q, const quaternion& r);
  308. /// Multiplication by a quaternion
  309. quaternion operator*(const quaternion& q, const quaternion& r);
  310. /// Multiplication by a constant
  311. quaternion operator*(const quaternion& q, const double d);
  312. /// Multiplication by a constant
  313. quaternion operator*(double d, const quaternion& q);
  314. /// Division by a quaternion
  315. quaternion operator/(const quaternion& q, const quaternion& r);
  316. /// Division by a constant
  317. quaternion operator/(const quaternion& q, const double d);
  318. /// Equality test
  319. bool operator==(const quaternion& q, const quaternion& r);
  320. /// Non-equality test
  321. bool operator!=(const quaternion& q, const quaternion& r);
  322.  
  323. /// Output inserter
  324. std::ostream& operator<<(std::ostream& Stream, const quaternion& Arg);
  325. /// Input extractor
  326. std::istream& operator>>(std::istream& Stream, quaternion& Arg);
  327.  
  328. /////////////////////////////////////////////////////////////////////////////
  329. // angle_axis
  330.  
  331. /// Encapsulates an angle/axis representation of an object's orientation
  332. class angle_axis
  333. {
  334. public:
  335.     double angle;
  336.     vector3 axis;
  337.  
  338.     // Constructors ...
  339.     angle_axis();
  340.     angle_axis(const double Angle, const point3& Axis);
  341.     angle_axis(const double Angle, const vector3& Axis);
  342.     angle_axis(const double Angle, const double X, const double Y, const double Z);
  343.     angle_axis(const double AngleAxis[4]);
  344.     angle_axis(const quaternion& Quaternion);
  345.     /// Copy constructor
  346.     angle_axis(const angle_axis& AngleAxis);
  347.     /// Assignment of an angle_axis
  348.     angle_axis& operator=(const angle_axis& AngleAxis);
  349.     /// Assignment of a quaternion
  350.     angle_axis& operator=(const quaternion& Quaternion);
  351.     /// Assignment of a C/C++ style array
  352.     angle_axis& operator=(const double AngleAxis[4]);
  353.     /// Casting to a C/C++ style array pointer
  354.     operator double*() { return ∠ }
  355.     /// Copies the vector into a C/C++ style array
  356.     void CopyArray(float AngleAxis[4]) const;
  357.     /// Copies the vector into a C/C++ style array
  358.     void CopyArray(double AngleAxis[4]) const;
  359. };
  360.  
  361. /// Equality test
  362. bool operator==(const angle_axis& a, const angle_axis& b);
  363. /// Non-equality test
  364. bool operator!=(const angle_axis& a, const angle_axis& b);
  365.  
  366. /// Output inserter
  367. std::ostream& operator<<(std::ostream& Stream, const angle_axis& Arg);
  368. /// Input extractor
  369. std::istream& operator>>(std::istream& Stream, angle_axis& Arg);
  370.  
  371. #define SDPEULERANGLEORDER(initialaxis, parity, repetition, frame) ((((((initialaxis << 1) + parity) << 1) + repetition) << 1) + frame)
  372.  
  373. /// Encapsulates a set of Euler angles, including the order in which they should be applied
  374. class euler_angles
  375. {
  376. public:
  377.  
  378.     /// Enumerates what gets rotated - the axes themselves, or the frame they represent
  379.     typedef enum
  380.     {
  381.         StaticFrame = 0,
  382.         RotatingFrame = 1,
  383.     } OrderFrame;
  384.  
  385.     /// Enumerates whether the last axis will be the same as the first
  386.     typedef enum
  387.     {
  388.         NoRepetition = 0,
  389.         Repeats = 1,
  390.     } OrderRepetition;
  391.  
  392.     /// Enumerates which axes will become the second axis
  393.     typedef enum
  394.     {
  395.         EvenParity = 0,
  396.         OddParity = 1,
  397.     } OrderParity;
  398.  
  399.     /// Enumerates all possible axis order permutations
  400.     typedef enum
  401.     {
  402.         XYZstatic = SDPEULERANGLEORDER(0, EvenParity, NoRepetition, StaticFrame),
  403.         XYXstatic = SDPEULERANGLEORDER(0, EvenParity, Repeats, StaticFrame),
  404.         XZYstatic = SDPEULERANGLEORDER(0, OddParity, NoRepetition, StaticFrame),
  405.         XZXstatic = SDPEULERANGLEORDER(0, OddParity, Repeats, StaticFrame),
  406.         YZXstatic = SDPEULERANGLEORDER(1, EvenParity, NoRepetition, StaticFrame),
  407.         YZYstatic = SDPEULERANGLEORDER(1, EvenParity, Repeats, StaticFrame),
  408.         YXZstatic = SDPEULERANGLEORDER(1, OddParity, NoRepetition, StaticFrame),
  409.         YXYstatic = SDPEULERANGLEORDER(1, OddParity, Repeats, StaticFrame),
  410.         ZXYstatic = SDPEULERANGLEORDER(2, EvenParity, NoRepetition, StaticFrame),
  411.         ZXZstatic = SDPEULERANGLEORDER(2, EvenParity, Repeats, StaticFrame),
  412.         ZYXstatic = SDPEULERANGLEORDER(2, OddParity, NoRepetition, StaticFrame),
  413.         ZYZstatic = SDPEULERANGLEORDER(2, OddParity, Repeats, StaticFrame),
  414.         ZYXrotating = SDPEULERANGLEORDER(0, EvenParity, NoRepetition, RotatingFrame),
  415.         XYXrotating = SDPEULERANGLEORDER(0, EvenParity, Repeats, RotatingFrame),
  416.         YZXrotating = SDPEULERANGLEORDER(0, OddParity, NoRepetition, RotatingFrame),
  417.         XZXrotating = SDPEULERANGLEORDER(0, OddParity, Repeats, RotatingFrame),
  418.         XZYrotating = SDPEULERANGLEORDER(1, EvenParity, NoRepetition, RotatingFrame),
  419.         YZYrotating = SDPEULERANGLEORDER(1, EvenParity, Repeats, RotatingFrame),
  420.         ZXYrotating = SDPEULERANGLEORDER(1, OddParity, NoRepetition, RotatingFrame),
  421.         YXYrotating = SDPEULERANGLEORDER(1, OddParity, Repeats, RotatingFrame),
  422.         YXZrotating = SDPEULERANGLEORDER(2, EvenParity, NoRepetition, RotatingFrame),
  423.         ZXZrotating = SDPEULERANGLEORDER(2, EvenParity, Repeats, RotatingFrame),
  424.         XYZrotating = SDPEULERANGLEORDER(2, OddParity, NoRepetition, RotatingFrame),
  425.         ZYZrotating = SDPEULERANGLEORDER(2, OddParity, Repeats, RotatingFrame),
  426.     } AngleOrder;
  427.  
  428.     /// Stores the three axis angles
  429.     double n[3];
  430.     /// Stores the axis order in packed format
  431.     AngleOrder order;
  432.  
  433.     euler_angles();
  434.     euler_angles(point3 Angles, AngleOrder Order);
  435.     euler_angles(double X, double Y, double Z, AngleOrder Order);
  436.     /// Conversion from a quaternion
  437.     euler_angles(const quaternion& Quaternion, AngleOrder Order);
  438.     /// Conversion from a 4x4 matrix
  439.     euler_angles(const matrix4& Matrix, AngleOrder Order);
  440.  
  441.     /// Returns the frame type from a packed order representation
  442.     static OrderFrame Frame(AngleOrder& Order) { return OrderFrame(Order & 1); }
  443.     /// Returns the repetition type from a packed order representation
  444.     static OrderRepetition Repetition(AngleOrder& Order) { return OrderRepetition((Order >> 1) & 1); }
  445.     /// Returns the parity type from a packed order representation
  446.     static OrderParity Parity(AngleOrder& Order) { return OrderParity((Order >> 2) & 1); }
  447.     /// Returns the axes in order, from a packed order representation
  448.     static void Axes(AngleOrder& Order, int& i, int& j, int& k);
  449.  
  450.     /// Returns the frame type of this instance
  451.     OrderFrame Frame() { return Frame(order); }
  452.     /// Returns the repetition type of this instance
  453.     OrderRepetition Repetition() { return Repetition(order); }
  454.     /// Returns the parity type of this instance
  455.     OrderParity Parity() { return Parity(order); }
  456.     /// Returns the axes in order, for this instance
  457.     void Axes(int& i, int& j, int& k) { Axes(order, i, j, k); }
  458.  
  459.     /// Returns the given angle by reference
  460.     double& operator[](int i);
  461.     /// Returns the given angle by value
  462.     double operator[](int i) const;
  463. };
  464.  
  465. /// Output inserter
  466. std::ostream& operator<<(std::ostream& Stream, const euler_angles& Arg);
  467. /// Input extractor
  468. std::istream& operator>>(std::istream& Stream, euler_angles& Arg);
  469.  
  470. /// Returns a three-dimensional translation matrix
  471. inline const matrix4 translate3(const vector3& v)
  472. {
  473.     return matrix4(
  474.         vector4(1, 0, 0, v[0]),
  475.         vector4(0, 1, 0, v[1]),
  476.         vector4(0, 0, 1, v[2]),
  477.         vector4(0, 0, 0, 1));
  478. }
  479.  
  480. /// Returns a three-dimensional translation matrix
  481. inline const matrix4 translate3(const double_t X, const double_t Y, const double_t Z)
  482. {
  483.     return matrix4(
  484.         vector4(1, 0, 0, X),
  485.         vector4(0, 1, 0, Y),
  486.         vector4(0, 0, 1, Z),
  487.         vector4(0, 0, 0, 1));
  488. }
  489.  
  490. /// Returns a three-dimensional rotation matrix about an arbitrary axis
  491. inline const matrix4 rotate3(const double Angle, vector3 Axis)
  492. {
  493.     const double c = cos(Angle), s = sin(Angle), t = 1.0 - c;
  494.  
  495.     Axis = normalize(Axis);
  496.  
  497.     return matrix4(
  498.         vector4(t * Axis[0] * Axis[0] + c, t * Axis[0] * Axis[1] - s * Axis[2], t * Axis[0] * Axis[2] + s * Axis[1], 0),
  499.         vector4(t * Axis[0] * Axis[1] + s * Axis[2], t * Axis[1] * Axis[1] + c, t * Axis[1] * Axis[2] - s * Axis[0], 0),
  500.         vector4(t * Axis[0] * Axis[2] - s * Axis[1], t * Axis[1] * Axis[2] + s * Axis[0], t * Axis[2] * Axis[2] + c, 0),
  501.         vector4(0, 0, 0, 1));
  502. }
  503. /// Returns a three-dimensional rotation matrix about an arbitrary axis
  504. inline const matrix4 rotate3(const angle_axis& AngleAxis)
  505. {
  506.     return rotate3(AngleAxis.angle, AngleAxis.axis);
  507. }
  508. /// Returns a three-dimensional rotation matrix based on a set of Euler angles
  509. inline const matrix4 rotate3(const point3& EulerAngles)
  510. {
  511.     matrix4 matrix = identity3();
  512.     matrix = matrix * rotate3(EulerAngles[1], vector3(0, 1, 0));
  513.     matrix = matrix * rotate3(EulerAngles[0], vector3(1, 0, 0));
  514.     matrix = matrix * rotate3(EulerAngles[2], vector3(0, 0, 1));
  515.  
  516.     return matrix;
  517. }
  518. /// Returns a three-dimensional rotation matrix based on a quaternion
  519. inline const matrix4 rotate3(const quaternion& Quaternion)
  520. {
  521.     return rotate3(angle_axis(Quaternion));
  522. }
  523. /// Returns a quaternion based on a three-dimensional rotation matrix
  524. inline const quaternion rotate3(const matrix4& m)
  525. {
  526.     double d0 = m[0][0];
  527.     double d1 = m[1][1];
  528.     double d2 = m[2][2];
  529.  
  530.     double trace = d0 + d1 + d2 + 1;
  531.     if(trace > 0.0)
  532.     {
  533.         double s = 0.5 / std::sqrt(trace);
  534.         return quaternion(0.25 / s, s * point3(m[2][1] - m[1][2], m[0][2] - m[2][0], m[1][0] - m[0][1]));
  535.     }
  536.  
  537.     // Identify the major diagonal element with greatest value
  538.     if(d0 >= d1 && d0 >= d2)
  539.     {
  540.         double s = std::sqrt(1.0 + d0 - d1 - d2) * 2.0;
  541.         return quaternion(m[1][2] - m[2][1], point3(0.5, m[0][1] - m[1][0], m[0][2] - m[2][0])) / s;
  542.     }
  543.     else if(d1 >= d0 && d1 >= d2)
  544.     {
  545.         double s = std::sqrt(1.0 + d1 - d0 - d2) * 2.0;
  546.         return quaternion(m[0][2] - m[2][0], point3(m[0][1] - m[1][0], 0.5, m[1][2] - m[2][1])) / s;
  547.     }
  548.     else
  549.     {
  550.         double s = std::sqrt(1.0 + d2 - d0 - d1) * 2.0;
  551.         return quaternion(m[0][1] - m[1][0], point3(m[0][2] - m[2][0], m[1][2] - m[2][1], 0.5)) / s;
  552.     }
  553. }
  554.  
  555. /// Returns a three-dimensional scaling matrix that scales uniformly along each dimension.
  556. inline const matrix4 scale3(const double_t S)
  557. {
  558.     return matrix4(
  559.         vector4(S, 0, 0, 0),
  560.         vector4(0, S, 0, 0),
  561.         vector4(0, 0, S, 0),
  562.         vector4(0, 0, 0, 1));
  563. }
  564.  
  565. /// Returns a three-dimensional scaling matrix that can scale non-uniformly along each dimension.
  566. inline const matrix4 scale3(const double_t X, const double_t Y, const double_t Z)
  567. {
  568.     return matrix4(
  569.         vector4(X, 0, 0, 0),
  570.         vector4(0, Y, 0, 0),
  571.         vector4(0, 0, Z, 0),
  572.         vector4(0, 0, 0, 1));
  573. }
  574. /// Returns a three-dimensional perspective matrix.
  575. inline const matrix4 perspective3(const double d)
  576. {
  577.     return matrix4(
  578.         vector4(1, 0, 0, 0),
  579.         vector4(0, 1, 0, 0),
  580.         vector4(0, 0, 1, 0),
  581.         vector4(0, 0, 1/d, 0));
  582. }
  583. /// Returns a three-dimensional shearing matrix.
  584. inline const matrix4 shear3(const double XY, const double XZ, const double YX, const double YZ, const double ZX, const double ZY)
  585. {
  586.     return matrix4(
  587.         vector4(1, XY, XZ, 0),
  588.         vector4(YX, 1, YZ, 0),
  589.         vector4(ZX, ZY, 1, 0),
  590.         vector4(0, 0, 0, 1));
  591. }
  592.  
  593. /// Extract a "look" vector from a view matrix
  594. inline const vector3 look_vector(const matrix4& Matrix)
  595. {
  596.     return normalize(Matrix * vector3(0, 0, 1));
  597. }
  598.  
  599. /// Extract an "up" vector from a view matrix
  600. inline const vector3 up_vector(const matrix4& Matrix)
  601. {
  602.     return normalize(Matrix * vector3(0, 1, 0));
  603. }
  604.  
  605. /// Extract a "right" vector from a view matrix
  606. inline const vector3 right_vector(const matrix4& Matrix)
  607. {
  608.     return normalize(Matrix * vector3(1, 0, 0));
  609. }
  610.  
  611. /// Extract position from a view matrix
  612. inline const point3 position(const matrix4& Matrix)
  613. {
  614.     return point3(Matrix[0][3], Matrix[1][3], Matrix[2][3]);
  615. }
  616.  
  617. /// Construct a view matrix from look, up, right, and position data
  618. inline const matrix4 view_matrix(const vector3& Look, const vector3& Up, const point3& Position)
  619. {
  620.     const vector3 look = normalize(Look);
  621.     const vector3 right = normalize(Up ^ look);
  622.     const vector3 up = normalize(look ^ right);
  623.  
  624.     return matrix4(
  625.         vector4(right[0], up[0], look[0], Position[0]),
  626.         vector4(right[1], up[1], look[1], Position[1]),
  627.         vector4(right[2], up[2], look[2], Position[2]),
  628.         vector4(0, 0, 0, 1));
  629. }
  630.  
  631. //    Matrix utilities for affine matrices.
  632. //
  633. //    The input matrix must be affine, but need not be orthogonal.
  634. //    In other words, it may contain scaling, rotation, and translation,
  635. //    but not perspective projection.
  636. //    Ref. Foley and Van Dam, 2nd ed, p. 217.
  637. //
  638. /// Extracts translation from a three-dimensional matrix
  639. inline const vector3 extract_translation(const matrix4& m)
  640. {
  641.     return(vector3(m[0][3], m[1][3], m[2][3]));
  642. }
  643. /// Extracts scaling from a matrix
  644. inline const matrix4 extract_scaling(const matrix4& m)
  645. {
  646.     return matrix4(
  647.         vector4(sqrt(m[0][0] * m[0][0] + m[1][0] * m[1][0] + m[2][0] * m[2][0]), 0, 0, 0),
  648.         vector4(0, sqrt(m[0][1] * m[0][1] + m[1][1] * m[1][1] + m[2][1] * m[2][1]), 0, 0),
  649.         vector4(0, 0, sqrt(m[0][2] * m[0][2] + m[1][2] * m[1][2] + m[2][2] * m[2][2]), 0),
  650.         vector4(0, 0, 0, 1));
  651. }
  652. /// Extracts rotation from a matrix
  653. inline const matrix4 extract_rotation(const matrix4& m)
  654. {
  655.     // Get scaling ...
  656.     const double scale_x = sqrt(m[0][0] * m[0][0] + m[1][0] * m[1][0] + m[2][0] * m[2][0]);
  657.     const double scale_y = sqrt(m[0][1] * m[0][1] + m[1][1] * m[1][1] + m[2][1] * m[2][1]);
  658.     const double scale_z = sqrt(m[0][2] * m[0][2] + m[1][2] * m[1][2] + m[2][2] * m[2][2]);
  659.     return_val_if_fail(scale_x && scale_y && scale_z, identity3());
  660.  
  661.     // Apply inverse of scaling as a transformation, to get unit scale ...
  662.     matrix4 unscaled(m * scale3(1.0 / scale_x, 1.0 / scale_y, 1.0 / scale_z));
  663.  
  664.     return matrix4(
  665.         vector4(unscaled[0][0], unscaled[0][1], unscaled[0][2], 0),
  666.         vector4(unscaled[1][0], unscaled[1][1], unscaled[1][2], 0),
  667.         vector4(unscaled[2][0], unscaled[2][1], unscaled[2][2], 0),
  668.         vector4(0, 0, 0, 1));
  669. }
  670.  
  671. /////////////////////////////////////////////////////////////////////////////
  672. // point3 implementation
  673.  
  674. inline point3 operator*(const matrix4& a, const point3& v)
  675. {
  676.     const point4 result = a * point4(v[0], v[1], v[2], 1);
  677.     return point3(result[0] / result[3], result[1] / result[3], result[2] / result[3]);
  678. }
  679.  
  680. inline point3 operator*(const point3& v, const matrix4& a)
  681. {
  682.     return transpose(a) * v;
  683. }
  684.  
  685. /////////////////////////////////////////////////////////////////////////////
  686. // point4 implementation
  687.  
  688. inline point4 operator*(const matrix4& a, const point4& v)
  689. {
  690. #define ROWCOL(i) a.v[i].n[0]*v.n[0] + a.v[i].n[1]*v.n[1] + a.v[i].n[2]*v.n[2] + a.v[i].n[3]*v.n[3]
  691.     return point4(ROWCOL(0), ROWCOL(1), ROWCOL(2), ROWCOL(3));
  692. #undef ROWCOL // (i)
  693. }
  694.  
  695. inline point4 operator*(const point4& v, const matrix4& a)
  696. {
  697.     return transpose(a) * v;
  698. }
  699.  
  700. /////////////////////////////////////////////////////////////////////////////
  701. // matrix4 implementation
  702.  
  703. inline matrix4::matrix4() {}
  704.  
  705. inline matrix4::matrix4(const vector4& v0, const vector4& v1, const vector4& v2, const vector4& v3)
  706. { v[0] = v0; v[1] = v1; v[2] = v2; v[3] = v3; }
  707.  
  708. inline matrix4::matrix4(const double d)
  709. { v[0] = v[1] = v[2] = v[3] = vector4(d, d, d, d); }
  710.  
  711. inline matrix4::matrix4(const matrix4& m)
  712. { v[0] = m.v[0]; v[1] = m.v[1]; v[2] = m.v[2]; v[3] = m.v[3]; }
  713.  
  714. inline matrix4::matrix4(euler_angles Angles)
  715. {
  716.     const euler_angles::OrderFrame frame = Angles.Frame();
  717.     const euler_angles::OrderParity parity = Angles.Parity();
  718.     const euler_angles::OrderRepetition repetition = Angles.Repetition();
  719.     int i, j, k;
  720.     Angles.Axes(i, j, k);
  721.  
  722.     if(frame == euler_angles::RotatingFrame)
  723.         std::swap(Angles.n[0], Angles.n[2]);
  724.  
  725.     if(parity == euler_angles::OddParity)
  726.     {
  727.         Angles.n[0] = -Angles.n[0];
  728.         Angles.n[1] = -Angles.n[1];
  729.         Angles.n[2] = -Angles.n[2];
  730.     }
  731.  
  732.     const double ti = Angles.n[0], tj = Angles.n[1], th = Angles.n[2];
  733.     const double ci = cos(ti), cj = cos(tj), ch = cos(th);
  734.     const double si = sin(ti), sj = sin(tj), sh = sin(th);
  735.     const double cc = ci*ch, cs = ci*sh;
  736.     const double sc = si*ch, ss = si*sh;
  737.  
  738.     matrix4 m = identity3();
  739.     if(repetition == euler_angles::Repeats)
  740.     {
  741.         m[i][i] =  cj;        m[i][j] =  sj*si;    m[i][k] =  sj*ci;
  742.         m[j][i] =  sj*sh;    m[j][j] = -cj*ss+cc;    m[j][k] = -cj*cs-sc;
  743.         m[k][i] = -sj*ch;    m[k][j] =  cj*sc+cs;    m[k][k] =  cj*cc-ss;
  744.     }
  745.     else
  746.     {
  747.         m[i][i] =  cj*ch;    m[i][j] = sj*sc-cs;    m[i][k] = sj*cc+ss;
  748.         m[j][i] =  cj*sh;    m[j][j] = sj*ss+cc;    m[j][k] = sj*cs-sc;
  749.         m[k][i] = -sj;        m[k][j] = cj*si;    m[k][k] = cj*ci;
  750.     }
  751.  
  752.     *this = m;
  753. }
  754.  
  755. inline matrix4& matrix4::operator=(const matrix4& m)
  756. { v[0] = m.v[0]; v[1] = m.v[1]; v[2] = m.v[2]; v[3] = m.v[3];
  757. return *this; }
  758.  
  759. /*
  760. inline matrix4& matrix4::operator=(const double d[4][4])
  761. { v[0] = d[0]; v[1] = d[1]; v[2] = d[2]; v[3] = d[3]; return *this; }
  762. */
  763.  
  764. inline matrix4& matrix4::operator=(const double d[16])
  765. { memcpy(&v[0][0], &d[0], 16 * sizeof(double)); return *this; }
  766.  
  767. inline matrix4& matrix4::operator+=(const matrix4& m)
  768. { v[0] += m.v[0]; v[1] += m.v[1]; v[2] += m.v[2]; v[3] += m.v[3];
  769. return *this; }
  770.  
  771. inline matrix4& matrix4::operator-=(const matrix4& m)
  772. { v[0] -= m.v[0]; v[1] -= m.v[1]; v[2] -= m.v[2]; v[3] -= m.v[3];
  773. return *this; }
  774.  
  775. inline matrix4& matrix4::operator*=(const double d)
  776. { v[0] *= d; v[1] *= d; v[2] *= d; v[3] *= d; return *this; }
  777.  
  778. inline matrix4& matrix4::operator/=(const double d)
  779. {
  780.     return_val_if_fail(d, *this);
  781.  
  782.     const double inv = 1./d;
  783.     v[0] *= inv;
  784.     v[1] *= inv;
  785.     v[2] *= inv;
  786.     v[3] *= inv;
  787.     return *this;
  788. }
  789.  
  790. inline vector4& matrix4::operator[](int i)
  791. {
  792.     return v[i];
  793. }
  794.  
  795. inline const vector4& matrix4::operator[](int i) const
  796. {
  797.     return v[i];
  798. }
  799.  
  800. inline void matrix4::CopyArray(float m[16]) const
  801. {
  802.     unsigned long index = 0;
  803.     for(unsigned long i = 0; i < 4; ++i)
  804.         for(unsigned long j = 0; j < 4; ++j)
  805.             m[index++] = float(v[i][j]);
  806. }
  807.  
  808. inline void matrix4::CopyArray(double m[16]) const
  809. {
  810.     unsigned long index = 0;
  811.     for(unsigned long i = 0; i < 4; ++i)
  812.         for(unsigned long j = 0; j < 4; ++j)
  813.             m[index++] = v[i][j];
  814. }
  815.  
  816. inline matrix4 operator-(const matrix4& a)
  817. { return matrix4(-a.v[0], -a.v[1], -a.v[2], -a.v[3]); }
  818.  
  819. inline matrix4 operator+(const matrix4& a, const matrix4& b)
  820. { return matrix4(a.v[0] + b.v[0], a.v[1] + b.v[1], a.v[2] + b.v[2], a.v[3] + b.v[3]); }
  821.  
  822. inline matrix4 operator-(const matrix4& a, const matrix4& b)
  823. { return matrix4(a.v[0] - b.v[0], a.v[1] - b.v[1], a.v[2] - b.v[2], a.v[3] - b.v[3]); }
  824.  
  825. inline matrix4 operator*(const matrix4& a, const matrix4& b)
  826. {
  827. #define ROWCOL(i, j) a.v[i].n[0]*b.v[0][j] + a.v[i].n[1]*b.v[1][j] + a.v[i].n[2]*b.v[2][j] + a.v[i].n[3]*b.v[3][j]
  828.     return matrix4(
  829.         vector4(ROWCOL(0,0), ROWCOL(0,1), ROWCOL(0,2), ROWCOL(0,3)),
  830.         vector4(ROWCOL(1,0), ROWCOL(1,1), ROWCOL(1,2), ROWCOL(1,3)),
  831.         vector4(ROWCOL(2,0), ROWCOL(2,1), ROWCOL(2,2), ROWCOL(2,3)),
  832.         vector4(ROWCOL(3,0), ROWCOL(3,1), ROWCOL(3,2), ROWCOL(3,3)));
  833. }
  834.  
  835. inline matrix4 operator*(const matrix4& a, const double d)
  836. { return matrix4(a.v[0] * d, a.v[1] * d, a.v[2] * d, a.v[3] * d); }
  837.  
  838. inline matrix4 operator*(const double d, const matrix4& a)
  839. { return a*d; }
  840.  
  841. inline matrix4 operator/(const matrix4& a, const double d)
  842. {
  843.     return_val_if_fail(d, matrix4());
  844.  
  845.     const double inv = 1./d;
  846.     return matrix4(a.v[0] * inv, a.v[1] * inv, a.v[2] * inv, a.v[3] * inv);
  847. }
  848.  
  849. inline bool operator==(const matrix4& a, const matrix4& b)
  850. { return ((a.v[0] == b.v[0]) && (a.v[1] == b.v[1]) && (a.v[2] == b.v[2]) && (a.v[3] == b.v[3])); }
  851.  
  852. inline bool operator!=(const matrix4& a, const matrix4& b)
  853. { return !(a == b); }
  854.  
  855. inline std::ostream& operator<<(std::ostream& Stream, const matrix4& Arg)
  856. {
  857.     Stream << Arg[0] << " " << Arg[1] << " " << Arg[2] << " " << Arg[3];
  858.     return Stream;
  859. }
  860.  
  861. inline std::istream& operator>>(std::istream& Stream, matrix4& Arg)
  862. {
  863.     Stream >> Arg[0] >> Arg[1] >> Arg[2] >> Arg[3];
  864.     return Stream;
  865. }
  866.  
  867. /////////////////////////////////////////////////////////////////////////////
  868. // quaternion implementation
  869.  
  870. inline quaternion::quaternion()
  871. { w = 1.0; v = vector3(0, 0, 0); }
  872.  
  873. inline quaternion::quaternion(const double u, const point3& t)
  874. { w = u; v = to_vector(t); }
  875.  
  876. inline quaternion::quaternion(const double u, const vector3& t)
  877. { w = u; v = t; }
  878.  
  879. inline quaternion::quaternion(const double u, const double x, const double y, const double z)
  880. { w = u; v = vector3(x, y, z); }
  881.  
  882. inline quaternion::quaternion(const angle_axis& AngleAxis)
  883. { w = cos(AngleAxis.angle * 0.5); vector3 axis = k3d::normalize(AngleAxis.axis); v = sin(AngleAxis.angle * 0.5) * axis; }
  884.  
  885. inline quaternion::quaternion(euler_angles Angles)
  886. {
  887.     const euler_angles::OrderFrame frame = Angles.Frame();
  888.     const euler_angles::OrderParity parity = Angles.Parity();
  889.     const euler_angles::OrderRepetition repetition = Angles.Repetition();
  890.     int i, j, k;
  891.     Angles.Axes(i, j, k);
  892.  
  893.     if(frame == euler_angles::RotatingFrame)
  894.         std::swap(Angles.n[0], Angles.n[2]);
  895.  
  896.     if(parity == euler_angles::OddParity)
  897.         Angles.n[1] = -Angles.n[1];
  898.  
  899.     const double ti = Angles.n[0]*0.5, tj = Angles.n[1]*0.5, th = Angles.n[2]*0.5;
  900.     const double ci = cos(ti), cj = cos(tj), ch = cos(th);
  901.     const double si = sin(ti), sj = sin(tj), sh = sin(th);
  902.     const double cc = ci*ch, cs = ci*sh;
  903.     const double sc = si*ch, ss = si*sh;
  904.  
  905.     if(repetition == euler_angles::Repeats)
  906.     {
  907.         v[i] = cj*(cs + sc);
  908.         v[j] = sj*(cc + ss);
  909.         v[k] = sj*(cs - sc);
  910.         w = cj*(cc - ss);
  911.     }
  912.     else
  913.     {
  914.         v[i] = cj*sc - sj*cs;
  915.         v[j] = cj*ss + sj*cc;
  916.         v[k] = cj*cs - sj*sc;
  917.         w = cj*cc + sj*ss;
  918.     }
  919.  
  920.     if(parity == euler_angles::OddParity)
  921.         v[j] = -v[j];
  922. }
  923.  
  924. inline quaternion& quaternion::operator=(const quaternion& q)
  925. { w = q.w; v = q.v; return *this; }
  926.  
  927. inline quaternion& quaternion::operator=(const angle_axis& AngleAxis)
  928. { w = cos(AngleAxis.angle * 0.5); vector3 axis = k3d::normalize(AngleAxis.axis); v = sin(AngleAxis.angle * 0.5) * axis; return *this; }
  929.  
  930. inline quaternion& quaternion::operator*=(const double d)
  931. { w *= d; v *= d; return *this; }
  932.  
  933. inline quaternion& quaternion::operator/=(const double d)
  934. {
  935.     return_val_if_fail(d, *this);
  936.  
  937.     const double inv = 1./d;
  938.     w *= inv;
  939.     v *= inv;
  940.     return *this;
  941. }
  942.  
  943. inline quaternion& quaternion::operator+=(const quaternion& q)
  944. { w += q.w; v += q.v; return *this; }
  945.  
  946. inline quaternion& quaternion::operator-=(const quaternion& q)
  947. { w -= q.w; v -= q.v; return *this; }
  948.  
  949. inline quaternion& quaternion::operator*=(const quaternion& q)
  950. { w = w*q.w - (v*q.v); v = q.w*v + w*q.v + (v^q.v); return *this; }
  951.  
  952. inline quaternion& quaternion::operator/=(const quaternion& q)
  953. { quaternion t = q; (*this) *= t.Inverse(); return *this; }
  954.  
  955. inline double quaternion::Magnitude() const
  956. { return std::sqrt(w*w + v.length2()); }
  957.  
  958. inline quaternion& quaternion::Normalize()
  959. {
  960.     if(const double magnitude = Magnitude())
  961.         *this /= magnitude;
  962.  
  963.     return *this;
  964. }
  965.  
  966. inline quaternion& quaternion::Conjugate()
  967. { v = -v; return *this; }
  968.  
  969. inline quaternion& quaternion::Inverse()
  970. { if(Magnitude() != 0.0) { const double i = 1.0 / Magnitude(); w *= i; v *= -i; } return *this; }
  971.  
  972. inline quaternion& quaternion::Square()
  973. { const double t = 2*w; w = w*w - v.length2(); v *= t; return *this; }
  974.  
  975. inline quaternion& quaternion::AxisRotation(const double phi)
  976. { v = k3d::normalize(v); v *= sin(phi/2.0); w = cos(phi/2); return *this; }
  977.  
  978. inline quaternion operator-(const quaternion& q)
  979. { return quaternion(-q.w,-q.v); }
  980.  
  981. inline quaternion operator+(const quaternion& q, const quaternion& r)
  982. { return quaternion(q.w + r.w, q.v + r.v); }
  983.  
  984. inline quaternion operator-(const quaternion& q, const quaternion& r)
  985. { return quaternion(q.w - r.w, q.v - r.v); }
  986.  
  987. inline quaternion operator*(const quaternion& q, const quaternion& r)
  988. { return quaternion(q.w*r.w - (q.v*r.v), q.w*r.v + r.w*q.v + (q.v^r.v)); }
  989.  
  990. inline quaternion operator*(const quaternion& q, const double d)
  991. { return quaternion(q.w*d, q.v*d); }
  992.  
  993. inline quaternion operator*(const double d, const quaternion& q)
  994. { return quaternion(q.w*d, q.v*d); }
  995.  
  996. inline quaternion operator/(const quaternion& q, const quaternion& r)
  997. { quaternion t = r; return q*t.Inverse(); }
  998.  
  999. inline quaternion operator/(const quaternion& q, const double d)
  1000. {
  1001.     return_val_if_fail(d, quaternion());
  1002.  
  1003.     const double inv = 1./d;
  1004.     return quaternion(q.w * inv, q.v * inv);
  1005. }
  1006.  
  1007. inline bool operator==(const quaternion& q, const quaternion& r)
  1008. { return ((q.w == r.w) && (q.v == r.v)); }
  1009.  
  1010. inline bool operator!=(const quaternion& q, const quaternion& r)
  1011. { return !(q == r); }
  1012.  
  1013. inline std::ostream& operator<<(std::ostream& Stream, const quaternion& Arg)
  1014. {
  1015.     Stream << Arg.w << " " << Arg.v[0] << " " << Arg.v[1] << " " << Arg.v[2];
  1016.     return Stream;
  1017. }
  1018.  
  1019. inline std::istream& operator>>(std::istream& Stream, quaternion& Arg)
  1020. {
  1021.     Stream >> Arg.w >> Arg.v[0] >> Arg.v[1] >> Arg.v[2];
  1022.     return Stream;
  1023. }
  1024.  
  1025. /////////////////////////////////////////////////////////////////////////////
  1026. // angle_axis implementation ...
  1027.  
  1028. inline angle_axis::angle_axis()
  1029. { angle = 0.0; axis = vector3(0, 0, 1); }
  1030.  
  1031. inline angle_axis::angle_axis(const double Angle, const point3& Axis)
  1032. { angle = Angle; axis = normalize(to_vector(Axis)); }
  1033.  
  1034. inline angle_axis::angle_axis(const double Angle, const vector3& Axis)
  1035. { angle = Angle; axis = normalize(Axis); }
  1036.  
  1037. inline angle_axis::angle_axis(const double Angle, const double X, const double Y, const double Z)
  1038. { angle = Angle; axis[0] = X; axis[1] = Y; axis[2] = Z; axis = normalize(axis); }
  1039.  
  1040. inline angle_axis::angle_axis(const double AngleAxis[4])
  1041. { angle = AngleAxis[0]; axis[0] = AngleAxis[1]; axis[1] = AngleAxis[2]; axis[2] = AngleAxis[3]; axis = normalize(axis); }
  1042.  
  1043. inline angle_axis::angle_axis(const quaternion& Quaternion)
  1044. {
  1045.     quaternion q = Quaternion;
  1046.     q.Normalize();
  1047.     const double halftheta = acos(q.w);
  1048.     const double sinhalftheta = sin(halftheta);
  1049.  
  1050.     angle = 2.0 * halftheta;
  1051.     if(sinhalftheta != 0.0)
  1052.         axis = q.v / sinhalftheta;
  1053.     else
  1054.         axis = vector3(0, 0, 1);
  1055. }
  1056.  
  1057. inline angle_axis::angle_axis(const angle_axis& AngleAxis)
  1058. { angle = AngleAxis.angle; axis = AngleAxis.axis; }
  1059.  
  1060. inline angle_axis& angle_axis::operator=(const angle_axis& AngleAxis)
  1061. { angle = AngleAxis.angle; axis = AngleAxis.axis; return *this; }
  1062.  
  1063. inline angle_axis& angle_axis::operator=(const quaternion& Quaternion)
  1064. {
  1065.     const double halftheta = acos(Quaternion.w);
  1066.     const double sinhalftheta = sin(halftheta);
  1067.  
  1068.     angle = 2.0 * halftheta;
  1069.     if(sinhalftheta != 0.0)
  1070.         axis = Quaternion.v / sinhalftheta;
  1071.     else
  1072.         axis = vector3(0, 0, 1);
  1073.  
  1074.     return *this;
  1075. }
  1076.  
  1077. inline angle_axis& angle_axis::operator=(const double AngleAxis[4])
  1078. { angle = AngleAxis[0]; axis[0] = AngleAxis[1]; axis[1] = AngleAxis[2]; axis[2] = AngleAxis[3]; return *this; }
  1079.  
  1080. inline void angle_axis::CopyArray(float AngleAxis[4]) const
  1081. {    AngleAxis[0] = float(angle); AngleAxis[1] = float(axis[0]); AngleAxis[2] = float(axis[1]); AngleAxis[3] = float(axis[2]); }
  1082.  
  1083. inline void angle_axis::CopyArray(double AngleAxis[4]) const
  1084. {    AngleAxis[0] = angle; AngleAxis[1] = axis[0]; AngleAxis[2] = axis[1]; AngleAxis[3] = axis[2]; }
  1085.  
  1086. inline bool operator==(const angle_axis& a, const angle_axis& b)
  1087. {    return ((a.angle == b.angle) && (a.axis == b.axis)); }
  1088. inline bool operator!=(const angle_axis& a, const angle_axis& b)
  1089. {    return !(a == b); }
  1090.  
  1091. inline std::ostream& operator<<(std::ostream& Stream, const angle_axis& Arg)
  1092. {
  1093.     Stream << Arg.angle << " " << Arg.axis[0] << " " << Arg.axis[1] << " " << Arg.axis[2];
  1094.     return Stream;
  1095. }
  1096.  
  1097. inline std::istream& operator>>(std::istream& Stream, angle_axis& Arg)
  1098. {
  1099.     Stream >> Arg.angle >> Arg.axis[0] >> Arg.axis[1] >> Arg.axis[2];
  1100.     return Stream;
  1101. }
  1102.  
  1103. //    Unit quaternion utilities.
  1104. //
  1105. //    Those special functions deal with rotations, thus require unit quaternions.
  1106. //
  1107.  
  1108. //    Euler/Matrix/Quaternion conversion functions are based on Ken Shoemake code,
  1109. //    from Graphic Gems IV
  1110.  
  1111. /// Returns the spherical linear interpolation between two quaternions
  1112. inline quaternion Slerp(const quaternion& q1, const quaternion& q2, double t)
  1113. {
  1114.     // Calculate the angle between the two quaternions ...
  1115.     double cosangle = q1.w * q2.w + q1.v * q2.v;
  1116.  
  1117.     if(cosangle < -1.0)
  1118.         cosangle = -1.0;
  1119.     if(cosangle > 1.0)
  1120.         cosangle = 1.0;
  1121.  
  1122.     // If the angle is reasonably large, do the spherical interpolation
  1123.     if(1.0 - cosangle > 16 * FLT_EPSILON)
  1124.     {
  1125.         const double angle = acos(cosangle);
  1126.         return (sin((1.0 - t)*angle)*q1 + sin(t*angle)*q2) / sin(angle);
  1127.     }
  1128.  
  1129.     // The angle is too close to zero - do a linear interpolation
  1130.     return mix(q1, q2, t);
  1131. }
  1132.  
  1133. /////////////////////////////////////////////////////////////////////////////
  1134. // euler_angles implementation
  1135.  
  1136. inline euler_angles::euler_angles()
  1137. {
  1138.     n[0] = n[1] = n[2] = 0.0;
  1139.     order = YXZstatic;
  1140. }
  1141.  
  1142. inline euler_angles::euler_angles(point3 Angles, AngleOrder Order)
  1143. {
  1144.     n[0] = Angles[0];
  1145.     n[1] = Angles[1];
  1146.     n[2] = Angles[2];
  1147.     order = Order;
  1148. }
  1149.  
  1150. inline euler_angles::euler_angles(double X, double Y, double Z, AngleOrder Order)
  1151. {
  1152.     n[0] = X;
  1153.     n[1] = Y;
  1154.     n[2] = Z;
  1155.     order = Order;
  1156. }
  1157.  
  1158. inline euler_angles::euler_angles(const quaternion& Quaternion, AngleOrder Order)
  1159. {
  1160.     const double NQuaternion = Quaternion.Magnitude();
  1161.     const double s = (NQuaternion > 0.0) ? (2.0 / NQuaternion) : 0.0;
  1162.  
  1163.     const double xs = Quaternion.v[0]*s, ys = Quaternion.v[1]*s, zs = Quaternion.v[2]*s;
  1164.     const double wx = Quaternion.w*xs, wy = Quaternion.w*ys, wz = Quaternion.w*zs;
  1165.     const double xx = Quaternion.v[0]*xs, xy = Quaternion.v[0]*ys, xz = Quaternion.v[0]*zs;
  1166.     const double yy = Quaternion.v[1]*ys, yz = Quaternion.v[1]*zs, zz = Quaternion.v[2]*zs;
  1167.  
  1168.     matrix4 matrix = identity3();
  1169.     matrix[0][0] = 1.0 - (yy + zz);
  1170.     matrix[0][1] = xy - wz;
  1171.     matrix[0][2] = xz + wy;
  1172.     matrix[1][0] = xy + wz;
  1173.     matrix[1][1] = 1.0 - (xx + zz);
  1174.     matrix[1][2] = yz - wx;
  1175.     matrix[2][0] = xz - wy;
  1176.     matrix[2][1] = yz + wx;
  1177.     matrix[2][2] = 1.0 - (xx + yy);
  1178.  
  1179.     *this = euler_angles(matrix, Order);
  1180. }
  1181.  
  1182. inline euler_angles::euler_angles(const matrix4& Matrix, AngleOrder Order)
  1183. {
  1184.     OrderRepetition repetition = Repetition(Order);
  1185.     OrderParity parity = Parity(Order);
  1186.     OrderFrame frame = Frame(Order);
  1187.     int i, j, k;
  1188.     Axes(Order, i, j, k);
  1189.  
  1190.     if(repetition == euler_angles::Repeats)
  1191.     {
  1192.         const double sy = sqrt(Matrix[i][j]*Matrix[i][j] + Matrix[i][k]*Matrix[i][k]);
  1193.         if(sy > 16*FLT_EPSILON)
  1194.         {
  1195.             n[0] = atan2(Matrix[i][j], Matrix[i][k]);
  1196.             n[1] = atan2(sy, Matrix[i][i]);
  1197.             n[2] = atan2(Matrix[j][i], -Matrix[k][i]);
  1198.         }
  1199.         else
  1200.         {
  1201.             n[0] = atan2(-Matrix[j][k], Matrix[j][j]);
  1202.             n[1] = atan2(sy, Matrix[i][i]);
  1203.             n[2] = 0;
  1204.         }
  1205.     }
  1206.     else
  1207.     {
  1208.         const double cy = sqrt(Matrix[i][i]*Matrix[i][i] + Matrix[j][i]*Matrix[j][i]);
  1209.         if(cy > 16*FLT_EPSILON)
  1210.         {
  1211.             n[0] = atan2(Matrix[k][j], Matrix[k][k]);
  1212.             n[1] = atan2(-Matrix[k][i], cy);
  1213.             n[2] = atan2(Matrix[j][i], Matrix[i][i]);
  1214.         }
  1215.         else
  1216.         {
  1217.             n[0] = atan2(-Matrix[j][k], Matrix[j][j]);
  1218.             n[1] = atan2(-Matrix[k][i], cy);
  1219.             n[2] = 0;
  1220.         }
  1221.     }
  1222.  
  1223.     if(parity == euler_angles::OddParity)
  1224.     {
  1225.         n[0] = -n[0];
  1226.         n[1] = -n[1];
  1227.         n[2] = -n[2];
  1228.     }
  1229.  
  1230.     if(frame == euler_angles::RotatingFrame)
  1231.         std::swap(n[0], n[2]);
  1232.  
  1233.     order = Order;
  1234. }
  1235.  
  1236. inline void euler_angles::Axes(AngleOrder& Order, int& i, int& j, int& k)
  1237. {
  1238.     const int safe[4] = {0, 1, 2, 0};
  1239.     const int next[4] = {1, 2, 0, 1};
  1240.  
  1241.     i = safe[(Order >> 3) & 3];
  1242.     j = next[i + Parity(Order)];
  1243.     k = next[i + 1 - Parity(Order)];
  1244. }
  1245.  
  1246. inline double& euler_angles::operator[](int i)
  1247. {
  1248.     return_val_if_fail((i >= 0 && i <= 2), n[0]);
  1249.     return n[i];
  1250. }
  1251.  
  1252. inline double euler_angles::operator[](int i) const
  1253. {
  1254.     return_val_if_fail((i >= 0 && i <= 2), n[0]);
  1255.     return n[i];
  1256. }
  1257.  
  1258. inline std::ostream& operator<<(std::ostream& Stream, const euler_angles& Arg)
  1259. {
  1260.     Stream << Arg.n[0] << " " << Arg.n[1] << " " << Arg.n[2] << " " << int(Arg.order);
  1261.     return Stream;
  1262. }
  1263.  
  1264. inline std::istream& operator>>(std::istream& Stream, euler_angles& Arg)
  1265. {
  1266.     int order;
  1267.     Stream >> Arg.n[0] >> Arg.n[1] >> Arg.n[2] >> order;
  1268.  
  1269.     Arg.order = euler_angles::AngleOrder(order);
  1270.  
  1271.     return Stream;
  1272. }
  1273.  
  1274. /// Specialization of almost_equal that tests two matrix4 objects for near-equality
  1275. template<>
  1276. class almost_equal<matrix4>
  1277. {
  1278.     typedef matrix4 T;
  1279. public:
  1280.     almost_equal(const boost::uint64_t Threshold) : threshold(Threshold) { }
  1281.     inline const bool operator()(const T& A, const T& B) const
  1282.     {
  1283.         return std::equal(A.v, A.v + 4, B.v, almost_equal<vector4>(threshold));
  1284.     }
  1285.  
  1286. private:
  1287.     const boost::uint64_t threshold;
  1288. };
  1289.  
  1290. } // namespace k3d
  1291.  
  1292. #endif // !K3DSDK_ALGEBRA_H
  1293.  
  1294.