Base_Navigator.h

Go to the documentation of this file.

//     Authors:
// Anselmo Cervera, Juan J. Gomez-Cadenas and Jose Angel Hernando

#ifndef Navigator_h
#define Navigator_h 1

#include <assert.h>

#include "Kalman/Interface_IModel.h"
#include "Kalman/Interface_IModelConverter.h"
#include "Kalman/Interface_IVolumeIntersector.h"
#include "Kalman/Interface_IModelSurfaceIntersector.h"
#include "Kalman/Interface_ITrajectory.h"
#include "Kalman/Base_Setup.h"


//! This is the machine that operate Trajectories, Points and States
//! This machine can:
//!     -Fit a trajectory using a IFitter.
//!     -Propagate an state using a IPropagator (belonging to the model)
//!     -Project states or points using a IProjector. (belonging tothe model)
//! The User can set a Fitter to fit trajectories.
//! The User can set a Trajectory Model to be use by the fitter


class Navigator
{
public:
      
  
  //! default constructor
  Navigator(){m_verbosity = 0;}

  //! constructor with selection of Model in a unique volume
  Navigator(std::string Model) { std::cout << "NavigatorB constructor called with " << Model << std::endl; } //ME added warning cout
  
  //! default destructor
  virtual ~Navigator(){};


  //-------------  Model  with respect to a Volume-----------------------------

  //! add a model to the list
  void addVolumeIntersector(std::string volumeType, IVolumeIntersector* p)
    {m_volumeIntersectorMap[volumeType]=p;}  

  bool enterInVolume(const IState& iniState, 
                 const std::vector<std::string> nextVolVector, 
                 const std::string thisModelName, 
                 int sens,
                 std::string& nextVolName, 
                 ISurface& intersectedSurf,
                 HyperVector& runHV);

  bool exitFromVolume(const IState& iniState, 
                  const std::string thisVolName, 
                  const std::string thisModelName, 
                  int sens,
                  ISurface& intersectedSurf,
                  HyperVector& runHV);

  //! compute the running sens
  int computeSens(const IState& iniState, const ISurface& endSurf) const;  

  //--------------- INTERPRETATION OF STATE PARAMETERS ------------------------------

  //! interpretation of states
  const EVector directionVector(const IState& state) 
    {return model(state.modelName()).directionVector(state);}  
  const EVector positionVector(const IState& state)  
    {return model(state.modelName()).positionVector(state);}  
  const EMatrix directionMatrix(const IState& state) 
    {return model(state.modelName()).directionMatrix(state);}  
  const EMatrix positionMatrix(const IState& state) 
    {return model(state.modelName()).positionMatrix(state);}  
  const EVector momentumVector(const IState& state) 
    {return model(state.modelName()).momentumVector(state);}  
  const EMatrix momentumMatrix(const IState& state) 
    {return model(state.modelName()).momentumMatrix(state);}  
  const double momentum(const IState& state) 
    {return model(state.modelName()).momentum(state);}  
  const double momentumError2(const IState& state) 
    {return model(state.modelName()).momentumError2(state);}  
  const double charge(const IState& state) 
    {return model(state.modelName()).charge(state);}  


  //! interpretation of vertex parameters
  const EVector directionVectorVertex(const IState& state) 
    {return model(state.modelName()).directionVectorVertex(state);}  
  const EVector positionVectorVertex(const IState& state)  
    {return model(state.modelName()).positionVectorVertex(state);}  
  const EMatrix directionMatrixVertex(const IState& state) 
    {return model(state.modelName()).directionMatrixVertex(state);}  
  const EMatrix positionMatrixVertex(const IState& state) 
    {return model(state.modelName()).positionMatrixVertex(state);}  
  const EVector momentumVectorVertex(const IState& state) 
    {return model(state.modelName()).momentumVectorVertex(state);}  
  const EMatrix momentumMatrixVertex(const IState& state) 
    {return model(state.modelName()).momentumMatrixVertex(state);}  

  const EMatrix positionMomentumMatrixVertex(const IState& state) 
    {return model(state.modelName()).positionMomentumMatrixVertex(state);}  


  //-------------  MODELS ----------------------------------------------------

  //! add a model to the list
  void addModel(std::string name, IModel* p)
    { p->setVerbosity(m_verbosity);
      p->setName(name); 
      m_modelMap[name]=p;
    }  

  //! add a model to the list whith specific properties
  void addModel(std::string name, IModel* p, void* prop)
    { p->setModelProperties(prop);
      p->setVerbosity(m_verbosity);
      p->setName(name); 
      m_modelMap[name]=p;
    }  



  //! returns specific model properties 
  std::string modelProperties(std::string modelName)
    {return m_modelMap[modelName]->modelProperties();}
  //! set properties of the model in the case there is a choice
  void setModelProperties(std::string modelName, void* prop)
     {m_modelMap[modelName]->setModelProperties(prop);} 

  //! Returns the model name in a given volume
  std::string modelNameInVolume(std::string volName) {return volume(volName).modelName();}
  
  //-------  PROJECTOR, PROPAGATOR AND NOISER ARE CONTAINED IN THE MODELS

  //! add a projector to the model
  void addModelProjector(std::string modelName,std::string name, IProjector* p) 
      {m_modelMap[modelName]->addProjector(name,p);}

  //! project the state of the point 
  bool project(IPoint& point)
    {return m_modelMap[ point.modelName() ]->project(point);} 
  
  //!   Project the dynamic vector
  EVector projectionVector(const IState& state, const IMeasurement& meas)
    {return m_modelMap[state.modelName() ]->projectionVector(state, meas);}  



  //!   Project the dynamic matrix
  EMatrix projectionMatrix(const IState& state, const IMeasurement& meas)
    {return m_modelMap[ state.modelName() ]->projectionMatrix(state, meas);}  

  //!   Project the dynamicHV
  HyperVector projectionHV(const IState& state, const IMeasurement& meas)
    {return m_modelMap[ state.modelName() ]->projectionHV(state, meas);}



  //! returns the H matrix for the state
  EMatrix HMatrix(const IState& state, const IMeasurement& meas)
    {return m_modelMap[state.modelName() ]->HMatrix(state, meas);}  


  //! Returns the A matrix (dx/dr) for the Kalman vertex fit
  EMatrix AMatrix(const IState& state, const IMeasurement& meas)
    {return m_modelMap[ state.modelName() ]->AMatrix(state, meas);}

  //! Returns the B matrix (dx/dp) for the Kalman vertex fit
  EMatrix BMatrix(const IState& state, const IMeasurement& meas)
    {return m_modelMap[ state.modelName() ]->BMatrix(state, meas);}


  //! Returns the c0 vector  for the Kalman vertex fit
  EVector c0(const IState& state, const IMeasurement& meas)
    {return m_modelMap[ state.modelName() ]->c0(state, meas);}


  
  EMatrix measurementProjector(const IState& state, const IMeasurement& meas)
    {return m_modelMap[ state.modelName() ]->measurementProjector(state, meas);}

  //--------


  //! returns the propagator name
  std::string modelPropagatorName(std::string modelName) 
    {return m_modelMap[modelName]->propagatorName();}
  //! add a propagator to the model
  void addModelPropagator(std::string modelName,std::string name, IPropagator* p) 
    {m_modelMap[modelName]->addPropagator(name,p);}
  //! set the propagator to the model
  void setModelPropagator(std::string modelName,std::string name) 
    {m_modelMap[modelName]->setPropagator(name);}


  //!   If the Propagator it is linear returns the F matrix
  //!   (if not returns the 1st term of Taylor expansion)
  EMatrix FMatrix()  {return m_F;}
  
  //!   Returns the matrix of random extra errors added to the state during the
  //!   progation (null if non). This matrix is calculated by the Noiser.
  EMatrix QMatrix() {return m_Q;}
  


  //! Propagate an state to a surface
  //! It compute the dynamicHV and the runningHV in the new surface, set the values in the state
  //! It has the extra possibility of propagate an state with a different model

  bool propagate(const IState& iniState,
             const ISurface& surface, 
             IState& finalState);
  
  bool propagate(const ITrajectory& traj,
             const ISurface& surface, 
             IState& finalState);
  


  //!  Returns the dynamicHV of the state when propagate an state to a surface
  HyperVector propagateDynamicHV(const IState& ini, const ISurface& is);

  //!  Computes the running HyperVector to a given surface
  HyperVector propagateRunningHV(const IState& iniState, const ISurface& surf);

  //! Computes the path length between a state and a surface
  bool pathLength(const IState& state, const ISurface& surf, HyperDouble& hd);
  bool pathLength(const IState& state, std::string logObjectName, HyperDouble& hd);


  //! Computes the path length from the beginning to the end of a trjectory
  //! (firstPointUsed to lastPointUsed)
  //! It also compute the partial length from the firstPointUsed to the i-th point
  void computePathLength(ITrajectory& traj);  

  //! Computes the path length from the firstPointUsed to a given surface 
  bool pathLength(ITrajectory& traj, const ISurface& endSurf, HyperDouble& length);

  //! Computes the path length from the firstPointUsed to the entrance of a logical object
  bool pathLength(ITrajectory& traj, std::string logObjectName, HyperDouble& length);
 
  //! Computes the path length from the one surface to another provided a fitted trajectory
  bool pathLength(ITrajectory& traj, 
              const ISurface& iniSurf, 
              const ISurface& endSurf, 
              HyperDouble& length);  


  // computes the path lentgh between a state and a straight line, 
  // providing rl0 and ul0 such that rl = rl0 + ul0*s
  double pathLengthToStraightLine(const IState& iniS, 
                  const EVector& rl0,
                  const EVector& ul0)
    {return model(iniS.modelName()).pathLengthToStraightLine(iniS,rl0,ul0);}


  /*
  //! set propagator verbosity 
  void setPropagatorVerbosity(std::string modelName, int v)
    {m_modelMap[modelName]->setPropagatorVerbosity(v);}
  //! returns specific propagator properties
  std::string propagatorProperties(std::string modelName)
    {return m_modelMap[modelName]->propagatorProperties();}
  //! set properties of the propagator in the case there is a choice
  void setPropagatorProperties(std::string modelName, void* prop)
     {m_modelMap[modelName]->setPropagatorProperties(prop);} 
  */


  //-------------

  //! add a noiser to the model
  void addModelNoiser(std::string modelName, std::string name, INoiser* p) 
    {m_modelMap[modelName]->addNoiser(name, p);}
  //! set is/(is not) used 
  void setModelNoiserIsUsed(std::string modelName, std::string name, bool flag) 
    {m_modelMap[modelName]->setNoiserIsUsed(name,flag);}

  //--------------

  //! add a model converter
  void addModelConverter(std::string firstModel, std::string secondModel, IModelConverter* mc) {
    mc->setFirstModel(firstModel); mc->setSecondModel(secondModel); 
    m_modelConverterVector.push_back(mc);
  }
  

  //! converts state from a model to another
  bool convertModelState(std::string modelName, IState& state);

    
  //! convers a square matrix from a model to another
  EMatrix convertModelFMatrix(std::string iniModel, 
                      IState& iniS, IState& endS);



  //------------

  //! add a SurfaceIntersector to the model
  void addModelSurfaceIntersector(std::string modelName, 
                      std::string name, 
                      IModelSurfaceIntersector* p) 
    {m_modelMap[modelName]->addSurfaceIntersector(name,p);}


  //! builds a surface normal to the trajectory and computes the natural parameter 
  bool naturalHVAtNormalSurface(const IState& prevState, 
                    const HyperVector& measHV,
                    const std::string surfType,
                    ISurface& surf,
                    HyperVector& natHV)
    {return m_modelMap[prevState.modelName()]->naturalHVAtNormalSurface(prevState,
                                    measHV,
                                    surfType,
                                    surf,
                                    natHV);}


  //! build the surface normal to the running direction 
  void runningSurface(const IMeasurement& meas,
                  ISurface& surf);
  void runningSurface(const IMeasurement& meas,
                  const std::string modelName,
                  ISurface& surf);


  //! Computes the surface in which the residual will be calculated (according to surfacePropagation()) and
  //! set it to the point 
  bool computeResidualSurface(const IState& iniS,
                      const IMeasurement& meas,
                      ISurface& surf);


  //! returns the residual surface type
  void setResidualSurfaceType(std::string type) {m_residualSurfaceType = type;}
  std::string residualSurfaceType() const {return m_residualSurfaceType;}

  
  //! Computes the residual and set it to the point 
  void computeResidual(IPoint& point);


  //---------------- SETUP ---------------------

  //! set the setup 
  void setSetup(Setup& p){m_setup = &p;}

  //! Returns a reference to the volume
    IVolume& volume(std::string name="") const
       {return setup().volume(name);} 


  //---------- UTILITIES ------------------------

  //! returns the closest point to a surface
  IPoint& closestPointToSurface (const ITrajectory& traj, const ISurface& surf) const;
  IPoint& closestUpdatedPointToSurface (const ITrajectory& traj, const ISurface& surf) const;

  //! returns the closest point to a measurement
  IPoint& closestUpdatedPointToMeasurement(const ITrajectory& traj, const IMeasurement& meas) const;

  double distanceFromPointToSurface(const IPoint& point, const ISurface& surf) const;
  double distanceFromPointToMeasurement(const IPoint& point, const IMeasurement& meas) const;

  //--------------- GENERAL ------------------

  //! get dimension of the dynamic vector for a given model
  const int dynamicNdim(std::string modelName) {return m_modelMap[modelName]->dynamicNdim();}
  
  //! get dimension of the measurement for a given model 
  const int measNdim(std::string modelName) {return m_modelMap[modelName]->measNdim();}
  
  //! returns a reference to the setup
  Setup& setup() const {return *m_setup;}
  
  //! set navigator verbosity 
  void setVerbosity(int v);
  //! return navigator verbosity 
  int verbosity() const {return m_verbosity;}  



  //-------- DUMPING FUNCTIONS ---------------

  void dumpPropagation(std::string volIni, std::string volFinal);

  //! list the available options
  void menu();

  //! returns a reference to the model  //TODO
  IModel& model(std::string modelName) {return *m_modelMap[modelName];}

 protected:
  
  //! returns a reference to the model converter
  IModelConverter& modelConverter(std::string iniModel, std::string finalModel); 
 
  //! returns the volume intersector
  IVolumeIntersector& volumeIntersector(std::string volType){ 
    if (volumeIntersectorExists(volType))
      return *m_volumeIntersectorMap[volType];      
    else if (setup().volumeDefinitionExists(volType) && 
         volumeIntersectorExists(setup().volumeDefinition(volType).type()))
      return *m_volumeIntersectorMap[setup().volumeDefinition(volType).type()];      
    else{
      cout << "FATAL ERROR: No volume intersector  "+ volType +"  defined" << endl;
      assert(false);
    }
  }

  //! check if model exists 
  bool modelExists(std::string name){
    if (m_modelMap.count(name) != 0) return true;
    else return false;
  } 


  //! check if the volumeIntersector exists for a give surface type 
  bool volumeIntersectorExists(std::string type){
    if (m_volumeIntersectorMap.count(type) != 0) return true;
    else return false;
  } 


 protected:

  // setup in use:from the manager
  Setup* m_setup;

  // models
  std::map<std::string, IModel*> m_modelMap;

  // model converters
  std::vector<IModelConverter*> m_modelConverterVector;

  // intersectors
  std::map<std::string, IVolumeIntersector*> m_volumeIntersectorMap;

  // Verbosity
  int m_verbosity; 

  //! the F matrix of the last propagation  
  EMatrix m_F;
  
  //! The Q matrix of the last propagation
  EMatrix m_Q;

  std::string m_residualSurfaceType;
};
#endif

---