auto_time_stepping_ssc_integrator.h

/* Copyright (C) 2004-2009  MBSim Development Team
 
 * This library is free software; you can redistribute it and/or 
 * modify it under the terms of the GNU Lesser General Public 
 * License as published by the Free Software Foundation; either 
 * version 2.1 of the License, or (at your option) any later version. 
 *  
 * This library is distributed in the hope that it will be useful, 
 * but WITHOUT ANY WARRANTY; without even the implied warranty of 
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 
 * Lesser General Public License for more details. 
 *  
 * You should have received a copy of the GNU Lesser General Public 
 * License along with this library; if not, write to the Free Software 
 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
 *
 * Contact:
 *   jan.p.clauberg@googlemail.com
 *   huber@amm.mw.tum.de
 */
 
#ifndef _AUTO_TIME_STEPPING_SSC_INTEGRATOR_H_ 
#define _AUTO_TIME_STEPPING_SSC_INTEGRATOR_H_
 
#include "integrator.h"
#include "mbsim/utils/stopwatch.h"
 
namespace MBSim {
  class Link;
}
 
namespace MBSim {
 
  class AutoTimeSteppingSSCIntegrator : public Integrator { 
 
    protected:
      MBSim::DynamicSystemSolver* sysT1;
      MBSim::DynamicSystemSolver* sysT2;
      MBSim::DynamicSystemSolver* sysT3;
      MBSim::DynamicSystemSolver* sysTP;
 
      double dt, dtOld, dte;
      double dtMin, dtMax;
      double dt_SSC_vorGapControl;
      bool driftCompensation;
      double t, tPlot, tPlotP;
      int qSize, xSize, uSize, zSize;
      fmatvec::Vec ze, zeP, zi, ziP, zT1, zT2, zT3, zTP, z1d, z2d, z2dRE, z3d, z4d, z6d, z2b, z3b, z4b, z6b, zStern;
      
      fmatvec::VecInt LS, LSe, LSA, LSB1, LSB2, LSC1, LSC2, LSC3, LSC4, LSD1, LSD2, LSD3, LSD4, LSD5, LSD6;
      fmatvec::VecInt LS_Reg, LSe_Reg, LSA_Reg, LSB1_Reg, LSB2_Reg, LSC1_Reg, LSC2_Reg, LSC3_Reg, LSC4_Reg, LSD1_Reg, LSD2_Reg, LSD3_Reg, LSD4_Reg, LSD5_Reg, LSD6_Reg;
      fmatvec::VecInt LS_Reg_T1, LS_Reg_T2, LS_Reg_T3;
      fmatvec::VecInt LS_Reg_z2b, S_Reg_z1d, LS_Reg_z4b, LS_Reg_z6b, LS_Reg_z1d, LS_Reg_z3b, LSB1_2_Reg;
      fmatvec::VecInt LS_tmp;
 
      fmatvec::Vec la, laP, lae, laeP, la1d, la2b;
      fmatvec::VecInt laSizes, laSizesP, laeSizes, laeSizesP, la1dSizes, la2bSizes;
      fmatvec::Vec qT1, qT2, qT3, qTP;
      fmatvec::Vec uT1, uT2, uT3, uTP;
      fmatvec::Vec xT1, xT2, xT3, xTP;
      fmatvec::Vec gInActive, gdInActive;
      std::vector<MBSim::Link*> SetValuedLinkListT1;
      std::vector<MBSim::Link*> SetValuedLinkListT2;
      std::vector<MBSim::Link*> SetValuedLinkListT3;
      std::vector<MBSim::Link*> SetValuedLinkListTP;
 
      int StepsWithUnchangedConstraints;
 
      int FlagErrorTest;
      
      bool FlagErrorTestAlwaysValid;
      
      fmatvec::Vec aTol;
      
      fmatvec::Vec rTol;
      
      bool FlagSSC;
      
      int maxOrder;
      
      int method;
      
      /* Flag for Gap Control - Gap Control not available for implicit integration*/
      bool FlagGapControl;
      
      double gapTol;
      
      double maxGainSSC;
      
      double safetyFactorSSC; 
      
      std::ofstream integPlot;
      
      bool FlagPlotIntegrator;
      
      bool FlagPlotIntegrationSum;
      
      bool FlagPlotEveryStep;
      
      bool outputInterpolation;
      
      double safetyFactorGapControl;
      
      int GapControlStrategy;
      
      int numThreads, numThreadsB1;
      
      double time, dhdztime, timeB1, timeB2, timePlot, timePlotPar;
      
      MBSim::StopWatch Timer, dhdzTimer, TimerB1, TimerB2, TimerPlot, TimerPlotPar;
 
      int iter, iterA, iterB1, iterB2, iterC1, iterC2, iterC3, iterC4, iterB2RE, maxIterUsed, maxIter, sumIter;
 
      int integrationSteps, integrationStepswithChange, refusedSteps, refusedStepsWithImpact;
      
      int JacUpdateT1, JacUpdateT2, JacUpdateT3;
 
      int wrongAlertGapControl, stepsOkAfterGapControl, stepsRefusedAfterGapControl, statusGapControl;
      
      int singleStepsT1, singleStepsT2, singleStepsT3;
 
      double dtRelGapControl, qUncertaintyByExtrapolation;
      int indexLSException;
      fmatvec::Vec gUniActive;
      double Penetration, PenetrationCounter, PenetrationLog, PenetrationMin, PenetrationMax;
      double maxdtUsed, mindtUsed;
      bool ChangeByGapControl;
      bool calcBlock2;
      bool IterConvergence;
      bool ConstraintsChanged, ConstraintsChangedBlock1, ConstraintsChangedBlock2, ConstraintsChanged_B1_A;
      int integrationStepsOrder1, integrationStepsOrder2;
      int order;
      int StepTrials;
      int AnzahlAktiverKontakte;
      double gNDurchschnittprostep;
 
      int iter_T1, iter_T2, iter_T3, step;
      
      fmatvec::RangeV Iq, Iu, Ix;
      
      bool expInt;
 
      int it_T1, it_T2, it_T3;
      
      bool gAC_T1, gAC_T2, gAC_T3, gAC_reg_T1, gAC_reg_T2, gAC_reg_T3;
      
      bool upgedated_T1, upgedated_T2, upgedated_T3;
 
      bool JacUpdate_B1_T1, JacUpdate_B2_T1, JacUpdate_B1_T2, JacUpdate_B2_T2, JacUpdate_T1, JacUpdate_T2;
      
      int itMax;
      
      double itTol;
      
      fmatvec::Mat dhdq_n_T1, dhdu_n_T1, dhdq_n_T2, dhdu_n_T2, dhdq_n_T3, dhdu_n_T3, dhdq_T1, dhdu_T1, dhdq_T2, dhdu_T2, dhdq_T3, dhdu_T3;
      fmatvec::Mat dhdq_z1d, dhdu_z1d, dhdq_z2b, dhdq_z3b, dhdq_z4b, dhdq_z6b, dhdu_z2b, dhdu_z3b, dhdu_z4b, dhdu_z6b, dhdq_end, dhdu_end, dhdq_z2d, dhdu_z2d, dhdq_z2dRE, dhdu_z2dRE, dhdq_z3d, dhdu_z3d, dhdq_z4d, dhdu_z4d, dhdq_z6d, dhdu_z6d;
      
      double theta;
 
      bool parJac;
      bool parInt;
 
      std::vector<MBSim::DynamicSystemSolver*> *psystems;
 
      bool inexactJac;
 
      int maxImpIter;
      int JacConstSteps;
 
      bool SetValuedForceLawsExplicit;
 
      bool plotParallel;
 
      // TODO delme
      fmatvec::Mat JacVgl, JacVgl2, JacVgl3;
 
      int JacCounter;
 
    public:
      
      AutoTimeSteppingSSCIntegrator();
      
      ~AutoTimeSteppingSSCIntegrator();
      
      void setInitialStepSize(double dt_) {dt = dt_;}
      
      void setStepSizeMax(double dtMax_) {dtMax = dtMax_;}
      
      void setStepSizeMin(double dtMin_) {dtMin = dtMin_;}
      
      void setmaxGainSSC(double maxGain) {maxGainSSC = maxGain;}
      
      void setSafetyFactorSSC(double sfactor) {safetyFactorSSC=sfactor;}
      void setSafetyFactorGapControl(double s){safetyFactorGapControl=s;}
      
      void setOutputInterpolation(bool flag=true) {outputInterpolation = flag;}
      
      void setFlagPlotIntegrator(bool flag=true) {FlagPlotIntegrator = flag;}
      
      void setGapControl(int strategy=1) {FlagGapControl=(strategy>=0); GapControlStrategy=(strategy<0)?0:strategy; }
      
      void setDriftCompensation(bool dc) {driftCompensation = dc;}
      
      void setMaxOrder(int order_, int method_=0);
      
      /* deactivate step size control */
      void deactivateSSC(bool flag=false) {FlagSSC=flag;}
      
      void setFlagErrorTest(int Flag, bool alwaysValid=true);
 
      void integrate(MBSim::DynamicSystemSolver& system_);
      void integrate(MBSim::DynamicSystemSolver& system_, std::vector<MBSim::DynamicSystemSolver*> systems);
 
      void integrate(MBSim::DynamicSystemSolver& systemT1_, MBSim::DynamicSystemSolver& systemT2_, MBSim::DynamicSystemSolver& systemT3_, MBSim::DynamicSystemSolver& systemTP_, int Threads=0);
      void integrate(MBSim::DynamicSystemSolver& systemT1_, MBSim::DynamicSystemSolver& systemT2_, MBSim::DynamicSystemSolver& systemT3_, MBSim::DynamicSystemSolver& systemTP_, std::vector<MBSim::DynamicSystemSolver*> systems, int Threads=0);
      
      void setAbsoluteTolerance(const fmatvec::Vec &aTol_) {aTol <<= aTol_;}
      void setAbsoluteTolerance(double aTol_) {aTol.resize(1,fmatvec::INIT,aTol_);}
      void setRelativeTolerance(const fmatvec::Vec &rTol_) {rTol <<= rTol_;}
      void setRelativeTolerance(double rTol_) {rTol.resize(1,fmatvec::INIT,rTol_);}
      void setgapTolerance(double gTol) {gapTol = gTol;}
 
      void preIntegrate(MBSim::DynamicSystemSolver& system);
      void subIntegrate(MBSim::DynamicSystemSolver& system, double tStop);
      void postIntegrate(MBSim::DynamicSystemSolver& system);
      void preIntegrate(MBSim::DynamicSystemSolver& systemT1_, MBSim::DynamicSystemSolver& systemT2_, MBSim::DynamicSystemSolver& systemT3_, MBSim::DynamicSystemSolver& systemTP_);
       
      void getAllSetValuedla(fmatvec::Vec& la_,fmatvec::VecInt& la_Sizes,std::vector<MBSim::Link*> &SetValuedLinkList);
      void setAllSetValuedla(const fmatvec::Vec& la_,const fmatvec::VecInt& la_Sizes,std::vector<MBSim::Link*> &SetValuedLinkList);
      void getDataForGapControl();
      bool testTolerances();
      bool GapControl(double qUnsafe, bool SSCTestOK); 
      bool changedLinkStatus(const fmatvec::VecInt &L1, const fmatvec::VecInt &L2, int ex);
      double calculatedtNewRel(const fmatvec::Vec &ErrorLocal, double H);
      void plot();
      void plotPar();
      void doIntegPlot();
 
      virtual void initializeUsingXML(xercesc::DOMElement *element);
      
      void update(MBSim::DynamicSystemSolver& system, const fmatvec::Vec& z, double t, int nrSys_=1);
      
      void doStep(MBSim::DynamicSystemSolver& system_, fmatvec::Vec& z_, int nrSys_, double t_, double dt_, bool exp_);
      
      void doExpStep(MBSim::DynamicSystemSolver& system_, fmatvec::Vec& z_, int nrSys_, double t_, double dt_);
      
      void doImpStep(MBSim::DynamicSystemSolver& system_, fmatvec::Vec& z_, int nrSys_, double t_, double dt_);
     
      void doLinImpStep(MBSim::DynamicSystemSolver& system_, fmatvec::Vec& z_, int nrSys_, double t_, double dt_);
 
      void setTheta(double theta_ ) { theta  = theta_; }
 
      void getdhdqdhdu(fmatvec::Mat& dhdq_, fmatvec::Mat& dhdu_, const fmatvec::Vec z_, const double t_, const int nsys_);
 
      void setItMax(int itMax_) {itMax = itMax_;}
      
      void setInexactJac(bool inexactJac_) {inexactJac = inexactJac_;}
      
      void setItTol(double itTol_) {itTol = itTol_;}
 
      void setSetValuedForceLawsExplicit(bool SetValuedForceLawsExplicit_) {SetValuedForceLawsExplicit = SetValuedForceLawsExplicit_;}
 
      void setPlotParallel(bool plotParallel_) {plotParallel = plotParallel_;}
  };
 
}
 
#endif