/home/jgoppert/Projects/uvsim/uvsim/src/uvsim/utilities/utilities.cc

Go to the documentation of this file.

/*
 * utilities.cc
 * Copyright (C) James Goppert 2009 <jgoppert@purdue.edu>
 *
 * utilities.cc is free software: you can redistribute it and/or modify it
 * under the terms of the GNU General Public License as published by the
 * Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * utilities.cc is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
 * See the GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License along
 * with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "uvsim/utilities/utilities.h"

using namespace boost::numeric::ublas;
using namespace boost::numeric::bindings;

namespace uvsim
{


bool fileExists(const char * filename)
{
    if (FILE * file = fopen(filename, "r"))
    {
        fclose(file);
        return true;
    }
    return false;
}
std::string doubleToString(double val)
{
    std::ostringstream out;
    out << val;
    return out.str();
}
std::string intToString(int val)
{
    std::ostringstream out;
    out << std::setfill('0') << std::setw(3) << val;
    return out.str();
}

static char *console_clearscreen    = 0;
static char *console_cursorposition = 0;

static bool console_initialize_terminfo()
{
    int result;
    if (cur_term) return true;
    setupterm( NULL, STDOUT_FILENO, &result );
    if (result > 0)
    {
        console_clearscreen    = tigetstr( "clear" );
        console_cursorposition = tigetstr( "cup"   );
    }
    return (result > 0);
}

void cursorxy( int col, int line )
{
    if (console_initialize_terminfo())
        putp( tparm( console_cursorposition, line, col, 0, 0, 0, 0, 0, 0, 0 ) );
}

void clear( bool gohome )
{
    if (console_initialize_terminfo())
    {
        putp( console_clearscreen );
        if (gohome) cursorxy( 0, 0 );
    }
}

void lowPass(const double &freq, const vector<double> &freqCut,
             const vector<double> &x, vector<double> &y)
{
    double alpha, dt;
    dt = 1/freq;
    for (int i=0; i<y.size(); i++)
    {
        alpha = dt / (1/ (freqCut(i)*M_PI) + dt);
        y(i) = alpha*x(i) + (1-alpha)*y(i);
    }
}

matrix<double> ones(int m, int n)
{
    matrix<double> A(m,n);
    for (int i=0;i<m;i++)
        for (int j=0;j<n;j++)
            A(i,j) = 1;
    return A;
}

matrix<double> skew(vector<double> &v)
{
    matrix<double> m(3,3);
    m(0,0) = 0;
    m(1,0) = -v(2);
    m(2,0) = v(1);
    m(0,1) = v(2);
    m(1,1) = 0;
    m(2,1) = -v(0);
    m(0,2) = -v(1);
    m(1,2) = v(0);
    m(2,2) = 0;
    return m;
}

double max(vector<double> v)
{
    double max = v(0);
    for (int i=1;i<v.size();i++)
    {
        if (v(i) > max) max = v(i);
    }
    return max;
}

matrix<double> inv(const matrix<double>& input)
{
    typedef permutation_matrix<std::size_t> pmatrix;
    // create a working copy of the input
        int n = input.size1();
        if (input.size2() != n) std::cout << "Error: matrix must be square." << std::endl;
        matrix<double> A(input), inverse(n,n);
    // create a permutation matrix for the LU-factorization
    pmatrix pm(A.size1());

    // perform LU-factorization
    int res = lu_factorize(A,pm);
    if ( res != 0 ) std::cout << "Error in inverse" << std::endl;

    // create identity matrix of "inverse"
    inverse.assign(identity_matrix<double>(n));

    // backsubstitute to get the inverse
    lu_substitute(A, pm, inverse);
        return inverse;
}

matrix<double> pinv(matrix<double> A)
{
    matrix<double, column_major> Ac = A;
    int m = A.size1(), n = A.size2();
    int maxDim = std::max(m,n), minDim = std::min(m,n);
    vector<double> Sig(minDim);
    matrix<double, column_major> U(m,m), SigI(n,m), VT(n,n);
    lapack::gesvd('A','A', Ac, Sig, U, VT);

    // Computer pseudo inverse of Sigma
    double tol = std::numeric_limits<double>::epsilon()*maxDim*max(Sig);
    //std::cout << "epsilon: " << std::numeric_limits<double>::epsilon() << std::endl;
    //std::cout << "maxDim: " << maxDim << std::endl;
    //std::cout << "maxSig: " << max(Sig) << std::endl;
    //std::cout << "tol: " << tol << std::endl;

    SigI = zero_matrix<double>(n,m);
    for (int i=0;i<minDim;i++)
    {
        if (Sig(i) > tol)
        {
            SigI(i,i) = 1/Sig(i);
        }
        else
        {
            SigI(i,i) = 0;
        }
    }

    // Debug
    //std::cout << "U: " << U << std::endl;
    //std::cout << "Sig: " << Sig << std::endl;
    //std::cout << "SigI: " << SigI << std::endl;
    //std::cout << "V: " << trans(VT) << std::endl;

    // Computer pseudo inverse of A
    matrix<double> temp = prod(trans(VT),SigI);
    return prod(temp,trans(U));
}

bool select(double real, double imag)
{
        if( real*real + imag*imag < 1) return 1;
        else return 0;
}

matrix<double> dare(matrix<double> A, matrix<double> B, matrix<double> R,matrix<double>Q)
{
        //A,Q,X:n*n, B:n*m, R:m*m
        int n=A.size1(),m=B.size2();
        matrix<double>Z11(n,n);
        matrix<double>Z12(n,n);
        matrix<double>Z21(n,n);
        matrix<double>Z22(n,n);
        matrix<double>G(n,n);

        matrix<double>temp(n,n);
        temp=prod(pinv(R),trans(B));
        G=prod(B,temp);
        temp=prod(G,pinv(trans(A)));
        Z12=-temp;
        Z11=A+prod(temp,Q);

        temp=trans(pinv(A));
        Z22=temp;
        Z21=prod(-temp,Q);

        //construct the Z with Z11,Z12,Z21,Z22
        matrix<double>Z(2*n,2*n);
        for (int i=0;i<2*n;i++)
        {
                for(int j=0;j<2*n;j++)
                {
                        if (i<n && j<n) //transfer Z11 data to Z
                        {
                                Z(i,j)=Z11(i,j);
                        }
                        if (i>=n && j<n)//transfer Z12 data to Z
                        {
                                Z(i,j)=Z12(i-n,j);
                        }
                        if (i<n && j>=n)//transfer Z21 data to Z
                        {
                                Z(i,j)=Z21(i,j-n);
                        }
                        if (i>=n && j>=n)//transfer Z22 data to Z
                        {
                                Z(i,j)=Z21(i-n,j-n);
                        }
                }
        }
        
        //Schur factorization
        matrix<double>U(2*n,2*n);
        vector<double> eig(2*n);
        //vector_type<double> e1;
        traits::complex_d * w;
        traits::complex_d * vs;
        int sdim;
        traits::complex_d* work;
        double* rwork;
        bool * bwork;
        int info;

        //lapack::gees<1>('N',Z,eig,Z,lapack::optimal_workspace()); //////////////////// WRONG??
        //boost::numeric::bindings::lapack::detail::gees('N','S',(logical_t*)select,n,Z,n,sdim,w,vs,n,work,-1,rwork,bwork,info);
        matrix<double>U11(n,n);
        matrix<double>U21(n,n);
        for(int i=0;i<n;i++)
        {
                for(int j=0;j<n;j++)
                {
                U11(i,j)=U(i,j);
                U21(i,j)=U(i,j+n);
                }               
        }

        return prod(U21,trans(U11));
}

matrix<double> prod(matrix<double> A, matrix<double> B, matrix<double> C)
{
        return prod(A, matrix<double> (prod(B,C)));
}

void discretize(matrix<double>& Ac, matrix<double>& Bc, matrix<double>& Ad, matrix<double>& Bd, double T, int order)
{
        Ad.resize(Ac.size1(), Ac.size2());
        Bd.resize(Bc.size1(), Bc.size2());
        
        Ad = identity_matrix<double>(Ac.size1(), Ac.size2()) + T*Ac;
        Bd = T*Bc;
        
        double T_power = T;
        matrix<double> Ac_power = Ac;
        int ifac = 1;
        
        for (int i = 2; i <= order; i++)
        {
                T_power *= T;
                ifac *= ifac+1;
                
                Bd = Bd + T_power/ifac * prod(Ac_power, Bc);
                Ac_power = prod(Ac_power, Ac);
                Ad = Ad + T_power/ifac * Ac_power;

        }

}

// Vector Integration
vector<double> integrate(vector<double> i1, vector<double> i0, vector<double> initial, double freq)
{
    vector<double> ans;
    ans = (i0 + i1)/2;
    ans = ans/freq;
    ans = ans + initial;
    return ans;
}

// Integration
double integrate (double i1, double i0, double initial, double freq)
{
    double ans;
    ans = (i0 + i1) / 2;
    ans = ans/freq;
    ans = ans + initial ;
    return ans;
}

// Calculate elapsed time
double elapsedTime(timeval time0, timeval time)
{
    double timeElapsed = (time.tv_sec - time0.tv_sec);
    timeElapsed += (time.tv_usec - time0.tv_usec) / 1e6;
    return timeElapsed;
}

// Quaternion Math
vector<double> quatProd(vector<
                                    double> q1, vector<double> q2)
{
    vector<double> prod(4);
    prod(0) = (q1(0) * q2(0) - q1(1) * q2(1) - q1(2) * q2(2) - q1(3) * q2(3));
    prod(1) = (q1(0) * q2(1) + q1(1) * q2(0) + q1(2) * q2(3) - q1(3) * q2(2));
    prod(2) = (q1(0) * q2(2) - q1(1) * q2(3) + q1(2) * q2(0) + q1(3) * q2(1));
    prod(3) = (q1(0) * q2(3) + q1(1) * q2(2) - q1(2) * q2(1) + q1(3) * q2(0));
    return prod;
}

vector<double> quatRotate(vector<double> q, vector<double> vec)
{
    vector<double> vec4(4);
    vector<double> qConj(4);
    vector<double> vec4Rotated(4);
    vector<double> vec3Rotated(3);
    vec4(0) = 0;
    subrange(vec4, 1, 4) = subrange(vec, 0, 3);
    vec4Rotated = quatProd(q,vec4);
    qConj = quatConj(q);
    vec4Rotated = quatProd(vec4Rotated, qConj);
    subrange(vec3Rotated, 0, 3) = subrange(vec4Rotated, 1, 4);
    return vec3Rotated;
}

vector<double> crossProd(vector<
                                     double> v1, vector<double> v2)
{
    vector<double> prod(3);
    prod(0) = v1(1) * v2(2) - v1(2) * v2(1);
    prod(1) = v1(2) * v2(0) - v1(0) * v2(2);
    prod(2) = v1(0) * v2(1) - v1(1) * v2(0);
    return prod;
}

vector<double> quatConj(vector<double> q)
{
    vector<double> qConj(4);
    qConj(0) = q(0);
    qConj(1) = -q(1);
    qConj(2) = -q(2);
    qConj(3) = -q(3);
    return qConj;
}

vector<double> quat2Euler(vector<double> q)
{
    vector<double> eul(3);
    double roll, pitch, yaw;
    double sq0 = q(0) * q(0);
    double sq1 = q(1) * q(1);
    double sq2 = q(2) * q(2);
    double sq3 = q(3) * q(3);
    double test = q(0) * q(2) - q(3) * q(1);


    if (test > 0.499999)   // singularity at north pole
    {
        yaw = 2 * atan2(q(1), q(0));
        pitch = M_PI / 2.;
        roll = 0;

    }
    else if (test < -0.499999)   // singularity at south pole
    {
        yaw = -2 * atan2(q(1), q(0));
        pitch = -M_PI / 2;
        roll = 0;

    }
    else
    {
        roll = atan2(2 * (q(0) * q(1) + q(2) * q(3)), sq0 - sq1 - sq2 + sq3);
        pitch = asin(-2 * (q(1) * q(3) - q(0) * q(2)));
        yaw = atan2(2 * (q(1) * q(2) + q(0) * q(3)), sq0 + sq1 - sq2 - sq3);
    }
    eul(0) = roll;
    eul(1) = pitch;
    eul(2) = yaw;
    return eul;
}

vector<double> euler2Quat(vector<double> euler)
{
    vector<double> quat(4);
        double phi = euler(0), th = euler(1), gam = euler(2);
    quat(0) = cos(phi/2)*cos(th/2)*cos(gam/2)+sin(phi/2)*sin(th/2)*sin(gam/2);
    quat(1) = sin(phi/2)*cos(th/2)*cos(gam/2)-cos(phi/2)*sin(th/2)*sin(gam/2); 
    quat(2) = cos(phi/2)*sin(th/2)*cos(gam/2)+sin(phi/2)*cos(th/2)*sin(gam/2); 
    quat(3) = cos(phi/2)*cos(th/2)*sin(gam/2)+sin(phi/2)*sin(th/2)*cos(gam/2);
    return norm(quat);
}

vector<double> axisAngle2Quat(vector<double> axis, double angle)
{
    vector<double> quat(4);

    quat(0) = cos(angle/2);
    quat(1) = sin(angle/2)*axis(0);
    quat(2) = sin(angle/2)*axis(1);
    quat(3) = sin(angle/2)*axis(2);
    return norm(quat);
}

vector<double> norm(vector<double> vec)
{
    double mag = norm_2(vec);
    if (mag == 0)
    {
        return zero_vector<double>(vec.size());
    }
    else
    {
        return vec*(1/norm_2(vec));
    }
}


}
// vim:ts=4:sw=4

---