Aldo Marchioni, Brescia (Italy) - Free source code written by Aldo Marchioni. Linear algebra library, vector, matrix, scalar product, cross product, matrix inversion, determinant computation. C++ template library.

C++ linear algebra template library

This is the linear algebra library that I wrote for my own use, in the beginning to calculate rotations and roto-translation of vectors in space.
You can find, free of charge, other linear algebra libraries, even open source like this one, an probably better than this one. And I have seen they are of two kinds: simple to understand, and fully comprehensive.
Those easier to understand usually are just a little bit more than academic exercises. They are useful for those who know little or nothing of linear algebra, and need to understand something, and need some easy implementation for some very specific task, but they are of little or no use in real programs. Comprehensive libraries, on the other hand, are beautiful, and they really do everything. They are much, much more than you really need in practice. But like everything that is powerful, however, they are usually difficult or very difficult to use and control. This applies to software as to cars, motorcycles, watch dogs ...
After considering a couple of those comprehensive libraries, I realized that if I wrote a simple one, but comprehensive for my needs, I needed less time than to master and integrate an existing one in my development environment.
Here there is everything I need, or i needed, and nothing more. Namely:
Scalar (dot) product of vectors, and cross or external product
Product of matrices, row by columns
Determinant computation
Matrix inversion: computation of the inverse matrix
Calculation of eigenvalues and eigenvectors (for symmetric matrices only)
Construction of rotation matrices, in plane and in space
The library is complete and fully functional, and it is in the template form: therefore, it requires no special prerequisites, just include it as a header file in your source code, and any decent program calling itself a C++ compiler should be able to compile. I used it in Borland C++ environment, from Builder 5 to RAD 2007, and usually I use the double type, not float.
There is also room for some optimizations: for example, the calculation of the determinant is based on the definition of the determinant itself, that is, it is done by calculating all the covariant factors of the matrix. The method is notoriously very inefficient, and matrices of a certain size could make your CPU to sweat some.
The calculation of eigenvalues and eigenvectors, which uses the very good Jacobi algorithm, optimized, is limited to symmetric matrices. Some willing soul could try to extend the library to any matrix.
The same good willing soul, or another one, could try to extend the library to complex numbers. This is a template, so almost everything should work on any kind of object T, given the four operations, and square root. However, certainly the reversion of matrix currently only works with real numbers. Of course, to extend the library it is not enough to be able to program, some mathematic skill is needed as well.
//---------------------------------------------------------------------------
#ifndef AFMTvectorH
#define AFMTvectorH
//
// Aldo F. Marchioni, Brescia, Italy
// Last update: March 2011
//
//---------------------------------------------------------------------------
#include <algorith.h>
#include <math.hpp>
#include <math.h>
//---------------------------------------------------------------------------

// Normally 3, or 4 if you use quaternions
extern int AFMmatrixDefaultSize;

template <typename T> class AFMTmatrix;

template <typename T = double>
class AFMTvector
{
public:
    typedef enum {
        Empty,
        Iversor, Jversor, Kversor,
        Xversor, Yversor, Zversor
    } AFMvectorType;

public:
    AFMTvector(AFMvectorType Type);
    AFMTvector() { init(AFMmatrixDefaultSize, true); }
    AFMTvector(const int size) { init(size, true) ; }
    AFMTvector(const AFMTvector<T> &v)
    {
        init(v.l, false);
        memcpy (data, v.data, memSize);
    }
    AFMTvector(const AFMTvector<T> *v)
    {
        init(v->l, false);
        memcpy (data, v->data, memSize);
    }
    AFMTvector(T x, T y)
    {
        init(2, false);
        data[0] = x; data[1] = y;
    }
    AFMTvector(T x, T y, T z)
    {
        init(3, false);
        data[0] = x; data[1] = y; data[2] = z;
    }
    AFMTvector(T x, T y, T z, T t)
    {
        init(4, false);
        data[0] = x; data[1] = y; data[2] = z; data[3] = t;
    }
    AFMTvector(T x, T y, T z, T alfa, T beta)
    {
        init(5, false);
        data[0] = x ; data[1] = y; data[2] = z; data[3] = alfa; data[4] = beta;
    }
    AFMTvector(T x, T y, T z, T alfa, T beta, T gamma)
    {
        init(6, false);
        data[0] = x; data[1] = y; data[2] = z;
        data[3] = alfa; data[4] = beta; data[5] = gamma;
    }
   ~AFMTvector() { delete[] data ; data = NULL ; }

public:
    const T& operator[](int i) const { return data[i]; }
    T& operator[](int i) { return data[i]; }

    AFMTvector<T>& operator=(const AFMTvector<T> &v) ;

    bool operator==(const AFMTvector<T> &v) const ;
    bool operator!=(const AFMTvector<T> &v) const ;

    AFMTvector<T> operator-(void) ;
    AFMTvector<T> operator+(AFMTvector<T> &v) ;
    AFMTvector<T> operator-(AFMTvector<T> &v) ;
    AFMTvector<T> operator+(const AFMTvector<T> &v) const ;
    AFMTvector<T> operator-(const AFMTvector<T> &v) const ;
    AFMTvector<T> operator+=(const AFMTvector<T> &v) ;
    AFMTvector<T> operator-=(const AFMTvector<T> &v) ;

    AFMTvector<T> operator*(const T d) ;
    AFMTvector<T> operator/(const T d) ;
    AFMTvector<T> operator*=(const T d) ;
    AFMTvector<T> operator/=(const T d) ;

    AFMTvector<T> operator*(const AFMTmatrix<T> &m) ;        // Linear Transformation
    AFMTvector<T> operator*=(const AFMTmatrix<T> &m) ;        // Linear Transformation

    T operator*(const AFMTvector<T> &v) ;                    // Scalar (dot) product
    T operator*(const AFMTvector<T> &v) const ;                // Scalar (dot) product
    AFMTvector<T> Vector(const AFMTvector<T>& v) ;            // Vector product
    AFMTvector<T> Vector(const AFMTvector<T>& v) const ;    // Vector product

    AFMTvector<T> RotateDeg(const double Psi, const double Teta, const double Phi) ;
    AFMTvector<T> RotateRad(const double Psi, const double Teta, const double Phi) ;

public:
    void I(); // Unit canonical vector
    void J(); // Unit canonical vector
    void K(); // Unit canonical vector
    AFMTvector<T> N() ; // Reduce itself to a unit vector
    AFMTvector<T> Zero() ;
    AFMTvector<T> SetLength(double len) ;

public:
    __property int l = {read=_m} ;
    __property T x = {read=getX, write=setX} ;
    __property T y = {read=getY, write=setY} ;
    __property T z = {read=getZ, write=setZ} ;
    __property T t = {read=getAlfa, write=setAlfa} ;
    __property T alfa = {read=getAlfa, write=setAlfa} ;
    __property T beta = {read=getBeta, write=setBeta} ;
    __property T gamma = {read=getGamma, write=setGamma} ;
    __property T a = {read=getX, write=setX} ;
    __property T b = {read=getY, write=setY} ;
    __property T c = {read=getZ, write=setZ} ;
    __property T d = {read=getAlfa, write=setAlfa} ;
    __property T e = {read=getBeta, write=setBeta} ;
    __property T f = {read=getGamma, write=setGamma} ;
    __property T LengthSquare = {read=getLengthSquare} ;
    __property double Length = {read=getLength} ;
    __property bool IsNull = {read=getIsNull} ;

protected:
    T *data ;

private:
    int _m ;
    int memSize ;

    T getX() const { return data[0] ; }
    T getY() const { return _m > 1 ? data[1] : 0 ; }
    T getZ() const { return _m > 2 ? data[2] : 0 ; }
    T getAlfa() const { return _m > 3 ? data[3] : 0 ; }
    T getBeta() const { return _m > 4 ? data[4] : 0 ; }
    T getGamma() const { return _m > 5 ? data[5] : 0 ; }
    void setX(T value) { data[0] = value ; }
    void setY(T value) { if (_m > 1) data[1] = value ; }
    void setZ(T value) { if (_m > 2) data[2] = value ; }
    void setAlfa(T value) { if (_m > 3) data[3] = value ; }
    void setBeta(T value) { if (_m > 4) data[4] = value ; }
    void setGamma(T value) { if (_m > 5) data[5] = value ; }

    bool getIsNull(void) const ;
    double getLength(void) const ;
    T getLengthSquare(void) const ;

    void init(int m, bool clear) ;
    inline void zero() { memset(data, 0, memSize) ; } ;
} ;


template <typename T>
AFMTvector<T>::AFMTvector(AFMvectorType Type)
{
    switch (Type) {
    case AFMTvector<T>::Empty:
        init(AFMmatrixDefaultSize, true);
        break;
    case AFMTvector<T>::Iversor:
    case AFMTvector<T>::Xversor:
        init(3, true);
        data[0] = 1;
        break;
    case AFMTvector<T>::Jversor:
    case AFMTvector<T>::Yversor:
        init(3, true);
        data[1] = 1;
        break;
    case AFMTvector<T>::Kversor:
    case AFMTvector<T>::Zversor:
        init(3, true);
        data[2] = 1;
        break;
    default:
        init(AFMmatrixDefaultSize, true);
        break;
    }
}

template <typename T>
void AFMTvector<T>::init(int m, bool clear)
{
    _m = m ;
    data = new T[m] ;
    memSize = sizeof(T) * m ;
    if (clear) zero() ;
}

template <typename T>
bool AFMTvector<T>::getIsNull(void) const
{
    for (int i = 0; i < l; i++)
        if (data[i] != 0) return false ;
    return true ;
}

template <typename T>
double AFMTvector<T>::getLength(void) const
{
    double radical = getLengthSquare() ;
       return (radical > 0 ? sqrt(radical) : 0) ;
}

template <typename T>
T AFMTvector<T>::getLengthSquare(void) const
{
    T result = 0 ;
    for (int i = 0; i < l; i++) result += (data[i] * data[i]) ;
    return result ;
}

template <typename T>
AFMTvector<T>& AFMTvector<T>::operator=(const AFMTvector<T> &v)
{
    if (this != &v) {
        if (l != v.l)
            throw EInvalidArgument("Incongruent vector dimension in assignement operation") ;
        memcpy(data, v.data, memSize) ;
    }
    return *this ;
}

template <typename T>
bool AFMTvector<T>::operator==(const AFMTvector<T> &v) const
{
    if (l != v.l) return false ;
    for (int i = 0; i < l; i++)
        if (data[i] != v[i]) return false ;
    return true ;
}

template <typename T>
bool AFMTvector<T>::operator!=(const AFMTvector<T> &v) const
{
    if (l != v.l) return true ;
    for (int i = 0; i < l; i++)
        if (data[i] != v[i]) return true ;
    return false ;
}

template <typename T>
AFMTvector<T> AFMTvector<T>::operator-(void)
{
    AFMTvector result(this) ;
    for (int i = 0; i < l; i++)
        result[i] = -data[i] ;
    return result ;
}

template <typename T>
AFMTvector<T> AFMTvector<T>::operator+(AFMTvector<T>& v)
{
    int len = Min(l, v.l) ;
    AFMTvector result(this) ;
    for (int i = 0; i < len; i++)
        result[i] = data[i] + v[i] ;
    return result ;
}

template <typename T>
AFMTvector<T> AFMTvector<T>::operator-(AFMTvector<T>& v)
{
    int len = Min(l, v.l) ;
    AFMTvector result(this) ;
    for (int i = 0; i < len; i++)
        result[i] = data[i] - v[i] ;
    return result ;
}

template <typename T>
AFMTvector<T> AFMTvector<T>::operator+(const AFMTvector<T>& v) const
{
    int len = Min(l, v.l) ;
    AFMTvector result(this) ;
    for (int i = 0; i < len; i++)
        result[i] = data[i] + v[i] ;
    return result ;
}

template <typename T>
AFMTvector<T> AFMTvector<T>::operator-(const AFMTvector<T>& v) const
{
    int len = Min(l, v.l) ;
    AFMTvector result(this) ;
    for (int i = 0; i < len; i++)
        result[i] = data[i] - v[i] ;
    return result ;
}

template <typename T>
AFMTvector<T> AFMTvector<T>::operator+=(const AFMTvector<T>& v)
{
    int len = Min(l, v.l) ;
    for (int i = 0; i < len; i++)
        data[i] += v[i] ;
    return this ;
}

template <typename T>
AFMTvector<T> AFMTvector<T>::operator-=(const AFMTvector<T>& v)
{
    int len = Min(l, v.l) ;
    for (int i = 0; i < len; i++)
        data[i] -= v[i] ;
    return this ;
}


    // Scalar multiplication & division
template <typename T>
AFMTvector<T> AFMTvector<T>::operator*(const T d)
{
    AFMTvector result(l) ;
    for (int i = 0; i < l; i++)
        result[i] = data[i] * d ;
    return result ;
}

template <typename T>
AFMTvector<T> AFMTvector<T>::operator/(const T d)
{
    AFMTvector result(l) ;
    for (int i = 0; i < l; i++)
        result[i] = data[i] / d ;
    return result ;
}

template <typename T>
AFMTvector<T> AFMTvector<T>::operator*=(const T d)
{
    for (int i = 0; i < l; i++)
        data[i] *= d ;
    return this ;
}

template <typename T>
AFMTvector<T> AFMTvector<T>::operator/=(const T d)
{
    for (int i = 0; i < l; i++)
        data[i] /= d ;
    return this ;
}

    // Scalar (dot) product
template <typename T>
T AFMTvector<T>::operator*(const AFMTvector<T> &v)
{
    if (l != v.l)
        throw EInvalidArgument("Incongruent vector dimension in vector scalar product") ;
    T result = 0;
    for (int i = 0; i < l; i++)
        result += (data[i] * v[i]);
    return result;
}

template <typename T>
T AFMTvector<T>::operator*(const AFMTvector<T> &v) const
{
    if (l != v.l)
        throw EInvalidArgument("Incongruent vector dimension in vector scalar product") ;
    T result = 0;
    for (int i = 0; i < l; i++)
        result += (data[i] * v[i]) ;
    return result ;
}

    // Linear Transformation
template <typename T>
AFMTvector<T> AFMTvector<T>::operator*(const AFMTmatrix<T> &m)
{
    if (m.n != l)
        throw EInvalidArgument("Incongruent matrix dimension in linear transformation") ;

    AFMTvector result(l) ;
    for (int i = 0 ; i < l ; i++) {
           T mult = 0 ;
        T *pRow = m[i] ;
        for (int k = 0 ; k < l ; k++)
               mult += *pRow++ * data[k] ;
        result[i] = mult ;
    }
    return result ;
}

template <typename T>
AFMTvector<T> AFMTvector<T>::operator*=(const AFMTmatrix<T> &m)
{
    if (m.n != l)
        throw EInvalidArgument("Incongruent matrix dimension in linear transformation") ;
    AFMTvector result(l) ;
    for (int i = 0 ; i < l ; i++) {
           T mult = 0 ;
        T *pRow = m[i] ;
        for (int k = 0 ; k < l ; k++)
               mult += *pRow++ * data[k] ;
        result[i] = mult ;
    }
    *this = result ;
    return this ;
}

    // Vector product
template <typename T>
AFMTvector<T> AFMTvector<T>::Vector(const AFMTvector<T>& v)
{
    if (l != 3)
        throw EInvalidArgument("Extern vector product is implemented for 3D vectors only. Sorry.") ;
    if (v.l != l)
        throw EInvalidArgument("Incongruent vector dimensions in extern product") ;

    T x1 =  (y * v.z - z * v.y);
    T y1 = -(x * v.z - z * v.x);
    T z1 =  (x * v.y - y * v.x);

    return AFMTvector<T>(x1, y1, z1) ;
}

template <typename T>
AFMTvector<T> AFMTvector<T>::Vector(const AFMTvector<T>& v) const
{
    if (l != 3)
        throw EInvalidArgument("Extern vector product is implemented for 3D vectors only. Sorry.") ;
    if (v.l != l)
        throw EInvalidArgument("Incongruent vector dimensions in extern product") ;

    T x1 =  (y * v.z - z * v.y);
    T y1 = -(x * v.z - z * v.x);
    T z1 =  (x * v.y - y * v.x);

    return AFMTvector<T>(x1, y1, z1) ;
}

    // Unit canonical vectors
template <typename T>
void AFMTvector<T>::I()
{
    zero() ;
    data[0] = 1 ;
}

template <typename T>
void AFMTvector<T>::J()
{
    zero() ;
    data[1] = 1 ;
}

template <typename T>
void AFMTvector<T>::K()
{
    zero() ;
    data[2] = 1 ;
}

    // Normalize the vector
template <typename T>
AFMTvector<T> AFMTvector<T>::N()
{
    if (Length > 0) *this /= Length ;
    return this ;
}

template <typename T>
AFMTvector<T> AFMTvector<T>::SetLength(double len)
{
    if (Length > 0) {
        double k = len / Length ;
        *this *= k ;
    }
    return this ;
}

template <typename T>
AFMTvector<T> AFMTvector<T>::Zero()
{
    zero() ;
    return this ;
}

template <typename T>
AFMTvector<T> AFMTvector<T>::RotateDeg(const double Psi, const double Teta, const double Phi)
{
    AFMTmatrix<T> m(3,3) ;
    m.RotationDeg(Psi, Teta, Phi) ;
    *this *= m ;
    return this ;
}

template <typename T>
AFMTvector<T> AFMTvector<T>::RotateRad(const double Psi, const double Teta, const double Phi)
{
    AFMTmatrix<T> m(3,3) ;
    m.RotationRad(Psi, Teta, Phi) ;
    *this *= m ;
    return this ;
}


//---------------------------------------------------------------------------
//---------------------------------------------------------------------------
//---------------------------------------------------------------------------

template <typename T = double>
class AFMTmatrix
{
public:
    AFMTmatrix() { init(AFMmatrixDefaultSize, AFMmatrixDefaultSize, true) ; }
    AFMTmatrix(int rows, int columns) { init(rows, columns, true) ; }
    AFMTmatrix(int order) { init(order, order, true) ; }
    AFMTmatrix(AFMTmatrix<T> &mt) ;
    AFMTmatrix(AFMTmatrix<T> *mt) ;
   ~AFMTmatrix(void) ;

public:
    T* operator[](int i) const { return data[i]; }
    T* operator[](int i) { return data[i]; }

    AFMTmatrix<T>& operator=(const AFMTmatrix<T> &M) ;

    bool operator ==(const AFMTmatrix<T> &M) const;
    bool operator !=(const AFMTmatrix<T> &M) const;

    AFMTmatrix<T> operator+(const AFMTmatrix<T> &M);
    AFMTmatrix<T> operator-(const AFMTmatrix<T> &M);
    AFMTmatrix<T> operator+=(const AFMTmatrix<T> &M);
    AFMTmatrix<T> operator-=(const AFMTmatrix<T> &M);

    AFMTmatrix<T> operator*(const T d) ;
    AFMTmatrix<T> operator/(const T d) ;
    AFMTmatrix<T> operator*=(const T d) ;
    AFMTmatrix<T> operator/=(const T d) ;

    AFMTvector<T> operator*(const AFMTvector<T> &v);
    AFMTmatrix<T> operator*(const AFMTmatrix<T> &M);
    AFMTmatrix<T> operator*=(const AFMTmatrix<T> &M);

    AFMTmatrix<T> Invert();
    AFMTmatrix<T> Translate();
    T Determinant(void);
    bool IsSimmetric();

    AFMTvector<T> ComputeEigenvalues(double Tolerance = 0.00001);
    __property AFMTvector<T> Eigenvalues = {read=getEigenvalues};
    __property AFMTvector<T> Eigenvector[int i] = {read=getEigenvector};
    void FreeEigenvalues();

public:
    AFMTmatrix<T> I();
    AFMTmatrix<T> Zero(void);
    AFMTmatrix<T> RotationDeg(const double Psi, const double Teta, const double Phi);
    AFMTmatrix<T> RotationRad(const double Psi, const double Teta, const double Phi);
    AFMTmatrix<T> RotationAToB(AFMTvector<T> A, AFMTvector<T> B);

public:
    __property int m = {read=_m};
    __property int n = {read=_n};

private:
    int _m, _n ;
    int rowSize ;
protected:
    T **data ;
    void init(int m, int n, bool clear);

private:
    AFMTvector<T> getEigenvalues();
    AFMTvector<T> getEigenvector(int i);
protected:
    AFMTvector<T> *eigenvalues;
    AFMTmatrix<T> *eigenvectors;
} ;

template <typename T>
AFMTmatrix<T>::AFMTmatrix(AFMTmatrix &mt)
{
    init (mt.m, mt.n, false) ;
    for (int i = 0; i < m; i++)
        memcpy (data[i], mt[i], rowSize) ;
}

template <typename T>
AFMTmatrix<T>::AFMTmatrix(AFMTmatrix *mt)
{
    init (mt->m, mt->n, false) ;
    for (int i = 0; i < m; i++)
        memcpy (data[i], mt->data[i], rowSize) ;
}

template <typename T>
void AFMTmatrix<T>::init(int m, int n, bool clear)
{
    _m = m ;
    _n = n ;
    data = new T*[m];
    rowSize = sizeof(T) * n;
    for (int i = 0; i < m; i++) {
        data[i] = new T[n];
        if (clear) memset(data[i], 0, rowSize);
    }
    eigenvectors = NULL;
    eigenvalues = NULL;
}

template <typename T>
AFMTmatrix<T>::~AFMTmatrix(void)
{
    if (data != NULL) {
        for (int i = 0; i < m;  i++)
           delete[] data[i] ;
        delete[] data ;
        data = NULL ;
    }
    delete eigenvectors;
    delete eigenvalues;
}

template <typename T>
AFMTmatrix<T>& AFMTmatrix<T>::operator=(const AFMTmatrix &M)
{
    if (this != &M) {
        if (m != M.m || n != M.n)
            throw EInvalidArgument("Incongruent matrix dimensions in assignement operation") ;
        for (int i = 0; i < m; i++)
            memcpy(data[i], M[i], rowSize);
    }
    return *this ;
}

template <typename T>
bool AFMTmatrix<T>::operator==(const AFMTmatrix &M) const
{
    if (m != M.m || n != M.n)
           throw EInvalidArgument("Incongruent matrix dimensions in comparision operation") ;
    for (int i = 0; i < m; i++)
        for (int j = 0; j < n; j++)
            if (data[i][j] != M[i][j])
                return false ;
    return true ;
}

template <typename T>
bool AFMTmatrix<T>::operator!=(const AFMTmatrix &M) const
{
    if (m != M.m || n != M.n)
           throw EInvalidArgument("Incongruent matrix dimensions in comparision operation") ;
    for (int i = 0; i < m; i++)
        for (int j = 0; j < n; j++)
            if (data[i][j] != M[i][j])
                return true ;
    return false ;
}

template <typename T>
AFMTmatrix<T> AFMTmatrix<T>::operator+(const AFMTmatrix& M)
{
    if (m != M.m || n != M.n)
           throw EInvalidArgument("Incongruent matrix dimensions in sum operation") ;
    AFMTmatrix result(m, n) ;
    for (int i = 0; i < m; i++)
        for (int j = 0; j < n; j++)
            result[i][j] = data[i][j] + M[i][j] ;
    return result ;
}

template <typename T>
AFMTmatrix<T> AFMTmatrix<T>::operator-(const AFMTmatrix& M)
{
    if (m != M.m || n != M.n)
           throw EInvalidArgument("Incongruent matrix dimensions in subtraction operation") ;
    AFMTmatrix result(m, n) ;
    for (int i = 0; i < m; i++)
        for (int j = 0; j < n; j++)
            result[i][j] = data[i][j] - M[i][j] ;
    return result ;
}

template <typename T>
AFMTmatrix<T> AFMTmatrix<T>::operator+=(const AFMTmatrix& M)
{
    if (m != M.m || n != M.n)
           throw EInvalidArgument("Incongruent matrix dimensions in sum operation") ;
    for (int i = 0; i < m; i++)
        for (int j = 0; j < n; j++)
            data[i][j] += M[i][j] ;
    return this;
}

template <typename T>
AFMTmatrix<T> AFMTmatrix<T>::operator-=(const AFMTmatrix& M)
{
    if (m != M.m || n != M.n)
           throw EInvalidArgument("Incongruent matrix dimensions in subtraction operation") ;
    for (int i = 0; i < m; i++)
        for (int j = 0; j < n; j++)
            data[i][j] -= M[i][j] ;
    return this;
}

template <typename T>
AFMTmatrix<T> AFMTmatrix<T>::operator*(const T d)
{
    AFMTmatrix result(m, n) ;
    for (int i = 0; i < m; i++)
        for (int j = 0; j < n; j++)
            result[i][i] = data[i][j] * d ;
    return result ;
}

template <typename T>
AFMTmatrix<T> AFMTmatrix<T>::operator/(const T d)
{
    AFMTmatrix result(m, n) ;
    for (int i = 0; i < m; i++)
        for (int j = 0; j < n; j++)
            result[i][i] = data[i][j] / d ;
    return result ;
}

template <typename T>
AFMTmatrix<T> AFMTmatrix<T>::operator*=(const T d)
{
    for (int i = 0; i < m; i++)
        for (int j = 0; j < n; j++)
            data[i][j] *= d ;
    return this;
}

template <typename T>
AFMTmatrix<T> AFMTmatrix<T>::operator/=(const T d)
{
    for (int i = 0; i < m; i++)
        for (int j = 0; j < n; j++)
            data[i][j] /= d ;
    return this;
}

template <typename T>
AFMTvector<T> AFMTmatrix<T>::operator*(const AFMTvector<T> &v)
{
    if (n != v.l)
        throw EInvalidArgument("Incongruent matrix dimension in linear transformation") ;

    AFMTvector<T> result(n) ;
    for (int i = 0 ; i < m ; i++) {
           T mult = 0 ;
        for (int j = 0 ; j < n ; j++)
               mult += data[i][j] * v[j] ;
        result[i] = mult ;
    }
    return result ;
}

template <typename T>
AFMTmatrix<T> AFMTmatrix<T>::operator*(const AFMTmatrix &M)
{
       T mult;

    if (n != M.m || m != M.n)
        throw EInvalidArgument("Incongruent matrix dimension in linear transformation") ;

    AFMTmatrix result(m, M.n) ;
    for (int i = 0 ; i < result.m ; i++) {
        for (int j = 0 ; j < result.n ; j++) {
            mult = 0 ;
            for (int k = 0 ; k < n ; k++) {
                T a = data[i][k] ;
                T b = M[k][j] ;
                mult += a * b ;
            }
            result[i][j] = mult ;
        }
    }

    return result ;
}

template <typename T>
AFMTmatrix<T> AFMTmatrix<T>::operator*=(const AFMTmatrix &M)
{
    *this = *this * M ;
    return this;
}

template <typename T>
AFMTmatrix<T> AFMTmatrix<T>::I()
{
    if (_m != _n)
        throw EInvalidArgument("Can't make a unit of a not square matrix") ;
    for (int i = 0; i < m; i++)
        for (int j = 0; j < n; j++)
            data[i][j] = (i == j) ? 1 : 0 ;
    return this;
}

template <typename T>
AFMTmatrix<T> AFMTmatrix<T>::Zero()
{
       for (int i = 0 ; i < _m ; i++) memset(data[i], 0, rowSize) ;
    return this;
}

template <typename T>
AFMTmatrix<T> AFMTmatrix<T>::RotationDeg(const double Psi, const double Teta, const double Phi)
{
    return RotationRad(AFMDegToRad(Psi), AFMDegToRad(Teta), AFMDegToRad(Phi)) ;
}

template <typename T>
AFMTmatrix<T> AFMTmatrix<T>::RotationRad(const double Psi, const double Teta, const double Phi)
{
    if (_m != 3 || _n != 3)
        throw EInvalidArgument("Can't make a 3D space rotation operator of a matrix of dimension not 3x3") ;

    AFMTmatrix<> mPsi ;        // Rotation matrix around X
    AFMTmatrix<> mTeta ;    // Rotation matrix around Y
    AFMTmatrix<> mPhi ;        // Rotation matrix around Z

    mPsi.I() ;
    double sinPsi = sin(Psi) ;
    double cosPsi = cos(Psi) ;
    mPsi[1][1] =  cosPsi ;
       mPsi[1][2] = -sinPsi ;
    mPsi[2][1] =  sinPsi ;
       mPsi[2][2] =  cosPsi ;

    mTeta.I() ;
    double sinTeta = sin(Teta) ;
    double cosTeta = cos(Teta) ;
    mTeta[0][0] =  cosTeta ;
       mTeta[0][2] =  sinTeta ;
    mTeta[2][0] = -sinTeta ;
       mTeta[2][2] =  cosTeta ;

    mPhi.I() ;
    double sinPhi = sin(Phi) ;
    double cosPhi = cos(Phi) ;
       mPhi[0][0] =  cosPhi ;
    mPhi[0][1] = -sinPhi ;
       mPhi[1][0] =  sinPhi ;
    mPhi[1][1] =  cosPhi ;

    *this = (mPhi * mTeta * mPsi) ;
    return this ;
}


    // This uses the computation of the determiant of the covariant matrices
    // It is VERY inefficient for large matrices
template <typename T>
AFMTmatrix<T> AFMTmatrix<T>::Invert()
{
    if (m != n) throw EInvalidArgument("Inversion is meaningless for a non squared matrix");

    AFMTmatrix<T> inv(n);
    AFMTmatrix<T> c(n - 1);
    T det = this->Determinant();

    int i, ii;
    int j, jj;
    for (int r = 0; r < m; r++) {
        for (int s = 0; s < n; s++) {
            for (i = ii = 0; i < m; i++) {
                if (i == r) continue;
                for (j = jj = 0; j < n; j++) {
                    if (j == s) continue;
                    c[ii][jj] = data[i][j];
                    jj++;
                }
                ii++;
            }
            double cofactor = c.Determinant();
               if (((r + s) % 2) != 0) cofactor = - cofactor;
            inv[s][r] = cofactor / det;
        }
    }

    return inv;
}

template <typename T>
AFMTmatrix<T> AFMTmatrix<T>::Translate()
{
    AFMTmatrix<T> matrix(n, m);
    for (int i = 0; i < m; i++) {
        for (int j = 0; j < n; j++) {
            matrix[j][i] = data[i][j];
        }
    }
    return matrix;
}

template <typename T>
bool AFMTmatrix<T>::IsSimmetric()
{
    if (m != n) throw EInvalidArgument("Simmetricity is meaningless for a non squared matrix");
    for (int i = 0; i < m; i++) {
        for (int j = 0; j < n; j++) {
            if (i != j && data[i][j] != data[j][i]) return false;
        }
    }
    return true;
}

template <typename T>
T AFMTmatrix<T>::Determinant(void)
{
    if (m != n) throw EInvalidArgument("Determinat is meaningless for a non squared matrix");
    if (n == 1) return data[0][0];

    int i, j, k;
    double det = 1;

    if (m == 2) {
        det = data[0][0] * data[1][1] - data[0][1] * data[1][0];
    } else {
        AFMTmatrix<T> matrix(this);

        for (k = 0; k < n; k++) {
            if (matrix[k][k] == 0) {
                bool ok = false;
                for (j = k + 1; j < n; j++) {
                    if (matrix[j][k] != 0) {
                        ok = true;
                        break;
                    }
                }
                if (!ok) return 0;

                for (i = k; i < n; i++) std::swap(matrix[j][i], matrix[k][i]);

                det = -det;
            }

            det *= matrix[k][k];

            if (k + 1 < n) {
                for (i = k + 1; i < n; i++) {
                    for (j = k + 1; j < n; j++)
                        matrix[i][j] = matrix[i][j] - matrix[i][k] * matrix[k][j] / matrix[k][k];
                }
            }
        }
    }

    return det;
}

    // http://steve.hollasch.net/cgindex/math/rotvecs.html
    // See to bottom the shortcut "nasty" solution
    // Difficult to understand, but it works, so, who cares?
template <typename T>
AFMTmatrix<T> AFMTmatrix<T>::RotationAToB(AFMTvector<T> v2, AFMTvector<T> v1)
{
    v1.N(); v2.N();
    AFMTvector<T> v3 = v1.Vector(v2).N();
    AFMTvector<T> v4 = v3.Vector(v1).N();

    AFMTmatrix<T> m1(3);
       for (int j = 0; j < 3; j++) m1[0][j] = v1[j];
       for (int j = 0; j < 3; j++) m1[1][j] = v4[j];
       for (int j = 0; j < 3; j++) m1[2][j] = v3[j];

    AFMTmatrix<> m2(3);
    double cos = v2 * v1;
    double sin = v2 * v4;
    m2[0][0] =  cos; m2[0][1] = sin;
    m2[1][0] = -sin; m2[1][1] = cos;
    m2[2][2] = 1;

    *this = m1.Invert() * m2 * m1;

    return *this;
}

template <typename T>
AFMTvector<T> AFMTmatrix<T>::getEigenvalues()
{
    if (eigenvalues == NULL) ComputeEigenvalues();
    return *eigenvalues;
}

template <typename T>
AFMTvector<T> AFMTmatrix<T>::getEigenvector(int i)
{
    if (eigenvectors == NULL) ComputeEigenvalues();
    AFMTvector<T> eigenvector(n);
    for (int j = 0; j < n; j++)
        eigenvector[j] = (*eigenvectors)[j][i];
    return eigenvector;
}

template <typename T>
void AFMTmatrix<T>::FreeEigenvalues()
{
    delete eigenvalues;
    delete eigenvectors;
    eigenvectors = NULL;
    eigenvalues = NULL;
}

template <typename T>
AFMTvector<T> AFMTmatrix<T>::ComputeEigenvalues(double Tolerance)
{
    if (m != n) throw EInvalidArgument("Eigenvalues are meaningless for a non squared matrix");
    if (Tolerance < 0) throw EInvalidArgument("Tolerance must be a (small) positive");

    eigenvalues = new AFMTvector<T>(n);
    eigenvectors = new AFMTmatrix(n);

    if (IsSimmetric()) {
        // Jacobi algorithm
        // ported from FORTRAN as found at
        // http://www.csiberkeley.com/Tech_Info/d.pdf
        // with some modification as taken by my numerical analisys professor
        // Marco Cugiani (1918-2003)

        // Matrix to reduce to diagonal
        // When diagonal, eigenvalues are on the diagonal
        AFMTmatrix<> A = AFMTmatrix<>(this);

        double sum;
        for (int i = 0; i < m; i++) {
            for (int j = 0; j < n; j++) {
                if (i == j) (*eigenvectors)[i][i] = 1;
                sum += A[i][j];
            }
        }
        sum /= (n * n);

            // Reduce matrix to diagonal
        if (n > 1 && sum > 0) {
            double ssum, amax;
            double sinus, cosinus;
            int iterations = 0;

            do {
                amax = ssum = 0;
                for (int j = 1; j < n; j++) {
                    for (int i = 0; i < j; i++) {
                        // C---- CHECK IF A(I,J) IS TO BE REDUCED -----
                        double aa = fabs(A[i][j]);
                        if (aa > amax) amax = aa;
                        ssum += aa;

                        if (aa <= (amax * 0.1)) continue;

                        // C---- CALCULATE ROTATION ANGLE ----------
                        // M. Cugiani algorithm avoids atan, sin, cos:
                        // square root is, or should be,
                        // numerically much less demanding
                        double tau = 2.0 * A[i][j];
                        double omega = A[i][i] - A[j][j];
                        double radSmall = sqrt(tau * tau + omega * omega);
                        double co2 = (radSmall + fabs(omega)) / (2.0 * radSmall);
                        double omegaTau = tau * omega;
                        if (omegaTau == 0) {
                            sinus = 0.0;
                        } else {
                            double si2 = (radSmall - fabs(omega)) / (2.0 * radSmall);
                            sinus = sqrt(si2) * ((omegaTau > 0) ? 1 : -1);
                        }
                        cosinus = sqrt(co2);

                        // C---- MODIFY "I" AND "J" COLUMNS --------
                        for (int k = 0; k < n; k++) {
                            double tt = A[k][i];
                            A[k][i] =  cosinus * tt +   sinus * A[k][j];
                            A[k][j] =   -sinus * tt + cosinus * A[k][j];
                            tt = (*eigenvectors)[k][i];
                            (*eigenvectors)[k][i] =  cosinus * tt +   sinus * (*eigenvectors)[k][j];
                            (*eigenvectors)[k][j] =   -sinus * tt + cosinus * (*eigenvectors)[k][j];
                        }

                        // C---- MODIFY DIAGONAL TERMS -------------
                        A[i][i] =  cosinus * A[i][i] +   sinus * A[j][i];
                        A[j][j] =   -sinus * A[i][j] + cosinus * A[j][j];
                        A[i][j] = 0.0;

                        // C---- MAKE "A" MATRIX SYMMETRICAL -------
                        for (int k = 0; k < n; k++) {
                            A[i][k] = A[k][i];
                            A[j][k] = A[k][j];
                        }
                        // C---- A(I,J) MADE ZERO BY ROTATION ------
                    }
                }
            } while ((fabs(ssum)/sum) >= Tolerance && iterations++ < 100);

            if (iterations >= 100)
                throw EInvalidArgument("Eigenvalue computation following Jacobi algorithm does not converge");

            for (int k = 0; k < n; k++)
                (*eigenvalues)[k] = A[k][k];
        }
    } else {
        throw EInvalidArgument("Eigenvalue computation to be implemented for a non simmetric matrix");
    }

    return *eigenvalues;
}

#endif
You can download this source code from here.
Top
Free software in source code.

C++ linear algebra template library
