text: /*
   * Gray: A Ray Tracing-based Monte Carlo Simulator for PET
   *
   * Copyright (c) 2018, David Freese, Peter Olcott, Sam Buss, Craig Levin
   *
   * This software is distributed under the terms of the MIT License unless
   * otherwise noted.  See LICENSE for further details.
   *
   */
  
  //
  // Linear Algebra Classes over R3
  //
  //
  // A. Vector and Position classes
  //
  //    A.1. VectorR3: a real column vector of length 3.
  //
  //	  A.2. VectorHgR3: a column vector of length 4 which is
  //			the homogenous representation of a vector in 3-space
  //
  // B. Matrix Classes
  //
  //	  B.1 LinearMapR3 - arbitrary linear map; 3x3 real matrix
  //
  //	  B.2 AffineMapR3 - arbitrary affine map; a 3x4 real matrix
  //
  //	  B.3 RotationMapR3 - orthonormal 3x3 matrix
  //
  //	  B.4 RigidMapR3 - RotationMapR3 plus displacement
  //
  
  #ifndef LINEAR_R3_H
  #define LINEAR_R3_H
  
  #include <cmath>
  #include <cassert>
  #include <iostream>
  #include "Gray/VrMath/MathMisc.h"
  
  class VectorR3;				// Space Vector (length 3)
  class VectorHgR3;			// Homogenous Space Vector
  class VectorR4;				// Space Vector (length 4)
  
  class LinearMapR3;			// Linear Map (3x3 Matrix)
  class AffineMapR3;			// Affine Map (3x4 Matrix)
  class RotationMapR3;		// Rotation (3x3 orthonormal matrix)
  class RigidMapR3;			// 3x4 matrix, first 3 columns orthonormal
  
  // Most for internal use:
  class Matrix3x3;
  class Matrix3x4;
  
  class Quaternion;
  
  // **************************************
  // VectorR3 class                       *
  // * * * * * * * * * * * * * * * * * * **
  
  class VectorR3
  {
  
  public:
      double x, y, z;		// The x & y & z coordinates.
  
      static const VectorR3 Zero;
      // Deprecated due to unsafeness of global initialization
      //static const VectorR3 UnitX;
      //static const VectorR3 UnitY;
      //static const VectorR3 UnitZ;
      //static const VectorR3 NegUnitX;
      //static const VectorR3 NegUnitY;
      //static const VectorR3 NegUnitZ;
  
  public:
      VectorR3( ) : x(0.0), y(0.0), z(0.0) {}
      VectorR3( double xVal, double yVal, double zVal )
          : x(xVal), y(yVal), z(zVal) {}
      VectorR3( const VectorHgR3& uH );
  
      VectorR3& Set( const Quaternion& );	// Convert quat to rotation vector
      VectorR3& Set( double xx, double yy, double zz )
      {
          x=xx;
          y=yy;
          z=zz;
          return *this;
      }
      VectorR3& SetFromHg( const VectorR4& );	// Convert homogeneous VectorR4 to VectorR3
      VectorR3& SetZero()
      {
          x=0.0;
          y=0.0;
          z=0.0;
          return *this;
      }
      VectorR3& SetUnitX()
      {
          x=1.0;
          y=0.0;
          z=0.0;
          return *this;
      }
      VectorR3& SetUnitY()
      {
          x=0.0;
          y=1.0;
          z=0.0;
          return *this;
      }
      VectorR3& SetUnitZ()
      {
          x=0.0;
          y=0.0;
          z=1.0;
          return *this;
      }
      VectorR3& SetNegUnitX()
      {
          x=-1.0;
          y=0.0;
          z=0.0;
          return *this;
      }
      VectorR3& SetNegUnitY()
      {
          x=0.0;
          y=-1.0;
          z=0.0;
          return *this;
      }
      VectorR3& SetNegUnitZ()
      {
          x=0.0;
          y=0.0;
          z=-1.0;
          return *this;
      }
      VectorR3& Load( const double* v );
      VectorR3& Load( const float* v );
      void Dump( double* v ) const;
      void Dump( float* v ) const;
  
      inline double operator[]( int i ) const;
  
      VectorR3& operator= ( const VectorR3& v )
      {
          x=v.x;
          y=v.y;
          z=v.z;
          return(*this);
      }
      VectorR3& operator+= ( const VectorR3& v )
      {
          x+=v.x;
          y+=v.y;
          z+=v.z;
          return(*this);
      }
      VectorR3& operator-= ( const VectorR3& v )
      {
          x-=v.x;
          y-=v.y;
          z-=v.z;
          return(*this);
      }
      VectorR3& operator*= ( double m )
      {
          x*=m;
          y*=m;
          z*=m;
          return(*this);
      }
      VectorR3& operator/= ( double m )
      {
          double mInv = 1.0/m;
          x*=mInv;
          y*=mInv;
          z*=mInv;
          return(*this);
      }
      VectorR3 operator- () const
      {
          return ( VectorR3(-x, -y, -z) );
      }
      VectorR3& operator*= (const VectorR3& v);	// Cross Product
      VectorR3& CrossProductLeft (const VectorR3& v);	// Cross Product on left
      VectorR3& ArrayProd(const VectorR3&);		// Component-wise product
  
      VectorR3& AddScaled( const VectorR3& u, double s );
      VectorR3& SubtractFrom( const VectorR3& u );
      VectorR3& AddCrossProduct( const VectorR3& u, const VectorR3& v );
  
      bool IsZero() const
      {
          return ( x==0.0 && y==0.0 && z==0.0 );
      }
      double Norm() const
      {
          return ( (double)sqrt( x*x + y*y + z*z ) );
      }
      double NormSq() const
      {
          return ( x*x + y*y + z*z );
      }
      double MaxAbs() const;
      double Dist( const VectorR3& u ) const;	// Distance from u
      double DistSq( const VectorR3& u ) const;	// Distance from u squared
      VectorR3& Negate()
      {
          x = -x;
          y = -y;
          z = -z;
          return *this;
      }
      VectorR3& Normalize ()
      {
          *this /= Norm();    // No error checking
          return *this;
      }
      inline VectorR3& MakeUnit();		// Normalize() with error checking
      inline VectorR3& ReNormalize();
      bool IsUnit( ) const
      {
          double norm = Norm();
          return ( 1.000001>=norm && norm>=0.999999 );
      }
      bool IsUnit( double tolerance ) const
      {
          double norm = Norm();
          return ( 1.0+tolerance>=norm && norm>=1.0-tolerance );
      }
      bool NearZero(double tolerance) const
      {
          return( MaxAbs()<=tolerance );
      }
      // tolerance should be non-negative
      inline bool operator==(const VectorR3& u) const
      {
          return (x==u.x && y==u.y && z==u.z);
      }
      inline bool operator!=(const VectorR3& u) const
      {
          return (x!=u.x || y!=u.y || z!=u.z);
      }
  
      double YaxisDistSq() const
      {
          return (x*x+z*z);
      }
      double YaxisDist() const
      {
          return sqrt(x*x+z*z);
      }
  
      VectorR3& Rotate( double theta, const VectorR3& u); // rotate around u.
      VectorR3& RotateUnitInDirection ( const VectorR3& dir);	// rotate in direction dir
      VectorR3& Rotate( const Quaternion& );	// Rotate according to quaternion
      // Defined in Quaternion.cpp
  
      friend std::ostream& operator<< ( std::ostream& os, const VectorR3& u );
  
  };
  
  // Specialize std::hash for VectorR3
  namespace std
  {
      template<> struct hash<VectorR3>
      {
          typedef VectorR3 argument_type;
          typedef std::size_t result_type;
          result_type operator()(argument_type const& s) const noexcept
          {
              result_type const h1 ( std::hash<double>{}(s.x) );
              result_type const h2 ( std::hash<double>{}(s.y) );
              result_type const h3 ( std::hash<double>{}(s.z) );
              return h1 ^ (h2 << 1) ^ (h3 << 2);
          }
      };
  }
  
  
  inline VectorR3 operator+( const VectorR3& u, const VectorR3& v );
  inline VectorR3 operator-( const VectorR3& u, const VectorR3& v );
  inline VectorR3 operator*( const VectorR3& u, double m);
  inline VectorR3 operator*( double m, const VectorR3& u);
  inline VectorR3 operator/( const VectorR3& u, double m);
  
  inline double operator^ (const VectorR3& u, const VectorR3& v ); // Dot Product
  inline double InnerProduct(const VectorR3& u, const VectorR3& v )
  {
      return (u^v);
  }
  inline VectorR3 operator* (const VectorR3& u, const VectorR3& v);	 // Cross Product
  inline VectorR3 ArrayProd ( const VectorR3& u, const VectorR3& v );
  
  inline double Mag(const VectorR3& u)
  {
      return u.Norm();
  }
  inline double Dist(const VectorR3& u, const VectorR3& v)
  {
      return u.Dist(v);
  }
  inline double DistSq(const VectorR3& u, const VectorR3& v)
  {
      return u.DistSq(v);
  }
  inline double NormalizeError (const VectorR3& u);
  
  inline VectorR3 ToVectorR3( const Quaternion& q )
  {
      return VectorR3().Set(q);
  }
  
  
  // ****************************************
  // VectorHgR3 class                       *
  // * * * * * * * * * * * * * * * * * * * **
  
  class VectorHgR3
  {
  
  public:
      double x, y, z, w;		// The x & y & z & w coordinates.
  
  public:
      VectorHgR3( ) : x(0.0), y(0.0), z(0.0), w(1.0) {}
      VectorHgR3( double xVal, double yVal, double zVal )
          : x(xVal), y(yVal), z(zVal), w(1.0) {}
      VectorHgR3( double xVal, double yVal, double zVal, double wVal )
          : x(xVal), y(yVal), z(zVal), w(wVal) {}
      VectorHgR3 ( const VectorR3& u ) : x(u.x), y(u.y), z(u.z), w(1.0) {}
  };
  
  //
  // Advanced vector and position functions (prototypes)
  //
  
  VectorR3 Interpolate( const VectorR3& start, const VectorR3& end, double a);
  
  // *****************************************
  // Matrix3x3 class                         *
  // * * * * * * * * * * * * * * * * * * * * *
  
  class Matrix3x3
  {
  public:
  
      double m11, m12, m13, m21, m22, m23, m31, m32, m33;
  
      // Implements a 3x3 matrix: m_i_j - row-i and column-j entry
  
      // Deprecated due to unsafeness of global initialization
      //static const Matrix3x3 Identity;
  
  public:
      inline Matrix3x3();
      inline Matrix3x3(const VectorR3&, const VectorR3&, const VectorR3&); // Sets by columns!
      inline Matrix3x3(double, double, double, double, double, double,
                       double, double, double );	// Sets by columns
  
      inline void SetIdentity ();		// Set to the identity map
      inline void Set ( const Matrix3x3& );	// Set to the matrix.
      inline void Set3x3 ( const Matrix3x4& );	// Set to the 3x3 part of the matrix.
      inline void Set( const VectorR3&, const VectorR3&, const VectorR3& );
      inline void Set( double, double, double,
                       double, double, double,
                       double, double, double );
      inline void SetByRows( double, double, double, double, double, double,
                             double, double, double );
      inline void SetByRows( const VectorR3&, const VectorR3&, const VectorR3& );
      inline void LoadByRows( const double* );
  
      inline void SetColumn1 ( double, double, double );
      inline void SetColumn2 ( double, double, double );
      inline void SetColumn3 ( double, double, double );
      inline void SetColumn1 ( const VectorR3& );
      inline void SetColumn2 ( const VectorR3& );
      inline void SetColumn3 ( const VectorR3& );
      inline VectorR3 Column1() const;
      inline VectorR3 Column2() const;
      inline VectorR3 Column3() const;
  
      inline void SetRow1 ( double, double, double );
      inline void SetRow2 ( double, double, double );
      inline void SetRow3 ( double, double, double );
      inline void SetRow1 ( const VectorR3& );
      inline void SetRow2 ( const VectorR3& );
      inline void SetRow3 ( const VectorR3& );
      inline VectorR3 Row1() const;
      inline VectorR3 Row2() const;
      inline VectorR3 Row3() const;
  
      inline void SetDiagonal( double, double, double );
      inline void SetDiagonal( const VectorR3& );
      inline double Diagonal( int );
  
      // Set this so that  (this)v  =  u*v   where * is vector cross product
      inline void SetCrossProductMatrix( const VectorR3& u );
  
      // Set this = u * v^T.
      inline void SetOuterProduct( const VectorR3& u, const VectorR3& v );
  
      inline void MakeTranspose();					// Transposes it.
      Matrix3x3& ReNormalize();
      VectorR3 Solve(const VectorR3&) const;	// Returns solution
  
      inline void Transform( VectorR3* ) const;
      inline void Transform( const VectorR3& src, VectorR3* dest) const;
      inline void TransformTranspose( VectorR3* ) const;
      inline void TransformTranspose( const VectorR3& src, VectorR3* dest) const;
  
      double Trace() const
      {
          return m11+m22+m33;
      }
      double SumSquaresNorm() const;		// Returns sum of squares of entries
  
  protected:
      void OperatorTimesEquals( const Matrix3x3& ); // Internal use only
      void RightMultiplyByTranspose( const Matrix3x3& );	  // Internal use only. Set this = this * M^T
      void LeftMultiplyBy( const Matrix3x3& );	  // Internal use only. Set this = M * this
      void LeftMultiplyByTranspose( const Matrix3x3& );	  // Internal use only.  Set this = M^T * this
      void SetZero ();							  // Set to the zero map
  
  };
  
  inline VectorR3 operator* ( const Matrix3x3&, const VectorR3& );
  
  std::ostream & operator<< (std::ostream& os, const Matrix3x3& A);
  
  
  // *****************************************
  // Matrix3x4 class                         *
  // * * * * * * * * * * * * * * * * * * * * *
  
  class Matrix3x4
  {
  public:
      double m11, m12, m13, m21, m22, m23, m31, m32, m33;
      double m14;
      double m24;
      double m34;
  
      static Matrix3x4 Identity() {
          Matrix3x4 val;
          val.SetIdentity();
          return val;
      }
  
  public:
      // Constructors set by columns!
      Matrix3x4() {}
      Matrix3x4(const VectorR3&, const VectorR3&, const VectorR3&, const VectorR3& );
      Matrix3x4(double, double, double, double, double, double,
                double, double, double, double, double, double );	// Sets by columns
      Matrix3x4( const Matrix3x3&, const VectorR3& );
  
      void SetIdentity ();		// Set to the identity map
      void Set ( const Matrix3x4& );	// Set to the matrix.
      void Set3x3 ( const Matrix3x3& );	// Set linear part to the matrix.
      void Set ( const Matrix3x3&, const VectorR3& );	// Set to the matrix plus 4th column
      void Set( const VectorR3&, const VectorR3&, const VectorR3&, const VectorR3& );
      void Set( double, double, double,
                double, double, double,
                double, double, double,
                double, double, double );	  // Sets by columns
      void Set3x3( double, double, double,
                   double, double, double,
                   double, double, double );	  // Sets by columns
      void SetByRows( double, double, double, double, double, double,
                      double, double, double, double, double, double );
  
      void SetColumn1 ( double, double, double );
      void SetColumn2 ( double, double, double );
      void SetColumn3 ( double, double, double );
      void SetColumn4 ( double, double, double );
      void SetColumn1 ( const VectorR3& );
      void SetColumn2 ( const VectorR3& );
      void SetColumn3 ( const VectorR3& );
      void SetColumn4 ( const VectorR3& );
      VectorR3 Column1() const;
      VectorR3 Column2() const;
      VectorR3 Column3() const;
      VectorR3 Column4() const;
      void SetRow1 ( double x, double y, double z, double w );
      void SetRow2 ( double x, double y, double z, double w );
      void SetRow3 ( double x, double y, double z, double w );
      void SetRow4 ( double x, double y, double z, double w );
  
      Matrix3x4& ApplyTranslationLeft( const VectorR3& u );
      Matrix3x4& ApplyTranslationRight( const VectorR3& u );
      Matrix3x4& ApplyXRotationLeft( double theta );
      Matrix3x4& ApplyXRotationLeft( double costheta, double sintheta );
      Matrix3x4& ApplyYRotationLeft( double theta );
      Matrix3x4& ApplyYRotationLeft( double costheta, double sintheta );
  
      Matrix3x4& ReNormalize();
      VectorR3 Solve(const VectorR3&) const;	// Returns solution
  
      inline void Transform( VectorR3* ) const;
      inline void Transform( VectorR4* ) const;
      inline void Transform3x3( VectorR3* ) const;
      inline void Transform( const VectorR3& src, VectorR3*  dest ) const;
      inline void Transform( const VectorR4& src, VectorR4*  dest ) const;
      inline void Transform3x3( const VectorR3& src, VectorR3*  dest ) const;
      inline void Transform3x3Transpose( VectorR3*  dest ) const;
      inline void Transform3x3Transpose( const VectorR3& src, VectorR3*  dest ) const;
  
  protected:
      void SetZero ();			// Set to the zero map
      void OperatorTimesEquals( const Matrix3x3& ); // Internal use only
      void OperatorTimesEquals( const Matrix3x4& ); // Internal use only
  };
  
  inline VectorR3 operator* ( const Matrix3x4&, const VectorR3& );
  
  std::ostream & operator << (std::ostream & os, const Matrix3x4 & A);
  
  // *****************************************
  // LinearMapR3 class                       *
  // * * * * * * * * * * * * * * * * * * * * *
  
  class LinearMapR3 : public Matrix3x3
  {
  
  public:
  
      LinearMapR3();
      LinearMapR3( const VectorR3&, const VectorR3&, const VectorR3& );
      LinearMapR3( double, double, double, double, double, double,
                   double, double, double );		// Sets by columns
      LinearMapR3 ( const Matrix3x3& );
  
      void SetZero ();			// Set to the zero map
      inline void Negate();
  
      inline LinearMapR3& operator+= (const Matrix3x3& );
      inline LinearMapR3& operator-= (const Matrix3x3& );
      inline LinearMapR3& operator*= (double);
      inline LinearMapR3& operator/= (double);
      inline void SubtractFrom( const Matrix3x3& m);	// Sets this = (m - this).
      LinearMapR3& operator*= (const Matrix3x3& );	// Matrix product
      void RightMultiplyByTranspose( const Matrix3x3& M )
      {
          Matrix3x3::RightMultiplyByTranspose(M);
      }
      void LeftMultiplyBy( const Matrix3x3& M )
      {
          Matrix3x3::LeftMultiplyBy(M);
      }
      void LeftMultiplyByTranspose( const Matrix3x3& M )
      {
          Matrix3x3::LeftMultiplyByTranspose(M);
      }
  
      inline LinearMapR3 Transpose() const;	// Returns the transpose
      double Determinant () const;			// Returns the determinant
      LinearMapR3 Inverse() const;			// Returns inverse
      LinearMapR3& Invert();					// Converts into inverse.
      VectorR3 Solve(const VectorR3&) const;	// Returns solution
      LinearMapR3 InverseSym() const;			// Get inverse of symmetric matrix
      LinearMapR3 InversePosDef() const;		// Get inverse of symmetric positive definite matrix
      void InverseSym( LinearMapR3* inverse ) const;	// Get inverse of symmetric matrix
      void InversePosDef( LinearMapR3* inverse ) const;	// Get inverse of symmetric positive-definite matrix
      LinearMapR3& InvertSym();		// Converts to inverse of symmetric matrix
      LinearMapR3& InvertPosDef();		// Converts to inverse of symmetric positive-definite matrix
      LinearMapR3& InvertPosDefSafe();		// Converts to inverse of symmetric positive-definite matrix
      LinearMapR3 PseudoInverse() const;	// Returns pseudo-inverse TO DO
      VectorR3 PseudoSolve(const VectorR3&);	// Finds least squares solution TO DO
  
  };
  
  inline LinearMapR3 operator+ (const LinearMapR3&, const LinearMapR3&);
  inline LinearMapR3 operator+ (const LinearMapR3&, const Matrix3x3&);
  inline LinearMapR3 operator+ (const Matrix3x3&, const LinearMapR3&);
  inline LinearMapR3 operator- (const LinearMapR3&);
  inline LinearMapR3 operator- (const LinearMapR3&, const LinearMapR3&);
  inline LinearMapR3 operator- (const LinearMapR3&, const Matrix3x3&);
  inline LinearMapR3 operator- (const Matrix3x3&, const LinearMapR3&);
  inline LinearMapR3 operator* ( const LinearMapR3&, double);
  inline LinearMapR3 operator* ( double, const LinearMapR3& );
  inline LinearMapR3 operator/ ( const LinearMapR3&, double );
  LinearMapR3 operator* ( const LinearMapR3&, const LinearMapR3& );
  // Matrix product (composition)
  
  
  // *****************************************************
  // * AffineMapR3 class                 				   *
  // * * * * * * * * * * * * * * * * * * * * * * * * * * *
  
  class AffineMapR3 : public Matrix3x4
  {
  
  public:
      AffineMapR3();
      AffineMapR3( double, double, double, double, double, double,
                   double, double, double, double, double, double );  // Sets by columns
      AffineMapR3 ( const VectorR3&, const VectorR3&, const VectorR3&, const VectorR3&);
      AffineMapR3 ( const LinearMapR3&, const VectorR3& );
  
      void SetIdentity ();		// Set to the identity map
      void SetZero ();			// Set to the zero map
  
      AffineMapR3& operator+= (const Matrix3x4& );
      AffineMapR3& operator-= (const Matrix3x4& );
      AffineMapR3& operator*= (double);
      AffineMapR3& operator/= (double);
      AffineMapR3& operator*= (const Matrix3x3& );	// Composition
      AffineMapR3& operator*= (const Matrix3x4& );	// Composition
  
      AffineMapR3& ApplyTranslationLeft( const VectorR3& u )
      {
          Matrix3x4::ApplyTranslationLeft( u );
          return *this;
      }
      AffineMapR3& ApplyTranslationRight( const VectorR3& u )
      {
          Matrix3x4::ApplyTranslationRight( u );
          return *this;
      }
      AffineMapR3& ApplyXRotationLeft( double theta )
      {
          Matrix3x4::ApplyXRotationLeft( theta );
          return *this;
      }
      AffineMapR3& ApplyXRotationLeft( double costheta, double sintheta )
      {
          Matrix3x4::ApplyXRotationLeft( costheta, sintheta );
          return *this;
      }
      AffineMapR3& ApplyYRotationLeft( double theta )
      {
          Matrix3x4::ApplyYRotationLeft( theta );
          return *this;
      }
      AffineMapR3& ApplyYRotationLeft( double costheta, double sintheta )
      {
          Matrix3x4::ApplyYRotationLeft( costheta, sintheta );
          return *this;
      }
  
      AffineMapR3 Inverse() const;					// Returns inverse
      AffineMapR3& Invert();							// Converts into inverse.
      VectorR3 Solve(const VectorR3&) const;	// Returns solution
      AffineMapR3 PseudoInverse() const;		// Returns pseudo-inverse // TO DO
      VectorR3 PseudoSolve(const VectorR3&);	// Least squares solution // TO DO
  };
  
  inline AffineMapR3 operator+ (const AffineMapR3&, const Matrix3x4&);
  inline AffineMapR3 operator+ (const Matrix3x4&, const AffineMapR3&);
  inline AffineMapR3 operator+ (const AffineMapR3&, const Matrix3x3&);
  inline AffineMapR3 operator+ (const Matrix3x3&, const AffineMapR3&);
  inline AffineMapR3 operator- (const AffineMapR3&, const Matrix3x4&);
  inline AffineMapR3 operator- (const Matrix3x4&, const AffineMapR3&);
  inline AffineMapR3 operator- (const AffineMapR3&, const Matrix3x3&);
  inline AffineMapR3 operator- (const Matrix3x3&, const AffineMapR3&);
  inline AffineMapR3 operator* (const AffineMapR3&, double);
  inline AffineMapR3 operator* (double, const AffineMapR3& );
  inline AffineMapR3 operator/ (const AffineMapR3&, double );
  
  // Composition operators
  AffineMapR3 operator* ( const AffineMapR3&, const AffineMapR3& );
  AffineMapR3 operator* ( const LinearMapR3&, const AffineMapR3& );
  AffineMapR3 operator* ( const AffineMapR3&, const LinearMapR3& );
  
  
  // *******************************************
  // RotationMapR3 class                       *
  // * * * * * * * * * * * * * * * * * * * * * *
  
  class RotationMapR3 : public Matrix3x3
  {
  
  public:
      static const RotationMapR3 Identity;
  
  public:
  
      RotationMapR3();
      RotationMapR3( const VectorR3&, const VectorR3&, const VectorR3& );
      RotationMapR3( double, double, double, double, double, double,
                     double, double, double );
  
      RotationMapR3& Set( const Quaternion& );			// Defined in Quaternion.cpp
      RotationMapR3& Set( const VectorR3&, double theta ); // Set rotation axis and angle
      RotationMapR3& Set( const VectorR3&, double sintheta, double costheta );
  
      RotationMapR3& operator*= (const RotationMapR3& );	// Matrix product
      void RightMultiplyByTranspose( const RotationMapR3& M )
      {
          Matrix3x3::RightMultiplyByTranspose(M);
      }
      void LeftMultiplyBy( const RotationMapR3& M )
      {
          Matrix3x3::LeftMultiplyBy(M);
      }
      void LeftMultiplyByTranspose( const RotationMapR3& M )
      {
          Matrix3x3::LeftMultiplyByTranspose(M);
      }
  
      RotationMapR3 Transpose() const
      {
          return Inverse();
      };	// Returns the transpose
      RotationMapR3 Inverse() const;			// Returns inverse
      RotationMapR3& Invert();				// Converts into inverse.
      VectorR3 Solve(const VectorR3&) const;	// Returns solution	// Was named Invert
  
      double ToAngle() const;  // returns angle of rotation (-PI,PI] range (from atan2)
      bool ToAxisAndAngle( VectorR3* u, double* theta ) const;  // returns unit vector u and angle
  
  };
  
  RotationMapR3 operator* ( const RotationMapR3&, const RotationMapR3& );
  // Matrix product (composition)
  
  inline RotationMapR3 ToRotationMapR3( const Quaternion& q )
  {
      return( RotationMapR3().Set(q) );
  }
  
  std::ostream & operator<< (std::ostream & os, const RotationMapR3 & A);
  
  
  
  // ***************************************************************
  // * RigidMapR3 class - prototypes.								 *													 *
  // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
  
  class RigidMapR3 : public Matrix3x4
  {
  
  public:
      RigidMapR3();
      RigidMapR3(const RigidMapR3&) = default;
      RigidMapR3( const VectorR3&, const VectorR3&, const VectorR3&, const VectorR3& );
      RigidMapR3( double, double, double, double, double, double,
                  double, double, double, double, double, double );
      RigidMapR3( const Matrix3x3&, const VectorR3& );
      static RigidMapR3 Identity() {
          RigidMapR3 val;
          val.SetIdentity();
          return val;
      }
  
      RigidMapR3& Set( const Matrix3x3&, const VectorR3& );   // Set to RotationMap & Vector
      RigidMapR3& SetTranslationPart( const VectorR3& );		// Set the translation part
      RigidMapR3& SetTranslationPart( double, double, double );	// Set the translation part
      RigidMapR3& SetRotationPart( const Matrix3x3& );		// Set the rotation part
      RigidMapR3& SetRotationPart( const Quaternion& );		// Still needs to be written (See RotationMapR3.Set.)
      RigidMapR3& SetRotationPart( const VectorR3&, double theta ); // Set rotation axis and angle
      RigidMapR3& SetRotationPart( const VectorR3&, double sintheta, double costheta );
  
      RigidMapR3& ApplyTranslationLeft( const VectorR3& u )
      {
          Matrix3x4::ApplyTranslationLeft( u );
          return *this;
      }
      RigidMapR3& ApplyTranslationRight( const VectorR3& u )
      {
          Matrix3x4::ApplyTranslationRight( u );
          return *this;
      }
      RigidMapR3& ApplyXRotationLeft( double theta )
      {
          Matrix3x4::ApplyXRotationLeft( theta );
          return *this;
      }
      RigidMapR3& ApplyXRotationLeft( double costheta, double sintheta )
      {
          Matrix3x4::ApplyXRotationLeft( costheta, sintheta );
          return *this;
      }
      RigidMapR3& ApplyYRotationLeft( double theta )
      {
          Matrix3x4::ApplyYRotationLeft( theta );
          return *this;
      }
      RigidMapR3& ApplyYRotationLeft( double costheta, double sintheta )
      {
          Matrix3x4::ApplyYRotationLeft( costheta, sintheta );
          return *this;
      }
  
      RigidMapR3& operator*=(const RotationMapR3& );	// Composition
      RigidMapR3& operator*=(const RigidMapR3& );		// Composition
  
      RigidMapR3 Inverse() const;			// Returns inverse
      RigidMapR3& Invert();				// Converts into inverse.
  
      bool CalcGlideRotation( VectorR3* u, VectorR3* v,
                              double *glideDist, double *rotation ) const;
  
      void Transform3x3Inverse( VectorR3* dest ) const
      {
          Matrix3x4::Transform3x3Transpose( dest );
      }
      void Transform3x3Inverse( const VectorR3& src, VectorR3* dest ) const
      {
          Matrix3x4::Transform3x3Transpose( src, dest );
      }
  
  };
  
  
  // ***************************************************************
  // * 3-space vector and matrix utilities (prototypes)			 *
  // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
  
  // Returns the solid angle between vectors v and w.
  inline double SolidAngle( const VectorR3& v, const VectorR3& w);
  
  // Returns a righthanded orthonormal basis to complement unit vector x
  void GetOrtho( const VectorR3& x,  VectorR3& y, VectorR3& z);
  // Returns a vector v orthonormal to unit vector x
  void GetOrtho( const VectorR3& x,  VectorR3& y );
  
  // Returns a vector v orthonormal to unit vector x
  VectorR3 GetOrtho( const VectorR3& x);
  
  // Projections
  
  // The next three functions are templated below.
  //inline VectorR3 ProjectToUnit ( const VectorR3& u, const VectorR3& v); // Project u onto v
  //inline VectorR3 ProjectPerpUnit ( const VectorR3& u, const VectorR3 & v); // Project perp to v
  //inline VectorR3 ProjectPerpUnitDiff ( const VectorR3& u, const VectorR3& v)
  // v must be a unit vector.
  
  // Projection maps (LinearMapR3s)
  
  inline LinearMapR3 VectorProjectMap( const VectorR3& u );
  inline LinearMapR3 PlaneProjectMap ( const VectorR3& w );
  inline LinearMapR3 PlaneProjectMap ( const VectorR3& u, const VectorR3 &v );
  // u,v,w - must be unit vector.  u and v must be orthonormal and
  // specify the plane they are parallel to.  w specifies the plane
  // it is orthogonal to.
  
  // VrRotate is similar to glRotate.  Returns a matrix (RotationMapR3)
  //		that will perform the rotation.  u should be a unit vector.
  RotationMapR3 VrRotate( double theta, const VectorR3& u );
  RotationMapR3 VrRotate( double costheta, double sintheta, const VectorR3& u );
  RotationMapR3 VrRotateAlign( const VectorR3& fromVec, const VectorR3& toVec);
  RotationMapR3 RotateToMap( const VectorR3& fromVec, const VectorR3& toVec);
  // fromVec and toVec should be unit vectors for RotateToMap
  
  // ***************************************************************
  // * Stream Output Routines	(Prototypes)						 *
  // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
  
  std::ostream & operator << (std::ostream & os, const VectorR3 & u);
  
  
  // *****************************************************
  // * VectorR3 class - inlined functions				   *
  // * * * * * * * * * * * * * * * * * * * * * * * * * * *
  
  inline VectorR3& VectorR3::Load( const double* v )
  {
      x = *v;
      y = *(v+1);
      z = *(v+2);
      return *this;
  }
  
  inline VectorR3& VectorR3::Load( const float* v )
  {
      x = *v;
      y = *(v+1);
      z = *(v+2);
      return *this;
  }
  
  inline 	void VectorR3::Dump( double* v ) const
  {
      *v = x;
      *(v+1) = y;
      *(v+2) = z;
  }
  
  inline 	void VectorR3::Dump( float* v ) const
  {
      *v = (float)x;
      *(v+1) = (float)y;
      *(v+2) = (float)z;
  }
  
  inline double VectorR3::operator[]( int i ) const
  {
      switch (i) {
      case 0:
          return x;
      case 1:
          return y;
      case 2:
          return z;
      default:
          assert(0);
          return 0.0;
      }
  }
  
  inline VectorR3& VectorR3::MakeUnit ()			// Convert to unit vector (or leave zero).
  {
      double nSq = NormSq();
      if (nSq != 0.0) {
          *this /= sqrt(nSq);
      }
      return *this;
  }
  
  inline VectorR3 operator+( const VectorR3& u, const VectorR3& v )
  {
      return VectorR3(u.x+v.x, u.y+v.y, u.z+v.z);
  }
  inline VectorR3 operator-( const VectorR3& u, const VectorR3& v )
  {
      return VectorR3(u.x-v.x, u.y-v.y, u.z-v.z);
  }
  inline VectorR3 operator*( const VectorR3& u, double m)
  {
      return VectorR3( u.x*m, u.y*m, u.z*m);
  }
  inline VectorR3 operator*( double m, const VectorR3& u)
  {
      return VectorR3( u.x*m, u.y*m, u.z*m);
  }
  inline VectorR3 operator/( const VectorR3& u, double m)
  {
      double mInv = 1.0/m;
      return VectorR3( u.x*mInv, u.y*mInv, u.z*mInv);
  }
  
  inline double operator^ ( const VectorR3& u, const VectorR3& v ) // Dot Product
  {
      return ( u.x*v.x + u.y*v.y + u.z*v.z );
  }
  
  inline VectorR3 operator* (const VectorR3& u, const VectorR3& v)	// Cross Product
  {
      return (VectorR3(	u.y*v.z - u.z*v.y,
                          u.z*v.x - u.x*v.z,
                          u.x*v.y - u.y*v.x  ) );
  }
  
  inline VectorR3 ArrayProd ( const VectorR3& u, const VectorR3& v )
  {
      return ( VectorR3( u.x*v.x, u.y*v.y, u.z*v.z ) );
  }
  
  inline VectorR3& VectorR3::operator*= (const VectorR3& v)		// Cross Product
  {
      double tx=x, ty=y;
      x =  y*v.z -  z*v.y;
      y =  z*v.x - tx*v.z;
      z = tx*v.y - ty*v.x;
      return ( *this );
  }
  
  // Cross Product on left
  //  Set   this := v*this;
  inline VectorR3& VectorR3::CrossProductLeft (const VectorR3& v)
  {
      double tx=x, ty=y;
      x =  z*v.y - y*v.z;
      y = tx*v.z - z*v.x;
      z = ty*v.x - tx*v.y;
      return ( *this );
  }
  
  // (*this) += u*v;
  inline VectorR3& VectorR3::AddCrossProduct( const VectorR3& u, const VectorR3& v )
  {
      x += u.y*v.z - u.z*v.y;
      y += u.z*v.x - u.x*v.z;
      z += u.x*v.y - u.y*v.x;
      return *this;
  }
  
  inline VectorR3& VectorR3::ArrayProd (const VectorR3& v)		// Component-wise Product
  {
      x *= v.x;
      y *= v.y;
      z *= v.z;
      return ( *this );
  }
  
  inline VectorR3& VectorR3::AddScaled( const VectorR3& u, double s )
  {
      x += s*u.x;
      y += s*u.y;
      z += s*u.z;
      return(*this);
  }
  
  inline VectorR3& VectorR3::SubtractFrom( const VectorR3& u  )
  {
      x = u.x - x;;
      y = u.y - y;;
      z = u.z - z;;
      return(*this);
  }
  
  inline VectorR3::VectorR3( const VectorHgR3& uH )
      : x(uH.x), y(uH.y), z(uH.z)
  {
      *this /= uH.w;
  }
  
  inline VectorR3& VectorR3::ReNormalize()			// Convert near unit back to unit
  {
      double nSq = NormSq();
      double mFact = 1.0-0.5*(nSq-1.0);	// Multiplicative factor
      *this *= mFact;
      return *this;
  }
  
  inline double NormalizeError (const VectorR3& u)
  {
      double discrepancy;
      discrepancy = u.x*u.x + u.y*u.y + u.z*u.z - 1.0;
      if ( discrepancy < 0.0 ) {
          discrepancy = -discrepancy;
      }
      return discrepancy;
  }
  
  inline double VectorR3::Dist( const VectorR3& u ) const 	// Distance from u
  {
      return sqrt( DistSq(u) );
  }
  
  inline double VectorR3::DistSq( const VectorR3& u ) const	// Distance from u
  {
      return ( (x-u.x)*(x-u.x) + (y-u.y)*(y-u.y) + (z-u.z)*(z-u.z) );
  }
  
  //
  // Interpolation routines (not just Spherical Interpolation)
  //
  
  // Interpolate(start,end,frac) - linear interpolation
  //		- allows overshooting the end points
  inline VectorR3 Interpolate( const VectorR3& start, const VectorR3& end, double a)
  {
      VectorR3 ret;
      Lerp( start, end, a, ret );
      return ret;
  }
  
  
  // ******************************************************
  // * Matrix3x3  class - inlined functions				*
  // * * * * * * * * * * * * * * * * * * * * * * * * * * **
  
  inline Matrix3x3::Matrix3x3() {}
  
  inline Matrix3x3::Matrix3x3( const VectorR3& u, const VectorR3& v,
                               const VectorR3& s )
  {
      m11 = u.x;		// Column 1
      m21 = u.y;
      m31 = u.z;
      m12 = v.x;		// Column 2
      m22 = v.y;
      m32 = v.z;
      m13 = s.x;		// Column 3
      m23 = s.y;
      m33 = s.z;
  }
  
  inline Matrix3x3::Matrix3x3( double a11, double a21, double a31,
                               double a12, double a22, double a32,
                               double a13, double a23, double a33)
  // Values specified in column order!!!
  {
      m11 = a11;		// Row 1
      m12 = a12;
      m13 = a13;
      m21 = a21;		// Row 2
      m22 = a22;
      m23 = a23;
      m31 = a31;		// Row 3
      m32 = a32;
      m33 = a33;
  }
  
  inline void Matrix3x3::SetIdentity ( )
  {
      m11 = m22 = m33 = 1.0;
      m12 = m13 = m21 = m23 = m31 = m32 = 0.0;
  }
  
  inline void Matrix3x3::SetZero( )
  {
      m11 = m12 = m13 = m21 = m22 = m23 = m31 = m32 = m33 = 0.0;
  }
  
  inline void Matrix3x3::Set ( const Matrix3x3& A )	// Set to the matrix.
  {
      m11 = A.m11;
      m21 = A.m21;
      m31 = A.m31;
      m12 = A.m12;
      m22 = A.m22;
      m32 = A.m32;
      m13 = A.m13;
      m23 = A.m23;
      m33 = A.m33;
  }
  
  inline void Matrix3x3::Set3x3 ( const Matrix3x4& A )	// Set to the 3x3 part of the matrix.
  {
      m11 = A.m11;
      m21 = A.m21;
      m31 = A.m31;
      m12 = A.m12;
      m22 = A.m22;
      m32 = A.m32;
      m13 = A.m13;
      m23 = A.m23;
      m33 = A.m33;
  }
  
  inline void Matrix3x3::Set( const VectorR3& u, const VectorR3& v,
                              const VectorR3& w)
  {
      m11 = u.x;		// Column 1
      m21 = u.y;
      m31 = u.z;
      m12 = v.x;		// Column 2
      m22 = v.y;
      m32 = v.z;
      m13 = w.x;		// Column 3
      m23 = w.y;
      m33 = w.z;
  }
  
  inline void Matrix3x3::Set( double a11, double a21, double a31,
                              double a12, double a22, double a32,
                              double a13, double a23, double a33)
  // Values specified in column order!!!
  {
      m11 = a11;		// Row 1
      m12 = a12;
      m13 = a13;
      m21 = a21;		// Row 2
      m22 = a22;
      m23 = a23;
      m31 = a31;		// Row 3
      m32 = a32;
      m33 = a33;
  }
  
  inline void Matrix3x3::SetByRows( double a11, double a12, double a13,
                                    double a21, double a22, double a23,
                                    double a31, double a32, double a33)
  // Values specified in row order!!!
  {
      m11 = a11;		// Row 1
      m12 = a12;
      m13 = a13;
      m21 = a21;		// Row 2
      m22 = a22;
      m23 = a23;
      m31 = a31;		// Row 3
      m32 = a32;
      m33 = a33;
  }
  
  inline void Matrix3x3::LoadByRows( const double* a)
  {
      // Values in ROW order
      m11 = *(a++);		// Row 1
      m12 = *(a++);
      m13 = *(a++);
      m21 = *(a++);		// Row 2
      m22 = *(a++);
      m23 = *(a++);
      m31 = *(a++);		// Row 3
      m32 = *(a++);
      m33 = *a;
  }
  
  
  inline void Matrix3x3::SetByRows( const VectorR3& u, const VectorR3& v,
                                    const VectorR3& s )
  {
      m11 = u.x;		// Row 1
      m12 = u.y;
      m13 = u.z;
      m21 = v.x;		// Row 2
      m22 = v.y;
      m23 = v.z;
      m31 = s.x;		// Row 3
      m32 = s.y;
      m33 = s.z;
  }
  
  inline void Matrix3x3::SetColumn1 ( double x, double y, double z)
  {
      m11 = x;
      m21 = y;
      m31= z;
  }
  
  inline void Matrix3x3::SetColumn2 ( double x, double y, double z)
  {
      m12 = x;
      m22 = y;
      m32= z;
  }
  
  inline void Matrix3x3::SetColumn3 ( double x, double y, double z)
  {
      m13 = x;
      m23 = y;
      m33= z;
  }
  
  inline void Matrix3x3::SetColumn1 ( const VectorR3& u )
  {
      m11 = u.x;
      m21 = u.y;
      m31 = u.z;
  }
  
  inline void Matrix3x3::SetColumn2 ( const VectorR3& u )
  {
      m12 = u.x;
      m22 = u.y;
      m32 = u.z;
  }
  
  inline void Matrix3x3::SetColumn3 ( const VectorR3& u )
  {
      m13 = u.x;
      m23 = u.y;
      m33 = u.z;
  }
  
  inline void Matrix3x3::SetRow1 ( double x, double y, double z )
  {
      m11 = x;
      m12 = y;
      m13 = z;
  }
  
  inline void Matrix3x3::SetRow2 ( double x, double y, double z )
  {
      m21 = x;
      m22 = y;
      m23 = z;
  }
  
  inline void Matrix3x3::SetRow3 ( double x, double y, double z )
  {
      m31 = x;
      m32 = y;
      m33 = z;
  }
  
  
  
  inline VectorR3 Matrix3x3::Column1() const
  {
      return ( VectorR3(m11, m21, m31) );
  }
  
  inline VectorR3 Matrix3x3::Column2() const
  {
      return ( VectorR3(m12, m22, m32) );
  }
  
  inline VectorR3 Matrix3x3::Column3() const
  {
      return ( VectorR3(m13, m23, m33) );
  }
  
  inline VectorR3 Matrix3x3::Row1() const
  {
      return ( VectorR3(m11, m12, m13) );
  }
  
  inline VectorR3 Matrix3x3::Row2() const
  {
      return ( VectorR3(m21, m22, m23) );
  }
  
  inline VectorR3 Matrix3x3::Row3() const
  {
      return ( VectorR3(m31, m32, m33) );
  }
  
  inline void Matrix3x3::SetDiagonal( double x, double y, double z )
  {
      m11 = x;
      m22 = y;
      m33 = z;
  }
  
  inline void Matrix3x3::SetDiagonal( const VectorR3& u )
  {
      SetDiagonal ( u.x, u.y, u.z );
  }
  
  inline double Matrix3x3::Diagonal( int i )
  {
      switch (i) {
      case 0:
          return m11;
      case 1:
          return m22;
      case 2:
          return m33;
      default:
          assert(0);
          return 0.0;
      }
  }
  
  // Set this so that  (this)v  =  u*v   where * is vector cross product
  inline void Matrix3x3::SetCrossProductMatrix( const VectorR3& u )
  {
      m11 = m22 = m33 = 0.0;
      m21 = u.z;
      m12 = -u.z;
      m13 = u.y;
      m31 = -u.y;
      m32 = u.x;
      m23 = -u.x;
  }
  
  
  // Set this = u * v^T
  inline void Matrix3x3::SetOuterProduct( const VectorR3& u, const VectorR3& v )
  {
      m11 = u.x*v.x;
      m12 = u.x*v.y;
      m13 = u.x*v.z;
      m21 = u.y*v.x;
      m22 = u.y*v.y;
      m23 = u.y*v.z;
      m31 = u.z*v.x;
      m32 = u.z*v.y;
      m33 = u.z*v.z;
  }
  
  inline void Matrix3x3::MakeTranspose()	// Transposes it.
  {
      double temp;
      temp = m12;
      m12 = m21;
      m21=temp;
      temp = m13;
      m13 = m31;
      m31 = temp;
      temp = m23;
      m23 = m32;
      m32 = temp;
  }
  
  inline VectorR3 operator* ( const Matrix3x3& A, const VectorR3& u)
  {
      return( VectorR3( A.m11*u.x + A.m12*u.y + A.m13*u.z,
                        A.m21*u.x + A.m22*u.y + A.m23*u.z,
                        A.m31*u.x + A.m32*u.y + A.m33*u.z ) );
  }
  
  
  // See LinearR4.h for the code for the VectorR4 versions of the next two functions.
  
  inline void Matrix3x3::Transform( VectorR3* u ) const
  {
      double newX, newY;
      newX = m11*u->x + m12*u->y + m13*u->z;
      newY = m21*u->x + m22*u->y + m23*u->z;
      u->z = m31*u->x + m32*u->y + m33*u->z;
      u->x = newX;
      u->y = newY;
  }
  
  inline void Matrix3x3::Transform( const VectorR3& src, VectorR3* dest ) const
  {
      dest->x = m11*src.x + m12*src.y + m13*src.z;
      dest->y = m21*src.x + m22*src.y + m23*src.z;
      dest->z = m31*src.x + m32*src.y + m33*src.z;
  }
  
  inline void Matrix3x3::TransformTranspose( VectorR3* u ) const
  {
      double newX, newY;
      newX = m11*u->x + m21*u->y + m31*u->z;
      newY = m12*u->x + m22*u->y + m32*u->z;
      u->z = m13*u->x + m23*u->y + m33*u->z;
      u->x = newX;
      u->y = newY;
  }
  
  inline void Matrix3x3::TransformTranspose( const VectorR3& src, VectorR3* dest ) const
  {
      dest->x = m11*src.x + m21*src.y + m31*src.z;
      dest->y = m12*src.x + m22*src.y + m32*src.z;
      dest->z = m13*src.x + m23*src.y + m33*src.z;
  }
  
  
  // ******************************************************
  // * Matrix3x4  class - inlined functions				*
  // * * * * * * * * * * * * * * * * * * * * * * * * * * **
  
  inline Matrix3x4::Matrix3x4(const VectorR3& u, const VectorR3& v,
                              const VectorR3& s, const VectorR3& t)
  {
      m11 = u.x;		// Column 1
      m21 = u.y;
      m31 = u.z;
      m12 = v.x;		// Column 2
      m22 = v.y;
      m32 = v.z;
      m13 = s.x;		// Column 3
      m23 = s.y;
      m33 = s.z;
      m14 = t.x;
      m24 = t.y;
      m34 = t.z;
  }
  
  inline Matrix3x4::Matrix3x4(double a11, double a21, double a31,
                              double a12, double a22, double a32,
                              double a13, double a23, double a33,
                              double a14, double a24, double a34)
  {
      // Values in COLUMN order!
      m11 = a11;		// Row 1
      m12 = a12;
      m13 = a13;
      m14 = a14;
      m21 = a21;		// Row 2
      m22 = a22;
      m23 = a23;
      m24 = a24;
      m31 = a31;		// Row 3
      m32 = a32;
      m33 = a33;
      m34 = a34;
  }
  
  inline Matrix3x4::Matrix3x4( const Matrix3x3& A, const VectorR3& u )
  {
      Set(A, u);
  }
  
  inline void Matrix3x4::SetIdentity ()		// Set to the identity map
  {
      m11 = m22 = m33 = 1.0;
      m12 = m13 = m21 = m23 = m31 = m32 = 0.0;
      m14 = m24 = m34 = 0.0;
  }
  
  inline void Matrix3x4::SetZero ()			// Set to the zero map
  {
      m11 = m22 = m33 = 0.0;
      m12 = m13 = m21 = m23 = m31 = m32 = 0.0;
      m14 = m24 = m34 = 0.0;
  }
  
  inline void Matrix3x4::Set ( const Matrix3x4& A )	// Set to the matrix.
  {
      m11 = A.m11;
      m21 = A.m21;
      m31 = A.m31;
      m12 = A.m12;
      m22 = A.m22;
      m32 = A.m32;
      m13 = A.m13;
      m23 = A.m23;
      m33 = A.m33;
      m14 = A.m14;
      m24 = A.m24;
      m34 = A.m34;
  }
  
  inline void Matrix3x4::Set ( const Matrix3x3& A, const VectorR3& t )	// Set to the matrix plus 4th column
  {
      m11 = A.m11;
      m21 = A.m21;
      m31 = A.m31;
      m12 = A.m12;
      m22 = A.m22;
      m32 = A.m32;
      m13 = A.m13;
      m23 = A.m23;
      m33 = A.m33;
      m14 = t.x;		// Column 4
      m24 = t.y;
      m34 = t.z;
  }
  
  // Set linear part to the matrix
  inline void Matrix3x4::Set3x3 ( const Matrix3x3& A )
  {
      m11 = A.m11;
      m21 = A.m21;
      m31 = A.m31;
      m12 = A.m12;
      m22 = A.m22;
      m32 = A.m32;
      m13 = A.m13;
      m23 = A.m23;
      m33 = A.m33;
  }
  
  inline void Matrix3x4::Set( const VectorR3& u, const VectorR3& v,
                              const VectorR3& w, const VectorR3& t )
  {
      m11 = u.x;		// Column 1
      m21 = u.y;
      m31 = u.z;
      m12 = v.x;		// Column 2
      m22 = v.y;
      m32 = v.z;
      m13 = w.x;		// Column 3
      m23 = w.y;
      m33 = w.z;
      m14 = t.x;		// Column 4
      m24 = t.y;
      m34 = t.z;
  }
  
  inline void Matrix3x4::Set( double a11, double a21, double a31,
                              double a12, double a22, double a32,
                              double a13, double a23, double a33,
                              double a14, double a24, double a34 )
  // Values specified in column order!!!
  {
      m11 = a11;		// Row 1
      m12 = a12;
      m13 = a13;
      m14 = a14;
      m21 = a21;		// Row 2
      m22 = a22;
      m23 = a23;
      m24 = a24;
      m31 = a31;		// Row 3
      m32 = a32;
      m33 = a33;
      m34 = a34;
  }
  
  inline void Matrix3x4::Set3x3( double a11, double a21, double a31,
                                 double a12, double a22, double a32,
                                 double a13, double a23, double a33 )
  // Values specified in column order!!!
  {
      m11 = a11;		// Row 1
      m12 = a12;
      m13 = a13;
      m21 = a21;		// Row 2
      m22 = a22;
      m23 = a23;
      m31 = a31;		// Row 3
      m32 = a32;
      m33 = a33;
  }
  
  inline void Matrix3x4::SetByRows( double a11, double a12, double a13, double a14,
                                    double a21, double a22, double a23, double a24,
                                    double a31, double a32, double a33, double a34 )
  // Values specified in row order!!!
  {
      m11 = a11;		// Row 1
      m12 = a12;
      m13 = a13;
      m14 = a14;
      m21 = a21;		// Row 2
      m22 = a22;
      m23 = a23;
      m24 = a24;
      m31 = a31;		// Row 3
      m32 = a32;
      m33 = a33;
      m34 = a34;
  }
  
  inline void Matrix3x4::SetColumn1 ( double x, double y, double z)
  {
      m11 = x;
      m21 = y;
      m31= z;
  }
  
  inline void Matrix3x4::SetColumn2 ( double x, double y, double z)
  {
      m12 = x;
      m22 = y;
      m32= z;
  }
  
  inline void Matrix3x4::SetColumn3 ( double x, double y, double z)
  {
      m13 = x;
      m23 = y;
      m33= z;
  }
  
  inline void Matrix3x4::SetColumn4 ( double x, double y, double z )
  {
      m14 = x;
      m24 = y;
      m34= z;
  }
  
  inline void Matrix3x4::SetColumn1 ( const VectorR3& u )
  {
      m11 = u.x;
      m21 = u.y;
      m31 = u.z;
  }
  
  inline void Matrix3x4::SetColumn2 ( const VectorR3& u )
  {
      m12 = u.x;
      m22 = u.y;
      m32 = u.z;
  }
  
  inline void Matrix3x4::SetColumn3 ( const VectorR3& u )
  {
      m13 = u.x;
      m23 = u.y;
      m33 = u.z;
  }
  
  inline void Matrix3x4::SetColumn4 ( const VectorR3& u )
  {
      m14 = u.x;
      m24 = u.y;
      m34 = u.z;
  }
  
  inline VectorR3 Matrix3x4::Column1() const
  {
      return ( VectorR3(m11, m21, m31) );
  }
  
  inline VectorR3 Matrix3x4::Column2() const
  {
      return ( VectorR3(m12, m22, m32) );
  }
  
  inline VectorR3 Matrix3x4::Column3() const
  {
      return ( VectorR3(m13, m23, m33) );
  }
  
  inline VectorR3 Matrix3x4::Column4() const
  {
      return ( VectorR3(m14, m24, m34) );
  }
  
  inline void Matrix3x4::SetRow1 ( double x, double y, double z, double w )
  {
      m11 = x;
      m12 = y;
      m13 = z;
      m14 = w;
  }
  
  inline void Matrix3x4::SetRow2 ( double x, double y, double z, double w )
  {
      m21 = x;
      m22 = y;
      m23 = z;
      m24 = w;
  }
  
  inline void Matrix3x4::SetRow3 ( double x, double y, double z, double w )
  {
      m31 = x;
      m32 = y;
      m33 = z;
      m34 = w;
  }
  
  // Left multiply with a translation (so the translation is applied afterwards).
  inline Matrix3x4& Matrix3x4::ApplyTranslationLeft( const VectorR3& u )
  {
      m14 += u.x;
      m24 += u.y;
      m34 += u.z;
      return *this;
  }
  
  // Right multiply with a translation (so the translation is applied first).
  inline Matrix3x4& Matrix3x4::ApplyTranslationRight( const VectorR3& u )
  {
      double new14 = m14 + m11*u.x + m12*u.y + m13*u.z;
      double new24 = m24 + m21*u.x + m22*u.y + m23*u.z;
      m34 = m34 + m31*u.x + m32*u.y + m33*u.z;
      m14 = new14;
      m24 = new24;
      return *this;
  }
  
  // Left-multiply with a rotation around the x-axis.
  inline Matrix3x4& Matrix3x4::ApplyXRotationLeft( double theta )
  {
      double costheta = cos(theta);
      double sintheta = sin(theta);
      return ApplyXRotationLeft( costheta, sintheta );
  }
  
  // Left-multiply with a rotation around the x-axis.
  inline Matrix3x4& Matrix3x4::ApplyXRotationLeft( double costheta, double sintheta )
  {
      double tmp;
      tmp = costheta*m21-sintheta*m31;
      m31 = costheta*m31+sintheta*m21;
      m21 = tmp;
  
      tmp = costheta*m22-sintheta*m32;
      m32 = costheta*m32+sintheta*m22;
      m22 = tmp;
  
      tmp = costheta*m23-sintheta*m33;
      m33 = costheta*m33+sintheta*m23;
      m23 = tmp;
  
      tmp = costheta*m24-sintheta*m34;
      m34 = costheta*m34+sintheta*m24;
      m24 = tmp;
  
      return *this;
  }
  
  // Left-multiply with a rotation around the y-axis.
  inline Matrix3x4& Matrix3x4::ApplyYRotationLeft( double theta )
  {
      double costheta = cos(theta);
      double sintheta = sin(theta);
      return ApplyYRotationLeft( costheta, sintheta );
  }
  
  inline Matrix3x4& Matrix3x4::ApplyYRotationLeft( double costheta, double sintheta )
  {
      double tmp;
      tmp = costheta*m11+sintheta*m31;
      m31 = costheta*m31-sintheta*m11;
      m11 = tmp;
  
      tmp = costheta*m12+sintheta*m32;
      m32 = costheta*m32-sintheta*m12;
      m12 = tmp;
  
      tmp = costheta*m13+sintheta*m33;
      m33 = costheta*m33-sintheta*m13;
      m13 = tmp;
  
      tmp = costheta*m14+sintheta*m34;
      m34 = costheta*m34-sintheta*m14;
      m14 = tmp;
  
      return *this;
  }
  
  inline VectorR3 Matrix3x4::Solve(const VectorR3& u) const	// Returns solution
  {
      Matrix3x3 A;
      A.Set3x3(*this);
      return ( A.Solve( VectorR3(m14-u.x, m24-u.y, m34-u.z) ) );
  }
  
  inline void Matrix3x4::Transform( VectorR3* u ) const
  {
      double newX, newY;
      newX = m11*u->x + m12*u->y + m13*u->z + m14;
      newY = m21*u->x + m22*u->y + m23*u->z + m24;
      u->z = m31*u->x + m32*u->y + m33*u->z + m34;
      u->x = newX;
      u->y = newY;
  }
  
  inline void Matrix3x4::Transform3x3( VectorR3* u ) const
  {
      double newX, newY;
      newX = m11*u->x + m12*u->y + m13*u->z;
      newY = m21*u->x + m22*u->y + m23*u->z;
      u->z = m31*u->x + m32*u->y + m33*u->z;
      u->x = newX;
      u->y = newY;
  }
  
  inline void Matrix3x4::Transform( const VectorR3& src, VectorR3* dest ) const
  {
      dest->x = m11*src.x + m12*src.y + m13*src.z + m14;
      dest->y = m21*src.x + m22*src.y + m23*src.z + m24;
      dest->z = m31*src.x + m32*src.y + m33*src.z + m34;
  }
  
  inline void Matrix3x4::Transform3x3( const VectorR3& src, VectorR3* dest ) const
  {
      dest->x = m11*src.x + m12*src.y + m13*src.z;
      dest->y = m21*src.x + m22*src.y + m23*src.z;
      dest->z = m31*src.x + m32*src.y + m33*src.z;
  }
  
  inline void Matrix3x4::Transform3x3Transpose( VectorR3* u ) const
  {
      double newX, newY;
      newX = m11*u->x + m21*u->y + m31*u->z;
      newY = m12*u->x + m22*u->y + m32*u->z;
      u->z = m13*u->x + m23*u->y + m33*u->z;
      u->x = newX;
      u->y = newY;
  }
  
  inline void Matrix3x4::Transform3x3Transpose( const VectorR3& src, VectorR3* dest ) const
  {
      dest->x = m11*src.x + m21*src.y + m31*src.z;
      dest->y = m12*src.x + m22*src.y + m32*src.z;
      dest->z = m13*src.x + m23*src.y + m33*src.z;
  }
  
  inline VectorR3 operator* ( const Matrix3x4& A, const VectorR3& u )
  {
      return( VectorR3( A.m11*u.x + A.m12*u.y + A.m13*u.z + A.m14,
                        A.m21*u.x + A.m22*u.y + A.m23*u.z + A.m24,
                        A.m31*u.x + A.m32*u.y + A.m33*u.z + A.m34) );
  }
  
  
  // ******************************************************
  // * LinearMapR3 class - inlined functions				*
  // * * * * * * * * * * * * * * * * * * * * * * * * * * **
  
  inline LinearMapR3::LinearMapR3()
  {
      SetZero();
      return;
  }
  
  inline LinearMapR3::LinearMapR3( const VectorR3& u, const VectorR3& v,
                                   const VectorR3& s )
      :Matrix3x3 ( u, v, s )
  { }
  
  inline LinearMapR3::LinearMapR3(
      double a11, double a21, double a31,
      double a12, double a22, double a32,
      double a13, double a23, double a33)
  // Values specified in column order!!!
      :Matrix3x3 ( a11, a21, a31, a12, a22, a32, a13, a23, a33)
  { }
  
  inline LinearMapR3::LinearMapR3 ( const Matrix3x3& A )
      : Matrix3x3 (A)
  {}
  
  inline void LinearMapR3::SetZero( )
  {
      Matrix3x3::SetZero();
  }
  
  inline void LinearMapR3::Negate()
  {
      m11 = -m11;		// Row 1
      m12 = -m12;
      m13 = -m13;
      m21 = -m21;		// Row 2
      m22 = -m22;
      m23 = -m23;
      m31 = -m31;		// Row 3
      m32 = -m32;
      m33 = -m33;
  }
  
  
  inline LinearMapR3& LinearMapR3::operator+= (const Matrix3x3& B)
  {
      m11 += B.m11;
      m12 += B.m12;
      m13 += B.m13;
      m21 += B.m21;
      m22 += B.m22;
      m23 += B.m23;
      m31 += B.m31;
      m32 += B.m32;
      m33 += B.m33;
      return ( *this );
  }
  
  inline LinearMapR3& LinearMapR3::operator-= (const Matrix3x3& B)
  {
      m11 -= B.m11;
      m12 -= B.m12;
      m13 -= B.m13;
      m21 -= B.m21;
      m22 -= B.m22;
      m23 -= B.m23;
      m31 -= B.m31;
      m32 -= B.m32;
      m33 -= B.m33;
      return( *this );
  }
  
  inline LinearMapR3 operator+ (const LinearMapR3& A, const LinearMapR3& B)
  {
      return (LinearMapR3( A.m11+B.m11, A.m21+B.m21, A.m31+B.m31,
                           A.m12+B.m12, A.m22+B.m22, A.m32+B.m32,
                           A.m13+B.m13, A.m23+B.m23, A.m33+B.m33 ) );
  }
  
  inline LinearMapR3 operator+ (const LinearMapR3& A, const Matrix3x3& B)
  {
      return (LinearMapR3( A.m11+B.m11, A.m21+B.m21, A.m31+B.m31,
                           A.m12+B.m12, A.m22+B.m22, A.m32+B.m32,
                           A.m13+B.m13, A.m23+B.m23, A.m33+B.m33 ) );
  }
  
  inline LinearMapR3 operator+ (const Matrix3x3& A, const LinearMapR3& B)
  {
      return (LinearMapR3( A.m11+B.m11, A.m21+B.m21, A.m31+B.m31,
                           A.m12+B.m12, A.m22+B.m22, A.m32+B.m32,
                           A.m13+B.m13, A.m23+B.m23, A.m33+B.m33 ) );
  }
  
  inline LinearMapR3 operator- (const LinearMapR3& A)
  {
      return( LinearMapR3( -A.m11, -A.m21, -A.m31,
                           -A.m12, -A.m22, -A.m32,
                           -A.m13, -A.m23, -A.m33 ) );
  }
  
  inline LinearMapR3 operator- (const LinearMapR3& A, const LinearMapR3& B)
  {
      return( LinearMapR3( A.m11-B.m11, A.m21-B.m21, A.m31-B.m31,
                           A.m12-B.m12, A.m22-B.m22, A.m32-B.m32,
                           A.m13-B.m13, A.m23-B.m23, A.m33-B.m33 ) );
  }
  
  inline LinearMapR3 operator- (const Matrix3x3& A, const LinearMapR3& B)
  {
      return( LinearMapR3( A.m11-B.m11, A.m21-B.m21, A.m31-B.m31,
                           A.m12-B.m12, A.m22-B.m22, A.m32-B.m32,
                           A.m13-B.m13, A.m23-B.m23, A.m33-B.m33 ) );
  }
  
  inline LinearMapR3 operator- (const LinearMapR3& A, const Matrix3x3& B)
  {
      return( LinearMapR3( A.m11-B.m11, A.m21-B.m21, A.m31-B.m31,
                           A.m12-B.m12, A.m22-B.m22, A.m32-B.m32,
                           A.m13-B.m13, A.m23-B.m23, A.m33-B.m33 ) );
  }
  
  inline LinearMapR3& LinearMapR3::operator*= (double b)
  {
      m11 *= b;
      m12 *= b;
      m13 *= b;
      m21 *= b;
      m22 *= b;
      m23 *= b;
      m31 *= b;
      m32 *= b;
      m33 *= b;
      return ( *this);
  }
  
  inline LinearMapR3 operator* ( const LinearMapR3& A, double b)
  {
      return( LinearMapR3( A.m11*b, A.m21*b, A.m31*b,
                           A.m12*b, A.m22*b, A.m32*b,
                           A.m13*b, A.m23*b, A.m33*b ) );
  }
  
  inline LinearMapR3 operator* ( double b, const LinearMapR3& A)
  {
      return( LinearMapR3( A.m11*b, A.m21*b, A.m31*b,
                           A.m12*b, A.m22*b, A.m32*b,
                           A.m13*b, A.m23*b, A.m33*b ) );
  }
  
  inline LinearMapR3 operator/ ( const LinearMapR3& A, double b)
  {
      double bInv = 1.0/b;
      return( LinearMapR3( A.m11*bInv, A.m21*bInv, A.m31*bInv,
                           A.m12*bInv, A.m22*bInv, A.m32*bInv,
                           A.m13*bInv, A.m23*bInv, A.m33*bInv ) );
  }
  
  inline LinearMapR3& LinearMapR3::operator/= (double b)
  {
      double bInv = 1.0/b;
      return ( *this *= bInv );
  }
  
  // Sets this = (m - this).
  inline void LinearMapR3::SubtractFrom( const Matrix3x3& m)
  {
      m11 = m.m11 - m11;
      m12 = m.m12 - m12;
      m13 = m.m13 - m13;
      m21 = m.m21 - m21;
      m22 = m.m22 - m22;
      m23 = m.m23 - m23;
      m31 = m.m31 - m31;
      m32 = m.m32 - m32;
      m33 = m.m33 - m33;
  }
  
  inline LinearMapR3& LinearMapR3::operator*= (const Matrix3x3& B)	// Matrix product
  {
      OperatorTimesEquals( B );
      return( *this );
  }
  
  inline VectorR3 LinearMapR3::Solve(const VectorR3& u) const	// Returns solution
  {
      return ( Matrix3x3::Solve( u ) );
  }
  
  inline LinearMapR3 LinearMapR3::Transpose() const	// Returns the transpose
  {
      return ( LinearMapR3 ( m11, m12, m13, m21, m22, m23, m31, m32, m33) );
  }
  
  // ******************************************************
  // * AffineMapR3 class - inlined functions				*
  // * * * * * * * * * * * * * * * * * * * * * * * * * * **
  
  inline AffineMapR3::AffineMapR3( double a11, double a21, double a31,
                                   double a12, double a22, double a32,
                                   double a13, double a23, double a33,
                                   double a14, double a24, double a34)
  {
      // Values in COLUMN order!
      m11 = a11;		// Row 1
      m12 = a12;
      m13 = a13;
      m14 = a14;
      m21 = a21;		// Row 2
      m22 = a22;
      m23 = a23;
      m24 = a24;
      m31 = a31;		// Row 3
      m32 = a32;
      m33 = a33;
      m34 = a34;
  }
  
  inline AffineMapR3::AffineMapR3 (const VectorR3& u, const VectorR3& v,
                                   const VectorR3& w, const VectorR3& t)
  {
      m11 = u.x;		// Column 1
      m21 = u.y;
      m31 = u.z;
      m12 = v.x;		// Column 2
      m22 = v.y;
      m32 = v.z;
      m13 = w.x;		// Column 3
      m23 = w.y;
      m33 = w.z;
      m14 = t.x;		// Column 4
      m24 = t.y;
      m34 = t.z;
  }
  
  inline AffineMapR3::AffineMapR3 (const LinearMapR3& A, const VectorR3& t)
  {
      m11 = A.m11;
      m12 = A.m12;
      m13 = A.m13;
      m14 = t.x;
      m21 = A.m21;
      m22 = A.m22;
      m23 = A.m23;
      m24 = t.y;
      m31 = A.m31;
      m32 = A.m32;
      m33 = A.m33;
      m34 = t.z;
  
  }
  
  inline void AffineMapR3::SetIdentity ( )
  {
      Matrix3x4::SetIdentity();
  }
  
  inline void AffineMapR3::SetZero( )
  {
      Matrix3x4::SetZero();
  }
  
  inline VectorR3 AffineMapR3::Solve(const VectorR3& u) const	// Returns solution
  {
      return ( Matrix3x4::Solve( u ) );
  }
  
  
  inline AffineMapR3& AffineMapR3::operator+= (const Matrix3x4& B)
  {
      m11 += B.m11;
      m21 += B.m21;
      m31 += B.m31;
      m12 += B.m12;
      m22 += B.m22;
      m32 += B.m32;
      m13 += B.m13;
      m23 += B.m23;
      m33 += B.m33;
      m14 += B.m14;
      m24 += B.m24;
      m34 += B.m34;
      return (*this);
  }
  
  inline AffineMapR3& AffineMapR3::operator-= (const Matrix3x4& B)
  {
      m11 -= B.m11;
      m21 -= B.m21;
      m31 -= B.m31;
      m12 -= B.m12;
      m22 -= B.m22;
      m32 -= B.m32;
      m13 -= B.m13;
      m23 -= B.m23;
      m33 -= B.m33;
      m14 -= B.m14;
      m24 -= B.m24;
      m34 -= B.m34;
      return (*this);
  
  }
  
  inline AffineMapR3 operator+ (const AffineMapR3& A, const AffineMapR3& B)
  {
      return( AffineMapR3( A.m11+B.m11, A.m21+B.m21, A.m31+B.m31,
                           A.m12+B.m12, A.m22+B.m22, A.m32+B.m32,
                           A.m13+B.m13, A.m23+B.m23, A.m33+B.m33,
                           A.m14+B.m14, A.m23+B.m24, A.m34+B.m34) );
  }
  
  inline AffineMapR3 operator+ (const AffineMapR3& A, const Matrix3x3& B)
  {
      return( AffineMapR3( A.m11+B.m11, A.m21+B.m21, A.m31+B.m31,
                           A.m12+B.m12, A.m22+B.m22, A.m32+B.m32,
                           A.m13+B.m13, A.m23+B.m23, A.m33+B.m33,
                           A.m14,       A.m23,       A.m34        ) );
  }
  
  inline AffineMapR3 operator+ (const Matrix3x3& B, const AffineMapR3& A)
  {
      return( AffineMapR3( A.m11+B.m11, A.m21+B.m21, A.m31+B.m31,
                           A.m12+B.m12, A.m22+B.m22, A.m32+B.m32,
                           A.m13+B.m13, A.m23+B.m23, A.m33+B.m33,
                           A.m14,       A.m23,       A.m34        ) );
  }
  
  inline AffineMapR3 operator- (const AffineMapR3& A, const AffineMapR3& B)
  {
      return( AffineMapR3( A.m11-B.m11, A.m21-B.m21, A.m31-B.m31,
                           A.m12-B.m12, A.m22-B.m22, A.m32-B.m32,
                           A.m13-B.m13, A.m23-B.m23, A.m33-B.m33,
                           A.m14-B.m14, A.m23-B.m24, A.m34-B.m34) );
  }
  
  inline AffineMapR3 operator- (const AffineMapR3& A, const LinearMapR3& B)
  {
      return ( AffineMapR3(  A.m11-B.m11, A.m21-B.m21, A.m31-B.m31,
                             A.m12-B.m12, A.m22-B.m22, A.m32-B.m32,
                             A.m13-B.m13, A.m23-B.m23, A.m33-B.m33,
                             A.m14,       A.m23,       A.m34        ) );
  }
  
  inline AffineMapR3 operator- (const LinearMapR3& B, const AffineMapR3& A)
  {
      return( AffineMapR3( A.m11-B.m11, A.m21-B.m21, A.m31-B.m31,
                           A.m12-B.m12, A.m22-B.m22, A.m32-B.m32,
                           A.m13-B.m13, A.m23-B.m23, A.m33-B.m33,
                           A.m14,       A.m23,       A.m34        ) );
  }
  
  
  inline AffineMapR3& AffineMapR3::operator*= (double b)
  {
      m11 *= b;
      m12 *= b;
      m13 *= b;
      m21 *= b;
      m22 *= b;
      m23 *= b;
      m31 *= b;
      m32 *= b;
      m33 *= b;
      m14 *= b;
      m24 *= b;
      m34 *= b;
      return (*this);
  }
  
  inline AffineMapR3 operator* (const AffineMapR3& A, double b)
  {
      return(AffineMapR3( A.m11*b, A.m21*b, A.m31*b,
                          A.m12*b, A.m22*b, A.m32*b,
                          A.m13*b, A.m23*b, A.m33*b,
                          A.m14*b, A.m24*b, A.m34*b ) );
  }
  
  inline AffineMapR3 operator* (double b, const AffineMapR3& A)
  {
      return(AffineMapR3( A.m11*b, A.m21*b, A.m31*b,
                          A.m12*b, A.m22*b, A.m32*b,
                          A.m13*b, A.m23*b, A.m33*b,
                          A.m14*b, A.m24*b, A.m34*b ) );
  }
  
  inline AffineMapR3& AffineMapR3::operator/= (double b)
  {
      double bInv = 1.0/b;
      *this *= bInv;
      return( *this );
  }
  
  inline AffineMapR3 operator/ (const AffineMapR3& A, double b)
  {
      double bInv = 1.0/b;
      return(AffineMapR3( A.m11*bInv, A.m21*bInv, A.m31*bInv,
                          A.m12*bInv, A.m22*bInv, A.m32*bInv,
                          A.m13*bInv, A.m23*bInv, A.m33*bInv,
                          A.m14*bInv, A.m24*bInv, A.m34*bInv ) );
  }
  
  inline AffineMapR3& AffineMapR3::operator*= (const Matrix3x3& B)	// Composition
  {
      OperatorTimesEquals( B );
      return( *this );
  }
  
  inline AffineMapR3& AffineMapR3::operator*= (const Matrix3x4& B)	// Composition
  {
      OperatorTimesEquals( B );
      return( *this );
  }
  
  // **************************************************************
  // RotationMapR3 class (inlined functions)                      *
  // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * **
  
  inline RotationMapR3::RotationMapR3()
  {
      SetIdentity();
      return;
  }
  
  inline RotationMapR3::RotationMapR3( const VectorR3& u, const VectorR3& v,
                                       const VectorR3& s )
      :Matrix3x3 ( u, v, s )
  { }
  
  inline RotationMapR3::RotationMapR3(
      double a11, double a21, double a31,
      double a12, double a22, double a32,
      double a13, double a23, double a33)
  // Values specified in column order!!!
      :Matrix3x3 ( a11, a21, a31, a12, a22, a32, a13, a23, a33)
  { }
  
  inline RotationMapR3 RotationMapR3::Inverse() const		// Returns inverse
  {
      return ( RotationMapR3 ( m11, m12, m13,			// In column order!
                               m21, m22, m23,
                               m31, m32, m33 ) );
  }
  
  inline RotationMapR3& RotationMapR3::Invert()			// Converts into inverse.
  {
      double temp;
      temp = m12;
      m12 = m21;
      m21 = temp;
      temp = m13;
      m13 = m31;
      m31 = temp;
      temp = m23;
      m23 = m32;
      m32 = temp;
      return ( *this );
  }
  
  inline VectorR3 RotationMapR3::Solve(const VectorR3& u) const  // Returns solution
  {
      return( VectorR3( m11*u.x + m21*u.y + m31*u.z,
                        m12*u.x + m22*u.y + m32*u.z,
                        m13*u.x + m23*u.y + m33*u.z ) );
  }
  
  inline RotationMapR3& RotationMapR3::operator*= (const RotationMapR3& B)	// Matrix product
  {
      OperatorTimesEquals( B );
      return( *this );
  }
  
  
  // **************************************************************
  // RigidMapR3 class (inlined functions)		                    *
  // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * **
  
  inline RigidMapR3::RigidMapR3()
  {
      SetIdentity();
      return;
  }
  
  inline RigidMapR3::RigidMapR3( const VectorR3& u, const VectorR3& v,
                                 const VectorR3& s, const VectorR3& t )
      :Matrix3x4 ( u, v, s, t )
  {}
  
  inline RigidMapR3::RigidMapR3(
      double a11, double a21, double a31,
      double a12, double a22, double a32,
      double a13, double a23, double a33,
      double a14, double a24, double a34)
  // Values specified in column order!!!
      :Matrix3x4 ( a11, a21, a31, a12, a22, a32, a13, a23, a33, a14, a24, a34 )
  {}
  
  inline RigidMapR3::RigidMapR3( const Matrix3x3& A, const VectorR3& u )  // Set to RotationMap & Vector
      : Matrix3x4( A, u )
  {}
  
  
  inline RigidMapR3& RigidMapR3::Set( const Matrix3x3& A, const VectorR3& u )  // Set to RotationMap & Vector
  {
      Matrix3x4::Set( A, u );
      return *this;
  }
  
  inline RigidMapR3& RigidMapR3::SetTranslationPart( const VectorR3& u)	// Set the translation part
  {
      SetColumn4( u );
      return *this;
  }
  
  inline RigidMapR3& RigidMapR3::SetTranslationPart( double x, double y, double z)	// Set the translation part
  {
      SetColumn4( x, y, z );
      return *this;
  }
  
  inline RigidMapR3& RigidMapR3::SetRotationPart( const Matrix3x3& A)		// Set the rotation part
  {
      Matrix3x4::Set3x3( A );
      return *this;
  }
  
  inline RigidMapR3& RigidMapR3::operator*= (const RotationMapR3& B)	// Composition
  {
      OperatorTimesEquals( B );
      return( *this );
  }
  
  inline RigidMapR3& RigidMapR3::operator*= (const RigidMapR3& B)	// Composition
  {
      OperatorTimesEquals( B );
      return( *this );
  }
  
  inline RigidMapR3 RigidMapR3::Inverse() const		// Returns inverse
  {
      double new14 = -(m11*m14 + m21*m24 + m31*m34);
      double new24 = -(m12*m14 + m22*m24 + m32*m34);
      double new34 = -(m13*m14 + m23*m24 + m33*m34);
      return ( RigidMapR3 ( m11, m12, m13,			// In column order!
                            m21, m22, m23,
                            m31, m32, m33,
                            new14, new24, new34 ) );
  }
  
  inline RigidMapR3& RigidMapR3::Invert()			// Converts into inverse.
  {
      double new14 = -(m11*m14 + m21*m24 + m31*m34);
      double new24 = -(m12*m14 + m22*m24 + m32*m34);
      m34 = -(m13*m14 + m23*m24 + m33*m34);
      m14 = new14;
      m24 = new24;
  
      double temp;
      temp = m12;
      m12 = m21;
      m21 = temp;
      temp = m13;
      m13 = m31;
      m31 = temp;
      temp = m23;
      m23 = m32;
      m32 = temp;
      return ( *this );
  }
  
  // ***************************************************************
  // * 3-space vector and matrix utilities (inlined functions)	 *
  // * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
  
  // Returns the projection of u onto unit v
  inline VectorR3 ProjectToUnit ( const VectorR3& u, const VectorR3& v)
  {
      return (u^v)*v;
  }
  
  // Returns the projection of u onto the plane perpindicular to the unit vector v
  inline VectorR3 ProjectPerpUnit ( const VectorR3& u, const VectorR3& v)
  {
      return ( u - ((u^v)*v) );
  }
  
  // Returns the projection of u onto the plane perpindicular to the unit vector v
  //    This one is more stable when u and v are nearly equal.
  inline VectorR3 ProjectPerpUnitDiff ( const VectorR3& u, const VectorR3& v)
  {
      VectorR3 ans = u;
      ans -= v;
      ans -= ((ans^v)*v);
      return ans;				// ans = (u-v) - ((u-v)^v)*v
  }
  
  // VectorProjectMap returns map projecting onto a given vector u.
  //		u should be a unit vector (otherwise the returned map is
  //		scaled according to the magnitude of u.
  inline LinearMapR3 VectorProjectMap( const VectorR3& u )
  {
      double a = u.x*u.y;
      double b = u.x*u.z;
      double c = u.y*u.z;
      return( LinearMapR3( u.x*u.x,     a,     b,
                           a,    u.y*u.y,   c,
                           b,        c,   u.z*u.z ) );
  }
  
  // PlaneProjectMap returns map projecting onto a given plane.
  //		The plane is the plane orthognal to w.
  //		w must be a unit vector (otherwise the returned map is
  //		garbage).
  inline LinearMapR3 PlaneProjectMap ( const VectorR3& w )
  {
      double a = -w.x*w.y;
      double b = -w.x*w.z;
      double c = -w.y*w.z;
      return( LinearMapR3( 1.0-w.x*w.x,     a,         b,
                           a,        1.0-w.y*w.y,   c,
                           b,            c,       1.0-w.z*w.z ) );
  }
  
  // PlaneProjectMap returns map projecting onto a given plane.
  //		The plane is the plane containing the two orthonormal vectors u,v.
  //		If u, v are orthonormal, this is a projection with scaling.
  //		If they are not orthonormal, the results are more difficult
  //		to interpret.
  inline LinearMapR3 PlaneProjectMap ( const VectorR3& u, const VectorR3 &v )
  {
      double a = u.x*u.y + v.x*v.y;
      double b = u.x*u.z + v.x*v.z;
      double c = u.y*u.z + v.y*v.z;
      return( LinearMapR3( u.x*u.x+v.x*v.x,     a,            b,
                           a,           u.y*u.y+u.y*u.y,   c,
                           b,                c,         u.z*u.z+v.z*v.z ) );
  }
  
  // Returns the solid angle between unit vectors v and w.
  inline double SolidAngle( const VectorR3& v, const VectorR3& w)
  {
      return atan2 ( (v*w).Norm(), v^w );
  }
  
  
  #endif
  
  // ******************* End of header material ********************
  RotationMapR3 RefVecToMap(const VectorR3 & ref_axis);
  RigidMapR3 RefAxisPlusTransToMap(const VectorR3 & ref_axis, const VectorR3 & trans);
  