00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #include "interpolator.hxx"
00020
00021 #include <simgear/math/SGMath.hxx>
00022
00023 void SGInterpolator::addNew(SGPropertyNode* prop, int nPoints)
00024 {
00025
00026
00027 prop->setDoubleValue(prop->getDoubleValue());
00028 cancel(prop);
00029
00030 Interp* iterp = new Interp();
00031 iterp->target = prop;
00032 iterp->nPoints = nPoints;
00033 iterp->curve = new double[2*nPoints];
00034
00035
00036
00037 iterp->next = _list;
00038 _list = iterp;
00039 }
00040
00041 void SGInterpolator::interpolate(SGPropertyNode* prop, int nPoints,
00042 double* values, double* deltas)
00043 {
00044 addNew(prop, nPoints);
00045 for(int i=0; i<nPoints; i++) {
00046 _list->dt(i) = deltas[i];
00047 _list->val(i) = values[i];
00048 }
00049 }
00050
00051 void SGInterpolator::interpolate(SGPropertyNode* prop, double val, double dt)
00052 {
00053 addNew(prop, 1);
00054 _list->dt(0) = dt;
00055 _list->val(0) = val;
00056 }
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067 #define DELETE_WHERE(EXPR)\
00068 Interp *p = _list, **last = &_list; \
00069 while(p) { \
00070 if(EXPR) { \
00071 *last = p->next; \
00072 delete p; \
00073 p = (*last) ? (*last)->next : 0; \
00074 } else { \
00075 last = &(p->next); \
00076 p = p->next; } }
00077
00078 void SGInterpolator::cancel(SGPropertyNode* prop)
00079 {
00080 DELETE_WHERE(p->target == prop)
00081 }
00082
00083 void SGInterpolator::update(double dt)
00084 {
00085 DELETE_WHERE(interp(p, dt))
00086 }
00087
00088
00089
00090
00091 bool SGInterpolator::interp(Interp* rec, double dt)
00092 {
00093 double val = rec->target->getDoubleValue();
00094 int i;
00095 for(i=0; i < rec->nPoints; i++) {
00096 if(rec->dt(i) > 0 && dt < rec->dt(i)) {
00097 val += (dt / rec->dt(i)) * (rec->val(i) - val);
00098 rec->dt(i) -= dt;
00099 break;
00100 }
00101 dt -= rec->dt(i);
00102 val = rec->val(i);
00103 }
00104 rec->target->setDoubleValue(val);
00105
00106
00107 return i == rec->nPoints;
00108 }