Fonction de calcul
fr


curve_c

Contenu du fichier


#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_ */