AzimuthCurve.h

#ifndef AzimuthCurve_h__
#define AzimuthCurve_h__
 
#include <vector>
#include <cstddef>
#include <Eigen/Eigen>
 
template <class T>
class AzimuthCurve{
public:
    AzimuthCurve( std::vector<T> coeff, std::vector<double> aziAngle ){
        MakeCofficients(coeff, aziAngle);
    }
 
    AzimuthCurve( std::vector<T> coeff ){
        size_t size = coeff.size();
        m_coeff_size = size;
        m_coeff.resize(size);
        for (size_t i =0; i < size; i++){
            m_coeff[i] = coeff[i];
        }
    }
 
    T eval( double angle ){
        T retval = m_coeff[0];
        for( size_t j=1; j < ((m_coeff_size - 1) / 2 + 1); j++)
            retval += m_coeff[j]*cos( j * angle);
        for( size_t j=((m_coeff_size - 1) / 2 + 1); j < m_coeff_size; j++)
            retval +=  m_coeff[j]*sin( (j-(m_coeff_size - 1) / 2) * angle);
        return retval;
    }
 
    std::vector<T> eval( std::vector<T> angle ){
        std::vector<T> retval; retval.resize(angle.size());
        for( size_t i=0; i < angle.size(); i++)
            retval[i] = eval( angle[i] );
        return retval;
    }
 
private:
    std::vector<T> m_coeff;
    size_t m_coeff_size;
 
    void MakeCofficients(std::vector<T> values, std::vector<double> aziAngle){
 
        size_t size = values.size();
 
        // Solve 2*N + 1 unknown (A0 + A1..AN + B1..BN
        size_t N = static_cast<size_t>(std::floor((size - 1) / 2));
        m_coeff_size = 2*N+1;
        m_coeff.resize(m_coeff_size);
 
        Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> A = Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic>::Zero(m_coeff_size, m_coeff_size);
        Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> B = Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic>::Zero(m_coeff_size, 1);
        Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic> x = Eigen::Matrix<T,Eigen::Dynamic,Eigen::Dynamic>::Zero(m_coeff_size, 1);
 
        // right hand side of equation system
        for(size_t k=0; k < m_coeff_size; k++)
            B(k) =  values[k];
 
        // build system matrix, left hand side. A0, A1..AN and B1..BN
        // build for each B value.
        for(size_t k=0; k < m_coeff_size; k++){
            A(k, 0) = 1;
            for (size_t j = 1; j < N + 1; j++) {
                A(k, j + 0) = cos(j * aziAngle[k]);
                A(k, j + N) = sin(j * aziAngle[k]);
            }
        }
 
        for(size_t k=0; k < size; k++)
            for(size_t j=0; j < size; j++){
                if( (std::abs(A(k,j)) < 1e-6) )
                    A(k,j) = 0;
            }
 
        x = A.colPivHouseholderQr().solve(B);
        for(size_t k=0; k < size; k++)
            m_coeff[k] = x(k);
    }
};
 
#endif // AzimuthCurve_h__