simgear/misc/interpolator.cxx

   1 // Written by Andrew J. Ross, started December 2003
   2 //
   3 // Copyright (C) 2003  Andrew J. Ross - andy@plausible.org
   4 //
   5 // This library is free software; you can redistribute it and/or
   6 // modify it under the terms of the GNU Library General Public
   7 // License as published by the Free Software Foundation; either
   8 // version 2 of the License, or (at your option) any later version.
   9 //
  10 // This library is distributed in the hope that it will be useful,
  11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  13 // Library General Public License for more details.
  14 //
  15 // You should have received a copy of the GNU General Public License
  16 // along with this program; if not, write to the Free Software
  17 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
  18
  19 #ifdef HAVE_CONFIG_H
  20 #  include <simgear_config.h>
  21 #endif
  22
  23 #include "interpolator.hxx"
  24
  25 #include <simgear/math/SGMath.hxx>
  26
  27 void SGInterpolator::addNew(SGPropertyNode* prop, int nPoints)
  28 {
  29     // Set the property type to a double, if it isn't already, and
  30     // make sure we aren't already managing this node.
  31     prop->setDoubleValue(prop->getDoubleValue());
  32     cancel(prop);
  33
  34     Interp* iterp = new Interp();
  35     iterp->target = prop;
  36     iterp->nPoints = nPoints;
  37     iterp->curve = new double[2*nPoints];
  38
  39     // Dirty trick: leave the new value sitting in _list to avoid
  40     // having to return a pointer to a private type.
  41     iterp->next = _list;
  42     _list = iterp;
  43 }
  44
  45 void SGInterpolator::interpolate(SGPropertyNode* prop, int nPoints,
  46                                  double* values, double* deltas)
  47 {
  48     addNew(prop, nPoints);
  49     for(int i=0; i<nPoints; i++) {
  50         _list->dt(i)  = deltas[i];
  51         _list->val(i) = values[i];
  52     }
  53 }
  54
  55 void SGInterpolator::interpolate(SGPropertyNode* prop, double val, double dt)
  56 {
  57     addNew(prop, 1);
  58     _list->dt(0) = dt;
  59     _list->val(0) = val;
  60 }
  61
  62 //
  63 // Delete all the list elements where "expr" is true.
  64 //
  65 // Silly preprocessor hack to avoid writing the linked list code in
  66 // two places.  You would think that an STL set would be the way to
  67 // go, but I had terrible trouble getting it to work with the
  68 // dynamically allocated "curve" member.  Frankly, this is easier to
  69 // write, and the code is smaller to boot...
  70 //
  71 #define DELETE_WHERE(EXPR)\
  72 Interp *p = _list, **last = &_list;      \
  73 while(p) {                               \
  74     if(EXPR) {                           \
  75         *last = p->next;                 \
  76         delete p;                        \
  77         p = (*last) ? (*last)->next : 0; \
  78     } else {                             \
  79         last = &(p->next);               \
  80         p = p->next; } }
  81
  82 void SGInterpolator::cancel(SGPropertyNode* prop)
  83 {
  84     DELETE_WHERE(p->target == prop)
  85 }
  86
  87 void SGInterpolator::update(double dt)
  88 {
  89     DELETE_WHERE(interp(p, dt))
  90 }
  91
  92 // This is the where the only "real" work happens.  Walk through the
  93 // data points until we find one with some time left, slurp it up and
  94 // repeat until we run out of dt.
  95 bool SGInterpolator::interp(Interp* rec, double dt)
  96 {
  97     double val = rec->target->getDoubleValue();
  98     int i;
  99     for(i=0; i < rec->nPoints; i++) {
 100         if(rec->dt(i) > 0 && dt < rec->dt(i)) {
 101             val += (dt / rec->dt(i)) * (rec->val(i) - val);
 102             rec->dt(i) -= dt;
 103             break;
 104         }
 105         dt -= rec->dt(i);
 106         val = rec->val(i);
 107     }
 108     rec->target->setDoubleValue(val);
 109
 110     // Return true if this one is done
 111     return i == rec->nPoints;
 112 }