flatten 20260225

tjp/core/math/detail/Wm5.cpp

@@ -0,0 +1,692 @@
+//
+//  Wm5.cpp
+//  algorithm
+//
+//  Created by Timothy Prepscius on 5/3/18.
+//  Copyright © 2018 Timothy Prepscius. All rights reserved.
+//
+
+#include "Wm5.h"
+
+#define assertion(...)
+
+namespace Wm5 {
+
+template<> const float Math<float>::ZERO_TOLERANCE = 1e-06f;
+
+template<> const double Math<double>::ZERO_TOLERANCE = 1e-08;
+
+/************************************************************************/
+
+//----------------------------------------------------------------------------
+template <typename Real>
+EigenDecomposition<Real>::EigenDecomposition (int size)
+    :
+    mMatrix(size, size)
+{
+    assertion(size >= 2, "Invalid size in Eigendecomposition constructor\n");
+
+    mSize = size;
+    mDiagonal = new1<Real>(mSize);
+    mSubdiagonal = new1<Real>(mSize);
+    mIsRotation = false;
+}
+/*
+//----------------------------------------------------------------------------
+template <typename Real>
+EigenDecomposition<Real>::EigenDecomposition (const Matrix2<Real>& mat)
+    :
+    mMatrix(2, 2, &mat[0][0])
+{
+    mSize = 2;
+    mDiagonal = new1<Real>(mSize);
+    mSubdiagonal = new1<Real>(mSize);
+    mIsRotation = false;
+}
+//----------------------------------------------------------------------------
+template <typename Real>
+EigenDecomposition<Real>::EigenDecomposition (const Matrix3<Real>& mat)
+    :
+    mMatrix(3, 3, &mat[0][0])
+{
+    mSize = 3;
+    mDiagonal = new1<Real>(mSize);
+    mSubdiagonal = new1<Real>(mSize);
+    mIsRotation = false;
+}
+*/
+//----------------------------------------------------------------------------
+template <typename Real>
+EigenDecomposition<Real>::EigenDecomposition (const GMatrix<Real>& mat)
+    :
+    mMatrix(mat)
+{
+    mSize = mat.GetRows();
+    assertion(mSize >= 2 && (mat.GetColumns() == mSize),
+        "Square matrix required in EigenDecomposition constructor\n");
+
+    mDiagonal = new1<Real>(mSize);
+    mSubdiagonal = new1<Real>(mSize);
+    mIsRotation = false;
+}
+//----------------------------------------------------------------------------
+template <typename Real>
+EigenDecomposition<Real>::~EigenDecomposition ()
+{
+    delete1(mDiagonal);
+    delete1(mSubdiagonal);
+}
+//----------------------------------------------------------------------------
+template <typename Real>
+Real& EigenDecomposition<Real>::operator() (int row, int column)
+{
+    return mMatrix[row][column];
+}
+//----------------------------------------------------------------------------
+/*
+template <typename Real>
+EigenDecomposition<Real>& EigenDecomposition<Real>::operator= (
+    const Matrix2<Real>& mat)
+{
+    mMatrix.SetMatrix(2, 2, &mat[0][0]);
+    mSize = 2;
+    delete1(mDiagonal);
+    delete1(mSubdiagonal);
+    mDiagonal = new1<Real>(mSize);
+    mSubdiagonal = new1<Real>(mSize);
+    return *this;
+}
+//----------------------------------------------------------------------------
+template <typename Real>
+EigenDecomposition<Real>& EigenDecomposition<Real>::operator= (
+    const Matrix3<Real>& mat)
+{
+    mMatrix.SetMatrix(3, 3, &mat[0][0]);
+    mSize = 3;
+    delete1(mDiagonal);
+    delete1(mSubdiagonal);
+    mDiagonal = new1<Real>(mSize);
+    mSubdiagonal = new1<Real>(mSize);
+    return *this;
+}
+*/
+//----------------------------------------------------------------------------
+template <typename Real>
+EigenDecomposition<Real>& EigenDecomposition<Real>::operator= (
+    const GMatrix<Real>& mat)
+{
+    mMatrix = mat;
+    return *this;
+}
+//----------------------------------------------------------------------------
+template <typename Real>
+void EigenDecomposition<Real>::Solve (bool increasingSort)
+{
+    if (mSize == 2)
+    {
+        Tridiagonal2();
+    }
+    else if (mSize == 3)
+    {
+        Tridiagonal3();
+    }
+    else
+    {
+        TridiagonalN();
+    }
+
+    QLAlgorithm();
+
+    if (increasingSort)
+    {
+        IncreasingSort();
+    }
+    else
+    {
+        DecreasingSort();
+    }
+
+    GuaranteeRotation();
+}
+//----------------------------------------------------------------------------
+template <typename Real>
+Real EigenDecomposition<Real>::GetEigenvalue (int i) const
+{
+    assertion(0 <= i && i < mSize, "Invalid index in GetEigenvalue\n");
+    return mDiagonal[i];
+}
+//----------------------------------------------------------------------------
+template <typename Real>
+const Real* EigenDecomposition<Real>::GetEigenvalues () const
+{
+    return mDiagonal;
+}
+//----------------------------------------------------------------------------
+/*
+template <typename Real>
+Vector2<Real> EigenDecomposition<Real>::GetEigenvector2 (int i) const
+{
+    assertion(mSize == 2, "Mismatched dimension in GetEigenvector2\n");
+
+    if (mSize == 2)
+    {
+        Vector2<Real> eigenvector;
+        for (int row = 0; row < mSize; ++row)
+        {
+            eigenvector[row] = mMatrix[row][i];
+        }
+        return eigenvector;
+    }
+    return Vector2<Real>::ZERO;
+}
+//----------------------------------------------------------------------------
+template <typename Real>
+Matrix2<Real> EigenDecomposition<Real>::GetEigenvectors2 () const
+{
+    assertion(mSize == 2, "Mismatched dimension in GetEigenvectors2\n");
+
+    Matrix2<Real> eigenvectors;
+    for (int row = 0; row < 2; ++row)
+    {
+        for (int column = 0; column < 2; ++column)
+        {
+            eigenvectors[row][column] = mMatrix[row][column];
+        }
+    }
+    return eigenvectors;
+}
+//----------------------------------------------------------------------------
+template <typename Real>
+Vector3<Real> EigenDecomposition<Real>::GetEigenvector3 (int i) const
+{
+    assertion(mSize == 3, "Mismatched dimension in GetEigenvector3\n");
+
+    if (mSize == 3)
+    {
+        Vector3<Real> eigenvector;
+        for (int row = 0; row < mSize; ++row)
+        {
+            eigenvector[row] = mMatrix[row][i];
+        }
+        return eigenvector;
+    }
+    return Vector3<Real>::ZERO;
+}
+//----------------------------------------------------------------------------
+template <typename Real>
+Matrix3<Real> EigenDecomposition<Real>::GetEigenvectors3 () const
+{
+    assertion(mSize == 3, "Mismatched dimension in GetEigenvectors2\n");
+
+    Matrix3<Real> eigenvectors;
+    for (int row = 0; row < 3; ++row)
+    {
+        for (int column = 0; column < 3; ++column)
+        {
+            eigenvectors[row][column] = mMatrix[row][column];
+        }
+    }
+    return eigenvectors;
+}
+*/
+//----------------------------------------------------------------------------
+template <typename Real>
+GVector<Real> EigenDecomposition<Real>::GetEigenvector (int i) const
+{
+    return mMatrix.GetColumn(i);
+}
+//----------------------------------------------------------------------------
+template <typename Real>
+const GMatrix<Real>& EigenDecomposition<Real>::GetEigenvectors () const
+{
+    return mMatrix;
+}
+//----------------------------------------------------------------------------
+template <typename Real>
+void EigenDecomposition<Real>::Tridiagonal2 ()
+{
+    // The matrix is already tridiagonal.
+    mDiagonal[0] = mMatrix[0][0];
+    mDiagonal[1] = mMatrix[1][1];
+    mSubdiagonal[0] = mMatrix[0][1];
+    mSubdiagonal[1] = (Real)0;
+    mMatrix[0][0] = (Real)1;
+    mMatrix[0][1] = (Real)0;
+    mMatrix[1][0] = (Real)0;
+    mMatrix[1][1] = (Real)1;
+
+    mIsRotation = true;
+}
+//----------------------------------------------------------------------------
+template <typename Real>
+void EigenDecomposition<Real>::Tridiagonal3 ()
+{
+    Real m00 = mMatrix[0][0];
+    Real m01 = mMatrix[0][1];
+    Real m02 = mMatrix[0][2];
+    Real m11 = mMatrix[1][1];
+    Real m12 = mMatrix[1][2];
+    Real m22 = mMatrix[2][2];
+
+    mDiagonal[0] = m00;
+    mSubdiagonal[2] = (Real)0;
+    if (Math<Real>::FAbs(m02) > Math<Real>::ZERO_TOLERANCE)
+    {
+        Real length = Math<Real>::Sqrt(m01*m01 + m02*m02);
+        Real invLength = ((Real)1)/length;
+        m01 *= invLength;
+        m02 *= invLength;
+        Real q = ((Real)2)*m01*m12 + m02*(m22 - m11);
+        mDiagonal[1] = m11 + m02*q;
+        mDiagonal[2] = m22 - m02*q;
+        mSubdiagonal[0] = length;
+        mSubdiagonal[1] = m12 - m01*q;
+        mMatrix[0][0] = (Real)1;
+        mMatrix[0][1] = (Real)0;
+        mMatrix[0][2] = (Real)0;
+        mMatrix[1][0] = (Real)0;
+        mMatrix[1][1] = m01;
+        mMatrix[1][2] = m02;
+        mMatrix[2][0] = (Real)0;
+        mMatrix[2][1] = m02;
+        mMatrix[2][2] = -m01;
+        mIsRotation = false;
+    }
+    else
+    {
+        mDiagonal[1] = m11;
+        mDiagonal[2] = m22;
+        mSubdiagonal[0] = m01;
+        mSubdiagonal[1] = m12;
+        mMatrix[0][0] = (Real)1;
+        mMatrix[0][1] = (Real)0;
+        mMatrix[0][2] = (Real)0;
+        mMatrix[1][0] = (Real)0;
+        mMatrix[1][1] = (Real)1;
+        mMatrix[1][2] = (Real)0;
+        mMatrix[2][0] = (Real)0;
+        mMatrix[2][1] = (Real)0;
+        mMatrix[2][2] = (Real)1;
+        mIsRotation = true;
+    }
+}
+//----------------------------------------------------------------------------
+template <typename Real>
+void EigenDecomposition<Real>::TridiagonalN ()
+{
+    // TODO:  This is Numerical Recipes in C code.  Rewrite to avoid
+    // copyright issues.
+
+    int i0, i1, i2, i3;
+
+    for (i0 = mSize - 1, i3 = mSize - 2; i0 >= 1; --i0, --i3)
+    {
+        Real value0 = (Real)0;
+        Real scale = (Real)0;
+
+        if (i3 > 0)
+        {
+            for (i2 = 0; i2 <= i3; ++i2)
+            {
+                scale += Math<Real>::FAbs(mMatrix[i0][i2]);
+            }
+            if (scale == (Real)0)
+            {
+                mSubdiagonal[i0] = mMatrix[i0][i3];
+            }
+            else
+            {
+                Real invScale = ((Real)1)/scale;
+                for (i2 = 0; i2 <= i3; ++i2)
+                {
+                    mMatrix[i0][i2] *= invScale;
+                    value0 += mMatrix[i0][i2]*mMatrix[i0][i2];
+                }
+
+                Real value1 = mMatrix[i0][i3];
+                Real value2 = Math<Real>::Sqrt(value0);
+                if (value1 > (Real)0)
+                {
+                    value2 = -value2;
+                }
+
+                mSubdiagonal[i0] = scale*value2;
+                value0 -= value1*value2;
+                mMatrix[i0][i3] = value1-value2;
+                value1 = (Real)0;
+                Real invValue0 = ((Real)1)/value0;
+                for (i1 = 0; i1 <= i3; ++i1)
+                {
+                    mMatrix[i1][i0] = mMatrix[i0][i1]*invValue0;
+                    value2 = (Real)0;
+                    for (i2 = 0; i2 <= i1; ++i2)
+                    {
+                        value2 += mMatrix[i1][i2]*mMatrix[i0][i2];
+                    }
+                    for (i2 = i1+1; i2 <= i3; ++i2)
+                    {
+                        value2 += mMatrix[i2][i1]*mMatrix[i0][i2];
+                    }
+                    mSubdiagonal[i1] = value2*invValue0;
+                    value1 += mSubdiagonal[i1]*mMatrix[i0][i1];
+                }
+
+                Real value3 = ((Real)0.5)*value1*invValue0;
+                for (i1 = 0; i1 <= i3; ++i1)
+                {
+                    value1 = mMatrix[i0][i1];
+                    value2 = mSubdiagonal[i1] - value3*value1;
+                    mSubdiagonal[i1] = value2;
+                    for (i2 = 0; i2 <= i1; i2++)
+                    {
+                        mMatrix[i1][i2] -=
+                            value1*mSubdiagonal[i2] + value2*mMatrix[i0][i2];
+                    }
+                }
+            }
+        }
+        else
+        {
+            mSubdiagonal[i0] = mMatrix[i0][i3];
+        }
+
+        mDiagonal[i0] = value0;
+    }
+
+    mDiagonal[0] = (Real)0;
+    mSubdiagonal[0] = (Real)0;
+    for (i0 = 0, i3 = -1; i0 <= mSize - 1; ++i0, ++i3)
+    {
+        if (mDiagonal[i0] != (Real)0)
+        {
+            for (i1 = 0; i1 <= i3; ++i1)
+            {
+                Real sum = (Real)0;
+                for (i2 = 0; i2 <= i3; ++i2)
+                {
+                    sum += mMatrix[i0][i2]*mMatrix[i2][i1];
+                }
+                for (i2 = 0; i2 <= i3; ++i2)
+                {
+                    mMatrix[i2][i1] -= sum*mMatrix[i2][i0];
+                }
+            }
+        }
+        mDiagonal[i0] = mMatrix[i0][i0];
+        mMatrix[i0][i0] = (Real)1;
+        for (i1 = 0; i1 <= i3; ++i1)
+        {
+            mMatrix[i1][i0] = (Real)0;
+            mMatrix[i0][i1] = (Real)0;
+        }
+    }
+
+    // Reordering needed by EigenDecomposition::QLAlgorithm.
+    for (i0 = 1, i3 = 0; i0 < mSize; ++i0, ++i3)
+    {
+        mSubdiagonal[i3] = mSubdiagonal[i0];
+    }
+    mSubdiagonal[mSize-1] = (Real)0;
+
+    mIsRotation = ((mSize % 2) == 0);
+}
+//----------------------------------------------------------------------------
+template <typename Real>
+bool EigenDecomposition<Real>::QLAlgorithm ()
+{
+    // TODO:  This is Numerical Recipes in C code.  Rewrite to avoid
+    // copyright issues.
+
+    const int iMaxIter = 32;
+
+    for (int i0 = 0; i0 < mSize; ++i0)
+    {
+        int i1;
+        for (i1 = 0; i1 < iMaxIter; ++i1)
+        {
+            int i2;
+            for (i2 = i0; i2 <= mSize - 2; ++i2)
+            {
+                Real tmp = Math<Real>::FAbs(mDiagonal[i2]) +
+                    Math<Real>::FAbs(mDiagonal[i2+1]);
+
+                if (Math<Real>::FAbs(mSubdiagonal[i2]) + tmp == tmp)
+                {
+                    break;
+                }
+            }
+            if (i2 == i0)
+            {
+                break;
+            }
+
+            Real value0 = (mDiagonal[i0 + 1]
+                - mDiagonal[i0])/(((Real)2)*mSubdiagonal[i0]);
+            Real value1 = Math<Real>::Sqrt(value0*value0 + (Real)1);
+            if (value0 < (Real)0)
+            {
+                value0 = mDiagonal[i2] - mDiagonal[i0]
+                    + mSubdiagonal[i0]/(value0 - value1);
+            }
+            else
+            {
+                value0 = mDiagonal[i2] - mDiagonal[i0]
+                    + mSubdiagonal[i0]/(value0 + value1);
+            }
+
+            Real sn = (Real)1, cs = (Real)1, value2 = (Real)0;
+            for (int i3 = i2 - 1; i3 >= i0; --i3)
+            {
+                Real value3 = sn*mSubdiagonal[i3];
+                Real value4 = cs*mSubdiagonal[i3];
+                if (Math<Real>::FAbs(value3) >= Math<Real>::FAbs(value0))
+                {
+                    cs = value0/value3;
+                    value1 = Math<Real>::Sqrt(cs*cs + (Real)1);
+                    mSubdiagonal[i3 + 1] = value3*value1;
+                    sn = ((Real)1)/value1;
+                    cs *= sn;
+                }
+                else
+                {
+                    sn = value3/value0;
+                    value1 = Math<Real>::Sqrt(sn*sn + (Real)1);
+                    mSubdiagonal[i3 + 1] = value0*value1;
+                    cs = ((Real)1)/value1;
+                    sn *= cs;
+                }
+                value0 = mDiagonal[i3 + 1] - value2;
+                value1 = (mDiagonal[i3] - value0)*sn + ((Real)2)*value4*cs;
+                value2 = sn*value1;
+                mDiagonal[i3 + 1] = value0 + value2;
+                value0 = cs*value1 - value4;
+
+                for (int i4 = 0; i4 < mSize; ++i4)
+                {
+                    value3 = mMatrix[i4][i3 + 1];
+                    mMatrix[i4][i3 + 1] = sn*mMatrix[i4][i3] + cs*value3;
+                    mMatrix[i4][i3] = cs*mMatrix[i4][i3] - sn*value3;
+                }
+            }
+            mDiagonal[i0] -= value2;
+            mSubdiagonal[i0] = value0;
+            mSubdiagonal[i2] = (Real)0;
+        }
+        if (i1 == iMaxIter)
+        {
+            return false;
+        }
+    }
+
+    return true;
+}
+//----------------------------------------------------------------------------
+template <typename Real>
+void EigenDecomposition<Real>::DecreasingSort ()
+{
+    // Sort the eigenvalues in decreasing order, e[0] >= ... >= e[mSize-1]
+    for (int i0 = 0, i1; i0 <= mSize - 2; ++i0)
+    {
+        // Locate the maximum eigenvalue.
+        i1 = i0;
+        Real maxValue = mDiagonal[i1];
+        int i2;
+        for (i2 = i0 + 1; i2 < mSize; ++i2)
+        {
+            if (mDiagonal[i2] > maxValue)
+            {
+                i1 = i2;
+                maxValue = mDiagonal[i1];
+            }
+        }
+
+        if (i1 != i0)
+        {
+            // Swap the eigenvalues.
+            mDiagonal[i1] = mDiagonal[i0];
+            mDiagonal[i0] = maxValue;
+
+            // Swap the eigenvectors corresponding to the eigenvalues.
+            for (i2 = 0; i2 < mSize; ++i2)
+            {
+                Real tmp = mMatrix[i2][i0];
+                mMatrix[i2][i0] = mMatrix[i2][i1];
+                mMatrix[i2][i1] = tmp;
+                mIsRotation = !mIsRotation;
+            }
+        }
+    }
+}
+//----------------------------------------------------------------------------
+template <typename Real>
+void EigenDecomposition<Real>::IncreasingSort ()
+{
+    // Sort the eigenvalues in increasing order, e[0] <= ... <= e[mSize-1]
+    for (int i0 = 0, i1; i0 <= mSize - 2; ++i0)
+    { 
+        // Locate the minimum eigenvalue.
+        i1 = i0;
+        Real minValue = mDiagonal[i1];
+        int i2;
+        for (i2 = i0 + 1; i2 < mSize; ++i2)
+        {
+            if (mDiagonal[i2] < minValue)
+            {
+                i1 = i2;
+                minValue = mDiagonal[i1];
+            }
+        }
+
+        if (i1 != i0)
+        {
+            // Swap the eigenvalues.
+            mDiagonal[i1] = mDiagonal[i0];
+            mDiagonal[i0] = minValue;
+
+            // Swap the eigenvectors corresponding to the eigenvalues.
+            for (i2 = 0; i2 < mSize; ++i2)
+            {
+                Real tmp = mMatrix[i2][i0];
+                mMatrix[i2][i0] = mMatrix[i2][i1];
+                mMatrix[i2][i1] = tmp;
+                mIsRotation = !mIsRotation;
+            }
+        }
+    }
+}
+//----------------------------------------------------------------------------
+template <typename Real>
+void EigenDecomposition<Real>::GuaranteeRotation ()
+{
+    if (!mIsRotation)
+    {
+        // Change sign on the first column.
+        for (int row = 0; row < mSize; ++row)
+        {
+            mMatrix[row][0] = -mMatrix[row][0];
+        }
+    }
+}
+//----------------------------------------------------------------------------
+
+//----------------------------------------------------------------------------
+// Explicit instantiation.
+//----------------------------------------------------------------------------
+template WM5_MATHEMATICS_ITEM
+class EigenDecomposition<float>;
+
+template WM5_MATHEMATICS_ITEM
+class EigenDecomposition<double>;
+
+//----------------------------------------------------------------------------
+template <typename Real>
+Line3<Real> OrthogonalLineFit3 (int numPoints, const Vector3<Real>* points)
+{
+    Line3<Real> line(Vector3<Real>(3), Vector3<Real>(3));
+
+    // Compute the mean of the points.
+    line.Origin = points[0];
+    int i;
+    for (i = 1; i < numPoints; i++)
+    {
+        line.Origin += points[i];
+    }
+    Real invNumPoints = ((Real)1)/numPoints;
+    line.Origin *= invNumPoints;
+
+    // Compute the covariance matrix of the points.
+    Real sumXX = (Real)0, sumXY = (Real)0, sumXZ = (Real)0;
+    Real sumYY = (Real)0, sumYZ = (Real)0, sumZZ = (Real)0;
+    for (i = 0; i < numPoints; i++) 
+    {
+        Vector3<Real> diff = points[i] - line.Origin;
+        sumXX += diff[0]*diff[0];
+        sumXY += diff[0]*diff[1];
+        sumXZ += diff[0]*diff[2];
+        sumYY += diff[1]*diff[1];
+        sumYZ += diff[1]*diff[2];
+        sumZZ += diff[2]*diff[2];
+    }
+
+    sumXX *= invNumPoints;
+    sumXY *= invNumPoints;
+    sumXZ *= invNumPoints;
+    sumYY *= invNumPoints;
+    sumYZ *= invNumPoints;
+    sumZZ *= invNumPoints;
+
+    // Set up the eigensolver.
+    EigenDecomposition<Real> esystem(3);
+    esystem(0,0) = sumYY+sumZZ;
+    esystem(0,1) = -sumXY;
+    esystem(0,2) = -sumXZ;
+    esystem(1,0) = esystem(0,1);
+    esystem(1,1) = sumXX+sumZZ;
+    esystem(1,2) = -sumYZ;
+    esystem(2,0) = esystem(0,2);
+    esystem(2,1) = esystem(1,2);
+    esystem(2,2) = sumXX+sumYY;
+
+    // Compute eigenstuff, smallest eigenvalue is in last position.
+    esystem.Solve(false);
+
+    // Unit-length direction for best-fit line.
+    line.Direction = esystem.GetEigenvector(2);
+
+    return line;
+}
+//----------------------------------------------------------------------------
+
+//----------------------------------------------------------------------------
+// Explicit instantiation.
+//----------------------------------------------------------------------------
+template WM5_MATHEMATICS_ITEM
+Line3<float> OrthogonalLineFit3<float> (int, const Vector3<float>*);
+
+template WM5_MATHEMATICS_ITEM
+Line3<double> OrthogonalLineFit3<double> (int, const Vector3<double>*);
+//----------------------------------------------------------------------------
+
+
+
+} // namespace

tjp/core/math/detail/Wm5.h

@@ -0,0 +1,355 @@
+//
+//  Wm5.h
+//  algorithm
+//
+//  Created by Timothy Prepscius on 5/3/18.
+//  Copyright © 2018 Timothy Prepscius. All rights reserved.
+//
+
+#pragma once
+
+#include <stddef.h>
+#include <memory.h>
+#include <math.h>
+
+namespace Wm5 {
+
+#define WM5_MATHEMATICS_ITEM 
+#define assertion(...)
+
+template <typename T>
+T *new1(size_t size)
+{
+	return new T[size];
+}
+
+template <typename T>
+void delete1(T *t)
+{
+	delete[] t;
+}
+
+// -----
+
+template <typename Real>
+struct Math
+{
+    WM5_MATHEMATICS_ITEM static const Real ZERO_TOLERANCE;
+
+    static Real FAbs (Real value);
+	static Real Sqrt (Real value);
+} ;
+
+typedef Math<float> Mathf;
+typedef Math<double> Mathd;
+
+// -----
+
+template <typename Real>
+class GVector
+{
+public:
+    // Construction and destruction.
+    GVector (int size = 0);
+    GVector (int size, const Real* tuple);
+    GVector (const GVector& vec);
+    ~GVector ();
+
+    // Coordinate access.
+    void SetSize (int size);
+    inline int GetSize () const;
+    inline operator const Real* () const;
+    inline operator Real* ();
+    inline const Real& operator[] (int i) const;
+    inline Real& operator[] (int i);
+
+    // Assignment.
+    GVector& operator= (const GVector& vec);
+
+    // Comparison (for use by STL containers).
+    bool operator== (const GVector& vec) const;
+    bool operator!= (const GVector& vec) const;
+    bool operator<  (const GVector& vec) const;
+    bool operator<= (const GVector& vec) const;
+    bool operator>  (const GVector& vec) const;
+    bool operator>= (const GVector& vec) const;
+
+    // Arithmetic operations.
+    GVector operator+ (const GVector& vec) const;
+    GVector operator- (const GVector& vec) const;
+    GVector operator* (Real scalar) const;
+    GVector operator/ (Real scalar) const;
+    GVector operator- () const;
+
+    friend GVector operator* (Real scalar, const GVector& vec)
+    {
+        return vec*scalar;
+    }
+
+    // Arithmetic updates.
+    GVector& operator+= (const GVector& vec);
+    GVector& operator-= (const GVector& vec);
+    GVector& operator*= (Real scalar);
+    GVector& operator/= (Real scalar);
+
+    // Vector operations.
+    Real Length () const;
+    Real SquaredLength () const;
+    Real Dot (const GVector& vec) const;
+    Real Normalize (Real epsilon = Math<Real>::ZERO_TOLERANCE);
+
+protected:
+    int mSize;
+    Real* mTuple;
+};
+
+typedef GVector<float> GVectorf;
+typedef GVector<double> GVectord;
+
+// -----
+
+template <typename Real>
+class GMatrix
+{
+public:
+    // Construction and destruction.
+    GMatrix (int rows = 0, int columns = 0);
+    GMatrix (int rows, int columns, const Real* entry);
+    GMatrix (int rows, int columns, const Real** matrix);
+    GMatrix (const GMatrix& mat);
+    ~GMatrix ();
+
+    // Coordinate access.
+    void SetSize (int rows, int columns);
+    inline void GetSize (int& rows, int& columns) const;
+    inline int GetRows () const;
+    inline int GetColumns () const;
+    inline int GetQuantity () const;
+    inline operator const Real* () const;
+    inline operator Real* ();
+    inline const Real* operator[] (int row) const;
+    inline Real* operator[] (int row);
+    inline const Real& operator() (int row, int column) const;
+    inline Real& operator() (int row, int column);
+    void SwapRows (int row0, int row1);
+    void SetRow (int row, const GVector<Real>& vec);
+    GVector<Real> GetRow (int row) const;
+    void SetColumn (int column, const GVector<Real>& vec);
+    GVector<Real> GetColumn (int column) const;
+    void SetMatrix (int rows, int columns, const Real* entry);
+    void SetMatrix (int rows, int columns, const Real** matrix);
+    void GetColumnMajor (Real* columnMajor) const;
+
+    // Assignment.
+    GMatrix& operator= (const GMatrix& mat);
+
+    // Comparison (for use by STL containers).
+    bool operator== (const GMatrix& mat) const;
+    bool operator!= (const GMatrix& mat) const;
+    bool operator<  (const GMatrix& mat) const;
+    bool operator<= (const GMatrix& mat) const;
+    bool operator>  (const GMatrix& mat) const;
+    bool operator>= (const GMatrix& mat) const;
+
+    // Arithmetic operations.
+    GMatrix operator+ (const GMatrix& mat) const;
+    GMatrix operator- (const GMatrix& mat) const;
+    GMatrix operator* (const GMatrix& mat) const;
+    GMatrix operator* (Real fScalar) const;
+    GMatrix operator/ (Real fScalar) const;
+    GMatrix operator- () const;
+
+    // Arithmetic updates.
+    GMatrix& operator+= (const GMatrix& mat);
+    GMatrix& operator-= (const GMatrix& mat);
+    GMatrix& operator*= (Real fScalar);
+    GMatrix& operator/= (Real fScalar);
+
+    // M*vec
+    GVector<Real> operator* (const GVector<Real>& vec) const;
+
+    // u^T*M*v
+    Real QForm (const GVector<Real>& u, const GVector<Real>& v) const;
+
+    // M^T
+    GMatrix Transpose () const;
+
+    // M^T*mat
+    GMatrix TransposeTimes (const GMatrix& mat) const;
+
+    // M*mat^T
+    GMatrix TimesTranspose (const GMatrix& mat) const;
+
+    // M^T*mat^T
+    GMatrix TransposeTimesTranspose (const GMatrix& mat) const;
+
+    // Inversion.  The matrix must be square.  The function returns 'true'
+    // whenever the matrix is square and invertible.
+    bool GetInverse (GMatrix<Real>& inverse) const;
+
+    // c * M
+    friend GMatrix<Real> operator* (Real scalar, const GMatrix<Real>& mat)
+    {
+        return mat*scalar;
+    }
+
+    // v^T * M
+    friend GVector<Real> operator* (const GVector<Real>& vec,
+        const GMatrix<Real>& mat)
+    {
+        assertion(vec.GetSize() == mat.GetRows(), "Mismatch in operator*\n");
+        GVector<Real> prod(mat.GetColumns());
+        Real* entry = prod;
+        for (int c = 0; c < mat.GetColumns(); ++c)
+        {
+            for (int r = 0; r < mat.GetRows(); ++r)
+            {
+                entry[c] += vec[r]*mat[r][c];
+            }
+        }
+        return prod;
+    }
+
+protected:
+    // Support for allocation and deallocation.  The allocation call requires
+    // m_iRows, m_iCols, and m_iQuantity to have already been correctly
+    // initialized.
+    void Allocate (bool setToZero);
+    void Deallocate ();
+
+    int mRows, mColumns, mQuantity;
+
+    // The matrix is stored in row-major form as a 1-dimensional array.
+    Real* mData;
+
+    // An array of pointers to the rows of the matrix.  The separation of
+    // row pointers and actual data supports swapping of rows in linear
+    // algebraic algorithms such as solving linear systems of equations.
+    Real** mEntry;
+};
+
+
+typedef GMatrix<float> GMatrixf;
+typedef GMatrix<double> GMatrixd;
+
+// -----
+
+
+template <typename Real>
+class WM5_MATHEMATICS_ITEM EigenDecomposition
+{
+public:
+    // Construction and destruction.  The matrix of an eigensystem must be
+    // symmetric.
+    EigenDecomposition (int size);
+//    EigenDecomposition (const Matrix2<Real>& mat);
+//    EigenDecomposition (const Matrix3<Real>& mat);
+    EigenDecomposition (const GMatrix<Real>& mat);
+    ~EigenDecomposition ();
+
+    // Set the matrix for the eigensystem.
+    Real& operator() (int row, int column);
+//    EigenDecomposition& operator= (const Matrix2<Real>& mat);
+//    EigenDecomposition& operator= (const Matrix3<Real>& mat);
+    EigenDecomposition& operator= (const GMatrix<Real>& mat);
+
+    // Solve the eigensystem.  Set 'increasingSort' to 'true' when you want
+    // the eigenvalues to be sorted in increasing order; otherwise, the
+    // eigenvalues are sorted in decreasing order.
+    void Solve (bool increasingSort);
+
+    // Get the results.  The calls to GetEigenvector2, GetEigenvectors2,
+    // GetEigenvector3, and GetEigenvector3 should be made only if you know
+    // that the eigensystem is of the corresponding size.
+    Real GetEigenvalue (int i) const;
+    const Real* GetEigenvalues () const;
+//    Vector2<Real> GetEigenvector2 (int i) const;
+//    Matrix2<Real> GetEigenvectors2 () const;
+//    Vector3<Real> GetEigenvector3 (int i) const;
+//    Matrix3<Real> GetEigenvectors3 () const;
+    GVector<Real> GetEigenvector (int i) const;
+    const GMatrix<Real>& GetEigenvectors () const;
+
+private:
+    int mSize;
+    GMatrix<Real> mMatrix;
+    Real* mDiagonal;
+    Real* mSubdiagonal;
+
+    // For odd size matrices, the Householder reduction involves an odd
+    // number of reflections.  The product of these is a reflection.  The
+    // QL algorithm uses rotations for further reductions.  The final
+    // orthogonal matrix whose columns are the eigenvectors is a reflection,
+    // so its determinant is -1.  For even size matrices, the Householder
+    // reduction involves an even number of reflections whose product is a
+    // rotation.  The final orthogonal matrix has determinant +1.  Many
+    // algorithms that need an eigendecomposition want a rotation matrix.
+    // We want to guarantee this is the case, so mRotation keeps track of
+    // this.  The DecrSort and IncrSort further complicate the issue since
+    // they swap columns of the orthogonal matrix, causing the matrix to
+    // toggle between rotation and reflection.  The value mRotation must
+    // be toggled accordingly.
+    bool mIsRotation;
+    void GuaranteeRotation ();
+
+    // Householder reduction to tridiagonal form.
+    void Tridiagonal2 ();
+    void Tridiagonal3 ();
+    void TridiagonalN ();
+
+    // QL algorithm with implicit shifting.  This function is called for
+    // tridiagonal matrices.
+    bool QLAlgorithm ();
+
+    // Sort eigenvalues from largest to smallest.
+    void DecreasingSort ();
+
+    // Sort eigenvalues from smallest to largest.
+    void IncreasingSort ();
+};
+
+typedef EigenDecomposition<float> EigenDecompositionf;
+typedef EigenDecomposition<double> EigenDecompositiond;
+
+
+// -----
+
+template<typename Real>
+using Vector3 = GVector<Real>;
+
+template <typename Real>
+class Line3
+{
+public:
+    // The line is represented as P+t*D where P is the line origin, D is a
+    // unit-length direction vector, and t is any real number.  The user must
+    // ensure that D is indeed unit length.
+
+    // Construction and destruction.
+    Line3 () {};  // uninitialized
+    ~Line3 () {};
+
+    Line3 (const Vector3<Real>& origin, const Vector3<Real>& direction) :
+		Origin(origin),
+		Direction(direction)
+	{
+	}
+
+    Vector3<Real> Origin, Direction;
+};
+
+typedef Line3<float> Line3f;
+typedef Line3<double> Line3d;
+
+// -------
+
+// Least-squares fit of a line to (x,y,z) data by using distance measurements
+// orthogonal to the proposed line.
+template <typename Real> WM5_MATHEMATICS_ITEM
+Line3<Real> OrthogonalLineFit3 (int numPoints, const Vector3<Real>* points);
+
+// ------
+
+#include "Wm5.inl"
+
+} // namespace