mbs_w_methods.c 9.89 KB
Newer Older
1
2
3
4
/**
* \file mbs_w_methods.c
*
* \brief This file implements the functions of the
5
6
* W Methods implicit integration method in C.
* Equivalent to Implicit Euler with two (or more) stages
7
*
8
* Creation date: April 2018
9
10
11
* \author Sebastien Timmermans
*
* @source   Arnold M. et al, Linearly implicit time integration methods in real-time applications: DAEs and stiff ODEs
12
*             Multibody System Dynamics, 2007, 17:99–117
13
14
15
16
17
*
* (c) Universite catholique de Louvain
*
*/

18
#include "mbs_w_methods.h"
19
20
21
22
23
24
25
#include "mbs_message.h"
#include "integrator.h"

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

    MbsDirdynOptions* opts = mbs_dd->options;

26
    // Allocate WMethods  structure
27
28
    mbs_dd->integrator_struct = malloc(sizeof(MbsDirdynWMethods));

29
    int n = mbs_dd->nState;
30
    int nqu = mbs_data->nqu;
31

32
33
34
35
36
37
38
39
    if (mbs_dd->nState != 0)
    {
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->indx = get_ivec_0(n);
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->dfdx = get_dvec_0(n);
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->dfdy = get_dmat_0(n, n);
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->f = get_dvec_0(n);
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->dysav = get_dvec_0(n);
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->ysav = get_dvec_0(n);
40

41
42
43
44
        /*  to freeze jacobian */
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->Freeze_dydx = get_dvec_0(mbs_dd->nState);
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->Freeze_dfdx = get_dvec_0(mbs_dd->nState);
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->Freeze_dfdy = get_dmat_0(mbs_dd->nState, mbs_dd->nState);
45

46
47
48
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->A = get_dmat_0(nqu, nqu);                        // left side of the equation, at step i
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->B = get_dvec_0(nqu);                            // right side of the equation, at step i
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->sum_alpha_gamma = get_dvec_0(nqu);                // internal step variable
49
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->partial_right_hand_term = get_dvec_0(nqu);        // partial rigth hand step variable
50
51
52
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->jpxv = get_dvec_0(nqu);                            // useful vector to store Jnp * xv n
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->delta_p = get_dmat_0(nqu, W_S);                // to store delta p
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->delta_v = get_dmat_0(nqu, W_S);                // to store delta v
53

54
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->A_param = get_dvec_0(W_S - 1);                    // Parameter A
55
56
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->A_param[0] = W_A21;

57
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->B_param = get_dvec_0(W_S);                        // Parameter B
58
59
60
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->B_param[0] = W_B1;
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->B_param[1] = W_B2;

61
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->C_param = get_dvec_0(W_S);                        // Parameter C
62
63
64
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->C_param[0] = W_C1;
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->C_param[1] = W_C2;

65
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->GAM_param = get_dmat_0(W_S, W_S);                // Parameter Gamma // ou *2-1 si vecteur  ?
66
67
68
69
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->GAM_param[0][0] = W_GAMMA11;
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->GAM_param[1][1] = W_GAMMA22;
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->GAM_param[1][0] = W_GAMMA21;

70
        ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->intermediate_y = get_dvec_0(n);                            // useful vector to store y [1xnqu]
71
    }
72
73
    ((MbsDirdynWMethods *)mbs_dd->integrator_struct)->Freeze_jacobian_index = 0;

74
    mbs_warning_msg("The W Methods integrator is still in development and may not work for specific problems ! \n ");
75
76
}

77
int loop_w_methods(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
78
79

    double h = tf - t0;
80
    int err = 0;  // error management
81

82
83
84
85
    // Check remaining time
    if (h > mbs_dd->dt) {
        h = mbs_dd->dt;
    }
86

87
88
    // first call of f'
    user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
89
90
91
92
93
    err = mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
    if (err < 0) {
        error_w_methods(mbs_data, mbs_dd, err);
        return err;
    }
94

95
    while (mbs_dd->tsim < tf) {
96

97
        user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
98
99
100
101
102
        
        if (mbs_data->flag_stop) {
            // stop the simulation if 'flag_stop' set to 1
            break;
        }
103

104
105
        if (mbs_dd->options->flag_precise_dynamics) // Compute f' if asked
        {
106
107
            err = mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
            if (err < 0){
108
109
                error_w_methods(mbs_data, mbs_dd, err);
                return err;
110
            }
111
        }
112

113
114
115
        if (mbs_dd->nState != 0)
        {
            // Call the W Method function
116
            err = w_methods(mbs_dd->y, mbs_dd->yd, mbs_dd->nState, mbs_dd->tsim, h, mbs_dd->yout, mbs_fct_dirdyn, mbs_data, mbs_dd);
117
            if (err < 0){
118
119
                error_w_methods(mbs_data, mbs_dd, err);
                return err;
120
            }
121
122
123
        }
        else
        {
124
125
            error_w_methods(mbs_data, mbs_dd, err);
            return err;
126
        }
127

128
129
        mbs_dd->tsim += h;

130
131
        err = mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd); // next f'
        if (err < 0){
132
133
            error_w_methods(mbs_data, mbs_dd, err);
            return err;
134
        }
135
        if (((!mbs_dd->options->flag_solout_wp) || (mbs_dd->options->flag_solout_wp && mbs_dd->tsim == tf))) {
136
137
138
139
140
141
            err = save_realtime_update(mbs_dd, mbs_data);
            if (err < 0)
            {
                error_w_methods(mbs_data, mbs_dd, err);
                return err;
            }
142
143
        }
    }
144
    return 0;
145
146
147
148
}

void finish_w_methods(MbsData *mbs_data, MbsDirdyn *dd) {

149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
    if (!dd) { // Check dd has not been set to NULL
        if (dd->nState != 0 && dd->integrator_struct) // Check integrator structure has not been set to NULL
        {
            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->dfdx);
            ((MbsDirdynWMethods *)dd->integrator_struct)->dfdx = NULL;
            free_dmat_0(((MbsDirdynWMethods *)dd->integrator_struct)->dfdy);
            ((MbsDirdynWMethods *)dd->integrator_struct)->dfdy = NULL;
            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->f);
            ((MbsDirdynWMethods *)dd->integrator_struct)->f = NULL;
            free_ivec_0(((MbsDirdynWMethods *)dd->integrator_struct)->indx);
            ((MbsDirdynWMethods *)dd->integrator_struct)->indx = NULL;
            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->dysav);
            ((MbsDirdynWMethods *)dd->integrator_struct)->dysav = NULL;
            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->ysav);
            ((MbsDirdynWMethods *)dd->integrator_struct)->ysav = NULL;

            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->Freeze_dydx);
            ((MbsDirdynWMethods *)dd->integrator_struct)->Freeze_dydx = NULL;
            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->Freeze_dfdx);
            ((MbsDirdynWMethods *)dd->integrator_struct)->Freeze_dfdx = NULL;
            free_dmat_0(((MbsDirdynWMethods *)dd->integrator_struct)->Freeze_dfdy);
            ((MbsDirdynWMethods *)dd->integrator_struct)->Freeze_dfdy = NULL;

            // Free the memory
            free_dmat_0(((MbsDirdynWMethods *)dd->integrator_struct)->delta_p);
            ((MbsDirdynWMethods *)dd->integrator_struct)->delta_p = NULL;
            free_dmat_0(((MbsDirdynWMethods *)dd->integrator_struct)->delta_v);
            ((MbsDirdynWMethods *)dd->integrator_struct)->delta_v = NULL;
            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->A_param);
            ((MbsDirdynWMethods *)dd->integrator_struct)->A_param = NULL;
            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->B_param);
            ((MbsDirdynWMethods *)dd->integrator_struct)->B_param = NULL;
            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->C_param);
            ((MbsDirdynWMethods *)dd->integrator_struct)->C_param = NULL;
            free_dmat_0(((MbsDirdynWMethods *)dd->integrator_struct)->GAM_param);
            ((MbsDirdynWMethods *)dd->integrator_struct)->GAM_param = NULL;
            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->sum_alpha_gamma);
            ((MbsDirdynWMethods *)dd->integrator_struct)->sum_alpha_gamma = NULL;
            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->partial_right_hand_term);
            ((MbsDirdynWMethods *)dd->integrator_struct)->partial_right_hand_term = NULL;
            free_dmat_0(((MbsDirdynWMethods *)dd->integrator_struct)->A);
            ((MbsDirdynWMethods *)dd->integrator_struct)->A = NULL;
            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->B);
            ((MbsDirdynWMethods *)dd->integrator_struct)->B = NULL;
            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->jpxv);
            ((MbsDirdynWMethods *)dd->integrator_struct)->jpxv = NULL;

            free_dvec_0(((MbsDirdynWMethods *)dd->integrator_struct)->intermediate_y);
            ((MbsDirdynWMethods *)dd->integrator_struct)->intermediate_y = NULL;
        }
199
    }
200
}
201
202
203
204
205

void error_w_methods(MbsData *mbs_data, MbsDirdyn *dd, int err) {
    mbs_msg("\t >>DIRDYN>>\n");
    mbs_msg("\t >>DIRDYN>> Error during integration in direct dynamics at time %g s !\n", mbs_data->tsim);
    mbs_msg("\t >>DIRDYN>> During W Methods integrator [%d] \n", err);
206
    finish_w_methods(mbs_data, dd); // already called in mbs_dirdyn_finish
207
}