Added Kinsol solver source to cpp runtime

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

Author: niklwors

SimulationRuntime/cpp/Source/System/Kinsol/Implementation/CMakeLists.txt

@@ -0,0 +1,20 @@
+cmake_minimum_required (VERSION 2.6)
+
+project(Kinsol)
+# add the solver default implementation library
+add_library(Kinsol SHARED KinsolCall.cpp KinsolCallSettings.cpp )
+target_link_libraries( Kinsol ${Boost_LIBRARIES})
+
+
+
+install (TARGETS Kinsol DESTINATION bin)
+#install (FILES "../Interfaces/NewtonSettings.xml" 
+#  DESTINATION bin/config)
+
+
+ 
+
+
+  
+  
+ 

SimulationRuntime/cpp/Source/System/Kinsol/Implementation/KinsolCall.cpp

@@ -0,0 +1,287 @@
+#include "stdafx.h"
+#include "KinsolCall.h"
+#include "KinsolSettings.h"
+
+#include "Math/Interfaces/ILapack.h"				// needed for solution of linear system with Lapack
+#include "Math/Implementation/Constants.h"		// definition of constants like uround
+
+KinsolCall::KinsolCall(IAlgLoop* algLoop, IKinsolSettings* settings)
+: _algLoop			(algLoop)
+, _kinsolSettings	((IKinsolSettings*)settings)
+, _y				(NULL)
+, _yHelp			(NULL)
+, _f				(NULL)
+, _fHelp			(NULL)
+, _jac				(NULL)
+, _dimSys			(0)
+, _firstCall		(true)
+, _iterationStatus	(CONTINUE)
+{
+	_data = ((void*)this);
+}
+
+KinsolCall::~KinsolCall()
+{	
+	if(_y) 		delete []	_y;
+	if(_yHelp)	delete []	_yHelp;
+	if(_f)		delete []	_f;	
+	if(_fHelp)	delete []	_fHelp;	
+	if(_jac)	delete []	_jac;
+
+	N_VDestroy_Serial(_Kin_y);
+	N_VDestroy_Serial(_Kin_yScale);
+	N_VDestroy_Serial(_Kin_fScale);
+
+	KINFree(&_kinMem);
+}
+
+void KinsolCall::init()
+{
+	int idid;
+
+	_firstCall = false;
+
+	//(Re-) Initialization of algebraic loop
+	_algLoop->init();
+
+	// Dimension of the system (number of variables)
+	int 
+		dimDouble	= _algLoop->getDimVars(IAlgLoop::REAL),
+		dimInt		= 0,
+		dimBool		= 0; 
+
+	// Check system dimension
+	if (dimDouble != _dimSys)
+	{
+		_dimSys = dimDouble;
+
+		if(_dimSys > 0)
+		{
+			// Initialization of vector of unknowns
+			if(_y) 		delete []	_y;
+			if(_f)		delete []	_f;	
+			if(_yHelp)	delete []	_yHelp;
+			if(_fHelp)	delete []	_fHelp;	
+			if(_jac)	delete []	_jac;
+			
+			_y			= new double[_dimSys];
+			_f			= new double[_dimSys];	
+			_yHelp		= new double[_dimSys];
+			_fHelp		= new double[_dimSys];
+			_jac		= new double[_dimSys*_dimSys];
+
+			_algLoop->giveVars(_y,NULL,NULL);
+			memset(_f,0,_dimSys*sizeof(double));
+			memset(_yHelp,0,_dimSys*sizeof(double));
+			memset(_fHelp,0,_dimSys*sizeof(double));
+			memset(_jac,0,_dimSys*_dimSys*sizeof(double));  // Wird nur benötigt, falls symbolisch vorhanden
+
+			for (int i=0;i<_dimSys;i++)
+				_yHelp[i] = 1; 
+
+			_Kin_y = N_VMake_Serial(_dimSys, _y);
+			_Kin_yScale = N_VMake_Serial(_dimSys, _yHelp);
+			_Kin_fScale = N_VMake_Serial(_dimSys, _yHelp);
+			_kinMem = KINCreate();
+
+			//Set Options
+			idid = KINSetNumMaxIters(_kinMem, _kinsolSettings->getNewtMax());
+			idid = KINInit(_kinMem, kin_fCallback, _Kin_y);
+			 if (check_flag(&idid, "KINInit", 1)) 
+				throw std::invalid_argument("KinsolCall::init()");
+			idid = KINSetUserData(_kinMem, _data);
+			if (check_flag(&idid, "KINSetUserData", 1)) 
+				 throw std::invalid_argument("KinsolCall::init()");
+			//idid = KINDense(_kinMem, _dimSys);
+			idid = KINSpgmr(_kinMem,_dimSys);
+			if (check_flag(&idid, "KINSpgmr", 1)) 
+				throw std::invalid_argument("KinsolCall::init()");
+		}
+		else
+		{
+			_iterationStatus = SOLVERERROR;
+		}
+	}
+
+	
+}
+
+void KinsolCall::solve(const IContinous::UPDATE command)
+{
+
+	int idid;
+	idid = KINSol(_kinMem, _Kin_y, KIN_LINESEARCH, _Kin_yScale, _Kin_yScale);
+	if (check_flag(&idid, "KINSol", 1)) 
+		throw std::invalid_argument("KinsolCall::solve()");
+	/*
+	long int
+		dimRHS	= 1,					// Dimension of right hand side of linear system (=b)
+		irtrn	= 0;					// Retrun-flag of Fortran code
+
+	int 
+		totStps	= 0;					// Total number of steps
+
+	// If init() was not called yet
+	if (_firstCall)
+		init();	
+
+	// Get initial values from system
+	_algLoop->giveVars(_y,NULL,NULL);
+	//_algLoop->update(command);
+	_algLoop->giveRHS(_f,NULL,NULL);
+
+
+	// Reset status flag
+	_iterationStatus = CONTINUE;
+
+	while(_iterationStatus == CONTINUE)
+	{
+		_iterationStatus = DONE;
+
+		// Check stopping criterion
+		if(totStps)
+		{
+			for(int i=0; i<_dimSys; ++i)
+			{
+				if(fabs(_f[i]) > _kinsolSettings->getRtol() * (_kinsolSettings->getAtol() + fabs(_f[i])))
+				{
+					_iterationStatus = CONTINUE;
+					break;
+				}
+			}
+		}
+		else
+			_iterationStatus = CONTINUE;
+
+		// New right hand side
+		calcFunction(_y,_f);
+
+		if(_iterationStatus == CONTINUE)
+		{
+			if(totStps < _kinsolSettings->getNewtMax())
+			{
+				// Determination of Jacobian (Fortran-format)
+				if(_algLoop->isLinear())
+				{
+					//calcFunction(_yHelp,_fHelp);
+					_algLoop->giveAMatrix(_jac);
+					//dgesv_(&_dimSys,&dimRHS,_jac,&_dimSys,_fHelp,_f,&_dimSys,&irtrn);
+					memcpy(_y,_f,_dimSys*sizeof(double));
+					_algLoop->setVars(_y,NULL,NULL);
+					_iterationStatus = DONE;
+					break;
+
+				}
+				else
+				{
+					calcJacobian();
+				}
+				// Solve linear System
+				//dgesv_(&_dimSys,&dimRHS,_jac,&_dimSys,_fHelp,_f,&_dimSys,&irtrn);
+
+				if(irtrn!=0)
+				{
+					// TODO: Throw an error message here. 
+					_iterationStatus = SOLVERERROR;
+					break;
+				}
+
+				// Increase counter
+				++ totStps;
+
+				// New solution
+				for(int i=0; i<_dimSys; ++i)
+					_y[i] -= _kinsolSettings->getDelta() * _f[i];
+				
+			}
+			else 
+				_iterationStatus = SOLVERERROR;
+		}
+	}
+	*/
+}
+
+IAlgLoopSolver::ITERATIONSTATUS KinsolCall::getIterationStatus()
+{
+	return _iterationStatus;
+}
+
+
+void KinsolCall::calcFunction(const double *y, double *residual)
+{
+	_algLoop->setVars(y,NULL,NULL);
+	_algLoop->update(IContinous::CONTINOUS);
+	_algLoop->giveRHS(residual,NULL,NULL);
+}
+
+int KinsolCall::kin_fCallback(N_Vector y,N_Vector fval, void *user_data)
+{
+	((KinsolCall*) user_data)->calcFunction(NV_DATA_S(y),NV_DATA_S(fval));
+
+	return(0);
+}
+
+
+
+void KinsolCall::calcJacobian()
+{
+	for(int j=0; j<_dimSys; ++j)
+	{
+		// Reset variables for every column
+		memcpy(_yHelp,_y,_dimSys*sizeof(double));
+
+		// Finite difference
+		_yHelp[j] += 1e-6;
+
+		calcFunction(_yHelp,_fHelp);
+
+		// Build Jacobian in Fortran format
+		for(int i=0; i<_dimSys; ++i)
+			_jac[i+j*_dimSys] = (_fHelp[i] - _f[i]) / 1e-6;
+	}
+
+}
+
+
+ int KinsolCall::check_flag(void *flagvalue, char *funcname, int opt)
+{
+  int *errflag;
+
+  /* Check if SUNDIALS function returned NULL pointer - no memory allocated */
+  if (opt == 0 && flagvalue == NULL) {
+    fprintf(stderr, 
+            "\nSUNDIALS_ERROR: %s() failed - returned NULL pointer\n\n",
+	    funcname);
+    return(1);
+  }
+
+  /* Check if flag < 0 */
+  else if (opt == 1) {
+    errflag = (int *) flagvalue;
+    if (*errflag < 0) {
+      fprintf(stderr,
+              "\nSUNDIALS_ERROR: %s() failed with flag = %d\n\n",
+	      funcname, *errflag);
+      return(1); 
+    }
+  }
+
+  /* Check if function returned NULL pointer - no memory allocated */
+  else if (opt == 2 && flagvalue == NULL) {
+    fprintf(stderr,
+            "\nMEMORY_ERROR: %s() failed - returned NULL pointer\n\n",
+	    funcname);
+    return(1);
+  }
+
+  return(0);
+}
+
+using boost::extensions::factory;
+
+BOOST_EXTENSION_TYPE_MAP_FUNCTION {
+  types.get<std::map<std::string, factory<IAlgLoopSolver,IAlgLoop*, IKinsolSettings*> > >()
+    ["KinsolCall"].set<KinsolCall>();
+  types.get<std::map<std::string, factory<IKinsolSettings> > >()
+    ["KinsolSettings"].set<KinsolSettings>();
+ }

SimulationRuntime/cpp/Source/System/Kinsol/Implementation/KinsolCall.h

@@ -0,0 +1,75 @@
+
+#pragma once
+
+#include "System/Interfaces/IAlgLoop.h"				// Interface to AlgLoop
+#include "System/Interfaces/IAlgLoopSolver.h"		// Export function from dll
+
+#include "System/Kinsol/Interfaces/IKinsolSettings.h"
+#include<kinsol.h>
+#include<nvector_serial.h>
+#include<kinsol_dense.h>
+
+#include<kinsol_spgmr.h>
+
+
+
+class KinsolCall : public IAlgLoopSolver
+{
+public:
+
+	KinsolCall(IAlgLoop* algLoop,IKinsolSettings* settings);
+
+	virtual ~KinsolCall();
+
+	/// (Re-) initialize the solver
+	virtual void init();
+
+	/// Solution of a (non-)linear system of equations
+	virtual void solve(const IContinous::UPDATE command = IContinous::UNDEF_UPDATE);
+
+	/// Returns the status of iteration
+	virtual ITERATIONSTATUS getIterationStatus();
+	
+
+private:
+	/// Encapsulation of determination of residuals to given unknowns
+	void calcFunction(const double* y, double* residual);
+
+	/// Encapsulation of determination of Jacobian
+	void calcJacobian(); 
+	int check_flag(void *flagvalue, char *funcname, int opt);
+	static int kin_fCallback(N_Vector y, N_Vector fval, void *user_data);
+
+
+	// Member variables
+	//---------------------------------------------------------------
+	IKinsolSettings
+		*_kinsolSettings;			///< Settings for the solver
+
+	IAlgLoop
+		*_algLoop;					///< Algebraic loop to be solved
+
+	ITERATIONSTATUS		
+		_iterationStatus;			///< Output		- Denotes the status of iteration
+
+	long int			
+		_dimSys;					///< Temp		- Number of unknowns (=dimension of system of equations)
+
+	bool
+		_firstCall;					///< Temp		- Denotes the first call to the solver, init() is called
+
+	double
+		*_y,						///< Temp		- Unknowns
+		*_f,						///< Temp		- Residuals
+		*_yHelp,					///< Temp		- Auxillary variables
+		*_fHelp,					///< Temp		- Auxillary variables
+		*_jac;						///< Temp		- Jacobian
+
+	N_Vector 
+		_Kin_y,						///< Temp			- Initial values in the Sundials Format
+		_Kin_yScale,
+		_Kin_fScale;
+	void
+		*_kinMem,					///< Temp			- Memory for the solver
+		*_data;						///< Temp			- User data. Contains pointer to KinsolCall
+};