quaternions library

Mesh/stat_coordsys.h

+// -*-c++-*-
+#ifndef IG_STAT_COORDSYS_H
+#define IG_STAT_COORDSYS_H
+
+// $Id: stat_coordsys.h,v 1.1.2.6 2005/05/30 09:14:25 hkuiper Exp $
+
+// CwMtx matrix and vector math library
+// Copyright (C) 1999-2001  Harry Kuiper
+// Copyright (C) 2000  Will DeVore (template conversion)
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser 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
+
+#ifndef IG_STAT_SMATRIX_H
+#include "stat_smatrix.h"
+#endif
+
+#ifndef IG_STAT_QUATERNION_H
+#include "stat_quatern.h"
+#endif
+
+#ifndef IG_STAT_SVECTOR_H
+#include "stat_svector.h"
+#endif
+
+// CWTSSquareMatrix utilities for use in coordinate systems
+
+namespace CwMtx
+{
+
+  // NOTE: These template functions only work with floating point
+  // template arguments due to the use of sin(3), cos(3), tan(3), etc.
+
+  // PLEASE, READ THIS!!!
+
+  // Function smatFromEuler321(sclrAbout3, sclrAbout2, sclrAbout1)
+  // returns a transformation matrix which transforms coordinates from
+  // the reference axis system to coordinates in an axis system with
+  // Euler angles: sclrAbout3, sclrAbout2, sclrAbout1 relative to that
+  // reference axis system DO NOT CHANGE THIS DEFINITION!!!
+
+  // rotation about axis 3 (yaw angle), axis 2 (pitch angle) axis 1
+  // (roll angle)
+  template <class T>
+  CWTSSquareMatrix<3, T>
+  smatFromEuler321Angles(T sclrAbout3,
+			 T sclrAbout2,
+			 T sclrAbout1)
+  {
+    CWTSSquareMatrix<3, T> smat;
+
+    // to reduce the number of calls to CPU expensive sin(..) and
+    // cos(..) functions
+    T sin3 = sin(sclrAbout3);
+    T sin2 = sin(sclrAbout2);
+    T sin1 = sin(sclrAbout1);
+
+    T cos3 = cos(sclrAbout3);
+    T cos2 = cos(sclrAbout2);
+    T cos1 = cos(sclrAbout1);
+
+    // first row
+    smat[0][0] =  cos3*cos2;
+    smat[0][1] =  sin3*cos2;
+    smat[0][2] = -sin2;
+
+    // second row
+    smat[1][0] = -sin3*cos1 + cos3*sin2*sin1;
+    smat[1][1] =  cos3*cos1 + sin3*sin2*sin1;
+    smat[1][2] =  cos2*sin1;
+
+    // third row
+    smat[2][0] =  sin3*sin1 + cos3*sin2*cos1;
+    smat[2][1] = -cos3*sin1 + sin3*sin2*cos1;
+    smat[2][2] =  cos2*cos1;
+
+    return smat;
+  }
+
+  // Next three functions calculate Euler angles from a transformation matrix
+  // WARNING!!! They suffer from the ubiquitos singularities at 0, pi, 2*pi,
+  // etc.
+
+  // yaw angle, rotation angle about Z-axis
+  template <class T>
+  T
+  euler321Angle3FromSmat(const CWTSSquareMatrix<3, T> &smat)
+  {
+    return atan2(smat[0][1], smat[0][0]);
+  }
+
+  // pitch angle, rotation angle about Y-axis
+  template <class T>
+  T
+  euler321Angle2FromSmat(const CWTSSquareMatrix<3, T> &smat)
+  {
+    return asin(-smat[0][2]);
+  }
+
+  // roll angle, rotation angle about X-axis
+  template <class T>
+  T
+  euler321Angle1FromSmat(const CWTSSquareMatrix<3, T> &smat)
+  {
+    return atan2(smat[1][2], smat[2][2]);
+  }
+
+  // CWTSQuaternion utilities for use in coordinate systems
+
+  // PLEASE, READ THIS!!!
+
+  // next three functions calculate Euler angles from a quaternion
+  // that represents a rigid body's attitude WARNING!!! They do suffer
+  // from the ubiquitos singularities around 0, pi and 2*pi.
+
+  // yaw angle, rotation angle about Z-axis
+  template <class T>
+  T
+  euler321Angle3FromQtn(const CWTSQuaternion<T> &qtn)
+  {
+    // to reduce the number of calls to the subscript operator
+    T qtn0 = qtn[0];
+    T qtn1 = qtn[1];
+    T qtn2 = qtn[2];
+    T qtn3 = qtn[3];
+
+    // NOTE: Speed tip from Jeffrey T Duncan: use the identity: 1 =
+    // q0^2 + q1^2 + q2^2 + q3^2 which simplifies: q0^2 - q1^2 -
+    // q2^2 + q3^2 down to: 2*(q0^2 + q3^2) - 1
+    return atan2(2*(qtn0*qtn1 + qtn2*qtn3),
+		 2*(qtn0*qtn0 + qtn3*qtn3) - 1);
+  }
+
+  // pitch angle, rotation angle about Y-axis
+  template <class T>
+  T
+  euler321Angle2FromQtn(const CWTSQuaternion<T> &qtn)
+  {
+    return asin(-2*(qtn[0]*qtn[2] - qtn[1]*qtn[3]));
+  }
+
+  // roll angle, rotation angle about X-axis
+  template <class T>
+  T
+  euler321Angle1FromQtn(const CWTSQuaternion<T> &qtn)
+  {
+    // to reduce the number of calls to the subscript operator
+    T qtn0 = qtn[0];
+    T qtn1 = qtn[1];
+    T qtn2 = qtn[2];
+    T qtn3 = qtn[3];
+
+    return atan2(2*(qtn1*qtn2 + qtn0*qtn3),
+		 2*(qtn2*qtn2 + qtn3*qtn3) - 1);
+  }
+
+  // Function QtnFromEulerAxisAndAngle returns a quaternion
+  // representing a rigid body's attitude from its Euler axis and
+  // angle. NOTE: Argument vector svecEulerAxis MUST be a unit vector.
+  template <class T>
+  CWTSQuaternion<T>
+  qtnFromEulerAxisAndAngle(const CWTSSpaceVector<T> &svecEulerAxis,
+			   const T &sclrEulerAngle)
+  {
+    return CWTSQuaternion<T>(1, svecEulerAxis, 0.5*sclrEulerAngle);
+  }
+
+  // returns Euler axis
+  template <class T>
+  CWTSSpaceVector<T>
+  eulerAxisFromQtn(const CWTSQuaternion<T> &qtn)
+  {
+    T sclrTmp = sqrt(1 - qtn[3]*qtn[3]);
+
+    return CWTSSpaceVector<T>(qtn[0]/sclrTmp,
+			      qtn[1]/sclrTmp,
+			      qtn[2]/sclrTmp);
+  }
+
+  // returns Euler angle
+  template <class T>
+  T
+  eulerAngleFromQtn(const CWTSQuaternion<T> &qtn)
+  {
+    return 2*acos(qtn[3]);
+  }
+
+  // Function QtnFromEuler321 returns a quaternion representing a
+  // rigid body's attitude. The quaternion elements correspond to the
+  // Euler symmetric parameters of the body. The body's attitude must
+  // be entered in Euler angle representation with rotation order
+  // 3-2-1, i.e. first about Z-axis next about Y-axis and finally
+  // about X-axis.
+
+  // rotation about axis 3 (yaw angle), axis 2 (pitch angle) axis 1
+  // (roll angle)
+  template <class T>
+  CWTSQuaternion<T>
+  qtnFromEuler321Angles(T sclrAbout3,
+			T sclrAbout2,
+			T sclrAbout1)
+  {
+    return
+      CWTSQuaternion<T>(1, CWTSSpaceVector<T>(0, 0, 1), 0.5*sclrAbout3)
+      *CWTSQuaternion<T>(1, CWTSSpaceVector<T>(0, 1, 0), 0.5*sclrAbout2)
+      *CWTSQuaternion<T>(1, CWTSSpaceVector<T>(1, 0, 0), 0.5*sclrAbout1);
+  }
+
+  // SmatFromQtn returns the transformation matrix corresponding to a
+  // quaternion
+  template <class T>
+  CWTSSquareMatrix<3, T>
+  smatFromQtn(const CWTSQuaternion<T> &qtn)
+  {
+    CWTSSquareMatrix<3, T> smat;
+
+    // to reduce the number of calls to the subscript operator
+    T qtn0 = qtn[0];
+    T qtn1 = qtn[1];
+    T qtn2 = qtn[2];
+    T qtn3 = qtn[3];
+
+    smat[0][0] = 1 - 2*(qtn1*qtn1 + qtn2*qtn2);
+    smat[0][1] = 2*(qtn0*qtn1 + qtn2*qtn3);
+    smat[0][2] = 2*(qtn0*qtn2 - qtn1*qtn3);
+
+    smat[1][0] = 2*(qtn0*qtn1 - qtn2*qtn3);
+    smat[1][1] = 1 - 2*(qtn0*qtn0 + qtn2*qtn2);
+    smat[1][2] = 2*(qtn1*qtn2 + qtn0*qtn3);
+
+    smat[2][0] = 2*(qtn0*qtn2 + qtn1*qtn3);
+    smat[2][1] = 2*(qtn1*qtn2 - qtn0*qtn3);
+    smat[2][2] = 1 - 2*(qtn0*qtn0 + qtn1*qtn1);
+
+    return smat;
+  }
+
+  // QtnFromSmat returns the quaternion corresponding to a
+  // transformation matrix
+  template <class T>
+  CWTSQuaternion<T>
+  qtnFromSmat(const CWTSSquareMatrix<3, T> &smat)
+  {
+    // Check trace to prevent loss of accuracy
+    T sclrTmp = tr(smat);
+    CWTSQuaternion<T> qtn;
+
+    if(sclrTmp > 0)
+      {
+	// No trouble, we can use standard expression
+	sclrTmp = 0.5*sqrt(1 + sclrTmp);
+	qtn[0] = 0.25*(smat[1][2] - smat[2][1])/sclrTmp;
+	qtn[1] = 0.25*(smat[2][0] - smat[0][2])/sclrTmp;
+	qtn[2] = 0.25*(smat[0][1] - smat[1][0])/sclrTmp;
+	qtn[3] = sclrTmp;
+      }
+    else
+      {
+	// We could be in trouble, so we have to take
+	// precautions. NOTE: Parts of this piece of code were taken
+	// from the example in the article "Rotating Objects Using
+	// Quaternions" in Game Developer Magazine written by Nick
+	// Bobick, july 3, 1998, vol. 2, issue 26.
+	int i = 0;
+	if (smat[1][1] > smat[0][0]) i = 1;
+	if (smat[2][2] > smat[i][i]) i = 2;
+
+	switch (i)
+	  {
+	  case 0:
+	    sclrTmp = 0.5*sqrt(1 + smat[0][0] - smat[1][1] - smat[2][2]);
+	    qtn[0] = sclrTmp;
+	    qtn[1] = 0.25*(smat[0][1] + smat[1][0])/sclrTmp;
+	    qtn[2] = 0.25*(smat[0][2] + smat[2][0])/sclrTmp;
+	    qtn[3] = 0.25*(smat[1][2] - smat[2][1])/sclrTmp;
+	    break;
+
+	  case 1:
+	    sclrTmp = 0.5*sqrt(1 - smat[0][0] + smat[1][1] - smat[2][2]);
+	    qtn[0] = 0.25*(smat[1][0] + smat[0][1])/sclrTmp;
+	    qtn[1] = sclrTmp;
+	    qtn[2] = 0.25*(smat[1][2] + smat[2][1])/sclrTmp;
+	    qtn[3] = 0.25*(smat[2][0] - smat[0][2])/sclrTmp;
+	    break;
+
+	  case 2:
+	    sclrTmp = 0.5*sqrt(1 - smat[0][0] - smat[1][1] + smat[2][2]);
+	    qtn[0] = 0.25*(smat[2][0] + smat[0][2])/sclrTmp;
+	    qtn[1] = 0.25*(smat[2][1] + smat[1][2])/sclrTmp;
+	    qtn[2] = sclrTmp;
+	    qtn[3] = 0.25*(smat[0][1] - smat[1][0])/sclrTmp;
+
+	    break;
+	  }
+      }
+
+    return qtn;
+  }
+
+  // NOTE: This function duplicates eulerAxisFromQtn(qtn) it only has
+  // a different return type.
+  template <class T>
+  CWTSVector<3, T>
+  axisAngleFromQtn( const CWTSQuaternion<T> &qtn )
+  {
+    return eulerAxisFromQtn(qtn);
+  }
+
+  // NOTE: This function duplicates qtnFromEulerAxisAndAngle(..) it
+  // only has a different function profile.
+  template <class T>
+  CWTSQuaternion<T>
+  qtnFromAxisAndAngle(const CWTSVector<3, T> &vAxis,
+                      const T sAngle)
+  {
+    return qtnFromEulerAxisAndAngle(vAxis, sAngle);
+  }
+
+  template <class T>
+  CWTSSquareMatrix<3, T>
+  changeOfBasis(CWTSSpaceVector< CWTSSpaceVector<T> >&from, 
+		CWTSSpaceVector< CWTSSpaceVector<T> >&to)
+  {
+    CWTSSquareMatrix<3, T> A;
+
+    // This is a "change of basis" from the "from" frame to the "to"
+    // frame.  the "to" frame is typically the "Standard basis"
+    // frame.  The "from" is a frame that represents the current
+    // orientation of the object in the shape of an orthonormal
+    // tripod.
+
+    enum { x , y , z };
+
+    // _X,_Y,_Z is typically the standard basis frame.
+
+    //  | x^_X , y^_X , z^_X |
+    //  | x^_Y , y^_Y , z^_Y |
+    //  | x^_Z , y^_Z , z^_Z |
+	
+    A[0][0] = from[x]*to[x];
+    A[0][1] = from[y]*to[x];
+    A[0][2] = from[z]*to[x];
+
+    A[1][0] = from[x]*to[y];
+    A[1][1] = from[y]*to[y];
+    A[1][2] = from[z]*to[y];
+
+    A[2][0] = from[x]*to[z];
+    A[2][1] = from[y]*to[z];
+    A[2][2] = from[z]*to[z];
+	
+    // This is simply the transpose done manually which would save
+    // and inverse call.
+    //	| x ^ _X , x ^ _Y , x ^ _Z |
+    //	| y ^ _X , y ^ _Y , y ^ _Z |
+    //	| z ^ _X , z ^ _Y , z ^ _Z |
+	
+    // And finally we return the matrix that takes the "from" frame
+    // to the "to"/"Standard basis" frame.
+    return A;
+  }
+
+}
+
+#endif   // IG_STAT_COORDSYS_H

Mesh/stat_cwmtx.h

+// -*-c++-*-
+#ifndef IG_STAT_CWMTX_H
+#define IG_STAT_CWMTX_H
+
+// $Id: stat_cwmtx.h,v 1.1.2.1 2002/01/15 01:25:21 hkuiper Exp $
+
+// CwMtx matrix and vector math library
+// Copyright (C) 1999  Harry Kuiper
+// Copyright (C) 2000  Will DeVore (template conversion)
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser 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
+
+//---------------------------------------------------------------------------
+//
+// Master include file for CWTSMatrix and CWTSVector classes
+//
+//---------------------------------------------------------------------------
+
+#ifndef IG_STAT_MATRIX_H
+#include "stat_matrix.h"
+#endif
+
+#ifndef IG_STAT_SMATRIX_H
+#include "stat_smatrix.h"
+#endif
+
+#ifndef IG_STAT_VECTOR_H
+#include "stat_vector.h"
+#endif
+
+#ifndef IG_STAT_SVECTOR_H
+#include "stat_svector.h"
+#endif
+
+#ifndef IG_STAT_QUATERN_H
+#include "stat_quatern.h"
+#endif
+
+#ifndef IG_STAT_COORDSYS_H
+#include "stat_coordsys.h"
+#endif
+
+#endif   // IG_STAT_CWMTX_H

Mesh/stat_matrix.h

+// -*-c++-*-
+#ifndef IG_STAT_MATRIX_H
+#define IG_STAT_MATRIX_H
+
+// $Id: stat_matrix.h,v 1.1.2.12 2005/07/02 15:41:46 hkuiper Exp $
+
+// CwMtx matrix and vector math library
+// Copyright (C) 1999-2001  Harry Kuiper
+// Copyright (C) 2000  Will DeVore (template conversion)
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser 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
+
+#include <iostream>
+
+// CWMatrix class
+
+// This library was designed to mirror as closely as possible the
+// notation used in mathematical writing. A matrix is indexed:
+// matMatrixName[row][col].
+
+// CAUTION!!!
+
+// This matrix library was implemented with emphasis on
+// speed. Consequently no attempts were made to trap and report
+// errors. It is left entirely to the user to write code that does not
+// cause errors within the matrix routines.
+
+namespace CwMtx
+{
+  using std::ostream;
+  
+  // forward declarations
+
+  template <unsigned r, unsigned c, class T>
+  class CWTSMatrix;
+
+  template <unsigned r, class T>
+  class CWTSSquareMatrix;
+
+  template <unsigned r, class T>
+  class CWTSVector;
+
+  template <class T>
+  class CWTSSpaceVector;
+
+  template <class T>
+  class CWTSQuaternion;
+
+  // Template classes that create zero "objects".
+
+  template <class T>
+  class CWTSZero
+  {
+  public:
+    operator T() { return 0; }
+  };
+
+  template <unsigned r, unsigned c, class T>
+  class CWTSZero< CWTSMatrix<r, c, T> >:
+    public CWTSMatrix<r, c, T>
+  {
+  public:
+    CWTSZero() { fill(CWTSZero<T>()); }
+  };
+
+  template <unsigned r, class T>
+  class CWTSZero< CWTSSquareMatrix<r, T> >:
+    public CWTSSquareMatrix<r, T>
+  {
+  public:
+    CWTSZero() { fill(CWTSZero<T>()); }
+  };
+
+  template <unsigned r, class T>
+  class CWTSZero< CWTSVector<r, T> >:
+    public CWTSVector<r, T>
+  {
+  public:
+    CWTSZero() { fill(CWTSZero<T>()); }
+  };
+
+  template <class T>
+  class CWTSZero< CWTSSpaceVector<T> >:
+    public CWTSSpaceVector<T>
+  {
+  public:
+    CWTSZero() { fill(CWTSZero<T>()); }
+  };
+
+  template <class T>
+  class CWTSZero< CWTSQuaternion<T> >:
+    public CWTSQuaternion<T>
+  {
+  public:
+    CWTSZero() { fill(CWTSZero<T>()); }
+  };
+
+  // Template classes that create unity "objects".
+
+  template <class T>
+  class CWTSUnity
+  {
+  public:
+    operator T() { return 1; }
+  };
+
+  // NOTE: Only a matrix with an equal number of rows and columns can
+  // be a unity matrix.
+  template <unsigned r, class T>
+  class CWTSUnity< CWTSMatrix<r, r, T> >:
+    public CWTSSquareMatrix<r, T>
+  {
+  public:
+    CWTSUnity() { this->makeUnity(); }
+  };
+
+  template <unsigned r, class T>
+  class CWTSUnity< CWTSSquareMatrix<r, T> >:
+    public CWTSSquareMatrix<r, T>
+  {
+  public:
+    CWTSUnity() { this->makeUnity(); }
+  };
+
+  template <class T>
+  class CWTSUnity< CWTSQuaternion<T> >:
+    public CWTSQuaternion<T>
+  {
+  public:
+    CWTSUnity()
+    {
+      (*this)[0] = (*this)[1] = (*this)[2] = CWTSZero<T>();
+      (*this)[3] = CWTSUnity<T>();
+    }
+  };
+
+  // This template defaults to double. Most of the time this template
+  // will be working with math functions that only work with
+  // doubles. For example, the transcendental function sin(x) takes
+  // and returns a double which would force the compiler to convert
+  // back and forth from some other data type to a double.
+
+  // prefix mat
+  template <unsigned r, unsigned c, class T = double>
+  class CWTSMatrix
+  {
+  public:
+    typedef T element;
+
+    CWTSMatrix() {}
+    CWTSMatrix(const CWTSMatrix &);
+
+    ~CWTSMatrix() {}
+
+    unsigned getRows() const { return r; }
+    unsigned getCols() const { return c; }
+
+    // basic matrix operations
+
+    // returns a row of modifyable elements
+    T* operator [](unsigned irow) { return m_rgrow[irow]; }
+    // returns a row of non-modifyable elements
+    const T* operator [](unsigned irow) const { return m_rgrow[irow]; }
+
+    CWTSMatrix operator +(const CWTSMatrix &) const;
+    CWTSMatrix operator -(const CWTSMatrix &) const;
+    CWTSMatrix operator -() const;
+    CWTSMatrix operator *(const T &) const;
+    CWTSMatrix operator /(const T &value) const;
+
+    // not inherited
+    CWTSMatrix & operator =(const CWTSMatrix &);
+    CWTSMatrix & operator +=(const CWTSMatrix &);
+    CWTSMatrix & operator -=(const CWTSMatrix &);
+    CWTSMatrix & operator *=(const T &);
+    CWTSMatrix & operator /=(const T &value);
+
+    bool operator ==(const CWTSMatrix &) const;
+    bool operator !=(const CWTSMatrix &mat) const;
+
+    void interchangeRows(unsigned, unsigned);
+    void addRowToRow(unsigned, unsigned, const T & = CWTSUnity<T>());
+    void multiplyRow(unsigned, const T &);
+
+    // fills the whole array with a value.
+    void fill(const T &);
+    // stores matrix + matrix in this
+    void storeSum(const CWTSMatrix &, const CWTSMatrix &);
+    // stores CWMatrix at indicated position in this
+    template <unsigned r2, unsigned c2>
+    void storeAtPosition(unsigned, unsigned, const CWTSMatrix<r2, c2, T> &);
+
+  private:
+    // an array of rows
+    T m_rgrow[r][c];
+  };
+
+  template <unsigned r, unsigned c, class T >
+  inline CWTSMatrix<r, c, T>::CWTSMatrix(const CWTSMatrix<r, c, T> &mat)
+  {
+    // copy contents of input matrix
+    (*this) = mat;
+  }
+
+  template <unsigned r, unsigned c, class T>
+  CWTSMatrix<r, c, T> 
+  CWTSMatrix<r, c, T>::operator +(const CWTSMatrix<r, c, T> &mat) const
+  {
+    // copy this and add argument
+    return CWTSMatrix(*this) += mat;
+  }
+
+  template <unsigned r, unsigned c, class T>
+  CWTSMatrix<r, c, T> 
+  CWTSMatrix<r, c, T>::operator -(const CWTSMatrix<r, c, T> &mat) const
+  {
+    // copy this and subtract argument
+    return CWTSMatrix(*this) -= mat;
+  }
+
+  template <unsigned r, unsigned c, class T>
+  CWTSMatrix<r, c, T>
+  CWTSMatrix<r, c, T>::operator -() const
+  {
+    return (*this)*(CWTSZero<T>() - CWTSUnity<T>());
+  }
+
+  template <unsigned r, unsigned c, class T>
+  CWTSMatrix<r, c, T>
+  CWTSMatrix<r, c, T>::operator *(const T &value) const
+  {
+    // copy this and multiply by argument
+    return CWTSMatrix(*this) *= value;
+  }
+
+  template <unsigned r, unsigned c, class T>
+  inline CWTSMatrix<r, c, T>
+  CWTSMatrix<r, c, T>::operator /(const T &value) const
+  {
+    return (*this)*(CWTSUnity<T>()/value);
+  }
+
+  // assignment operator
+  template <unsigned r, unsigned c, class T>
+  CWTSMatrix<r, c, T> & 
+  CWTSMatrix<r, c, T>::operator =(const CWTSMatrix<r, c, T> &mat)
+  {
+    for (unsigned irow = 0; irow < r; ++irow)
+      {
+	for (unsigned icol = 0; icol < c; ++icol)
+	  {
+	    m_rgrow[irow][icol] = mat.m_rgrow[irow][icol];
+	  }
+      }
+
+    return (*this);
+  }
+
+  template <unsigned r, unsigned c, class T>
+  CWTSMatrix<r, c, T> & 
+  CWTSMatrix<r, c, T>::operator +=(const CWTSMatrix<r, c, T> &mat)
+  {
+    for (unsigned irow = 0; irow < r; ++irow)
+      {
+	for (unsigned icol = 0; icol < c; ++icol)
+	  {
+	    m_rgrow[irow][icol] += mat.m_rgrow[irow][icol];
+	  }
+      }
+
+    return (*this);
+  }
+
+  template <unsigned r, unsigned c, class T>
+  CWTSMatrix<r, c, T> & 
+  CWTSMatrix<r, c, T>::operator -=(const CWTSMatrix<r, c, T> &mat)
+  {
+    for (unsigned irow = 0; irow < r; ++irow)
+      {
+	for (unsigned icol = 0; icol < c; ++icol)
+	  {
+	    m_rgrow[irow][icol] -= mat.m_rgrow[irow][icol];
+	  }
+      }
+
+    return (*this);
+  }
+
+  template <unsigned r, unsigned c, class T>
+  CWTSMatrix<r, c, T> &
+  CWTSMatrix<r, c, T>::operator *=(const T &value)
+  {
+    for (unsigned irow = 0; irow < r; ++irow)
+      {
+	for (unsigned icol = 0; icol < c; ++icol)
+	  {
+	    m_rgrow[irow][icol] *= value;
+	  }
+      }
+
+    return (*this);
+  }
+
+  template <unsigned r, unsigned c, class T>
+  inline CWTSMatrix<r, c, T> &
+  CWTSMatrix<r, c, T>::operator /=(const T &value)
+  {
+    return (*this) *= CWTSUnity<T>()/value;
+  }
+
+  template <unsigned r, unsigned c, class T>
+  bool
+  CWTSMatrix<r, c, T>::operator ==(const CWTSMatrix<r, c, T> &mat) const
+  {
+    for (unsigned irow = 0; irow < r; ++irow)
+      {
+	for (unsigned icol = 0; icol < c; ++icol)
+	  {
+	    if (m_rgrow[irow][icol] != mat.m_rgrow[irow][icol])
+	      {
+		return false;
+	      }
+	  }
+      }
+
+    return true;
+  }
+
+  template <unsigned r, unsigned c, class T>
+  inline bool
+  CWTSMatrix<r, c, T>::operator !=(const CWTSMatrix &mat) const
+  {
+    return !( (*this) == mat );
+  }
+
+  template <unsigned r, unsigned c, class T>
+  void
+  CWTSMatrix<r, c, T>::fill(const T &elemFill)
+  {
+    for (unsigned irow = 0; irow < r; ++irow)
+      {
+	for (unsigned icol = 0; icol < c; ++icol)
+	  {
+	    m_rgrow[irow][icol] = elemFill;
+	  }
+      }
+  }
+
+  // stores mat1 + mat2 in this
+  template <unsigned r, unsigned c, class T>
+  void
+  CWTSMatrix<r, c, T>::storeSum(const CWTSMatrix<r, c, T> &mat1,
+				const CWTSMatrix<r, c, T> &mat2)
+  {
+    for (unsigned irow = 0; irow < r; ++irow)
+      {
+	for (unsigned icol = 0; icol < c; ++icol)
+	  {
+	    m_rgrow[irow][icol] = 
+	      mat1.m_rgrow[irow][icol] + mat2.m_rgrow[irow][icol];
+	  }
+      }
+  }
+
+  template <unsigned r, unsigned c, class T>
+  template <unsigned r2, unsigned c2>
+  void
+  CWTSMatrix<r, c, T>::storeAtPosition(unsigned irowStart,
+				       unsigned icolStart,
+				       const CWTSMatrix<r2, c2, T> &mat)
+  {
+    for (unsigned irow = 0; irow < r2; ++irow)
+      {
+	for (unsigned icol = 0; icol < c2; ++icol)
+	  {
+	    m_rgrow[irow + irowStart][icol + icolStart] = mat[irow][icol];
+	  }
+      }
+  }
+
+  template <unsigned r, unsigned c, class T>
+  void
+  CWTSMatrix<r, c, T>::interchangeRows(unsigned irow1, unsigned irow2)
+  {
+    T elemTmp;
+
+    for (unsigned icol = 0; icol < c; ++icol)
+      {
+	elemTmp = m_rgrow[irow1][icol];
+	m_rgrow[irow1][icol] = m_rgrow[irow2][icol];
+	m_rgrow[irow2][icol] = elemTmp;
+      }
+  }
+
+  template <unsigned r, unsigned c, class T>
+  void
+  CWTSMatrix<r, c, T>::multiplyRow(unsigned irow, const T &value)
+  {
+    for (unsigned icol = 0; icol < c; ++icol)
+      {
+	m_rgrow[irow][icol] *= value;
+      }
+  }
+
+  template <unsigned r, unsigned c, class T>
+  void
+  CWTSMatrix<r, c, T>::addRowToRow(unsigned irowSrc,
+				   unsigned irowDest,
+				   const T &value)
+  {
+    for (unsigned icol = 0; icol < c; ++icol)
+      {
+	m_rgrow[irowDest][icol] += m_rgrow[irowSrc][icol]*value;
+      }
+  }
+
+  // template functions designed to work with the base matrix class
+
+  template <unsigned r, unsigned c, class T>
+  inline CWTSMatrix<r, c, T>
+  operator *(const T &value, const CWTSMatrix<r, c, T> &mat)
+  {
+    return (mat*value);
+  }
+
+  template <unsigned r, unsigned c, unsigned c2, class T>
+  CWTSMatrix<r, c2, T>
+  operator *(CWTSMatrix<r, c, T> mat1, const CWTSMatrix<c, c2, T> &mat2)
+  {
+    CWTSMatrix<r, c2, T> matResult;
+
+    for (unsigned irow = 0; irow < r; ++irow)
+      {
+	for (unsigned icol = 0; icol < c2; ++icol)
+	  {
+	    matResult[irow][icol] = CWTSZero<T>();
+
+	    for (unsigned icol2 = 0; icol2 < c; ++icol2)
+	      {
+		matResult[irow][icol] += mat1[irow][icol2]*mat2[icol2][icol];
+	      }
+	  }
+      }
+
+    return matResult;
+  }
+
+  template <unsigned r, unsigned c, class T>
+  CWTSMatrix<r, c, T>
+  transpose(const CWTSMatrix<c, r, T> &mat)
+  {
+    CWTSMatrix<r, c, T> matTranspose;
+
+    for (unsigned irow = 0; irow < r; ++irow)
+      {
+	for (unsigned icol = 0; icol < c; ++icol)
+	  {
+	    matTranspose[irow][icol] = mat[icol][irow];
+	  }
+      }
+
+    return matTranspose;
+  }
+
+  template <unsigned r, unsigned c, class T>
+  ostream &
+  operator <<(ostream &os, const CWTSMatrix<r, c, T> &mtx)
+  {
+    os << "[" ;
+
+    for (unsigned i = 0; i < r; i++)
+      {
+	if (i > 0)
+	  {
+	    os << "; ";
+	  }
+
+        os << mtx[i][0];
+
+	for (unsigned j = 1; j < c; j++)
+	  {
+	    os << ", " << mtx[i][j];
+	  }
+      }
+
+    os << "]";
+
+    return os;
+  }
+
+}
+
+#endif // IG_STAT_MATRIX_H
+

Mesh/stat_quatern.h

+// -*-c++-*-
+#ifndef IG_STAT_QUATERN_H
+#define IG_STAT_QUATERN_H
+
+// $Id: stat_quatern.h,v 1.1.2.8 2005/07/02 15:41:46 hkuiper Exp $
+
+// CwMtx matrix and vector math library
+// Copyright (C) 1999-2001  Harry Kuiper
+// Copyright (C) 2000  Will DeVore (template conversion)
+// Copyright (C) 2000  Jiri Ecer (constructor from exponential form)
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser 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
+
+#ifndef IG_STAT_SVECTOR_H
+#include "stat_svector.h"
+#endif
+
+namespace CwMtx
+{
+  using std::exp;
+  using std::log;
+
+  // prefix qtn
+  template <class T = double>
+  class CWTSQuaternion : public CWTSVector<4, T>
+  {
+  public:
+    CWTSQuaternion(): CWTSVector<4, T>() {}
+    CWTSQuaternion(const CWTSMatrix<4, 1, T> &mat): CWTSVector<4, T>(mat) {}
+    // assumption: vec has four elements
+    CWTSQuaternion(const CWTSVector<4, T> &vec): CWTSVector<4, T>(vec) {}
+    CWTSQuaternion(const CWTSQuaternion &qtn): CWTSVector<4, T>(qtn) {}
+    // the space vector will become the quaternion's imaginary part, T
+    // will become its real part
+    CWTSQuaternion(const CWTSSpaceVector<T> &, const T & = CWTSZero<T>());
+    // creates a quaternion, index runs from left to right
+    CWTSQuaternion(const T &, const T &, const T &, const T &);
+    // creates a quaternion from a scalar
+    CWTSQuaternion(const T &);
+
+    // constructor from exponential form: q = r*exp(svec*angle), svec
+    // should be a unity vector, angle is in radians
+    CWTSQuaternion(const T &r, const CWTSSpaceVector<T> &svec, const T &angle);
+
+    ~CWTSQuaternion() {}
+
+    CWTSQuaternion operator +(const CWTSQuaternion &) const;
+    CWTSQuaternion operator -(const CWTSQuaternion &) const;
+    CWTSQuaternion operator -() const;
+    CWTSQuaternion operator *(const T &) const;
+    CWTSQuaternion operator *(const CWTSQuaternion &) const;
+    CWTSQuaternion operator /(const T &value) const;
+    CWTSQuaternion operator /(const CWTSQuaternion &) const;
+
+    // not inherited
+    CWTSQuaternion & operator =(const CWTSQuaternion &);
+    CWTSQuaternion & operator +=(const CWTSQuaternion &);
+    CWTSQuaternion & operator -=(const CWTSQuaternion &);
+    CWTSQuaternion & operator *=(const T &);
+    CWTSQuaternion & operator *=(const CWTSQuaternion &);
+    CWTSQuaternion & operator /=(const T &);
+    CWTSQuaternion & operator /=(const CWTSQuaternion &);
+
+    // stores product of qtn*qtn in this
+    void storeProduct(const CWTSQuaternion &, const CWTSQuaternion &);
+    // stores conjugate of argument in this
+    void storeConjugate(const CWTSQuaternion &);
+    // makes this its own conjugate
+    void makeConjugate();
+
+    // returns a unit quaternion with same direction as this
+    CWTSQuaternion unit() const { return (*this)/(this->norm()); }
+  };
+
+  template <class T>
+  inline CWTSQuaternion<T>::CWTSQuaternion(const T &elemIm0,
+					   const T &elemIm1,
+					   const T &elemIm2,
+					   const T &elemReal)
+    :
+    CWTSVector<4, T>()
+  {
+    (*this)[0] = elemIm0;
+    (*this)[1] = elemIm1;
+    (*this)[2] = elemIm2;
+    (*this)[3] = elemReal;
+  }
+
+  template <class T>
+  inline CWTSQuaternion<T>::CWTSQuaternion(const T &elemReal)
+  {
+    (*this)[0] = CWTSZero<T>();
+    (*this)[1] = CWTSZero<T>();
+    (*this)[2] = CWTSZero<T>();
+    (*this)[3] = elemReal;
+  }
+
+  template <class T>
+  inline CWTSQuaternion<T>::CWTSQuaternion(const CWTSSpaceVector<T> &svecIm,
+					   const T &elemReal)
+    :
+    CWTSVector<4, T>()
+  {
+    (*this)[0] = svecIm[0];
+    (*this)[1] = svecIm[1];
+    (*this)[2] = svecIm[2];
+    (*this)[3] = elemReal;
+  }
+
+  // NOTE: This only works with template arguments that can be
+  // converted to floating point types due to the use of sin(3) and
+  // cos(3).
+  template <class T>
+  CWTSQuaternion<T>::CWTSQuaternion(const T &r,
+				    const CWTSSpaceVector<T> &svec,
+				    const T &angle)
+    :
+    CWTSVector<4, T>()
+  {
+    T rsina = r*sin(angle);
+
+    (*this)[0] = svec[0]*rsina;
+    (*this)[1] = svec[1]*rsina;
+    (*this)[2] = svec[2]*rsina;
+    (*this)[3] = r*cos(angle);
+  }
+
+  // not inherited
+  template <class T>
+  inline CWTSQuaternion<T> &
+  CWTSQuaternion<T>::operator =(const CWTSQuaternion<T> &qtn)
+  {
+    return static_cast<CWTSQuaternion &>(CWTSMatrix<4, 1, T>::operator=(qtn));
+  }
+
+  template <class T>
+  inline CWTSQuaternion<T> &
+  CWTSQuaternion<T>::operator +=(const CWTSQuaternion<T> &qtn)
+  {
+    return static_cast<CWTSQuaternion &>(CWTSMatrix<4, 1, T>::operator+=(qtn));
+  }
+
+  template <class T>
+  inline CWTSQuaternion<T> &
+  CWTSQuaternion<T>::operator -=(const CWTSQuaternion<T> &qtn)
+  {
+    return static_cast<CWTSQuaternion &>(CWTSMatrix<4, 1, T>::operator-=(qtn));
+  }
+
+  template <class T>
+  inline CWTSQuaternion<T> &
+  CWTSQuaternion<T>::operator *=(const T &value)
+  {
+    return
+      static_cast<CWTSQuaternion &>(CWTSMatrix<4, 1, T>::operator*=(value));
+  }
+
+  template <class T>
+  inline CWTSQuaternion<T> &
+  CWTSQuaternion<T>::operator *=(const CWTSQuaternion<T> &qtn)
+  {
+    return (*this) = (*this)*qtn;
+  }
+
+  template <class T>
+  inline CWTSQuaternion<T> &
+  CWTSQuaternion<T>::operator /=(const T &value)
+  {
+    return (*this) *= CWTSUnity<T>()/value;
+  }
+
+  template <class T>
+  inline CWTSQuaternion<T> &
+  CWTSQuaternion<T>::operator /=(const CWTSQuaternion<T> &qtn)
+  {
+    return (*this) = (*this)/qtn;
+  }
+
+  template <class T>
+  CWTSQuaternion<T>
+  CWTSQuaternion<T>::operator +(const CWTSQuaternion<T> &qtn) const
+  {
+    return CWTSQuaternion(*this) += qtn;
+  }
+
+  template <class T>
+  CWTSQuaternion<T>
+  CWTSQuaternion<T>::operator -(const CWTSQuaternion<T> &qtn) const
+  {
+    return CWTSQuaternion(*this) -= qtn;
+  }
+
+  template <class T>
+  CWTSQuaternion<T>
+  CWTSQuaternion<T>::operator -() const
+  {
+    return (*this)*(CWTSZero<T>() - CWTSUnity<T>());
+  }
+
+  template <class T>
+  CWTSQuaternion<T>
+  CWTSQuaternion<T>::operator *(const T &value) const
+  {
+    return CWTSQuaternion(*this) *= value;
+  }
+
+  template <class T>
+  inline CWTSQuaternion<T>
+  CWTSQuaternion<T>::operator /(const T &value) const
+  {
+    return (*this)*(CWTSUnity<T>()/value);
+  }
+
+  template <class T>
+  void
+  CWTSQuaternion<T>::storeProduct(const CWTSQuaternion<T> &qtnLeft,
+				  const CWTSQuaternion<T> &qtnRight)
+  {
+    // to reduce the number of calls to the subscript operator
+    T qtnLeft0 = qtnLeft[0];
+    T qtnLeft1 = qtnLeft[1];
+    T qtnLeft2 = qtnLeft[2];
+    T qtnLeft3 = qtnLeft[3];
+
+    T qtnRight0 = qtnRight[0];
+    T qtnRight1 = qtnRight[1];
+    T qtnRight2 = qtnRight[2];
+    T qtnRight3 = qtnRight[3];
+
+    (*this)[0] =
+      qtnLeft0*qtnRight3 + qtnLeft1*qtnRight2
+      - qtnLeft2*qtnRight1 + qtnLeft3*qtnRight0;
+
+    (*this)[1] =
+      -qtnLeft0*qtnRight2 + qtnLeft1*qtnRight3
+      + qtnLeft2*qtnRight0 + qtnLeft3*qtnRight1;
+
+    (*this)[2] =
+      qtnLeft0*qtnRight1 - qtnLeft1*qtnRight0
+      + qtnLeft2*qtnRight3 + qtnLeft3*qtnRight2;
+
+    (*this)[3] =
+      -qtnLeft0*qtnRight0 - qtnLeft1*qtnRight1
+      - qtnLeft2*qtnRight2 + qtnLeft3*qtnRight3;
+  }
+
+  template <class T>
+  CWTSQuaternion<T>
+  CWTSQuaternion<T>::operator *(const CWTSQuaternion<T> &qtn) const
+  {
+    CWTSQuaternion qtnResult;
+    qtnResult.storeProduct(*this, qtn);
+    return qtnResult;
+  }
+
+  template <class T>
+  CWTSQuaternion<T>
+  CWTSQuaternion<T>::operator /(const CWTSQuaternion<T> &qtn) const
+  {
+    return (*this)*inv(qtn);
+  }
+
+  // stores conjugate of argument in this
+  template <class T>
+  void
+  CWTSQuaternion<T>::storeConjugate(const CWTSQuaternion<T> &qtn)
+  {
+    // copy argument into this
+    (*this) = qtn;
+    makeConjugate();
+  }
+
+  template <class T>
+  void
+  CWTSQuaternion<T>::makeConjugate()
+  {
+    T tmp = CWTSZero<T>() - CWTSUnity<T>();
+
+    (*this)[0] *= tmp;
+    (*this)[1] *= tmp;
+    (*this)[2] *= tmp;
+  }
+
+  // template functions designed work with the Quaternion class.
+
+  template <class T>
+  inline CWTSQuaternion<T>
+  operator *(const T &value, const CWTSQuaternion<T> &qtn)
+  {
+    return qtn*value;
+  }
+
+  // return real part of a quaternion
+  template <class T>
+  inline T
+  re(const CWTSQuaternion<T> &qtn)
+  {
+    return qtn[3];
+  }
+
+  // returns imaginary (vector) part of a quaternion
+  template <class T>
+  CWTSSpaceVector<T>
+  im(const CWTSQuaternion<T> &qtn)
+  {
+    return CWTSSpaceVector<T>(qtn[0], qtn[1], qtn[2]);
+  }
+
+  // the conjugate
+  template <class T>
+  CWTSQuaternion<T>
+  conj(const CWTSQuaternion<T> &qtn)
+  {
+    // copy input quaternion
+    CWTSQuaternion<T> qtnResult(qtn);
+    qtnResult.makeConjugate();
+    return qtnResult;
+  }
+
+  // the inverse
+  template <class T>
+  CWTSQuaternion<T>
+  inv(const CWTSQuaternion<T> &qtn)
+  {
+    T qtn0 = qtn[0];
+    T qtn1 = qtn[1];
+    T qtn2 = qtn[2];
+    T qtn3 = qtn[3];
+
+    return conj(qtn)/static_cast<const T &>(qtn0*qtn0
+                                            + qtn1*qtn1
+                                            + qtn2*qtn2
+                                            + qtn3*qtn3);
+  }
+
+  // the sign of a quaternion is a unit quaternion with the same
+  // direction
+  template <class T>
+  inline CWTSQuaternion<T>
+  sgn(const CWTSQuaternion<T> &qtn)
+  {
+    return qtn.unit();
+  }
+
+  // the argument of a quaternion is the angle between it and the
+  // scalar number 1 (analogous to the argument of a complex number)
+  template <class T>
+  inline T
+  arg(const CWTSQuaternion<T> &qtn)
+  {
+    return atan2(norm(im(qtn)), re(qtn));
+  }
+
+  // quaternion exponentiation
+  template <class T>
+  CWTSQuaternion<T>
+  exp(const CWTSQuaternion<T> &qtn)
+  {
+    CWTSSpaceVector<T> svec = im(qtn);
+    T len = norm(svec);
+
+    if (len == CWTSZero<T>())
+      {
+        return exp(re(qtn))*CWTSQuaternion<T>(CWTSZero<T>(),
+                                              CWTSZero<T>(),
+                                              CWTSZero<T>(),
+                                              cos(len));
+      }
+    else
+      {
+        return exp(re(qtn))*CWTSQuaternion<T>(sgn(svec)*sin(len), cos(len));
+      }
+  }
+
+  // quaternion logarithm (with base e)
+  template <class T>
+  CWTSQuaternion<T>
+  log(const CWTSQuaternion<T> &qtn)
+  {
+    CWTSSpaceVector<T> svec = im(qtn);
+    T len = norm(svec);
+
+    if (len == CWTSZero<T>())
+      {
+        return CWTSQuaternion<T>(CWTSZero<T>(),
+                                 CWTSZero<T>(),
+                                 CWTSZero<T>(),
+                                 log(norm(qtn)));
+      }
+    else
+      {
+        return CWTSQuaternion<T>(sgn(svec)*arg(qtn), log(norm(qtn)));
+      }
+  }
+
+  // quaternion power! raise qtn1 to the power qtn2
+  template <class T>
+  inline CWTSQuaternion<T>
+  pow(const CWTSQuaternion<T> &qtn1, const CWTSQuaternion<T> &qtn2)
+  {
+    return exp(qtn2*log(qtn1));
+  }
+}
+
+#endif // IG_STAT_QUATERN_H

Mesh/stat_smatrix.h

+// -*-c++-*-
+#ifndef IG_STAT_SMATRIX_H
+#define IG_STAT_SMATRIX_H
+
+// $Id: stat_smatrix.h,v 1.1.2.7 2005/07/02 15:41:46 hkuiper Exp $
+
+// CwMtx matrix and vector math library
+// Copyright (C) 1999-2001  Harry Kuiper
+// Copyright (C) 2000  Will DeVore (template conversion)
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser 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
+
+#ifndef IG_STAT_MATRIX_H
+#include "stat_matrix.h"
+#endif
+
+namespace CwMtx
+{
+  // prefix smat
+  template <unsigned r, class T = double>
+  class CWTSSquareMatrix : public CWTSMatrix<r, r, T>
+  {
+  public:
+    CWTSSquareMatrix(): CWTSMatrix<r, r, T>() {}
+    CWTSSquareMatrix(const CWTSMatrix<r, r, T> &mat):
+      CWTSMatrix<r, r, T>(mat) {}
+    CWTSSquareMatrix(const CWTSSquareMatrix &smat):
+      CWTSMatrix<r, r, T>(smat) {}
+
+    ~CWTSSquareMatrix() {}
+
+    CWTSSquareMatrix operator +(const CWTSSquareMatrix &) const;
+    CWTSSquareMatrix operator -(const CWTSSquareMatrix &) const;
+    CWTSSquareMatrix operator -() const;
+    CWTSSquareMatrix operator *(const T &) const;
+    CWTSSquareMatrix operator *(const CWTSSquareMatrix &) const;
+    CWTSSquareMatrix operator /(const T &value) const;
+    CWTSSquareMatrix operator /(const CWTSSquareMatrix &) const;
+
+    // not inherited
+    CWTSSquareMatrix & operator =(const CWTSSquareMatrix &smat);
+    CWTSSquareMatrix & operator +=(const CWTSSquareMatrix &smat);
+    CWTSSquareMatrix & operator -=(const CWTSSquareMatrix &smat);
+    CWTSSquareMatrix & operator *=(const T &value);
+    CWTSSquareMatrix & operator *=(const CWTSSquareMatrix &);
+    CWTSSquareMatrix & operator /=(const T &value);
+    CWTSSquareMatrix & operator /=(const CWTSSquareMatrix &);
+
+    // stores the adjoint of argument in this
+    void storeAdjoint(const CWTSSquareMatrix &);
+    // stores the inverse of argument in this
+    void storeInverse(const CWTSSquareMatrix &);
+    // makes this its own adjoint
+    void makeAdjoint();
+    // makes this its own inverse
+    void makeInverse();
+    // makes this a unity matrix
+    void makeUnity();
+  };
+
+  // not inherited
+  template <unsigned r, class T>
+  inline CWTSSquareMatrix<r, T> &
+  CWTSSquareMatrix<r, T>::operator =(const CWTSSquareMatrix<r, T> &smat)
+  {
+    return
+      static_cast<CWTSSquareMatrix<r, T> &>(CWTSMatrix<r, r, T>::
+					    operator=(smat));
+  }
+
+  template <unsigned r, class T>
+  inline CWTSSquareMatrix<r, T> &
+  CWTSSquareMatrix<r, T>::operator +=(const CWTSSquareMatrix<r, T> &smat)
+  {
+    return
+      static_cast<CWTSSquareMatrix<r, T> &>(CWTSMatrix<r, r, T>::
+					    operator+=(smat));
+  }
+
+  template <unsigned r, class T>
+  inline CWTSSquareMatrix<r, T> &
+  CWTSSquareMatrix<r, T>::operator -=(const CWTSSquareMatrix &smat)
+  {
+    return
+      static_cast<CWTSSquareMatrix<r, T> &>(CWTSMatrix<r, r, T>::
+					    operator-=(smat));
+  }
+
+  template <unsigned r, class T>
+  inline CWTSSquareMatrix<r, T> &
+  CWTSSquareMatrix<r, T>::operator *=(const T &value)
+  {
+    return
+      static_cast<CWTSSquareMatrix<r, T> &>(CWTSMatrix<r, r, T>::
+					    operator*=(value));
+  }
+
+  template <unsigned r, class T>
+  inline CWTSSquareMatrix<r, T> &
+  CWTSSquareMatrix<r, T>::operator *=(const CWTSSquareMatrix<r, T> &smat)
+  {
+    return (*this) = (*this)*smat;
+  }
+
+  template <unsigned r, class T>
+  inline CWTSSquareMatrix<r, T> &
+  CWTSSquareMatrix<r, T>::operator /=(const T &value)
+  {
+    return (*this) *= CWTSUnity<T>()/value;
+  }
+
+  template <unsigned r, class T>
+  inline CWTSSquareMatrix<r, T> &
+  CWTSSquareMatrix<r, T>::operator /=(const CWTSSquareMatrix<r, T> &smat)
+  {
+    return (*this) = (*this)/smat;
+  }
+
+  template <unsigned r, class T>
+  CWTSSquareMatrix<r, T>
+  CWTSSquareMatrix<r, T>::operator +(const CWTSSquareMatrix<r, T> &smat) const
+  {
+    return CWTSSquareMatrix(*this) += smat;
+  }
+
+  template <unsigned r, class T>
+  CWTSSquareMatrix<r, T>
+  CWTSSquareMatrix<r, T>::operator -(const CWTSSquareMatrix<r, T> &smat) const
+  {
+    return CWTSSquareMatrix(*this) -= smat;
+  }
+
+  template <unsigned r, class T>
+  CWTSSquareMatrix<r, T> CWTSSquareMatrix<r, T>::operator -() const
+  {
+    return (*this)*(CWTSZero<T>() - CWTSUnity<T>());
+  }
+
+  template <unsigned r, class T>
+  CWTSSquareMatrix<r, T>
+  CWTSSquareMatrix<r, T>::operator *(const T &value) const
+  {
+    return CWTSSquareMatrix(*this) *= value;
+  }
+
+  template <unsigned r, class T>
+  inline CWTSSquareMatrix<r, T>
+  CWTSSquareMatrix<r, T>::operator /(const T &value) const
+  {
+    return (*this)*(CWTSUnity<T>()/value);
+  }
+
+  template <unsigned r, class T>
+  CWTSSquareMatrix<r, T>
+  CWTSSquareMatrix<r, T>::operator *(const CWTSSquareMatrix<r, T> &smat) const
+  {
+    CWTSSquareMatrix<r, T> smatResult;
+
+    for (unsigned irow = 0; irow < r; ++irow)
+      {
+	for (unsigned icol = 0; icol < r; ++icol)
+	  {
+	    smatResult[irow][icol] = CWTSZero<T>();
+
+	    for (unsigned icol2 = 0; icol2 < r; ++icol2)
+	      {
+		smatResult[irow][icol] +=
+		  (*this)[irow][icol2]*smat[icol2][icol];
+	      }
+	  }
+      }
+
+    return smatResult;
+  }
+
+  template <unsigned r, class T>
+  CWTSSquareMatrix<r, T>
+  CWTSSquareMatrix<r, T>::operator /(const CWTSSquareMatrix<r, T> &smat) const
+  {
+    return (*this)*inv(smat);
+  }
+
+  // stores inverse of argument in this
+  template <unsigned r, class T>
+  void
+  CWTSSquareMatrix<r, T>::storeInverse(const CWTSSquareMatrix<r, T> &smat)
+  {
+    // copy input matrix in this
+    (*this) = smat;
+    makeInverse();
+  }
+
+  // makes this a unity matrix
+  template <unsigned r, class T>
+  void
+  CWTSSquareMatrix<r, T>::makeUnity()
+  {
+    for (unsigned irow = 0; irow < r; ++irow)
+      {
+	for (unsigned icol = 0; icol < r; ++icol)
+	  {
+	    if (irow == icol)
+	      {
+		(*this)[irow][icol] = CWTSUnity<T>();
+	      }
+	    else
+	      {
+		(*this)[irow][icol] = CWTSZero<T>();
+	      }
+	  }
+      }
+  }
+
+  // makes this its own adjoint
+  template <unsigned r, class T>
+  void
+  CWTSSquareMatrix<r, T>::makeAdjoint()
+  {
+    // we need a copy of this
+    CWTSSquareMatrix<r, T> smatCopy(*this);
+    T elemTmp;
+    // will eventually contain det(smatCopy)
+    T elemDet = CWTSUnity<T>();
+
+    // make this a unity matrix
+    makeUnity();
+
+    // start row reduction
+    for (unsigned irow = 0; irow < r; ++irow)
+      {
+	// if element [irow][irow] is zero, add a row with non-zero
+	// element at column irow
+	if (smatCopy[irow][irow] == CWTSZero<T>())
+	  {
+	    for (unsigned irowTmp = irow; irowTmp < r; ++irowTmp)
+	      {
+		if (smatCopy[irowTmp][irow] != CWTSZero<T>())
+		  {
+		    // has no effect on adj(smat)
+		    smatCopy.addRowToRow(irowTmp, irow);
+		    // repeat action on this
+		    this->addRowToRow(irowTmp, irow);
+		    break;
+		  }
+	      }
+	  }
+
+	elemTmp = smatCopy[irow][irow];
+        T tTmp = CWTSUnity<T>();
+        tTmp /= elemTmp;
+	smatCopy.multiplyRow(irow, tTmp);
+	// repeat action on this
+	multiplyRow(irow, tTmp);
+	elemDet *= elemTmp;
+
+	for (unsigned irowToAddTo = 0; irowToAddTo < r; ++irowToAddTo)
+	  {
+	    if (irowToAddTo != irow)
+	      {
+		elemTmp = -smatCopy[irowToAddTo][irow];
+		smatCopy.addRowToRow(irow, irowToAddTo, elemTmp);
+		// repeat action on this
+		addRowToRow(irow, irowToAddTo, elemTmp);
+	      }
+	  }
+      }
+
+    // this now contains its adjoint
+    (*this) *= elemDet;
+  }
+
+  // stores adjoint of input matrix in this
+  template <unsigned r, class T>
+  void
+  CWTSSquareMatrix<r, T>::storeAdjoint(const CWTSSquareMatrix<r, T> &smat)
+  {
+    // copy input matrix in this
+    (*this) = smat;
+    makeAdjoint();
+  }
+
+  // makes this its own inverse
+  template <unsigned r, class T>
+  void
+  CWTSSquareMatrix<r, T>::makeInverse()
+  {
+    // we need a copy of this
+    CWTSSquareMatrix<r, T> smatCopy(*this);
+    T elemTmp;
+
+    // make this a unity matrix
+    makeUnity();
+
+    // start row reduction
+    for (unsigned irow = 0; irow < r; ++irow)
+      {
+	// if element [irow][irow] is zero, add a row with non-zero
+	// element at column irow
+	if (smatCopy[irow][irow] == CWTSZero<T>())
+	  {
+	    for (unsigned irowTmp = irow; irowTmp < r; ++irowTmp)
+	      {
+		// has no effect on inv(smat)
+		if (smatCopy[irowTmp][irow] != CWTSZero<T>())
+		  {
+		    smatCopy.addRowToRow(irowTmp, irow);
+		    // repeat action on this
+		    this->addRowToRow(irowTmp, irow);
+		    break;
+		  }
+	      }
+	  }
+
+	elemTmp = smatCopy[irow][irow];
+        T tTmp = CWTSUnity<T>();
+        tTmp /= elemTmp;
+	smatCopy.multiplyRow(irow, tTmp);
+	multiplyRow(irow, tTmp);
+
+	for (unsigned irowToAddTo = 0; irowToAddTo < r; ++irowToAddTo)
+	  {
+	    if (irowToAddTo != irow)
+	      {
+		elemTmp = -smatCopy[irowToAddTo][irow];
+		smatCopy.addRowToRow(irow, irowToAddTo, elemTmp);
+		// repeat action on this
+		addRowToRow(irow, irowToAddTo, elemTmp);
+	      }
+	  }
+      }
+
+    // this now contains its inverse
+  }
+
+  // template functions designed work with the Square Matrix class.
+
+  template <unsigned r, class T>
+  inline CWTSSquareMatrix<r, T>
+  operator *(const T &value, const CWTSSquareMatrix<r, T> &smat)
+  {
+    return smat*value;
+  }
+
+  template <unsigned r, class T>
+  CWTSSquareMatrix<r, T>
+  transpose(const CWTSSquareMatrix<r, T> &smat)
+  {
+    CWTSSquareMatrix<r, T> smatTranspose;
+
+    for (unsigned irow = 0; irow < r; ++irow)
+      {
+	for (unsigned icol = 0; icol < r; ++icol)
+	  {
+	    smatTranspose[irow][icol] = smat[icol][irow];
+	  }
+      }
+
+    return smatTranspose;
+  }
+
+  // returns the adjoint of a square matrix
+  template <unsigned r, class T>
+  CWTSSquareMatrix<r, T>
+  adj(const CWTSSquareMatrix<r, T> &smat)
+  {
+    CWTSSquareMatrix<r, T> smatResult(smat);   // copy input matrix
+    smatResult.makeAdjoint();
+    return smatResult;
+  }
+
+  // calculates the inverse of a square matrix
+  template <unsigned r, class T>
+  CWTSSquareMatrix<r, T>
+  inv(const CWTSSquareMatrix<r, T> &smat)
+  {
+    // copy input matrix
+    CWTSSquareMatrix<r, T> smatResult(smat);
+    smatResult.makeInverse();
+    return smatResult;
+  }
+
+  // calculates the determinant of a square matrix
+  template <unsigned r, class T>
+  T
+  det(const CWTSSquareMatrix<r, T> &smat)
+  {
+    // a copy of the input matrix
+    CWTSSquareMatrix<r, T> smatCopy(smat);
+
+    // start row reduction
+    T elemTmp;
+    // will eventually contain det(smatCopy)
+    T elemDet = CWTSUnity<T>();
+
+    for (unsigned irow = 0; irow < r; ++irow)
+      {
+	// if element [irow][irow] is zero, add a row with non-zero
+	// element at column irow
+	if (smatCopy[irow][irow] == CWTSZero<T>())
+	  {
+	    for (unsigned irowTmp = irow; irowTmp < r; ++irowTmp)
+	      {
+		// has no effect on inv(smat)
+		if (smatCopy[irowTmp][irow] != CWTSZero<T>())
+		  {
+		    smatCopy.addRowToRow(irowTmp, irow);
+		    break;
+		  }
+	      }
+	  }
+
+	elemTmp = smatCopy[irow][irow];
+	elemDet *= elemTmp;
+
+	if (elemDet == CWTSZero<T>())
+	  {
+	    // once elemDet is zero it will stay zero
+	    return elemDet;
+	  }
+
+	smatCopy.multiplyRow(irow, CWTSUnity<T>()/elemTmp);
+
+	for (unsigned irowToAddTo = 0; irowToAddTo < r; ++irowToAddTo)
+	  {
+	    if (irowToAddTo != irow)
+	      {
+		smatCopy.addRowToRow(irow,
+				     irowToAddTo,
+				     -smatCopy[irowToAddTo][irow]);
+	      }
+	  }
+      }
+
+    return elemDet;
+  }
+
+  // trace
+  template <unsigned r, class T>
+  T
+  tr(const CWTSSquareMatrix<r, T> &smat)
+  {
+    T elemTmp = CWTSZero<T>();
+
+    for (unsigned c = 0; c < r; c++)
+      {
+	elemTmp += smat[c][c];
+      }
+
+    return elemTmp;
+  }
+
+}
+
+#endif // IG_STAT_SMATRIX_H

Mesh/stat_svector.h

+// -*-c++-*-
+#ifndef IG_STAT_SVECTOR_H
+#define IG_STAT_SVECTOR_H
+
+// $Id: stat_svector.h,v 1.1.2.7 2005/07/02 15:41:46 hkuiper Exp $
+
+// CwMtx matrix and vector math library
+// Copyright (C) 1999-2001  Harry Kuiper
+// Copyright (C) 2000  Will DeVore (template conversion)
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser 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
+
+#ifndef IG_STAT_VECTOR_H
+#include "stat_vector.h"
+#endif
+
+#ifndef IG_STAT_SMATRIX_H
+#include "stat_smatrix.h"
+#endif
+
+namespace CwMtx
+{
+
+  // prefix svec
+  template <class T = double>
+  class CWTSSpaceVector: public CWTSVector<3, T>
+  {
+  public:
+    CWTSSpaceVector(): CWTSVector<3, T>() {}
+    CWTSSpaceVector(const CWTSMatrix<3, 1, T> &mat): CWTSVector<3, T>(mat) {}
+    CWTSSpaceVector(const CWTSVector<3, T> &vec): CWTSVector<3, T>(vec) {}
+    CWTSSpaceVector(const CWTSSpaceVector &svec): CWTSVector<3, T>(svec) {}
+    // construct from 3 elements
+    CWTSSpaceVector(const T &, const T &, const T &);
+
+    ~CWTSSpaceVector() {}
+
+    CWTSSpaceVector operator +(const CWTSSpaceVector &) const;
+    CWTSSpaceVector operator -(const CWTSSpaceVector &) const;
+    CWTSSpaceVector operator -() const;
+    CWTSSpaceVector operator *(const T &) const;
+    // inner product
+    T operator *(const CWTSSpaceVector &) const;
+    // outer product
+    CWTSSpaceVector operator %(const CWTSSpaceVector &) const;
+    CWTSSpaceVector operator /(const T &value) const;
+
+    // not inherited
+    CWTSSpaceVector & operator =(const CWTSSpaceVector &);
+    CWTSSpaceVector & operator +=(const CWTSSpaceVector &);
+    CWTSSpaceVector & operator -=(const CWTSSpaceVector &);
+    CWTSSpaceVector & operator *=(const T &);
+    // outer product
+    CWTSSpaceVector & operator %=(const CWTSSpaceVector &);
+    CWTSSpaceVector & operator /=(const T &value);
+
+    void storeOuterProduct(const CWTSSpaceVector &, const CWTSSpaceVector &);
+
+    // returns a unit vector with same direction as this
+    CWTSSpaceVector unit() const { return (*this)/(this->norm()); }
+  };
+
+  template <class T>
+  inline CWTSSpaceVector<T>::CWTSSpaceVector(const T &elem1,
+					     const T &elem2,
+					     const T &elem3)
+    :
+    CWTSVector<3, T>()
+  {
+    (*this)[0] = elem1;
+    (*this)[1] = elem2;
+    (*this)[2] = elem3;
+  }
+
+  template <class T>
+  inline CWTSSpaceVector<T>
+  CWTSSpaceVector<T>::operator /(const T &value) const
+  {
+    return (*this)*(CWTSUnity<T>()/value);
+  }
+
+  // not inherited
+  template <class T>
+  inline CWTSSpaceVector<T> &
+  CWTSSpaceVector<T>::operator =(const CWTSSpaceVector<T> &svec)
+  {
+    return
+      static_cast<CWTSSpaceVector &>(CWTSMatrix<3, 1, T>::operator=(svec));
+  }
+
+  template <class T>
+  inline CWTSSpaceVector<T> &
+  CWTSSpaceVector<T>::operator +=(const CWTSSpaceVector<T> &svec)
+  {
+    return
+      static_cast<CWTSSpaceVector &>(CWTSMatrix<3, 1, T>::operator+=(svec));
+  }
+
+  template <class T>
+  inline CWTSSpaceVector<T> &
+  CWTSSpaceVector<T>::operator -=(const CWTSSpaceVector<T> &svec)
+  {
+    return
+      static_cast<CWTSSpaceVector &>(CWTSMatrix<3, 1, T>::operator-=(svec));
+  }
+
+  template <class T>
+  inline CWTSSpaceVector<T> &
+  CWTSSpaceVector<T>::operator *=(const T &value)
+  {
+    return
+      static_cast<CWTSSpaceVector &>(
+				     CWTSMatrix<3, 1, T>::operator*=(value));
+  }
+
+  template <class T>
+  inline CWTSSpaceVector<T> &
+  CWTSSpaceVector<T>::operator /=(const T &value)
+  {
+    return (*this) *= CWTSUnity<T>()/value;
+  }
+
+  // outer product
+  template <class T>
+  inline CWTSSpaceVector<T> &
+  CWTSSpaceVector<T>::operator %=(const CWTSSpaceVector<T> &vec)
+  {
+    return (*this) = (*this)%vec;
+  }
+
+  template <class T>
+  CWTSSpaceVector<T>
+  CWTSSpaceVector<T>::operator +(const CWTSSpaceVector<T> &svec) const
+  {
+    return CWTSSpaceVector(*this) += svec;
+  }
+
+  template <class T>
+  CWTSSpaceVector<T>
+  CWTSSpaceVector<T>::operator -(const CWTSSpaceVector<T> &svec) const
+  {
+    return CWTSSpaceVector(*this) -= svec;
+  }
+
+  template <class T>
+  CWTSSpaceVector<T>
+  CWTSSpaceVector<T>::operator -() const
+  {
+    return (*this)*(CWTSZero<T>() - CWTSUnity<T>());
+  }
+
+  template <class T>
+  CWTSSpaceVector<T>
+  CWTSSpaceVector<T>::operator *(const T &value) const
+  {
+    return CWTSSpaceVector(*this) *= value;
+  }
+
+  // inner product
+  template <class T>
+  T
+  CWTSSpaceVector<T>::operator *(const CWTSSpaceVector<T> &vec) const
+  {
+    return CWTSVector<3, T>::operator*(vec);
+  }
+
+  template <class T>
+  void
+  CWTSSpaceVector<T>::storeOuterProduct(const CWTSSpaceVector<T> &svecLeft,
+					const CWTSSpaceVector<T> &svecRight)
+  {
+    // to reduce the number of calls to the subscript operator
+    T svecLeft0 = svecLeft[0];
+    T svecLeft1 = svecLeft[1];
+    T svecLeft2 = svecLeft[2];
+
+    T svecRight0 = svecRight[0];
+    T svecRight1 = svecRight[1];
+    T svecRight2 = svecRight[2];
+
+    (*this)[0] = svecLeft1*svecRight2 - svecLeft2*svecRight1;
+    (*this)[1] = svecLeft2*svecRight0 - svecLeft0*svecRight2;
+    (*this)[2] = svecLeft0*svecRight1 - svecLeft1*svecRight0;
+  }
+
+  template <class T>
+  CWTSSpaceVector<T>
+  CWTSSpaceVector<T>::operator %(const CWTSSpaceVector<T> &svec) const
+  {
+    CWTSSpaceVector<T> svecResult;
+    svecResult.storeOuterProduct(*this, svec);
+    return svecResult;
+  }
+
+  // template functions designed work with the base matrix class.
+
+  template <class T>
+  inline CWTSSpaceVector<T>
+  operator *(const T &value, const CWTSSpaceVector<T> &svec)
+  {
+    return svec*value;
+  }
+
+  // (square matrix)*(space vector) must yield a space vector
+  template <class T>
+  CWTSSpaceVector<T>
+  operator *(const CWTSSquareMatrix<3, T> &smat,
+	     const CWTSSpaceVector<T> &svec)
+  {
+    CWTSSpaceVector<T> svecResult;
+
+    for (unsigned irow = 0; irow < 3; ++irow)
+      {
+	svecResult[irow] = CWTSZero<T>();
+
+	for (unsigned icol = 0; icol < 3; ++icol)
+	  {
+	    svecResult[irow] += smat[irow][icol]*svec[icol];
+	  }
+      }
+
+    return svecResult;
+  }
+
+  // the sign of a vector is a unit vector with the same direction
+  template <class T>
+  inline CWTSSpaceVector<T>
+  sgn(const CWTSSpaceVector<T> &svec)
+  {
+    return svec.unit();
+  }
+}
+
+#endif // IG_STAT_SVECTOR_H

Mesh/stat_vector.h

+// -*-c++-*-
+#ifndef IG_STAT_VECTOR_H
+#define IG_STAT_VECTOR_H
+
+// $Id: stat_vector.h,v 1.1.2.6 2005/05/30 09:14:25 hkuiper Exp $
+
+// CwMtx matrix and vector math library
+// Copyright (C) 1999-2001  Harry Kuiper
+// Copyright (C) 2000  Will DeVore (template conversion)
+
+// This library is free software; you can redistribute it and/or
+// modify it under the terms of the GNU Lesser 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
+// Lesser General Public License for more details.
+
+// You should have received a copy of the GNU Lesser 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
+
+#include <cmath>
+
+#ifndef IG_STAT_MATRIX_H
+#include "stat_matrix.h"
+#endif
+
+namespace CwMtx
+{
+
+  template <unsigned r, class T = double>
+  class CWTSVector : public CWTSMatrix<r, 1, T>
+  {
+  public:
+    CWTSVector(): CWTSMatrix<r, 1, T>() {}
+    CWTSVector(const CWTSMatrix<r, 1, T>& mat): CWTSMatrix<r, 1, T>(mat) {}
+    CWTSVector(const CWTSVector& vec): CWTSMatrix<r, 1, T>(vec) {}
+
+    ~CWTSVector() {}
+
+    T & operator [](unsigned irow);
+    const T & operator [](unsigned irow) const;
+
+    CWTSVector operator +(const CWTSVector &) const;
+    CWTSVector operator -(const CWTSVector &) const;
+    CWTSVector operator -() const;
+    CWTSVector operator *(const T &) const;
+    CWTSVector operator /(const T &value) const;
+    // CWTSVector*CWTSVector, inner product
+    T operator *(const CWTSVector &) const;
+
+    // not inherited
+    CWTSVector & operator =(const CWTSVector &vec);
+    CWTSVector & operator +=(const CWTSVector &vec);
+    CWTSVector & operator -=(const CWTSVector &vec);
+    CWTSVector & operator *=(const T &value);
+    CWTSVector & operator /=(const T &value);
+
+    // CWTSVector norm
+    T operator !() const { return (*this).norm(); }
+    // returns vector norm (length)
+    T norm() const;
+    // returns a unit vector with same direction as this
+    CWTSVector unit() const { return (*this)/norm(); }
+
+    // make this a unit vector
+    void makeUnit() { (*this) /= norm(); }
+
+    template <unsigned r2>
+    void storeAtRow(unsigned, const CWTSVector<r2, T> &);
+  };
+
+  template <unsigned r, class T>
+  inline T &
+  CWTSVector<r, T>::operator [](unsigned irow)
+  {
+    return this->CWTSMatrix<r, 1, T>::operator[](irow)[0];
+  }
+
+  template <unsigned r, class T>
+  inline const T &
+  CWTSVector<r, T>::operator [](unsigned irow) const
+  {
+    return this->CWTSMatrix<r, 1, T>::operator[](irow)[0];
+  }
+
+  // not inherited
+  template <unsigned r, class T>
+  inline CWTSVector<r, T> &
+  CWTSVector<r, T>::operator =(const CWTSVector<r, T> &vec)
+  {
+    return static_cast<CWTSVector &>(CWTSMatrix<r, 1, T>::operator=(vec));
+  }
+
+  template <unsigned r, class T>
+  inline CWTSVector<r, T> &
+  CWTSVector<r, T>::operator +=(const CWTSVector<r, T> &vec)
+  {
+    return static_cast<CWTSVector &>(CWTSMatrix<r, 1, T>::operator+=(vec));
+  }
+
+  template <unsigned r, class T>
+  inline CWTSVector<r, T> &
+  CWTSVector<r, T>::operator -=(const CWTSVector<r, T> &vec)
+  {
+    return static_cast<CWTSVector &>(CWTSMatrix<r, 1, T>::operator-=(vec));
+  }
+
+  template <unsigned r, class T>
+  inline CWTSVector<r, T> &
+  CWTSVector<r, T>::operator *=(const T &value)
+  {
+    return static_cast<CWTSVector &>(CWTSMatrix<r, 1, T>::operator*=(value));
+  }
+
+  template <unsigned r, class T>
+  inline CWTSVector<r, T> &
+  CWTSVector<r, T>::operator /=(const T &value)
+  {
+    return (*this) *= CWTSUnity<T>()/value;
+  }
+
+  template <unsigned r, class T>
+  CWTSVector<r, T>
+  CWTSVector<r, T>::operator +(const CWTSVector<r, T> &vec) const
+  {
+    return CWTSVector<r, T>(*this) += vec;
+  }
+
+  template <unsigned r, class T>
+  CWTSVector<r, T>
+  CWTSVector<r, T>::operator -(const CWTSVector<r, T> &vec) const
+  {
+    return CWTSVector<r, T>(*this) -= vec;
+  }
+
+  template <unsigned r, class T>
+  CWTSVector<r, T>
+  CWTSVector<r, T>::operator -() const
+  {
+    return (*this)*(CWTSZero<T>() - CWTSUnity<T>());
+  }
+
+  template <unsigned r, class T>
+  CWTSVector<r, T>
+  CWTSVector<r, T>::operator *(const T &value) const
+  {
+    return CWTSVector<r, T>(*this) *= value;
+  }
+
+  template <unsigned r, class T>
+  CWTSVector<r, T>
+  CWTSVector<r, T>::operator /(const T &value) const
+  {
+    return (*this)*(CWTSUnity<T>()/value);
+  }
+
+  template <unsigned r, class T>
+  T
+  CWTSVector<r, T>::operator *(const CWTSVector<r, T> &vec) const
+  {
+    T elemResult = CWTSZero<T>();
+
+    for (unsigned irow = 0; irow < r; ++irow)
+      {
+	elemResult += (*this)[irow]*vec[irow];
+      }
+
+    return elemResult;
+  }
+
+  // length of vector
+  template <unsigned r, class T>
+  T
+  CWTSVector<r, T>::norm() const
+  {
+    T elemResult = CWTSZero<T>();
+
+    elemResult = (*this)*(*this);
+
+    return sqrt(elemResult);
+  }
+
+  template <unsigned r, class T>
+  template <unsigned r2>
+  inline void
+  CWTSVector<r, T>::storeAtRow(unsigned irowStart,
+			       const CWTSVector<r2, T> &vec)
+  {
+    CWTSMatrix<r, 1, T>::storeAtPosition(irowStart, 0, vec);
+  }
+
+  // template functions designed work with the vector class.
+
+  template <unsigned r, class T>
+  inline CWTSVector<r, T>
+  operator *(const T &value, const CWTSVector<r, T> &vec)
+  {
+    return vec*value;
+  }
+
+  // matrix*vector must yield a vector
+  template <unsigned r, unsigned c, class T>
+  CWTSVector<r, T>
+  operator *(const CWTSMatrix<r, c, T> &mat, const CWTSVector<r, T> &vec)
+  {
+    CWTSVector<r, T> vecResult;
+
+    for (unsigned irow = 0; irow < r; ++irow)
+      {
+	vecResult[irow] = CWTSZero<T>();
+
+	for (unsigned icol = 0; icol < c; ++icol)
+	  {
+	    vecResult[irow] += mat[irow][icol]*vec[icol];
+	  }
+      }
+
+    return vecResult;
+  }
+
+  // norm computation as a function
+  template <unsigned r, class T>
+  inline T
+  norm(const CWTSVector<r, T> &vec)
+  {
+    return vec.norm();
+  }
+
+  // the sign of a vector is a unit vector with the same direction
+  template <unsigned r, class T>
+  inline CWTSVector<r, T>
+  sgn(const CWTSVector<r, T> &vec)
+  {
+    return vec.unit();
+  }
+}
+
+#endif // IG_STAT_VECTOR_H