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: martin.o.foerg@googlemail.com
 */
 
#ifndef _TIME_STEPPING_SSC_INTEGRATOR_H_ 
#define _TIME_STEPPING_SSC_INTEGRATOR_H_
 
#include "integrator.h"
#include "mbsim/utils/stopwatch.h"
 
namespace MBSim {
  class Link;
}
 
namespace MBSim {
 
  class TimeSteppingSSCIntegrator : public Integrator { 
 
    public:
      enum Method {
        extrapolation=0,
        embedded,
        embeddedHigherOrder,
        unknownMethod
      };
 
      enum GapControl {
        noGapControl=0,
        biggestRoot,
        scoring,
        gapTolerance,
        smallestRoot,
        unknownGapControl
      };
 
      enum ErrorTest {
        all=0,
        scale=2,
        exclude=3,
        unknownErrorTest
      };
 
    protected:
      void resize(MBSim::DynamicSystemSolver *system);
 
      MBSim::DynamicSystemSolver* sysT1{NULL};
      MBSim::DynamicSystemSolver* sysT2{NULL};
      MBSim::DynamicSystemSolver* sysT3{NULL};
 
      double dt{1e-6}, dtOld{1e-6}, dte{1e-6};
      double dtMin{0}, dtMax{1e-3};
      double dt_SSC_vorGapControl{0};
      bool driftCompensation{false};
      double t{0}, tPlot{0};
      int qSize{0}, xSize{0}, uSize{0}, zSize{0};
      fmatvec::Vec ze, zi, z1d, z2d, z2dRE, z3d, z4d, z6d, z2b, z3b, z4b, z6b, zStern;
      fmatvec::VecInt LS, LSe, LStmp_T1, LStmp_T2, LStmp_T3, LSA, LSB1, LSB2, LSC1, LSC2, LSC3, LSC4, LSD1, LSD2, LSD3, LSD4, LSD5, LSD6;
      fmatvec::Vec la, lae, la1d, la2b;
      fmatvec::VecInt laSizes, laeSizes, la1dSizes, la2bSizes;
      fmatvec::Vec gInActive, gdInActive;
      std::vector<MBSim::Link*> SetValuedLinkListT1;
      std::vector<MBSim::Link*> SetValuedLinkListT2;
      std::vector<MBSim::Link*> SetValuedLinkListT3;
 
      int StepsWithUnchangedConstraints{-1};
 
      int FlagErrorTest{2};
      bool FlagErrorTestAlwaysValid{true};
      fmatvec::Vec aTol;
      fmatvec::Vec rTol;
      bool FlagSSC{true};
      int maxOrder{1};
      Method method{extrapolation};
      /* Flag for Gap Control */
      bool FlagGapControl{false};
      double gapTol{1e-6};
      double maxGainSSC{2.2};
      double safetyFactorSSC{0.7};
      bool FlagOutputOnlyAtTPlot{false};
      bool FlagPlotEveryStep{false};
      bool outputInterpolation{false};
      double safetyFactorGapControl{-1};
      GapControl GapControlStrategy{biggestRoot};
      int numThreads{1};
      double time{0};
      MBSim::StopWatch Timer;
      int iter{0}, iterA{0}, iterB1{0}, iterB2{0}, iterC1{0}, iterC2{0}, iterC3{0}, iterC4{0}, iterB2RE{0}, maxIterUsed{0}, maxIter{0}, sumIter{0};
      int integrationSteps{0}, integrationStepswithChange{0}, refusedSteps{0}, refusedStepsWithImpact{0};
      int wrongAlertGapControl{0}, stepsOkAfterGapControl{0}, stepsRefusedAfterGapControl{0}, statusGapControl{0};
      int singleStepsT1{0}, singleStepsT2{0}, singleStepsT3{0};
      double dtRelGapControl{1}, qUncertaintyByExtrapolation{0};
      int indexLSException{-1};
      fmatvec::Vec gUniActive;
      double Penetration{0}, PenetrationCounter{0}, PenetrationLog{0}, PenetrationMin{0}, PenetrationMax{0};
      double maxdtUsed{0}, mindtUsed{0};
      bool ChangeByGapControl{false};
      bool calcBlock2{0};
      bool IterConvergence{0};
      bool ConstraintsChanged{0}, ConstraintsChangedBlock1{0}, ConstraintsChangedBlock2{0};
      int integrationStepsOrder1{0};
      int integrationStepsOrder2{0};
      int order{1};
      int StepTrials{0};
      int AnzahlAktiverKontakte{0};
      double gNDurchschnittprostep{0};
 
      fmatvec::Vec bi;
 
    public:
      ~TimeSteppingSSCIntegrator() override;
      void setInitialStepSize(double dt_) { dt = dt_; }
      void setMaximumStepSize(double dtMax_) { dtMax = dtMax_; }
      void setMinimumStepSize(double dtMin_) { dtMin = dtMin_; }
      void setMaximumGain(double maxGain) { maxGainSSC = maxGain; }
      void setSafetyFactor(double sfactor) { safetyFactorSSC=sfactor; }
      void setSafetyFactorForGapControl(double s) { safetyFactorGapControl = s; }
      void setOutputInterpolation(bool flag) { outputInterpolation = flag; }
      void setFlagOutputOnlyAtTPlot(bool flag) { FlagOutputOnlyAtTPlot = flag; }
      void setGapControl(GapControl gapControl) { GapControlStrategy = gapControl; }
      void setDriftCompensation(bool dc) { driftCompensation = dc; }
      void setMaximumOrder(int maxOrder_) { maxOrder = maxOrder_; }
      void setMethod(Method method_) { method = method_; }
      /* deactivate step size control */
      void setStepSizeControl(bool flag) { FlagSSC = flag; }
      void setErrorTest(ErrorTest errorTest) { FlagErrorTest = errorTest; }
      void setAlwaysValid(bool alwaysValid) { FlagErrorTestAlwaysValid = alwaysValid; }
 
      using Integrator::integrate;
      void integrate() override;
      void integrate(MBSim::DynamicSystemSolver& systemT1_, MBSim::DynamicSystemSolver& systemT2_, MBSim::DynamicSystemSolver& systemT3_, 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() override;
      void subIntegrate(double tStop) override;
      void postIntegrate() override;
      void preIntegrate(MBSim::DynamicSystemSolver& systemT1_, MBSim::DynamicSystemSolver& systemT2_, MBSim::DynamicSystemSolver& systemT3_);
       
      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();
 
      virtual void initializeUsingXML(xercesc::DOMElement *element) override;
 
      void updatebi();
      void updatela();
      void updatezd();
  };
 
}
 
#endif