Logo Search packages:      
Sourcecode: k3d version File versions

algebra.h

Go to the documentation of this file.
#ifndef K3DSDK_ALGEBRA_H
#define K3DSDK_ALGEBRA_H

// K-3D
// Copyright (c) 1995-2004, Timothy M. Shead
//
// Contact: tshead@k-3d.com
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public
// License as published by the Free Software Foundation; either
// version 2 of the License, or (at your option) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

/** \file
            \brief Algebra classes
            \author Tim Shead (tshead@k-3d.com)
*/

#include "basic_math.h"
#include "vectors.h"

#ifndef FLT_EPSILON
#define FLT_EPSILON 0.000001
#endif

/****************************************************************
*
* C++ Vector and Matrix Algebra routines
* Author: Jean-Francois DOUE
* Version 3.1 --- October 1993
*
****************************************************************/

//
//    From "Graphics Gems IV / Edited by Paul S. Heckbert
//    Academic Press, 1994, ISBN 0-12-336156-9
//    "You are free to use and modify this code in any way
//    you like." (p. xv)
//
//    Modified by J. Nagle, March 1997
//    -     All functions are inline.
//    -     All functions are const-correct.
//    -     All checking is via the standard "assert" macro.
//

// Modified by Tim Shead for use with K-3D, January 1998

namespace k3d
{

class matrix3;
class matrix4;
class quaternion;
class angle_axis;
class euler_angles;

/////////////////////////////////////////////////////////////////////////////
// vector2

/// Multiplication by a matrix
vector2 operator * (const matrix3& a, const vector2& v);
/// Multiplication by a matrix
vector2 operator * (const vector2& v, const matrix3& a);

/////////////////////////////////////////////////////////////////////////////
// vector3

/// Matrix multiplication
vector3 operator * (const matrix4& a, const vector3& v);
/// Matrix multiplication
vector3 operator * (const vector3& v, const matrix4& a);
/// Returns a linear transformation
vector2 operator * (const matrix3& a, const vector2& v);
/// Returns a matrix product
matrix3 operator * (const matrix3& a, const matrix3& b);

/////////////////////////////////////////////////////////////////////////////
// vector4

/// Matrix multiplication
vector4 operator * (const matrix4& a, const vector4& v);
/// Matrix multiplication
vector4 operator * (const vector4& v, const matrix4& a);
/// Linear transformation
vector3 operator * (const matrix4& a, const vector3& v);
/// Matrix multiplication
matrix4 operator * (const matrix4& a, const matrix4& b);

/////////////////////////////////////////////////////////////////////////////
// matrix3

/// A 3x3 matrix
00102 class matrix3
{
public:
      /// Stores the matrix data
00106       vector3 v[3];
      // Constructors
      matrix3();
      /// Creates the matrix from three vectors
      matrix3(const vector3& v0, const vector3& v1, const vector3& v2);
      /// Creates a matrix of identical elements
      matrix3(const double d);
      /// Copy constructor
      matrix3(const matrix3& m);
      /// Assignment
      matrix3& operator = (const matrix3& m);
      /// Addition
      matrix3& operator += (const matrix3& m);
      /// Subtraction
      matrix3& operator -= (const matrix3& m);
      /// Multiplication by a constant
      matrix3& operator *= (const double d);
      /// Division by a constant
      matrix3& operator /= (const double d);
      /// Returns a vector reference by index
      vector3& operator [] (int i);
      /// Returns a vector reference by index
      const vector3& operator [] (int i) const;
      /// Returns the matrix transposition
      matrix3 Transpose() const;
      /// Returns the matrix inverse
      matrix3 Inverse() const;
      /// Decomposes the matrix returning rotations around X, Y and Z axes
      void Decompose(double& x, double& y, double& z) const;
};

/// Negation
matrix3 operator - (const matrix3& a);
/// Addition
matrix3 operator + (const matrix3& a, const matrix3& b);
/// Subtraction
matrix3 operator - (const matrix3& a, const matrix3& b);
/// Matrix multiplication
matrix3 operator * (const matrix3& a, const matrix3& b);
/// Multiplication by a constant
matrix3 operator * (const matrix3& a, const double d);
/// Multiplication by a constant
matrix3 operator * (const double d, const matrix3& a);
/// Division by a constant
matrix3 operator / (const matrix3& a, const double d);
/// Equality
bool operator == (const matrix3& a, const matrix3& b);
/// Non-equality
bool operator != (const matrix3& a, const matrix3& b);
/// Linear transformation
vector3 operator * (const matrix3& a, const vector3& v);
/// Linear transformation
vector2 operator * (const matrix3& a, const vector2& v);

/// Output inserter
std::ostream& operator<<(std::ostream& Stream, const matrix3& Arg);
/// Input extractor
std::istream& operator>>(std::istream& Stream, matrix3& Arg);

/////////////////////////////////////////////////////////////////////////////
// matrix4

/// A 4x4 matrix
00169 class matrix4
{
public:
      /// Stores the matrix elements
00173       vector4 v[4];
      // Constructors
      matrix4();
      matrix4(const vector4& v0, const vector4& v1, const vector4& v2, const vector4& v3);
      matrix4(const double d);
      matrix4(euler_angles Angles);
      /// Copy constructor
      matrix4(const matrix4& m);
      /// Assignment of an matrix4
      matrix4& operator	= ( const matrix4& m );
      /// Assignment of a C/C++ array
      matrix4& operator  = ( const double d[4][4]);
      /// Assignment of a C/C++ array
      matrix4& operator  = ( const double d[16]);
      /// Addition
      matrix4& operator += ( const matrix4& m );
      /// Subtraction
      matrix4& operator -= ( const matrix4& m );
      /// Multiplication by a constant
      matrix4& operator *= ( const double d );
      /// Division by a constant
      matrix4& operator /= ( const double d );
      /// Returns a vector by index
      vector4& operator [] ( int i);
      /// Returns a vector by index
      const vector4& operator [] ( int i) const;
      /// Returns the matrix transposition
      matrix4 Transpose() const;
      /// Returns the matrix inverse
      matrix4 Inverse() const;
      /// Copies the matrix contents into a C/C++ style array
      void CopyArray(float m[16]) const;
      /// Copies the matrix contents into a C/C++ style array
      void CopyArray(double m[16]) const;
      /// Casts the matrix to a C/C++ style array
00208       operator double*() { return &v[0][0]; }
};

/// Negation
matrix4 operator - (const matrix4& a);
/// Addition
matrix4 operator + (const matrix4& a, const matrix4& b);
/// Subtraction
matrix4 operator - (const matrix4& a, const matrix4& b);
/// Matrix multiplication
matrix4 operator * (const matrix4& a, const matrix4& b);
/// Multiplication by a constant
matrix4 operator * (const matrix4& a, const double d);
/// Multiplication by a constant
matrix4 operator * (const double d, const matrix4& a);
/// Division by a constant
matrix4 operator / (const matrix4& a, const double d);
/// Equality
bool operator == (const matrix4& a, const matrix4& b);
/// Non-equality
bool operator != (const matrix4& a, const matrix4& b);
/// Linear transformation
vector4 operator * (const matrix4& a, const vector4& v);
/// Linear transformation
vector3 operator * (const matrix4& a, const vector3& v);

/// Output inserter
std::ostream& operator<<(std::ostream& Stream, const matrix4& Arg);
/// Input extractor
std::istream& operator>>(std::istream& Stream, matrix4& Arg);

/////////////////////////////////////////////////////////////////////////////
// quaternion

/// Encapsulates a quaternion
00243 class quaternion
{
public:
      // The scalar+vector representation is used
      double w;
      vector3 v;

      // Constructors
      quaternion();
      quaternion(const double u, const vector3& t);
      quaternion(const double u, const double x, const double y, const double z);
      quaternion(const angle_axis& AngleAxis);
      quaternion(euler_angles Angles);
      /// Assignment of a quaternion
      quaternion& operator = (const quaternion& q);
      /// Assignment of an axis/angle
      quaternion& operator = (const angle_axis& AngleAxis);
      /// Multiplication by a constant
      quaternion& operator *= (const double f);
      /// Division by a constant
      quaternion& operator /= (const double f);
      /// Addition
      quaternion& operator += (const quaternion& q);
      /// Substraction
      quaternion& operator -= (const quaternion& q);
      /// Multiplication
      quaternion& operator *= (const quaternion& q);
      /// Division
      quaternion& operator /= (const quaternion& q);
      /// Magnitude
      double Magnitude() const;
      /// Normalization
      quaternion& Normalize();
      /// Conjugate
      quaternion& Conjugate();
      /// Inversion in place
      quaternion& Inverse();
      /// Squares the quaternion in place
      quaternion& Square();
      /// Axis rotation
      quaternion& AxisRotation(const double phi);
};

/// Negation
quaternion operator - (quaternion& q);
/// Addition
quaternion operator + (const quaternion& q, const quaternion& r);
/// Substraction
quaternion operator - (const quaternion& q, const quaternion& r);
/// Multiplication by a quaternion
quaternion operator * (const quaternion& q, const quaternion& r);
/// Multiplication by a constant
quaternion operator * (const quaternion& q, const double f);
/// Multiplication by a constant
quaternion operator * (double f, const quaternion& q);
/// Division by a quaternion
quaternion operator / (const quaternion& q, const quaternion& r);
/// Division by a constant
quaternion operator / (const quaternion& q, const double f);
/// Equality test
bool operator == (const quaternion& q, const quaternion& r);
/// Non-equality test
bool operator != (const quaternion& q, const quaternion& r);

/// Output inserter
std::ostream& operator<<(std::ostream& Stream, const quaternion& Arg);
/// Input extractor
std::istream& operator>>(std::istream& Stream, quaternion& Arg);

/////////////////////////////////////////////////////////////////////////////
// angle_axis

/// Encapsulates an angle/axis representation of an object's orientation
00316 class angle_axis
{
public:
      double angle;
      vector3 axis;

      // Constructors ...
      angle_axis();
      angle_axis(const double Angle, const vector3& Axis);
      angle_axis(const double Angle, const double X, const double Y, const double Z);
      angle_axis(const double AngleAxis[4]);
      angle_axis(const quaternion& Quaternion);
      /// Copy constructor
      angle_axis(const angle_axis& AngleAxis);
      /// Assignment of an angle_axis
      angle_axis& operator = ( const angle_axis& AngleAxis );
      /// Assignment of a quaternion
      angle_axis& operator = ( const quaternion& Quaternion );
      /// Assignment of a C/C++ style array
      angle_axis& operator = ( const double AngleAxis[4]);
      /// Casting to a C/C++ style array pointer
00337       operator double*() { return &angle; }
      /// Copies the vector into a C/C++ style array
      void CopyArray(float AngleAxis[4]) const;
      /// Copies the vector into a C/C++ style array
      void CopyArray(double AngleAxis[4]) const;
};

/// Equality test
bool operator == (const angle_axis& a, const angle_axis& b);
/// Non-equality test
bool operator != (const angle_axis& a, const angle_axis& b);

/// Output inserter
std::ostream& operator<<(std::ostream& Stream, const angle_axis& Arg);
/// Input extractor
std::istream& operator>>(std::istream& Stream, angle_axis& Arg);

#define SDPEULERANGLEORDER(initialaxis, parity, repetition, frame) ((((((initialaxis << 1) + parity) << 1) + repetition) << 1) + frame)

/// Encapsulates a set of Euler angles, including the order in which they should be applied
00357 class euler_angles
{
public:

      /// Enumerates what gets rotated - the axes themselves, or the frame they represent
00362       typedef enum
      {
            StaticFrame = 0,
            RotatingFrame = 1,
      } OrderFrame;

      /// Enumerates whether the last axis will be the same as the first
00369       typedef enum
      {
            NoRepetition = 0,
            Repeats = 1,
      } OrderRepetition;

      /// Enumerates which axes will become the second axis
00376       typedef enum
      {
            EvenParity = 0,
            OddParity = 1,
      } OrderParity;

      /// Enumerates all possible axis order permutations
00383       typedef enum
      {
            XYZstatic = SDPEULERANGLEORDER(0, EvenParity, NoRepetition, StaticFrame),
            XYXstatic = SDPEULERANGLEORDER(0, EvenParity, Repeats, StaticFrame),
            XZYstatic = SDPEULERANGLEORDER(0, OddParity, NoRepetition, StaticFrame),
            XZXstatic = SDPEULERANGLEORDER(0, OddParity, Repeats, StaticFrame),
            YZXstatic = SDPEULERANGLEORDER(1, EvenParity, NoRepetition, StaticFrame),
            YZYstatic = SDPEULERANGLEORDER(1, EvenParity, Repeats, StaticFrame),
            YXZstatic = SDPEULERANGLEORDER(1, OddParity, NoRepetition, StaticFrame),
            YXYstatic = SDPEULERANGLEORDER(1, OddParity, Repeats, StaticFrame),
            ZXYstatic = SDPEULERANGLEORDER(2, EvenParity, NoRepetition, StaticFrame),
            ZXZstatic = SDPEULERANGLEORDER(2, EvenParity, Repeats, StaticFrame),
            ZYXstatic = SDPEULERANGLEORDER(2, OddParity, NoRepetition, StaticFrame),
            ZYZstatic = SDPEULERANGLEORDER(2, OddParity, Repeats, StaticFrame),
            ZYXrotating = SDPEULERANGLEORDER(0, EvenParity, NoRepetition, RotatingFrame),
            XYXrotating = SDPEULERANGLEORDER(0, EvenParity, Repeats, RotatingFrame),
            YZXrotating = SDPEULERANGLEORDER(0, OddParity, NoRepetition, RotatingFrame),
            XZXrotating = SDPEULERANGLEORDER(0, OddParity, Repeats, RotatingFrame),
            XZYrotating = SDPEULERANGLEORDER(1, EvenParity, NoRepetition, RotatingFrame),
            YZYrotating = SDPEULERANGLEORDER(1, EvenParity, Repeats, RotatingFrame),
            ZXYrotating = SDPEULERANGLEORDER(1, OddParity, NoRepetition, RotatingFrame),
            YXYrotating = SDPEULERANGLEORDER(1, OddParity, Repeats, RotatingFrame),
            YXZrotating = SDPEULERANGLEORDER(2, EvenParity, NoRepetition, RotatingFrame),
            ZXZrotating = SDPEULERANGLEORDER(2, EvenParity, Repeats, RotatingFrame),
            XYZrotating = SDPEULERANGLEORDER(2, OddParity, NoRepetition, RotatingFrame),
            ZYZrotating = SDPEULERANGLEORDER(2, OddParity, Repeats, RotatingFrame),
      } AngleOrder;

      /// Stores the three axis angles
00412       double n[3];
      /// Stores the axis order in packed format
00414       AngleOrder order;

      euler_angles();
      euler_angles(vector3 Angles, AngleOrder Order);
      euler_angles(double X, double Y, double Z, AngleOrder Order);
      /// Conversion from a quaternion
      euler_angles(const quaternion& Quaternion, AngleOrder Order);
      /// Conversion from a 4x4 matrix
      euler_angles(const matrix4& Matrix, AngleOrder Order);

      /// Returns the frame type from a packed order representation
00425       static OrderFrame Frame(AngleOrder& Order) { return OrderFrame(Order & 1); }
      /// Returns the repetition type from a packed order representation
00427       static OrderRepetition Repetition(AngleOrder& Order) { return OrderRepetition((Order >> 1) & 1); }
      /// Returns the parity type from a packed order representation
00429       static OrderParity Parity(AngleOrder& Order) { return OrderParity((Order >> 2) & 1); }
      /// Returns the axes in order, from a packed order representation
      static void Axes(AngleOrder& Order, int& i, int& j, int& k);

      /// Returns the frame type of this instance
00434       OrderFrame Frame() { return Frame(order); }
      /// Returns the repetition type of this instance
00436       OrderRepetition Repetition() { return Repetition(order); }
      /// Returns the parity type of this instance
00438       OrderParity Parity() { return Parity(order); }
      /// Returns the axes in order, for this instance
00440       void Axes(int& i, int& j, int& k) { Axes(order, i, j, k); }

      /// Returns the given angle by reference
      double& operator [] ( int i);
      /// Returns the given angle by value
      double operator[] (int i) const;
};

/// Output inserter
std::ostream& operator<<(std::ostream& Stream, const euler_angles& Arg);
/// Input extractor
std::istream& operator>>(std::istream& Stream, euler_angles& Arg);

/////////////////////////////////////////////////////////////////////////////
// 2D and 3D functions

/// Returns a two-dimensional identity matrix
matrix3 identity2D();
/// Returns a three-dimensional identity matrix
matrix4 identity3D();
/// Returns a two-dimensional translation matrix
matrix3 translation2D(const vector2& v);
/// Returns a three-dimensional translation matrix
matrix4 translation3D(const vector3& v);
/// Returns a two-dimensional rotation matrix
matrix3 rotation2D(const vector2& Center, const double Angle);
/// Returns a three-dimensional rotation matrix about an arbitrary axis
matrix4 rotation3D(const double Angle, vector3 Axis);
/// Returns a three-dimensional rotation matrix about an arbitrary axis
matrix4 rotation3D(const angle_axis& AngleAxis);
/// Returns a three-dimensional rotation matrix based on a set of Euler angles
matrix4 rotation3D(const vector3& EulerAngles);
/// Returns a three-dimensional rotation matrix based on a quaternion
matrix4 rotation3D(const quaternion& Quaternion);
/// Returns a quaternion based on a three-dimensional rotation matrix
quaternion rotation3D(const matrix4& m);
/// Converts a 3x3 rotation matrix into 4x4
matrix4 rotation3D(const matrix3& m);
/// Returns a two-dimensional scaling matrix
matrix3 scaling2D(const vector2& scaleVector);
/// Returns a three-dimensional scaling matrix
matrix4 scaling3D(const vector3& scaleVector);
/// Returns a three-dimensional perspective matrix
matrix4 perspective3D(const double d);
/// Extracts rotation from a three-dimensional matrix
matrix3 extractRotation(const matrix4& pose);
/// Extracts translation from a three-dimensional matrix
vector3 extractTranslation(const matrix4& pose);
/// Extracts scaling from a three-dimensional matrix
vector3 extractScaling(const matrix4& pose);

/////////////////////////////////////////////////////////////////////////////
// vector2 implementation

00494 inline vector2 operator * (const matrix3& a, const vector2& v) {
      vector3 av;

      av.n[VX] = a.v[0].n[VX]*v.n[VX] + a.v[0].n[VY]*v.n[VY] + a.v[0].n[VZ];
      av.n[VY] = a.v[1].n[VX]*v.n[VX] + a.v[1].n[VY]*v.n[VY] + a.v[1].n[VZ];
      av.n[VZ] = a.v[2].n[VX]*v.n[VX] + a.v[2].n[VY]*v.n[VY] + a.v[2].n[VZ];
      return av;
}

00503 inline vector2 operator * (const vector2& v, const matrix3& a)
{ return a.Transpose() * v; }

/////////////////////////////////////////////////////////////////////////////
// vector3 implementation

00509 inline vector3 operator * (const matrix4& a, const vector3& v)
{ return a * vector4(v); }

00512 inline vector3 operator * (const vector3& v, const matrix4& a)
{ return a.Transpose() * v; }

/////////////////////////////////////////////////////////////////////////////
// vector4 implementation

00518 inline vector4 operator * (const matrix4& a, const vector4& v)
{
#define ROWCOL(i) a.v[i].n[0]*v.n[VX] + a.v[i].n[1]*v.n[VY] + a.v[i].n[2]*v.n[VZ] + a.v[i].n[3]*v.n[VW]
      return vector4(ROWCOL(0), ROWCOL(1), ROWCOL(2), ROWCOL(3));
#undef ROWCOL // (i)
}

00525 inline vector4 operator * (const vector4& v, const matrix4& a)
{ return a.Transpose() * v; }

/////////////////////////////////////////////////////////////////////////////
// matrix3 implementation

inline matrix3::matrix3() {}

00533 inline matrix3::matrix3(const vector3& v0, const vector3& v1, const vector3& v2)
{ v[0] = v0; v[1] = v1; v[2] = v2; }

00536 inline matrix3::matrix3(const double d)
{ v[0] = v[1] = v[2] = vector3(d); }

00539 inline matrix3::matrix3(const matrix3& m)
{ v[0] = m.v[0]; v[1] = m.v[1]; v[2] = m.v[2]; }

00542 inline matrix3& matrix3::operator = ( const matrix3& m )
{ v[0] = m.v[0]; v[1] = m.v[1]; v[2] = m.v[2]; return *this; }

00545 inline matrix3& matrix3::operator += ( const matrix3& m )
{ v[0] += m.v[0]; v[1] += m.v[1]; v[2] += m.v[2]; return *this; }

00548 inline matrix3& matrix3::operator -= ( const matrix3& m )
{ v[0] -= m.v[0]; v[1] -= m.v[1]; v[2] -= m.v[2]; return *this; }

00551 inline matrix3& matrix3::operator *= ( const double d )
{ v[0] *= d; v[1] *= d; v[2] *= d; return *this; }

00554 inline matrix3& matrix3::operator /= ( const double d )
{ v[0] /= d; v[1] /= d; v[2] /= d; return *this; }

00557 inline vector3& matrix3::operator [] ( int i) {
      assert(! (i < VX || i > VZ));
      return v[i];
}

00562 inline const vector3& matrix3::operator [] ( int i) const {
      assert(!(i < VX || i > VZ));
      return v[i];
}

00567 inline matrix3 matrix3::Transpose() const {
      return matrix3(vector3(v[0][0], v[1][0], v[2][0]),
            vector3(v[0][1], v[1][1], v[2][1]),
            vector3(v[0][2], v[1][2], v[2][2]));
}

00573 inline matrix3 matrix3::Inverse()   const // Gauss-Jordan elimination with partial pivoting
{
      matrix3 a(*this),  // As a evolves from original mat into identity
      b(identity2D()); // b evolves from identity into Inverse(a)

      // Loop over cols of a from left to right, eliminating above and below diag
      for (int j=0; j<3; ++j) { // Find largest pivot in column j among rows j..2
            int i1 = j;        // Row with largest pivot candidate
            for (int i=j+1; i<3; ++i) {
                  if (fabs(a.v[i].n[j]) > fabs(a.v[i1].n[j]))
                        i1 = i;
            }

            // Swap rows i1 and j in a and b to put pivot on diagonal
            std::swap(a.v[i1], a.v[j]);
            std::swap(b.v[i1], b.v[j]);

            // Scale row j to have a unit diagonal
            if(!a.v[j].n[j])
                  {
                        // Singular matrix - can't invert
                        std::cerr << __PRETTY_FUNCTION__ << " : singular matrix, can't invert!" << std::endl;
                        return b;
                  }

            b.v[j] /= a.v[j].n[j];
            a.v[j] /= a.v[j].n[j];

            // Eliminate off-diagonal elems in col j of a, doing identical ops to b
            for (int i=0; i<3; ++i) {
                  if (i!=j) {
                        b.v[i] -= a.v[i].n[j]*b.v[j];
                        a.v[i] -= a.v[i].n[j]*a.v[j];
                  }
            }
      }
      return b;
}

00612 inline void matrix3::Decompose(double& x, double& y, double& z)   const
{
      y = atan2(-v[2][0], sqrt(v[0][0]*v[0][0]) + v[1][0]*v[1][0]);
      double t = cos(y);
      if(t != 0.0) {
            t = 1.0/t;
            z = atan2(v[1][0]*t, v[0][0]*t);
            x = atan2(v[2][1]*t, v[2][2]*t);
      } else {
            z = atan2(v[1][0], v[0][0]);
            x = atan2(v[2][1], v[2][2]);
      }
}

00626 inline matrix3 operator - (const matrix3& a)
{ return matrix3(-a.v[0], -a.v[1], -a.v[2]); }

00629 inline matrix3 operator + (const matrix3& a, const matrix3& b)
{ return matrix3(a.v[0] + b.v[0], a.v[1] + b.v[1], a.v[2] + b.v[2]); }

00632 inline matrix3 operator - (const matrix3& a, const matrix3& b)
{ return matrix3(a.v[0] - b.v[0], a.v[1] - b.v[1], a.v[2] - b.v[2]); }

00635 inline matrix3 operator * (const matrix3& a, const matrix3& b)
{
#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]
      return matrix3(vector3(ROWCOL(0,0), ROWCOL(0,1), ROWCOL(0,2)),
                              vector3(ROWCOL(1,0), ROWCOL(1,1), ROWCOL(1,2)),
                              vector3(ROWCOL(2,0), ROWCOL(2,1), ROWCOL(2,2)));
#undef ROWCOL // (i, j)
}

00644 inline matrix3 operator * (const matrix3& a, const double d)
{ return matrix3(a.v[0] * d, a.v[1] * d, a.v[2] * d); }

00647 inline matrix3 operator * (const double d, const matrix3& a)
{ return a*d; }

00650 inline matrix3 operator / (const matrix3& a, const double d)
{ return matrix3(a.v[0] / d, a.v[1] / d, a.v[2] / d); }

00653 inline bool operator == (const matrix3& a, const matrix3& b)
{ return (a.v[0] == b.v[0]) && (a.v[1] == b.v[1]) && (a.v[2] == b.v[2]); }

00656 inline bool operator != (const matrix3& a, const matrix3& b)
{ return !(a == b); }

00659 inline std::ostream& operator<<(std::ostream& Stream, const matrix3& Arg)
{
      Stream << Arg[0] << " " << Arg[1] << " " << Arg[2];
      return Stream;
}

00665 inline std::istream& operator>>(std::istream& Stream, matrix3& Arg)
{
      Stream >> Arg[0] >> Arg[1] >> Arg[2];
      return Stream;
}

/////////////////////////////////////////////////////////////////////////////
// matrix4 implementation

inline matrix4::matrix4() {}

inline matrix4::matrix4(const vector4& v0, const vector4& v1, const vector4& v2, const vector4& v3)
{ v[0] = v0; v[1] = v1; v[2] = v2; v[3] = v3; }

inline matrix4::matrix4(const double d)
{ v[0] = v[1] = v[2] = v[3] = vector4(d); }

00682 inline matrix4::matrix4(const matrix4& m)
{ v[0] = m.v[0]; v[1] = m.v[1]; v[2] = m.v[2]; v[3] = m.v[3]; }

inline matrix4::matrix4(euler_angles Angles)
{
      const euler_angles::OrderFrame frame = Angles.Frame();
      const euler_angles::OrderParity parity = Angles.Parity();
      const euler_angles::OrderRepetition repetition = Angles.Repetition();
      int i, j, k;
      Angles.Axes(i, j, k);

      if(frame == euler_angles::RotatingFrame)
            std::swap(Angles.n[0], Angles.n[2]);

      if(parity == euler_angles::OddParity)
      {
            Angles.n[0] = -Angles.n[0];
            Angles.n[1] = -Angles.n[1];
            Angles.n[2] = -Angles.n[2];
      }

      const double ti = Angles.n[0], tj = Angles.n[1], th = Angles.n[2];
      const double ci = cos(ti), cj = cos(tj), ch = cos(th);
      const double si = sin(ti), sj = sin(tj), sh = sin(th);
      const double cc = ci*ch, cs = ci*sh;
      const double sc = si*ch, ss = si*sh;

      matrix3 m;
      if(repetition == euler_angles::Repeats)
      {
            m[i][i] =  cj;          m[i][j] =  sj*si; m[i][k] =  sj*ci;
            m[j][i] =  sj*sh; m[j][j] = -cj*ss+cc;    m[j][k] = -cj*cs-sc;
            m[k][i] = -sj*ch; m[k][j] =  cj*sc+cs;    m[k][k] =  cj*cc-ss;
      }
      else
      {
            m[i][i] =  cj*ch; m[i][j] = sj*sc-cs;     m[i][k] = sj*cc+ss;
            m[j][i] =  cj*sh; m[j][j] = sj*ss+cc;     m[j][k] = sj*cs-sc;
            m[k][i] = -sj;          m[k][j] = cj*si;  m[k][k] = cj*ci;
      }

      *this = rotation3D(m);
}

00726 inline matrix4& matrix4::operator = ( const matrix4& m )
{ v[0] = m.v[0]; v[1] = m.v[1]; v[2] = m.v[2]; v[3] = m.v[3];
return *this; }

00730 inline matrix4& matrix4::operator = (const double d[4][4])
{ v[0] = d[0]; v[1] = d[1]; v[2] = d[2]; v[3] = d[3]; return *this; }

00733 inline matrix4& matrix4::operator = (const double d[16])
{ memcpy(&v[0][0], &d[0], 16 * sizeof(double)); return *this; }

00736 inline matrix4& matrix4::operator += ( const matrix4& m )
{ v[0] += m.v[0]; v[1] += m.v[1]; v[2] += m.v[2]; v[3] += m.v[3];
return *this; }

00740 inline matrix4& matrix4::operator -= ( const matrix4& m )
{ v[0] -= m.v[0]; v[1] -= m.v[1]; v[2] -= m.v[2]; v[3] -= m.v[3];
return *this; }

00744 inline matrix4& matrix4::operator *= ( const double d )
{ v[0] *= d; v[1] *= d; v[2] *= d; v[3] *= d; return *this; }

00747 inline matrix4& matrix4::operator /= ( const double d )
{ v[0] /= d; v[1] /= d; v[2] /= d; v[3] /= d; return *this; }

00750 inline vector4& matrix4::operator [] ( int i) {
      assert(! (i < VX || i > VW));
      return v[i];
}

00755 inline const vector4& matrix4::operator [] ( int i) const {
      assert(! (i < VX || i > VW));
      return v[i];
}

00760 inline matrix4 matrix4::Transpose() const{
      return matrix4(vector4(v[0][0], v[1][0], v[2][0], v[3][0]),
            vector4(v[0][1], v[1][1], v[2][1], v[3][1]),
            vector4(v[0][2], v[1][2], v[2][2], v[3][2]),
            vector4(v[0][3], v[1][3], v[2][3], v[3][3]));
}

00767 inline matrix4 matrix4::Inverse()   const // Gauss-Jordan elimination with partial pivoting
{
      matrix4 a(*this),  // As a evolves from original mat into identity
      b(identity3D()); // b evolves from identity into Inverse(a)

      // Loop over cols of a from left to right, eliminating above and below diag
      for (int j=0; j<4; ++j) { // Find largest pivot in column j among rows j..3
            int i1 = j;        // Row with largest pivot candidate

            for (int i=j+1; i<4; ++i) {
                  if (fabs(a.v[i].n[j]) > fabs(a.v[i1].n[j]))
                        i1 = i;
            }

            // Swap rows i1 and j in a and b to put pivot on diagonal
            std::swap(a.v[i1], a.v[j]);
            std::swap(b.v[i1], b.v[j]);

            // Scale row j to have a unit diagonal
            if(!a.v[j].n[j])
                  {
                        // Singular matrix - can't invert
                        std::cerr << __PRETTY_FUNCTION__ << " : singular matrix, can't invert!" << std::endl;
                        return b;
                  }

            b.v[j] /= a.v[j].n[j];
            a.v[j] /= a.v[j].n[j];

            // Eliminate off-diagonal elems in col j of a, doing identical ops to b
            for (int i=0; i<4; ++i) {
                  if (i!=j) {
                        b.v[i] -= a.v[i].n[j]*b.v[j];
                        a.v[i] -= a.v[i].n[j]*a.v[j];
                  }
            }
      }
      return b;
}

00807 inline void matrix4::CopyArray(float m[16]) const
{
      unsigned long index = 0;
      for(unsigned long i = 0; i < 4; ++i)
            for(unsigned long j = 0; j < 4; ++j)
                  m[index++] = float(v[i][j]);
}

00815 inline void matrix4::CopyArray(double m[16]) const
{
      unsigned long index = 0;
      for(unsigned long i = 0; i < 4; ++i)
            for(unsigned long j = 0; j < 4; ++j)
                  m[index++] = v[i][j];
}

00823 inline matrix4 operator - (const matrix4& a)
{ return matrix4(-a.v[0], -a.v[1], -a.v[2], -a.v[3]); }

00826 inline matrix4 operator + (const matrix4& a, const matrix4& b)
{ 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]); }

00829 inline matrix4 operator - (const matrix4& a, const matrix4& b)
{ 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]); }

00832 inline matrix4 operator * (const matrix4& a, const matrix4& b)
{
#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]
      return matrix4(vector4(ROWCOL(0,0), ROWCOL(0,1), ROWCOL(0,2), ROWCOL(0,3)),
                                                      vector4(ROWCOL(1,0), ROWCOL(1,1), ROWCOL(1,2), ROWCOL(1,3)),
                                                      vector4(ROWCOL(2,0), ROWCOL(2,1), ROWCOL(2,2), ROWCOL(2,3)),
                                                      vector4(ROWCOL(3,0), ROWCOL(3,1), ROWCOL(3,2), ROWCOL(3,3)));
}

00841 inline matrix4 operator * (const matrix4& a, const double d)
{ return matrix4(a.v[0] * d, a.v[1] * d, a.v[2] * d, a.v[3] * d); }

00844 inline matrix4 operator * (const double d, const matrix4& a)
{ return a*d; }

00847 inline matrix4 operator / (const matrix4& a, const double d)
{ return matrix4(a.v[0] / d, a.v[1] / d, a.v[2] / d, a.v[3] / d); }

00850 inline bool operator == (const matrix4& a, const matrix4& b)
{ 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])); }

00853 inline bool operator != (const matrix4& a, const matrix4& b)
{ return !(a == b); }

00856 inline std::ostream& operator<<(std::ostream& Stream, const matrix4& Arg)
{
      Stream << Arg[0] << " " << Arg[1] << " " << Arg[2] << " " << Arg[3];
      return Stream;
}

00862 inline std::istream& operator>>(std::istream& Stream, matrix4& Arg)
{
      Stream >> Arg[0] >> Arg[1] >> Arg[2] >> Arg[3];
      return Stream;
}

/////////////////////////////////////////////////////////////////////////////
// quaternion implementation

inline quaternion::quaternion()
{ w = 1.0; v = vector3(0.0, 0.0, 0.0); }

inline quaternion::quaternion(const double u, const vector3& t)
{ w = u; v = t; }

inline quaternion::quaternion(const double u, const double x, const double y, const double z)
{ w = u; v = vector3(x, y, z); }

inline quaternion::quaternion(const angle_axis& AngleAxis)
{ w = cos(AngleAxis.angle * 0.5); vector3 axis = AngleAxis.axis; axis.Normalize(); v = sin(AngleAxis.angle * 0.5) * axis; }

inline quaternion::quaternion(euler_angles Angles)
{
      const euler_angles::OrderFrame frame = Angles.Frame();
      const euler_angles::OrderParity parity = Angles.Parity();
      const euler_angles::OrderRepetition repetition = Angles.Repetition();
      int i, j, k;
      Angles.Axes(i, j, k);

      if(frame == euler_angles::RotatingFrame)
            std::swap(Angles.n[0], Angles.n[2]);

      if(parity == euler_angles::OddParity)
            Angles.n[1] = -Angles.n[1];

      const double ti = Angles.n[0]*0.5, tj = Angles.n[1]*0.5, th = Angles.n[2]*0.5;
      const double ci = cos(ti), cj = cos(tj), ch = cos(th);
      const double si = sin(ti), sj = sin(tj), sh = sin(th);
      const double cc = ci*ch, cs = ci*sh;
      const double sc = si*ch, ss = si*sh;

      if(repetition == euler_angles::Repeats)
      {
            v[i] = cj*(cs + sc);
            v[j] = sj*(cc + ss);
            v[k] = sj*(cs - sc);
            w = cj*(cc - ss);
      }
      else
      {
            v[i] = cj*sc - sj*cs;
            v[j] = cj*ss + sj*cc;
            v[k] = cj*cs - sj*sc;
            w = cj*cc + sj*ss;
      }

      if(parity == euler_angles::OddParity)
            v[j] = -v[j];
}

00922 inline quaternion& quaternion::operator = (const quaternion& q)
{ w = q.w; v = q.v; return *this; }

00925 inline quaternion& quaternion::operator = (const angle_axis& AngleAxis)
{ w = cos(AngleAxis.angle * 0.5); vector3 axis = AngleAxis.axis; axis.Normalize(); v = sin(AngleAxis.angle * 0.5) * axis; return *this; }

00928 inline quaternion& quaternion::operator *= (const double f)
{ w *= f; v *= f; return *this; }

00931 inline quaternion& quaternion::operator /= (const double f)
{ if(f != 0.0) { w /= f; v /= f; } return *this; }

00934 inline quaternion& quaternion::operator += (const quaternion& q)
{ w += q.w; v += q.v; return *this; }

00937 inline quaternion& quaternion::operator -= (const quaternion& q)
{ w -= q.w; v -= q.v; return *this; }

00940 inline quaternion& quaternion::operator *= (const quaternion& q)
{ w = w*q.w - (v*q.v); v = q.w*v + w*q.v + (v^q.v); return *this; }

00943 inline quaternion& quaternion::operator /= (const quaternion& q)
{ quaternion t = q; (*this) *= t.Inverse(); return *this; }

00946 inline double quaternion::Magnitude() const
{ return std::sqrt(w*w + v.Length2()); }

00949 inline quaternion& quaternion::Normalize()
{
      if(const double magnitude = Magnitude())
            *this /= magnitude;

      return *this;
}

00957 inline quaternion& quaternion::Conjugate()
{ v = -v; return *this; }

00960 inline quaternion& quaternion::Inverse()
{ if(Magnitude() != 0.0) { const double i = 1.0 / Magnitude(); w *= i; v *= -i; } return *this; }

00963 inline quaternion& quaternion::Square()
{ const double t = 2*w; w = w*w - v.Length2(); v *= t; return *this; }

00966 inline quaternion& quaternion::AxisRotation(const double phi)
{ v.Normalize(); v *= sin(phi/2.0); w = cos(phi/2); return *this; }

inline quaternion operator - (const quaternion& q)
{ return quaternion(-q.w,-q.v); }

00972 inline quaternion operator + (const quaternion& q, const quaternion& r)
{ return quaternion(q.w + r.w, q.v + r.v); }

00975 inline quaternion operator - (const quaternion& q, const quaternion& r)
{ return quaternion(q.w - r.w, q.v - r.v); }

00978 inline quaternion operator * (const quaternion& q, const quaternion& r)
{ return quaternion(q.w*r.w - (q.v*r.v), q.w*r.v + r.w*q.v + (q.v^r.v)); }

00981 inline quaternion operator * (const quaternion& q, const double f)
{ return quaternion(q.w*f, q.v*f); }

00984 inline quaternion operator * (const double f, const quaternion& q)
{ return quaternion(q.w*f, q.v*f); }

00987 inline quaternion operator / (const quaternion& q, const quaternion& r)
{ quaternion t = r; return q*t.Inverse(); }

00990 inline quaternion operator / (const quaternion& q, const double f)
{ if( f == 0.0 ) return quaternion(); else return quaternion(q.w/f, q.v/f); }

00993 inline bool operator == (const quaternion& q, const quaternion& r)
{ return ((q.w == r.w) && (q.v == r.v)); }

00996 inline bool operator != (const quaternion& q, const quaternion& r)
{ return !(q == r); }

00999 inline std::ostream& operator<<(std::ostream& Stream, const quaternion& Arg)
{
      Stream << Arg.w << " " << Arg.v[0] << " " << Arg.v[1] << " " << Arg.v[2];
      return Stream;
}

01005 inline std::istream& operator>>(std::istream& Stream, quaternion& Arg)
{
      Stream >> Arg.w >> Arg.v[0] >> Arg.v[1] >> Arg.v[2];
      return Stream;
}

/////////////////////////////////////////////////////////////////////////////
// angle_axis implementation ...

inline angle_axis::angle_axis()
{ angle = 0.0; axis = vector3(0.0, 0.0, 1.0); }

inline angle_axis::angle_axis(const double Angle, const vector3& Axis)
{ angle = Angle; axis = Axis; axis.Normalize(); }

inline angle_axis::angle_axis(const double Angle, const double X, const double Y, const double Z)
{ angle = Angle; axis[0] = X; axis[1] = Y; axis[2] = Z; axis.Normalize(); }

inline angle_axis::angle_axis(const double AngleAxis[4])
{ angle = AngleAxis[0]; axis[0] = AngleAxis[1]; axis[1] = AngleAxis[2]; axis[2] = AngleAxis[3]; axis.Normalize(); }

inline angle_axis::angle_axis(const quaternion& Quaternion)
{
      quaternion q = Quaternion;
      q.Normalize();
      const double halftheta = acos(q.w);
      const double sinhalftheta = sin(halftheta);

      angle = 2.0 * halftheta;
      if(sinhalftheta != 0.0)
            axis = q.v / sinhalftheta;
      else
            axis = vector3(0.0, 0.0, 1.0);
}

01040 inline angle_axis::angle_axis(const angle_axis& AngleAxis)
{ angle = AngleAxis.angle; axis = AngleAxis.axis; }

01043 inline angle_axis& angle_axis::operator = (const angle_axis& AngleAxis)
{ angle = AngleAxis.angle; axis = AngleAxis.axis; return *this; }

01046 inline angle_axis& angle_axis::operator = (const quaternion& Quaternion)
{
      const double halftheta = acos(Quaternion.w);
      const double sinhalftheta = sin(halftheta);

      angle = 2.0 * halftheta;
      if(sinhalftheta != 0.0)
            axis = Quaternion.v / sinhalftheta;
      else
            axis = vector3(0.0, 0.0, 1.0);

      return *this;
}

01060 inline angle_axis& angle_axis::operator = (const double AngleAxis[4])
{ angle = AngleAxis[0]; axis[0] = AngleAxis[1]; axis[1] = AngleAxis[2]; axis[2] = AngleAxis[3]; return *this; }

01063 inline void angle_axis::CopyArray(float AngleAxis[4]) const
{     AngleAxis[0] = float(angle); AngleAxis[1] = float(axis[0]); AngleAxis[2] = float(axis[1]); AngleAxis[3] = float(axis[2]); }

01066 inline void angle_axis::CopyArray(double AngleAxis[4]) const
{     AngleAxis[0] = angle; AngleAxis[1] = axis[0]; AngleAxis[2] = axis[1]; AngleAxis[3] = axis[2]; }

01069 inline bool operator == (const angle_axis& a, const angle_axis& b)
{     return ((a.angle == b.angle) && (a.axis == b.axis)); }
01071 inline bool operator != (const angle_axis& a, const angle_axis& b)
{     return !(a == b); }

01074 inline std::ostream& operator<<(std::ostream& Stream, const angle_axis& Arg)
{
      Stream << Arg.angle << " " << Arg.axis[0] << " " << Arg.axis[1] << " " << Arg.axis[2];
      return Stream;
}

01080 inline std::istream& operator>>(std::istream& Stream, angle_axis& Arg)
{
      Stream >> Arg.angle >> Arg.axis[0] >> Arg.axis[1] >> Arg.axis[2];
      return Stream;
}


/////////////////////////////////////////////////////////////////////////////
// 2D and 3D function implementations

01090 inline matrix3 identity2D()
{
      return matrix3(vector3(1.0, 0.0, 0.0), vector3(0.0, 1.0, 0.0), vector3(0.0, 0.0, 1.0));
}

01095 inline matrix3 translation2D(const vector2& v)
{
      return matrix3(vector3(1.0, 0.0, v[VX]), vector3(0.0, 1.0, v[VY]), vector3(0.0, 0.0, 1.0));
}

01100 inline matrix3 rotation2D(const vector2& Center, const double Angle)
{
      const double c = cos(Angle);
      const double s = sin(Angle);

      return matrix3(vector3(c, -s, Center[VX] * (1.0-c) + Center[VY] * s),
            vector3(s, c, Center[VY] * (1.0-c) - Center[VX] * s),
            vector3(0.0, 0.0, 1.0));
}

01110 inline matrix3 scaling2D(const vector2& scaleVector)
{
      return matrix3(vector3(scaleVector[VX], 0.0, 0.0), vector3(0.0, scaleVector[VY], 0.0), vector3(0.0, 0.0, 1.0));
}

01115 inline matrix4 identity3D()
{
      return matrix4(vector4(1.0, 0.0, 0.0, 0.0),
            vector4(0.0, 1.0, 0.0, 0.0),
            vector4(0.0, 0.0, 1.0, 0.0),
            vector4(0.0, 0.0, 0.0, 1.0));
}

01123 inline matrix4 translation3D(const vector3& v)
{
      return matrix4(vector4(1.0, 0.0, 0.0, v[VX]),
            vector4(0.0, 1.0, 0.0, v[VY]),
            vector4(0.0, 0.0, 1.0, v[VZ]),
            vector4(0.0, 0.0, 0.0, 1.0));
}

inline matrix4 shearing3D(const double XY, const double XZ, const double YX, const double YZ, const double ZX, const double ZY)
{
      return matrix4(
            vector4(1, XY, XZ, 0),
            vector4(YX, 1, YZ, 0),
            vector4(ZX, ZY, 1, 0),
            vector4(0, 0, 0, 1));
}

01140 inline matrix4 rotation3D(const double Angle, vector3 Axis)
{
      const double c = cos(Angle), s = sin(Angle), t = 1.0 - c;

      Axis.Normalize();
      return matrix4(vector4(t * Axis[VX] * Axis[VX] + c, t * Axis[VX] * Axis[VY] - s * Axis[VZ], t * Axis[VX] * Axis[VZ] + s * Axis[VY], 0.0),
            vector4(t * Axis[VX] * Axis[VY] + s * Axis[VZ], t * Axis[VY] * Axis[VY] + c, t * Axis[VY] * Axis[VZ] - s * Axis[VX], 0.0),
            vector4(t * Axis[VX] * Axis[VZ] - s * Axis[VY], t * Axis[VY] * Axis[VZ] + s * Axis[VX], t * Axis[VZ] * Axis[VZ] + c, 0.0),
            vector4(0.0, 0.0, 0.0, 1.0));
}

01151 inline matrix4 rotation3D(const angle_axis& AngleAxis)
{
      return rotation3D(AngleAxis.angle, AngleAxis.axis);
}

01156 inline matrix4 rotation3D(const vector3& EulerAngles)
{
      matrix4 matrix = identity3D();
      matrix = matrix * rotation3D(EulerAngles[1], vector3(0.0, 1.0, 0.0));
      matrix = matrix * rotation3D(EulerAngles[0], vector3(1.0, 0.0, 0.0));
      matrix = matrix * rotation3D(EulerAngles[2], vector3(0.0, 0.0, 1.0));

      return matrix;
}

01166 inline matrix4 rotation3D(const quaternion& Quaternion)
{
      return rotation3D(angle_axis(Quaternion));
}

01171 inline quaternion rotation3D(const matrix4& m)
{
      double d0 = m[0][0];
      double d1 = m[1][1];
      double d2 = m[2][2];

      double trace = d0 + d1 + d2 + 1;
      if(trace > 0.0)
            {
                  double s = 0.5 / std::sqrt(trace);
                  return quaternion(0.25 / s, s * vector3(m[2][1] - m[1][2], m[0][2] - m[2][0], m[1][0] - m[0][1]));
            }

      // Identify the major diagonal element with greatest value
      if(d0 >= d1 && d0 >= d2)
            {
                  double s = std::sqrt(1.0 + d0 - d1 - d2) * 2.0;
                  return quaternion(m[1][2] - m[2][1], vector3(0.5, m[0][1] - m[1][0], m[0][2] - m[2][0])) / s;
            }
      else if(d1 >= d0 && d1 >= d2)
            {
                  double s = std::sqrt(1.0 + d1 - d0 - d2) * 2.0;
                  return quaternion(m[0][2] - m[2][0], vector3(m[0][1] - m[1][0], 0.5, m[1][2] - m[2][1])) / s;
            }
      else
            {
                  double s = std::sqrt(1.0 + d2 - d0 - d1) * 2.0;
                  return quaternion(m[0][1] - m[1][0], vector3(m[0][2] - m[2][0], m[1][2] - m[2][1], 0.5)) / s;
            }
}

01202 inline matrix4 scaling3D(const vector3& scaleVector)
{
      return matrix4(vector4(scaleVector[VX], 0.0, 0.0, 0.0),
            vector4(0.0, scaleVector[VY], 0.0, 0.0),
            vector4(0.0, 0.0, scaleVector[VZ], 0.0),
            vector4(0.0, 0.0, 0.0, 1.0));
}

01210 inline matrix4 perspective3D(const double d)
{
      return matrix4(vector4(1.0, 0.0, 0.0, 0.0),
            vector4(0.0, 1.0, 0.0, 0.0),
            vector4(0.0, 0.0, 1.0, 0.0),
            vector4(0.0, 0.0, 1.0/d, 0.0));
}

01218 inline matrix4 rotation3D(const matrix3& m)
{
//    return(matrix4(vector4(m[0][0], m[0][1], m[0][2], 0.0),
//                                                    vector4(m[1][0], m[1][1], m[1][2], 0.0),
//                                                    vector4(m[2][0], m[2][1], m[2][2], 0.0),
//                                                    vector4(0.0, 0.0, 0.0, 1.0)));

      // This slightly-more-verbose version eliminates a strange parse error in egcs ...
      vector4 a(m[0][0], m[0][1], m[0][2], 0.0);
      vector4 b(m[1][0], m[1][1], m[1][2], 0.0);
      vector4 c(m[2][0], m[2][1], m[2][2], 0.0);
      vector4 d(0.0, 0.0, 0.0, 1.0);

      return matrix4(a, b, c, d);
}

//
//    Matrix utilities for affine matrices.
//
//    The input matrix must be affine, but need not be orthogonal.
//    In other words, it may contain scaling, rotation, and translation,
//    but not perspective projection.
//

//    Ref. Foley and Van Dam, 2nd ed, p. 217.
/// Extracts the translation vector from a matrix
01244 inline vector3 extractTranslation(const matrix4& m)
{
      return(vector3(m[0][3], m[1][3], m[2][3]));
}

//    Per Valerie Demers, Softimage.
//    Transposed for matrix4 matrices
/// Extracts the scaling vector from a matrix
01252 inline vector3 extractScaling(const matrix4& m)
{
      return(vector3(sqrt(m[0][0] * m[0][0] + m[1][0] * m[1][0] + m[2][0] * m[2][0]),
            sqrt(m[0][1] * m[0][1] + m[1][1] * m[1][1] + m[2][1] * m[2][1]),
            sqrt(m[0][2] * m[0][2] + m[1][2] * m[1][2] + m[2][2] * m[2][2])));
}

/// Extracts a rotation matrix from a matrix
01260 inline matrix3 extractRotation(const matrix4& m)
{
      // Get scaling ...
      vector3 scale(extractScaling(m));
      // Compute the inverse of scaling ...
      vector3 invscale(1.0 / scale[0], 1.0 / scale[1], 1.0 / scale[2]);
      // Apply Inverse of scaling as a transformation, to get unit scale ...
      matrix4 unscaled(m * scaling3D(invscale));

      // Piece-together the rotation matrix ...
      vector3 a(unscaled[0], VW);
      vector3 b(unscaled[1], VW);
      vector3 c(unscaled[2], VW);

      return matrix3(a, b, c);
}

//
//    Unit quaternion utilities.
//
//    Those special functions deal with rotations, thus require unit quaternions.
//

//    Euler/Matrix/Quaternion conversion functions are based on Ken Shoemake code,
//    from Graphic Gems IV

/// Returns the spherical linear interpolation between two quaternions
01287 inline quaternion Slerp(const quaternion& q1, const quaternion& q2, double t)
{
      // Calculate the angle between the two quaternions ...
      double cosangle = q1.w * q2.w + q1.v * q2.v;

      if(cosangle < -1.0)
            cosangle = -1.0;
      if(cosangle > 1.0)
            cosangle = 1.0;

      // If the angle is reasonably large, do the spherical interpolation
      if(1.0 - cosangle > 16 * FLT_EPSILON)
            {
                  const double angle = acos(cosangle);
                  return (sin((1.0 - t)*angle)*q1 + sin(t*angle)*q2) / sin(angle);
            }

      // The angle is too close to zero - do a linear interpolation
      return k3d::mix(q1, q2, t);
}

/////////////////////////////////////////////////////////////////////////////
// euler_angles implementation

inline euler_angles::euler_angles()
{
      n[0] = n[1] = n[2] = 0.0;
      order = YXZstatic;
}

inline euler_angles::euler_angles(vector3 Angles, AngleOrder Order)
{
      n[0] = Angles[0];
      n[1] = Angles[1];
      n[2] = Angles[2];
      order = Order;
}

inline euler_angles::euler_angles(double X, double Y, double Z, AngleOrder Order)
{
      n[0] = X;
      n[1] = Y;
      n[2] = Z;
      order = Order;
}

01333 inline euler_angles::euler_angles(const quaternion& Quaternion, AngleOrder Order)
{
      const double NQuaternion = Quaternion.Magnitude();
      const double s = (NQuaternion > 0.0) ? (2.0 / NQuaternion) : 0.0;

      const double xs = Quaternion.v[0]*s, ys = Quaternion.v[1]*s, zs = Quaternion.v[2]*s;
      const double wx = Quaternion.w*xs, wy = Quaternion.w*ys, wz = Quaternion.w*zs;
      const double xx = Quaternion.v[0]*xs, xy = Quaternion.v[0]*ys, xz = Quaternion.v[0]*zs;
      const double yy = Quaternion.v[1]*ys, yz = Quaternion.v[1]*zs, zz = Quaternion.v[2]*zs;

      matrix3 matrix;
      matrix[0][0] = 1.0 - (yy + zz);
      matrix[0][1] = xy - wz;
      matrix[0][2] = xz + wy;
      matrix[1][0] = xy + wz;
      matrix[1][1] = 1.0 - (xx + zz);
      matrix[1][2] = yz - wx;
      matrix[2][0] = xz - wy;
      matrix[2][1] = yz + wx;
      matrix[2][2] = 1.0 - (xx + yy);

      matrix4 rotmatrix = rotation3D(matrix);
      *this = euler_angles(rotmatrix, Order);
}

01358 inline euler_angles::euler_angles(const matrix4& Matrix, AngleOrder Order)
{
      OrderRepetition repetition = Repetition(Order);
      OrderParity parity = Parity(Order);
      OrderFrame frame = Frame(Order);
      int i, j, k;
      Axes(Order, i, j, k);


      if(repetition == euler_angles::Repeats )
      {
            const double sy = sqrt(Matrix[i][j]*Matrix[i][j] + Matrix[i][k]*Matrix[i][k]);
            if( sy > 16*FLT_EPSILON )
            {
                  n[0] = atan2(Matrix[i][j], Matrix[i][k]);
                  n[1] = atan2(sy, Matrix[i][i]);
                  n[2] = atan2(Matrix[j][i], -Matrix[k][i]);
            }
            else
            {
                  n[0] = atan2(-Matrix[j][k], Matrix[j][j]);
                  n[1] = atan2(sy, Matrix[i][i]);
                  n[2] = 0;
            }
      }
      else
      {
            const double cy = sqrt(Matrix[i][i]*Matrix[i][i] + Matrix[j][i]*Matrix[j][i]);
            if( cy > 16*FLT_EPSILON )
            {
                  n[0] = atan2(Matrix[k][j], Matrix[k][k]);
                  n[1] = atan2(-Matrix[k][i], cy);
                  n[2] = atan2(Matrix[j][i], Matrix[i][i]);
            }
            else
            {
                  n[0] = atan2(-Matrix[j][k], Matrix[j][j]);
                  n[1] = atan2(-Matrix[k][i], cy);
                  n[2] = 0;
            }
      }

      if(parity == euler_angles::OddParity )
      {
            n[0] = -n[0];
            n[1] = -n[1];
            n[2] = -n[2];
      }

      if(frame == euler_angles::RotatingFrame )
            std::swap(n[0], n[2]);

      order = Order;
}

01413 inline void euler_angles::Axes(AngleOrder& Order, int& i, int& j, int& k)
{
      const int safe[4] = {0, 1, 2, 0};
      const int next[4] = {1, 2, 0, 1};

      i = safe[(Order >> 3) & 3];
      j = next[i + Parity(Order)];
      k = next[i + 1 - Parity(Order)];
}

01423 inline double& euler_angles::operator [] ( int i)
{
      assert(! (i < 0 || i > 2));
      return n[i];
}

01429 inline double euler_angles::operator [] ( int i) const
{
      assert(! (i < 0 || i > 2));
      return n[i];
}

01435 inline std::ostream& operator<<(std::ostream& Stream, const euler_angles& Arg)
{
      Stream << Arg.n[0] << " " << Arg.n[1] << " " << Arg.n[2] << " " << int(Arg.order);
      return Stream;
}

01441 inline std::istream& operator>>(std::istream& Stream, euler_angles& Arg)
{
      int order;
      Stream >> Arg.n[0] >> Arg.n[1] >> Arg.n[2] >> order;

      Arg.order = euler_angles::AngleOrder(order);

      return Stream;
}

} // namespace k3d

#endif // K3DSDK_ALGEBRA_H



Generated by  Doxygen 1.6.0   Back to index