link.h

/* Copyright (C) 2004-2014 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 _LINK_H_
#define _LINK_H_
 
#include "mbsim/element.h"
#include "mbsim/mbsim_event.h"
 
namespace H5 {
  class Group;
}
 
namespace MBSim {
 
  extern const PlotFeatureEnum generalizedRelativePosition, generalizedRelativeVelocity, generalizedForce;
 
  //class Link : public Element, public LinkInterface, public ExtraDynamicInterface {
  class Link : public Element {
    public:
      Link(const std::string &name);
 
      virtual void calcSize() { }
      int getGeneralizedRelativePositionSize() { if(updSize) calcSize(); return ng; }
      int getGeneralizedRelativeVelocitySize() { if(updSize) calcSize(); return ngd; }
      int getGeneralizedForceSize() { if(updSize) calcSize(); return nla; }
 
      virtual void updateg() { }
      virtual void updategd() { }
      virtual void updatewb() { }
      virtual void updateW(int i=0) { }
      virtual void updateV(int i=0) { }
      virtual void updater(int i=0) { }
      virtual void updateJrla(int i=0) { }
      virtual void updateh(int i=0) { }
      virtual void updateStopVector() { }
      virtual void updateStopVectorParameters() { }
      virtual void updateLinkStatus() { }
      virtual void updateLinkStatusReg() { }
      virtual void updateJacobians(int j=0) { }
      virtual void updateb() { }
 
      virtual void updatedx();
      virtual void updatexd() { }
      virtual void calcxSize() { xSize = 0; }
      const fmatvec::Vec& getx() const { return x; }
      fmatvec::Vec& getx() { return x; }
      const fmatvec::Vec& getxd(bool check=true) const;
      fmatvec::Vec& getxd(bool check=true);
      int getxInd() { return xInd; }
      virtual void setxInd(int xInd_) { xInd = xInd_; };
      int getxSize() const { return xSize; }
      virtual void updatexRef(fmatvec::Vec& ref);
      virtual void updatexdRef(fmatvec::Vec& ref);
      virtual void updatedxRef(fmatvec::Vec& ref);
      virtual void updatebRef(fmatvec::Mat &hRef);
      void init(InitStage stage, const InitConfigSet &config) override;
      virtual void initz();
      virtual void writez(H5::GroupBase *group);
      virtual void readz0(H5::GroupBase *group);
 
      virtual void setbInd(int bInd_) { bInd = bInd_; };
 
      /* INHERITED INTERFACE OF ELEMENT */
      void plot() override;
      /***************************************************/
 
      /* INTERFACE TO BE DEFINED IN DERIVED CLASS */
      virtual void updateWRef(fmatvec::Mat& ref, int i=0) = 0;
 
      virtual void updateVRef(fmatvec::Mat& ref, int i=0) = 0;
 
      virtual void updatehRef(fmatvec::Vec &hRef, int i=0) = 0;
 
      virtual void updaterRef(fmatvec::Vec &ref, int i=0) = 0;
 
      virtual void updateJrlaRef(fmatvec::Mat &ref, int i=0) {}
 
      virtual void updatewbRef(fmatvec::Vec &ref);
 
      virtual void updatelaRef(fmatvec::Vec& ref);
 
      virtual void updateLaRef(fmatvec::Vec& ref);
 
      virtual void deletelaRef();
 
      virtual void updateInternalStateRef(fmatvec::Vec& cur, fmatvec::Vec& next);
 
      virtual void updategRef(fmatvec::Vec& ref);
 
      virtual void updategdRef(fmatvec::Vec& ref);
 
      virtual void updateresRef(fmatvec::Vec& ref);
 
      virtual void updaterFactorRef(fmatvec::Vec& ref);
 
      virtual void updatesvRef(fmatvec::Vec &sv);
 
      virtual void updatejsvRef(fmatvec::VecInt &jsvParent);
 
       virtual void updateLinkStatusRef(fmatvec::VecInt &LinkStatusParent);
 
       virtual void updateLinkStatusRegRef(fmatvec::VecInt &LinkStatusRegParent);
 
      virtual void calclaSize(int j) { laSize = 0; }
 
      int getisInd() { return isInd; }
      int getisSize() { return isSize; }
      virtual void calcisSize() { isSize = 0; }
 
      virtual void aboutToUpdateInternalState() {}
 
      virtual void postprocessing() {}
 
      virtual void calcgSize(int j) { gSize = 0; }
 
      virtual void calcgdSize(int j) { gdSize = 0; }
 
      virtual void calcrFactorSize(int j) { rFactorSize = 0; }
 
      virtual void calcbSize() { bSize = 0; }
 
      virtual void calcsvSize() { svSize = 0; }
      
      virtual void calcLinkStatusSize() { LinkStatusSize =0;}
 
      virtual void calcLinkStatusRegSize() { LinkStatusRegSize =0;}
 
      virtual bool isSetValued() const { return false; }
 
      virtual bool isSingleValued() const { return false; }
 
      virtual bool isActive() const = 0;
 
      virtual bool gActiveChanged() = 0;
      
      virtual bool detectImpact() { return false; }
 
      virtual void solveImpactsFixpointSingle() { throwError("(Link::solveImpactsFixpointSingle): Not implemented."); }
 
      virtual void solveConstraintsFixpointSingle() { throwError("(Link::solveConstraintsFixpointSingle): Not implemented."); }
 
      virtual void solveImpactsGaussSeidel() { throwError("(Link::solveImpactsGaussSeidel): Not implemented."); }
 
      virtual void solveConstraintsGaussSeidel() { throwError("(Link::solveConstraintsGaussSeidel): Not implemented."); }
 
      virtual void solveImpactsRootFinding() { throwError("(Link::solveImpactsRootFinding): Not implemented."); }
 
      virtual void solveConstraintsRootFinding() { throwError("(Link::solveConstraintsRootFinding): Not implemented."); }
 
      virtual void jacobianConstraints() { throwError("(Link::jacobianConstraints): Not implemented."); }
 
      virtual void jacobianImpacts() { throwError("(Link::jacobianImpacts): Not implemented."); }
 
      virtual void updaterFactors() { throwError("(Link::updaterFactors): Not implemented."); }
 
      virtual void checkImpactsForTermination() { throwError("(Link::checkImpactsForTermination): Not implemented."); }
      
      virtual void checkConstraintsForTermination() { throwError("(Link::checkConstraintsForTermination): Not implemented."); }
 
      virtual void checkActive(int j) { }
 
      virtual void setGeneralizedForceTolerance(double tol) { laTol = tol; }
      virtual void setGeneralizedImpulseTolerance(double tol) { LaTol = tol; }
      virtual void setGeneralizedRelativePositionTolerance(double tol) { gTol = tol; }
      virtual void setGeneralizedRelativeVelocityTolerance(double tol) { gdTol = tol; }
      virtual void setGeneralizedRelativeAccelerationTolerance(double tol) { gddTol = tol; }
      virtual void setGeneralizedRelativePositionCorrectionValue(double corr) { gCorr = corr; }
      virtual void setGeneralizedRelativeVelocityCorrectionValue(double corr) { gdCorr = corr; }
      virtual void setrMax(double rMax_) { rMax = rMax_; }
      virtual void setLinkStatusInd(int LinkStatusInd_) { LinkStatusInd = LinkStatusInd_; };
      virtual void setLinkStatusRegInd(int LinkStatusRegInd_) { LinkStatusRegInd = LinkStatusRegInd_; };
      virtual void setlaInd(int laInd_) { laInd = laInd_;Ila=fmatvec::RangeV(laInd,laInd+laSize-1); }
      virtual void setisInd(int isInd_) { isInd = isInd_; }
      virtual void setgInd(int gInd_) { gInd = gInd_; Ig=fmatvec::RangeV(gInd,gInd+gSize-1); }
      virtual void setgdInd(int gdInd_) { gdInd = gdInd_; }
      virtual void setrFactorInd(int rFactorInd_) { rFactorInd = rFactorInd_; }
      virtual void LinearImpactEstimation(double t, fmatvec::Vec &gInActive_,fmatvec::Vec &gdInActive_,int *IndInActive_,fmatvec::Vec &gAct_,int *IndActive_) { }
      
      virtual void SizeLinearImpactEstimation(int *sizeInActive_, int *sizeActive_) { }
      virtual void updatecorr(int j) { corr.init(0); }
      virtual void updatecorrRef(fmatvec::Vec &ref);
      virtual void calccorrSize(int j) { corrSize = 0; }
      virtual void setcorrInd(int corrInd_) { corrInd = corrInd_; }
      virtual void checkRoot() { }
      /***************************************************/
 
      void setx(const fmatvec::Vec &x_) { x = x_; }
      
      virtual void setsvInd(int svInd_) { svInd = svInd_; };
      int getsvSize() const { return svSize; }
 
      int getLinkStatusSize() const { return LinkStatusSize; }
 
      int getLinkStatusRegSize() const { return LinkStatusRegSize; }
 
      const fmatvec::Vec& getla(bool check=true) const;
      const fmatvec::Vec& getLa(bool check=true) const;
      fmatvec::Vec& getla(bool check=true);
      fmatvec::Vec& getLa(bool check=true);
      fmatvec::Vec& getwb(bool check=true);
 
      int getlaInd() const { return laInd; } 
      int getlaSize() const { return laSize; } 
      int getbSize() const { return bSize; }
 
      const fmatvec::Vec& evalg();
      const fmatvec::Vec& evalgd();
      const fmatvec::Vec& evalla();
      const fmatvec::Vec& evalLa();
      const fmatvec::Vec& evalwb();
      const fmatvec::Vec& evalxd();
 
      int getgdInd() const { return gdInd; } 
      int getgSize() const { return gSize; } 
      int getgdSize() const { return gdSize; } 
 
      int getrFactorSize() const { return rFactorSize; } 
      
      const fmatvec::VecInt& getrFactorUnsure() const { return rFactorUnsure; }
 
      void resetUpToDate() override { updrrel = true; updvrel = true; updla = true; }
 
      virtual void updateGeneralizedPositions() { updrrel = false; }
      virtual void updateGeneralizedVelocities() { updvrel = false; }
      virtual void updateGeneralizedForces() { updla = false; }
 
      const fmatvec::VecV& evalGeneralizedRelativePosition() { if(updrrel) updateGeneralizedPositions(); return rrel; }
      const fmatvec::VecV& evalGeneralizedRelativeVelocity() { if(updvrel) updateGeneralizedVelocities(); return vrel; }
      const fmatvec::VecV& evalGeneralizedForce() { if(updla) updateGeneralizedForces(); return lambda; }
 
      fmatvec::VecV& getGeneralizedRelativePosition(bool check=true) {  assert((not check) or (not updrrel)); return rrel; }
      fmatvec::VecV& getGeneralizedRelativeVelocity(bool check=true) {  assert((not check) or (not updvrel)); return vrel; }
      fmatvec::VecV& getGeneralizedForce(bool check=true) {  assert((not check) or (not updla)); return lambda; }
 
      void savela();
 
      void saveLa();
 
      void initla();
 
      void initLa();
 
      void decreaserFactors();
 
      
      int getcorrSize() const { return corrSize; } 
 
      bool getUpdaterrel() const { return updrrel; }
 
      void createPlotGroup() override;
 
    protected:
      int ng, ngd, nla;
 
      fmatvec::Vec x;
 
      fmatvec::Vec xd;
 
      fmatvec::Vec dx;
 
      fmatvec::Vec x0;
 
      int xSize, xInd;
 
      fmatvec::Vec sv;
 
      fmatvec::VecInt jsv;
 
      int svSize, svInd;
             
      fmatvec::VecInt LinkStatus;
 
       int LinkStatusSize, LinkStatusInd;
    
      fmatvec::VecInt LinkStatusReg;
 
       int LinkStatusRegSize, LinkStatusRegInd; 
 
      fmatvec::Vec curis, nextis;
 
      fmatvec::Vec g, gd, la, La;
      
      fmatvec::Vec laS;
 
      int isSize, isInd;
 
      int gSize, gInd;
 
      int gdSize, gdInd;
 
      int laSize, laInd;
 
      int bSize, bInd;
 
      fmatvec::RangeV Ig, Ila;
      
      double gTol, gdTol, gddTol, laTol, LaTol;
 
      double gCorr, gdCorr;
      
      fmatvec::Vec la0, La0;
 
      fmatvec::Vec rFactor;
 
      fmatvec::VecInt rFactorUnsure;
 
      int rFactorSize, rFactorInd;
      
      double rMax;
 
      fmatvec::Vec res;
 
      std::vector<fmatvec::Mat> W[2];
 
      std::vector<fmatvec::Mat> V[2];
      
      std::vector<fmatvec::Vec> h[2];
      
      std::vector<fmatvec::Mat> dhdq;
      std::vector<fmatvec::Mat> dhdu;
      std::vector<fmatvec::Vec> dhdt;
      
      std::vector<fmatvec::Vec> r[2];
      
      std::vector<fmatvec::Mat> Jrla[2];
      
      fmatvec::Vec wb;
 
      fmatvec::Mat b;
 
      int corrSize, corrInd;
      fmatvec::Vec corr;
 
      fmatvec::VecV rrel, vrel, lambda;
 
      bool updSize, updrrel, updvrel, updla;
  };
}
 
#endif /* _LINK_H_ */