CSplineInterpolator1D.cpp

Go to the documentation of this file.

/* +------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)            |
   |                          http://www.mrpt.org/                          |
   |                                                                        |
   | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file     |
   | See: http://www.mrpt.org/Authors - All rights reserved.                |
   | Released under BSD License. See details in http://www.mrpt.org/License |
   +------------------------------------------------------------------------+ */

#include "base-precomp.h"  // Precompiled headers

#include <mrpt/utils/types_math.h>  // for dynamic_vector, CVector...
#include <mrpt/math/CSplineInterpolator1D.h>  // for CSplineInterpolator1D
#include <mrpt/utils/CStream.h>  // for CStream, operator<<
#include <mrpt/utils/stl_serialization.h>  // for operator<<, operator>>
#include <map>  // for _Rb_tree_const_iterator
#include <mrpt/math/interp_fit.hpp>  // for spline
#include <mrpt/utils/CObject.h>  // for CSplineInterpolator1D::...
#include <mrpt/utils/CSerializable.h>  // for CSerializable, CSeriali...
#include <mrpt/utils/bits.h>  // for format
#include <mrpt/utils/mrpt_macros.h>  // for MRPT_THROW_UNKNOWN_SERI...

using namespace mrpt;
using namespace mrpt::math;
using namespace mrpt::utils;
using namespace std;

// This must be added to any CSerializable class implementation file.
IMPLEMENTS_SERIALIZABLE(CSplineInterpolator1D, CSerializable, mrpt::math)

/*---------------------------------------------------------------
                                                Constructor
  ---------------------------------------------------------------*/
CSplineInterpolator1D::CSplineInterpolator1D(bool wrap2pi) : m_wrap2pi(wrap2pi)
{
}

/*---------------------------------------------------------------
                                                appendXY
  ---------------------------------------------------------------*/
void CSplineInterpolator1D::appendXY(double x, double y) { m_x2y[x] = y; }
/*---------------------------------------------------------------
                                                query
  ---------------------------------------------------------------*/
double& CSplineInterpolator1D::query(double x, double& y, bool& out_valid) const
{
        out_valid = false;
        y = 0;

        std::pair<double, double> p1, p2, p3, p4;

        std::map<double, double>::const_iterator it_ge1 = m_x2y.lower_bound(x);

        // Exact match?
        if (it_ge1 != m_x2y.end() && it_ge1->first == x)
        {
                y = it_ge1->second;
                out_valid = true;
                return y;
        }

        // Are we in the beginning or the end of the path?
        if (it_ge1 == m_x2y.end() || it_ge1 == m_x2y.begin())
        {
                return y;
        }

        p3 = *it_ge1;  // Third pair
        ++it_ge1;
        if (it_ge1 == m_x2y.end())
        {
                return y;
        }
        p4 = *it_ge1;  // Fourth pair

        --it_ge1;
        --it_ge1;
        p2 = *it_ge1;  // Second pair

        if (it_ge1 == m_x2y.begin())
        {
                return y;
        }

        p1 = *(--it_ge1);  // First pair

        // ---------------------------------------
        //    SPLINE INTERPOLATION
        // ---------------------------------------
        CVectorDouble xs(4);
        xs[0] = p1.first;
        xs[1] = p2.first;
        xs[2] = p3.first;
        xs[3] = p4.first;

        CVectorDouble ys(4);
        ys[0] = p1.second;
        ys[1] = p2.second;
        ys[2] = p3.second;
        ys[3] = p4.second;

        out_valid = true;
        return y = math::spline(x, xs, ys, m_wrap2pi);
}

/*---------------------------------------------------------------
        Implements the writing to a CStream capability of
                CSerializable objects
  ---------------------------------------------------------------*/
void CSplineInterpolator1D::writeToStream(
        mrpt::utils::CStream& out, int* version) const
{
        if (version)
                *version = 0;
        else
        {
                out << m_x2y << m_wrap2pi;
        }
}

/*---------------------------------------------------------------
        Implements the reading from a CStream capability of
                CSerializable objects
  ---------------------------------------------------------------*/
void CSplineInterpolator1D::readFromStream(
        mrpt::utils::CStream& in, int version)
{
        switch (version)
        {
                case 0:  // floats
                {
                        in >> m_x2y >> m_wrap2pi;
                }
                break;
                default:
                        MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version)
        };
}