solution.cpp
Go to the documentation of this file.
00001 
00002 
00003 
00006 
00007 // Copyright (C) 1994: R B Davies
00008 
00009 
00010 #define WANT_STREAM                  // include.h will get stream fns
00011 #define WANT_MATH                    // include.h will get math fns
00012 
00013 #include "include.h"
00014 #include "myexcept.h"
00015 
00016 #include "solution.h"
00017 
00018 #ifdef use_namespace
00019 namespace RBD_COMMON {
00020 #endif
00021 
00022 
00023 void R1_R1::Set(Real X)
00024 {
00025    if ((!minXinf && X <= minX) || (!maxXinf && X >= maxX))
00026        Throw(SolutionException("X value out of range"));
00027    x = X; xSet = true;
00028 }
00029 
00030 R1_R1::operator Real()
00031 {
00032    if (!xSet) Throw(SolutionException("Value of X not set"));
00033    Real y = operator()();
00034    return y;
00035 }
00036 
00037 unsigned long SolutionException::Select;
00038 
00039 SolutionException::SolutionException(const char* a_what) : BaseException()
00040 {
00041    Select = BaseException::Select;
00042    AddMessage("Error detected by solution package\n");
00043    AddMessage(a_what); AddMessage("\n");
00044    if (a_what) Tracer::AddTrace();
00045 }
00046 
00047 inline Real square(Real x) { return x*x; }
00048 
00049 void OneDimSolve::LookAt(int V)
00050 {
00051    lim--;
00052    if (!lim) Throw(SolutionException("Does not converge"));
00053    Last = V;
00054    Real yy = function(x[V]) - YY;
00055    Finish = (fabs(yy) <= accY) || (Captured && fabs(x[L]-x[U]) <= accX );
00056    y[V] = vpol*yy;
00057 }
00058 
00059 void OneDimSolve::HFlip() { hpol=-hpol; State(U,C,L); }
00060 
00061 void OneDimSolve::VFlip()
00062    { vpol = -vpol; y[0] = -y[0]; y[1] = -y[1]; y[2] = -y[2]; }
00063 
00064 void OneDimSolve::Flip()
00065 {
00066    hpol=-hpol; vpol=-vpol; State(U,C,L);
00067    y[0] = -y[0]; y[1] = -y[1]; y[2] = -y[2];
00068 }
00069 
00070 void OneDimSolve::State(int I, int J, int K) { L=I; C=J; U=K; }
00071 
00072 void OneDimSolve::Linear(int I, int J, int K)
00073 {
00074    x[J] = (x[I]*y[K] - x[K]*y[I])/(y[K] - y[I]);
00075    // cout << "Linear\n";
00076 }
00077 
00078 void OneDimSolve::Quadratic(int I, int J, int K)
00079 {
00080    // result to overwrite I
00081    Real YJK, YIK, YIJ, XKI, XKJ;
00082    YJK = y[J] - y[K]; YIK = y[I] - y[K]; YIJ = y[I] - y[J];
00083    XKI = (x[K] - x[I]);
00084    XKJ = (x[K]*y[J] - x[J]*y[K])/YJK;
00085    if ( square(YJK/YIK)>(x[K] - x[J])/XKI ||
00086       square(YIJ/YIK)>(x[J] - x[I])/XKI )
00087    {
00088       x[I] = XKJ;
00089       // cout << "Quadratic - exceptional\n";
00090    }
00091    else
00092    {
00093       XKI = (x[K]*y[I] - x[I]*y[K])/YIK;
00094       x[I] = (XKJ*y[I] - XKI*y[J])/YIJ;
00095       // cout << "Quadratic - normal\n";
00096    }
00097 }
00098 
00099 Real OneDimSolve::Solve(Real Y, Real X, Real Dev, int Lim)
00100 {
00101    enum Loop { start, captured1, captured2, binary, finish };
00102    Tracer et("OneDimSolve::Solve");
00103    lim=Lim; Captured = false;
00104    if ( Dev == 0.0 ) Throw(SolutionException("Dev is zero"));
00105    L=0; C=1; U=2; vpol=1; hpol=1; y[C]=0.0; y[U]=0.0;
00106    if (Dev<0.0) { hpol=-1; Dev = -Dev; }
00107    YY=Y;                                // target value
00108    x[L] = X;                            // initial trial value
00109    if (!function.IsValid(X))
00110       Throw(SolutionException("Starting value is invalid"));
00111    Loop TheLoop = start;
00112    for (;;)
00113    {
00114       switch (TheLoop)
00115       {
00116       case start:
00117          LookAt(L); if (Finish) { TheLoop = finish; break; }
00118          if (y[L]>0.0) VFlip();               // so Y[L] < 0
00119 
00120          x[U] = X + Dev * hpol;
00121          if (!function.maxXinf && x[U] > function.maxX)
00122             x[U] = (function.maxX + X) / 2.0;
00123          if (!function.minXinf && x[U] < function.minX)
00124             x[U] = (function.minX + X) / 2.0;
00125 
00126          LookAt(U); if (Finish) { TheLoop = finish; break; }
00127          if (y[U] > 0.0) { TheLoop = captured1; Captured = true; break; }
00128          if (y[U] == y[L])
00129             Throw(SolutionException("Function is flat"));
00130          if (y[U] < y[L]) HFlip();             // Change direction
00131          State(L,U,C);
00132          for (i=0; i<20; i++)
00133          {
00134             // cout << "Searching for crossing point\n";
00135             // Have L C then crossing point, Y[L]<Y[C]<0
00136             x[U] = x[C] + Dev * hpol;
00137             if (!function.maxXinf && x[U] > function.maxX)
00138             x[U] = (function.maxX + x[C]) / 2.0;
00139             if (!function.minXinf && x[U] < function.minX)
00140             x[U] = (function.minX + x[C]) / 2.0;
00141 
00142             LookAt(U); if (Finish) { TheLoop = finish; break; }
00143             if (y[U] > 0) { TheLoop = captured2; Captured = true; break; }
00144             if (y[U] < y[C])
00145                 Throw(SolutionException("Function is not monotone"));
00146             Dev *= 2.0;
00147             State(C,U,L);
00148          }
00149          if (TheLoop != start ) break;
00150          Throw(SolutionException("Cannot locate a crossing point"));
00151 
00152       case captured1:
00153          // cout << "Captured - 1\n";
00154          // We have 2 points L and U with crossing between them
00155          Linear(L,C,U);                   // linear interpolation
00156                                           // - result to C
00157          LookAt(C); if (Finish) { TheLoop = finish; break; }
00158          if (y[C] > 0.0) Flip();            // Want y[C] < 0
00159          if (y[C] < 0.5*y[L]) { State(C,L,U); TheLoop = binary; break; }
00160 
00161       case captured2:
00162          // cout << "Captured - 2\n";
00163          // We have L,C before crossing, U after crossing
00164          Quadratic(L,C,U);                // quad interpolation
00165                                           // - result to L
00166          State(C,L,U);
00167          if ((x[C] - x[L])*hpol <= 0.0 || (x[C] - x[U])*hpol >= 0.0)
00168             { TheLoop = captured1; break; }
00169          LookAt(C); if (Finish) { TheLoop = finish; break; }
00170          // cout << "Through first stage\n";
00171          if (y[C] > 0.0) Flip();
00172          if (y[C] > 0.5*y[L]) { TheLoop = captured2; break; }
00173          else { State(C,L,U); TheLoop = captured1; break; }
00174 
00175       case binary:
00176          // We have L, U around crossing - do binary search
00177          // cout << "Binary\n";
00178          for (i=3; i; i--)
00179          {
00180             x[C] = 0.5*(x[L]+x[U]);
00181             LookAt(C); if (Finish) { TheLoop = finish; break; }
00182             if (y[C]>0.0) State(L,U,C); else State(C,L,U);
00183          }
00184          if (TheLoop != binary) break;
00185          TheLoop = captured1; break;
00186 
00187       case finish:
00188          return x[Last];
00189 
00190       }
00191    }
00192 }
00193 
00194 bool R1_R1::IsValid(Real X)
00195 {
00196    Set(X);
00197    return (minXinf || x > minX) && (maxXinf || x < maxX);
00198 }
00199 
00200 #ifdef use_namespace
00201 }
00202 #endif
00203 
00204 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Tue May 28 2013 14:52:54