Line data    Source code

       1              : #ifndef DE5D4081_1ED8_4BFB_B9C5_32EE6E10ADFC
       2              : #define DE5D4081_1ED8_4BFB_B9C5_32EE6E10ADFC
       3              : 
       4              : #include "motion/state_vec_converter.h"
       5              : 
       6              : namespace tracking
       7              : {
       8              : namespace motion
       9              : {
      10              : 
      11              : template <typename MM>
      12              : inline void StateVecConverter<MM, MM>::convertFrom(typename MM::StateVec& dstVec, const typename MM::StateVec& srcVec)
      13              : {
      14              :   if (&srcVec != &dstVec)
      15              :   {
      16              :     dstVec = srcVec;
      17              :   }
      18              : }
      19              : 
      20              : template <typename CovarianceMatrixPolicy_>
      21            2 : inline void StateVecConverter<MotionModelCV<CovarianceMatrixPolicy_>, MotionModelCA<CovarianceMatrixPolicy_>>::convertFrom(
      22              :     typename MotionModelCV<CovarianceMatrixPolicy_>::StateVec&       dstVec,
      23              :     const typename MotionModelCA<CovarianceMatrixPolicy_>::StateVec& srcVec)
      24              : {
      25              :   using DstType = MotionModelCV<CovarianceMatrixPolicy_>;
      26              :   using SrcType = MotionModelCA<CovarianceMatrixPolicy_>;
      27              : 
      28            2 :   dstVec.at_unsafe(DstType::X)  = srcVec.at_unsafe(SrcType::X);
      29            2 :   dstVec.at_unsafe(DstType::VX) = srcVec.at_unsafe(SrcType::VX);
      30            2 :   dstVec.at_unsafe(DstType::Y)  = srcVec.at_unsafe(SrcType::Y);
      31            2 :   dstVec.at_unsafe(DstType::VY) = srcVec.at_unsafe(SrcType::VY);
      32            2 : }
      33              : 
      34              : template <typename CovarianceMatrixPolicy_>
      35            2 : inline void StateVecConverter<MotionModelCA<CovarianceMatrixPolicy_>, MotionModelCV<CovarianceMatrixPolicy_>>::convertFrom(
      36              :     typename MotionModelCA<CovarianceMatrixPolicy_>::StateVec&       dstVec,
      37              :     const typename MotionModelCV<CovarianceMatrixPolicy_>::StateVec& srcVec)
      38              : {
      39              :   using DstType    = MotionModelCA<CovarianceMatrixPolicy_>;
      40              :   using SrcType    = MotionModelCV<CovarianceMatrixPolicy_>;
      41              :   using value_type = typename CovarianceMatrixPolicy_::value_type;
      42              : 
      43            2 :   dstVec.at_unsafe(DstType::X)  = srcVec.at_unsafe(SrcType::X);
      44            2 :   dstVec.at_unsafe(DstType::VX) = srcVec.at_unsafe(SrcType::VX);
      45            2 :   dstVec.at_unsafe(DstType::AX) = static_cast<value_type>(0.0);
      46            2 :   dstVec.at_unsafe(DstType::Y)  = srcVec.at_unsafe(SrcType::Y);
      47            2 :   dstVec.at_unsafe(DstType::VY) = srcVec.at_unsafe(SrcType::VY);
      48            2 :   dstVec.at_unsafe(DstType::AY) = static_cast<value_type>(0.0);
      49            2 : }
      50              : 
      51              : } // namespace motion
      52              : } // namespace tracking
      53              : 
      54              : #endif // DE5D4081_1ED8_4BFB_B9C5_32EE6E10ADFC