MBSim

Project Page
auto_time_stepping_ssc_integrator.h
00001 /* Copyright (C) 2004-2009  MBSim Development Team
00002 
00003  * This library is free software; you can redistribute it and/or 
00004  * modify it under the terms of the GNU Lesser General Public 
00005  * License as published by the Free Software Foundation; either 
00006  * version 2.1 of the License, or (at your option) any later version. 
00007  *  
00008  * This library is distributed in the hope that it will be useful, 
00009  * but WITHOUT ANY WARRANTY; without even the implied warranty of 
00010  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 
00011  * Lesser General Public License for more details. 
00012  *  
00013  * You should have received a copy of the GNU Lesser General Public 
00014  * License along with this library; if not, write to the Free Software 
00015  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
00016  *
00017  * Contact:
00018  *   jan.p.clauberg@googlemail.com
00019  *   huber@amm.mw.tum.de
00020  */
00021 
00022 #ifndef _AUTO_TIME_STEPPING_SSC_INTEGRATOR_H_ 
00023 #define _AUTO_TIME_STEPPING_SSC_INTEGRATOR_H_
00024 
00025 #include "integrator.h"
00026 #include "mbsim/utils/stopwatch.h"
00027 
00028 namespace MBSim {
00029   class Link;
00030 }
00031 
00032 namespace MBSimIntegrator {
00033 
00093   class AutoTimeSteppingSSCIntegrator : public Integrator { 
00094 
00095     protected:
00096       MBSim::DynamicSystemSolver* sysT1;
00097       MBSim::DynamicSystemSolver* sysT2;
00098       MBSim::DynamicSystemSolver* sysT3;
00099       MBSim::DynamicSystemSolver* sysTP;
00100 
00101       double dt, dtOld, dte;
00102       double dtMin, dtMax;
00103       double dt_SSC_vorGapControl;
00104       bool driftCompensation;
00105       double t, tPlot, tPlotP;
00106       int qSize, xSize, uSize, zSize;
00107       fmatvec::Vec ze, zeP, zi, ziP, zT1, zT2, zT3, zTP, z1d, z2d, z2dRE, z3d, z4d, z6d, z2b, z3b, z4b, z6b, zStern;
00108       
00109       fmatvec::VecInt LS, LSe, LSA, LSB1, LSB2, LSC1, LSC2, LSC3, LSC4, LSD1, LSD2, LSD3, LSD4, LSD5, LSD6;
00110       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;
00111       fmatvec::VecInt LS_Reg_T1, LS_Reg_T2, LS_Reg_T3;
00112       fmatvec::VecInt LS_Reg_z2b, S_Reg_z1d, LS_Reg_z4b, LS_Reg_z6b, LS_Reg_z1d, LS_Reg_z3b, LSB1_2_Reg;
00113       fmatvec::VecInt LS_tmp;
00114 
00115       fmatvec::Vec la, laP, lae, laeP, la1d, la2b;
00116       fmatvec::VecInt laSizes, laSizesP, laeSizes, laeSizesP, la1dSizes, la2bSizes;
00117       fmatvec::Vec qT1, qT2, qT3, qTP;
00118       fmatvec::Vec uT1, uT2, uT3, uTP;
00119       fmatvec::Vec xT1, xT2, xT3, xTP;
00120       fmatvec::Vec gInActive, gdInActive;
00121       std::vector<MBSim::Link*> SetValuedLinkListT1;
00122       std::vector<MBSim::Link*> SetValuedLinkListT2;
00123       std::vector<MBSim::Link*> SetValuedLinkListT3;
00124       std::vector<MBSim::Link*> SetValuedLinkListTP;
00125 
00126       int StepsWithUnchangedConstraints;
00127 
00129       int FlagErrorTest;
00130       
00132       bool FlagErrorTestAlwaysValid;
00133       
00135       fmatvec::Vec aTol;
00136       
00138       fmatvec::Vec rTol;
00139       
00141       bool FlagSSC;
00142       
00146       int maxOrder;
00147       
00152       int method;
00153       
00154       /* Flag for Gap Control - Gap Control not available for implicit integration*/
00155       bool FlagGapControl;
00156       
00158       double gapTol;
00159       
00161       double maxGainSSC;
00162       
00164       double safetyFactorSSC; 
00165       
00167       std::ofstream integPlot;
00168       
00170       bool FlagPlotIntegrator;
00171       
00173       bool FlagPlotIntegrationSum;
00174       
00176       bool FlagCoutInfo;
00177       
00179       bool FlagPlotEveryStep;
00180       
00182       bool outputInterpolation;
00183       
00185       double safetyFactorGapControl;
00186       
00194       int GapControlStrategy;
00195       
00197       int numThreads, numThreadsB1;
00198       
00200       double time, dhdztime, timeB1, timeB2, timePlot, timePlotPar;
00201       
00203       MBSim::StopWatch Timer, dhdzTimer, TimerB1, TimerB2, TimerPlot, TimerPlotPar;
00204 
00206       int iter, iterA, iterB1, iterB2, iterC1, iterC2, iterC3, iterC4, iterB2RE, maxIterUsed, maxIter, sumIter;
00207 
00209       int integrationSteps, integrationStepswithChange, refusedSteps, refusedStepsWithImpact;
00210       
00212       int JacUpdateT1, JacUpdateT2, JacUpdateT3;
00213 
00215       int wrongAlertGapControl, stepsOkAfterGapControl, stepsRefusedAfterGapControl, statusGapControl;
00216       
00218       int singleStepsT1, singleStepsT2, singleStepsT3;
00219 
00221       double dtRelGapControl, qUncertaintyByExtrapolation;
00222       int indexLSException;
00223       fmatvec::Vec gUniActive;
00224       double Penetration, PenetrationCounter, PenetrationLog, PenetrationMin, PenetrationMax;
00225       double maxdtUsed, mindtUsed;
00226       bool ChangeByGapControl;
00227       bool calcBlock2;
00228       bool IterConvergence;
00229       bool ConstraintsChanged, ConstraintsChangedBlock1, ConstraintsChangedBlock2, ConstraintsChanged_B1_A;
00230       int integrationStepsOrder1, integrationStepsOrder2;
00231       int order;
00232       int StepTrials;
00233       int AnzahlAktiverKontakte;
00234       double gNDurchschnittprostep;
00235 
00237       int iter_T1, iter_T2, iter_T3, step;
00238       
00239       fmatvec::Index Iq, Iu, Ix;
00240       
00242       bool expInt;
00243 
00245       int it_T1, it_T2, it_T3;
00246       
00248       bool gAC_T1, gAC_T2, gAC_T3, gAC_reg_T1, gAC_reg_T2, gAC_reg_T3;
00249       
00251       bool upgedated_T1, upgedated_T2, upgedated_T3;
00252 
00254       bool JacUpdate_B1_T1, JacUpdate_B2_T1, JacUpdate_B1_T2, JacUpdate_B2_T2, JacUpdate_T1, JacUpdate_T2;
00255       
00257       int itMax;
00258       
00260       double itTol;
00261       
00263       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;
00264       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;
00265       
00267       double theta;
00268 
00269       bool parJac;
00270       bool parInt;
00271 
00272       std::vector<MBSim::DynamicSystemSolver*> *psystems;
00273 
00275       bool inexactJac;
00276 
00277       int maxImpIter;
00278       int JacConstSteps;
00279 
00281       bool SetValuedForceLawsExplicit;
00282 
00284       bool debugOutput;
00285 
00287       bool plotParallel;
00288 
00289       // TODO delme
00290       fmatvec::Mat JacVgl, JacVgl2, JacVgl3;
00291 
00293       int JacCounter;
00294 
00295     public:
00296       
00298       AutoTimeSteppingSSCIntegrator();
00299       
00301       ~AutoTimeSteppingSSCIntegrator();
00302       
00304       void setInitialStepSize(double dt_) {dt = dt_;}
00305       
00307       void setStepSizeMax(double dtMax_) {dtMax = dtMax_;}
00308       
00310       void setStepSizeMin(double dtMin_) {dtMin = dtMin_;}
00311       
00313       void setmaxGainSSC(double maxGain) {maxGainSSC = maxGain;}
00314       
00316       void setSafetyFactorSSC(double sfactor) {safetyFactorSSC=sfactor;}
00317       void setSafetyFactorGapControl(double s){safetyFactorGapControl=s;}
00318       
00320       void setOutputInterpolation(bool flag=true) {outputInterpolation = flag;}
00321       
00323       void setFlagPlotIntegrator(bool flag=true) {FlagPlotIntegrator = flag;}
00324       
00334       void setGapControl(int strategy=1) {FlagGapControl=(strategy>=0); GapControlStrategy=(strategy<0)?0:strategy; }
00335       
00337       void setDriftCompensation(bool dc) {driftCompensation = dc;}
00338       
00343       void setMaxOrder(int order_, int method_=0);
00344       
00345       /* deactivate step size control */
00346       void deactivateSSC(bool flag=false) {FlagSSC=flag;}
00347       
00352       void setFlagErrorTest(int Flag, bool alwaysValid=true);
00353 
00355       void integrate(MBSim::DynamicSystemSolver& system_);
00356       void integrate(MBSim::DynamicSystemSolver& system_, std::vector<MBSim::DynamicSystemSolver*> systems);
00357 
00358       void integrate(MBSim::DynamicSystemSolver& systemT1_, MBSim::DynamicSystemSolver& systemT2_, MBSim::DynamicSystemSolver& systemT3_, MBSim::DynamicSystemSolver& systemTP_, int Threads=0);
00359       void integrate(MBSim::DynamicSystemSolver& systemT1_, MBSim::DynamicSystemSolver& systemT2_, MBSim::DynamicSystemSolver& systemT3_, MBSim::DynamicSystemSolver& systemTP_, std::vector<MBSim::DynamicSystemSolver*> systems, int Threads=0);
00360       
00362       void setAbsoluteTolerance(const fmatvec::Vec &aTol_) {aTol.resize() = aTol_;}
00363       void setAbsoluteTolerance(double aTol_) {aTol.resize() = fmatvec::Vec(1,fmatvec::INIT,aTol_);}
00364       void setRelativeTolerance(const fmatvec::Vec &rTol_) {rTol.resize() = rTol_;}
00365       void setRelativeTolerance(double rTol_) {rTol.resize() = fmatvec::Vec(1,fmatvec::INIT,rTol_);}
00366       void setgapTolerance(double gTol) {gapTol = gTol;}
00367 
00369       void preIntegrate(MBSim::DynamicSystemSolver& system);
00370       void subIntegrate(MBSim::DynamicSystemSolver& system, double tStop);
00371       void postIntegrate(MBSim::DynamicSystemSolver& system);
00372       void preIntegrate(MBSim::DynamicSystemSolver& systemT1_, MBSim::DynamicSystemSolver& systemT2_, MBSim::DynamicSystemSolver& systemT3_, MBSim::DynamicSystemSolver& systemTP_);
00373        
00375       void getAllSetValuedla(fmatvec::Vec& la_,fmatvec::VecInt& la_Sizes,std::vector<MBSim::Link*> &SetValuedLinkList);
00376       void setAllSetValuedla(const fmatvec::Vec& la_,const fmatvec::VecInt& la_Sizes,std::vector<MBSim::Link*> &SetValuedLinkList);
00377       void getDataForGapControl();
00378       bool testTolerances();
00379       bool GapControl(double qUnsafe, bool SSCTestOK); 
00380       bool changedLinkStatus(const fmatvec::VecInt &L1, const fmatvec::VecInt &L2, int ex);
00381       double calculatedtNewRel(const fmatvec::Vec &ErrorLocal, double H);
00382       void plot();
00383       void plotPar();
00384       void doIntegPlot();
00385 
00386       virtual void initializeUsingXML(xercesc::DOMElement *element);
00387       
00395       void update(MBSim::DynamicSystemSolver& system, const fmatvec::Vec& z, double t, int nrSys_=1);
00396       
00397       void doStep(MBSim::DynamicSystemSolver& system_, fmatvec::Vec& z_, int nrSys_, double t_, double dt_, bool exp_);
00398       
00407       void doExpStep(MBSim::DynamicSystemSolver& system_, fmatvec::Vec& z_, int nrSys_, double t_, double dt_);
00408       
00417       void doImpStep(MBSim::DynamicSystemSolver& system_, fmatvec::Vec& z_, int nrSys_, double t_, double dt_);
00418      
00427       void doLinImpStep(MBSim::DynamicSystemSolver& system_, fmatvec::Vec& z_, int nrSys_, double t_, double dt_);
00428 
00430       void setTheta(double theta_ ) { theta  = theta_; }
00431 
00433       void getdhdqdhdu(fmatvec::Mat& dhdq_, fmatvec::Mat& dhdu_, const fmatvec::Vec z_, const double t_, const int nsys_);
00434 
00436       void setItMax(int itMax_) {itMax = itMax_;}
00437       
00439       void setInexactJac(bool inexactJac_) {inexactJac = inexactJac_;}
00440       
00442       void setItTol(double itTol_) {itTol = itTol_;}
00443 
00445       void setSetValuedForceLawsExplicit(bool SetValuedForceLawsExplicit_) {SetValuedForceLawsExplicit = SetValuedForceLawsExplicit_;}
00446 
00448       void setDebugOutput(bool debugOutput_) {debugOutput = debugOutput_;}
00449 
00451       void setPlotParallel(bool plotParallel_) {plotParallel = plotParallel_;}
00452   };
00453 
00454 }
00455 
00456 #endif