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