utils.cpp
Go to the documentation of this file.
1 /*
2 ROBOOP -- A robotics object oriented package in C++
3 Copyright (C) 1996-2004 Richard Gourdeau
4 
5 This library is free software; you can redistribute it and/or modify
6 it under the terms of the GNU Lesser General Public License as
7 published by the Free Software Foundation; either version 2.1 of the
8 License, or (at your option) any later version.
9 
10 This library is distributed in the hope that it will be useful,
11 but WITHOUT ANY WARRANTY; without even the implied warranty of
12 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 GNU Lesser General Public License for more details.
14 
15 You should have received a copy of the GNU Lesser General Public
16 License along with this library; if not, write to the Free Software
17 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18 
19 
20 Report problems and direct all questions to:
21 
22 Richard Gourdeau
23 Professeur Agrege
24 Departement de genie electrique
25 Ecole Polytechnique de Montreal
26 C.P. 6079, Succ. Centre-Ville
27 Montreal, Quebec, H3C 3A7
28 
29 email: richard.gourdeau@polymtl.ca
30 
31 -------------------------------------------------------------------------------
32 Revision_history:
33 
34 2003/02/03: Etienne Lachance
35  -Added functions x_prod_matrix, vec_dot_prod, Integ_Trap and sign.
36  -Working on Runge_Kutta4_etienne.
37 
38 2004/07/01: Etienne Lachance
39  -Removed function vec_x_prod and vec_dot_prod. Now we use CrossProduct
40  and DotProduct, from Newmat library.
41  -Added doxygen documentation.
42 
43 2004/07/01: Ethan Tira-Thompson
44  -Added support for newmat's use_namespace #define, using ROBOOP namespace
45 
46 2005/06/10: Carmine Lia
47  -Added pinv
48 
49 2005/06/29 Richard Gourdeau
50  -Fixed max() bug for VC++ 6.0
51 -------------------------------------------------------------------------------
52 */
53 
59 static const char rcsid[] = "$Id: utils.cpp,v 1.26 2006/05/16 16:11:15 gourdeau Exp $";
61 
62 #include "utils.h"
63 
64 #ifdef _MSC_VER
65 #if _MSC_VER < 1300 // Microsoft
66 #ifndef GUARD_minmax_H
67 #define GUARD_minmax_H
68 // needed to cope with bug in MS library:
69 // it fails to define min/max
70 template <class T> inline T max(const T& a, const T& b)
71 {
72  return (a > b) ? a : b;
73 }
74 template <class T> inline T min(const T& a, const T& b)
75 {
76  return (a < b) ? a : b;
77 }
78 #endif
79 #endif
80 #endif
81 
82 #ifdef use_namespace
83 namespace ROBOOP {
84  using namespace NEWMAT;
85 #endif
86 
87 
90 {
91  Matrix S(3,3); S = 0.0;
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);
95 
96  S.Release(); return (S);
97 }
98 
121 {
122  /* floating point eps */
123  const Real eps = numeric_limits<Real>::epsilon();
124 
125  int m = M.nrows();
126  int n = M.ncols();
127 
128  if(n > m)
129  {
130  Matrix X = pinv(M.t()).t();
131  return X;
132  }
133 
134  Matrix U, V;
135 
136  DiagonalMatrix Q;
137 
138  Matrix X(n, m);
139 
140  SVD(M, Q, U, V);
141  //cout << "Q\n" << Q << "U\n" << U << "V\n" << V << endl;
142  Real tol = max(m,n)*Q(1)*eps;
143  // cout << "\ntol\n" << tol << "\neps\n" << eps << endl;
144  // conteggio dei sv != 0
145  int r = 0;
146  for(int i = 1; i <= Q.size(); i++)
147  if(Q(i) > tol)
148  r++;
149 
150  if(r != 0)
151  {
152  DiagonalMatrix D(r);
153  for(int i = 1; i <= r; i++)
154  D(i) = 1/Q(i);
155  //cout << V.SubMatrix(1,V.nrows(),1,r) << endl;
156  //cout << D << endl;
157  //cout << U.SubMatrix(1,U.nrows(),1,r).t() << endl;
158  X = V.SubMatrix(1,V.nrows(),1,r)*D*U.SubMatrix(1,U.nrows(),1,r).t();
159 
160  }
161  X.Release();
162  return X;
163 }
164 
166  const Real dt)
168 {
169  ColumnVector integration(present.Nrows());
170  integration = (past+present)*0.5*dt;
171  past = present;
172 
173  integration.Release();
174  return ( integration );
175 }
176 
177 
179  bool & exit, bool & init),
180  const Matrix & xo, Real to, Real tf, int nsteps)
182 {
183  static Real h, h2, t;
184  static Matrix k1, k2, k3, k4, x;
185  static bool first_pass = true, init = false, exit = false;
186 
187  if(first_pass || init)
188  {
189  h = (tf-to)/nsteps;
190  h2 = h/2.0;
191  t = to;
192  x = xo;
193  first_pass = false;
194  }
195  k1 = (*xdot)(t, x, exit, init) * h;
196  if(exit) return;
197  k2 = (*xdot)(t+h2, x+k1*0.5, exit, init)*h;
198  if(exit) return;
199  k3 = (*xdot)(t+h2, x+k2*0.5, exit, init)*h;
200  if(exit) return;
201  k4 = (*xdot)(t+h, x+k3, exit, init)*h;
202  if(exit) return;
203  x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
204  t += h;
205 }
206 
207 void Runge_Kutta4_Real_time(ReturnMatrix (*xdot)(const Real time, const Matrix & xin),
208  const Matrix & xo, const Real to, const Real tf, const int nsteps)
209 {
210  static Real h, h2, t;
211  static Matrix k1, k2, k3, k4, x;
212  static bool first_pass = true;
213 
214  if(first_pass)
215  {
216  h = (tf-to)/nsteps;
217  h2 = h/2.0;
218  t = to;
219  x = xo;
220  first_pass = false;
221  }
222  k1 = (*xdot)(t, x) * h;
223  t += h2;
224  k2 = (*xdot)(t, x+k1*0.5)*h;
225  k3 = (*xdot)(t, x+k2*0.5)*h;
226  t += h2;
227  k4 = (*xdot)(t, x+k3)*h;
228  x = x + (k1*0.5+ k2 + k3 + k4*0.5)*(1.0/3.0);
229 }
230 
231 
232 void Runge_Kutta4(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
233  const Matrix & xo, Real to, Real tf, int nsteps,
234  RowVector & tout, Matrix & xout)
236 {
237  Real h, h2, t;
238  Matrix k1, k2, k3, k4, x;
239 
240  h = (tf-to)/nsteps;
241  h2 = h/2.0;
242  t = to;
243  x = xo;
244  xout = Matrix(xo.Nrows(),(nsteps+1)*xo.Ncols());
245  xout.SubMatrix(1,xo.Nrows(),1,xo.Ncols()) = x;
246  tout = RowVector(nsteps+1);
247  tout(1) = to;
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);
254  t += h;
255  tout(i+1) = t;
256  xout.SubMatrix(1,xo.Nrows(),i*xo.Ncols()+1,(i+1)*xo.Ncols()) = x;
257  }
258 }
259 
260 ReturnMatrix rk4(const Matrix & x, const Matrix & dxdt, Real t, Real h,
261  ReturnMatrix (*xdot)(Real time, const Matrix & xin))
270 {
271  Matrix xt, xout, dxm, dxt;
272  Real th, hh, h6;
273 
274  hh = h*0.5;
275  h6 = h/6.0;
276  th = t + hh;
277  xt = x + hh*dxdt;
278  dxt = (*xdot)(th,xt);
279  xt = x + hh*dxt;
280  dxm = (*xdot)(th,xt);
281  xt = x + h*dxm;
282  dxm += dxt;
283  dxt = (*xdot)(t+h,xt);
284  xout = x+h6*(dxdt+dxt+2.0*dxm);
285 
286  xout.Release(); return xout;
287 }
288 
289 #define PGROW -0.20
290 #define PSHRNK -0.25
291 #define FCOR 0.06666666
292 #define SAFETY 0.9
293 #define ERRCON 6.0E-4
294 
295 void rkqc(Matrix & x, Matrix & dxdt, Real & t, Real htry,
296  Real eps, Matrix & xscal, Real & hdid, Real & hnext,
297  ReturnMatrix (*xdot)(Real time, const Matrix & xin))
306 {
307  Real tsav, hh, h, temp, errmax;
308  Matrix dxsav, xsav, xtemp;
309 
310  tsav = t;
311  xsav = x;
312  dxsav = dxdt;
313  h = htry;
314  for(;;) {
315  hh = 0.5*h;
316  xtemp = rk4(xsav,dxsav,tsav,hh,xdot);
317  t = tsav+hh;
318  dxdt = (*xdot)(t,xtemp);
319  x = rk4(xtemp,dxdt,t,hh,xdot);
320  t = tsav+h;
321  if(t == tsav) {
322  cerr << "Step size too small in routine RKQC\n";
323  exit(1);
324  }
325  xtemp = rk4(xsav,dxsav,tsav,h,xdot);
326  errmax = 0.0;
327  xtemp = x-xtemp;
328  for(int i = 1; i <= x.Nrows(); i++) {
329  temp = fabs(xtemp(i,1)/xscal(i,1));
330  if(errmax < temp) errmax = temp;
331  }
332  errmax /= eps;
333  if(errmax <= 1.0) {
334  hdid = h;
335  hnext = (errmax > ERRCON ?
336  SAFETY*h*exp(PGROW*log(errmax)) : 4.0*h);
337  break;
338  }
339  h = SAFETY*h*exp(PSHRNK*log(errmax));
340  }
341  x += xtemp*FCOR;
342 }
343 
344 #define MAXSTP 10000
345 #define TINY 1.0e-30
346 
347 void odeint(ReturnMatrix (*xdot)(Real time, const Matrix & xin),
348  Matrix & xo, Real to, Real tf, Real eps, Real h1, Real hmin,
349  int & nok, int & nbad,
350  RowVector & tout, Matrix & xout, Real dtsav)
360 {
361  Real tsav, t, hnext, hdid, h;
362  RowVector tv(1);
363 
364  Matrix xscal, x, dxdt;
365 
366  tv = to;
367  tout = tv;
368  xout = xo;
369  xscal = xo;
370  t = to;
371  h = (tf > to) ? fabs(h1) : -fabs(h1);
372  nok = (nbad) = 0;
373  x = xo;
374  tsav = t;
375  for(int nstp = 1; nstp <= MAXSTP; nstp++){
376  dxdt = (*xdot)(t,x);
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) {
383  xo = x;
384  tv = t;
385  tout = tout | tv;
386  xout = xout | x;
387  return;
388  }
389  if(fabs(t-tsav) > fabs(dtsav)) {
390  tv = t;
391  tout = tout | tv;
392  xout = xout | x;
393  tsav = t;
394  }
395  if(fabs(hnext) <= hmin) {
396  cerr << "Step size too small in ODEINT\n";
397  cerr << setw(7) << setprecision(3) << (tout & xout).t();
398  exit(1);
399  }
400  h = hnext;
401  }
402  cerr << "Too many step in routine ODEINT\n";
403  exit(1);
404 }
405 
408 {
409  Matrix out = x;
410 
411  for(int i = 1; i <= out.Ncols(); i++) {
412  for(int j = 1; j <= out.Nrows(); j++) {
413  if(out(j,i) > 0.0) {
414  out(j,i) = 1.0;
415  } else if(out(j,i) < 0.0) {
416  out(j,i) = -1.0;
417  }
418  }
419  }
420 
421  out.Release(); return out;
422 }
423 
424 
425 short sign(const Real x)
427 {
428  short i = 1;
429  if(x > 0.0)
430  i = 1;
431  else
432  i = -1;
433 
434  return (i);
435 }
436 
437 #ifdef use_namespace
438 }
439 #endif
440 
#define SAFETY
Definition: utils.cpp:292
void Release()
Definition: newmat.h:514
ReturnMatrix Integ_Trap(const ColumnVector &present, ColumnVector &past, const Real dt)
Trapezoidal integration.
Definition: utils.cpp:165
static const char rcsid[]
RCS/CVS version.
Definition: utils.cpp:60
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.
Definition: utils.cpp:178
ReturnMatrix x_prod_matrix(const ColumnVector &x)
Cross product matrix.
Definition: utils.cpp:88
double Real
Definition: include.h:307
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.
Definition: utils.cpp:232
int Nrows() const
Definition: newmat.h:494
Utility header file.
int size() const
Definition: newmat.h:501
TransposedMatrix t() const
Definition: newmat6.cpp:320
ReturnMatrix pinv(const Matrix &M)
Matrix pseudo inverse using SVD.
Definition: utils.cpp:99
void SVD(const Matrix &, DiagonalMatrix &, Matrix &, Matrix &, bool=true, bool=true)
Definition: svd.cpp:30
#define TINY
Definition: utils.cpp:345
The usual rectangular matrix.
Definition: newmat.h:625
#define max(a, b)
Definition: kmlExt.cpp:32
FloatVector FloatVector * a
int Ncols() const
Definition: newmat.h:495
Diagonal matrix.
Definition: newmat.h:896
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.
Definition: utils.cpp:260
GetSubMatrix SubMatrix(int fr, int lr, int fc, int lc) const
Definition: newmat.h:2146
int ncols() const
Definition: newmat.h:500
ReturnMatrix sign(const Matrix &x)
Sign of a matrix.
Definition: utils.cpp:406
#define MAXSTP
Definition: utils.cpp:344
ReturnMatrix xdot(Real t, const Matrix &x)
Definition: demo.cpp:231
int nrows() const
Definition: newmat.h:499
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...
Definition: utils.cpp:347
#define PSHRNK
Definition: utils.cpp:290
Row vector.
Definition: newmat.h:953
#define ERRCON
Definition: utils.cpp:293
#define PGROW
Definition: utils.cpp:289
Column vector.
Definition: newmat.h:1008
#define FCOR
Definition: utils.cpp:291
const double epsilon
Definition: utils.h:126
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.
Definition: utils.cpp:295


kni
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:17