spline.cc
Go to the documentation of this file.
00001 /******************************************************************************
00002 Copyright (c) 2018, Alexander W. Winkler. All rights reserved.
00003 
00004 Redistribution and use in source and binary forms, with or without
00005 modification, are permitted provided that the following conditions are met:
00006 
00007 * Redistributions of source code must retain the above copyright notice, this
00008   list of conditions and the following disclaimer.
00009 
00010 * Redistributions in binary form must reproduce the above copyright notice,
00011   this list of conditions and the following disclaimer in the documentation
00012   and/or other materials provided with the distribution.
00013 
00014 * Neither the name of the copyright holder nor the names of its
00015   contributors may be used to endorse or promote products derived from
00016   this software without specific prior written permission.
00017 
00018 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00021 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
00022 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00023 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
00024 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00025 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
00026 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00027 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00028 ******************************************************************************/
00029 
00030 #include <towr/variables/spline.h>
00031 
00032 #include <numeric> // std::accumulate
00033 
00034 namespace towr {
00035 
00036 Spline::Spline(const VecTimes& poly_durations, int n_dim)
00037 {
00038   uint n_polys = poly_durations.size();
00039 
00040   cubic_polys_.assign(n_polys, CubicHermitePolynomial(n_dim));
00041   for (int i=0; i<cubic_polys_.size(); ++i) {
00042     cubic_polys_.at(i).SetDuration(poly_durations.at(i));
00043   }
00044 
00045   UpdatePolynomialCoeff();
00046 }
00047 
00048 int
00049 Spline::GetSegmentID(double t_global, const VecTimes& durations)
00050 {
00051   double eps = 1e-10; // double precision
00052   assert(t_global >= 0.0);
00053 
00054    double t = 0;
00055    int i=0;
00056    for (double d: durations) {
00057      t += d;
00058 
00059      if (t >= t_global-eps) // at junctions, returns previous spline (=)
00060        return i;
00061 
00062      i++;
00063    }
00064 
00065    assert(false); // this should never be reached
00066 }
00067 
00068 std::pair<int,double>
00069 Spline::GetLocalTime (double t_global, const VecTimes& durations) const
00070 {
00071   int id = GetSegmentID(t_global, durations);
00072 
00073   double t_local = t_global;
00074   for (int i=0; i<id; i++)
00075     t_local -= durations.at(i);
00076 
00077   return std::make_pair(id, t_local);
00078 }
00079 
00080 const State
00081 Spline::GetPoint(double t_global) const
00082 {
00083   int id; double t_local;
00084   std::tie(id, t_local) = GetLocalTime(t_global, GetPolyDurations());
00085 
00086   return GetPoint(id, t_local);
00087 }
00088 
00089 const State
00090 Spline::GetPoint(int poly_id, double t_local) const
00091 {
00092   return cubic_polys_.at(poly_id).GetPoint(t_local);
00093 }
00094 
00095 void
00096 Spline::UpdatePolynomialCoeff()
00097 {
00098   for (auto& p : cubic_polys_)
00099     p.UpdateCoeff();
00100 }
00101 
00102 int
00103 Spline::GetPolynomialCount () const
00104 {
00105   return cubic_polys_.size();
00106 }
00107 
00108 Spline::VecTimes
00109 Spline::GetPolyDurations() const
00110 {
00111   VecTimes poly_durations;
00112   for (const auto& p : cubic_polys_)
00113     poly_durations.push_back(p.GetDuration());
00114 
00115   return poly_durations;
00116 }
00117 
00118 double
00119 Spline::GetTotalTime() const
00120 {
00121   auto v = GetPolyDurations();
00122   return std::accumulate(v.begin(), v.end(), 0.0);
00123 }
00124 
00125 } /* namespace towr */
00126 


towr_core
Author(s): Alexander W. Winkler
autogenerated on Mon Apr 9 2018 03:12:44