Added extrapolation of variable trajectories as starting values for n…

…onlinear equation systems. Required for solving nonlinear systems with multiple solutions, such as x^2-c =0 (Need to extrapolate to go from positive to negative solution)

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

Compiler/DAELow.mo

@@ -109,20 +109,6 @@ uniontype Var "- Variables"
 
 end Var;
 
-public 
-uniontype StateSelect "- State Selection"
-  record NEVER end NEVER;
-
-  record AVOID end AVOID;
-
-  record DEFAULT end DEFAULT;
-
-  record PREFER end PREFER;
-
-  record ALWAYS end ALWAYS;
-
-end StateSelect;
-
 public 
 uniontype Equation "- Equation"
   record EQUATION
@@ -2250,6 +2236,27 @@ algorithm
   end matchcontinue;
 end varFixed;
 
+public function varStateSelect "
+  author: PA
+ 
+  Extacts the state select attribute of a variable. If no stateselect explicilty set, return 
+  StateSelect.default 
+ 
+"
+  input Var inVar;
+  output DAE.StateSelect outStateSelect;
+algorithm 
+  outStateSelect:=
+  matchcontinue (inVar)
+    local
+      DAE.StateSelect stateselect;
+      Var v;
+    case (VAR(values = SOME(DAE.VAR_ATTR_REAL(_,_,_,_,_,_,_,SOME(stateselect))))) 
+    then stateselect; 
+      case (_) then DAE.DEFAULT();
+  end matchcontinue;
+end varStateSelect;
+
 public function vararrayList "function: vararrayList
  
   Transforms a VariableArray to a Var list
@@ -7757,20 +7764,25 @@ protected function selectDummyState "function: selectDummyState
               IncidenceMatrixT) 
   outputs: (Exp.ComponentRef, int)
 "
-  input list<Exp.ComponentRef> inExpComponentRefLst;
-  input list<Integer> inIntegerLst;
+  input list<Exp.ComponentRef> varCrefs;
+  input list<Integer> varIndices;
   input DAELow inDAELow;
   input IncidenceMatrix inIncidenceMatrix;
   input IncidenceMatrixT inIncidenceMatrixT;
   output Exp.ComponentRef outComponentRef;
   output Integer outInteger;
 algorithm 
   (outComponentRef,outInteger):=
-  matchcontinue (inExpComponentRefLst,inIntegerLst,inDAELow,inIncidenceMatrix,inIncidenceMatrixT)
+  matchcontinue (varCrefs,varIndices,inDAELow,inIncidenceMatrix,inIncidenceMatrixT)
     local
       Exp.ComponentRef s;
       Value sn;
-    case ((s :: _),(sn :: _),_,_,_) then (s,sn);  /* for now, select the first one... */ 
+      Variables vars;
+      list<tuple<Exp.ComponentRef,Integer,Real>> prioTuples;
+    case (varCrefs,varIndices,DAELOW(orderedVars=vars),_,_)  equation
+ 		  prioTuples = calculateVarPriorities(varCrefs,varIndices,vars);
+ 		  (s,sn) = selectMinPrio(prioTuples);     
+   	then (s,sn);  
     case ({},_,_,_,_)
       equation 
         print("Error, no state to select\n");
@@ -7779,6 +7791,87 @@ algorithm
   end matchcontinue;
 end selectDummyState;
 
+protected function selectMinPrio "Selects the state with lowest priority. This will become a dummy state"
+  input list<tuple<Exp.ComponentRef,Integer,Real>> tuples;
+  output Exp.ComponentRef s;
+  output Integer sn;
+algorithm
+  (s,sn) := matchcontinue(tuples)
+    case(tuples) equation
+      ((s,sn,_)) = Util.listReduce(tuples,ssPrioTupleMin);
+      then (s,sn);
+  end matchcontinue;
+end selectMinPrio;
+
+protected function ssPrioTupleMin "Select the minimum tuple of two tuples"
+  input tuple<Exp.ComponentRef,Integer,Real> tuple1;
+  input tuple<Exp.ComponentRef,Integer,Real> tuple2;
+  output tuple<Exp.ComponentRef,Integer,Real> tuple3;
+algorithm
+  tuple3 := matchcontinue(tuple1,tuple2)
+    local Exp.ComponentRef cr1,cr2;
+      Integer ns1,ns2;
+      Real rs1,rs2;      
+    case((cr1,ns1,rs1),(cr2,ns2,rs2)) equation
+      true = (rs1 <. rs2);
+    then ((cr1,ns1,rs1));
+
+    case ((cr1,ns1,rs1),(cr2,ns2,rs2)) equation
+      true = (rs2 <. rs1);
+    then ((cr2,ns2,rs2));
+      //exactly equal, choose first one.
+
+    case ((cr1,ns1,rs1),(cr2,ns2,rs2)) then ((cr1,ns1,rs1));
+  end matchcontinue;
+end ssPrioTupleMin;
+
+protected function calculateVarPriorities "Calculates state selection priorities"
+	input list<Exp.ComponentRef> varCrefs;
+  input list<Integer> varIndices; 
+  input Variables vars;
+  output list<tuple<Exp.ComponentRef,Integer,Real>> tuples;
+algorithm
+  tuples := matchcontinue(varCrefs,varIndices,vars)
+  local Exp.ComponentRef varCref;
+    Integer varIndx;
+    Var v;
+    Real prio;
+    list<tuple<Exp.ComponentRef,Integer,Real>> prios;
+    case({},{},_) then {};
+    case (varCref::varCrefs,varIndx::varIndices,vars) equation
+      prios = calculateVarPriorities(varCrefs,varIndices,vars);
+      (v::_,_) = getVar(varCref,vars);
+      prio = varStateSelectPrio(v);
+    then ((varCref,varIndx,prio)::prios);
+  end matchcontinue;
+end calculateVarPriorities;
+
+protected function varStateSelectPrio "helper function to calculateVarPriorities"
+	input Var v;
+	output Real prio;
+	protected
+	DAE.StateSelect ss;
+algorithm
+  ss := varStateSelect(v);
+  prio := varStateSelectPrio2(ss);
+end varStateSelectPrio;
+
+
+protected function varStateSelectPrio2 "helper function to varStateSelectPrio"
+	input DAE.StateSelect ss;
+	output Real prio;
+algorithm
+	ss := matchcontinue(ss)
+	  case (DAE.NEVER()) then -10.0;
+	  case (DAE.AVOID()) then 0.0;
+	  case (DAE.DEFAULT()) then 10.0;
+	  case (DAE.PREFER()) then 50.0;
+	  case (DAE.ALWAYS()) then 100.0;
+	end matchcontinue;
+end varStateSelectPrio2;
+
+
+
 protected function calculateDummyStatePriorities "function: calculate_dummy_state_priority
  
   Calculates a priority for dummy state candidates.

Compiler/SimCodegen.mo

@@ -643,13 +643,22 @@ extObjConstructorsDecl_str,
   returnData->nHelpVars = NHELP;\n
   if(flags & STATES && returnData->nStates){\n
     returnData->states = (double*) malloc(sizeof(double)*returnData->nStates);\n
+    returnData->oldStates = (double*) malloc(sizeof(double)*returnData->nStates);\n
+    returnData->oldStates2 = (double*) malloc(sizeof(double)*returnData->nStates);\n
   }else{\n
     returnData->states = 0;\n
+    returnData->oldStates = 0;\n
+    returnData->oldStates2 = 0;\n
   }\n
   if(flags & STATESDERIVATIVES && returnData->nStates){\n
     returnData->statesDerivatives = (double*) malloc(sizeof(double)*returnData->nStates);\n
+    returnData->oldStatesDerivatives = (double*) malloc(sizeof(double)*returnData->nStates);\n
+    returnData->oldStatesDerivatives2 = (double*) malloc(sizeof(double)*returnData->nStates);\n
+
   }else{\n
     returnData->statesDerivatives = 0;\n
+    returnData->oldStatesDerivatives = 0;\n
+    returnData->oldStatesDerivatives2 = 0;\n
   }\n
   if(flags & HELPVARS && returnData->nHelpVars){\n
     returnData->helpVars = (double*) malloc(sizeof(double)*returnData->nHelpVars);\n
@@ -658,8 +667,12 @@ extObjConstructorsDecl_str,
   }\n", "
   if(flags & ALGEBRAICS && returnData->nAlgebraic){\n
     returnData->algebraics = (double*) malloc(sizeof(double)*returnData->nAlgebraic);\n
+    returnData->oldAlgebraics = (double*) malloc(sizeof(double)*returnData->nAlgebraic);\n
+    returnData->oldAlgebraics2 = (double*) malloc(sizeof(double)*returnData->nAlgebraic);\n        
   }else{\n
     returnData->algebraics = 0;\n
+    returnData->oldAlgebraics = 0;\n
+    returnData->oldAlgebraics2 = 0;\n    
   }\n
   if(flags & PARAMETERS && returnData->nParameters){\n
     returnData->parameters = (double*) malloc(sizeof(double)*returnData->nParameters);\n
@@ -4448,7 +4461,7 @@ algorithm
         indx_str = intString(indx);
         indx_1 = indx + 1;
         (func,cg_id_1) = generateOdeSystem2NonlinearSetvector(crs, indx_1, cg_id);
-        stmt = Util.stringAppendList({"nls_x[",indx_str,"] = ",cr_str,";"});
+        stmt = Util.stringAppendList({"nls_x[",indx_str,"] = extraPolate(",cr_str,");"});
         func_1 = Codegen.cAddStatements(func, {stmt});
       then
         (func_1,cg_id_1);

c_runtime/matrix.h

@@ -22,6 +22,12 @@ void hybrd_(void (*) (int*, double *, double*, int*),
 	    int* nprint,int* info,int* nfev,double* fjac,
 	    int* ldfjac,double* r, int* lr, double* qtf,
 	    double* wa1,double* wa2,double* wa3,double* wa4);
+
+void * hybrj_(void(*) (int *,double*,double*,double *,int*, int*),
+	    int *n,double*x,double*fvec,double*fjac,int *ldfjac,double*xtol,int* maxfev,
+	    double* diag,int *mode,double*factor,int *nprint,int*info,int*nfev,int*njev,
+	    double* r,int *lr,double*qtf,double*wa1,double*wa2,
+        double* wa3,double* wa4);
 
 #if defined(__cplusplus)
 }
@@ -106,6 +112,35 @@ void hybrd_(void (*) (int*, double *, double*, int*),
 	 }\
 } while(0) /* (no trailing ;)*/ 
 
+#define solve_nonlinear_system_analytic_jac(residual,no) do { \
+	 int giveUp=0; \
+	 int retries = 0; \
+	 while(!giveUp) { \
+		 giveUp = 1; \
+		 hybrj_(residual,&n, nls_x,nls_fvec,nls_fjac,&ldfjac,&xtol,&maxfev,\
+    	    nls_diag,&mode,&factor,&nprint,&info,&nfev,&njev, \
+        nls_r,&lr,nls_qtf,nls_wa1,nls_wa2,nls_wa3,nls_wa4); \
+    	if (info == 0) { \
+    	    printf("improper input parameters to nonlinear eq. syst.\n"); \
+    	} \
+    	if ((info == 4 || info == 5 )&& retries < 3) { /* First try to decrease factor*/ \
+    		retries++; giveUp = 0; \
+    		factor = factor / 10.0; \
+    		 	if (sim_verbose)  \
+	    		printf("Solving nonlinear system: iteration not making progress, trying to decrease factor to %f\n",factor); \
+    	} else if ((info == 4 || info == 5) && retries < 5) { /* Secondly, try with different starting point*/  \
+    		int i; \
+    		for (i=0; i < n; i++) { nls_x[i]+=0.1; }; \
+    		retries++; giveUp=0; \
+    		if (sim_verbose) \
+   		 		printf("Solving nonlinear system: iteration not making progress, trying with different starting points (+1e-6)"); \
+    	} \
+    	else if (info >= 2 && info <= 5) { \
+    	    printf("error solving nonlinear system nr. %d at time %f\n",no,time); \
+    	} \
+	 }\
+} while(0) /* (no trailing ;)*/ 
+
 #define declare_matrix(A,nrows,ncols) double *A = new double[nrows*ncols]; \
 assert(A!=0); \
 for (int i=0;i<nrows*ncols;i++) A[i]=0.0;
@@ -166,8 +201,8 @@ double nls_wa1[size]; \
 double nls_wa2[size]; \
 double nls_wa3[size]; \
 double nls_wa4[size]; \
-double xtol = 1e-9; \
-double epsfcn=1e-9; \
+double xtol = 1e-12; \
+double epsfcn=1e-12; \
 int maxfev=8000; \
 int n=size; \
 int ml=size-1; \
@@ -180,8 +215,34 @@ int lr = (size*(size+1))/2; \
 int ldfjac = size; \
 double nls_fjac[size*size]
 
+#define start_nonlinear_system_analytic_jac(size) { double nls_x[size]; \
+double nls_fvec[size]; \
+double nls_fjac[size*size]; \
+double nls_diag[size]; \
+double nls_r[(size*(size+1)/2)]; \
+double nls_qtf[size]; \
+double nls_wa1[size]; \
+double nls_wa2[size]; \
+double nls_wa3[size]; \
+double nls_wa4[size]; \
+double xtol = 1e-12; \
+double epsfcn=1e-12; \
+int maxfev=8000; \
+int n=size; \
+int ml=size-1; \
+int mu = size-1; \
+int mode=1; \
+int info,nfev,njev; \
+double factor=100.0; \
+int nprint = 0; \
+int lr = (size*(size+1))/2; \
+int ldfjac = size; 
 #define end_nonlinear_system() } do {} while(0)
 
+#define extraPolate(v) (localData->oldTime == localData->oldTime2 ) ? v: \
+((old(&v)-old2(&v))/(localData->oldTime-localData->oldTime2)*localData->timeValue \
++(localData->oldTime*old2(&v)-localData->oldTime2*old(&v))/ \
+(localData->oldTime-localData->oldTime2))
 
 #define mixed_equation_system(size) do { \
 int found_solution = 0; \