param_func_spline0_dist.h

Go to the documentation of this file.

/***************************************************************************
 *   Software License Agreement (BSD License)                              *
 *   Copyright (C) 2016 by Horatiu George Todoran <todorangrg@gmail.com>   *
 *                                                                         *
 *   Redistribution and use in source and binary forms, with or without    *
 *   modification, are permitted provided that the following conditions    *
 *   are met:                                                              *
 *                                                                         *
 *   1. Redistributions of source code must retain the above copyright     *
 *      notice, this list of conditions and the following disclaimer.      *
 *   2. Redistributions in binary form must reproduce the above copyright  *
 *      notice, this list of conditions and the following disclaimer in    *
 *      the documentation and/or other materials provided with the         *
 *      distribution.                                                      *
 *   3. Neither the name of the copyright holder nor the names of its      *
 *      contributors may be used to endorse or promote products derived    *
 *      from this software without specific prior written permission.      *
 *                                                                         *
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS   *
 *   "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT     *
 *   LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS     *
 *   FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE        *
 *   COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,  *
 *   INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,  *
 *   BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;      *
 *   LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER      *
 *   CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT    *
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY *
 *   WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE           *
 *   POSSIBILITY OF SUCH DAMAGE.                                           *
 ***************************************************************************/

#ifndef PARAM_FUNC_SPLINE0_DIST_H
#define PARAM_FUNC_SPLINE0_DIST_H

#include <memory>
#include <vector>

#include <tuw_control/param_func/param_func_dist.h>

namespace tuw
{
class ParamFuncsSpline0Dist;
using ParamFuncsSpline0DistPtr = std::shared_ptr<ParamFuncsSpline0Dist>;
using ParamFuncsSpline0DistConstPtr = std::shared_ptr<ParamFuncsSpline0Dist const>;
class ParamFuncsSpline0Dist : public ParamFuncsDist
{
  //--------------------------------------- ParamFunc implementation ---------------------------------------//
protected:
  struct FuncCacheData
  {
    FuncCacheData(const FuncCtrlPt& _ctrlPt) : val(_ctrlPt.val), arc(_ctrlPt.arc)
    {
    }
    const double& val;
    const double& arc;
    std::array<double, nrModesMax_> cache;
  };

  // special class member functions
public:
  ParamFuncsSpline0Dist() = default;

public:
  virtual ~ParamFuncsSpline0Dist() = default;

public:
  ParamFuncsSpline0Dist(const ParamFuncsSpline0Dist&);  // = default;
public:
  ParamFuncsSpline0Dist& operator=(const ParamFuncsSpline0Dist&);  // = default;
public:
  ParamFuncsSpline0Dist(ParamFuncsSpline0Dist&&) = delete;

public:
  ParamFuncsSpline0Dist& operator=(ParamFuncsSpline0Dist&&) = delete;

  //(re-)implemented virtual functions
protected:
  virtual void initImpl() override;
public:
  virtual void precompute() override;

public:
  void setEvalArc(const double& _funcsArcEval,
                  const EvalArcGuarantee& _evalArcGuarantee = EvalArcGuarantee::NONE) override;

public:
  virtual double computeFuncVal(const std::size_t& _funcIdx) const override;

public:
  virtual double computeFuncDiff1(const std::size_t& _funcIdx) const override;

public:
  virtual double computeFuncDiff2(const std::size_t& _funcIdx) const override;

public:
  virtual double computeFuncInt1(const std::size_t& _funcIdx) const override;

public:
  virtual double computeFuncInt2(const std::size_t& _funcIdx) const override;

private:
  double computeFuncDeltaInt1(const std::size_t& _funcIdx, const double& _arcEnd) const;
private:
  double computeFuncDeltaInt2(const std::size_t& _funcIdx, const double& _arcEnd) const;

protected:
  std::vector<std::size_t> funcEvalArcCacheIdxUnder_;
protected:
  std::vector<std::vector<FuncCacheData> > funcEvalCache_;
private:
  std::vector<std::vector<std::size_t> > arc2func_;

  //--------------------------------------- ParamFuncDist implementation ---------------------------------------//

  //(re-)implemented virtual functions
public:
  void setDistCfMode(TraveledDistCfMode _distCfMode, const std::vector<std::size_t>& _distRelFuncIdx) override;

public:
  double computeS() const override;

public:
  double computeT(const double& _s, const EvalArcGuarantee& _evalArcGuarantee = EvalArcGuarantee::NONE) override;

public:
  void setEvalDist(const double& _funcsDistEval,
                   const EvalArcGuarantee& _evalArcGuarantee = EvalArcGuarantee::NONE) override;

public:
  void computeS2TLattice(const std::vector<double>& _sLattice, std::vector<double>& _tLattice) override;

public:
  void computeS2TLattice(const double& _arc0, const double& _ds, std::vector<double>& _tLattice) override;

public:
  static double computeDeltaS_V_AV(const double& _dt, const double& _v0, const double& _av);
public:
  static double computeDeltaT_V_AV(const double& _ds, const double& _v0, const double& _av);

private:
  void precomputeDist();
private:
  double computeTImpl(const double& _s, const EvalArcGuarantee& _evalArcGuarantee);
private:
  double computeS_V() const;
private:
  double computeS_AV() const;
private:
  double computeT_V(const double& _ds) const;
private:
  double computeT_AV(const double& _ds) const;

private:
  std::vector<std::size_t> distLinkedFuncIdx_;
private:
  std::vector<double> distEvalCache_;
private:
  TraveledDistCfMode distCfMode_;
private:
  std::vector<std::size_t> distRelFuncIdx_;
private:
  std::size_t distLinkedArcIdx_;

private:
  using ComputeSFuncPtr = double (ParamFuncsSpline0Dist::*)() const;

private:
  using ComputeDs2DtPtr = double (ParamFuncsSpline0Dist::*)(const double&) const;

private:
  ComputeSFuncPtr computeSFuncPtr_;

private:
  ComputeDs2DtPtr computeDs2DtPtr_;
};
}

#endif  // PARAM_FUNC_SPLINE0_DIST_H