mbs_rosenbrock.c 7.84 KB
Newer Older
1
/**
2
3
4
5
6
7
8
9
* \file mbs_rosenbrock.c
*
* \brief This file implements the functions of the
* Rosenbrock integration method in C.
* Specific functions implementation of the algorithm.
*
* Creation date: December 2016
* \author Olivier Lantsoght
10
*
11
12
13
* Modification date: April 2018
* \modified by Sebastien Timmermans
*
14
*
15
* \source  H. H. Rosenbrock, "Some general implicit processes for the numerical solution of differential equations", The Computer Journal (1963) 5(4): 329-330
16
*           Shampine, L.F. 1982, ACM Transactions on Mathematical Software, vol. 8, pp. 93-113
17
18
19
20
*
* (c) Universite catholique de Louvain
*
*/
21

22
#include "mbs_rosenbrock.h"
23
#include "mbs_message.h"
24
25
26
27
28
29


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

    MbsDirdynOptions* opts = mbs_dd->options;

30
    // Allocate Rosenbrock structure
31
    mbs_dd->integrator_struct = malloc(sizeof(MbsDirdynRosenbrock));
32
    int n = mbs_dd->nState;
33

34
35
    ((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->flag_save = 0;
    ((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->solout_last_t = 0.0;
36

37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
    if (n != 0)
    {
        ((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->dydt_save = get_dvec_0(mbs_dd->nState);

        ((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->indx = get_ivec_0(n);
        ((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->a = get_dmat_0(n, n);
        ((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->dfdx = get_dvec_0(n);
        ((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->dfdy = get_dmat_0(n, n);
        ((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->dysav = get_dvec_0(n);
        ((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->err = get_dvec_0(n);
        ((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->g1 = get_dvec_0(n);
        ((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->g2 = get_dvec_0(n);
        ((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->g3 = get_dvec_0(n);
        ((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->g4 = get_dvec_0(n);
        ((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->ysav = get_dvec_0(n);

        /*  to freeze jacobian */
        ((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->Freeze_dydx = get_dvec_0(mbs_dd->nState);
        ((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->Freeze_dfdx = get_dvec_0(mbs_dd->nState);
        ((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->Freeze_dfdy = get_dmat_0(mbs_dd->nState, mbs_dd->nState);
        ((MbsDirdynRosenbrock *)mbs_dd->integrator_struct)->Freeze_jacobian_index = 0;
58

59
    }
60
    mbs_warning_msg("The Rosenbrock integrator has not been tested extensively! \n ");
61
62
}

63
int loop_rosenbrock(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
64
65
66
67
68
69
70
    int i;
    double hdid, hnext;
    double dt0 = mbs_dd->options->dt0;
    double h_cur = dt0;
    double t = t0;
    double *scaling = get_dvec_0(mbs_dd->nState);
    double h_max = (mbs_dd->options->dt_max < tf - t) ? mbs_dd->options->dt_max : tf - t;
71
    int err = 0;
72

73
74
    // first f'
    user_dirdyn_loop(mbs_data, mbs_dd);
75
76
77
    err = mbs_fct_dirdyn(t, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
    if (err < 0){
        free_dvec_0(scaling);
78
79
        error_rosenbrock(mbs_data, mbs_dd, err);
        return err;
80
    }
81

82
    while (t < tf)
83
    {
84
        user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
85
86
87
88
89
        
        if (mbs_data->flag_stop) {
            // stop the simulation if 'flag_stop' set to 1
            break;
        }
90

91
92
        if (mbs_dd->options->flag_precise_dynamics) // Compute f' if asked
        {
93
94
95
            err = mbs_fct_dirdyn(t, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
            if (err < 0){
                free_dvec_0(scaling);
96
97
                error_rosenbrock(mbs_data, mbs_dd, err);
                return err;
98
            }
99
        }
100

101
        for (i = 0; i < mbs_dd->nState; i++) {
102
103
            scaling[i] = (1.0 > mbs_dd->y[i]) ? 1.0 : mbs_dd->y[i];
        }
104

105
        err = rosenbrock(mbs_dd->nState, mbs_fct_dirdyn, &t, mbs_dd->y, mbs_dd->options->toler,
106
107
                         h_max, h_cur, mbs_dd->options->nmax, mbs_dd->yd,
                         scaling, &hnext, mbs_data, mbs_dd, &hdid);
108

109
        if (err < 0) // Error management
110
        {
111
            free_dvec_0(scaling);
112
113
            error_rosenbrock(mbs_data, mbs_dd, err);
            return err;
114
        }
115

116
        mbs_dd->tsim = t; // Update of tsim
117

118
119
120
        h_cur = (hnext < tf - t) ? hnext : tf - t;
        h_max = (mbs_dd->options->dt_max < tf - t) ? mbs_dd->options->dt_max : tf - t;

121
122
123
        err = mbs_fct_dirdyn(t, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd); // next f'
        if (err < 0){
            free_dvec_0(scaling);
124
125
            error_rosenbrock(mbs_data, mbs_dd,err);
            return err;
126
        }
127

128
        if (((!mbs_dd->options->flag_solout_wp) || (mbs_dd->options->flag_solout_wp && t == tf))) {
129
130
131
132
133
134
            err = save_realtime_update(mbs_dd, mbs_data);
            if (err < 0)
            {
                error_rosenbrock(mbs_data, mbs_dd, err);
                return err;
            }
135
        }
136

137
    }
138

139
    free_dvec_0(scaling);
140
    return 0;
141
142
143
}

void finish_rosenbrock(MbsData *mbs_data, MbsDirdyn *dd) {
144
145
146
147
148
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
    if (dd) {  // New check dd has not been set to NULL
        if (dd->nState != 0 && dd->integrator_struct)  // New check integrator structure has not been set to NULL
        {
            free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->dydt_save);
            ((MbsDirdynRosenbrock *)dd->integrator_struct)->dydt_save = NULL;

            free_ivec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->indx);
            ((MbsDirdynRosenbrock *)dd->integrator_struct)->indx = NULL;
            free_dmat_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->a);
            ((MbsDirdynRosenbrock *)dd->integrator_struct)->a = NULL;
            free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->dfdx);
            ((MbsDirdynRosenbrock *)dd->integrator_struct)->dfdx = NULL;
            free_dmat_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->dfdy);
            ((MbsDirdynRosenbrock *)dd->integrator_struct)->dfdy = NULL;
            free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->dysav);
            ((MbsDirdynRosenbrock *)dd->integrator_struct)->dysav = NULL;
            free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->err);
            ((MbsDirdynRosenbrock *)dd->integrator_struct)->err = NULL;
            free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->g1);
            ((MbsDirdynRosenbrock *)dd->integrator_struct)->g1 = NULL;
            free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->g2);
            ((MbsDirdynRosenbrock *)dd->integrator_struct)->g2 = NULL;
            free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->g3);
            ((MbsDirdynRosenbrock *)dd->integrator_struct)->g3 = NULL;
            free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->g4);
            ((MbsDirdynRosenbrock *)dd->integrator_struct)->g4 = NULL;
            free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->ysav);
            ((MbsDirdynRosenbrock *)dd->integrator_struct)->ysav = NULL;

            free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->Freeze_dydx);
            ((MbsDirdynRosenbrock *)dd->integrator_struct)->Freeze_dydx = NULL;
            free_dvec_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->Freeze_dfdx);
            ((MbsDirdynRosenbrock *)dd->integrator_struct)->Freeze_dfdx = NULL;
            free_dmat_0(((MbsDirdynRosenbrock *)dd->integrator_struct)->Freeze_dfdy);
            ((MbsDirdynRosenbrock *)dd->integrator_struct)->Freeze_dfdy = NULL;
        }
180
    }
181
}
182
183
184
185
186
187

void error_rosenbrock(MbsData *mbs_data, MbsDirdyn *dd, int err)
{
    mbs_msg("\t >>Rosenbrock>>\n");
    mbs_msg("\t >>Rosenbrock>> Error during integration in direct dynamics at time %g s !\n", mbs_data->tsim);
    mbs_msg("\t >>Rosenbrock>> During Rosenbrock integrator [%d] \n", err);
188
    finish_rosenbrock(mbs_data, dd); // already called in mbs_dirdyn_finish
189
}