src/Replay/CatmullRomInterpolator.hpp

   1 /*
   2   Copyright_License {
   3
   4   XCSoar Glide Computer - http://www.xcsoar.org/
   5   Copyright (C) 2000-2013 The XCSoar Project
   6   A detailed list of copyright holders can be found in the file "AUTHORS".
   7
   8   This program is free software; you can redistribute it and/or
   9   modify it under the terms of the GNU General Public License
  10   as published by the Free Software Foundation; either version 2
  11   of the License, or (at your option) any later version.
  12
  13   This program is distributed in the hope that it will be useful,
  14   but WITHOUT ANY WARRANTY; without even the implied warranty of
  15   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16   GNU General Public License for more details.
  17
  18   You should have received a copy of the GNU General Public License
  19   along with this program; if not, write to the Free Software
  20   Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  21 }
  22 */
  23
  24 #ifndef XCSOAR_CATMULL_ROM_INTERPOLATOR_HPP
  25 #define XCSOAR_CATMULL_ROM_INTERPOLATOR_HPP
  26
  27 #include "Math/fixed.hpp"
  28 #include "Geo/GeoPoint.hpp"
  29 #include "Geo/GeoVector.hpp"
  30 #include "Util/Clamp.hpp"
  31
  32 #include <algorithm>
  33 #include <assert.h>
  34
  35 /**
  36  * A Catmull-Rom splines interpolator
  37  * @see http://www.cs.cmu.edu/~462/projects/assn2/assn2/catmullRom.pdf
  38  */
  39 class CatmullRomInterpolator
  40 {
  41 public:
  42   struct Record {
  43     GeoPoint location;
  44     fixed gps_altitude;
  45     fixed baro_altitude;
  46     fixed time;
  47   };
  48
  49 private:
  50   const fixed time;
  51
  52   unsigned num;
  53   Record p[4];
  54
  55 public:
  56   CatmullRomInterpolator(fixed _time):
  57     time(_time)
  58   {
  59     Reset();
  60   }
  61
  62   void
  63   Reset()
  64   {
  65     num = 0;
  66   }
  67
  68   void
  69   Update(fixed t, GeoPoint location, fixed alt, fixed palt)
  70   {
  71     if (num && (t <= p[3].time))
  72       return;
  73
  74     if (num < 4)
  75       num++;
  76
  77     std::copy(p + 1, p + 4, p);
  78
  79     p[3].location = location;
  80     p[3].gps_altitude = alt;
  81     p[3].baro_altitude = palt;
  82     p[3].time = t;
  83   }
  84
  85   bool
  86   Ready() const
  87   {
  88     return (num == 4);
  89   }
  90
  91   GeoVector
  92   GetVector(fixed _time) const
  93   {
  94     assert(Ready());
  95
  96     if (!positive(p[2].time-p[1].time))
  97       return GeoVector(fixed(0), Angle::Zero());
  98
  99     const Record r0 = Interpolate(_time - fixed(0.05));
 100     const Record r1 = Interpolate(_time + fixed(0.05));
 101
 102     fixed speed = p[1].location.Distance(p[2].location) / (p[2].time - p[1].time);
 103     Angle bearing = r0.location.Bearing(r1.location);
 104
 105     return GeoVector(speed, bearing);
 106   }
 107
 108   gcc_pure
 109   Record
 110   Interpolate(fixed _time) const
 111   {
 112     assert(Ready());
 113
 114     const fixed u = GetTimeFraction(_time, false);
 115
 116     /*
 117       ps = ( c0   c1    c2  c3)
 118            [  0    1     0   0] 1
 119            [ -t    0     t   0] u
 120            [ 2t  t-3  3-2t  -t] u^2
 121            [ -t  2-t   t-2   t] u^3
 122     */
 123
 124     const fixed u2 = sqr(u);
 125     const fixed u3 = u2 * u;
 126     const fixed c[4]= {-time * u3 + 2 * time * u2 - time * u,
 127                        (fixed(2) - time) * u3 + (time - fixed(3)) * u2 + fixed(1),
 128                        (time - fixed(2)) * u3 + (fixed(3) - 2 * time) * u2 + time * u,
 129                         time * u3 - time * u2};
 130
 131     Record r;
 132     r.location.latitude =
 133         p[0].location.latitude * c[0] + p[1].location.latitude * c[1] +
 134         p[2].location.latitude * c[2] + p[3].location.latitude * c[3];
 135
 136     r.location.longitude =
 137         p[0].location.longitude * c[0] + p[1].location.longitude * c[1] +
 138         p[2].location.longitude * c[2] + p[3].location.longitude * c[3];
 139
 140     r.gps_altitude =
 141         p[0].gps_altitude * c[0] + p[1].gps_altitude * c[1] +
 142         p[2].gps_altitude * c[2] + p[3].gps_altitude * c[3];
 143
 144     r.baro_altitude =
 145         p[0].baro_altitude * c[0] + p[1].baro_altitude * c[1] +
 146         p[2].baro_altitude * c[2] + p[3].baro_altitude * c[3];
 147
 148     r.time = _time;
 149
 150     return r;
 151   }
 152
 153   fixed
 154   GetMinTime() const
 155   {
 156     assert(Ready());
 157
 158     return p[0].time;
 159   }
 160
 161   fixed
 162   GetMaxTime() const
 163   {
 164     assert(Ready());
 165
 166     return std::max({fixed(0), p[0].time, p[1].time, p[2].time, p[3].time});
 167   }
 168
 169   bool
 170   NeedData(fixed t_simulation) const
 171   {
 172     return !Ready() || (p[2].time <= t_simulation + fixed(0.1));
 173   }
 174
 175 private:
 176   fixed
 177   GetTimeFraction(const fixed time, bool limit_range = true) const
 178   {
 179     assert(Ready());
 180     assert(p[2].time > p[1].time);
 181
 182     const fixed fraction = (time - p[1].time) / (p[2].time - p[1].time);
 183
 184     if (limit_range)
 185       return Clamp(fraction, fixed(0), fixed(1));
 186     else
 187       return fraction;
 188   }
 189 };
 190
 191 #endif