mbs_dopri5.c 4.84 KB
Newer Older
1
/**
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
* \file mbs_dopri5.c
*
* \brief This file implements the functions of the
* dopri5 integrator in C.
*
*
* Creation date: September 2015
* \author Nicolas Van der Noot
*
* Modification date: April 2018
* \modified by Sebastien Timmermans
*
*
* (c) Universite catholique de Louvain
*/
17

18
#include "mbs_dopri5.h"
19
20
21
22
23

void initialize_dopri5(MbsData *mbs_data, MbsDirdyn *mbs_dd) {
    int i;
    MbsDirdynOptions* opts = mbs_dd->options;

24

25
    // Allocate Dopri5 structure
26
27
    mbs_dd->integrator_struct = malloc(sizeof(MbsDirdynDopri5));

28
    if (mbs_dd->nState != 0)
29
    {
30
        ((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab = (double**)malloc(DOPRI5_NB_ALLOC_VEC * sizeof(double*));
31

32
33
34
35
        for (i = 0; i < DOPRI5_NB_ALLOC_VEC; i++)
        {
            ((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab[i] = get_dvec_0(mbs_dd->nState);
        }
36

37
38
39
40
        ((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->dydt_save = get_dvec_0(mbs_dd->nState);
        ((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->solout_last_t = mbs_data->t0 - mbs_data->dt0;
        ((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->flag_save = 0;
    }
41
42
}

43
int loop_dopri5(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
44
    double hout;
45
    int err = 0;
46

47
48
49
50
51
52
    if (mbs_dd->nState != 0)
    {
        err = dopri5(mbs_dd->nState, fcn_dopri5, t0, mbs_dd->y, tf, &(mbs_dd->options->rtoler), &(mbs_dd->options->atoler),
            0, solout_dopri5, 1, 0.0, 0.0, 0.0, 0.0, 0.0, mbs_dd->options->dt_max, mbs_dd->options->dt0, mbs_dd->options->nmax,
            1, 0, 0, NULL, 0, ((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab, mbs_data, mbs_dd, &hout);
    }
53

54
    if (err < 0) // Error management
55
    {
56
57
58
        mbs_msg("\t >>Dopri5>> During Dopri5 loop [%d] \n", err);
        error_dopri5(mbs_data, mbs_dd, err);
        return err;
59
    }
60

61
    return 0;
62
63
64
65
66
}

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

    int i = 0;
67

68
69
70

    if (!mbs_dd) { // Check dd has not been set to NULL
        if (mbs_dd->nState != 0 && mbs_dd->integrator_struct) // Check integrator structure has not been set to NULL
71
        {
72
73
74
75
76
77
78
79
80
81
            free(((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->dydt_save);
            ((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->dydt_save = NULL;

            for (i = 0; i < DOPRI5_NB_ALLOC_VEC; i++)
            {
                free(((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab[i]);
                ((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab[i] = NULL;
            }
            free(((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab);
            ((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab = NULL;
82
        }
83
84
85
    }
}

86
int fcn_dopri5(unsigned int n, long nr, double tsim, double y[], double dydt[], MbsData *s, MbsDirdyn *mbs_dd)
87
88
89
{
    unsigned int i;
    double *dydt_save;
90
    int err = 0;
91
92
93
    MbsDirdynDopri5 *dopri_struct = ((MbsDirdynDopri5 *)mbs_dd->integrator_struct);

    dydt_save = dopri_struct->dydt_save;
94

95

96
    if ((!nr) && dopri_struct->flag_save) // use previously computed solution for first time step
97
98
99
100
101
102
103
104
    {
        for (i = 0; i < n; i++)
        {
            dydt[i] = dydt_save[i];
        }
    }
    else // compute new solution
    {
105
106
107
108
109
110
        err = mbs_fct_dirdyn(tsim, y, dydt, s, mbs_dd);
        if (err < 0)
        {
            mbs_msg("\t >>Dopri5>> During function Dopri5 [%d] \n", err);
            return err;
        }
111
112
113
114
115
116
117
118
119
120
121

        if (nr == 6)
        {
            for (i = 0; i < n; i++)
            {
                dydt_save[i] = dydt[i];
            }

            dopri_struct->flag_save = 1;
        }
    }
122
    return 0;
123
124
}

125
int solout_dopri5(long nr, double tsim_old, double tsim, double y[], unsigned n, int* irtrn, int init_flag, MbsData *s, MbsDirdyn *mbs_dd)
126
127
{
    MbsDirdynDopri5 *dopri_struct = ((MbsDirdynDopri5 *)mbs_dd->integrator_struct);
128
    int err = 0; // error_management
129
130
131
132

    // when using waypoints, only waypoints are used if asked
    if ((!init_flag) && mbs_dd->options->flag_waypoint && mbs_dd->options->flag_solout_wp)
    {
133
        return 0;
134
135
136
137
138
139
140
141
142
143
144
    }

    // only call new values (used for waypoints method)
    if (tsim > dopri_struct->solout_last_t)
    {
        dopri_struct->solout_last_t = tsim;

        // user loop
        user_dirdyn_loop(s, mbs_dd);

        // save data + real-time features
145
146
147
148
149
150
        err = save_realtime_update(mbs_dd, s);
        if (err < 0)
        {
            error_dopri5(s, mbs_dd, err);
            return err;
        }
151
152
153
154

        // quit the simulation if requested
        *irtrn = (s->flag_stop) ? -1 : 1;
    }
155
    return 0;
156
}
157
158
159
160
161

void error_dopri5(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 Dopri 5 integrator [%d] \n", err);
162
    finish_dopri5(mbs_data, dd); // already called in mbs_dirdyn_finish
163
}