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