/brlcad/tags/rel-7-16-10/src/tab/tabinterp.c
C | 1240 lines | 876 code | 153 blank | 211 comment | 181 complexity | 5e90570afa0a4b0b3b00cea1f636527f MD5 | raw file
Possible License(s): GPL-2.0, LGPL-2.0, LGPL-2.1, Apache-2.0, AGPL-3.0, LGPL-3.0, GPL-3.0, MPL-2.0-no-copyleft-exception, CC-BY-SA-3.0, 0BSD, BSD-3-Clause
- /* T A B I N T E R P . C
- * BRL-CAD
- *
- * Copyright (c) 1988-2010 United States Government as represented by
- * the U.S. Army Research Laboratory.
- *
- * This program is free software; you can redistribute it and/or
- * modify it under the terms of the GNU Lesser General Public License
- * version 2.1 as published by the Free Software Foundation.
- *
- * This program is distributed in the hope that it will be useful, but
- * WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
- * Lesser General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public
- * License along with this file; see the file named COPYING for more
- * information.
- */
- /** @file tabinterp.c
- *
- * This program is the crucial middle step in the key-frame animation
- * software.
- *
- * First, one or more files, on different time scales, are read into
- * internal "channels", with FILE and RATE commands.
- *
- * Next, the TIMES command is given.
- *
- * Next, a section of those times is interpolated, and multi-channel
- * output is produced, on uniform time samples.
- *
- * This multi-channel output is fed to the next stage to generate
- * control scripts, etc.
- *
- */
- #include "common.h"
- #include <stdlib.h>
- #include <math.h>
- #include <string.h>
- #include "bio.h"
- #include "bu.h"
- #include "vmath.h"
- #include "bn.h"
- #include "raytrace.h"
- struct chan {
- /* INPUTS */
- int c_ilen; /* length of input array */
- char *c_itag; /* description of input source */
- fastf_t *c_itime; /* input time array */
- fastf_t *c_ival; /* input value array */
- /* OUTPUTS */
- fastf_t *c_oval; /* output value array */
- /* FLAGS */
- int c_interp; /* linear or spline? */
- #define INTERP_STEP 1
- #define INTERP_LINEAR 2
- #define INTERP_SPLINE 3
- #define INTERP_RATE 4
- #define INTERP_ACCEL 5
- #define INTERP_QUAT 6 /* first chan of 4 that define a quaternion */
- #define INTERP_QUAT2 7 /* an additional quaterion chan (2, 3, 4) */
- #define INTERP_NEXT 8 /* method to look forward/backward in time */
- int c_periodic; /* cyclic end conditions? */
- int c_sourcechan; /* index of source chan (QUAT, NEXT) */
- int c_offset; /* source offset (NEXT) */
- };
- extern int bu_optind;
- extern char *bu_optarg;
- int verbose = 1;
- int o_len; /* length of all output arrays */
- fastf_t *o_time; /* pointer to output time array */
- int fps; /* frames/sec of output */
- int nchans; /* number of chan[] elements in use */
- int max_chans; /* current size of chan[] array */
- struct chan *chan; /* ptr to array of chan structs */
- /*
- * Returns -
- * -1 on error
- * 0 if data loaded, and interpolator not yet set.
- * 1 if no data, or interpolator already set (message printed)
- */
- HIDDEN int
- chan_not_loaded_or_specified(int ch)
- {
- if (ch < 0 || ch >= nchans) return -1;
- if (chan[ch].c_ilen <= 0) {
- bu_log("error: attempt to set interpolation type on unallocated channel %d\n", ch);
- return 1;
- }
- if (chan[ch].c_interp > 0) {
- bu_log("error: attempt to modify channel %d which already has interpolation type set\n", ch);
- return 1;
- }
- return 0;
- }
- /*
- * C R E A T E _ C H A N
- */
- HIDDEN int
- create_chan(char *num, int len, char *itag)
- {
- int n;
- n = atoi(num);
- if (n < 0) return -1;
- if (n >= max_chans) {
- int prev = max_chans;
- if (max_chans <= 0) {
- max_chans = 32;
- chan = (struct chan *)bu_calloc(1,
- max_chans * sizeof(struct chan),
- "chan[]");
- } else {
- while (n >= max_chans)
- max_chans *= 2;
- if (verbose) bu_log("reallocating from %d to %d chans\n",
- prev, max_chans);
- chan = (struct chan *)bu_realloc((char *)chan,
- max_chans * sizeof(struct chan),
- "chan[]");
- memset((char *)(&chan[prev]), 0, (max_chans-prev)*sizeof(struct chan));
- }
- }
- /* Allocate and clear channels */
- while (nchans <= n) {
- if (chan[nchans].c_ilen > 0) {
- bu_log("create_chan: internal error\n");
- return -1;
- } else {
- memset((char *)&chan[nchans++], 0, sizeof(struct chan));
- }
- }
- if (verbose) bu_log("chan %d: %s\n", n, itag);
- chan[n].c_ilen = len;
- chan[n].c_itag = bu_strdup(itag);
- chan[n].c_ival = (fastf_t *)bu_malloc(len * sizeof(fastf_t), "c_ival");
- return n;
- }
- /*
- * C M _ F I L E
- */
- HIDDEN int
- cm_file(int argc, char **argv)
- {
- FILE *fp;
- char *file;
- char lbuf[512]; /* temporary label buffer */
- int *cnum;
- int i;
- int line;
- char **iwords;
- int nlines; /* number of lines in input file */
- int nwords; /* number of words on each input line */
- fastf_t *times;
- auto double d;
- int errors = 0;
- struct bu_vls buf; /* unlimited size input line buffer */
- file = argv[1];
- if ((fp = fopen(file, "r")) == NULL) {
- perror(file);
- return 0;
- }
- /* First step, count number of lines in file */
- nlines = 0;
- {
- int c;
- while ((c = fgetc(fp)) != EOF) {
- if (c == '\n')
- nlines++;
- }
- }
- rewind(fp);
- /* Intermediate dynamic memory */
- cnum = (int *)bu_malloc(argc * sizeof(int), "cnum[]");
- nwords = argc - 1;
- iwords = (char **)bu_malloc((nwords+1) * sizeof(char *), "iwords[]");
- bu_vls_init(&buf);
- /* Retained dynamic memory */
- times = (fastf_t *)bu_malloc(nlines * sizeof(fastf_t), "times");
- /* Now, create & allocate memory for each chan */
- for (i = 1; i < nwords; i++) {
- /* See if this column is not wanted */
- if (argv[i+1][0] == '-') {
- cnum[i] = -1;
- continue;
- }
- snprintf(lbuf, 512, "File '%s', Column %d", file, i);
- if ((cnum[i] = create_chan(argv[i+1], nlines, lbuf)) < 0) {
- errors = 1;
- goto out;
- }
- /* Share array of times */
- chan[cnum[i]].c_itime = times;
- }
- for (line=0; line < nlines; line++) {
- char *bp;
- bu_vls_trunc(&buf, 0);
- if (bu_vls_gets(&buf, fp) == -1) break;
- bp = bu_vls_addr(&buf);
- if (bp[0] == '#') {
- line--;
- nlines--;
- for (i = 1; i < nwords; i++) {
- if (cnum[i] < 0) continue;
- chan[cnum[i]].c_ilen--;
- }
- continue;
- }
- i = bu_argv_from_string(iwords, nwords, bp);
- if (i != nwords) {
- bu_log("File '%s', Line %d: expected %d columns, got %d\n",
- file, line, nwords, i);
- while (i < nwords) {
- iwords[i++] = "0.123456789";
- errors++;
- }
- }
- /* Obtain the time from the first column */
- sscanf(iwords[0], "%lf", &d);
- times[line] = d;
- if (line > 0 && times[line-1] > times[line]) {
- bu_log("File '%s', Line %d: time sequence error %g > %g\n",
- file, line, times[line-1], times[line]);
- errors++;
- }
- /* Obtain the desired values from the remaining columns, and
- * assign them to the channels indicated in cnum[]
- */
- for (i=1; i < nwords; i++) {
- if (cnum[i] < 0) continue;
- if (sscanf(iwords[i], "%lf", &d) != 1) {
- bu_log("File '%s', Line %d: scanf failure on '%s'\n",
- file, line, iwords[i]);
- d = 0.0;
- errors++;
- }
- chan[cnum[i]].c_ival[line] = d;
- }
- }
- fclose(fp);
- /* Free intermediate dynamic memory */
- out:
- bu_free((char *)cnum, "cnum[]");
- bu_free((char *)iwords, "iwords[]");
- bu_vls_free(&buf);
- if (errors)
- return -1; /* abort */
- return 0;
- }
- /*
- * P R _ I C H A N S
- *
- * Print input channel values.
- */
- HIDDEN void
- pr_ichan(int ch)
- {
- struct chan *cp;
- int i;
- if (ch < 0 || ch >= nchans) {
- bu_log("pr_ichan(%d) out of range\n", ch);
- return;
- }
- cp = &chan[ch];
- if (cp->c_itag == (char *)0) cp->c_itag = "_no_file_";
- bu_log("--- Channel %d, ilen=%d (%s):\n",
- ch, cp->c_ilen, cp->c_itag);
- for (i=0; i < cp->c_ilen; i++) {
- bu_log(" %g\t%g\n", cp->c_itime[i], cp->c_ival[i]);
- }
- }
- /*
- * C M _ I D U M P
- *
- * Dump the indicated input channels, or all, if none specified.
- */
- HIDDEN int
- cm_idump(int argc, char **argv)
- {
- int ch;
- int i;
- if (argc <= 1) {
- for (ch=0; ch < nchans; ch++) {
- pr_ichan(ch);
- }
- } else {
- for (i = 1; i < argc; i++) {
- pr_ichan(atoi(argv[i]));
- }
- }
- return 0;
- }
- /*
- * O U T P U T
- */
- HIDDEN void
- output()
- {
- int ch;
- struct chan *cp;
- int t;
- if (!o_time) {
- bu_log("times command not given, aborting\n");
- return;
- }
- for (t=0; t < o_len; t++) {
- printf("%g", o_time[t]);
- for (ch=0; ch < nchans; ch++) {
- cp = &chan[ch];
- if (cp->c_ilen <= 0) {
- printf("\t.");
- continue;
- }
- printf("\t%g", cp->c_oval[t]);
- }
- printf("\n");
- }
- }
- /*
- * C M _ T I M E S
- */
- HIDDEN int
- cm_times(int argc, char **argv)
- {
- double a, b;
- int i;
- if (argc < 3)
- bu_exit(1, "Internal error. Unexpected argument count encountered.");
- a = atof(argv[1]);
- b = atof(argv[2]);
- fps = atoi(argv[3]);
- if (a >= b) {
- bu_log("times: %g >= %g\n", a, b);
- return 0;
- }
- if (o_len > 0) {
- bu_log("times: already specified\n");
- return 0; /* ignore */
- }
- o_len = ((b-a) * fps) + 0.999;
- o_len++; /* one final step to reach endpoint */
- o_time = (fastf_t *)bu_malloc(o_len * sizeof(fastf_t), "o_time[]");
- /*
- * Don't use an incremental algorithm, to avoid acrueing error
- */
- for (i=0; i<o_len; i++)
- o_time[i] = a + ((double)i)/fps;
- return 0;
- }
- /*
- * C M _ I N T E R P
- */
- HIDDEN int
- cm_interp(int argc, char **argv)
- {
- int interp = 0;
- int periodic = 0;
- int i;
- int ch;
- struct chan *chp;
- if (strcmp(argv[1], "step") == 0) {
- interp = INTERP_STEP;
- periodic = 0;
- } else if (strcmp(argv[1], "cstep") == 0) {
- interp = INTERP_STEP;
- periodic = 1;
- } else if (strcmp(argv[1], "linear") == 0) {
- interp = INTERP_LINEAR;
- periodic = 0;
- } else if (strcmp(argv[1], "clinear") == 0) {
- interp = INTERP_LINEAR;
- periodic = 1;
- } else if (strcmp(argv[1], "spline") == 0) {
- interp = INTERP_SPLINE;
- periodic = 0;
- } else if (strcmp(argv[1], "cspline") == 0) {
- interp = INTERP_SPLINE;
- periodic = 1;
- } else if (strcmp(argv[1], "quat") == 0) {
- interp = INTERP_QUAT;
- periodic = 0;
- } else {
- bu_log("interpolation type '%s' unknown\n", argv[1]);
- interp = INTERP_LINEAR;
- }
- for (i = 2; i < argc; i++) {
- ch = atoi(argv[i]);
- chp = &chan[ch];
- if (chan_not_loaded_or_specified(ch)) continue;
- chp->c_interp = interp;
- chp->c_periodic = periodic;
- if (interp == INTERP_QUAT) {
- int j;
- for (j = 1; j < 4; j++) {
- chp = &chan[ch+j];
- if (chan_not_loaded_or_specified(ch+j)) continue;
- chp->c_interp = INTERP_QUAT2;
- chp->c_periodic = periodic;
- chp->c_sourcechan = ch;
- }
- }
- }
- return 0;
- }
- /*
- * N E X T _ I N T E R P O L A T E
- */
- HIDDEN void
- next_interpolate(struct chan *chp)
- {
- int t; /* output time index */
- int i; /* input time index */
- struct chan *ip;
- ip = &chan[chp->c_sourcechan];
- for (t=0; t<o_len; t++) {
- i = t + chp->c_offset;
- if (i <= 0) {
- chp->c_oval[t] = ip->c_oval[0];
- continue;
- }
- if (i >= o_len) {
- chp->c_oval[t] = ip->c_oval[o_len-1];
- continue;
- }
- chp->c_oval[t] = ip->c_oval[i];
- }
- }
- /*
- * S T E P _ I N T E R P O L A T E
- *
- * Simply select the value at the beinning of the interval. This
- * allows parameters to take instantaneous jumps in value at specified
- * times.
- *
- * This routine takes advantage of (and depends on) the fact that the
- * input and output is sorted in increasing time values.
- */
- HIDDEN void
- step_interpolate(struct chan *chp, fastf_t *times)
- {
- int t; /* output time index */
- int i; /* input time index */
- i = 0;
- for (t=0; t<o_len; t++) {
- /* Check for below initial time */
- if (times[t] < chp->c_itime[0]) {
- chp->c_oval[t] = chp->c_ival[0];
- continue;
- }
- /* Find time range in input data. */
- while (i < chp->c_ilen-1) {
- if (times[t] >= chp->c_itime[i] &&
- times[t] < chp->c_itime[i+1])
- break;
- i++;
- }
- /* Check for above final time */
- if (i >= chp->c_ilen-1) {
- chp->c_oval[t] = chp->c_ival[chp->c_ilen-1];
- continue;
- }
- /* Select value at beginning of interval */
- chp->c_oval[t] = chp->c_ival[i];
- }
- }
- /*
- * L I N E A R _ I N T E R P O L A T E
- *
- * This routine takes advantage of (and depends on) the fact that the
- * input and output arrays are sorted in increasing time values.
- */
- HIDDEN void
- linear_interpolate(struct chan *chp, fastf_t *times)
- {
- int t; /* output time index */
- int i; /* input time index */
- if (chp->c_ilen < 2) {
- bu_log("lienar_interpolate: need at least 2 points\n");
- return;
- }
- i = 0;
- for (t=0; t<o_len; t++) {
- /* Check for below initial time */
- if (times[t] < chp->c_itime[0]) {
- chp->c_oval[t] = chp->c_ival[0];
- continue;
- }
- /* Find time range in input data. */
- while (i < chp->c_ilen-1) {
- if (times[t] >= chp->c_itime[i] &&
- times[t] < chp->c_itime[i+1])
- break;
- i++;
- }
- /* Check for above final time */
- if (i >= chp->c_ilen-1) {
- chp->c_oval[t] = chp->c_ival[chp->c_ilen-1];
- continue;
- }
- /* Perform actual interpolation */
- chp->c_oval[t] = chp->c_ival[i] +
- (times[t] - chp->c_itime[i]) *
- (chp->c_ival[i+1] - chp->c_ival[i]) /
- (chp->c_itime[i+1] - chp->c_itime[i]);
- }
- }
- /*
- * R A T E _ I N T E R P O L A T E
- *
- * The one (and only) input value is interpreted as rate, in
- * unspecified units per second. This is really just a hack to allow
- * multiplying the time by a constant.
- */
- HIDDEN void
- rate_interpolate(struct chan *chp, fastf_t *times)
- {
- int t; /* output time index */
- double ival;
- double rate;
- times = times; /* quell warning */
- if (chp->c_ilen != 2) {
- bu_log("rate_interpolate: only 2 points (ival & rate) may be specified\n");
- return;
- }
- ival = chp->c_ival[0];
- rate = chp->c_ival[1];
- for (t=0; t < o_len; t++) {
- chp->c_oval[t] = ival + rate * t;
- }
- }
- /*
- * A C C E L _ I N T E R P O L A T E
- *
- */
- HIDDEN void
- accel_interpolate(struct chan *chp, fastf_t *times)
- {
- int t; /* output time index */
- double ival;
- double mul;
- double scale;
- times = times; /* quell warning */
- if (chp->c_ilen != 2) {
- bu_log("accel_interpolate: only 2 points (ival & mul) may be specified\n");
- return;
- }
- ival = chp->c_ival[0];
- mul = chp->c_ival[1];
- /* scale ^ fps = mul */
- scale = exp(log(mul) / fps);
- chp->c_oval[0] = ival;
- for (t=1; t < o_len; t++) {
- chp->c_oval[t] = chp->c_oval[t-1] * scale;
- }
- }
- /*
- * Q U A T _ I N T E R P O L A T E
- *
- * Do linear interpolation for first and last span. Use Bezier
- * interpolation for all the rest.
- *
- * This routine depends on the four input channels having identical
- * time stamps, because only the "x" input times are used.
- */
- HIDDEN void
- quat_interpolate(struct chan *x, struct chan *y, struct chan *z, struct chan *w, fastf_t *times)
- {
- int t; /* output time index */
- int i; /* input time index */
- #define QIGET(_q, _it) QSET((_q), x->c_ival[(_it)], y->c_ival[(_it)], z->c_ival[(_it)], w->c_ival[(_it)]);
- #define QPUT(_q) { x->c_oval[t] = (_q)[X]; y->c_oval[t] = (_q)[Y]; \
- z->c_oval[t] = (_q)[Z]; w->c_oval[t] = (_q)[W]; }
- i = 0;
- for (t=0; t<o_len; t++) {
- fastf_t now = times[t];
- /* Check for below initial time */
- if (now <= x->c_itime[0]) {
- quat_t q1;
- QIGET(q1, 0);
- QUNITIZE(q1);
- QPUT(q1);
- continue;
- }
- /* Find time range in input data. */
- while (i < x->c_ilen-1) {
- if (now >= x->c_itime[i] &&
- now < x->c_itime[i+1])
- break;
- i++;
- }
- /* Check for above final time */
- if (i >= x->c_ilen-1) {
- quat_t q1;
- i = x->c_ilen-1;
- QIGET(q1, i);
- QUNITIZE(q1);
- QPUT(q1);
- continue;
- }
- /* Check for being in first or last time span */
- if (i == 0 || i >= x->c_ilen-2)
- {
- fastf_t f;
- quat_t qout, q1, q2, q3, qtemp1, qtemp2, qtemp3;
- f = (now - x->c_itime[i]) /
- (x->c_itime[i+1] - x->c_itime[i]);
- if (i==0)
- {
- QIGET(q1, i);
- QIGET(q2, i+1);
- QIGET(q3, i+2);
- QUNITIZE(q1);
- QUNITIZE(q2);
- QUNITIZE(q3);
- quat_make_nearest(q2, q1);
- quat_make_nearest(q3, q2);
- }
- else
- {
- QIGET(q1, i+1);
- QIGET(q2, i);
- QIGET(q3, i-1);
- f = 1.0 - f;
- QUNITIZE(q1);
- QUNITIZE(q2);
- QUNITIZE(q3);
- quat_make_nearest(q2, q3);
- quat_make_nearest(q1, q2);
- }
- /* find middle control point */
- quat_slerp(qtemp1, q3, q2, 2.0);
- quat_slerp(qtemp2, qtemp1, q1, 0.5);
- quat_slerp(qtemp3, q2, qtemp2, 0.33333);
- /* do 3-point bezier interpolation */
- quat_slerp(qtemp1, q1, qtemp3, f);
- quat_slerp(qtemp2, qtemp3, q2, f);
- quat_slerp(qout, qtemp1, qtemp2, f);
- QPUT(qout);
- continue;
- }
- /* In an intermediate time span */
- {
- fastf_t f;
- quat_t qout, q1, q2, q3, q4, qa, qb, qtemp1, qtemp2;
- f = (now - x->c_itime[i]) /
- (x->c_itime[i+1] - x->c_itime[i]);
- QIGET(q1, i-1);
- QIGET(q2, i);
- QIGET(q3, i+1);
- QIGET(q4, i+2);
- QUNITIZE(q1);
- QUNITIZE(q2);
- QUNITIZE(q3);
- QUNITIZE(q4);
- quat_make_nearest(q2, q1);
- quat_make_nearest(q3, q2);
- quat_make_nearest(q4, q3);
- /* find two middle control points */
- quat_slerp(qtemp1, q1, q2, 2.0);
- quat_slerp(qtemp2, qtemp1, q3, 0.5);
- quat_slerp(qa, q2, qtemp2, 0.333333);
- quat_slerp(qtemp1, q4, q3, 2.0);
- quat_slerp(qtemp2, qtemp1, q2, 0.5);
- quat_slerp(qb, q3, qtemp2, 0.333333);
- quat_sberp(qout, q2, qa, qb, q3, f);
- QPUT(qout);
- }
- }
- }
- /*
- * S P L I N E
- *
- * Fit an interpolating spline to the data points given. Time in the
- * independent (x) variable, and the single channel of data values is
- * the dependent (y) variable.
- *
- * Returns -
- * 0 bad
- * 1 OK
- */
- HIDDEN int
- spline(struct chan *chp, fastf_t *times)
- {
- double d, s;
- double u = 0;
- double v = 0;
- double hi; /* horiz interval i-1 to i */
- double hi1; /* horiz interval i to i+1 */
- double D2yi; /* D2 of y[i] */
- double D2yi1; /* D2 of y[i+1] */
- double D2yn1; /* D2 of y[n-1] (last point) */
- double a;
- int end;
- double corr;
- double konst = 0.0; /* derriv. at endpts, non-periodic */
- double *diag = (double *)0;
- double *rrr = (double *)0;
- int i;
- int t;
- if (chp->c_ilen<3) {
- bu_log("spline(%s): need at least 3 points\n", chp->c_itag);
- goto bad;
- }
- /* First, as a quick hack, do linear interpolation to fill in
- * values off the endpoints, in non-periodic case
- */
- if (chp->c_periodic == 0)
- linear_interpolate(chp, times);
- if (chp->c_periodic
- && !NEAR_ZERO(chp->c_ival[0] - chp->c_ival[chp->c_ilen-1], SMALL_FASTF))
- {
- bu_log("spline(%s): endpoints don't match, replacing final data value\n", chp->c_itag);
- chp->c_ival[chp->c_ilen-1] = chp->c_ival[0];
- }
- i = (chp->c_ilen+1)*sizeof(double);
- diag = (double *)bu_malloc((unsigned)i, "diag");
- rrr = (double *)bu_malloc((unsigned)i, "rrr");
- if (!rrr || !diag) {
- bu_log("spline: malloc failure\n");
- goto bad;
- }
- if (chp->c_periodic) konst = 0;
- d = 1;
- rrr[0] = 0;
- s = chp->c_periodic?-1:0;
- /* triangularize */
- for (i=0; ++i < chp->c_ilen - !chp->c_periodic;) {
- double rhs;
- hi = chp->c_itime[i]-chp->c_itime[i-1];
- hi1 = (i==chp->c_ilen-1) ?
- chp->c_itime[1] - chp->c_itime[0] :
- chp->c_itime[i+1] - chp->c_itime[i];
- if (hi1*hi<=0) {
- bu_log(
- "spline: Horiz. interval changed sign at i=%d, time=%g\n",
- i, chp->c_itime[i]);
- goto bad;
- }
- if (i <= 1) {
- u = v = 0.0; /* First time through */
- } else {
- u = u - s * s / d;
- v = v - s * rrr[i-1] / d;
- }
- rhs = (i==chp->c_ilen-1) ?
- (chp->c_ival[1] - chp->c_ival[0]) /
- (chp->c_itime[1] - chp->c_itime[0]) :
- (chp->c_ival[i+1] - chp->c_ival[i]) /
- (chp->c_itime[i+1] - chp->c_itime[i]);
- rhs = 6 * (rhs -
- ((chp->c_ival[i] - chp->c_ival[i-1]) /
- (chp->c_itime[i] - chp->c_itime[i-1])));
- rrr[i] = rhs - hi * rrr[i-1] / d;
- s = -hi*s/d;
- a = 2*(hi+hi1);
- if (i==1) a += konst*hi;
- if (i==chp->c_ilen-2) a += konst*hi1;
- diag[i] = d = i==1? a:
- a - hi*hi/d;
- }
- D2yi = D2yn1 = 0;
- /* back substitute */
- for (i = chp->c_ilen - !chp->c_periodic; --i >= 0;) {
- end = i==chp->c_ilen-1;
- /* hi1 is range of time covered in this interval */
- hi1 = end ? chp->c_itime[1] - chp->c_itime[0]:
- chp->c_itime[i+1] - chp->c_itime[i];
- D2yi1 = D2yi;
- if (i>0) {
- hi = chp->c_itime[i]-chp->c_itime[i-1];
- corr = end ? 2*s+u : 0.0;
- D2yi = (end*v+rrr[i]-hi1*D2yi1-s*D2yn1)/
- (diag[i]+corr);
- if (end) D2yn1 = D2yi;
- if (i>1) {
- a = 2*(hi+hi1);
- if (i==1) a += konst*hi;
- if (i==chp->c_ilen-2) a += konst*hi1;
- d = diag[i-1];
- s = -s*d/hi;
- }
- }
- else D2yi = D2yn1;
- if (!chp->c_periodic) {
- if (i==0) D2yi = konst*D2yi1;
- if (i==chp->c_ilen-2) D2yi1 = konst*D2yi;
- }
- if (end) continue;
- /* Sweep downward in times[], looking for times in this span */
- for (t=o_len-1; t>=0; t--) {
- double x0; /* fraction from [i+0] */
- double x1; /* fraction from [i+1] */
- double yy;
- double cur_t;
- cur_t = times[t];
- if (cur_t > chp->c_itime[i+1])
- continue;
- if (cur_t < chp->c_itime[i])
- continue;
- x1 = (cur_t - chp->c_itime[i]) /
- (chp->c_itime[i+1] - chp->c_itime[i]);
- x0 = 1 - x1;
- /* Linear interpolation, with correction */
- yy = D2yi * (x0 - x0*x0*x0) + D2yi1 * (x1 - x1*x1*x1);
- yy = chp->c_ival[i] * x0 + chp->c_ival[i+1] * x1 -
- hi1 * hi1 * yy / 6;
- chp->c_oval[t] = yy;
- }
- }
- bu_free((char *)diag, "diag");
- bu_free((char *)rrr, "rrr");
- return 1;
- bad:
- if (diag) bu_free((char *)diag, "diag");
- if (rrr) bu_free((char *)rrr, "rrr");
- return 0;
- }
- /*
- * G O
- *
- * Perform the requested interpolation on each channel
- */
- HIDDEN void
- go()
- {
- int ch;
- struct chan *chp;
- fastf_t *times;
- int t;
- if (!o_time) {
- bu_log("times command not given\n");
- return;
- }
- times = (fastf_t *)bu_malloc(o_len*sizeof(fastf_t), "periodic times");
- /* First, get memory for all output channels */
- for (ch=0; ch < nchans; ch++) {
- chp = &chan[ch];
- if (chp->c_ilen <= 0)
- continue;
- /* Allocate memory for all the output values */
- chan[ch].c_oval = (fastf_t *)bu_malloc(
- o_len * sizeof(fastf_t), "c_oval[]");
- }
- /* Interpolate values for all "interp" channels */
- for (ch=0; ch < nchans; ch++) {
- chp = &chan[ch];
- if (chp->c_ilen <= 0)
- continue;
- if (chp->c_interp == INTERP_NEXT) continue;
- /* As a service to interpolators, if this is a periodic
- * interpolation, build the mapped time array.
- */
- if (chp->c_periodic) {
- for (t=0; t < o_len; t++) {
- double cur_t;
- cur_t = o_time[t];
- while (cur_t > chp->c_itime[chp->c_ilen-1]) {
- cur_t -= (chp->c_itime[chp->c_ilen-1] -
- chp->c_itime[0]);
- }
- while (cur_t < chp->c_itime[0]) {
- cur_t += (chp->c_itime[chp->c_ilen-1] -
- chp->c_itime[0]);
- }
- times[t] = cur_t;
- }
- } else {
- for (t=0; t < o_len; t++) {
- times[t] = o_time[t];
- }
- }
- again:
- switch (chp->c_interp) {
- default:
- bu_log("channel %d: unknown interpolation type %d\n", ch, chp->c_interp);
- break;
- case INTERP_LINEAR:
- linear_interpolate(chp, times);
- break;
- case INTERP_STEP:
- step_interpolate(chp, times);
- break;
- case INTERP_SPLINE:
- if (spline(chp, times) <= 0) {
- bu_log("spline failure, switching to linear\n");
- chp->c_interp = INTERP_LINEAR;
- goto again;
- }
- break;
- case INTERP_RATE:
- rate_interpolate(chp, times);
- break;
- case INTERP_ACCEL:
- accel_interpolate(chp, times);
- break;
- case INTERP_QUAT:
- quat_interpolate(chp, &chan[ch+1], &chan[ch+2], &chan[ch+3], times);
- break;
- case INTERP_QUAT2:
- /* Don't touch these here, handled above */
- continue;
- }
- }
- /* Copy out values for all "next" channels */
- for (ch=0; ch < nchans; ch++) {
- chp = &chan[ch];
- if (chp->c_ilen <= 0)
- continue;
- if (chp->c_interp != INTERP_NEXT) continue;
- next_interpolate(chp);
- }
- bu_free((char *)times, "loc times");
- }
- /*
- * C M _ R A T E
- *
- * Just to communiate with the "interpolator", use two input values.
- * First is initial value, second is change PER SECOND. Input time
- * values are meaningless.
- */
- HIDDEN int
- cm_rate(int argc, char **argv)
- {
- struct chan *chp;
- int ch;
- int nvals = 2;
- ch = create_chan(argv[1], nvals, argc>4?argv[4]:"rate chan");
- chp = &chan[ch];
- chp->c_interp = INTERP_RATE;
- chp->c_periodic = 0;
- chp->c_itime = (fastf_t *)bu_malloc(nvals * sizeof(fastf_t), "rate times");
- chp->c_itime[0] = chp->c_itime[1] = 0;
- chp->c_ival[0] = atof(argv[2]);
- chp->c_ival[1] = atof(argv[3]);
- return 0;
- }
- /*
- * C M _ A C C E L
- *
- * Just to communiate with the "interpolator", use two input values.
- * First is initial value, second is change PER SECOND. Input time
- * values are meaningless.
- */
- HIDDEN int
- cm_accel(int argc, char **argv)
- {
- struct chan *chp;
- int ch;
- int nvals = 2;
- ch = create_chan(argv[1], nvals, argc>4?argv[4]:"accel chan");
- chp = &chan[ch];
- chp->c_interp = INTERP_ACCEL;
- chp->c_periodic = 0;
- chp->c_itime = (fastf_t *)bu_malloc(nvals * sizeof(fastf_t), "accel times");
- chp->c_itime[0] = chp->c_itime[1] = 0;
- chp->c_ival[0] = atof(argv[2]);
- chp->c_ival[1] = atof(argv[3]);
- return 0;
- }
- HIDDEN int
- cm_next(int argc, char **argv)
- {
- int ochan, ichan;
- int offset = 1;
- char buf[128];
- ochan = atoi(argv[1]);
- ichan = atoi(argv[2]);
- if (argc > 3) offset = atoi(argv[3]);
- /* If input channel not loaded, or not interpolated, error */
- if (chan[ichan].c_ilen <= 0 || chan[ichan].c_interp <= 0) {
- bu_log("ERROR next: ichan %d not loaded yet\n", ichan);
- return 0;
- }
- /* If output channel is loaded, error */
- if (chan[ochan].c_ilen > 0) {
- bu_log("ERROR next: ochan %d previous loaded\n", ochan);
- return 0;
- }
- sprintf(buf, "next: value of chan %d [%d]", ichan, offset);
- if (create_chan(argv[1], chan[ichan].c_ilen, buf) < 0) {
- bu_log("ERROR next: uanble to create output channel\n");
- return 0;
- }
- /* c_ilen, c_itag, c_ival are now initialized */
- chan[ochan].c_interp = INTERP_NEXT;
- chan[ochan].c_sourcechan = ichan;
- chan[ochan].c_offset = offset;
- chan[ochan].c_periodic = 0;
- chan[ochan].c_itime = chan[ichan].c_itime; /* share time array */
- /* c_ival[] will not be loaded with anything */
- return 0;
- }
- #define OPT_STR "q"
- HIDDEN int
- get_args(int argc, char **argv)
- {
- int c;
- while ((c=bu_getopt(argc, argv, OPT_STR)) != EOF) {
- switch (c) {
- case 'q':
- verbose = 0;
- break;
- default:
- fprintf(stderr, "Unknown option: -%c\n", c);
- return 0;
- }
- }
- return 1;
- }
- HIDDEN int cm_help(int argc, char **argv);
- struct command_tab cmdtab[] = {
- {"file", "filename chan_num(s)", "load channels from file",
- cm_file, 3, 999},
- {"times", "start stop fps", "specify time range and fps rate",
- cm_times, 4, 4},
- {"interp", "{step|linear|spline|cspline|quat} chan_num(s)", "set interpolation type",
- cm_interp, 3, 999},
- {"next", "dest_chan src_chan [+/- #nsamp]", "lookahead in time",
- cm_next, 3, 4},
- {"idump", "[chan_num(s)]", "dump input channel values",
- cm_idump, 1, 999},
- {"rate", "chan_num init_value incr_per_sec [comment]", "create rate based channel",
- cm_rate, 4, 5},
- {"accel", "chan_num init_value mult_per_sec [comment]", "create acceleration based channel",
- cm_accel, 4, 5},
- {"help", "", "print help message",
- cm_help, 1, 999},
- {(char *)0, (char *)0, (char *)0,
- 0, 0, 0} /* END */
- };
- /*
- * XXX this really should go in librt/cmd.c as rt_help_cmd().
- */
- HIDDEN int
- cm_help(int argc, char **argv)
- {
- struct command_tab *ctp;
- if (argc <= 1) {
- bu_log("The following commands are available:\n\n");
- for (ctp = cmdtab; ctp->ct_cmd != (char *)0; ctp++) {
- bu_log("%s %s\n\t%s\n",
- ctp->ct_cmd, ctp->ct_parms,
- ctp->ct_comment);
- }
- return 0;
- }
- bu_log("%s: Detailed help is not yet available.\n", argv[0]);
- return -1;
- }
- /*
- * M A I N
- */
- int
- main(int argc, char *argv[])
- {
- char *buf;
- int ret;
- get_args(argc, argv);
- /*
- * All the work happens in the functions called by rt_do_cmd().
- * NOTE that the value of MAXWORDS in rt_do_cmd() limits the
- * maximum number of columns that a 'file' command can list.
- */
- while ((buf = rt_read_cmd(stdin)) != (char *)0) {
- if (verbose) bu_log("cmd: %s\n", buf);
- ret = rt_do_cmd(0, buf, cmdtab);
- bu_free(buf, "cmd buf");
- if (ret < 0) {
- if (verbose) bu_log("aborting\n");
- bu_exit(1, NULL);
- }
- }
- if (verbose) bu_log("performing interpolations\n");
- go();
- if (verbose) bu_log("writing output\n");
- output();
- bu_exit(0, NULL);
- }
- /*
- * Local Variables:
- * mode: C
- * tab-width: 8
- * indent-tabs-mode: t
- * c-file-style: "stroustrup"
- * End:
- * ex: shiftwidth=4 tabstop=8
- */