00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053 #include "robot.h"
00054 #include <iomanip>
00055
00056 using namespace std;
00057
00058 #ifdef use_namespace
00059 namespace ROBOOP {
00060 using namespace NEWMAT;
00061 #endif
00062
00063
00064 static const char rcsid[] __UNUSED__ = "$Id: utils.cpp,v 1.7 2007/11/11 23:57:25 ejt Exp $";
00065
00066
00067 ReturnMatrix x_prod_matrix(const ColumnVector & x)
00068
00069 {
00070 Matrix S(3,3); S = 0.0;
00071 S(1,2) = -x(3); S(1,3) = x(2);
00072 S(2,1) = x(3); S(2,3) = -x(1);
00073 S(3,1) = -x(2); S(3,2) = x(1);
00074
00075 S.Release(); return (S);
00076 }
00077
00078
00079 ReturnMatrix Integ_Trap(const ColumnVector & present, ColumnVector & past,
00080 const Real dt)
00081
00082 {
00083 ColumnVector integration(present.Nrows());
00084 integration = (past+present)*0.5*dt;
00085 past = present;
00086
00087 integration.Release();
00088 return ( integration );
00089 }
00090
00091
00092 void Runge_Kutta4_Real_time(ReturnMatrix (*xdot)(Real time, const Matrix & xin,
00093 bool & exit, bool & init),
00094 const Matrix & xo, Real to, Real tf, int nsteps)
00095
00096 {
00097 static Real h, h2, t;
00098 static Matrix k1, k2, k3, k4, x;
00099 static bool first_pass = true, init = false, exit = false;
00100
00101 if(first_pass || init)
00102 {
00103 h = (tf-to)/nsteps;
00104 h2 = h/2.0;
00105 t = to;
00106 x = xo;
00107 first_pass = false;
00108 }
00109 k1 = (*xdot)(t, x, exit, init) * h;
00110 if(exit) return;
00111 k2 = (*xdot)(t+h2, x+k1*0.5, exit, init)*h;
00112 if(exit) return;
00113 k3 = (*xdot)(t+h2, x+k2*0.5, exit, init)*h;
00114 if(exit) return;
00115 k4 = (*xdot)(t+h, x+k3, exit, init)*h;
00116 if(exit) return;
00117 x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
00118 t += h;
00119 }
00120
00121 void Runge_Kutta4_Real_time(ReturnMatrix (*xdot)(const Real time, const Matrix & xin),
00122 const Matrix & xo, const Real to, const Real tf, const int nsteps)
00123 {
00124 static Real h, h2, t;
00125 static Matrix k1, k2, k3, k4, x;
00126 static bool first_pass = true;
00127
00128 if(first_pass)
00129 {
00130 h = (tf-to)/nsteps;
00131 h2 = h/2.0;
00132 t = to;
00133 x = xo;
00134 first_pass = false;
00135 }
00136 k1 = (*xdot)(t, x) * h;
00137 t += h2;
00138 k2 = (*xdot)(t, x+k1*0.5)*h;
00139 k3 = (*xdot)(t, x+k2*0.5)*h;
00140 t += h2;
00141 k4 = (*xdot)(t, x+k3)*h;
00142 x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
00143 }
00144
00145
00146 void Runge_Kutta4(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
00147 const Matrix & xo, Real to, Real tf, int nsteps,
00148 RowVector & tout, Matrix & xout)
00149
00150 {
00151 Real h, h2, t;
00152 Matrix k1, k2, k3, k4, x;
00153
00154 h = (tf-to)/nsteps;
00155 h2 = h/2.0;
00156 t = to;
00157 x = xo;
00158 xout = Matrix(xo.Nrows(),(nsteps+1)*xo.Ncols());
00159 xout.SubMatrix(1,xo.Nrows(),1,xo.Ncols()) = x;
00160 tout = RowVector(nsteps+1);
00161 tout(1) = to;
00162 for (int i = 1; i <= nsteps; i++) {
00163 k1 = (*xdot)(t, x) * h;
00164 k2 = (*xdot)(t+h2, x+k1*0.5)*h;
00165 k3 = (*xdot)(t+h2, x+k2*0.5)*h;
00166 k4 = (*xdot)(t+h, x+k3)*h;
00167 x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
00168 t += h;
00169 tout(i+1) = t;
00170 xout.SubMatrix(1,xo.Nrows(),i*xo.Ncols()+1,(i+1)*xo.Ncols()) = x;
00171 }
00172 }
00173
00174 ReturnMatrix rk4(const Matrix & x, const Matrix & dxdt, Real t, Real h,
00175 ReturnMatrix (*xdot)(Real time, const Matrix & xin))
00176
00177
00178
00179
00180
00181
00182
00183
00184 {
00185 Matrix xt, xout, dxm, dxt;
00186 Real th, hh, h6;
00187
00188 hh = h*0.5;
00189 h6 = h/6.0;
00190 th = t + hh;
00191 xt = x + hh*dxdt;
00192 dxt = (*xdot)(th,xt);
00193 xt = x + hh*dxt;
00194 dxm = (*xdot)(th,xt);
00195 xt = x + h*dxm;
00196 dxm += dxt;
00197 dxt = (*xdot)(t+h,xt);
00198 xout = x+h6*(dxdt+dxt+2.0*dxm);
00199
00200 xout.Release(); return xout;
00201 }
00202
00203 #define PGROW -0.20
00204 #define PSHRNK -0.25
00205 #define FCOR 0.06666666
00206 #define SAFETY 0.9
00207 #define ERRCON 6.0E-4
00208
00209 void rkqc(Matrix & x, Matrix & dxdt, Real & t, Real htry,
00210 Real eps, Matrix & xscal, Real & hdid, Real & hnext,
00211 ReturnMatrix (*xdot)(Real time, const Matrix & xin))
00212
00213
00214
00215
00216
00217
00218
00219
00220 {
00221 Real tsav, hh, h, temp, errmax;
00222 Matrix dxsav, xsav, xtemp;
00223
00224 tsav = t;
00225 xsav = x;
00226 dxsav = dxdt;
00227 h = htry;
00228 for(;;) {
00229 hh = 0.5*h;
00230 xtemp = rk4(xsav,dxsav,tsav,hh,xdot);
00231 t = tsav+hh;
00232 dxdt = (*xdot)(t,xtemp);
00233 x = rk4(xtemp,dxdt,t,hh,xdot);
00234 t = tsav+h;
00235 if(t == tsav) {
00236 cerr << "Step size too small in routine RKQC\n";
00237 exit(1);
00238 }
00239 xtemp = rk4(xsav,dxsav,tsav,h,xdot);
00240 errmax = 0.0;
00241 xtemp = x-xtemp;
00242 for(int i = 1; i <= x.Nrows(); i++) {
00243 temp = fabs(xtemp(i,1)/xscal(i,1));
00244 if(errmax < temp) errmax = temp;
00245 }
00246 errmax /= eps;
00247 if(errmax <= 1.0) {
00248 hdid = h;
00249 hnext = (errmax > ERRCON ?
00250 SAFETY*h*exp(PGROW*log(errmax)) : 4.0*h);
00251 break;
00252 }
00253 h = SAFETY*h*exp(PSHRNK*log(errmax));
00254 }
00255 x += xtemp*FCOR;
00256 }
00257
00258 #define MAXSTP 10000
00259 #define TINY 1.0e-30
00260
00261 void odeint(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
00262 Matrix & xo, Real to, Real tf, Real eps, Real h1, Real hmin,
00263 int & nok, int & nbad,
00264 RowVector & tout, Matrix & xout, Real dtsav)
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274 {
00275 Real tsav, t, hnext, hdid, h;
00276 RowVector tv(1);
00277
00278 Matrix xscal, x, dxdt;
00279
00280 tv = to;
00281 tout = tv;
00282 xout = xo;
00283 xscal = xo;
00284 t = to;
00285 h = (tf > to) ? fabs(h1) : -fabs(h1);
00286 nok = (nbad) = 0;
00287 x = xo;
00288 tsav = t;
00289 for(int nstp = 1; nstp <= MAXSTP; nstp++){
00290 dxdt = (*xdot)(t,x);
00291 for(int i = 1; i <= x.Nrows(); i++)
00292 xscal(i,1) = fabs(x(i,1))+fabs(dxdt(i,1)*h)+TINY;
00293 if((t+h-tf)*(t+h-to) > 0.0) h = tf-t;
00294 rkqc(x,dxdt,t,h,eps,xscal,hdid,hnext,xdot);
00295 if(hdid == h) ++(nok); else ++(nbad);
00296 if((t-tf)*(tf-to) >= 0.0) {
00297 xo = x;
00298 tv = t;
00299 tout = tout | tv;
00300 xout = xout | x;
00301 return;
00302 }
00303 if(fabs(t-tsav) > fabs(dtsav)) {
00304 tv = t;
00305 tout = tout | tv;
00306 xout = xout | x;
00307 tsav = t;
00308 }
00309 if(fabs(hnext) <= hmin) {
00310 cerr << "Step size too small in ODEINT\n";
00311 cerr << setw(7) << setprecision(3) << (tout & xout).t();
00312 exit(1);
00313 }
00314 h = hnext;
00315 }
00316 cerr << "Too many step in routine ODEINT\n";
00317 exit(1);
00318 }
00319
00320 ReturnMatrix sign(const Matrix & x)
00321
00322 {
00323 Matrix out = x;
00324
00325 for(int i = 1; i <= out.Ncols(); i++) {
00326 for(int j = 1; j <= out.Nrows(); j++) {
00327 if(out(j,i) > 0.0) {
00328 out(j,i) = 1.0;
00329 } else if(out(j,i) < 0.0) {
00330 out(j,i) = -1.0;
00331 }
00332 }
00333 }
00334
00335 out.Release(); return out;
00336 }
00337
00338
00339 short sign(const Real x)
00340
00341 {
00342 short i = 1;
00343 if(x > 0.0)
00344 i = 1;
00345 else
00346 i = -1;
00347
00348 return (i);
00349 }
00350
00351 #ifdef use_namespace
00352 }
00353 #endif
00354