1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
|
/*
SPDX-FileCopyrightText: 2007 Vladimir Kuznetsov <ks.vladimir@gmail.com>
SPDX-License-Identifier: GPL-2.0-or-later
*/
#include "eulersolver.h"
#include "util.h"
#include <cmath>
#include <cstring>
namespace StepCore {
STEPCORE_META_OBJECT(GenericEulerSolver, QT_TRANSLATE_NOOP("ObjectClass", "GenericEulerSolver"), QT_TRANSLATE_NOOP("ObjectDescription", "Generic Euler solver"), MetaObject::ABSTRACT, STEPCORE_SUPER_CLASS(Solver),)
STEPCORE_META_OBJECT(EulerSolver, QT_TRANSLATE_NOOP("ObjectClass", "EulerSolver"), QT_TRANSLATE_NOOP("ObjectDescription", "Non-adaptive Euler solver"), 0, STEPCORE_SUPER_CLASS(GenericEulerSolver),)
STEPCORE_META_OBJECT(AdaptiveEulerSolver, QT_TRANSLATE_NOOP("ObjectClass", "AdaptiveEulerSolver"), QT_TRANSLATE_NOOP("ObjectDescription", "Adaptive Euler solver"), 0, STEPCORE_SUPER_CLASS(GenericEulerSolver),)
void GenericEulerSolver::init()
{
_yerr.resize(_dimension);
_ytemp.resize(_dimension);
_ydiff.resize(_dimension);
_ytempvar.resize(_dimension);
_ydiffvar.resize(_dimension);
}
void GenericEulerSolver::fini()
{
}
int GenericEulerSolver::doCalcFn(double* t, const VectorXd* y,
const VectorXd* yvar, VectorXd* f, VectorXd* fvar)
{
int ret = _function(*t, y->data(), yvar?yvar->data():nullptr,
f ? f->data() : _ydiff.data(),
fvar?fvar->data():nullptr, _params);
//if(f != NULL) std::memcpy(f, _ydiff, _dimension*sizeof(*f));
return ret;
}
int GenericEulerSolver::doStep(double t, double stepSize, VectorXd* y, VectorXd* yvar)
{
_localError = 0;
_localErrorRatio = 0;
//int ret = _function(t, y, _ydiff, _params);
//if(ret != OK) return ret;
VectorXd* ytempvar = yvar ? &_ytempvar : nullptr;
VectorXd* ydiffvar = yvar ? &_ydiffvar : nullptr;
// Error estimation: integration with timestep = stepSize
_yerr = - *y - stepSize*_ydiff;
// First integration with timestep = stepSize/2
_ytemp = *y + (stepSize/2)*_ydiff;
if(yvar) { // error calculation
*ytempvar = (yvar->array().sqrt().matrix()+(stepSize/2)*(*ydiffvar)).array().square().matrix();
}
int ret = _function(t + stepSize/2, _ytemp.data(), ytempvar?ytempvar->data():nullptr,
_ydiff.data(), ydiffvar?ydiffvar->data():nullptr, _params);
if(ret != OK) return ret;
for(int i=0; i<_dimension; ++i) {
// Second integration with timestep = stepSize/2
_ytemp[i] += stepSize/2*_ydiff[i];
// Error estimation and solution improve
_yerr[i] += _ytemp[i];
// Solution improvement
_ytemp[i] += _yerr[i];
// Maximal error calculation
double error = fabs(_yerr[i]);
if(error > _localError) _localError = error;
// Maximal error ration calculation
double errorRatio = error / (_toleranceAbs + _toleranceRel * fabs(_ytemp[i]));
if(errorRatio > _localErrorRatio) _localErrorRatio = errorRatio;
}
if(_localErrorRatio > 1.1) return ToleranceError;
// XXX
ret = _function(t + stepSize, _ytemp.data(), ytempvar?ytempvar->data():nullptr,
_ydiff.data(), ydiffvar?ydiffvar->data():nullptr, _params);
if(ret != OK) return ret;
*y = _ytemp;
if(yvar) { // error calculation
// XXX: Strictly speaking yerr are correlated between steps.
// For now we are using the following formula which
// assumes that yerr are equal and correlated on adjacent steps
// TODO: improve this formula
*yvar = (ytempvar->array().sqrt().matrix()+(stepSize/2)*(*ydiffvar)).array().square().matrix()
+ 3*_yerr.array().square().matrix();
}
return OK;
}
int GenericEulerSolver::doEvolve(double* t, double t1, VectorXd* y, VectorXd* yvar)
{
// XXX: add better checks
// XXX: replace asserts by error codes here
// or by check in world
// XXX: do the same checks in doStep
STEPCORE_ASSERT_NOABORT(*t + _stepSize != *t);
STEPCORE_ASSERT_NOABORT(*t != t1);
#ifndef NDEBUG
double realStepSize = fmin( _stepSize, t1 - *t );
#endif
const double S = 0.9;
int result;
VectorXd* ydiffvar = yvar ? &_ydiffvar : nullptr;
result = _function(*t, y->data(), yvar?yvar->data():nullptr, _ydiff.data(), ydiffvar?ydiffvar->data():nullptr, _params);
if(result != OK) return result;
while(*t < t1) {
STEPCORE_ASSERT_NOABORT(t1-*t > realStepSize/1000);
//double t11 = _stepSize < t1-*t ? *t + _stepSize : t1;
double t11 = t1 - *t > _stepSize*1.01 ? *t + _stepSize : t1;
result = doStep(*t, t11 - *t, y, yvar);
if(result != OK && result != ToleranceError) return result;
if(_adaptive) {
if(_localErrorRatio > 1.1) {
double r = S / _localErrorRatio;
if(r<0.2) r = 0.2;
_stepSize *= r;
} else if(_localErrorRatio < 0.5) {
double r = S / pow(_localErrorRatio, 0.5);
if(r>5.0) r = 5.0;
if(r<1.0) r = 1.0;
double newStepSize = _stepSize*r;
if(newStepSize < t1 - t11) _stepSize = newStepSize;
}
if(result != OK) {
result = _function(*t, y->data(), yvar?yvar->data():nullptr, _ydiff.data(),
ydiffvar?ydiffvar->data():nullptr, _params);
if(result != OK) return result;
continue;
}
} else {
if(result != OK) return ToleranceError;
}
*t = t11;
}
return OK;
}
} // namespace StepCore
|
