mbs_dopri5.c 4.43 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
    if (mbs_dd->nState != 0)
68
    {
69
70
71
72
73
74
75
        free(((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->dydt_save);

        for (i = 0; i < DOPRI5_NB_ALLOC_VEC; i++)
        {
            free(((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab[i]);
        }
        free(((MbsDirdynDopri5 *)mbs_dd->integrator_struct)->alloc_tab);
76
    }
77

78
79
}

80
int fcn_dopri5(unsigned int n, long nr, double tsim, double y[], double dydt[], MbsData *s, MbsDirdyn *mbs_dd)
81
82
83
{
    unsigned int i;
    double *dydt_save;
84
    int err = 0;
85
86
87
    MbsDirdynDopri5 *dopri_struct = ((MbsDirdynDopri5 *)mbs_dd->integrator_struct);

    dydt_save = dopri_struct->dydt_save;
88

89

90
    if ((!nr) && dopri_struct->flag_save) // use previously computed solution for first time step
91
92
93
94
95
96
97
98
    {
        for (i = 0; i < n; i++)
        {
            dydt[i] = dydt_save[i];
        }
    }
    else // compute new solution
    {
99
100
101
102
103
104
        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;
        }
105
106
107
108
109
110
111
112
113
114
115

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

            dopri_struct->flag_save = 1;
        }
    }
116
    return 0;
117
118
}

119
int solout_dopri5(long nr, double tsim_old, double tsim, double y[], unsigned n, int* irtrn, int init_flag, MbsData *s, MbsDirdyn *mbs_dd)
120
121
{
    MbsDirdynDopri5 *dopri_struct = ((MbsDirdynDopri5 *)mbs_dd->integrator_struct);
122
    int err = 0; // error_management
123
124
125
126

    // when using waypoints, only waypoints are used if asked
    if ((!init_flag) && mbs_dd->options->flag_waypoint && mbs_dd->options->flag_solout_wp)
    {
127
        return 0;
128
129
130
131
132
133
134
135
136
137
138
    }

    // 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
139
140
141
142
143
144
        err = save_realtime_update(mbs_dd, s);
        if (err < 0)
        {
            error_dopri5(s, mbs_dd, err);
            return err;
        }
145
146
147
148

        // quit the simulation if requested
        *irtrn = (s->flag_stop) ? -1 : 1;
    }
149
    return 0;
150
}
151
152
153
154
155

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);
156
    // finish_dopri5(mbs_data, dd); // already called in mbs_dirdyn_finish
157
}