EMatrix.h

/** This file is part of EMatrix, the C++ matrix library distribution.
 *  This project is licensed under the terms of the MIT license. The full text
 *  of the license file can be found in LICENSE.                         
 */

/// \file

#ifndef _EMATRIX_H
#define _EMATRIX_H 1

#include <cmath>
#include <cfloat>
#include <cstdlib>
#include <cstring>
#include <cstdarg>
#include <cassert>

#include <complex>
#include <fstream>
#include <iostream>
#include <initializer_list>
#include <random>
#include <limits>


#define cout_octave(x) octave((cout),(x),(#x))
#define HERE std::cerr << __FILE__ << ':' << __LINE__ << std::endl;

namespace ematrix {

//! These constants are from the gcc 3.2 <cmath> file (overkill??)
namespace cnst {
const  double PI         = 3.1415926535897932384626433832795029L;  // pi
const  double PI_2       = 1.5707963267948966192313216916397514L;  // pi/2
const  double RTD        = 180.0L/PI;                              // Radians to Degrees
const  double DTR        =   1.0L/RTD;                             // Degrees to Radians

/** WGS84 Earth constants:
 *  Titterton, D.H. and Weston, J.L.; <i>Strapdown Inertial Navigation
 *  Technology</i>; Peter Peregrinus Ltd.; London 1997; ISBN 0-86341-260-2; pg. 53.
 */
const  double REQ        = 6378137.0;                              // meters
const  double rEQ        = 6356752.3142;                           // meters
const  double f          = 1.0/298.257223563;
const  double ECC        = 0.0818191908426;
const  double ECC2       = ECC*ECC;
const  double WS         = 7.292115E-05;                           // rad/sec

const  double g          = 9.78032534;                             // m/s^2
const  double k          = 0.00193185;                             // ??
}

template < typename tData, size_t tRows, size_t tCols >
class Matrix {
protected:
    // Memory allocation method
    void matalloc (size_t iRowIndex, size_t iColIndex); // Tag:tested
    // Number of row and columns
    size_t iRows, iCols;
    // Storage element, matalloc above assigns an array of pointers to pointers
    tData *ij[tRows];
    tData storage[tRows*tCols];
public:

    //! Virtual destructor, no need though
    virtual ~Matrix (); // Tag:tested

    /** Default constructor
     *  Usage: Matrix<double,2,3> A;
     */
    Matrix (); // Tag:tested

    /** Copy constructor (not to be confused with the assignment operator)
     *  Usage: Matrix<float,2,3> A;
     *         Matrix<float,2,3> B=A;
     */
    inline Matrix (const Matrix< tData, tRows, tCols > & R); // Tag:tested

    /** Array initialize contructor
     *  Usage: float a[2][3] = {{1.0,2.0,3.0},{4.0,5.0,6.0}};
     *         Matrix<float,2,3> A(&a[0][0]);
     */
    Matrix (tData* tArray); // Tag:tested

    /** STL list initialize contructor (C++11)
     *  Usage: Matrix<double,3,3> A = {1.,2.,3.,0.,0.,0.,0,0,0};
     */
    Matrix (const std::initializer_list<tData>& l); // Tag:tested

    /** Assignment operator (not to be confused with the copy constructor)
     *  Usage: Y = X - Z;
     */
    inline const Matrix< tData, tRows, tCols > &operator = (const Matrix< tData, tRows, tCols > & R); // Tag:tested

    /** STL initializer_list assignment  (C++11)
     *  Usage: Matrix<double,3,2> A;
     *         A = {1.1,2.1,3.1,0.0,0.0,0.0};
     *
     */
    inline const Matrix< tData, tRows, tCols > &operator = (const std::initializer_list<tData>& l); // Tag:tested

    /** Array assignment
     *  Usage: double a[2][3] = {{1.1,2.1,3.1},{4.0,5.0,6.0}};
     *         Matrix<double,2,3> A;
     *         A.load(&a[0][0]);
     *  Warning: Not congnizant of size, can read from unintended memory location;
     */
    void load(const tData* tArray); // Tag:tested consider changing to memset

    /** C like element access (0 to n-1), get and set.
     *  Usage: Matrix<double,3,2> A = {1,2,3,4,5,6};
     *         A[0][0] = 7;
     *         cerr << A[2][1] << endl;
     *  Row operator returns the matrix row corresponding to iRowIndex,
     *  Warning: Does not provide column access safety.
     */
    inline tData * operator [] (size_t iRowIndex) const { // Tag:tested
        assert(iRowIndex<iRows);
        return ij[iRowIndex];
    }

    /** Data access operator for Octave and FORTRAN indexing ... From 1 to n
     *  Note this does not imply FORTRAN memory storage
     *  Usage: Matrix<double,3,2> A = {1,2,3,4,5,6};
     *         A(1,1) = 8;
     *         cerr << A(3,2) << endl;
     *  Note this looks similar to the deprecated memory initialize c_tor that uses va_arg.
     *  but is not the same thing
     */
    inline tData & operator () (size_t iRowIndex, size_t iColIndex) const; // Tag:tested

    /** Vector Data access operator for Octave and FORTRAN indexing ... From 1 to n
     *  Usage: Matrix<double,6,1> V = {1,2,3,4,5,6}; V(1) = 8; cerr << V(6) << endl;
     *  Usage: Matrix<double,1,6> U = {1,2,3,4,5,6}; U(1) = 8; cerr << U(6) << endl;
     *  Note a non 1xn or nx1 matrix will assertion fail.  Could not determine a way
     *  to force compile time error.
     */
    inline tData & operator () (size_t iIndex) const; // Tag:tested

    /** Overloaded output stream operator <<
     *  Usage: log_file << A;
     */
    template < class tData0, size_t tRows0, size_t tCols0 >
    friend std::ostream& operator << (std::ostream& s,const Matrix< tData0, tRows0, tCols0 >& A);// Tag:tested

    /** Octave text output format, complex<double>
     *  Usage: Matrix< complex<double>,3,3 > ZA;
     *         octave(cout,ZA,"ZA") << endl;
     *  Can define: #define cout_octave(x) octave(cout,x,#x) for
     *         cout_octave(ZA) << endl;
     */
    // This is broken but works well enough. See:
    // https://web.mst.edu/~nmjxv3/articles/templates.html
    template < class tData0, size_t tRows0, size_t tCols0 >
    friend std::ostream& octave (std::ostream& s, const Matrix< tData0, tRows0, tCols0 >& A, const char* Aname);// {  // Tag:tested

    /** Octave text output format, double
     *  Usage: Matrix<double,2,3> A(&a[0][0]);
     *         octave(cout,A,"A") << endl;
     *  Can define: #define cout_octave(x) octave(cout,x,#x) for
     *         cout_octave(A) << endl;
     */
    template < class tData0, size_t tRows0, size_t tCols0 >
    friend std::ostream& octave (std::ostream& s, const Matrix< std::complex<tData0>, tRows0, tCols0 >& A, const char* Aname);// {  // Tag:tested

    /** Get the storage pointer for the data in the matrix
     *  This is really only here for the friend functions
     *  Try not to use it
     *  Usage: tData* ptr = A.pIJ();
     */
    inline tData *pIJ (void) const { // Tag:tested
        return ij[0];
    }

    /** Get the number of rows in a matrix
     *  Usage: size_t i = A.rows();
     */
    inline size_t rows (void) const { // Tag:tested
        return iRows;
    }

    /** Get the number of cols in a matrix
     *  Usage: size_t i = A.cols();
     */
    inline size_t cols (void) const { // Tag:tested
        return iCols;
    }

    /** Boolean == operator
     *  Usage: if (A == B) ...
     */
    bool operator == (Matrix< tData, tRows, tCols > & R); // Tag:tested

    /** Boolean != operator
     *  Usage: if (A != B) ...
     */
    bool operator != (Matrix< tData, tRows, tCols > & R); // Tag:tested

    /** Unary + operator
     *  Usage: C = (+B); Just returns *this;
     */
    Matrix< tData, tRows, tCols > operator + (); // Tag:tested

    /** Unary - operator
     *  Usage: C = (-A);
     */
    Matrix< tData, tRows, tCols > operator - (); // Tag:tested

    /** Addition operator
     *  Usage: C = A + B;
     */
    Matrix< tData, tRows, tCols > operator + (const Matrix< tData, tRows, tCols > & R); // Tag:tested

    /** Subtaction operator
     *  Usage: C = A - B;
     */
    Matrix< tData, tRows, tCols > operator - (const Matrix< tData, tRows, tCols > & R); // Tag:tested

    /** Scalar multiplication operator
     *  Usage: C = A * scalar;
     */
    Matrix< tData, tRows, tCols > operator * (const tData & scalar); // Tag:tested

    /** Friend scalar multiplication operator
     *  Usage: C = scalar * A;
     */
    template< class tData0, size_t tRows0, size_t tCols0 >
    friend Matrix< tData0, tRows0, tCols0 > operator * (const tData0 & scalar,const Matrix< tData0, tRows0, tCols0 > & R); // Tag:tested

    /** Scalar division operator
     *  Usage: C = A / scalar;
     */
    Matrix< tData, tRows, tCols > operator / (const tData & scalar); // Tag:tested

    /** Matrix multiplication operator
     *  Usage: C = A * B;
     */
    template < size_t tColsR >
    Matrix< tData, tRows, tColsR > operator * (const Matrix< tData, tCols, tColsR >& R) { // Tag:tested
        tData x;
        Matrix< tData, tRows, tColsR > Result;

        for (size_t iIndex=0; iIndex<iRows; iIndex++) {
            for (size_t jIndex=0; jIndex<R.cols(); jIndex++) {
                x = tData(0);
                for (size_t kIndex=0; kIndex<R.rows(); kIndex++) {
                    x += ij[iIndex][kIndex] * R[kIndex][jIndex];
                }
                Result[iIndex][jIndex] = x;
            }
        }
        return Result;
    }

    /** Array multiplication operator
     *  Usage: C = (A *= B); Must use parentheses
     *  This mimics Octave's A .* B operator and not C's x *= 5 operator
     */
    Matrix< tData, tRows, tCols > operator *= (const Matrix< tData, tRows, tCols >  & R); // Tag:tested

    /** Array division operator
     *  Usage: C = (A /= B); Must use parentheses
     *  This mimics Octave's A ./ B operator and not C's x /= 5 operator
     */
    Matrix< tData, tRows, tCols > operator /= (const Matrix< tData, tRows, tCols >  & R); // Tag:tested

    /** Concatenate matrices top and botton
     *  Usage: C = (A & B); Must use parenthesis
    */
    template < class tData0, size_t tCols0, size_t tRowsT, size_t tRowsB>
    friend Matrix< tData0, tRowsT+tRowsB, tCols0 > operator & (const Matrix< tData0, tRowsT, tCols0 >& Top,
            const Matrix< tData0, tRowsB, tCols0 >& Bottom); // Tag:tested

    /** Concatenate matrices Left to Right
    *   Usage: C = (A | B); Must use parenthesis
    */
    template < class tData0, size_t tRows0, size_t tColsL, size_t tColsR >
    friend Matrix< tData0, tRows0, tColsL+tColsR > operator | (const Matrix< tData0, tRows0, tColsL >& Left,
            const Matrix< tData0, tRows0, tColsR >& Right); // Tag:tested

    /** Set contents to 0x0
     *  Usage: A.zeros();
     */
    Matrix< tData, tRows, tCols > zeros( void ); // Tag:tested

    /** Set contents to tData(1)
     *  Usage: A.ones();
     */
    Matrix< tData, tRows, tCols > ones( void ); // Tag:tested

    /** Set contents to the identity matrix
     *  Usage: A.eye();
     */
    Matrix< tData, tRows, tCols > eye( void ); // Tag:tested

    /** Set all elements of the current matrix random ~N(0,1);
     *  Usage: u.randn();
     */
    Matrix< tData, tRows, tCols > randn(void); // Tag:tested

    // Matrix transpose and complex conjugate transpose
    // Usage: A_trans = trans(A);
    template < class tData0, size_t tRows0, size_t tCols0 >
    friend Matrix< tData0, tCols0, tRows0 > trans( const Matrix< tData0, tRows0, tCols0 >& R ); // Tag:tested

    template < size_t tRows0, size_t tCols0 >
    friend Matrix< std::complex<float>, tCols0, tRows0 > trans( const Matrix< std::complex<float>, tRows0, tCols0 >& R ); // Tag:tested

    template < size_t tRows0, size_t tCols0 >
    friend Matrix< std::complex<double>, tCols0, tRows0 > trans( const Matrix< std::complex<double>, tRows0, tCols0 >& R ); // Tag:tested

    /** Matrix diagonal like Octave
     * This friend function does not modify input contents.
     * Usage: A_diag = diag(A);
     */
    template < class tData0, size_t tRows0 >
    friend Matrix< tData0, tRows0, 1 > diag( const Matrix< tData0, tRows0, tRows0 >& R ); // Tag:tested

    template < class tData0, size_t tRows0 >
    friend Matrix< tData0, tRows0, tRows0 > diag( const Matrix< tData0, tRows0, 1 >& R ); // Tag:tested

    template < class tData0, size_t tCols0 >
    friend Matrix< tData0, tCols0, tCols0 > diag( const Matrix< tData0, 1, tCols0 >& R ); // Tag:tested


    /* Construct a skew symmetric matrix from a 3x1 vector.
     * w = [wx;wy;wz];
     * skew(w) = [0.0 -wz +wy
     *            +wz 0.0 -wx
     *            -wy +wx 0.0];
     * Usage: omega_x = skew(w);
     */
    template < class tData0 >
    friend Matrix< tData0, 3, 3 > skew( const Matrix< tData0, 3, 1 >& R ); // Tag:tested

    /* Take the cross product of two 3x1 vectors
     * Usage: a = cross(lsr,Vc);
     */
    template < class tData0 >
    friend Matrix< tData0, 3, 1 > cross( const Matrix< tData0, 3, 1 >& L, const Matrix< tData0, 3, 1 >& R ); // Tag:tested

    /* Take the dot product of two 3x1 vectors
     * Usage: (norm(x))^2 = dot(x,x);
     */
    template < class tData0, size_t tRows0 >
    friend tData0 dot( const Matrix< tData0, tRows0, 1 >& L, const Matrix< tData0, tRows0, 1 >& R ); // Tag:tested

    /** Take the norm of two vectors
     *  Usage: norm_a = a.n();
     */
    tData n( void ) const; // Tag:tested

    /** Take the norm of two vectors
     *  Usage: norm_a = norm(a);
     */
    template < class tData0, size_t tRows0 >
    friend tData0 norm( const Matrix< tData0, tRows0, 1 >& R ); // Tag:tested

    /** return a unit vector in the direction of V
     *  Usage: u_v = V.u()
     */
    Matrix< tData, tRows, 1 > u( void ) const; // Tag:tested

    /** Matrix inverse, must link with Lapack
     *  Usage: A_inv = inv(A);
     */
    template < size_t tRows0 >
    friend Matrix< float, tRows0, tRows0 > inv( const Matrix< float, tRows0, tRows0 >& R );

    template < size_t tRows0>
    friend Matrix< std::complex<float>, tRows0, tRows0 > inv( const Matrix< std::complex<float>, tRows0, tRows0 >& R );

    template < size_t tRows0 >
    friend Matrix< double, tRows0, tRows0 > inv( const Matrix< double, tRows0, tRows0 >& R );

    template < size_t tRows0 >
    friend Matrix< std::complex<double>, tRows0, tRows0 > inv( const Matrix< std::complex<double>, tRows0, tRows0 >& R );

    /** Matrix determinent, must link with Lapack
     *  Usage: A_det = det(A);
     */
    template < size_t tRows0 >
    friend float det( const Matrix< float, tRows0, tRows0 >& R );

    template < size_t tRows0 >
    friend double det( const Matrix< double, tRows0, tRows0 >& R );

    template < size_t tRows0 >
    friend std::complex<float> det( const Matrix< std::complex<float>, tRows0, tRows0 >& R );

    template < size_t tRows0 >
    friend std::complex<double> det( const Matrix< std::complex<double>, tRows0, tRows0 >& R );

    /** Simple Rotation
     *  Usage: Rx = R(angle_rad, 'x');
     *  Note that r_ECEF = trans(R(ws*t, 'z'))*r_ECI if ws*t is a positive rotation
     *  about the z axis.  This is backwards on purpose.
     */
    template < class tData0 >
    friend Matrix< tData0, 3, 3 > R(tData0 angle, char axis);
    //friend Matrix< tData, 3, 3 > TIE(tData t);
    //friend Matrix< tData, 3, 3 > TEL(tData lat_rad,tData lon_rad);
    //friend Matrix< tData, 3, 3 > ang_to_tib(tData psi,tData the,tData phi);
    //friend void uv_to_ae(tData *az,tData *el,Matrix< tData, 3, 1 > &u);

};  // EOC: End of Class


template < class tData, size_t tRows, size_t tCols >
void Matrix< tData, tRows, tCols >::matalloc(size_t iRowIndex, size_t iColIndex) { // Tag:tested
    ij[0] = &storage[0];
    for (size_t iIndex = 1; iIndex < iRowIndex; iIndex++)
        ij[iIndex] = ij[iIndex - 1] + iColIndex;

    std::memset (storage, 0x0, sizeof(storage));
}

template < class tData, size_t tRows, size_t tCols >
Matrix< tData, tRows, tCols >::~Matrix() { // Tag:tested
    // HERE;
}

template < class tData, size_t tRows, size_t tCols >
Matrix< tData, tRows, tCols >::Matrix() // Tag:tested
    : iRows(tRows), iCols(tCols) {
    matalloc(tRows,tCols);
}

template < class tData, size_t tRows, size_t tCols >
inline Matrix< tData, tRows, tCols >::Matrix(const Matrix & R) // Tag:tested
    : iRows (R.iRows), iCols (R.iCols) {
    matalloc(R.iRows, R.iCols);
    std::memcpy(ij[0], R.ij[0], sizeof (tData) * iRows * iCols);
}

template < class tData, size_t tRows, size_t tCols >
Matrix< tData, tRows, tCols >::Matrix (tData* tArray) // Tag:tested
    : iRows (tRows), iCols (tCols) {
    matalloc(tRows,tCols);
    std::memcpy(storage, tArray, sizeof(storage));
}

template < class tData, size_t tRows, size_t tCols >
Matrix< tData, tRows, tCols >::Matrix (const std::initializer_list<tData>& l) // Tag:tested
    : iRows(tRows), iCols(tCols) {
    assert( iRows*iCols == l.size() );
    matalloc( iRows, iCols);
    for(size_t i = 0; i<(iRows*iCols); i++) {
        ij[0][i] = *(l.begin() + i);
    }
}

template < class tData, size_t tRows, size_t tCols >
inline const Matrix< tData, tRows, tCols > & Matrix< tData, tRows, tCols >::operator = (const Matrix< tData, tRows, tCols > & R) { // Tag:tested
    assert((iRows == R.iRows) && (iCols == R.iCols));
    if( this != &R ) {
        std::memcpy (ij[0], R.ij[0], sizeof(tData)*iRows*iCols);
    }
    return *this;
}

template < class tData, size_t tRows, size_t tCols >
inline const Matrix< tData, tRows, tCols>& Matrix< tData,tRows,tCols>::operator = (const std::initializer_list<tData>& l) { // Tag:tested
    assert( iRows*iCols == l.size() );
    for(size_t i = 0; i<(iRows*iCols); i++) {
        ij[0][i] = *(l.begin() + i);
    }
    return *this;
}

template < class tData, size_t tRows, size_t tCols >
void Matrix< tData, tRows, tCols >::load(const tData* tArray) { // Tag:tested
    std::memcpy(storage, tArray, sizeof(storage));
}

template < class tData, size_t tRows, size_t tCols >
inline tData & Matrix< tData, tRows, tCols >::operator () (size_t iRowIndex, size_t iColIndex) const { // Tag:tested
    iRowIndex--;
    iColIndex--;
    assert(0<=iRowIndex && iRowIndex<iRows);
    assert(0<=iColIndex && iColIndex<iCols);
    return ij[iRowIndex][iColIndex];
}

template < class tData, size_t tRows, size_t tCols >
inline tData & Matrix< tData, tRows, tCols >::operator () (size_t iIndex) const { // Tag:tested
    iIndex--;
    assert(iRows==1 || iCols==1);
    if(iCols == 1) {
        assert(0<=iIndex && iIndex<iRows);
        return ij[iIndex][0];
    } else { // (iRows == 1)
        assert(0<=iIndex && iIndex<iCols);
        return ij[0][iIndex];
    }
}

template < class tData, size_t tRows, size_t tCols >
std::ostream& operator << (std::ostream& s,const Matrix< tData, tRows, tCols >& A) { // Tag:tested
    // Sets new precision, returns old. Should figure out how to modify
    // without code change here.  Switch to exponential as well.
    std::streamsize old_precision = s.precision(8);

    //s.setf( std::ios::fixed, std:: ios::floatfield ); // floatfield set to fixed

    //s << "Address: 0x" << (&A) << std::endl;
    for (size_t i=0; i<A.iRows; i++) {
        for (size_t j=0; j<A.iCols; j++) {
            //s.width(25);
            s << (A[i][j]) << '\t';
        }
        s << std::endl;
    }
    s.flush();
    s.precision(old_precision);
    return s;
}

template < class tData0, size_t tRows0, size_t tCols0 >
std::ostream& octave (std::ostream& s, const Matrix< std::complex< tData0 >, tRows0, tCols0 >& A, const char* Aname) {  // Tag:tested
    s << "# name: " << Aname << std::endl;
    s << "# type: complex matrix" << std::endl;
    s << "# rows: " << A.iRows << std::endl;
    s << "# columns: " << A.iCols << std::endl;
    s << A;
    s.flush();
    return s;
}

template < class tData0, size_t tRows0, size_t tCols0 >
std::ostream& octave (std::ostream& s,const Matrix< tData0, tRows0, tCols0 >& A, const char* Aname) {  // Tag:tested
    s << "# name: " << Aname << std::endl;
    s << "# type: matrix" << std::endl;
    s << "# rows: " << A.iRows << std::endl;
    s << "# columns: " << A.iCols << std::endl;
    s << A;
    s.flush();
    return s;
}

template < class tData, size_t tRows, size_t tCols >
bool Matrix< tData, tRows, tCols >::operator == (Matrix< tData, tRows, tCols > & R) { // Tag:tested
    tData * pLeft  = ij[0];
    tData * pRight = R.ij[0];

    for (size_t iIndex = 0; iIndex < iRows * iCols; iIndex++)
        if((*pLeft++) != tData(*pRight++)) {
            return false;
        }

    return true;
}

template < class tData, size_t tRows, size_t tCols >
bool Matrix< tData, tRows, tCols >::operator != (Matrix< tData, tRows, tCols > & R) { // Tag:tested
    return !(*this == R);
}

template < class tData, size_t tRows, size_t tCols >
Matrix< tData, tRows, tCols > Matrix< tData, tRows, tCols >::operator + () { // Tag:tested
    return *this;
}

template < class tData, size_t tRows, size_t tCols >
Matrix< tData, tRows, tCols > Matrix< tData, tRows, tCols >::operator - () {  // Tag:tested
    Matrix< tData, tRows, tCols > Result;
    tData * pLeft = ij[0];
    tData * pResult = Result.ij[0];
    for (size_t iIndex = 0; iIndex < iRows * iCols; iIndex++)
        (*pResult++) = (-(*pLeft++));
    return Result;
}

template < class tData, size_t tRows, size_t tCols >
Matrix< tData, tRows, tCols > Matrix< tData, tRows, tCols >::operator + (const Matrix< tData, tRows, tCols > & R) { // Tag:tested
    Matrix< tData, tRows, tCols > Result;
    tData * pLeft   = ij[0];
    tData * pRight  = R.ij[0];
    tData * pResult = Result.ij[0];

    for (size_t iIndex = 0; iIndex < iRows * iCols; iIndex++)
        (*pResult++) = (*pLeft++) + (*pRight++);

    return Result;
}

template < class tData, size_t tRows, size_t tCols >
Matrix< tData, tRows, tCols > Matrix< tData, tRows, tCols >::operator - (const Matrix< tData, tRows, tCols > & R) { // Tag:tested
    Matrix< tData, tRows, tCols >  Result;
    tData * pLeft   = ij[0];
    tData * pRight  = R.ij[0];
    tData * pResult = Result.ij[0];

    for (size_t iIndex = 0; iIndex < iRows * iCols; iIndex++)
        (*pResult++) = (*pLeft++) - (*pRight++);
    return Result;
}

template < class tData, size_t tRows, size_t tCols >
Matrix< tData, tRows, tCols > Matrix< tData, tRows, tCols >::operator * (const tData &scalar) { // Tag:tested
    Matrix< tData, tRows, tCols >  Result = (*this);
    tData * pResult = Result.ij[0];
    for (size_t iIndex = 0; iIndex < iRows * iCols; iIndex++)
        (*pResult++) *= scalar;
    return Result;
}

template < class tData, size_t tRows, size_t tCols > // friend
Matrix< tData, tRows, tCols > operator * (const tData & scalar,const Matrix< tData, tRows, tCols > & R) { // Tag:tested
    size_t iRows = R.iRows;
    size_t iCols = R.iCols;
    Matrix< tData, tRows, tCols >  Result = R;
    tData * pResult = Result.ij[0];
    for (size_t iIndex = 0; iIndex < iRows * iCols; iIndex++)
        (*pResult++) *= scalar;
    return Result;
}

template < class tData, size_t tRows, size_t tCols >
Matrix< tData, tRows, tCols > Matrix< tData, tRows, tCols >::operator / (const tData &scalar) { // Tag:tested
    assert(scalar);
    Matrix< tData, tRows, tCols >  Result = (*this);
    tData * pResult = Result.ij[0];
    for (size_t iIndex = 0; iIndex < iRows * iCols; iIndex++)
        (*pResult++) /= scalar;
    return Result;
}

template < class tData, size_t tRows, size_t tCols >
Matrix< tData, tRows, tCols > Matrix< tData, tRows, tCols >::operator *= (const Matrix< tData, tRows, tCols > & R) { // Tag:tested
    Matrix< tData, tRows, tCols > Result;
    tData * pLeft   = ij[0];
    tData * pRight  = R.ij[0];
    tData * pResult = Result.ij[0];
    for (size_t iIndex = 0; iIndex < iRows*iCols; iIndex++)
        (*pResult++) = (*pLeft++) * tData(*pRight++);
    return Result;
}

template < class tData, size_t tRows, size_t tCols >
Matrix< tData, tRows, tCols > Matrix< tData, tRows, tCols >::operator /= (const Matrix< tData, tRows, tCols > & R) { // Tag:tested
    Matrix< tData, tRows, tCols > Result;
    tData * pLeft   = ij[0];
    tData * pRight  = R.ij[0];
    tData * pResult = Result.ij[0];
    for (size_t iIndex = 0; iIndex < iRows * iCols; iIndex++) {
        assert(*pRight != 0.0);
        (*pResult++) = (*pLeft++) / tData(*pRight++);
    }
    return Result;
}

template < class tData0, size_t tCols0, size_t tRowsT, size_t tRowsB>
Matrix< tData0, tRowsT+tRowsB, tCols0 > operator & (const Matrix< tData0, tRowsT, tCols0 >& Top,
        const Matrix< tData0, tRowsB, tCols0 >& Bottom) {  // Tag:tested
    Matrix< tData0, tRowsT+tRowsB, tCols0 > Result;

    size_t i,ii,j;
    for(i=0; i<Top.iRows; i++)
        for(j=0; j<Top.iCols; j++)
            Result[i][j] = Top[i][j];

    for(i=Top.iRows,ii=0; i<(Top.iRows+Bottom.iRows); i++,ii++) {
        for(j=0; j<Top.iCols; j++) {
            Result[i][j] = Bottom[ii][j];
        }
    }
    return Result;
}

template < class tData0, size_t tRows0, size_t tColsL, size_t tColsR >
Matrix< tData0, tRows0, tColsL+tColsR > operator | (const Matrix< tData0, tRows0, tColsL >& Left,
        const Matrix< tData0, tRows0, tColsR >& Right) {  // Tag:tested
    Matrix< tData0, tRows0, tColsL+tColsR > Result;
    size_t i,j,jj;
    for(i=0; i<Left.iRows; i++)
        for(j=0; j<Left.iCols; j++)
            Result[i][j] = Left[i][j];
    for(i=0; i<Left.iRows; i++) {
        for(j=Left.iCols,jj=0; j<(Left.iCols+Right.iCols); j++,jj++) {
            Result[i][j] = Right[i][jj];
        }
    }
    return Result;
}

template < class tData, size_t tRows, size_t tCols >
Matrix< tData, tRows, tCols > Matrix< tData, tRows, tCols >::zeros( void ) { // Tag:tested
    std::memset (ij[0], 0x0, sizeof(tData) * iRows * iCols);
    return *this;
}

template < class tData, size_t tRows, size_t tCols >
Matrix< tData, tRows, tCols > Matrix< tData, tRows, tCols >::ones( void ) { // Tag:tested
    tData * pThis = ij[0];
    for (size_t iIndex = 0; iIndex < iRows * iCols; iIndex++)
        (*pThis++) = tData(1);
    return *this;
}

template < class tData, size_t tRows, size_t tCols >
Matrix< tData, tRows, tCols > Matrix< tData, tRows, tCols >::eye( void ) { // Tag:tested
    assert(tRows==tCols);
    std::memset (ij[0], 0x0, sizeof(tData) * iRows * iCols);

    tData * pThis = ij[0];
    for (size_t iIndex = 0; iIndex < iRows; iIndex++, pThis+=iCols)
        (*pThis++) = tData(1);
    return *this;
}

template < class tData, size_t tRows, size_t tCols >
Matrix< tData, tRows, tCols > Matrix< tData, tRows, tCols >::randn(void) { // Tag:tested
    std::default_random_engine generator;
    std::normal_distribution<tData> distribution;
    for (size_t iIndex = 0; iIndex < iRows*iCols; iIndex++) {
        ij[0][iIndex] = distribution(generator);
    }
    return *this;
}

template < class tData, size_t tRows, size_t tCols >
Matrix< tData, tCols, tRows > trans( const Matrix< tData, tRows, tCols >& R ) { // Tag:tested
    Matrix< tData, tCols, tRows > Result;

    for (size_t iIndex = 0; iIndex < tCols; iIndex++)
        for (size_t jIndex = 0; jIndex < tRows; jIndex++)
            Result[iIndex][jIndex] = R[jIndex][iIndex];

    return Result;
}

template < size_t tRows, size_t tCols >
Matrix< std::complex<float>, tCols, tRows > trans( const Matrix< std::complex<float>, tRows, tCols >& R ) { // Tag:tested
    Matrix< std::complex<float>, tCols, tRows > Result;

    for (size_t iIndex = 0; iIndex < tCols; iIndex++)
        for (size_t jIndex = 0; jIndex < tRows; jIndex++)
            Result[iIndex][jIndex] = std::conj(R[jIndex][iIndex]);

    return Result;
}

template < size_t tRows, size_t tCols >
Matrix< std::complex<double>, tCols, tRows > trans( const Matrix< std::complex<double>, tRows, tCols >& R ) { // Tag:tested
    Matrix< std::complex<double>, tCols, tRows > Result;

    for (size_t iIndex = 0; iIndex < tCols; iIndex++)
        for (size_t jIndex = 0; jIndex < tRows; jIndex++)
            Result[iIndex][jIndex] = std::conj(R[jIndex][iIndex]);

    return Result;
}

template < class tData, size_t tRows >
Matrix< tData, tRows, 1 > diag( const Matrix< tData, tRows, tRows >& R ) { // Tag:tested
    Matrix< tData, tRows, 1 > Result;
    for (size_t iIndex = 0; iIndex < tRows; iIndex++ ) {
        Result[iIndex][0] = R[iIndex][iIndex];
    }
    return Result;
}

template < class tData, size_t tRows >
Matrix< tData, tRows, tRows > diag( const Matrix< tData, tRows, 1 >& R ) { // Tag:tested
    Matrix< tData, tRows, tRows > Result;
    for (size_t iIndex = 0; iIndex < tRows; iIndex++ ) {
        Result[iIndex][iIndex] = R[iIndex][0];
    }
    return Result;
}

template < class tData, size_t tCols >
Matrix< tData, tCols, tCols > diag( const Matrix< tData, 1, tCols >& R ) { // Tag:tested
    Matrix< tData, tCols, tCols > Result;
    for (size_t iIndex = 0; iIndex < tCols; iIndex++ ) {
        Result[iIndex][iIndex] = R[0][iIndex];
    }
    return Result;
}

template < class tData >
Matrix< tData, 3, 3 > skew( const Matrix< tData, 3, 1 >& R ) { // Tag:tested
    tData * pR  =  R.pIJ();
    tData z = tData(0);
    Matrix< tData, 3, 3 > Result = {
        z,-pR[2], pR[1],
        pR[2],     z,-pR[0],
        -pR[1], pR[0],  z
    };

    return Result;
}

template < class tData >
Matrix< tData, 3, 1 > cross( const Matrix< tData, 3, 1 >& L, const Matrix< tData, 3, 1 >& R ) { // Tag:tested
    return skew(L)*R;
}

template < class tData, size_t tRows >
tData dot( const Matrix< tData, tRows, 1 >& L, const Matrix< tData, tRows, 1 >& R ) { // Tag:tested
    tData Result = tData(0);

    tData * pR = R.pIJ();
    tData * pL = L.pIJ();

    for (size_t iIndex = 0; iIndex < tRows; iIndex++) {
        Result += (*pR++)*(*pL++);
    }
    return Result;
}

template < class tData, size_t tRows, size_t tCols >
tData Matrix< tData, tRows, tCols >::n( void ) const { // Tag:tested
    return std::sqrt(dot(*this,*this));
}

template < class tData, size_t tRows >
tData norm( const Matrix< tData, tRows, 1 >& R ) { // Tag:tested
    return std::sqrt(dot(R,R));
}

template < class tData, size_t tRows, size_t tCols >
Matrix< tData, tRows, 1 > Matrix< tData, tRows, tCols >::u( void ) const { // Tag:tested
    tData den = n();
    assert(den != 0.0);
    Matrix< tData, tRows, 1 > Result = *this;
    return Result*(1.0/den);
}

extern "C" void sgesv_( const int &n, const int &nrhs, float *A,
                        const int &lda, int* ipiv, float *B, const int &ldb, int *info);
extern "C" void cgesv_( const int &n, const int &nrhs, std::complex<float> *A,
                        const int &lda, int* ipiv, std::complex<float> *B, const int &ldb, int *info);
extern "C" void dgesv_( const int &n, const int &nrhs, double *A,
                        const int &lda, int* ipiv, double *B, const int &ldb, int *info);
extern "C" void zgesv_( const int &n, const int &nrhs, std::complex<double> *A,
                        const int &lda, int* ipiv, std::complex<double> *B, const int &ldb, int *info);

template < size_t tRows >
Matrix< float, tRows, tRows > inv( const Matrix< float, tRows, tRows >& R ) {
    int n = tRows;
    Matrix< float, tRows, tRows > a = R;
    int ipiv[tRows] = {0};
    Matrix< float, tRows, tRows > Result;
    Result.eye();
    int info = 0;

    sgesv_(n, n, a.pIJ(), n, ipiv, Result.pIJ(), n, &info);

    if(info != 0) {
        std::cerr << "?gesv returned error: " << info << std::endl;
        abort();
    }

    return Result;
}

template < size_t tRows >
Matrix< std::complex<float>, tRows, tRows > inv( const Matrix< std::complex<float>, tRows, tRows >& R ) {
    int n = tRows;
    Matrix< std::complex<float>, tRows, tRows > a = R;
    int ipiv[tRows] = {0};
    Matrix< std::complex<float>, tRows, tRows > Result;
    Result.eye();
    int info = 0;

    cgesv_(n, n, a.pIJ(), n, ipiv, Result.pIJ(), n, &info);

    if(info != 0) {
        std::cerr << "?gesv returned error: " << info << std::endl;
        abort();
    }

    return Result;
}

template < size_t tRows >
Matrix< double, tRows, tRows > inv( const Matrix< double, tRows, tRows >& R ) {
    int n = tRows;
    Matrix< double, tRows, tRows > a = R;
    int ipiv[tRows] = {0};
    Matrix< double, tRows, tRows > Result;
    Result.eye();
    int info = 0;

    dgesv_(n, n, a.pIJ(), n, ipiv, Result.pIJ(), n, &info);

    if(info != 0) {
        std::cerr << "?gesv returned error: " << info << std::endl;
        abort();
    }

    return Result;
}

template < size_t tRows >
Matrix< std::complex<double>, tRows, tRows > inv( const Matrix< std::complex<double>, tRows, tRows >& R ) {
    int n           = tRows;
    Matrix< std::complex<double>, tRows, tRows > a = R;
    int ipiv[tRows] = {0};
    Matrix< std::complex<double>, tRows, tRows > Result;
    Result.eye();
    int info        = 0;

    zgesv_(n, n, a.pIJ(), n, ipiv, Result.pIJ(), n, &info);

    if(info != 0) {
        std::cerr << "?gesv returned error: " << info << std::endl;
        abort();
    }

    return Result;
}

extern "C" void sgetrf_(const int &m, const int &n, float *A,
                        const int &lda, int *ipiv, int *info);
extern "C" void cgetrf_(const int &m, const int &n, std::complex<float> *A,
                        const int &lda, int *ipiv, int *info);
extern "C" void dgetrf_(const int &m, const int &n, double *A,
                        const int &lda, int *ipiv, int *info);
extern "C" void zgetrf_(const int &m, const int &n, std::complex<double> *A,
                        const int &lda, int *ipiv, int *info);

template < size_t tRows >
float det( const Matrix< float, tRows, tRows >& R ) {
    int n = tRows;
    Matrix< float, tRows, tRows > a = R;
    int ipiv[tRows] = {0};
    int info = 0;
    float result = 1.0;

    sgetrf_(n,n,a.pIJ(),n,&ipiv[0],&info);

    if (info == 0) {
        for(int i=0; i<n; i++) {
            if(ipiv[i] != (i+1)) result *= -a[i][i]; // i+1 for fortran
            else                 result *= +a[i][i];
        }
    } else {
        std::cerr << "?getrf returned error: " << info << std::endl;
        abort();
    }
    return result;
}

template < size_t tRows >
std::complex<float> det( const Matrix< std::complex<float>, tRows, tRows >& R ) {
    int n = tRows;
    Matrix< std::complex<float>, tRows, tRows > a = R;
    int ipiv[tRows] = {0};
    int info = 0;
    std::complex<float> result(1.0,0.0);

    cgetrf_(n,n,a.pIJ(),n,&ipiv[0],&info);

    if (info == 0) {
        for(int i=0; i<n; i++) {
            if(ipiv[i] != (i+1)) result *= -a[i][i]; // i+1 for fortran
            else                 result *= +a[i][i];
        }
    } else {
        std::cerr << "?getrf returned error: " << info << std::endl;
        abort();
    }
    return result;
}

template < size_t tRows >
double det( const Matrix< double, tRows, tRows >& R ) {
    int n = tRows;
    Matrix< double, tRows, tRows > a = R;
    int ipiv[tRows] = {0};
    int info = 0;
    double result = 1.0;

    dgetrf_(n,n,a.pIJ(),n,&ipiv[0],&info);

    if (info == 0) {
        for(int i=0; i<n; i++) {
            if(ipiv[i] != (i+1)) result *= -a[i][i]; // i+1 for fortran
            else                 result *= +a[i][i];
        }
    } else {
        std::cerr << "?getrf returned error: " << info << std::endl;
        abort();
    }
    return result;
}

template < size_t tRows >
std::complex<double> det( const Matrix< std::complex<double>, tRows, tRows >& R ) {
    int n = tRows;
    Matrix< std::complex<double>, tRows, tRows > a = R;
    int ipiv[tRows] = {0};
    int info = 0;
    std::complex<double> result(1.0,0.0);

    zgetrf_(n,n,a.pIJ(),n,&ipiv[0],&info);

    if (info == 0) {
        for(int i=0; i<n; i++) {
            if(ipiv[i] != (i+1)) result *= -a[i][i]; // i+1 for fortran
            else                 result *= +a[i][i];
        }
    } else {
        std::cerr << "?getrf returned error: " << info << std::endl;
        abort();
    }
    return result;
}

template < class tData >
Matrix< tData, 3, 3 > R(tData angle, char axis) {
    assert(axis == 'x' || axis == 'y' || axis == 'z');
    Matrix< tData, 3, 3 > result;

    if(axis == 'x') {
        result = {1.0,0.0,0.0,0.0,cos(angle),-sin(angle),0.0,sin(angle), cos(angle)};
    } else if(axis=='y') {
        result = {cos(angle),0.0,sin(angle),0.0,1.0,0.0,-sin(angle),0.0,cos(angle)};
    } else if(axis=='z') {
        result = {cos(angle),-sin(angle),0.0,sin(angle),cos(angle),0.0,0.0,0.0,1.0};
    } else  {
        abort();
    }
    return result;
}

template < class tData >
Matrix< tData, 3, 3 > TIE(tData t) {
    return R(cnst::WS*t,'z');
}

template < class tData >
Matrix< tData, 3, 3 >  TEL(tData lat_rad,tData lon_rad) {
    return (R(lon_rad,'z')*R(-lat_rad-cnst::PI_2,'y'));
}

template < class tData >
Matrix< tData, 3, 3 >  ang_to_tib(tData psi,tData the,tData phi) {
    tData a_r_psi[] = {cos(psi),-sin(psi),        0,
                       sin(psi), cos(psi),        0,
                       0,        0,        1
                      };
    Matrix< tData, 3, 3 > r_psi(a_r_psi);

    tData a_r_the[] = {cos(the),        0, sin(the),
                       0,        1,        0,
                       -sin(the),        0, cos(the)
                      };
    Matrix< tData, 3, 3 > r_the(a_r_the);

    tData a_r_phi[] = {       1,        0,        0,
                              0, cos(phi),-sin(phi),
                              0, sin(phi), cos(phi)
                      };
    Matrix< tData, 3, 3 > r_phi(a_r_phi);

    return r_psi*r_the*r_phi;
}

template < class tData >
void uv_to_ae(tData *az,tData *el,Matrix< tData, 3, 1 > &u) {
    if(norm(u) != 1.0) u=u*(1.0/norm(u));
    *az=atan2(u(2),u(1));
    *el=asin(-u(3));
    return;
}

} // from namespace

#endif // from _EMATRIX_H

Generated by GNU Enscript 1.6.5.90.