curve_c_routcos

Computational routine
eng

##
curve_c

### File content

    #include "scicos_block4.h"

    /*    Masoud Najafi, August 2007 */
    /*    Copyright INRIA
     *    Scicos block simulator
     *    Signal builder block
     */

    #if WIN32
    #define NULL    0
    #endif

    #define rpar     block->rpar
    #define nPoints  block->ipar[0]
    #define Order    block->ipar[1]
    #define Periodic block->ipar[2]
    #define T        rpar[nPoints-1]-rpar[0]

    int Myevalhermite(double *t, double *xa, double *xb, double *ya, double *yb, double *da, double *db, double *h, double *dh, double *ddh, double *dddh, int *i);

    void curve_c(scicos_block *block,int flag)
    {
      double t,a,b,c,y1,y2,t1,t2;
      int *ind,i,inow;
      double *y;
      double  d1,d2,h, dh, ddh, dddh;

      switch(flag)
      {
       /* init */
       case 4  : {/* the workspace is used to store discrete counter value */
                  if ((*block->work=scicos_malloc(4*sizeof(int)))==NULL) {
                    set_block_error(-16);
                    return;
                  }
                  ind=*block->work;
    	      ind[0]=nPoints-1;
    	      ind[1]=nPoints;
    	      for (i=0;i<nPoints;i++){
    		if (rpar[i]>=0 ) {
    		  ind[0]=i-1;
    		  ind[1]=i;
    		  break;
    		}
    	      }
    	      ind[0]=-1;
    	      ind[1]=0;
    	      ind[2]=0; /* event index */
    	      ind[3]=0; /* event counter */
    	      return;

                  break;
                 }
       /* event date computation */
      case 1  : {
                  y=GetRealOutPortPtrs(block,1);
                  ind=*block->work;
                  t=get_scicos_time();
    	      if (Periodic==1) {
    		t=t-(ind[3]-1)*T;
    	      }

    	      inow=nPoints-1;
    	      for (i=ind[0];i<nPoints;i++){
    		if (i==-1) continue;
    		if (t<rpar[i]) {
    		  inow=i-1;
    		  if (inow<ind[1]){
    		    ind[1]=inow;
    		  }else{
    		    ind[0]=ind[1];
    		    ind[1]=inow;
    		  }
    		  break;
    		}
    	      }

    	      if (inow<0) {y[0]=0.0;	break;}
    	      if (inow>=nPoints-1) {y[0]=rpar[nPoints*2-1];break;}

    	      if (Order==0) {
    		y[0]=rpar[nPoints+inow];
    		break;
    	      }
    	      if(Order==1) {
    		t1=rpar[inow];
    		t2=rpar[inow+1];
    		y1=rpar[nPoints+inow];
    		y2=rpar[nPoints+inow+1];
    		y[0]=(y2-y1)*(t-t1)/(t2-t1)+y1;
    		break;
    	      }

    	      if((Order==2)&&(nPoints>2)) {
    		t1=rpar[inow];
    		a=rpar[2*nPoints+inow];
    		b=rpar[2*nPoints+inow+nPoints-1];
    		c=rpar[2*nPoints+inow+2*nPoints-2];
    		y[0]=a*(t-t1)*(t-t1)+b*(t-t1)+c;
    		break;
    	      }

    	      if((Order>=3)) {
    		t1=rpar[inow];
    		t2=rpar[inow+1];
    		y1=rpar[nPoints+inow];
    		y2=rpar[nPoints+inow+1];
    		d1=rpar[2*nPoints+inow];
    		d2=rpar[2*nPoints+inow+1];
    		Myevalhermite(&t, &t1,&t2, &y1,&y2, &d1,&d2, &h, &dh, &ddh, &dddh, &inow);
    		y[0]=h;
    		break;
    	      }

                  break;
                 }
       /* event date computation */
      case 3  : {
                  ind=*block->work;

    	      /*---------*/
    	      if ((Order==1)||(Order==0)){
    		i=ind[2];
    		if (i==nPoints-1){
    		  if (Periodic==1) {
    		    i=0;
    		    ind[0]=-1;
    		    ind[1]=0;
    		  }
    		}
    		if (i<nPoints-1) {
    		  block->evout[0]=rpar[i+1]-rpar[i];

    		  ind[2]=i+1;
    		}
    		if (ind[2]==1)  ind[3]++;
    	      }
    	      /*-------------------*/
    	      if (Order>=2){
    		if ( Periodic) {
    		  block->evout[0]=T;
    		}else{
    		  if (ind[3]==0) {
    		    block->evout[0]=T;
    		  }
    		}
    		ind[3]++;
    		ind[0]=-1;
    		ind[1]=0;

    	      }
    	      break;
                 }

        /* finish */
       case 5  : {
                  scicos_free(*block->work); /*free the workspace*/
                  break;
                 }

       default : break;
      }
    }

    int Myevalhermite(double *t, double *xa, double *xb, double *ya, double *yb, double *da, double *db, double *h, double *dh, double *ddh, double *dddh, int *i)
    {
      double tmxa, p, c2, c3, dx;

      /*    if (old_i != *i) {*/
    /*        compute the following Newton form : */
    /*           h(t) = ya + da*(t-xa) + c2*(t-xa)^2 + c3*(t-xa)^2*(t-xb) */
    	dx = 1. / (*xb - *xa);
    	p = (*yb - *ya) * dx;
    	c2 = (p - *da) * dx;
    	c3 = (*db - p + (*da - p)) * (dx * dx);
    	/*	}	 old_i = *i;*/

    /*     eval h(t), h'(t), h"(t) and h"'(t), by a generalised Horner 's scheme */
        tmxa = *t - *xa;
        *h = c2 + c3 * (*t - *xb);
        *dh = *h + c3 * tmxa;
        *ddh = (*dh + c3 * tmxa) * 2.;
        *dddh = c3 * 6.;
        *h = *da + *h * tmxa;
        *dh = *h + *dh * tmxa;
        *h = *ya + *h * tmxa;
        return 0;
    } /* evalhermite_ */