mbs_euler_explicit.c 7.24 KB
Newer Older
1
/**
2
* \file mbs_euler_explicit.c
3
4
5
6
7
8
9
*
* \brief This file implements the functions of the
* Euler Explicit integration method in C (+ Eulaire method).
* Eulaire is a slightly different method from Euler explicit, see the implementation for more informations
*
* Creation date: April 2018
* \author Sebastien Timmermans
10
*
11
12
13
14
*
* (c) Universite catholique de Louvain
*
*/
15

16
#include "mbs_euler_explicit.h"
17
#include "mbs_message.h"
18
19
20

void initialize_eulerEx(MbsData *mbs_data, MbsDirdyn *mbs_dd) {

21
    MbsDirdynOptions* opts = mbs_dd->options;
22
23
24
25

    // Allocate EulerEx structure
    mbs_dd->integrator_struct = malloc(sizeof(MbsDirdynEulerEx));

26
    if (mbs_dd->options->integrator == AlphaM)
27
    {
28
29
30
31
32
        ((MbsDirdynEulerEx *)mbs_dd->integrator_struct)->choice_of_euler = alpha_method;
        mbs_warning_msg("The Alpha-Method integrator is (very) simplified here and has not been tested extensively! \n ");

        ((MbsDirdynEulerEx *)mbs_dd->integrator_struct)->a_n = get_dvec_0(mbs_dd->nState);
        ((MbsDirdynEulerEx *)mbs_dd->integrator_struct)->a_n_1 = get_dvec_0(mbs_dd->nState);
33
34
35
36
37
38
39
40
41

        double p_inf = P_INF_VALUE; 

        double one_over_p_inf_plus_1 = 1.0 / (p_inf + 1.0);
        ((MbsDirdynEulerEx *)mbs_dd->integrator_struct)->alpha_m = (2.0*p_inf - 1.0)  * one_over_p_inf_plus_1;
        ((MbsDirdynEulerEx *)mbs_dd->integrator_struct)->alpha_f = (p_inf)* one_over_p_inf_plus_1;
        ((MbsDirdynEulerEx *)mbs_dd->integrator_struct)->gamma = 0.5 + ((MbsDirdynEulerEx *)mbs_dd->integrator_struct)->alpha_f + ((MbsDirdynEulerEx *)mbs_dd->integrator_struct)->alpha_m;
        ((MbsDirdynEulerEx *)mbs_dd->integrator_struct)->beta = 0.25 * (((MbsDirdynEulerEx *)mbs_dd->integrator_struct)->gamma + 0.5)*(((MbsDirdynEulerEx *)mbs_dd->integrator_struct)->gamma + 0.5);
        ((MbsDirdynEulerEx *)mbs_dd->integrator_struct)->div_alpha_m = (1.0 / (1.0 - ((MbsDirdynEulerEx *)mbs_dd->integrator_struct)->alpha_m) );
42
    }
43
    else if (mbs_dd->options->integrator == EulerEx)
44
45
    {
        ((MbsDirdynEulerEx *)mbs_dd->integrator_struct)->choice_of_euler = eulaire;
46
        mbs_warning_msg("The Eulaire (aire!) integrator has not been tested extensively! \n ");
47
    }
48
49
50
51
52
    else
    {
        ((MbsDirdynEulerEx *)mbs_dd->integrator_struct)->choice_of_euler = euler_explicit;
        mbs_warning_msg("The Euler Explicit integrator has not been tested extensively! \n ");
    }
53
54
}

55
56
int loop_eulerEx(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
    int err;
57
    double h = tf - t0;
58

59
60
61
62
    // Check remaining time
    if (h > mbs_dd->dt) {
        h = mbs_dd->dt;
    }
63

64
65
    // first call of f'
    user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
66
67
    err = mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
    if (err < 0){
68
69
        error_eulerEx(mbs_data, mbs_dd, err);
        return err;
70
71
    }

72
73
    while (mbs_dd->tsim <= tf)
    {
74
        user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
75
76
77
78
79
        
        if (mbs_data->flag_stop) {
            // stop the simulation if 'flag_stop' set to 1
            break;
        }
80

81
82
        if (mbs_dd->options->flag_precise_dynamics) // Compute f' if asked
        {
83
84
            err = mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
            if (err < 0){
85
86
                error_eulerEx(mbs_data, mbs_dd, err);
                return err;
87
            }
88
        }
89

90
91
        // calls the correct euler implementation
        ((MbsDirdynEulerEx *)mbs_dd->integrator_struct)->choice_of_euler(h, mbs_data, mbs_dd);
92

93
        mbs_dd->tsim += h;
94
95
        err = mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd); // next f'
        if (err < 0){
96
97
            error_eulerEx(mbs_data, mbs_dd, err);
            return err;
98
        }
99

100
        // save results if always asked or if waypoint reached
101
        if ((!mbs_dd->options->flag_solout_wp) || (mbs_dd->options->flag_solout_wp && mbs_dd->tsim == tf)) {
102
            if (mbs_dd->tsim > mbs_dd->options->dt0) // special case of euler explicit, do not save the first step
103
            {
104
105
106
107
108
109
                err = save_realtime_update(mbs_dd, mbs_data);
                if (err < 0)
                {
                    error_eulerEx(mbs_data, mbs_dd, err);
                    return err;
                }
110
            }
111
112
        }
    }
113
    return 0;
114
115
116
}

void finish_eulerEx(MbsData *mbs_data, MbsDirdyn *dd) {
117

118
119
120
121
122
123
124
125
126
127
128
129
    if (!dd) { // Check dd has not been set to NULL
        if (dd->integrator_struct) // Check integrator structure has not been set to NULL
        {
            if (dd->options->integrator == AlphaM)
            {
                // free the vectors
                free_dvec_0(((MbsDirdynEulerEx *)dd->integrator_struct)->a_n_1);
                ((MbsDirdynEulerEx *)dd->integrator_struct)->a_n_1 = NULL;
                free_dvec_0(((MbsDirdynEulerEx *)dd->integrator_struct)->a_n);
                ((MbsDirdynEulerEx *)dd->integrator_struct)->a_n = NULL;
            }
        }
130
    }
131
}
132

133
134
135
136
137
void error_eulerEx(MbsData *mbs_data, MbsDirdyn *dd, int err)
{
    mbs_msg("\t >>Euler explicit>>\n");
    mbs_msg("\t >>Euler explicit>> Error during integration in direct dynamics at time %g s !\n", mbs_data->tsim);
    mbs_msg("\t >>Euler explicit>> During time integration [%d] \n", err);
138
    finish_eulerEx(mbs_data, dd); // already called in mbs_dirdyn_finish
139
140
141
}


142
143
144
145
146
147
// ###### start of EulerExplicit #########
void euler_explicit(double h, MbsData *mbs_data, MbsDirdyn *mbs_dd)
{
    int k;
    for (k = 0; k < mbs_dd->nState; k++)
    {
148
        mbs_dd->y[k] = mbs_dd->y[k] + h * mbs_dd->yd[k];
149
150
151
152
153
154
155
156
157
158
159
    }
}
// ###### end of EulerExplicit #########


// ###### start of Eulaire #########
void eulaire(double h, MbsData *mbs_data, MbsDirdyn *mbs_dd)
{
    int k;
    for (k = 0; k < mbs_data->nqu; k++)
    {
160
        mbs_dd->y[k + mbs_data->nqu] = mbs_dd->y[k + mbs_data->nqu] + h * mbs_dd->yd[k + mbs_data->nqu];
161
162
163
164
165
        mbs_dd->y[k] = mbs_dd->y[k] + 0.5* h*(mbs_dd->y[k + mbs_data->nqu] + mbs_dd->yd[k]);
    }
}
// ###### end of Eulaire #########

166
// ###### start of Alpha Method #########
167
void alpha_method(double h, MbsData *mbs_data, MbsDirdyn *mbs_dd)
168
169
{
    int k;
170

171
172
173
174
    double alpha_m = ((MbsDirdynEulerEx *)mbs_dd->integrator_struct)->alpha_m;
    double alpha_f = ((MbsDirdynEulerEx *)mbs_dd->integrator_struct)->alpha_f;
    double gamma = ((MbsDirdynEulerEx *)mbs_dd->integrator_struct)->gamma;
    double beta = ((MbsDirdynEulerEx *)mbs_dd->integrator_struct)->beta;
175
176
    double *a_n_1 = ((MbsDirdynEulerEx*)mbs_dd->integrator_struct)->a_n_1;
    double *an = ((MbsDirdynEulerEx*)mbs_dd->integrator_struct)->a_n;
177
    
178
    copy_dvec_0(mbs_dd->yd, an, mbs_dd->nState);
179

180
181
182
    for (k = 0; k < mbs_data->nqu; k++)
    {
        //a_{n+1} = (1/(1-alpha_m)) * (alpha_f*ydd - alpha_m*a_n)
183
        a_n_1[k + mbs_data->nqu] = ((MbsDirdynEulerEx*)mbs_dd->integrator_struct)->div_alpha_m* (alpha_f*mbs_dd->yd[k + mbs_data->nqu] - alpha_m * an[k + mbs_data->nqu]);
184
185
186
187
        // v_{n+1} = v_n + h* (1-gamma) *a_n + h*gamma*a_{n+1}
        mbs_dd->y[k + mbs_data->nqu] = mbs_dd->y[k + mbs_data->nqu] + h *(1.0-gamma) * an[k + mbs_data->nqu] + h* gamma * a_n_1[k + mbs_data->nqu];
        // q_{n+1} = q_n + h* v_n + h*h*(1/2 - beta)*a_n+h*h*beta*a_{n+1}
        mbs_dd->y[k] = mbs_dd->y[k] + h * mbs_dd->y[k + mbs_data->nqu] + h * h* (0.5 - beta)*an[k + mbs_data->nqu] + h * h*beta*a_n_1[k + mbs_data->nqu];
188
189
    }
}
190
// ###### end of Alpah Method #########