solver/state_handling.cpp

Go to the documentation of this file.

/****************************************
 * INCLUDES 
 ****************************************/

#include "state_handling.h"



/****************************************
 * FUNCTIONS 
 ****************************************/

namespace state_handling
{
    void tilde_to_bar (const double sinA, const double cosA, double *state)
    {
        double tmp =  cosA*state[0] + sinA*state[3];
        state[3]   = -sinA*state[0] + cosA*state[3];
        state[0]   = tmp;
    }

    void bar_to_tilde (const double sinA, const double cosA, double *state)
    {
        double tmp = cosA*state[0] - sinA*state[3];
        state[3]   = sinA*state[0] + cosA*state[3];
        state[0]   = tmp;
    }


    void tilde_to_orig (const double h, double *state)
    {
        state[0] = state[0] + h * state[2];
        state[3] = state[3] + h * state[5];
    }

    void orig_to_tilde (const double h, double *state)
    {
        state[0] = state[0] - h * state[2];
        state[3] = state[3] - h * state[5];
    }


    void get_state_tilde (const problem_parameters& sp, const double *X, const int ind, double *state)
    {
        int index;
        if (ind >= sp.N)
        {
            index = sp.N - 1;
        }
        else
        {
            index = ind;
        }

        for (int i = 0; i < SMPC_NUM_STATE_VAR; i++)
        {
            state[i] = X[index*SMPC_NUM_STATE_VAR + i];
        }

        state_handling::bar_to_tilde (sp.spar[index].sin, sp.spar[index].cos, state);
    }


    void get_state (const problem_parameters& sp, const double *X, const int ind, double *state)
    {
        int index;
        if (ind >= sp.N)
        {
            index = sp.N - 1;
        }
        else
        {
            index = ind;
        }
        get_state_tilde (sp, X, ind, state);
        state_handling::tilde_to_orig (sp.spar[index].h, state);
    }


    void get_controls (const int preview_window_size, const double *X, const int ind, double *controls)
    {
        int index;
        if (ind >= preview_window_size)
        {
            index = preview_window_size-1;
        }
        else
        {
            index = ind;
        }
        controls[0] = X[preview_window_size*SMPC_NUM_STATE_VAR + index*SMPC_NUM_CONTROL_VAR + 0];
        controls[1] = X[preview_window_size*SMPC_NUM_STATE_VAR + index*SMPC_NUM_CONTROL_VAR + 1];
    }
}