/brlcad/tags/rel-7-0-4/src/tab/tabinterp.c
C | 1287 lines | 929 code | 138 blank | 220 comment | 172 complexity | a40cf97a888f9966999ec0b5265d2769 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-2004 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 General Public License as
- * published by the Free Software Foundation; either version 2 of the
- * License, or (at your option) any later version.
- *
- * 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
- * General Public License for more details.
- *
- * You should have received a copy of the GNU 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.
- *
- * Author -
- * Michael John Muuss
- *
- * Source -
- * SECAD/VLD Computing Consortium, Bldg 394
- * The U. S. Army Ballistic Research Laboratory
- * Aberdeen Proving Ground, Maryland 21005-5066
- */
- #ifndef lint
- static const char RCSid[] = "@(#)$Header$ (BRL)";
- #endif
- #include "common.h"
- #include <stdio.h>
- #include <math.h>
- #ifdef HAVE_STRING_H
- #include <string.h>
- #else
- #include <strings.h>
- #endif
- #include "machine.h"
- #include "bu.h"
- #include "vmath.h"
- #include "bn.h"
- #include "raytrace.h"
- #include "../librt/debug.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 optind;
- extern char *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 */
- extern int cm_file();
- extern int cm_times();
- extern int cm_interp();
- extern int cm_idump();
- extern int cm_rate();
- extern int cm_accel();
- extern int cm_next();
- extern int cm_help();
- void linear_interpolate();
- void rate_interpolate();
- void accel_interpolate();
- void go();
- void output();
- void pr_ichan();
- void next_interpolate();
- void step_interpolate();
- void quat_interpolate();
- int get_args(int argc, char **argv);
- int create_chan( char *num, int len, char *itag );
- int chan_not_loaded_or_specified( int ch );
- int spline( struct chan *chp, fastf_t *times );
- 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 */
- };
- /*
- * M A I N
- */
- int
- main( argc, argv )
- int argc;
- char **argv;
- {
- register char *buf;
- register 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");
- exit(1);
- }
- }
- if(verbose) bu_log("performing interpolations\n");
- go();
- if(verbose) bu_log("writing output\n");
- output();
- exit(0);
- }
- /*
- * XXX this really should go in librt/cmd.c as rt_help_cmd().
- */
- int
- cm_help( argc, argv )
- int argc;
- char **argv;
- {
- register 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("Detailed help is not yet available.\n");
- return -1;
- }
- /*
- * C M _ F I L E
- */
- int
- cm_file( argc, argv )
- 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;
- {
- register 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;
- }
- sprintf( lbuf, "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++ ) {
- register 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 = rt_split_cmd( iwords, nwords+1, 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);
- }
- /*
- * C R E A T E _ C H A N
- */
- 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[]" );
- bzero( (char *)(&chan[prev]),
- (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 {
- bzero( (char *)&chan[nchans++], 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 _ I D U M P
- *
- * Dump the indicated input channels, or all, if none specified.
- */
- int
- cm_idump( argc, argv )
- int argc;
- char **argv;
- {
- register int ch;
- register 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);
- }
- /*
- * P R _ I C H A N S
- *
- * Print input channel values.
- */
- void
- pr_ichan( ch )
- register int ch;
- {
- register struct chan *cp;
- register 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]);
- }
- }
- /*
- * O U T P U T
- */
- void
- output()
- {
- register int ch;
- register struct chan *cp;
- register 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
- */
- int
- cm_times( argc, argv )
- int argc;
- char **argv;
- {
- double a, b;
- register int i;
- 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
- */
- int
- cm_interp( argc, argv )
- 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);
- }
- /*
- * G O
- *
- * Perform the requested interpolation on each channel
- */
- void
- go()
- {
- int ch;
- struct chan *chp;
- fastf_t *times;
- register 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++ ) {
- register 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");
- }
- /*
- * N E X T _ I N T E R P O L A T E
- */
- void
- next_interpolate( chp )
- register struct chan *chp;
- {
- register int t; /* output time index */
- register int i; /* input time index */
- register 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.
- */
- void
- step_interpolate( chp, times )
- register struct chan *chp;
- register fastf_t *times;
- {
- register int t; /* output time index */
- register 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.
- */
- void
- linear_interpolate( chp, times )
- register struct chan *chp;
- register fastf_t *times;
- {
- register int t; /* output time index */
- register 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.
- */
- void
- rate_interpolate( chp, times )
- register struct chan *chp;
- register fastf_t *times;
- {
- register int t; /* output time index */
- register double ival;
- register double rate;
- 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 * times[t];
- }
- }
- /*
- * A C C E L _ I N T E R P O L A T E
- *
- */
- void
- accel_interpolate( chp, times )
- register struct chan *chp;
- register fastf_t *times;
- {
- register int t; /* output time index */
- double ival;
- double mul;
- register double scale;
- 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;
- }
- }
- /*
- * 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
- */
- int
- spline( register 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;
- register int i;
- register 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 && chp->c_ival[0] != chp->c_ival[chp->c_ilen-1] ) {
- 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-- ) {
- register double x0; /* fraction from [i+0] */
- register double x1; /* fraction from [i+1] */
- register double yy;
- register 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);
- }
- /*
- * 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.
- */
- int
- cm_rate( argc, argv )
- int argc;
- char **argv;
- {
- register 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.
- */
- int
- cm_accel( argc, argv )
- int argc;
- char **argv;
- {
- register 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);
- }
- /*
- * 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.
- */
- void
- quat_interpolate( x, y, z, w, times )
- struct chan *x, *y, *z, *w;
- register fastf_t *times;
- {
- register int t; /* output time index */
- register 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++ ) {
- register 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 );
- }
- }
- }
- int
- cm_next( argc, argv )
- 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");
- return 0;
- }
- /* If output channel is loaded, error */
- if( chan[ochan].c_ilen > 0 ) {
- bu_log("ERROR next: ochan %d previous loaded\n");
- 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;
- }
- /*
- * Returns -
- * -1 on error
- * 0 if data loaded, and interpolator not yet set.
- * 1 if no data, or interpolator already set (message printed)
- */
- 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;
- }
- #define OPT_STR "q"
- int get_args(int argc, char **argv)
- {
- int c;
- while ( (c=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);
- }
- /*
- * Local Variables:
- * mode: C
- * tab-width: 8
- * c-basic-offset: 4
- * indent-tabs-mode: t
- * End:
- * ex: shiftwidth=4 tabstop=8
- */