JDynamics.hh

Go to the documentation of this file.

#ifndef __JDYNAMICS__JDYNAMICS__
#define __JDYNAMICS__JDYNAMICS__

#include <limits>
#include <ostream>

#include "JDetector/JDetector.hh"
#include "JDetector/JDetectorAddressMapToolkit.hh"

#include "JCompass/JEvt.hh"
#include "JCompass/JEvtToolkit.hh"

#include "JAcoustics/JModel.hh"
#include "JAcoustics/JGeometry.hh"
#include "JAcoustics/JEvt.hh"

#include "JLang/JObjectIterator.hh"
#include "JLang/JException.hh"

#include "JGeometry3D/JQuaternion3D.hh"
#include "JGeometry3D/JEigen3D.hh"

#include "JTools/JElement.hh"
#include "JTools/JCollection.hh"
#include "JTools/JPolfit.hh"
#include "JTools/JHashMap.hh"


/**
 * \file
 *
 * Dynamic detector calibration.
 * \author mdejong
 */

namespace JDYNAMICS {}
namespace JPP { using namespace JDYNAMICS; }

/**
 * Main namespace for dynamic position and orientation calibration.
 */
namespace JDYNAMICS {

  using JLANG::JObjectIterator;
  using JLANG::JValueOutOfRange;
  using JDETECTOR::JDetector;

  
  /**
   * Dynamic detector calibration.
   */
  struct JDynamics {

    /**
     * Get minimal abscissa of mapped function objects.
     *
     * \param  input                     input
     * \return                           minimal abscissa
     */
    template<class JKey_t, class JValue_t, class JEvaluator_t>
    static inline double getXmin(const JTOOLS::JHashMap<JKey_t, JValue_t, JEvaluator_t>& input)
    {
      double xmin = std::numeric_limits<double>::max();

      for (typename JTOOLS::JHashMap<JKey_t, JValue_t, JEvaluator_t>::const_iterator i = input.begin(); i != input.end(); ++i) {
        if (!i->second.empty() && i->second.getXmin() < xmin) {
          xmin = i->second.getXmin();
        }
      }
 
      return xmin;
    }


    /**
     * Get maximal abscissa of mapped function objects.
     *
     * \param  input                     input
     * \return                           maximal abscissa
     */
    template<class JKey_t, class JValue_t, class JEvaluator_t>
    static inline double getXmax(const JTOOLS::JHashMap<JKey_t, JValue_t, JEvaluator_t>& input)
    {
      double xmax = std::numeric_limits<double>::lowest();

      for (typename JTOOLS::JHashMap<JKey_t, JValue_t, JEvaluator_t>::const_iterator i = input.begin(); i != input.end(); ++i) {
        if (!i->second.empty() && i->second.getXmax() > xmax) {
          xmax = i->second.getXmax();
        }
      }

      return xmax;
    }


    /**
     * Dynamic orientation calibration.
     */
    struct JOrientation {

      enum {
        NUMBER_OF_POINTS  = 20,    //!< number of points  for interpolation
        NUMBER_OF_DEGREES =  1     //!< number of degrees for interpolation
      };

      typedef JTOOLS::JElement2D<double, JGEOMETRY3D::JQuaternion3D>          element_type;
      typedef JTOOLS::JPolfitFunction1D<NUMBER_OF_POINTS,
                                        NUMBER_OF_DEGREES,
                                        element_type, JTOOLS::JCollection>    function_type;
      typedef typename function_type::collection_type::container_type         container_type;
  
      typedef JTOOLS::JHashMap<int, JGEOMETRY3D::JQuaternion3D>               buffer_type;
      typedef JTOOLS::JHashMap<int, function_type>                            data_type;

      typedef data_type::const_iterator                                       const_iterator;
      typedef data_type::const_reverse_iterator                               const_reverse_iterator;
      

      /**
       * Constructor.
       *
       * \param  detector                 detector
       * \param  Tmax_s                   applicability period of calibration [s]
       */
      JOrientation(const JDetector& detector,
                   const double     Tmax_s) :
        Tmax_s(Tmax_s),
        t0_s(std::numeric_limits<double>::lowest())
      {
        using namespace JPP;

        if (hasDetectorAddressMap(detector.getID())) {

          const JDetectorAddressMap& demo = getDetectorAddressMap(detector.getID());

          for (JDetector::const_iterator module = detector.begin(); module != detector.end(); ++module) {
            if (module->getFloor() != 0) {
              buffer[module->getID()] = getRotation(getModule(demo.get(module->getID())), *module);
            }
          }

        } else {

          THROW(JValueOutOfRange, "No detector address map for detector identier " << detector.getID());
        }
      }


      /**
       * Load calibration data.
       *
       * \param  input                    calibration data
       */
      void load(JObjectIterator<JCOMPASS::JOrientation>& input)
      {
        t0_s = std::numeric_limits<double>::lowest();

        while (input.hasNext()) {

          const JCOMPASS::JOrientation* orientation = input.next();

          static_cast<container_type&>(calibration[orientation->id]).push_back(element_type(orientation->t, getQuaternion(*orientation)));
        }

        for (data_type::iterator i = calibration.begin(); i != calibration.end(); ++i) {
          i->second.sort();
          i->second.compile();
        }
      }


      bool                   empty()  const { return calibration.empty(); }    //!< empty
      const_iterator         begin()  const { return calibration.begin(); }    //!< begin of calibration data
      const_iterator         end()    const { return calibration.end(); }      //!< end   of calibration data
      const_reverse_iterator rbegin() const { return calibration.rbegin(); }   //!< begin of reverse of calibration data
      const_reverse_iterator rend()   const { return calibration.rend(); }     //!< begin of reverse of calibration data


      /**
       * Get applicability period.
       *
       * \return                          applicability period [s]
       */
      double getTmax() const
      {
        return Tmax_s;
      }


      /**
       * Set applicability period.
       *
       * \param  Tmax_s                   applicability period [s]
       */
      void setTmax(const double Tmax_s)
      {
        this->Tmax_s = Tmax_s;
      }


      /**
       * Calibrate given detector at given time.
       *
       * \param  detector                 detector (I/O)
       * \param  t1_s                     time [s]
       */
      void update(JDetector& detector, const double t1_s)
      {
        using namespace std;
        using namespace JPP;

        if (!calibration.empty()) {

          if (fabs(t1_s - t0_s) > Tmax_s) {

            for (JDetector::iterator module = detector.begin(); module != detector.end(); ++module) {
          
              if (module->getFloor() != 0 && calibration.has(module->getID()) && buffer.has(module->getID())) {

                const function_type& f1 = calibration.get(module->getID());

                if (!f1.empty()) {

                  if (t1_s >= f1.getXmin() && t1_s <= f1.getXmax()) {

                    JQuaternion3D Q0 = buffer[module->getID()];
                    JQuaternion3D Q1 = f1(t1_s);

                    const JPosition3D center = module->getPosition();

                    module->sub(center);

                    module->rotate(Q1 * Q0.conjugate());

                    module->add(center);

                    buffer[module->getID()] = Q1;
                  }
                }
              }
            }

            t0_s = t1_s;
          }
        }
      }


      /**
       * Get minimal abscissa.
       *
       * \return                           minimal abscissa
       */
      double getXmin() const
      {
        return JDynamics::getXmin(this->calibration);
      }


      /**
       * Get maximal abscissa.
       *
       * \return                           maximal abscissa
       */
      double getXmax() const
      {
        return JDynamics::getXmax(this->calibration);
      }


      /**
       * Write calibration to output stream.
       *
       * \param  out                       output stream
       * \param  calibration
       * \return                           output stream
       */
      friend inline std::ostream& operator<<(std::ostream& out, const JOrientation& calibration)
      {
        if (!calibration.empty())
          return out << "[" << FIXED(15,0) << calibration.getXmin()
                     << "," << FIXED(15,0) << calibration.getXmax()
                     << "]";
        else
          return out << "[,]";    
      }

    protected:
      buffer_type buffer;
      data_type   calibration;
      double      Tmax_s;

    private:
      double      t0_s;
    };


    /**
     * Dynamic position calibration.
     */
    struct JPosition {

      enum {
        NUMBER_OF_POINTS  = 7,    //!< number of points  for interpolation
        NUMBER_OF_DEGREES = 2     //!< number of degrees for interpolation
      };

      typedef JACOUSTICS::JGeometry                                           JGeometry;

      typedef JTOOLS::JElement2D<double, JACOUSTICS::JMODEL::JString>         element_type;
      typedef JTOOLS::JPolfitFunction1D<NUMBER_OF_POINTS,
                                        NUMBER_OF_DEGREES,
                                        element_type, JTOOLS::JCollection>    function_type;

      typedef JTOOLS::JHashMap<int, function_type>                            data_type;

      typedef data_type::const_iterator                                       const_iterator;
      typedef data_type::const_reverse_iterator                               const_reverse_iterator;

      
      /**
       * Constructor.
       *
       * \param  detector                 detector
       * \param  Tmax_s                   applicability period of calibration [s]
       */
      JPosition(const JDetector& detector,
                const double     Tmax_s) :
        geometry(detector),
        Tmax_s(Tmax_s),
        t0_s(std::numeric_limits<double>::lowest())
      {}


      /**
       * Load calibration data.
       *
       * \param  input                    calibration data
       */
      void load(JObjectIterator<JACOUSTICS::JEvt>& input)
      {
        using namespace JPP;
        
        t0_s = std::numeric_limits<double>::lowest();

        while (input.hasNext()) {

          const JACOUSTICS::JEvt* evt = input.next();
          const double t1_s = 0.5 * (evt->UNIXTimeStart + evt->UNIXTimeStop);

          for (JACOUSTICS::JEvt::const_iterator i = evt->begin(); i != evt->end(); ++i) {
            calibration[i->id][t1_s] = JMODEL::JString(i->tx, i->ty);
          }
        }

        for (data_type::iterator i = calibration.begin(); i != calibration.end(); ++i) {
          i->second.compile();
        }
      }


      bool                   empty()  const { return calibration.empty(); }    //!< empty
      const_iterator         begin()  const { return calibration.begin(); }    //!< begin of calibration data
      const_iterator         end()    const { return calibration.end(); }      //!< end   of calibration data
      const_reverse_iterator rbegin() const { return calibration.rbegin(); }   //!< begin of reverse of calibration data
      const_reverse_iterator rend()   const { return calibration.rend(); }     //!< begin of reverse of calibration data


      /**
       * Get applicability period.
       *
       * \return                          applicability period [s]
       */
      double getTmax() const
      {
        return Tmax_s;
      }


      /**
       * Set applicability period.
       *
       * \param  Tmax_s                   applicability period [s]
       */
      void setTmax(const double Tmax_s)
      {
        this->Tmax_s = Tmax_s;
      }


      /**
       * Get detector geometry.
       *
       * \return                          detector geometry
       */
      const JGeometry&  getGeometry() const
      {
        return geometry;
      }


      /**
       * Calibrate given detector at given time.
       *
       * \param  detector                 detector (I/O)
       * \param  t1_s                     time [s]
       */
      void update(JDetector& detector, const double t1_s)
      {
        using namespace std;
        using namespace JPP;

        if (!calibration.empty()) {

          if (fabs(t1_s - t0_s) > Tmax_s) {

            JHashMap<int, JMODEL::JString> buffer;

            for (JDetector::iterator module = detector.begin(); module != detector.end(); ++module) {

              if (module->getFloor() != 0) {

                if (!buffer.has(module->getString())) {
                  if (calibration.has(module->getString())) {
                    buffer[module->getString()] = calibration[module->getString()](t1_s);
                  }
                }

                if (buffer.has(module->getString())) {
                  module->set(geometry[module->getString()].getPosition(buffer[module->getString()], module->getFloor())  -  getPiezoPosition());
                }
              }
            }

            t0_s = t1_s;
          }
        }
      }


      /**
       * Get minimal abscissa.
       *
       * \return                           minimal abscissa
       */
      double getXmin() const
      {
        return JDynamics::getXmin(this->calibration);
      }


      /**
       * Get maximal abscissa.
       *
       * \return                           maximal abscissa
       */
      double getXmax() const
      {
        return JDynamics::getXmax(this->calibration);
      }


      /**
       * Write calibration to output stream.
       *
       * \param  out                       output stream
       * \param  calibration
       * \return                           output stream
       */
      friend inline std::ostream& operator<<(std::ostream& out, const JPosition& calibration)
      {
        if (!calibration.empty())
          return out << "[" << FIXED(15,0) << calibration.getXmin()
                     << "," << FIXED(15,0) << calibration.getXmax()
                     << "]";
        else
          return out << "[,]";
      }

    protected:
      JGeometry  geometry;
      data_type  calibration;
      double     Tmax_s;

    private:
      double     t0_s;
    };
    

    /**
     * Constructor.
     *
     * \param  detector                 detector
     * \param  Tmax_s                   applicability period of calibration [s]
     */
    JDynamics(const JDetector& detector,
              const double     Tmax_s) :
      detector   (detector),
      orientation(detector, Tmax_s),
      position   (detector, Tmax_s)
    {}


    /**
     * Load calibration data.
     *
     * \param  input                    detector calibration data
     */
    template<class JObjectIterator_t>
    void load(JObjectIterator_t& input)
    {
      try { orientation.load(dynamic_cast<JObjectIterator<JCOMPASS::JOrientation>&>(input)); } catch(const std::exception& error) {}
      try { position   .load(dynamic_cast<JObjectIterator<JACOUSTICS::JEvt>&>      (input)); } catch(const std::exception& error) {}
    }


    /**
     * Get detector calibrated at given time.
     *
     * \param  t1_s                     time [s]
     * \return                          detector
     */
    const JDetector& operator()(const double t1_s)
    {
      orientation.update(this->detector, t1_s);
      position   .update(this->detector, t1_s);

      return this->detector;
    }

  protected:
    JDetector    detector;
    
  public:
    JOrientation orientation;
    JPosition    position;
  };
}

#endif