fix for algloop event iteration

  -added isConsistent to IAlgLoop to check if a condition has changed
moved setCondition and getCondition  to SystemDefaultImplementation

git-svn-id: https://openmodelica.org/svn/OpenModelica/trunk@18361 f25d12d1-65f4-0310-ae8a-bbce733d8d8e

Compiler/Template/CodegenCpp.tpl

@@ -117,6 +117,7 @@ case SIMCODE(modelInfo=MODELINFO(__)) then
        //Literals
         <%literals |> literal hasindex i0 fromindex 0 => literalExpConst(literal,i0) ; separator="\n";empty%>
      private:
+    
        //Function return variables
        <%functionHeaderBodies3(functions,simCode)%>
 
@@ -279,6 +280,7 @@ case SIMCODE(modelInfo = MODELINFO(__)) then
    <%giveZeroFunc1(zeroCrossings,simCode)%>
    <%setConditions(simCode)%>
    <%geConditions(simCode)%>
+   <%isConsistent(simCode)%>
    <%generateStepCompleted(listAppend(allEquations,initialEquations),simCode)%>
    <%generatehandleTimeEvent(sampleLookup,simCode)%>
    <%generateDimTimeEvent(listAppend(allEquations,initialEquations),simCode)%>
@@ -2164,11 +2166,11 @@ case SIMCODE(modelInfo = MODELINFO(__)) then
       ~<%lastIdentOfPath(modelInfo.name)%>();
 
        <%generateMethodDeclarationCode(simCode)%>
-  bool getCondition(unsigned int index);
+     virtual  bool getCondition(unsigned int index);
   private:
     //Methods:
     
-  
+     bool isConsistent();
     
      /*
      void initialAnalyticJacobian();
@@ -2438,6 +2440,10 @@ int  <%modelname%>Algloop<%index%>::getDimRHS( ) const
     return(AlgLoopDefaultImplementation::getDimRHS());
 };
 
+bool  <%modelname%>Algloop<%index%>::isConsistent( ) 
+{
+    return _system->isConsistent();
+};
 
 /// Provide variables with given index to the system
 void  <%modelname%>Algloop<%index%>::getReal(double* vars)
@@ -2529,7 +2535,7 @@ case SIMCODE(modelInfo = MODELINFO(__)) then
     virtual bool isODE();
     // M is singular
     virtual bool isAlgebraic();
-    
+  
     
     virtual int getDimTimeEvent() const;
     //gibt die Time events (Startzeit und Frequenz) zurück
@@ -2618,6 +2624,7 @@ case SIMCODE(modelInfo = MODELINFO(__)) then
     /// Output routine (to be called by the solver after every successful integration step)
     virtual void getSystemMatrix(double* A_matrix);
     virtual bool isLinear();
+    virtual bool isConsistent();
     /// Set stream for output
     virtual void setOutput(ostream* outputStream)     ;
 
@@ -7285,7 +7292,7 @@ template setConditions(SimCode simCode)
 <<
  void <%lastIdentOfPath(modelInfo.name)%>::setConditions(bool* c)
   {
-    memcpy(_conditions,c,_dimZeroFunc*sizeof(bool));
+    SystemDefaultImplementation::setConditions(c);
   }
 >>
 end setConditions;
@@ -7297,11 +7304,23 @@ template geConditions(SimCode simCode)
 <<
  void <%lastIdentOfPath(modelInfo.name)%>::getConditions(bool* c)
   {
-    memcpy(c,_conditions,_dimZeroFunc*sizeof(bool));
+     SystemDefaultImplementation::getConditions(c);
   }
 >>
 end geConditions;
 
+template isConsistent(SimCode simCode)
+::=
+ match simCode
+  case SIMCODE(modelInfo = MODELINFO(__)) then
+<<
+ bool <%lastIdentOfPath(modelInfo.name)%>::isConsistent()
+  {
+     return SystemDefaultImplementation::isConsistent();
+  }
+>>
+end isConsistent;
+
 template saveConditions(SimCode simCode)
 ::=
  match simCode

SimulationRuntime/cpp/Core/System/SystemDefaultImplementation.cpp

@@ -170,8 +170,41 @@ void SystemDefaultImplementation::getContinuousStates(double* z)
     }
 
 };
-
-
+ bool SystemDefaultImplementation::isConsistent()
+ {
+     if(IEvent* system = dynamic_cast<IEvent*>(this))
+     {
+         unsigned int dim = system->getDimZeroFunc();
+         bool* conditions0 = new bool[dim];
+         bool* conditions1 = new bool[dim];
+         getConditions(conditions0);
+         IContinuous::UPDATETYPE pre_call_type=_callType;
+         _callType = IContinuous::DISCRETE;
+         for(int i=0;i<dim;i++)
+         {
+                system->getCondition(i);
+         }
+        getConditions(conditions1);
+        bool isConsistent =  std::equal (conditions1, conditions1+_dimZeroFunc,conditions0);  
+       _callType = pre_call_type;
+        setConditions(conditions0);
+       delete[] conditions0;
+       delete[] conditions1;
+        return isConsistent;
+     }
+     else
+        return true;
+
+
+ }
+ void SystemDefaultImplementation::setConditions(bool* c)
+   {
+     memcpy(_conditions,c,_dimZeroFunc*sizeof(bool));
+   }
+  void SystemDefaultImplementation::getConditions(bool* c)
+   {
+     memcpy(c,_conditions,_dimZeroFunc*sizeof(bool));
+   }
 
 
 

SimulationRuntime/cpp/Include/Core/System/IAlgLoop.h

@@ -56,7 +56,7 @@ class IAlgLoop
   virtual void getSystemMatrix(double* A_matrix) = 0;
 
   virtual bool isLinear() = 0;
-
+  virtual bool isConsistent() = 0;
   /*/// Fügt das übergebene Objekt als Across-Kante hinzu
   void addAcrossEdge(IObject& new_obj);
 

SimulationRuntime/cpp/Include/Core/System/IEvent.h

@@ -27,13 +27,13 @@ class IEvent
     /// Provides current values of root/zero functions
     virtual void getZeroFunc(double* f) = 0;
 
-
+    virtual void  setConditions(bool* c) =0;
     virtual void getConditions(bool* c)=0;
     virtual void saveDiscreteVars() = 0;
     /// Called to handle an event 
     virtual void handleEvent(const bool* events) = 0;
     ///Checks if a discrete variable has changed and triggered an event, returns true if a second event iteration is needed
     virtual bool checkForDiscreteEvents() = 0;    
-
+    virtual  bool getCondition(unsigned int index) = 0;
 };
 

SimulationRuntime/cpp/Include/Core/System/SystemDefaultImplementation.h

@@ -77,7 +77,8 @@ class BOOST_EXTENSION_SYSTEM_DECL SystemDefaultImplementation
     /// Provide the right hand side
     virtual void getRHS(double* f);
 
-
+    virtual void  setConditions(bool* c);
+    virtual void getConditions(bool* c);
     /// Provide boolean variables
     virtual void setBoolean(const bool* z);
 
@@ -110,6 +111,7 @@ class BOOST_EXTENSION_SYSTEM_DECL SystemDefaultImplementation
      void storeDelay(unsigned int expr_id,double expr_value,double time);
      void storeTime(double time);
      double delay(unsigned int expr_id,double expr_value, double delayTime, double delayMax);
+     bool isConsistent();
     template<class T>
     T getStartValue(T variable,string key)
     {

SimulationRuntime/cpp/Solver/Hybrj/Hybrj.cpp

@@ -29,6 +29,7 @@ Hybrj::Hybrj(IAlgLoop* algLoop, INonLinSolverSettings* settings)
      ,_x_ex(NULL)
     ,_x_nom(NULL)
      ,_x_scale(NULL)
+     ,_x_restart(NULL)
      ,_initial_factor(100)
      ,_usescale(false)
 {
@@ -67,7 +68,7 @@ void Hybrj::stepCompleted(double time)
 void Hybrj::initialize()
 {
     _firstCall = false;
-
+   
     //(Re-) Initialization of algebraic loop
     _algLoop->initialize();
 
@@ -96,6 +97,7 @@ void Hybrj::initialize()
             if(_x_nom) delete[] _x_nom;
             if(_x_scale) delete[] _x_scale;
             if(_x_ex) delete[] _x_scale;
+            if(_x_restart) delete[] _x_restart;
             _x            = new double[_dimSys];
             _f            = new double[_dimSys];    
             _xHelp        = new double[_dimSys];
@@ -107,19 +109,21 @@ void Hybrj::initialize()
             _x_nom = new double[_dimSys];
             _x_scale = new double[_dimSys];
             _x_ex = new double[_dimSys];
+            _x_restart = new double[_dimSys];
             //ToDo: nominal variablen abfragen
              _algLoop->getReal(_x0);
              _algLoop->getReal(_x1);
               _algLoop->getReal(_x2);
             _algLoop->getReal(_x);
              _algLoop->getReal(_x_ex);
+              _algLoop->getReal(_x_restart);
             std::fill_n(_f,_dimSys,0.0);
               std::fill_n(_x_scale,_dimSys,1.0);
            std::fill_n(_x_nom,_dimSys,1.0);
           std::fill_n(_xHelp,_dimSys,0.0);
           std::fill_n(_fHelp,_dimSys,0.0);
           std::fill_n(_jac,_dimSys*_dimSys,0.0);
-          
+
 
             _lr = (_dimSys*(_dimSys + 1)) / 2;
             _ldfjac= _dimSys;
@@ -184,6 +188,7 @@ void Hybrj::solve(const IContinuous::UPDATETYPE command)
         int iter_retry2 = 0;
         int info;
         _iterationStatus = CONTINUE;
+        bool isConsistent = true;
         while(_iterationStatus == CONTINUE)
         {
              /* Scaling x vector */
@@ -192,6 +197,13 @@ void Hybrj::solve(const IContinuous::UPDATETYPE command)
             __minpack_func__(hybrj)((minpack_funcder_nn)fcn, &_dimSys, _x, _f, _jac, &_ldfjac, &_xtol, &_maxfev, _diag, 
                 &_mode, &_factor, &_nprint, &info, &_nfev, &_njev, _r, &_lr, _qtf, 
                 _wa1, _wa2, _wa3, _wa4,_data);
+            //check if  the conditions of the system has changed 
+            if(isConsistent)
+            {
+               isConsistent = _algLoop->isConsistent();
+               if(!isConsistent)
+                  _algLoop->getReal(_x_restart);
+            }
             /* re-scaling x vector */
             if(_usescale)
                 std::transform (_x, _x+_dimSys, _x_scale, _x, std::multiplies<double>());
@@ -327,8 +339,12 @@ void Hybrj::solve(const IContinuous::UPDATETYPE command)
         }
         if((_iterationStatus == SOLVERERROR))
         {
-           /* std::cout << "error " << _fnorm << std::endl;*/
-            throw std::invalid_argument("Unsuccessful termination of HYBRJ, iteration is making no progress ");
+            if(!isConsistent)
+            {
+                    _algLoop->setReal(_x_restart);
+            }
+            else
+                 throw std::invalid_argument("Unsuccessful termination of HYBRJ, iteration is making no progress ");
 
 
         }

SimulationRuntime/cpp/Solver/Hybrj/Hybrj.h

@@ -63,6 +63,7 @@ class Hybrj : public IAlgLoopSolver
         *_x0,                    //old unknown variables
         *_x1,                    //old unknown variables
         *_x2,
+        *_x_restart,
         *_x_nom,                    //Nominal value of unknown variables
         *_x_ex,                 //extraplated unknown varibales
         *_x_scale,                 //current scale factor of unknown varibales
@@ -91,5 +92,6 @@ class Hybrj : public IAlgLoopSolver
     int _nfev;                              //NFEV is an integer output variable set to the number of calls to FCN with IFLAG = 1.
     int _njev;                              //NJEV is an integer output variable set to the number of calls to FCN with IFLAG = 2.
     const double _initial_factor;
+
 };