Main Page   Class Hierarchy   Alphabetical List   Data Structures   File List   Data Fields   Globals  

cubic.c File Reference

#include <math.h>
#include "cubic.h"

Include dependency graph for cubic.c:

Include dependency graph

Go to the source code of this file.

Defines

#define SEGMENT_TIME_SET   0x01
#define INTERPOLATION_RATE_SET   0x02
#define ALL_SET   (SEGMENT_TIME_SET | INTERPOLATION_RATE_SET)

Functions

char __attribute__ ((unused)) ident[]="$Id
double interpolateCubic (CUBIC_COEFF coeff, double t)
double interpolateVel (CUBIC_COEFF coeff, double t)
double interpolateAccel (CUBIC_COEFF coeff, double t)
double interpolateJerk (CUBIC_COEFF coeff, double t)
double wayPoint (double xMinus1, double x, double xPlus1)
double velPoint (double xMinus1, double xPlus1, double deltaT)
int cubicInit (CUBIC_STRUCT *ci)
int cubicSetSegmentTime (CUBIC_STRUCT *ci, double time)
double cubicGetSegmentTime (CUBIC_STRUCT *ci)
int cubicSetInterpolationRate (CUBIC_STRUCT *ci, int rate)
int cubicGetInterpolationRate (CUBIC_STRUCT *ci)
double cubicGetInterpolationIncrement (CUBIC_STRUCT *ci)
CUBIC_COEFF cubicGetCubicCoeff (CUBIC_STRUCT *ci)
int cubicAddPoint (CUBIC_STRUCT *ci, double point)
int cubicOffset (CUBIC_STRUCT *ci, double offset)
int cubicFilled (CUBIC_STRUCT *ci)
double cubicInterpolate (CUBIC_STRUCT *ci, double *x, double *v, double *a, double *j)
int cubicNeedNextPoint (CUBIC_STRUCT *ci)
int cubicDrain (CUBIC_STRUCT *ci)

Define Documentation

#define ALL_SET   (SEGMENT_TIME_SET | INTERPOLATION_RATE_SET)
 

#define INTERPOLATION_RATE_SET   0x02
 

#define SEGMENT_TIME_SET   0x01
 

Function Documentation

char __attribute__ (  (unused)    )  [static]
 

Definition at line 32 of file cubic.c.

00032                                                    : cubic.c,v 1.3 2001/07/13 15:08:16 wshackle Exp $";
00033 
00034 #define SEGMENT_TIME_SET 0x01
00035 #define INTERPOLATION_RATE_SET 0x02
00036 #define ALL_SET (SEGMENT_TIME_SET | INTERPOLATION_RATE_SET)
00037 
00038 /*
00039    cubicCoeff calculates the coefficients of the cubic spline
00040    fit to the values of x and v at 0 and t.
00041    deltaT is the length of the interval.
00042    The return value is a CUBIC_COEFF.
00043 
00044    Derivation:
00045 
00046    Solve
00047          a t^3 +  b t^2 + c t + d = x,
00048         3a t^2 + 2b t   + c       = v
00049 
00050    for a, b, c, and d, given
00051 
00052      x(t=0), v(t=0),
00053      x(t=deltaT), v(t=deltaT)
00054 */
00055 static CUBIC_COEFF cubicCoeff(double x0, double v0,
00056                               double xn, double vn,
00057                               double deltaT)
00058 {
00059   CUBIC_COEFF retval;
00060 
00061   /* first the easy ones */
00062   retval.d = x0;
00063   retval.c = v0;
00064 
00065   /* now the hard ones */
00066   retval.b = 3 * (xn - x0) / (deltaT*deltaT) -
00067     (2 * v0 + vn) / deltaT;
00068   retval.a = (vn - v0) / (3.0 * (deltaT*deltaT)) -
00069     (2.0 * retval.b) / (3.0 * deltaT);
00070 
00071   return retval;
00072 }

int cubicAddPoint (  CUBIC_STRUCT *    ci,
double    point
) 
 

Definition at line 251 of file cubic.c.

00252 {
00253   if (0 == ci ||
00254       ! (ci->configured == ALL_SET))
00255     {
00256       return -1;
00257     }
00258 
00259   if (! ci->needNextPoint)
00260     {
00261       return -1;
00262     }
00263 
00264   if (! ci->filled)
00265     {
00266       ci->x0 = point;
00267       ci->x1 = point;
00268       ci->x2 = point;
00269       ci->x3 = point;
00270       ci->filled = 1;
00271     }
00272   else
00273     {
00274       ci->x0 = ci->x1;
00275       ci->x1 = ci->x2;
00276       ci->x2 = ci->x3;
00277       ci->x3 = point;
00278     }
00279 
00280   /* calculate way points and coeff */
00281   ci->wp0 = wayPoint(ci->x0, ci->x1, ci->x2);
00282   ci->wp1 = wayPoint(ci->x1, ci->x2, ci->x3);
00283   ci->velp0 = velPoint(ci->x0, ci->x2, ci->segmentTime);
00284   ci->velp1 = velPoint(ci->x1, ci->x3, ci->segmentTime);
00285   ci->coeff = cubicCoeff(ci->wp0, ci->velp0, ci->wp1,
00286                          ci->velp1, ci->segmentTime);
00287   ci->interpolationTime = 0.0;
00288   ci->needNextPoint = 0;
00289 
00290   return 0;
00291 }

int cubicDrain (  CUBIC_STRUCT *    ci ) 
 

Definition at line 394 of file cubic.c.

00395 {
00396   ci->x0 = ci->x1 = ci->x2 = ci->x3 = 0.0;
00397   ci->wp0 = ci->wp1 = 0.0;
00398   ci->velp0 = ci->velp1 = 0.0;
00399   ci->filled = 0;
00400   ci->needNextPoint = 1;
00401   ci->coeff.a = 0.0;
00402   ci->coeff.b = 0.0;
00403   ci->coeff.c = 0.0;
00404   ci->coeff.d = 0.0;
00405 
00406   return 0;
00407 }

int cubicFilled (  CUBIC_STRUCT *    ci ) 
 

Definition at line 326 of file cubic.c.

00327 {
00328   if (0 == ci)
00329     {
00330       return 0;
00331     }
00332 
00333   return ci->filled;
00334 }

CUBIC_COEFF cubicGetCubicCoeff (  CUBIC_STRUCT *    ci ) 
 

Definition at line 219 of file cubic.c.

00220 {
00221   CUBIC_COEFF errorReturn;
00222 
00223   if (0 == ci ||
00224       ! ci->filled)
00225     {
00226       errorReturn.a = 0.0;
00227       errorReturn.b = 0.0;
00228       errorReturn.c = 0.0;
00229       errorReturn.d = 0.0;
00230 
00231       return errorReturn;
00232     }
00233 
00234   return ci->coeff;
00235 }

double cubicGetInterpolationIncrement (  CUBIC_STRUCT *    ci ) 
 

Definition at line 208 of file cubic.c.

00209 {
00210   if (0 == ci ||
00211       ci->configured != ALL_SET)
00212     {
00213       return 0;
00214     }
00215 
00216   return ci->interpolationIncrement;
00217 }

int cubicGetInterpolationRate (  CUBIC_STRUCT *    ci ) 
 

Definition at line 197 of file cubic.c.

00198 {
00199   if (0 == ci ||
00200       ! (ci->configured & INTERPOLATION_RATE_SET))
00201     {
00202       return 0;
00203     }
00204 
00205   return ci->interpolationRate;
00206 }

double cubicGetSegmentTime (  CUBIC_STRUCT *    ci ) 
 

Definition at line 168 of file cubic.c.

00169 {
00170   if (0 == ci ||
00171       ! (ci->configured & SEGMENT_TIME_SET))
00172     {
00173       return 0.0;
00174     }
00175 
00176   return ci->segmentTime;
00177 }

int cubicInit (  CUBIC_STRUCT *    ci ) 
 

Definition at line 134 of file cubic.c.

00135 {
00136   if (0 == ci)
00137     {
00138       return -1;
00139     }
00140 
00141   ci->configured = 0;
00142   ci->segmentTime = 0.0;
00143   ci->interpolationRate = 0;
00144   ci->interpolationIncrement = 0.0;
00145   cubicDrain(ci);
00146 
00147   return 0;
00148 }

double cubicInterpolate (  CUBIC_STRUCT *    ci,
double *    x,
double *    v,
double *    a,
double *    j
) 
 

Definition at line 336 of file cubic.c.

00341 {
00342   double retval;
00343 
00344   if (0 == ci ||
00345       ! (ci->configured == ALL_SET))
00346     {
00347       return 0.0;
00348     }
00349 
00350   if (ci->needNextPoint)
00351     {
00352       /* queue ran out-- fill right with last point */
00353       cubicAddPoint(ci, ci->x3);
00354     }
00355 
00356   retval = interpolateCubic(ci->coeff, ci->interpolationTime);
00357 
00358   /* do optional ones */
00359   if (x != 0)
00360     {
00361       *x = retval;
00362     }
00363   if (v != 0)
00364     {
00365       *v = interpolateVel(ci->coeff, ci->interpolationTime);
00366     }
00367   if (a != 0)
00368     {
00369       *a = interpolateAccel(ci->coeff, ci->interpolationTime);
00370     }
00371   if (j != 0)
00372     {
00373       *j = interpolateJerk(ci->coeff, ci->interpolationTime);
00374     }
00375 
00376   ci->interpolationTime += ci->interpolationIncrement;
00377 
00378   /* check to see if the next point is at (close to) the segment end */
00379   if (fabs(ci->segmentTime - ci->interpolationTime)
00380       < 0.5 * ci->interpolationIncrement)
00381     {
00382       /* just computed last point-- flag that we need a new one */
00383       ci->needNextPoint = 1;
00384     }
00385 
00386   return retval;
00387 }

int cubicNeedNextPoint (  CUBIC_STRUCT *    ci ) 
 

Definition at line 389 of file cubic.c.

00390 {
00391   return ci->needNextPoint;
00392 }

int cubicOffset (  CUBIC_STRUCT *    ci,
double    offset
) 
 

Definition at line 302 of file cubic.c.

00303 {
00304   if (0 == ci ||
00305       ! (ci->configured == ALL_SET))
00306     {
00307       return -1;
00308     }
00309 
00310   ci->x0 += offset;
00311   ci->x1 += offset;
00312   ci->x2 += offset;
00313   ci->x3 += offset;
00314   ci->wp0 += offset;
00315   ci->wp1 += offset;
00316 
00317   /* leave velp0, velp1 alone, since these are velocity points and
00318      are unaffected by position offsets */
00319 
00320   /* only the D coeff is affected, so we can change this directly */
00321   ci->coeff.d += offset;
00322 
00323   return 0;
00324 }

int cubicSetInterpolationRate (  CUBIC_STRUCT *    ci,
int    rate
) 
 

Definition at line 179 of file cubic.c.

00180 {
00181   if (0 == ci ||
00182       rate <= 0)
00183     {
00184       return -1;
00185     }
00186 
00187   ci->interpolationRate = rate;
00188   ci->configured |= INTERPOLATION_RATE_SET;
00189   if (ci->configured == ALL_SET)
00190     {
00191       ci->interpolationIncrement = ci->segmentTime / ci->interpolationRate;
00192     }
00193 
00194   return 0;
00195 }

int cubicSetSegmentTime (  CUBIC_STRUCT *    ci,
double    time
) 
 

Definition at line 150 of file cubic.c.

00151 {
00152   if (0 == ci ||
00153       time <= 0.0)
00154     {
00155       return -1;
00156     }
00157 
00158   ci->segmentTime = time;
00159   ci->configured |= SEGMENT_TIME_SET;
00160   if (ci->configured == ALL_SET)
00161     {
00162       ci->interpolationIncrement = ci->segmentTime / ci->interpolationRate;
00163     }
00164 
00165   return 0;
00166 }

double interpolateAccel (  CUBIC_COEFF    coeff,
double    t
)  [static]
 

Definition at line 95 of file cubic.c.

Referenced by cubicInterpolate(). 

00096 {
00097   return 6.0 * coeff.a * t + 2.0 * coeff.b;
00098 }

double interpolateCubic (  CUBIC_COEFF    coeff,
double    t
)  [static]
 

Definition at line 77 of file cubic.c.

Referenced by cubicInterpolate(). 

00078 {
00079   return coeff.a * (t*t*t) + coeff.b * (t*t) + coeff.c * t + coeff.d;
00080 }

double interpolateJerk (  CUBIC_COEFF    coeff,
double    t
)  [static]
 

Definition at line 104 of file cubic.c.

Referenced by cubicInterpolate(). 

00105 {
00106   return 6.0 * coeff.a;
00107 }

double interpolateVel (  CUBIC_COEFF    coeff,
double    t
)  [static]
 

Definition at line 86 of file cubic.c.

Referenced by cubicInterpolate(). 

00087 {
00088   return 3.0 * coeff.a * (t*t) + 2.0 * coeff.b * t + coeff.c;
00089 }

double velPoint (  double    xMinus1,
double    xPlus1,
double    deltaT
)  [static]
 

Definition at line 122 of file cubic.c.

Referenced by cubicAddPoint(). 

00123 {
00124   if (deltaT <= 0.0)
00125     {
00126       return 0.0;
00127     }
00128   else
00129     {
00130       return (xPlus1 - xMinus1) / (2.0 * deltaT);
00131     }
00132 }

double wayPoint (  double    xMinus1,
double    x,
double    xPlus1
)  [static]
 

Definition at line 113 of file cubic.c.

Referenced by cubicAddPoint(). 

00114 {
00115   return (xMinus1 + 4.0 * x + xPlus1) / 6.0;
00116 }

Generated on Sun Dec 2 15:27:50 2001 for EMC by doxygen1.2.11.1 written by Dimitri van Heesch, © 1997-2001