59 static const char rcsid[] =
"$Id: utils.cpp,v 1.26 2006/05/16 16:11:15 gourdeau Exp $";
65 #if _MSC_VER < 1300 // Microsoft 66 #ifndef GUARD_minmax_H 67 #define GUARD_minmax_H 70 template <
class T>
inline T
max(
const T&
a,
const T& b)
72 return (a > b) ? a : b;
74 template <
class T>
inline T min(
const T& a,
const T& b)
76 return (a < b) ? a : b;
84 using namespace NEWMAT;
92 S(1,2) = -x(3); S(1,3) = x(2);
93 S(2,1) = x(3); S(2,3) = -x(1);
94 S(3,1) = -x(2); S(3,2) = x(1);
146 for(
int i = 1; i <= Q.
size(); i++)
153 for(
int i = 1; i <= r; i++)
170 integration = (past+present)*0.5*dt;
174 return ( integration );
179 bool & exit,
bool & init),
183 static Real h, h2, t;
184 static Matrix k1, k2, k3, k4, x;
185 static bool first_pass =
true, init =
false, exit =
false;
187 if(first_pass || init)
195 k1 = (*xdot)(t, x, exit, init) * h;
197 k2 = (*xdot)(t+h2, x+k1*0.5, exit, init)*h;
199 k3 = (*xdot)(t+h2, x+k2*0.5, exit, init)*h;
201 k4 = (*xdot)(t+h, x+k3, exit, init)*h;
203 x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
208 const Matrix & xo,
const Real to,
const Real tf,
const int nsteps)
210 static Real h, h2, t;
211 static Matrix k1, k2, k3, k4, x;
212 static bool first_pass =
true;
222 k1 = (*xdot)(t, x) * h;
224 k2 = (*xdot)(t, x+k1*0.5)*h;
225 k3 = (*xdot)(t, x+k2*0.5)*h;
227 k4 = (*xdot)(t, x+k3)*h;
228 x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
248 for (
int i = 1; i <= nsteps; i++) {
249 k1 = (*xdot)(t, x) * h;
250 k2 = (*xdot)(t+h2, x+k1*0.5)*h;
251 k3 = (*xdot)(t+h2, x+k2*0.5)*h;
252 k4 = (*xdot)(t+h, x+k3)*h;
253 x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
271 Matrix xt, xout, dxm, dxt;
278 dxt = (*xdot)(th,xt);
280 dxm = (*xdot)(th,xt);
283 dxt = (*xdot)(t+h,xt);
284 xout = x+h6*(dxdt+dxt+2.0*dxm);
291 #define FCOR 0.06666666 293 #define ERRCON 6.0E-4 307 Real tsav, hh, h, temp, errmax;
308 Matrix dxsav, xsav, xtemp;
316 xtemp =
rk4(xsav,dxsav,tsav,hh,
xdot);
318 dxdt = (*xdot)(t,xtemp);
322 cerr <<
"Step size too small in routine RKQC\n";
325 xtemp =
rk4(xsav,dxsav,tsav,h,
xdot);
328 for(
int i = 1; i <= x.Nrows(); i++) {
329 temp = fabs(xtemp(i,1)/xscal(i,1));
330 if(errmax < temp) errmax = temp;
335 hnext = (errmax >
ERRCON ?
349 int & nok,
int & nbad,
361 Real tsav, t, hnext, hdid, h;
371 h = (tf > to) ? fabs(h1) : -fabs(h1);
375 for(
int nstp = 1; nstp <=
MAXSTP; nstp++){
377 for(
int i = 1; i <= x.
Nrows(); i++)
378 xscal(i,1) = fabs(x(i,1))+fabs(dxdt(i,1)*h)+
TINY;
379 if((t+h-tf)*(t+h-to) > 0.0) h = tf-t;
380 rkqc(x,dxdt,t,h,eps,xscal,hdid,hnext,
xdot);
381 if(hdid == h) ++(nok);
else ++(nbad);
382 if((t-tf)*(tf-to) >= 0.0) {
389 if(fabs(t-tsav) > fabs(dtsav)) {
395 if(fabs(hnext) <= hmin) {
396 cerr <<
"Step size too small in ODEINT\n";
397 cerr << setw(7) << setprecision(3) << (tout & xout).t();
402 cerr <<
"Too many step in routine ODEINT\n";
411 for(
int i = 1; i <= out.
Ncols(); i++) {
412 for(
int j = 1; j <= out.
Nrows(); j++) {
415 }
else if(out(j,i) < 0.0) {
ReturnMatrix Integ_Trap(const ColumnVector &present, ColumnVector &past, const Real dt)
Trapezoidal integration.
static const char rcsid[]
RCS/CVS version.
void Runge_Kutta4_Real_time(ReturnMatrix(*xdot)(Real time, const Matrix &xin, bool &exit, bool &init), const Matrix &xo, Real to, Real tf, int nsteps)
Fixed step size fourth-order Runge-Kutta integrator.
ReturnMatrix x_prod_matrix(const ColumnVector &x)
Cross product matrix.
void Runge_Kutta4(ReturnMatrix(*xdot)(Real time, const Matrix &xin), const Matrix &xo, Real to, Real tf, int nsteps, RowVector &tout, Matrix &xout)
Fixed step size fourth-order Runge-Kutta integrator.
TransposedMatrix t() const
ReturnMatrix pinv(const Matrix &M)
Matrix pseudo inverse using SVD.
void SVD(const Matrix &, DiagonalMatrix &, Matrix &, Matrix &, bool=true, bool=true)
The usual rectangular matrix.
FloatVector FloatVector * a
ReturnMatrix rk4(const Matrix &x, const Matrix &dxdt, Real t, Real h, ReturnMatrix(*xdot)(Real time, const Matrix &xin))
Compute one Runge-Kutta fourth order step.
GetSubMatrix SubMatrix(int fr, int lr, int fc, int lc) const
ReturnMatrix sign(const Matrix &x)
Sign of a matrix.
ReturnMatrix xdot(Real t, const Matrix &x)
void odeint(ReturnMatrix(*xdot)(Real time, const Matrix &xin), Matrix &xo, Real to, Real tf, Real eps, Real h1, Real hmin, int &nok, int &nbad, RowVector &tout, Matrix &xout, Real dtsav)
Integrate the ordinary differential equation xdot from time to to time tf using an adaptive step size...
void rkqc(Matrix &x, Matrix &dxdt, Real &t, Real htry, Real eps, Matrix &xscal, Real &hdid, Real &hnext, ReturnMatrix(*xdot)(Real time, const Matrix &xin))
Compute one adaptive step based on two rk4.