K-3D: algebra.h Source File

00001 #ifndef K3DSDK_ALGEBRA_H
00002 #define K3DSDK_ALGEBRA_H
00003 
00004 // K-3D
00005 // Copyright (c) 1995-2006, Timothy M. Shead
00006 //
00007 // Contact: tshead@k-3d.com
00008 //
00009 // This library is free software; you can redistribute it and/or
00010 // modify it under the terms of the GNU General Public
00011 // License as published by the Free Software Foundation; either
00012 // version 2 of the License, or (at your option) any later version.
00013 //
00014 // This library is distributed in the hope that it will be useful,
00015 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00016 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00017 // General Public License for more details.
00018 //
00019 // You should have received a copy of the GNU General Public
00020 // License along with this library; if not, write to the Free Software
00021 // Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00022 
00028 #include <k3dsdk/basic_math.h>
00029 #include <k3dsdk/log.h>
00030 #include <k3dsdk/vectors.h>
00031 
00032 #include <cfloat>
00033 #include <cstring>
00034 
00035 /****************************************************************
00036 *
00037 * C++ Vector and Matrix Algebra routines
00038 * Author: Jean-Francois DOUE
00039 * Version 3.1 --- October 1993
00040 *
00041 ****************************************************************/
00042 
00043 //
00044 //  From "Graphics Gems IV / Edited by Paul S. Heckbert
00045 //  Academic Press, 1994, ISBN 0-12-336156-9
00046 //  "You are free to use and modify this code in any way
00047 //  you like." (p. xv)
00048 //
00049 //  Modified by J. Nagle, March 1997
00050 //  - All functions are inline.
00051 //  - All functions are const-correct.
00052 //  - All checking is via the standard "assert" macro.
00053 //
00054 
00055 // Modified by Tim Shead for use with K-3D, January 1998
00056 
00057 namespace k3d
00058 {
00059 
00060 class matrix4;
00061 class quaternion;
00062 class angle_axis;
00063 class euler_angles;
00064 
00066 // point3
00067 
00069 point3 operator*(const matrix4& a, const point3& v);
00071 point3 operator*(const point3& v, const matrix4& a);
00072 
00074 // point4
00075 
00077 point4 operator*(const matrix4& a, const point4& v);
00079 point4 operator*(const point4& v, const matrix4& a);
00081 point3 operator*(const matrix4& a, const point3& v);
00083 matrix4 operator*(const matrix4& a, const matrix4& b);
00084 
00086 // matrix4
00087 
00089 class matrix4
00090 {
00091 public:
00093   vector4 v[4];
00094   // Constructors
00095   matrix4();
00096   matrix4(const vector4& v0, const vector4& v1, const vector4& v2, const vector4& v3);
00097   matrix4(const double d);
00098   matrix4(euler_angles Angles);
00099 
00101   template<typename IteratorT>
00102   static matrix4 row_major(IteratorT Begin, IteratorT End)
00103   {
00104     matrix4 result;
00105     std::copy(Begin, End, &result.v[0][0]);
00106     return result;
00107   }
00108 
00110   matrix4(const matrix4& m);
00112   matrix4& operator=(const matrix4& m);
00113 /*
00115   matrix4& operator=(const double d[4][4]);
00116 */
00118   matrix4& operator=(const double d[16]);
00120   matrix4& operator+=(const matrix4& m);
00122   matrix4& operator-=(const matrix4& m);
00124   matrix4& operator*=(const double d);
00126   matrix4& operator/=(const double d);
00128   vector4& operator[](int i);
00130   const vector4& operator[](int i) const;
00132   void CopyArray(float m[16]) const;
00134   void CopyArray(double m[16]) const;
00136   operator double*() { return &v[0][0]; }
00137 };
00138 
00140 matrix4 operator-(const matrix4& a);
00142 matrix4 operator+(const matrix4& a, const matrix4& b);
00144 matrix4 operator-(const matrix4& a, const matrix4& b);
00146 matrix4 operator*(const matrix4& a, const matrix4& b);
00148 matrix4 operator*(const matrix4& a, const double d);
00150 matrix4 operator*(const double d, const matrix4& a);
00152 matrix4 operator/(const matrix4& a, const double d);
00154 bool operator==(const matrix4& a, const matrix4& b);
00156 bool operator!=(const matrix4& a, const matrix4& b);
00158 point4 operator*(const matrix4& a, const point4& v);
00160 point3 operator*(const matrix4& a, const point3& v);
00162 inline matrix4 identity3()
00163 {
00164   return matrix4(
00165     vector4(1, 0, 0, 0),
00166     vector4(0, 1, 0, 0),
00167     vector4(0, 0, 1, 0),
00168     vector4(0, 0, 0, 1));
00169 }
00171 inline matrix4 transpose(const matrix4& v)
00172 {
00173   return matrix4(
00174     vector4(v[0][0], v[1][0], v[2][0], v[3][0]),
00175     vector4(v[0][1], v[1][1], v[2][1], v[3][1]),
00176     vector4(v[0][2], v[1][2], v[2][2], v[3][2]),
00177     vector4(v[0][3], v[1][3], v[2][3], v[3][3]));
00178 }
00180 inline matrix4 inverse(const matrix4& v)
00181 {
00182   matrix4 a(v),  // As a evolves from original mat into identity
00183   b(identity3()); // b evolves from identity into inverse(a)
00184 
00185   // Loop over cols of a from left to right, eliminating above and below diag
00186   for(int j=0; j<4; ++j) { // Find largest pivot in column j among rows j..3
00187     int i1 = j;    // Row with largest pivot candidate
00188 
00189     for(int i=j+1; i<4; ++i)
00190     {
00191       if (fabs(a.v[i].n[j]) > fabs(a.v[i1].n[j]))
00192         i1 = i;
00193     }
00194 
00195     // Swap rows i1 and j in a and b to put pivot on diagonal
00196     std::swap(a.v[i1], a.v[j]);
00197     std::swap(b.v[i1], b.v[j]);
00198 
00199     // Scale row j to have a unit diagonal
00200     if(!a.v[j].n[j])
00201     {
00202       // Singular matrix - can't invert
00203       log() << error << "Can't invert singular matrix!" << std::endl;
00204       return b;
00205     }
00206 
00207     b.v[j] /= a.v[j].n[j];
00208     a.v[j] /= a.v[j].n[j];
00209 
00210     // Eliminate off-diagonal elems in col j of a, doing identical ops to b
00211     for(int i=0; i<4; ++i)
00212     {
00213       if(i!=j)
00214       {
00215         b.v[i] -= a.v[i].n[j]*b.v[j];
00216         a.v[i] -= a.v[i].n[j]*a.v[j];
00217       }
00218     }
00219   }
00220   return b;
00221 }
00222 
00224 inline bool inside_out(const matrix4& m)
00225 {
00226   const vector3 a(m[0][0], m[0][1], m[0][2]);
00227   const vector3 b(m[1][0], m[1][1], m[1][2]);
00228   const vector3 c(m[2][0], m[2][1], m[2][2]);
00229 
00230   return ((a ^ b) * c) < 0 ? true : false;
00231 }
00232 
00234 inline const vector3 operator*(const matrix4& a, const vector3& v)
00235 {
00236   const vector4 temp((a * point4(v[0], v[1], v[2], 1)) - (a * point4(0, 0, 0, 1)));
00237   return vector3(temp.n[0], temp.n[1], temp.n[2]);
00238 }
00239 
00241 inline const vector3 operator*(const vector3& v, const matrix4& a)
00242 {
00243   return transpose(a) * v;
00244 }
00245 
00247 inline const normal3 operator*(const matrix4& a, const normal3& v)
00248 {
00249   const vector4 temp((a * point4(v[0], v[1], v[2], 1)) - (a * point4(0, 0, 0, 1)));
00250   return normal3(temp.n[0], temp.n[1], temp.n[2]);
00251 }
00252 
00254 inline const normal3 operator*(const normal3& v, const matrix4& a)
00255 {
00256   return transpose(a) * v;
00257 }
00258 
00260 std::ostream& operator<<(std::ostream& Stream, const matrix4& Arg);
00262 std::istream& operator>>(std::istream& Stream, matrix4& Arg);
00263 
00265 // quaternion
00266 
00268 class quaternion
00269 {
00270 public:
00271   // The scalar+vector representation is used
00272   double w;
00273   vector3 v;
00274 
00275   // Constructors
00276   quaternion();
00277   quaternion(const double u, const point3& t);
00278   quaternion(const double u, const vector3& t);
00279   quaternion(const double u, const double x, const double y, const double z);
00280   quaternion(const angle_axis& AngleAxis);
00281   quaternion(euler_angles Angles);
00283   quaternion& operator=(const quaternion& q);
00285   quaternion& operator=(const angle_axis& AngleAxis);
00287   quaternion& operator*=(const double d);
00289   quaternion& operator/=(const double d);
00291   quaternion& operator+=(const quaternion& q);
00293   quaternion& operator-=(const quaternion& q);
00295   quaternion& operator*=(const quaternion& q);
00297   quaternion& operator/=(const quaternion& q);
00299   double Magnitude() const;
00301   quaternion& Normalize();
00303   quaternion& Conjugate();
00305   quaternion& Inverse();
00307   quaternion& Square();
00309   quaternion& AxisRotation(const double phi);
00310 };
00311 
00313 quaternion operator-(quaternion& q);
00315 quaternion operator+(const quaternion& q, const quaternion& r);
00317 quaternion operator-(const quaternion& q, const quaternion& r);
00319 quaternion operator*(const quaternion& q, const quaternion& r);
00321 quaternion operator*(const quaternion& q, const double d);
00323 quaternion operator*(double d, const quaternion& q);
00325 quaternion operator/(const quaternion& q, const quaternion& r);
00327 quaternion operator/(const quaternion& q, const double d);
00329 bool operator==(const quaternion& q, const quaternion& r);
00331 bool operator!=(const quaternion& q, const quaternion& r);
00332 
00334 std::ostream& operator<<(std::ostream& Stream, const quaternion& Arg);
00336 std::istream& operator>>(std::istream& Stream, quaternion& Arg);
00337 
00339 // angle_axis
00340 
00342 class angle_axis
00343 {
00344 public:
00345   double angle;
00346   vector3 axis;
00347 
00348   // Constructors ...
00349   angle_axis();
00350   angle_axis(const double Angle, const point3& Axis);
00351   angle_axis(const double Angle, const vector3& Axis);
00352   angle_axis(const double Angle, const double X, const double Y, const double Z);
00353   angle_axis(const double AngleAxis[4]);
00354   angle_axis(const quaternion& Quaternion);
00356   angle_axis(const angle_axis& AngleAxis);
00358   angle_axis& operator=(const angle_axis& AngleAxis);
00360   angle_axis& operator=(const quaternion& Quaternion);
00362   angle_axis& operator=(const double AngleAxis[4]);
00364   operator double*() { return &angle; }
00366   void CopyArray(float AngleAxis[4]) const;
00368   void CopyArray(double AngleAxis[4]) const;
00369 };
00370 
00372 bool operator==(const angle_axis& a, const angle_axis& b);
00374 bool operator!=(const angle_axis& a, const angle_axis& b);
00375 
00377 std::ostream& operator<<(std::ostream& Stream, const angle_axis& Arg);
00379 std::istream& operator>>(std::istream& Stream, angle_axis& Arg);
00380 
00381 #define SDPEULERANGLEORDER(initialaxis, parity, repetition, frame) ((((((initialaxis << 1) + parity) << 1) + repetition) << 1) + frame)
00382 
00384 class euler_angles
00385 {
00386 public:
00387 
00389   typedef enum
00390   {
00391     StaticFrame = 0,
00392     RotatingFrame = 1,
00393   } OrderFrame;
00394 
00396   typedef enum
00397   {
00398     NoRepetition = 0,
00399     Repeats = 1,
00400   } OrderRepetition;
00401 
00403   typedef enum
00404   {
00405     EvenParity = 0,
00406     OddParity = 1,
00407   } OrderParity;
00408 
00410   typedef enum
00411   {
00412     XYZstatic = SDPEULERANGLEORDER(0, EvenParity, NoRepetition, StaticFrame),
00413     XYXstatic = SDPEULERANGLEORDER(0, EvenParity, Repeats, StaticFrame),
00414     XZYstatic = SDPEULERANGLEORDER(0, OddParity, NoRepetition, StaticFrame),
00415     XZXstatic = SDPEULERANGLEORDER(0, OddParity, Repeats, StaticFrame),
00416     YZXstatic = SDPEULERANGLEORDER(1, EvenParity, NoRepetition, StaticFrame),
00417     YZYstatic = SDPEULERANGLEORDER(1, EvenParity, Repeats, StaticFrame),
00418     YXZstatic = SDPEULERANGLEORDER(1, OddParity, NoRepetition, StaticFrame),
00419     YXYstatic = SDPEULERANGLEORDER(1, OddParity, Repeats, StaticFrame),
00420     ZXYstatic = SDPEULERANGLEORDER(2, EvenParity, NoRepetition, StaticFrame),
00421     ZXZstatic = SDPEULERANGLEORDER(2, EvenParity, Repeats, StaticFrame),
00422     ZYXstatic = SDPEULERANGLEORDER(2, OddParity, NoRepetition, StaticFrame),
00423     ZYZstatic = SDPEULERANGLEORDER(2, OddParity, Repeats, StaticFrame),
00424     ZYXrotating = SDPEULERANGLEORDER(0, EvenParity, NoRepetition, RotatingFrame),
00425     XYXrotating = SDPEULERANGLEORDER(0, EvenParity, Repeats, RotatingFrame),
00426     YZXrotating = SDPEULERANGLEORDER(0, OddParity, NoRepetition, RotatingFrame),
00427     XZXrotating = SDPEULERANGLEORDER(0, OddParity, Repeats, RotatingFrame),
00428     XZYrotating = SDPEULERANGLEORDER(1, EvenParity, NoRepetition, RotatingFrame),
00429     YZYrotating = SDPEULERANGLEORDER(1, EvenParity, Repeats, RotatingFrame),
00430     ZXYrotating = SDPEULERANGLEORDER(1, OddParity, NoRepetition, RotatingFrame),
00431     YXYrotating = SDPEULERANGLEORDER(1, OddParity, Repeats, RotatingFrame),
00432     YXZrotating = SDPEULERANGLEORDER(2, EvenParity, NoRepetition, RotatingFrame),
00433     ZXZrotating = SDPEULERANGLEORDER(2, EvenParity, Repeats, RotatingFrame),
00434     XYZrotating = SDPEULERANGLEORDER(2, OddParity, NoRepetition, RotatingFrame),
00435     ZYZrotating = SDPEULERANGLEORDER(2, OddParity, Repeats, RotatingFrame),
00436   } AngleOrder;
00437 
00439   double n[3];
00441   AngleOrder order;
00442 
00443   euler_angles();
00444   euler_angles(point3 Angles, AngleOrder Order);
00445   euler_angles(double X, double Y, double Z, AngleOrder Order);
00447   euler_angles(const quaternion& Quaternion, AngleOrder Order);
00449   euler_angles(const matrix4& Matrix, AngleOrder Order);
00450 
00452   static OrderFrame Frame(AngleOrder& Order) { return OrderFrame(Order & 1); }
00454   static OrderRepetition Repetition(AngleOrder& Order) { return OrderRepetition((Order >> 1) & 1); }
00456   static OrderParity Parity(AngleOrder& Order) { return OrderParity((Order >> 2) & 1); }
00458   static void Axes(AngleOrder& Order, int& i, int& j, int& k);
00459 
00461   OrderFrame Frame() { return Frame(order); }
00463   OrderRepetition Repetition() { return Repetition(order); }
00465   OrderParity Parity() { return Parity(order); }
00467   void Axes(int& i, int& j, int& k) { Axes(order, i, j, k); }
00468 
00470   double& operator[](int i);
00472   double operator[](int i) const;
00473 };
00474 
00476 std::ostream& operator<<(std::ostream& Stream, const euler_angles& Arg);
00478 std::istream& operator>>(std::istream& Stream, euler_angles& Arg);
00479 
00481 inline const matrix4 translate3(const vector3& v)
00482 {
00483   return matrix4(
00484     vector4(1, 0, 0, v[0]),
00485     vector4(0, 1, 0, v[1]),
00486     vector4(0, 0, 1, v[2]),
00487     vector4(0, 0, 0, 1));
00488 }
00489 
00491 inline const matrix4 translate3(const double_t X, const double_t Y, const double_t Z)
00492 {
00493   return matrix4(
00494     vector4(1, 0, 0, X),
00495     vector4(0, 1, 0, Y),
00496     vector4(0, 0, 1, Z),
00497     vector4(0, 0, 0, 1));
00498 }
00499 
00501 inline const matrix4 rotate3(const double Angle, vector3 Axis)
00502 {
00503   const double c = cos(Angle), s = sin(Angle), t = 1.0 - c;
00504 
00505   Axis = normalize(Axis);
00506 
00507   return matrix4(
00508     vector4(t * Axis[0] * Axis[0] + c, t * Axis[0] * Axis[1] - s * Axis[2], t * Axis[0] * Axis[2] + s * Axis[1], 0),
00509     vector4(t * Axis[0] * Axis[1] + s * Axis[2], t * Axis[1] * Axis[1] + c, t * Axis[1] * Axis[2] - s * Axis[0], 0),
00510     vector4(t * Axis[0] * Axis[2] - s * Axis[1], t * Axis[1] * Axis[2] + s * Axis[0], t * Axis[2] * Axis[2] + c, 0),
00511     vector4(0, 0, 0, 1));
00512 }
00514 inline const matrix4 rotate3(const angle_axis& AngleAxis)
00515 {
00516   return rotate3(AngleAxis.angle, AngleAxis.axis);
00517 }
00519 inline const matrix4 rotate3(const point3& EulerAngles)
00520 {
00521   matrix4 matrix = identity3();
00522   matrix = matrix * rotate3(EulerAngles[1], vector3(0, 1, 0));
00523   matrix = matrix * rotate3(EulerAngles[0], vector3(1, 0, 0));
00524   matrix = matrix * rotate3(EulerAngles[2], vector3(0, 0, 1));
00525 
00526   return matrix;
00527 }
00529 inline const matrix4 rotate3(const quaternion& Quaternion)
00530 {
00531   return rotate3(angle_axis(Quaternion));
00532 }
00534 inline const quaternion rotate3(const matrix4& m)
00535 {
00536   double d0 = m[0][0];
00537   double d1 = m[1][1];
00538   double d2 = m[2][2];
00539 
00540   double trace = d0 + d1 + d2 + 1;
00541   if(trace > 0.0)
00542   {
00543     double s = 0.5 / std::sqrt(trace);
00544     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]));
00545   }
00546 
00547   // Identify the major diagonal element with greatest value
00548   if(d0 >= d1 && d0 >= d2)
00549   {
00550     double s = std::sqrt(1.0 + d0 - d1 - d2) * 2.0;
00551     return quaternion(m[1][2] - m[2][1], point3(0.5, m[0][1] - m[1][0], m[0][2] - m[2][0])) / s;
00552   }
00553   else if(d1 >= d0 && d1 >= d2)
00554   {
00555     double s = std::sqrt(1.0 + d1 - d0 - d2) * 2.0;
00556     return quaternion(m[0][2] - m[2][0], point3(m[0][1] - m[1][0], 0.5, m[1][2] - m[2][1])) / s;
00557   }
00558   else
00559   {
00560     double s = std::sqrt(1.0 + d2 - d0 - d1) * 2.0;
00561     return quaternion(m[0][1] - m[1][0], point3(m[0][2] - m[2][0], m[1][2] - m[2][1], 0.5)) / s;
00562   }
00563 }
00564 
00566 inline const matrix4 scale3(const double_t S)
00567 {
00568   return matrix4(
00569     vector4(S, 0, 0, 0),
00570     vector4(0, S, 0, 0),
00571     vector4(0, 0, S, 0),
00572     vector4(0, 0, 0, 1));
00573 }
00574 
00576 inline const matrix4 scale3(const double_t X, const double_t Y, const double_t Z)
00577 {
00578   return matrix4(
00579     vector4(X, 0, 0, 0),
00580     vector4(0, Y, 0, 0),
00581     vector4(0, 0, Z, 0),
00582     vector4(0, 0, 0, 1));
00583 }
00585 inline const matrix4 perspective3(const double d)
00586 {
00587   return matrix4(
00588     vector4(1, 0, 0, 0),
00589     vector4(0, 1, 0, 0),
00590     vector4(0, 0, 1, 0),
00591     vector4(0, 0, 1/d, 0));
00592 }
00594 inline const matrix4 shear3(const double XY, const double XZ, const double YX, const double YZ, const double ZX, const double ZY)
00595 {
00596   return matrix4(
00597     vector4(1, XY, XZ, 0),
00598     vector4(YX, 1, YZ, 0),
00599     vector4(ZX, ZY, 1, 0),
00600     vector4(0, 0, 0, 1));
00601 }
00602 
00604 inline const vector3 look_vector(const matrix4& Matrix)
00605 {
00606   return normalize(Matrix * vector3(0, 0, 1));
00607 }
00608 
00610 inline const vector3 up_vector(const matrix4& Matrix)
00611 {
00612   return normalize(Matrix * vector3(0, 1, 0));
00613 }
00614 
00616 inline const vector3 right_vector(const matrix4& Matrix)
00617 {
00618   return normalize(Matrix * vector3(1, 0, 0));
00619 }
00620 
00622 inline const point3 position(const matrix4& Matrix)
00623 {
00624   return point3(Matrix[0][3], Matrix[1][3], Matrix[2][3]);
00625 }
00626 
00628 inline const matrix4 view_matrix(const vector3& Look, const vector3& Up, const point3& Position)
00629 {
00630   const vector3 look = normalize(Look);
00631   const vector3 right = normalize(Up ^ look);
00632   const vector3 up = normalize(look ^ right);
00633 
00634   return matrix4(
00635     vector4(right[0], up[0], look[0], Position[0]),
00636     vector4(right[1], up[1], look[1], Position[1]),
00637     vector4(right[2], up[2], look[2], Position[2]),
00638     vector4(0, 0, 0, 1));
00639 }
00640 
00641 //  Matrix utilities for affine matrices.
00642 //
00643 //  The input matrix must be affine, but need not be orthogonal.
00644 //  In other words, it may contain scaling, rotation, and translation,
00645 //  but not perspective projection.
00646 //  Ref. Foley and Van Dam, 2nd ed, p. 217.
00647 //
00649 inline const vector3 extract_translation(const matrix4& m)
00650 {
00651   return(vector3(m[0][3], m[1][3], m[2][3]));
00652 }
00654 inline const matrix4 extract_scaling(const matrix4& m)
00655 {
00656   return matrix4(
00657     vector4(sqrt(m[0][0] * m[0][0] + m[1][0] * m[1][0] + m[2][0] * m[2][0]), 0, 0, 0),
00658     vector4(0, sqrt(m[0][1] * m[0][1] + m[1][1] * m[1][1] + m[2][1] * m[2][1]), 0, 0),
00659     vector4(0, 0, sqrt(m[0][2] * m[0][2] + m[1][2] * m[1][2] + m[2][2] * m[2][2]), 0),
00660     vector4(0, 0, 0, 1));
00661 }
00663 inline const matrix4 extract_rotation(const matrix4& m)
00664 {
00665   // Get scaling ...
00666   const double scale_x = sqrt(m[0][0] * m[0][0] + m[1][0] * m[1][0] + m[2][0] * m[2][0]);
00667   const double scale_y = sqrt(m[0][1] * m[0][1] + m[1][1] * m[1][1] + m[2][1] * m[2][1]);
00668   const double scale_z = sqrt(m[0][2] * m[0][2] + m[1][2] * m[1][2] + m[2][2] * m[2][2]);
00669   return_val_if_fail(scale_x && scale_y && scale_z, identity3());
00670 
00671   // Apply inverse of scaling as a transformation, to get unit scale ...
00672   matrix4 unscaled(m * scale3(1.0 / scale_x, 1.0 / scale_y, 1.0 / scale_z));
00673 
00674   return matrix4(
00675     vector4(unscaled[0][0], unscaled[0][1], unscaled[0][2], 0),
00676     vector4(unscaled[1][0], unscaled[1][1], unscaled[1][2], 0),
00677     vector4(unscaled[2][0], unscaled[2][1], unscaled[2][2], 0),
00678     vector4(0, 0, 0, 1));
00679 }
00680 
00682 // point3 implementation
00683 
00684 inline point3 operator*(const matrix4& a, const point3& v)
00685 {
00686   const point4 result = a * point4(v[0], v[1], v[2], 1);
00687   return point3(result[0] / result[3], result[1] / result[3], result[2] / result[3]);
00688 }
00689 
00690 inline point3 operator*(const point3& v, const matrix4& a)
00691 {
00692   return transpose(a) * v;
00693 }
00694 
00696 // point4 implementation
00697 
00698 inline point4 operator*(const matrix4& a, const point4& v)
00699 {
00700 #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]
00701   return point4(ROWCOL(0), ROWCOL(1), ROWCOL(2), ROWCOL(3));
00702 #undef ROWCOL // (i)
00703 }
00704 
00705 inline point4 operator*(const point4& v, const matrix4& a)
00706 {
00707   return transpose(a) * v;
00708 }
00709 
00711 // matrix4 implementation
00712 
00713 inline matrix4::matrix4() {}
00714 
00715 inline matrix4::matrix4(const vector4& v0, const vector4& v1, const vector4& v2, const vector4& v3)
00716 { v[0] = v0; v[1] = v1; v[2] = v2; v[3] = v3; }
00717 
00718 inline matrix4::matrix4(const double d)
00719 { v[0] = v[1] = v[2] = v[3] = vector4(d, d, d, d); }
00720 
00721 inline matrix4::matrix4(const matrix4& m)
00722 { v[0] = m.v[0]; v[1] = m.v[1]; v[2] = m.v[2]; v[3] = m.v[3]; }
00723 
00724 inline matrix4::matrix4(euler_angles Angles)
00725 {
00726   const euler_angles::OrderFrame frame = Angles.Frame();
00727   const euler_angles::OrderParity parity = Angles.Parity();
00728   const euler_angles::OrderRepetition repetition = Angles.Repetition();
00729   int i, j, k;
00730   Angles.Axes(i, j, k);
00731 
00732   if(frame == euler_angles::RotatingFrame)
00733     std::swap(Angles.n[0], Angles.n[2]);
00734 
00735   if(parity == euler_angles::OddParity)
00736   {
00737     Angles.n[0] = -Angles.n[0];
00738     Angles.n[1] = -Angles.n[1];
00739     Angles.n[2] = -Angles.n[2];
00740   }
00741 
00742   const double ti = Angles.n[0], tj = Angles.n[1], th = Angles.n[2];
00743   const double ci = cos(ti), cj = cos(tj), ch = cos(th);
00744   const double si = sin(ti), sj = sin(tj), sh = sin(th);
00745   const double cc = ci*ch, cs = ci*sh;
00746   const double sc = si*ch, ss = si*sh;
00747 
00748   matrix4 m = identity3();
00749   if(repetition == euler_angles::Repeats)
00750   {
00751     m[i][i] =  cj;    m[i][j] =  sj*si; m[i][k] =  sj*ci;
00752     m[j][i] =  sj*sh; m[j][j] = -cj*ss+cc;  m[j][k] = -cj*cs-sc;
00753     m[k][i] = -sj*ch; m[k][j] =  cj*sc+cs;  m[k][k] =  cj*cc-ss;
00754   }
00755   else
00756   {
00757     m[i][i] =  cj*ch; m[i][j] = sj*sc-cs; m[i][k] = sj*cc+ss;
00758     m[j][i] =  cj*sh; m[j][j] = sj*ss+cc; m[j][k] = sj*cs-sc;
00759     m[k][i] = -sj;    m[k][j] = cj*si;  m[k][k] = cj*ci;
00760   }
00761 
00762   *this = m;
00763 }
00764 
00765 inline matrix4& matrix4::operator=(const matrix4& m)
00766 { v[0] = m.v[0]; v[1] = m.v[1]; v[2] = m.v[2]; v[3] = m.v[3];
00767 return *this; }
00768 
00769 /*
00770 inline matrix4& matrix4::operator=(const double d[4][4])
00771 { v[0] = d[0]; v[1] = d[1]; v[2] = d[2]; v[3] = d[3]; return *this; }
00772 */
00773 
00774 inline matrix4& matrix4::operator=(const double d[16])
00775 { memcpy(&v[0][0], &d[0], 16 * sizeof(double)); return *this; }
00776 
00777 inline matrix4& matrix4::operator+=(const matrix4& m)
00778 { v[0] += m.v[0]; v[1] += m.v[1]; v[2] += m.v[2]; v[3] += m.v[3];
00779 return *this; }
00780 
00781 inline matrix4& matrix4::operator-=(const matrix4& m)
00782 { v[0] -= m.v[0]; v[1] -= m.v[1]; v[2] -= m.v[2]; v[3] -= m.v[3];
00783 return *this; }
00784 
00785 inline matrix4& matrix4::operator*=(const double d)
00786 { v[0] *= d; v[1] *= d; v[2] *= d; v[3] *= d; return *this; }
00787 
00788 inline matrix4& matrix4::operator/=(const double d)
00789 {
00790   return_val_if_fail(d, *this);
00791 
00792   const double inv = 1./d;
00793   v[0] *= inv;
00794   v[1] *= inv;
00795   v[2] *= inv;
00796   v[3] *= inv;
00797   return *this;
00798 }
00799 
00800 inline vector4& matrix4::operator[](int i)
00801 {
00802   return v[i];
00803 }
00804 
00805 inline const vector4& matrix4::operator[](int i) const
00806 {
00807   return v[i];
00808 }
00809 
00810 inline void matrix4::CopyArray(float m[16]) const
00811 {
00812   unsigned long index = 0;
00813   for(unsigned long i = 0; i < 4; ++i)
00814     for(unsigned long j = 0; j < 4; ++j)
00815       m[index++] = float(v[i][j]);
00816 }
00817 
00818 inline void matrix4::CopyArray(double m[16]) const
00819 {
00820   unsigned long index = 0;
00821   for(unsigned long i = 0; i < 4; ++i)
00822     for(unsigned long j = 0; j < 4; ++j)
00823       m[index++] = v[i][j];
00824 }
00825 
00826 inline matrix4 operator-(const matrix4& a)
00827 { return matrix4(-a.v[0], -a.v[1], -a.v[2], -a.v[3]); }
00828 
00829 inline matrix4 operator+(const matrix4& a, const matrix4& b)
00830 { 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]); }
00831 
00832 inline matrix4 operator-(const matrix4& a, const matrix4& b)
00833 { 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]); }
00834 
00835 inline matrix4 operator*(const matrix4& a, const matrix4& b)
00836 {
00837 #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]
00838   return matrix4(
00839     vector4(ROWCOL(0,0), ROWCOL(0,1), ROWCOL(0,2), ROWCOL(0,3)),
00840     vector4(ROWCOL(1,0), ROWCOL(1,1), ROWCOL(1,2), ROWCOL(1,3)),
00841     vector4(ROWCOL(2,0), ROWCOL(2,1), ROWCOL(2,2), ROWCOL(2,3)),
00842     vector4(ROWCOL(3,0), ROWCOL(3,1), ROWCOL(3,2), ROWCOL(3,3)));
00843 }
00844 
00845 inline matrix4 operator*(const matrix4& a, const double d)
00846 { return matrix4(a.v[0] * d, a.v[1] * d, a.v[2] * d, a.v[3] * d); }
00847 
00848 inline matrix4 operator*(const double d, const matrix4& a)
00849 { return a*d; }
00850 
00851 inline matrix4 operator/(const matrix4& a, const double d)
00852 {
00853   return_val_if_fail(d, matrix4());
00854 
00855   const double inv = 1./d;
00856   return matrix4(a.v[0] * inv, a.v[1] * inv, a.v[2] * inv, a.v[3] * inv);
00857 }
00858 
00859 inline bool operator==(const matrix4& a, const matrix4& b)
00860 { 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])); }
00861 
00862 inline bool operator!=(const matrix4& a, const matrix4& b)
00863 { return !(a == b); }
00864 
00865 inline std::ostream& operator<<(std::ostream& Stream, const matrix4& Arg)
00866 {
00867   Stream << Arg[0] << " " << Arg[1] << " " << Arg[2] << " " << Arg[3];
00868   return Stream;
00869 }
00870 
00871 inline std::istream& operator>>(std::istream& Stream, matrix4& Arg)
00872 {
00873   Stream >> Arg[0] >> Arg[1] >> Arg[2] >> Arg[3];
00874   return Stream;
00875 }
00876 
00878 // quaternion implementation
00879 
00880 inline quaternion::quaternion()
00881 { w = 1.0; v = vector3(0, 0, 0); }
00882 
00883 inline quaternion::quaternion(const double u, const point3& t)
00884 { w = u; v = to_vector(t); }
00885 
00886 inline quaternion::quaternion(const double u, const vector3& t)
00887 { w = u; v = t; }
00888 
00889 inline quaternion::quaternion(const double u, const double x, const double y, const double z)
00890 { w = u; v = vector3(x, y, z); }
00891 
00892 inline quaternion::quaternion(const angle_axis& AngleAxis)
00893 { w = cos(AngleAxis.angle * 0.5); vector3 axis = k3d::normalize(AngleAxis.axis); v = sin(AngleAxis.angle * 0.5) * axis; }
00894 
00895 inline quaternion::quaternion(euler_angles Angles)
00896 {
00897   const euler_angles::OrderFrame frame = Angles.Frame();
00898   const euler_angles::OrderParity parity = Angles.Parity();
00899   const euler_angles::OrderRepetition repetition = Angles.Repetition();
00900   int i, j, k;
00901   Angles.Axes(i, j, k);
00902 
00903   if(frame == euler_angles::RotatingFrame)
00904     std::swap(Angles.n[0], Angles.n[2]);
00905 
00906   if(parity == euler_angles::OddParity)
00907     Angles.n[1] = -Angles.n[1];
00908 
00909   const double ti = Angles.n[0]*0.5, tj = Angles.n[1]*0.5, th = Angles.n[2]*0.5;
00910   const double ci = cos(ti), cj = cos(tj), ch = cos(th);
00911   const double si = sin(ti), sj = sin(tj), sh = sin(th);
00912   const double cc = ci*ch, cs = ci*sh;
00913   const double sc = si*ch, ss = si*sh;
00914 
00915   if(repetition == euler_angles::Repeats)
00916   {
00917     v[i] = cj*(cs + sc);
00918     v[j] = sj*(cc + ss);
00919     v[k] = sj*(cs - sc);
00920     w = cj*(cc - ss);
00921   }
00922   else
00923   {
00924     v[i] = cj*sc - sj*cs;
00925     v[j] = cj*ss + sj*cc;
00926     v[k] = cj*cs - sj*sc;
00927     w = cj*cc + sj*ss;
00928   }
00929 
00930   if(parity == euler_angles::OddParity)
00931     v[j] = -v[j];
00932 }
00933 
00934 inline quaternion& quaternion::operator=(const quaternion& q)
00935 { w = q.w; v = q.v; return *this; }
00936 
00937 inline quaternion& quaternion::operator=(const angle_axis& AngleAxis)
00938 { w = cos(AngleAxis.angle * 0.5); vector3 axis = k3d::normalize(AngleAxis.axis); v = sin(AngleAxis.angle * 0.5) * axis; return *this; }
00939 
00940 inline quaternion& quaternion::operator*=(const double d)
00941 { w *= d; v *= d; return *this; }
00942 
00943 inline quaternion& quaternion::operator/=(const double d)
00944 {
00945   return_val_if_fail(d, *this);
00946 
00947   const double inv = 1./d;
00948   w *= inv;
00949   v *= inv;
00950   return *this;
00951 }
00952 
00953 inline quaternion& quaternion::operator+=(const quaternion& q)
00954 { w += q.w; v += q.v; return *this; }
00955 
00956 inline quaternion& quaternion::operator-=(const quaternion& q)
00957 { w -= q.w; v -= q.v; return *this; }
00958 
00959 inline quaternion& quaternion::operator*=(const quaternion& q)
00960 { w = w*q.w - (v*q.v); v = q.w*v + w*q.v + (v^q.v); return *this; }
00961 
00962 inline quaternion& quaternion::operator/=(const quaternion& q)
00963 { quaternion t = q; (*this) *= t.Inverse(); return *this; }
00964 
00965 inline double quaternion::Magnitude() const
00966 { return std::sqrt(w*w + v.length2()); }
00967 
00968 inline quaternion& quaternion::Normalize()
00969 {
00970   if(const double magnitude = Magnitude())
00971     *this /= magnitude;
00972 
00973   return *this;
00974 }
00975 
00976 inline quaternion& quaternion::Conjugate()
00977 { v = -v; return *this; }
00978 
00979 inline quaternion& quaternion::Inverse()
00980 { if(Magnitude() != 0.0) { const double i = 1.0 / Magnitude(); w *= i; v *= -i; } return *this; }
00981 
00982 inline quaternion& quaternion::Square()
00983 { const double t = 2*w; w = w*w - v.length2(); v *= t; return *this; }
00984 
00985 inline quaternion& quaternion::AxisRotation(const double phi)
00986 { v = k3d::normalize(v); v *= sin(phi/2.0); w = cos(phi/2); return *this; }
00987 
00988 inline quaternion operator-(const quaternion& q)
00989 { return quaternion(-q.w,-q.v); }
00990 
00991 inline quaternion operator+(const quaternion& q, const quaternion& r)
00992 { return quaternion(q.w + r.w, q.v + r.v); }
00993 
00994 inline quaternion operator-(const quaternion& q, const quaternion& r)
00995 { return quaternion(q.w - r.w, q.v - r.v); }
00996 
00997 inline quaternion operator*(const quaternion& q, const quaternion& r)
00998 { return quaternion(q.w*r.w - (q.v*r.v), q.w*r.v + r.w*q.v + (q.v^r.v)); }
00999 
01000 inline quaternion operator*(const quaternion& q, const double d)
01001 { return quaternion(q.w*d, q.v*d); }
01002 
01003 inline quaternion operator*(const double d, const quaternion& q)
01004 { return quaternion(q.w*d, q.v*d); }
01005 
01006 inline quaternion operator/(const quaternion& q, const quaternion& r)
01007 { quaternion t = r; return q*t.Inverse(); }
01008 
01009 inline quaternion operator/(const quaternion& q, const double d)
01010 {
01011   return_val_if_fail(d, quaternion());
01012 
01013   const double inv = 1./d;
01014   return quaternion(q.w * inv, q.v * inv);
01015 }
01016 
01017 inline bool operator==(const quaternion& q, const quaternion& r)
01018 { return ((q.w == r.w) && (q.v == r.v)); }
01019 
01020 inline bool operator!=(const quaternion& q, const quaternion& r)
01021 { return !(q == r); }
01022 
01023 inline std::ostream& operator<<(std::ostream& Stream, const quaternion& Arg)
01024 {
01025   Stream << Arg.w << " " << Arg.v[0] << " " << Arg.v[1] << " " << Arg.v[2];
01026   return Stream;
01027 }
01028 
01029 inline std::istream& operator>>(std::istream& Stream, quaternion& Arg)
01030 {
01031   Stream >> Arg.w >> Arg.v[0] >> Arg.v[1] >> Arg.v[2];
01032   return Stream;
01033 }
01034 
01036 // angle_axis implementation ...
01037 
01038 inline angle_axis::angle_axis()
01039 { angle = 0.0; axis = vector3(0, 0, 1); }
01040 
01041 inline angle_axis::angle_axis(const double Angle, const point3& Axis)
01042 { angle = Angle; axis = normalize(to_vector(Axis)); }
01043 
01044 inline angle_axis::angle_axis(const double Angle, const vector3& Axis)
01045 { angle = Angle; axis = normalize(Axis); }
01046 
01047 inline angle_axis::angle_axis(const double Angle, const double X, const double Y, const double Z)
01048 { angle = Angle; axis[0] = X; axis[1] = Y; axis[2] = Z; axis = normalize(axis); }
01049 
01050 inline angle_axis::angle_axis(const double AngleAxis[4])
01051 { angle = AngleAxis[0]; axis[0] = AngleAxis[1]; axis[1] = AngleAxis[2]; axis[2] = AngleAxis[3]; axis = normalize(axis); }
01052 
01053 inline angle_axis::angle_axis(const quaternion& Quaternion)
01054 {
01055   quaternion q = Quaternion;
01056   q.Normalize();
01057   const double halftheta = acos(q.w);
01058   const double sinhalftheta = sin(halftheta);
01059 
01060   angle = 2.0 * halftheta;
01061   if(sinhalftheta != 0.0)
01062     axis = q.v / sinhalftheta;
01063   else
01064     axis = vector3(0, 0, 1);
01065 }
01066 
01067 inline angle_axis::angle_axis(const angle_axis& AngleAxis)
01068 { angle = AngleAxis.angle; axis = AngleAxis.axis; }
01069 
01070 inline angle_axis& angle_axis::operator=(const angle_axis& AngleAxis)
01071 { angle = AngleAxis.angle; axis = AngleAxis.axis; return *this; }
01072 
01073 inline angle_axis& angle_axis::operator=(const quaternion& Quaternion)
01074 {
01075   const double halftheta = acos(Quaternion.w);
01076   const double sinhalftheta = sin(halftheta);
01077 
01078   angle = 2.0 * halftheta;
01079   if(sinhalftheta != 0.0)
01080     axis = Quaternion.v / sinhalftheta;
01081   else
01082     axis = vector3(0, 0, 1);
01083 
01084   return *this;
01085 }
01086 
01087 inline angle_axis& angle_axis::operator=(const double AngleAxis[4])
01088 { angle = AngleAxis[0]; axis[0] = AngleAxis[1]; axis[1] = AngleAxis[2]; axis[2] = AngleAxis[3]; return *this; }
01089 
01090 inline void angle_axis::CopyArray(float AngleAxis[4]) const
01091 { AngleAxis[0] = float(angle); AngleAxis[1] = float(axis[0]); AngleAxis[2] = float(axis[1]); AngleAxis[3] = float(axis[2]); }
01092 
01093 inline void angle_axis::CopyArray(double AngleAxis[4]) const
01094 { AngleAxis[0] = angle; AngleAxis[1] = axis[0]; AngleAxis[2] = axis[1]; AngleAxis[3] = axis[2]; }
01095 
01096 inline bool operator==(const angle_axis& a, const angle_axis& b)
01097 { return ((a.angle == b.angle) && (a.axis == b.axis)); }
01098 inline bool operator!=(const angle_axis& a, const angle_axis& b)
01099 { return !(a == b); }
01100 
01101 inline std::ostream& operator<<(std::ostream& Stream, const angle_axis& Arg)
01102 {
01103   Stream << Arg.angle << " " << Arg.axis[0] << " " << Arg.axis[1] << " " << Arg.axis[2];
01104   return Stream;
01105 }
01106 
01107 inline std::istream& operator>>(std::istream& Stream, angle_axis& Arg)
01108 {
01109   Stream >> Arg.angle >> Arg.axis[0] >> Arg.axis[1] >> Arg.axis[2];
01110   return Stream;
01111 }
01112 
01113 //  Unit quaternion utilities.
01114 //
01115 //  Those special functions deal with rotations, thus require unit quaternions.
01116 //
01117 
01118 //  Euler/Matrix/Quaternion conversion functions are based on Ken Shoemake code,
01119 //  from Graphic Gems IV
01120 
01122 inline quaternion Slerp(const quaternion& q1, const quaternion& q2, double t)
01123 {
01124   // Calculate the angle between the two quaternions ...
01125   double cosangle = q1.w * q2.w + q1.v * q2.v;
01126 
01127   if(cosangle < -1.0)
01128     cosangle = -1.0;
01129   if(cosangle > 1.0)
01130     cosangle = 1.0;
01131 
01132   // If the angle is reasonably large, do the spherical interpolation
01133   if(1.0 - cosangle > 16 * FLT_EPSILON)
01134   {
01135     const double angle = acos(cosangle);
01136     return (sin((1.0 - t)*angle)*q1 + sin(t*angle)*q2) / sin(angle);
01137   }
01138 
01139   // The angle is too close to zero - do a linear interpolation
01140   return mix(q1, q2, t);
01141 }
01142 
01144 // euler_angles implementation
01145 
01146 inline euler_angles::euler_angles()
01147 {
01148   n[0] = n[1] = n[2] = 0.0;
01149   order = YXZstatic;
01150 }
01151 
01152 inline euler_angles::euler_angles(point3 Angles, AngleOrder Order)
01153 {
01154   n[0] = Angles[0];
01155   n[1] = Angles[1];
01156   n[2] = Angles[2];
01157   order = Order;
01158 }
01159 
01160 inline euler_angles::euler_angles(double X, double Y, double Z, AngleOrder Order)
01161 {
01162   n[0] = X;
01163   n[1] = Y;
01164   n[2] = Z;
01165   order = Order;
01166 }
01167 
01168 inline euler_angles::euler_angles(const quaternion& Quaternion, AngleOrder Order)
01169 {
01170   const double NQuaternion = Quaternion.Magnitude();
01171   const double s = (NQuaternion > 0.0) ? (2.0 / NQuaternion) : 0.0;
01172 
01173   const double xs = Quaternion.v[0]*s, ys = Quaternion.v[1]*s, zs = Quaternion.v[2]*s;
01174   const double wx = Quaternion.w*xs, wy = Quaternion.w*ys, wz = Quaternion.w*zs;
01175   const double xx = Quaternion.v[0]*xs, xy = Quaternion.v[0]*ys, xz = Quaternion.v[0]*zs;
01176   const double yy = Quaternion.v[1]*ys, yz = Quaternion.v[1]*zs, zz = Quaternion.v[2]*zs;
01177 
01178   matrix4 matrix = identity3();
01179   matrix[0][0] = 1.0 - (yy + zz);
01180   matrix[0][1] = xy - wz;
01181   matrix[0][2] = xz + wy;
01182   matrix[1][0] = xy + wz;
01183   matrix[1][1] = 1.0 - (xx + zz);
01184   matrix[1][2] = yz - wx;
01185   matrix[2][0] = xz - wy;
01186   matrix[2][1] = yz + wx;
01187   matrix[2][2] = 1.0 - (xx + yy);
01188 
01189   *this = euler_angles(matrix, Order);
01190 }
01191 
01192 inline euler_angles::euler_angles(const matrix4& Matrix, AngleOrder Order)
01193 {
01194   OrderRepetition repetition = Repetition(Order);
01195   OrderParity parity = Parity(Order);
01196   OrderFrame frame = Frame(Order);
01197   int i, j, k;
01198   Axes(Order, i, j, k);
01199 
01200   if(repetition == euler_angles::Repeats)
01201   {
01202     const double sy = sqrt(Matrix[i][j]*Matrix[i][j] + Matrix[i][k]*Matrix[i][k]);
01203     if(sy > 16*FLT_EPSILON)
01204     {
01205       n[0] = atan2(Matrix[i][j], Matrix[i][k]);
01206       n[1] = atan2(sy, Matrix[i][i]);
01207       n[2] = atan2(Matrix[j][i], -Matrix[k][i]);
01208     }
01209     else
01210     {
01211       n[0] = atan2(-Matrix[j][k], Matrix[j][j]);
01212       n[1] = atan2(sy, Matrix[i][i]);
01213       n[2] = 0;
01214     }
01215   }
01216   else
01217   {
01218     const double cy = sqrt(Matrix[i][i]*Matrix[i][i] + Matrix[j][i]*Matrix[j][i]);
01219     if(cy > 16*FLT_EPSILON)
01220     {
01221       n[0] = atan2(Matrix[k][j], Matrix[k][k]);
01222       n[1] = atan2(-Matrix[k][i], cy);
01223       n[2] = atan2(Matrix[j][i], Matrix[i][i]);
01224     }
01225     else
01226     {
01227       n[0] = atan2(-Matrix[j][k], Matrix[j][j]);
01228       n[1] = atan2(-Matrix[k][i], cy);
01229       n[2] = 0;
01230     }
01231   }
01232 
01233   if(parity == euler_angles::OddParity)
01234   {
01235     n[0] = -n[0];
01236     n[1] = -n[1];
01237     n[2] = -n[2];
01238   }
01239 
01240   if(frame == euler_angles::RotatingFrame)
01241     std::swap(n[0], n[2]);
01242 
01243   order = Order;
01244 }
01245 
01246 inline void euler_angles::Axes(AngleOrder& Order, int& i, int& j, int& k)
01247 {
01248   const int safe[4] = {0, 1, 2, 0};
01249   const int next[4] = {1, 2, 0, 1};
01250 
01251   i = safe[(Order >> 3) & 3];
01252   j = next[i + Parity(Order)];
01253   k = next[i + 1 - Parity(Order)];
01254 }
01255 
01256 inline double& euler_angles::operator[](int i)
01257 {
01258   return_val_if_fail((i >= 0 && i <= 2), n[0]);
01259   return n[i];
01260 }
01261 
01262 inline double euler_angles::operator[](int i) const
01263 {
01264   return_val_if_fail((i >= 0 && i <= 2), n[0]);
01265   return n[i];
01266 }
01267 
01268 inline std::ostream& operator<<(std::ostream& Stream, const euler_angles& Arg)
01269 {
01270   Stream << Arg.n[0] << " " << Arg.n[1] << " " << Arg.n[2] << " " << int(Arg.order);
01271   return Stream;
01272 }
01273 
01274 inline std::istream& operator>>(std::istream& Stream, euler_angles& Arg)
01275 {
01276   int order;
01277   Stream >> Arg.n[0] >> Arg.n[1] >> Arg.n[2] >> order;
01278 
01279   Arg.order = euler_angles::AngleOrder(order);
01280 
01281   return Stream;
01282 }
01283 
01284 namespace difference
01285 {
01286 
01288 inline void test(const matrix4& A, const matrix4& B, accumulator& Result)
01289 {
01290   range_test(A.v, A.v + 4, B.v, B.v + 4, Result);
01291 }
01292 
01293 } // namespace difference
01294 
01295 } // namespace k3d
01296 
01297 #endif // !K3DSDK_ALGEBRA_H
01298