00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include <math.h>
00022 #include "cubic.h"
00023
00024
00025 #ifndef __GNUC__
00026 #ifndef __attribute__
00027 #define __attribute__(x)
00028 #endif
00029 #endif
00030
00031
00032 static char __attribute__((unused)) ident[] = "$Id: 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
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
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
00062 retval.d = x0;
00063 retval.c = v0;
00064
00065
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 }
00073
00074
00075
00076
00077 static double interpolateCubic(CUBIC_COEFF coeff, double t)
00078 {
00079 return coeff.a * (t*t*t) + coeff.b * (t*t) + coeff.c * t + coeff.d;
00080 }
00081
00082
00083
00084
00085
00086 static double interpolateVel(CUBIC_COEFF coeff, double t)
00087 {
00088 return 3.0 * coeff.a * (t*t) + 2.0 * coeff.b * t + coeff.c;
00089 }
00090
00091
00092
00093
00094
00095 static double interpolateAccel(CUBIC_COEFF coeff, double t)
00096 {
00097 return 6.0 * coeff.a * t + 2.0 * coeff.b;
00098 }
00099
00100
00101
00102
00103
00104 static double interpolateJerk(CUBIC_COEFF coeff, double t)
00105 {
00106 return 6.0 * coeff.a;
00107 }
00108
00109
00110
00111
00112
00113 static double wayPoint(double xMinus1, double x, double xPlus1)
00114 {
00115 return (xMinus1 + 4.0 * x + xPlus1) / 6.0;
00116 }
00117
00118
00119
00120
00121
00122 static double velPoint(double xMinus1, double xPlus1, double deltaT)
00123 {
00124 if (deltaT <= 0.0)
00125 {
00126 return 0.0;
00127 }
00128 else
00129 {
00130 return (xPlus1 - xMinus1) / (2.0 * deltaT);
00131 }
00132 }
00133
00134 int cubicInit(CUBIC_STRUCT *ci)
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 }
00149
00150 int cubicSetSegmentTime(CUBIC_STRUCT *ci, double time)
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 }
00167
00168 double cubicGetSegmentTime(CUBIC_STRUCT *ci)
00169 {
00170 if (0 == ci ||
00171 ! (ci->configured & SEGMENT_TIME_SET))
00172 {
00173 return 0.0;
00174 }
00175
00176 return ci->segmentTime;
00177 }
00178
00179 int cubicSetInterpolationRate(CUBIC_STRUCT *ci, int rate)
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 }
00196
00197 int cubicGetInterpolationRate(CUBIC_STRUCT *ci)
00198 {
00199 if (0 == ci ||
00200 ! (ci->configured & INTERPOLATION_RATE_SET))
00201 {
00202 return 0;
00203 }
00204
00205 return ci->interpolationRate;
00206 }
00207
00208 double cubicGetInterpolationIncrement(CUBIC_STRUCT *ci)
00209 {
00210 if (0 == ci ||
00211 ci->configured != ALL_SET)
00212 {
00213 return 0;
00214 }
00215
00216 return ci->interpolationIncrement;
00217 }
00218
00219 CUBIC_COEFF cubicGetCubicCoeff(CUBIC_STRUCT *ci)
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 }
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251 int cubicAddPoint(CUBIC_STRUCT *ci, double point)
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
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 }
00292
00293
00294
00295
00296
00297
00298
00299
00300
00301
00302 int cubicOffset(CUBIC_STRUCT *ci, double offset)
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
00318
00319
00320
00321 ci->coeff.d += offset;
00322
00323 return 0;
00324 }
00325
00326 int cubicFilled(CUBIC_STRUCT *ci)
00327 {
00328 if (0 == ci)
00329 {
00330 return 0;
00331 }
00332
00333 return ci->filled;
00334 }
00335
00336 double cubicInterpolate(CUBIC_STRUCT *ci,
00337 double *x,
00338 double *v,
00339 double *a,
00340 double *j)
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
00353 cubicAddPoint(ci, ci->x3);
00354 }
00355
00356 retval = interpolateCubic(ci->coeff, ci->interpolationTime);
00357
00358
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
00379 if (fabs(ci->segmentTime - ci->interpolationTime)
00380 < 0.5 * ci->interpolationIncrement)
00381 {
00382
00383 ci->needNextPoint = 1;
00384 }
00385
00386 return retval;
00387 }
00388
00389 int cubicNeedNextPoint(CUBIC_STRUCT *ci)
00390 {
00391 return ci->needNextPoint;
00392 }
00393
00394 int cubicDrain(CUBIC_STRUCT *ci)
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 }
00408
00409 #ifdef MAIN
00410
00411 #include <stdio.h>
00412
00413
00414
00415
00416 int main(int argc, char *argv[])
00417 {
00418 CUBIC_STRUCT cubic;
00419 double segmentTime;
00420 int interpolationRate;
00421 double xin;
00422 double xout;
00423 double time = 0.0;
00424
00425 if (argc != 3)
00426 {
00427 fprintf(stderr, "syntax: %s <segment time> <interpolation rate>\n", argv[0]);
00428 return 1;
00429 }
00430
00431 if (1 != sscanf(argv[1], "%lf", &segmentTime) ||
00432 segmentTime <= 0.0)
00433 {
00434 fprintf(stderr, "invalid segment time %s\n", argv[1]);
00435 return 1;
00436 }
00437
00438 if (1 != sscanf(argv[2], "%d", &interpolationRate) ||
00439 interpolationRate <= 0)
00440 {
00441 fprintf(stderr, "invalid interpolation rate %s\n", argv[2]);
00442 return 1;
00443 }
00444
00445 if (0 != cubicInit(&cubic))
00446 {
00447 fprintf(stderr, "can't initialize interpolator\n");
00448 return 1;
00449 }
00450
00451 if (0 != cubicSetSegmentTime(&cubic, segmentTime))
00452 {
00453 fprintf(stderr, "can't set segment time\n");
00454 return 1;
00455 }
00456
00457 if (0 != cubicSetInterpolationRate(&cubic, interpolationRate))
00458 {
00459 fprintf(stderr, "can't set interpolation rate\n");
00460 return 1;
00461 }
00462
00463 while (! feof(stdin))
00464 {
00465 if (cubicNeedNextPoint(&cubic))
00466 {
00467 if (1 != scanf("%lf", &xin))
00468 {
00469 break;
00470 }
00471 else
00472 {
00473 cubicAddPoint(&cubic, xin);
00474 }
00475 }
00476
00477 xout = cubicInterpolate(&cubic, 0, 0, 0, 0);
00478 printf("%f %f\n", time, xout);
00479 time += segmentTime;
00480 }
00481 }
00482
00483 #endif