From 744f892252bd08ec06c3d04ad8c15cda2b5a5417 Mon Sep 17 00:00:00 2001
From: Francois Henrotte <francois.henrotte@ulg.ac.be>
Date: Fri, 15 Feb 2013 14:51:04 +0000
Subject: [PATCH] quaternions library
---
Mesh/stat_coordsys.h | 377 ++++++++++++++++++++++++++++++++
Mesh/stat_cwmtx.h | 56 +++++
Mesh/stat_matrix.h | 507 +++++++++++++++++++++++++++++++++++++++++++
Mesh/stat_quatern.h | 418 +++++++++++++++++++++++++++++++++++
Mesh/stat_smatrix.h | 467 +++++++++++++++++++++++++++++++++++++++
Mesh/stat_svector.h | 247 +++++++++++++++++++++
Mesh/stat_vector.h | 243 +++++++++++++++++++++
7 files changed, 2315 insertions(+)
create mode 100755 Mesh/stat_coordsys.h
create mode 100755 Mesh/stat_cwmtx.h
create mode 100755 Mesh/stat_matrix.h
create mode 100755 Mesh/stat_quatern.h
create mode 100755 Mesh/stat_smatrix.h
create mode 100755 Mesh/stat_svector.h
create mode 100755 Mesh/stat_vector.h
diff --git a/Mesh/stat_coordsys.h b/Mesh/stat_coordsys.h
new file mode 100755
index 0000000000..df818398cb
--- /dev/null
+++ b/Mesh/stat_coordsys.h
@@ -0,0 +1,377 @@
+// -*-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
diff --git a/Mesh/stat_cwmtx.h b/Mesh/stat_cwmtx.h
new file mode 100755
index 0000000000..fc4cc35e51
--- /dev/null
+++ b/Mesh/stat_cwmtx.h
@@ -0,0 +1,56 @@
+// -*-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
diff --git a/Mesh/stat_matrix.h b/Mesh/stat_matrix.h
new file mode 100755
index 0000000000..16526ff74f
--- /dev/null
+++ b/Mesh/stat_matrix.h
@@ -0,0 +1,507 @@
+// -*-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
+
diff --git a/Mesh/stat_quatern.h b/Mesh/stat_quatern.h
new file mode 100755
index 0000000000..1d4538e169
--- /dev/null
+++ b/Mesh/stat_quatern.h
@@ -0,0 +1,418 @@
+// -*-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
diff --git a/Mesh/stat_smatrix.h b/Mesh/stat_smatrix.h
new file mode 100755
index 0000000000..e25106b2d5
--- /dev/null
+++ b/Mesh/stat_smatrix.h
@@ -0,0 +1,467 @@
+// -*-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
diff --git a/Mesh/stat_svector.h b/Mesh/stat_svector.h
new file mode 100755
index 0000000000..f9099aec27
--- /dev/null
+++ b/Mesh/stat_svector.h
@@ -0,0 +1,247 @@
+// -*-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
diff --git a/Mesh/stat_vector.h b/Mesh/stat_vector.h
new file mode 100755
index 0000000000..089ba93454
--- /dev/null
+++ b/Mesh/stat_vector.h
@@ -0,0 +1,243 @@
+// -*-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
--
GitLab