comparison model/integrate.c @ 0:5242703e91d3 tip

Initial checkin for AIM92 aimR8.2 (last updated May 1997).

+/*
+    integrate.c
+    ===========
+
+*/
+
+#include <math.h>
+
+#include "stitch.h"
+#include "source.h"
+#include "calc.h"
+
+#include "integrate.h"
+
+
+struct _integral_state { ScalarType state, upward, downward, igain ; } ;
+
+static int new_integral_callback( state, buffer, end, input, skip )
+struct _integral_state *state ;
+DataType *buffer, *end, *input ;
+int skip ;
+{
+    register struct _integral_state *sptr = state ;
+    register DataType *iptr = input ;
+    register DataType *optr = buffer ;
+    register DataType *eptr = end ;
+
+    while( optr < eptr ) {
+
+	    /* the more accurate later version */
+
+	    *optr = DESCALE( sptr->state ) ;
+
+	    sptr->state += sptr->upward * DESCALE( *iptr * sptr->igain - sptr->state ) ;
+
+	    iptr += skip ;
+	    optr += skip ;
+    }
+
+    return ( sizeof ( DataType ) ) ;
+}
+
+static int old_integral_callback( state, buffer, end, input, skip )
+struct _integral_state *state ;
+DataType *buffer, *end, *input ;
+int skip ;
+{
+    register struct _integral_state *sptr = state ;
+    register DataType *iptr = input ;
+    register DataType *optr = buffer ;
+    register DataType *eptr = end ;
+
+    while( optr < eptr ) {
+
+	    /* the "interesting" one */
+
+	    *optr = sptr->state ;
+	    sptr->state += DESCALE( sptr->upward * ( DESCALE( *iptr * sptr->igain ) - sptr->state ) ) ;
+
+	    iptr += skip ;
+	    optr += skip ;
+    }
+
+    return ( sizeof ( DataType ) ) ;
+}
+
+static void integral_close( state )
+struct _integral_state *state ;
+{
+    Delete( state ) ;
+
+    return ;
+}
+
+Source LowpassDataTypeSource( source, stages, channels, upward, downward, igain )
+Source source ;
+int stages, channels ;
+double upward, downward, igain ;
+{
+    struct _integral_state **states ;
+    int stage, chan ;
+
+    for( stage=0 ; stage < abs( stages ) ; stage++ ) {
+
+	states = NewArray( struct _integral_state *, channels, "for states" ) ;
+
+	for( chan=0 ; chan<channels ; chan++ ) {
+
+	    states[chan] = New( struct _integral_state * ) ;
+
+	    states[chan]->state    = 0 ;
+	    states[chan]->upward   = SCALAR( 1. - exp( -1. /  upward  ) ) ;
+	    states[chan]->downward = SCALAR( 1. - exp( -1. / downward ) ) ;
+	    states[chan]->igain    = SCALAR(                  igain     ) ;
+
+	}
+
+	if( stages > 0 )
+	    source = NewMultiplexedSource( states, new_integral_callback, integral_close, channels, source, "integrate.c integration" ) ;
+	else
+	    source = NewMultiplexedSource( states, old_integral_callback, integral_close, channels, source, "integrate.c integration" ) ;
+    }
+
+    return ( source ) ;
+}
+
+
+
+
+
+struct _blocksample_state { struct _fillable_source parent ; Source source ; int spacing, channels ; } ;
+
+static Pointer blocksample_callback( state, bytes, buffer )
+struct _blocksample_state *state ;
+ByteCount *bytes ;
+DataType *buffer ;
+{
+    register int last = *bytes == 0 ;
+    register StoreType sum ;
+    register DataType *iptr ;
+    register DataType *input = (DataType *) Pull( state->source, *bytes * state->spacing ) ;
+    register DataType *end   = (DataType *) ( (Pointer) input  + *bytes * state->spacing ) ;
+    int channel ;
+
+    for( channel=0 ; channel<state->channels ; channel++ ) {
+
+	sum=0 ;
+
+	for( iptr=input+channel ; iptr < end ; iptr += state->channels )
+	    sum += *iptr ;
+
+	buffer[channel] = sum / state->spacing ;
+    }
+
+    if( !last )
+	return ( (Pointer) buffer ) ;
+    else
+	return ( DeleteFillableSource( state ) ) ;
+}
+
+Source BlockSampleSource( source, spacing, channels )
+Source source ;
+int spacing, channels ;
+{
+    DeclareNew( struct _blocksample_state *, state ) ;
+
+    state->source   = source ;
+    state->spacing  = spacing ;
+    state->channels = channels ;
+
+    source = SetFillableSource( state, blocksample_callback, "integrate.c block sampling" ) ;
+
+    source = NewSegmentingSource( source, channels * sizeof ( DataType ) ) ;
+
+    return ( source ) ;
+}
+
+
+struct _downsample_state { struct _pullable_source parent ; Source source ; int spacing, channels ; } ;
+
+static Pointer downsample_callback( state, bytes )
+struct _downsample_state *state ;
+ByteCount *bytes ;
+{
+    register int last = *bytes == 0 ;
+    Pointer output = Pull( state->source, *bytes * state->spacing ) + *bytes * state->spacing - state->channels * sizeof ( DataType ) ;
+
+    if( !last )
+	return ( output ) ;
+    else
+	return ( DeletePullableSource( state ) ) ;
+}
+
+Source DownSampleSource( source, spacing, channels )
+Source source ;
+int spacing, channels ;
+{
+    DeclareNew( struct _downsample_state *, state ) ;
+
+    state->source   = source ;
+    state->spacing  = spacing ;
+    state->channels = channels ;
+
+    source = SetPullableSource( state, downsample_callback, "integrate.c down sampling" ) ;
+
+    source = NewSegmentingSource( source, channels * sizeof ( DataType ) ) ;
+
+    return ( source ) ;
+}
+