/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void lookup2d(scicos_block *block, int flag)
{
double *y = NULL, *u1 = NULL, *u2 = NULL;
double *X = NULL, *Y = NULL, *Z = NULL;
int Nx = block->ipar[0];
int Ny = block->ipar[1];
int method = block->ipar[2];
X = block->rpar;
Y = X + Nx;
Z = Y + Ny;
switch (flag)
{
/* init */
case 4 :
case 1 :
u1 = GetRealInPortPtrs(block, 1);
u2 = GetRealInPortPtrs(block, 2);
y = GetRealOutPortPtrs(block, 1);
y[0] = computeZ2(X, Y, Z, Nx, Ny, method, u1[0], u2[0]);
break;
case 3 :
case 5 :
default :
break;
}
}
/** \fn void cmscope(scicos_block * block,int flag)
\brief the computational function
\param block A pointer to a scicos_block
\param flag An int which indicates the state of the block (init, update, ending)
*/
SCICOS_BLOCKS_IMPEXP void canimxy3d(scicos_block * block, scicos_flag flag)
{
int FigureUID;
sco_data *sco;
int j;
BOOL result;
switch (flag)
{
case Initialization:
sco = getScoData(block);
if (sco == NULL)
{
set_block_error(-5);
break;
}
FigureUID = getFigure(block);
if (FigureUID == 0)
{
// allocation error
set_block_error(-5);
break;
}
break;
case StateUpdate:
FigureUID = getFigure(block);
if (FigureUID == 0)
{
// allocation error
set_block_error(-5);
break;
}
appendData(block, GetRealInPortPtrs(block, 1), GetRealInPortPtrs(block, 2), GetRealInPortPtrs(block, 3));
for (j = 0; j < block->insz[0]; j++)
{
result = pushData(block, j);
if (result == FALSE)
{
Coserror("%s: unable to push some data.", "cscopxy3d");
break;
}
}
break;
case Ending:
freeScoData(block);
break;
default:
break;
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void matzmul_m(scicos_block *block, int flag)
{
int mu1 = GetInPortRows(block, 1);
int mu2 = GetInPortRows(block, 2);
int nu = GetInPortCols(block, 2);
double *u1r = GetRealInPortPtrs(block, 1);
double *u1i = GetImagInPortPtrs(block, 1);
double *u2r = GetRealInPortPtrs(block, 2);
double *u2i = GetImagInPortPtrs(block, 2);
double *yr = GetRealOutPortPtrs(block, 1);
double *yi = GetImagOutPortPtrs(block, 1);
C2F(wmmul)(u1r, u1i, &mu1, u2r, u2i, &mu2, yr, yi, &mu1, &mu1, &mu2, &nu);
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void extractz(scicos_block *block,int flag)
{
double *ur = NULL;
double *ui = NULL;
double *yr = NULL;
double *yi = NULL;
int *r = NULL;
int nu = 0,mu = 0,nr = 0,i = 0,j = 0,ij = 0,k = 0,nc = 0,nl = 0;
mu=GetInPortRows(block,1);
nu=GetInPortCols(block,1);
nr=GetNipar(block);
r=GetIparPtrs(block);
ur=GetRealInPortPtrs(block,1);
ui=GetImagInPortPtrs(block,1);
yr=GetRealOutPortPtrs(block,1);
yi=GetImagOutPortPtrs(block,1);
nc=r[nr-1];
nl=r[nr-2];
k=0;
for (j=0;j<nc;j++)
{for (i=0;i<nl;i++)
{ij=r[i]-1+(r[nl+j]-1)*mu;
*(yr+k)=*(ur+ij);
*(yi+k)=*(ui+ij);
k++;}}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void constraint_c(scicos_block *block, int flag)
{
double *y = NULL, *y2 = NULL, *u = NULL;
int i = 0;
int* property = GetXpropPtrs(block);
switch (flag)
{
case 4 :
{
for (i = 0; i < NX; i++)
{
property[i] = -1; /* xproperties*/
}
break;
}
case 7 :
{
for (i = 0; i < NX; i++)
{
property[i] = block->ipar[i];
}
break;
}
case 0 : /* the workspace is used to store discrete counter value */
{
u = GetRealInPortPtrs(block, 1);
for (i = 0; i < NX; i++)
{
block->res[i] = u[i];
}
break;
}
case 1 :
{
y = GetRealOutPortPtrs(block, 1);
for (i = 0; i < NX; i++)
{
y[i] = block->x[i];
}
if (GetNout(block) == 2)
{
y2 = GetRealOutPortPtrs(block, 2);
for (i = 0; i < NX; i++)
{
y2[i] = block->xd[i];
}
}
break;
}
default :
break;
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void matz_reimc(scicos_block *block,int flag)
{
int i = 0;
int mu = GetOutPortRows(block,1);
int nu = GetOutPortCols(block,1);
double *u1 = GetRealInPortPtrs(block,1);
double *u2 = GetRealInPortPtrs(block,2);
double *y1r = GetRealOutPortPtrs(block,1);
double *y1i = GetImagOutPortPtrs(block,1);
for (i=0;i<mu*nu;i++)
{
*(y1r+i)=*(u1+i);
*(y1i+i)=*(u2+i);
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void mattran_m(scicos_block *block,int flag)
{
int nu = GetInPortRows(block,1);
int mu = GetInPortCols(block,1);
double *u = GetRealInPortPtrs(block,1);
double *y = GetRealOutPortPtrs(block,1);
C2F(mtran)(u,&nu,y,&mu,&nu,&mu);
}
/** \fn void cmatview(scicos_block * block,int flag)
\brief the computational function
\param block A pointer to a scicos_block
\param flag An int which indicates the state of the block (init, update, ending)
*/
SCICOS_BLOCKS_IMPEXP void cmatview(scicos_block * block, scicos_flag flag)
{
char const* pFigureUID;
double *u;
sco_data *sco;
BOOL result;
switch (flag)
{
case Initialization:
sco = getScoData(block);
if (sco == NULL)
{
set_block_error(-5);
break;
}
pFigureUID = getFigure(block);
if (pFigureUID == NULL)
{
// allocation error
set_block_error(-5);
break;
}
break;
case StateUpdate:
pFigureUID = getFigure(block);
if (pFigureUID == NULL)
{
// allocation error
set_block_error(-5);
break;
}
u = GetRealInPortPtrs(block, 1);
result = pushData(block, u);
if (result == FALSE)
{
Coserror("%s: unable to push some data.", "cmatview");
break;
}
break;
case Ending:
freeScoData(block);
break;
default:
break;
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void summation(scicos_block *block, int flag)
{
int j = 0, k = 0;
double *y = GetRealOutPortPtrs(block, 1);
int nu = GetInPortRows(block, 1);
int mu = GetInPortCols(block, 1);
int *ipar = GetIparPtrs(block);
if (flag == 1)
{
if (GetNin(block) == 1)
{
double *u = GetRealInPortPtrs(block, 1);
y[0] = 0.0;
for (j = 0; j < nu * mu; j++)
{
y[0] = y[0] + u[j];
}
}
else
{
for (j = 0; j < nu * mu; j++)
{
y[j] = 0.0;
for (k = 0; k < GetNin(block); k++)
{
double *u = GetRealInPortPtrs(block, k + 1);
if (ipar[k] > 0)
{
y[j] = y[j] + u[j];
}
else
{
y[j] = y[j] - u[j];
}
}
}
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void root_coef(scicos_block *block, int flag)
{
int mu = GetInPortRows(block, 1);
double *u = GetRealInPortPtrs(block, 1);
double *y = GetRealOutPortPtrs(block, 1);
if (flag == 1 || flag == 6)
{
C2F(dprxc)(&mu, u, y);
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void matz_abs(scicos_block *block, int flag)
{
int i = 0;
int mu = GetOutPortRows(block, 1);
int nu = GetOutPortCols(block, 1);
double *u1r = GetRealInPortPtrs(block, 1);
double *u1i = GetImagInPortPtrs(block, 1);
double *y1 = GetRealOutPortPtrs(block, 1);
double *y2 = GetRealOutPortPtrs(block, 2);
for (i = 0; i < mu * nu; i++)
{
*(y1 + i) = pow((pow(*(u1r + i), 2) + pow(*(u1i + i), 2)), 0.5);
*(y2 + i) = atan2(*(u1i + i), *(u1r + i));
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void matz_conj(scicos_block *block,int flag)
{
int i = 0;
int mu = GetOutPortRows(block,1);
int nu = GetOutPortCols(block,1);
double *u1r = GetRealInPortPtrs(block,1);
double *u1i = GetImagInPortPtrs(block,1);
double *yr = GetRealOutPortPtrs(block,1);
double *yi = GetImagOutPortPtrs(block,1);
for (i=0;i<mu*nu;i++)
{
*(yr+i)=*(u1r+i);
*(yi+i)=-(*(u1i+i));
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void cumsumz_m(scicos_block *block,int flag)
{
double *ur = NULL;
double *ui = NULL;
double *yr = NULL;
double *yi = NULL;
int nu = 0,mu = 0,j = 0;
mu=GetInPortRows(block,1);
nu=GetInPortCols(block,1);
ur=GetRealInPortPtrs(block,1);
ui=GetImagInPortPtrs(block,1);
yr=GetRealOutPortPtrs(block,1);
yi=GetImagOutPortPtrs(block,1);
yr[0]=ur[0];
yi[0]=ui[0];
for(j=1;j<mu*nu;j++)
{
yr[j]=ur[j]+yr[j-1];
yi[j]=ui[j]+yi[j-1];
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void mat_suml(scicos_block *block, int flag)
{
int j = 0;
int mu = GetInPortRows(block, 1);
int nu = GetInPortCols(block, 1);
double *u = GetRealInPortPtrs(block, 1);
double *y = GetRealOutPortPtrs(block, 1);
for (j = 0; j < mu; j++)
{
double d = 0.;
int i = 0;
for (i = 0; i < nu; i++)
{
int ij = j + i * mu;
d += *(u + ij);
}
*(y + j) = d;
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void matz_sqrt(scicos_block *block,int flag)
{
if (flag==1)
{
int i = 0;
int mu = GetInPortRows(block,1);
int nu = GetInPortCols(block,1);
double *ur = GetRealInPortPtrs(block,1);
double *ui = GetImagInPortPtrs(block,1);
double *yr = GetRealOutPortPtrs(block,1);
double *yi = GetImagOutPortPtrs(block,1);
for(i=0;i<mu*nu;i++)
{
double inpr = ur[i];
double inpi = ui[i];
C2F(wsqrt)(&inpr,&inpi,&yr[i],&yi[i]);
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void exttriu(scicos_block *block, int flag)
{
double *u = NULL;
double *y = NULL;
int nu = 0, mu = 0, i = 0, j = 0, ij = 0;
mu = GetInPortRows(block, 1);
nu = GetInPortCols(block, 1);
u = GetRealInPortPtrs(block, 1);
y = GetRealOutPortPtrs(block, 1);
for (i = 0; i < mu * nu; i++)
{
*(y + i) = *(u + i);
}
for (j = 0; j < nu; j++)
{
for (i = j + 1; i < mu; i++)
{
ij = i + j * mu;
*(y + ij) = 0;
}
}
}
static int inout(scicos_block *block)
{
struct FlagTestDev * comdev = (struct FlagTestDev *) (*block->work);
int i;
if (get_scicos_time()==comdev->time){
comdev->step++;
} else {
comdev->time=get_scicos_time();
comdev->step=1;
}
printf("Flag 1: update output, step=%d, t=%f\n",comdev->step,comdev->time);
if (GetNin(block)>0){
double *u1 = GetRealInPortPtrs(block,1);
comdev->data=u1[0];
}
if (GetNout(block)>0){
double *y1 = GetRealOutPortPtrs(block,1);
y1[0]=comdev->data;
}
return 0;
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void cumsum_r(scicos_block *block, int flag)
{
double *u = NULL;
double *y = NULL;
int nu = 0, mu = 0, i = 0, j = 0, ij = 0;
mu = GetInPortRows(block, 1);
nu = GetInPortCols(block, 1);
u = GetRealInPortPtrs(block, 1);
y = GetRealOutPortPtrs(block, 1);
for (j = 0; j < nu; j++)
{
i = 0;
ij = i + j * mu;
y[ij] = u[ij];
for (i = 1; i < mu; i++)
{
ij = i + j * mu;
y[ij] = u[ij] + y[ij - 1];
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void matmul2_m(scicos_block *block,int flag)
{
if (flag==1){
int i = 0;
int ut=GetInType(block,1);
int mu=GetOutPortRows(block,1);
int nu=GetOutPortCols(block,1);
switch (ut)
{
case SCSREAL_N :{
double *u1=GetRealInPortPtrs(block,1);
double *u2=GetRealInPortPtrs(block,2);
double *y1=GetRealOutPortPtrs(block,1);
matmul2(y1,u1,u2,mu,nu);
break;}
case SCSINT32_N :{
long *u1=Getint32InPortPtrs(block,1);
long *u2=Getint32InPortPtrs(block,2);
long *y1=Getint32OutPortPtrs(block,1);
matmul2(y1,u1,u2,mu,nu);
break;}
case SCSINT16_N :{
short *u1=Getint16InPortPtrs(block,1);
short *u2=Getint16InPortPtrs(block,2);
short *y1=Getint16OutPortPtrs(block,1);
matmul2(y1,u1,u2,mu,nu);
break;}
case SCSINT8_N :{
char *u1=Getint8InPortPtrs(block,1);
char *u2=Getint8InPortPtrs(block,2);
char *y1=Getint8OutPortPtrs(block,1);
matmul2(y1,u1,u2,mu,nu);
break;}
case SCSUINT32_N :{
unsigned long *u1=Getuint32InPortPtrs(block,1);
unsigned long *u2=Getuint32InPortPtrs(block,2);
unsigned long *y1=Getuint32OutPortPtrs(block,1);
matmul2(y1,u1,u2,mu,nu);
break;}
case SCSUINT16_N :{
unsigned short *u1=Getuint16InPortPtrs(block,1);
unsigned short *u2=Getuint16InPortPtrs(block,2);
unsigned short *y1=Getuint16OutPortPtrs(block,1);
matmul2(y1,u1,u2,mu,nu);
break;}
case SCSUINT8_N :{
unsigned char *u1=Getuint8InPortPtrs(block,1);
unsigned char *u2=Getuint8InPortPtrs(block,2);
unsigned char *y1=Getuint8OutPortPtrs(block,1);
matmul2(y1,u1,u2,mu,nu);
break;}
case SCSCOMPLEX_N :{
double *u1r=GetRealInPortPtrs(block,1);
double *u2r=GetRealInPortPtrs(block,2);
double *y1r=GetRealOutPortPtrs(block,1);
double *u1i=GetImagInPortPtrs(block,1);
double *u2i=GetImagInPortPtrs(block,2);
double *y1i=GetImagOutPortPtrs(block,1);
for (i=0;i<mu*nu;i++)
{y1r[i]=(u1r[i]*u2r[i])-(u1i[i]*u2i[i]);
y1i[i]=(u1r[i]*u2i[i])+(u1i[i]*u2r[i]);}
break;}
default :{
set_block_error(-4);
return;}
}
}
}
void sci_mavlinkHilState(scicos_block *block, scicos::enumScicosFlags flag)
{
// data
double * u=GetRealInPortPtrs(block,1);
double * y=GetRealOutPortPtrs(block,1);
int * ipar=block->ipar;
static char * device;
static int baudRate;
static char ** stringArray;
static int * intArray;
static int count = 0;
static uint16_t packet_drops = 0;
//handle flags
if (flag==scicos::initialize || flag==scicos::reinitialize)
{
if (mavlink_comm_0_port == NULL)
{
getIpars(1,1,ipar,&stringArray,&intArray);
device = stringArray[0];
baudRate = intArray[0];
try
{
mavlink_comm_0_port = new BufferedAsyncSerial(device,baudRate);
}
catch(const boost::system::system_error & e)
{
Coserror((char *)e.what());
}
}
}
else if (flag==scicos::terminate)
{
if (mavlink_comm_0_port)
{
delete mavlink_comm_0_port;
mavlink_comm_0_port = NULL;
}
}
else if (flag==scicos::updateState)
{
}
else if (flag==scicos::computeDeriv)
{
}
else if (flag==scicos::computeOutput)
{
// channel
mavlink_channel_t chan = MAVLINK_COMM_0;
// loop rates
// TODO: clean this up to use scicos events w/ timers
static int attitudeRate = 50;
static int positionRate = 10;
static int airspeedRate = 1;
// initial times
double scicosTime = get_scicos_time();
static double attitudeTimeStamp = scicosTime;
static double positionTimeStamp = scicosTime;
static double airspeedTimeStamp = scicosTime;
// send airspeed message
if (scicosTime - airspeedTimeStamp > 1.0/airspeedRate)
{
airspeedTimeStamp = scicosTime;
// airspeed (true velocity m/s)
float Vt = u[0];
//double rawPress = 1;
//double airspeed = 1;
//mavlink_msg_raw_pressure_send(chan,timeStamp,airspeed,rawPress,0);
}
else if (scicosTime - airspeedTimeStamp < 0)
airspeedTimeStamp = scicosTime;
// send attitude message
if (scicosTime - attitudeTimeStamp > 1.0/attitudeRate)
{
attitudeTimeStamp = scicosTime;
// attitude states (rad)
float roll = u[1];
float pitch = u[2];
float yaw = u[3];
// body rates
float rollRate = u[4];
float pitchRate = u[5];
float yawRate = u[6];
mavlink_msg_attitude_send(chan,attitudeTimeStamp,roll,pitch,yaw,
rollRate,pitchRate,yawRate);
}
else if (scicosTime - attitudeTimeStamp < 0)
attitudeTimeStamp = scicosTime;
// send gps mesage
if (scicosTime - positionTimeStamp > 1.0/positionRate)
{
positionTimeStamp = scicosTime;
// gps
double cog = u[7];
double sog = u[8];
double lat = u[9]*rad2deg;
double lon = u[10]*rad2deg;
double alt = u[11];
//double rawPress = 1;
//double airspeed = 1;
mavlink_msg_gps_raw_send(chan,positionTimeStamp,1,lat,lon,alt,2,10,sog,cog);
//mavlink_msg_raw_pressure_send(chan,timeStamp,airspeed,rawPress,0);
}
else if (scicosTime - positionTimeStamp < 0)
positionTimeStamp = scicosTime;
// receive messages
mavlink_message_t msg;
mavlink_status_t status;
while(comm_get_available(MAVLINK_COMM_0))
{
uint8_t c = comm_receive_ch(MAVLINK_COMM_0);
// try to get new message
if(mavlink_parse_char(MAVLINK_COMM_0,c,&msg,&status))
{
switch(msg.msgid)
{
case MAVLINK_MSG_ID_RC_CHANNELS_SCALED:
{
//std::cout << "receiving messages" << std::endl;
mavlink_rc_channels_scaled_t rc_channels;
mavlink_msg_rc_channels_scaled_decode(&msg,&rc_channels);
y[0] = rc_channels.chan1_scaled/10000.0f;
y[1] = rc_channels.chan2_scaled/10000.0f;
y[2] = rc_channels.chan3_scaled/10000.0f;
y[3] = rc_channels.chan4_scaled/10000.0f;
y[4] = rc_channels.chan5_scaled/10000.0f;
y[5] = rc_channels.chan6_scaled/10000.0f;
y[6] = rc_channels.chan7_scaled/10000.0f;
y[7] = rc_channels.chan8_scaled/10000.0f;
break;
}
}
// update packet drop counter
packet_drops += status.packet_rx_drop_count;
}
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void mat_lu(scicos_block *block, int flag)
{
double *u = NULL;
double *y1 = NULL;
double *y2 = NULL;
int mu = 0;
int nu = 0;
int info = 0;
int i = 0, j = 0, l = 0, ij = 0, ik = 0;
mat_lu_struct *ptr = NULL;
mu = GetInPortRows(block, 1);
nu = GetInPortCols(block, 1);
u = GetRealInPortPtrs(block, 1);
y1 = GetRealOutPortPtrs(block, 1);
y2 = GetRealOutPortPtrs(block, 2);
l = Min(mu, nu);
/*init : initialization*/
if (flag == 4)
{
if ((*(block->work) = (mat_lu_struct*) scicos_malloc(sizeof(mat_lu_struct))) == NULL)
{
set_block_error(-16);
return;
}
ptr = *(block->work);
if ((ptr->ipiv = (int*) scicos_malloc(sizeof(int) * nu)) == NULL)
{
set_block_error(-16);
scicos_free(ptr);
return;
}
if ((ptr->dwork = (double*) scicos_malloc(sizeof(double) * (mu * nu))) == NULL)
{
set_block_error(-16);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
if ((ptr->IL = (double*) scicos_malloc(sizeof(double) * (mu * l))) == NULL)
{
set_block_error(-16);
scicos_free(ptr->dwork);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
if ((ptr->IU = (double*) scicos_malloc(sizeof(double) * (l * nu))) == NULL)
{
set_block_error(-16);
scicos_free(ptr->IL);
scicos_free(ptr->dwork);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
}
/* Terminaison */
else if (flag == 5)
{
ptr = *(block->work);
if ((ptr->IU) != NULL)
{
scicos_free(ptr->ipiv);
scicos_free(ptr->dwork);
scicos_free(ptr->IL);
scicos_free(ptr->IU);
scicos_free(ptr);
return;
}
}
else
{
ptr = *(block->work);
for (i = 0; i < (mu * nu); i++)
{
ptr->dwork[i] = u[i];
}
C2F(dgetrf)(&mu, &nu, ptr->dwork, &mu, ptr->ipiv, &info);
if (info != 0)
{
if (flag != 6)
{
set_block_error(-7);
return;
}
}
for (j = 0; j < l; j++)
{
for (i = 0; i < mu; i++)
{
ij = i + j * mu;
if (i == j)
{
*(y2 + ij) = 1;
}
else if (i > j)
{
*(y2 + ij) = *(ptr->dwork + ij);
}
else
{
*(y2 + ij) = 0;
}
}
}
for (j = 0; j < nu; j++)
{
for (i = 0; i < l; i++)
{
ij = i + j * l;
ik = i + j * mu;
if (i <= j)
{
*(y1 + ij) = *(ptr->dwork + ik);
}
else
{
*(y1 + ij) = 0;
}
}
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void matz_inv(scicos_block *block,int flag)
{
double *ur = NULL;
double *yr = NULL;
double *ui = NULL;
double *yi = NULL;
int nu = 0;
int info = 0;
int i = 0;
mat_inv_struct *ptr = NULL;
nu =GetInPortRows(block,1);
ur=GetRealInPortPtrs(block,1);
ui=GetImagInPortPtrs(block,1);
yr=GetRealOutPortPtrs(block,1);
yi=GetImagOutPortPtrs(block,1);
/*init : initialization*/
if (flag==4)
{
if((*(block->work)=(mat_inv_struct*) scicos_malloc(sizeof(mat_inv_struct)))==NULL)
{
set_block_error(-16);
return;
}
ptr=*(block->work);
if((ptr->ipiv=(int*) scicos_malloc(sizeof(int)*nu))==NULL)
{
set_block_error(-16);
scicos_free(ptr);
return;
}
if((ptr->wrk=(double*) scicos_malloc(sizeof(double)*(2*nu*nu)))==NULL)
{
set_block_error(-16);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
if((ptr->LX=(double*) scicos_malloc(sizeof(double)*(2*nu*nu)))==NULL)
{
set_block_error(-16);
scicos_free(ptr->LX);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
}
/* Terminaison */
else if (flag==5)
{
ptr=*(block->work);
if((ptr->LX)!=NULL)
{
scicos_free(ptr->ipiv);
scicos_free(ptr->LX);
scicos_free(ptr->wrk);
scicos_free(ptr);
return;
}
}
else
{
ptr=*(block->work);
for (i=0;i<(nu*nu);i++)
{
ptr->LX[2*i]=ur[i];
ptr->LX[2*i+1]=ui[i];
}
C2F(zgetrf)(&nu,&nu,ptr->LX,&nu,ptr->ipiv,&info);
if (info !=0)
{
if (flag!=6)
{set_block_error(-7);
return;
}
}
C2F(zgetri)(&nu,ptr->LX,&nu,ptr->ipiv,ptr->wrk,&nu,&info);
for (i=0;i<(nu*nu);i++)
{
yr[i]=ptr->LX[2*i];
yi[i]=ptr->LX[2*i+1];
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void mat_vpv(scicos_block *block,int flag)
{
double *u = NULL;
double *y1 = NULL;
double *y2 = NULL;
double *y3 = NULL;
double *y4 = NULL;
int nu = 0;
int info = 0;
int i = 0,j = 0,lwork = 0,lwork1 = 0,ii = 0,ij = 0,ij1 = 0,ji = 0;
int symmetric = 0;
mat_vps_struct *ptr = NULL;
nu =GetInPortRows(block,1);
u=GetRealInPortPtrs(block,1);
y1=GetRealOutPortPtrs(block,1);
y2=GetImagOutPortPtrs(block,1);
y3=GetRealOutPortPtrs(block,2);
y4=GetImagOutPortPtrs(block,2);
lwork1=4*nu;
lwork=3*nu-1;
/*init : initialization*/
if (flag==4)
{if((*(block->work)=(mat_vps_struct*) scicos_malloc(sizeof(mat_vps_struct)))==NULL)
{set_block_error(-16);
return;}
ptr=*(block->work);
if((ptr->LA=(double*) scicos_malloc(sizeof(double)*(nu*nu)))==NULL)
{set_block_error(-16);
scicos_free(ptr);
return;}
if((ptr->L0=(double*) scicos_malloc(sizeof(double)))==NULL)
{set_block_error(-16);
scicos_free(ptr->LA);
scicos_free(ptr);
return;}
if((ptr->LVR=(double*) scicos_malloc(sizeof(double)*(nu*nu)))==NULL)
{set_block_error(-16);
scicos_free(ptr->L0);
scicos_free(ptr->LA);
scicos_free(ptr);
return;}
if((ptr->LW=(double*) scicos_malloc(sizeof(double)*(nu*nu)))==NULL)
{set_block_error(-16);
scicos_free(ptr->LVR);
scicos_free(ptr->L0);
scicos_free(ptr->LA);
scicos_free(ptr);
return;}
if((ptr->LWR=(double*) scicos_malloc(sizeof(double)*(nu*1)))==NULL)
{set_block_error(-16);
scicos_free(ptr->LW);
scicos_free(ptr->LVR);
scicos_free(ptr->L0);
scicos_free(ptr->LA);
scicos_free(ptr);
return;}
if((ptr->LWI=(double*) scicos_malloc(sizeof(double)*(nu*1)))==NULL)
{set_block_error(-16);
scicos_free(ptr->LWR);
scicos_free(ptr->LW);
scicos_free(ptr->LVR);
scicos_free(ptr->L0);
scicos_free(ptr->LA);
scicos_free(ptr);
return;}
if((ptr->dwork=(double*) scicos_malloc(sizeof(double)*lwork))==NULL)
{set_block_error(-16);
scicos_free(ptr->LWI);
scicos_free(ptr->LWR);
scicos_free(ptr->LW);
scicos_free(ptr->LVR);
scicos_free(ptr->L0);
scicos_free(ptr->LA);
scicos_free(ptr);
return;}
if((ptr->dwork1=(double*) scicos_malloc(sizeof(double)*lwork1))==NULL)
{set_block_error(-16);
scicos_free(ptr->dwork);
scicos_free(ptr->LWI);
scicos_free(ptr->LWR);
scicos_free(ptr->LW);
scicos_free(ptr->LVR);
scicos_free(ptr->L0);
scicos_free(ptr->LA);
scicos_free(ptr);
return;}
}
/* Terminaison */
else if (flag==5)
{ptr=*(block->work);
if(ptr->dwork1!=NULL){
scicos_free(ptr->LA);
scicos_free(ptr->L0);
scicos_free(ptr->LVR);
scicos_free(ptr->LW);
scicos_free(ptr->LWI);
scicos_free(ptr->LWR);
scicos_free(ptr->dwork);
scicos_free(ptr->dwork1);
scicos_free(ptr);
return;}
}
else
{
ptr=*(block->work);
C2F(dlacpy)("F",&nu,&nu,u,&nu,ptr->LA,&nu);
symmetric=1;
for (j=0;j<nu;j++)
{for (i=j;i<nu;i++)
{ij=i+j*nu;
ji=j+i*nu;
if (i!=j)
{if (*(ptr->LA+ij)==*(ptr->LA+ji)) symmetric*= 1;
else { symmetric*=0;break;}}}}
if (symmetric==1)
{C2F(dsyev)("V","U",&nu,ptr->LA,&nu,ptr->LW,ptr->dwork,&lwork,&info);
if (info!=0)
{if (flag!=6)
{set_block_error(-7);
return;
}}
for (i=0;i<nu;i++)
{ii=i+i*nu;
*(y1+ii)=*(ptr->LW+i);
}
C2F(dlacpy)("F",&nu,&nu,ptr->LA,&nu,y3,&nu);
}
else
{C2F(dgeev)("N","V",&nu,ptr->LA,&nu,ptr->LWR,ptr->LWI,ptr->dwork1,&nu,ptr->LVR,&nu,ptr->dwork1,&lwork1,&info);
if (info!=0)
{if (flag!=6)
{set_block_error(-7);
return;
}}
*(ptr->L0)=0;
C2F(dlaset)("F",&nu,&nu,ptr->L0,ptr->L0,y1,&nu);
C2F(dlaset)("F",&nu,&nu,ptr->L0,ptr->L0,y2,&nu);
for (i=0;i<nu;i++)
{ii=i+i*nu;
*(y1+ii)=*(ptr->LWR+i);
*(y2+ii)=*(ptr->LWI+i);
}
for (j=0;j<nu;j++)
{if (*(ptr->LWI+j)==0)
for (i=0;i<nu;i++)
{ij=i+(j)*nu;
*(y3+ij)=*(ptr->LVR +ij);
*(y4+ij)=0;}
else{
for (i=0;i<nu;i++)
{ij=i+(j)*nu;
ij1=i+(j+1)*nu;
*(y3+ij)=*(ptr->LVR +ij);
*(y4+ij)=*(ptr->LVR +ij1);
*(y3+ij1)=*(ptr->LVR +ij);
*(y4+ij1)=-(*(ptr->LVR +ij1));}
j++;}
}
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void convert(scicos_block *block,int flag)
{
int m = 0,n = 0,i = 0;
int *ipar = NULL;
double v = 0.,w = 0.,k = 0.;
m=GetInPortRows(block,1);
n=GetInPortCols(block,1);
ipar=GetIparPtrs(block);
if ((flag==1)|(flag==6))
{
switch (*ipar){
case 1:{
void *u = NULL,*y = NULL;
int so;
so=GetSizeOfOut(block,1);
u=GetInPortPtrs(block,1);
y=GetOutPortPtrs(block,1);
memcpy(y,u,m*n*so);
break;
}
case 2:{
double *u = NULL;
long *y = NULL;
u=GetRealInPortPtrs(block,1);
y=Getint32OutPortPtrs(block,1);
k=pow(2,32);
for (i=0;i<m*n;i++)
{v=(double)u[i];
w=v-(double)((int)(v/k))*k;
if (fabs(w)>k/2-1)
{if (w>=0) w=(-k/2+fabs(w-(double)((int)(w/(k/2)))*(k/2)));
else w=-(-(k/2)+fabs(w-(double)((int)(w/(k/2)))*(k/2)));
}
y[i]=(long)w;}
break;}
case 3:{
double *u = NULL;
short *y = NULL;
u=GetRealInPortPtrs(block,1);
y=Getint16OutPortPtrs(block,1);
k=pow(2,16);
for (i=0;i<m*n;i++)
{v=(double)u[i];
w=v-(double)((int)(v/k))*k;
if (fabs(w)>k/2-1)
{if (w>=0) w=(-k/2+fabs(w-(double)((int)(w/(k/2)))*(k/2)));
else w=-(-(k/2)+fabs(w-(double)((int)(w/(k/2)))*(k/2)));
}
y[i]=(short)w;}
break;}
case 4:{
double *u = NULL;
char *y = NULL;
u=GetRealInPortPtrs(block,1);
y=Getint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{v=(double)u[i];
w=v-(double)((int)(v/k))*k;
if (fabs(w)>k/2-1)
{if (w>=0) w=(-k/2+fabs(w-(double)((int)(w/(k/2)))*(k/2)));
else w=-(-(k/2)+fabs(w-(double)((int)(w/(k/2)))*(k/2)));
}
y[i]=(char)w;}
break;}
case 5:{
double *u = NULL;
unsigned long *y = NULL;
u=GetRealInPortPtrs(block,1);
y=Getuint32OutPortPtrs(block,1);
k=pow(2,32);
for (i=0;i<m*n;i++)
{v=(double)u[i];
w=v-(double)((int)(v/k))*k;
y[i]=(unsigned long)w;}
break;}
case 6:{
double *u = NULL;
unsigned short *y = NULL;
u=GetRealInPortPtrs(block,1);
y=Getuint16OutPortPtrs(block,1);
k=pow(2,16);
for (i=0;i<m*n;i++)
{v=(double)u[i];
w=v-(double)((int)(v/k))*k;
y[i]=(unsigned short)w;}
break;}
case 7:{
double *u = NULL;
unsigned char *y = NULL;
u=GetRealInPortPtrs(block,1);
y=Getuint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{v=(double)u[i];
w=v-(double)((int)(v/k))*k;
y[i]=(unsigned char)w;}
break;}
case 8:{
long *u = NULL;
double *y = NULL;
u=Getint32InPortPtrs(block,1);
y=GetRealOutPortPtrs(block,1);
for (i=0;i<m*n;i++) y[i]=(double) u[i];
break;}
case 9:{
long *u = NULL;
short *y = NULL;
u=Getint32InPortPtrs(block,1);
y=Getint16OutPortPtrs(block,1);
k=pow(2,16);
for (i=0;i<m*n;i++)
{v=(double)u[i];
w=v-(double)((int)(v/k))*k;
if (fabs(w)>k/2-1)
{if (w>=0) w=(-k/2+fabs(w-(double)((int)(w/(k/2)))*(k/2)));
else w=-(-(k/2)+fabs(w-(double)((int)(w/(k/2)))*(k/2)));
}
y[i]=(short)w;}
break;}
case 10:{
long *u = NULL;
char *y = NULL;
u=Getint32InPortPtrs(block,1);
y=Getint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{v=(double)u[i];
w=v-(double)((int)(v/k))*k;
if (fabs(w)>k/2-1)
{if (w>=0) w=(-k/2+fabs(w-(double)((int)(w/(k/2)))*(k/2)));
else w=-(-(k/2)+fabs(w-(double)((int)(w/(k/2)))*(k/2)));
}
y[i]=(char)w;}
break;}
case 11:{
long *u = NULL;
unsigned short *y = NULL;
u=Getint32InPortPtrs(block,1);
y=Getuint16OutPortPtrs(block,1);
k=pow(2,16);
for (i=0;i<m*n;i++)
{v=(double)u[i];
w=v-(double)((int)(v/k))*k;
y[i]=(unsigned short)w;}
break;}
case 12:{
long *u = NULL;
unsigned char *y = NULL;
u=Getint32InPortPtrs(block,1);
y=Getuint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{v=(double)u[i];
w=v-(double)((int)(v/k))*k;
y[i]=(unsigned char)w;}
break;}
case 13:{
short *u = NULL;
double *y = NULL;
u=Getint16InPortPtrs(block,1);
y=GetRealOutPortPtrs(block,1);
for (i=0;i<m*n;i++) y[i]=(double) u[i];
break;}
case 14:{
short *u = NULL;
long *y = NULL;
u=Getint16InPortPtrs(block,1);
y=Getint32OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{y[i]=(long)u[i];}
break;}
case 15:{
short *u = NULL;
char *y = NULL;
u=Getint16InPortPtrs(block,1);
y=Getint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{v=(double)u[i];
w=v-(double)((int)(v/k))*k;
if (fabs(w)>k/2-1)
{if (w>=0) w=(-k/2+fabs(w-(double)((int)(w/(k/2)))*(k/2)));
else w=-(-(k/2)+fabs(w-(double)((int)(w/(k/2)))*(k/2)));
}
y[i]=(char)w;}
break;}
case 16:{
short *u = NULL;
unsigned long *y = NULL;
u=Getint16InPortPtrs(block,1);
y=Getuint32OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{y[i]=(unsigned long)u[i];}
break;}
case 17:{
short *u = NULL;
unsigned char *y = NULL;
u=Getint16InPortPtrs(block,1);
y=Getuint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{v=(double)u[i];
w=v-(double)((int)(v/k))*k;
y[i]=(unsigned char)w;}
break;}
case 18:{
char *u = NULL;
double *y = NULL;
u=Getint8InPortPtrs(block,1);
y=GetRealOutPortPtrs(block,1);
for (i=0;i<m*n;i++) y[i]=(double) u[i];
break;}
case 19:{
char *u = NULL;
long *y = NULL;
u=Getint8InPortPtrs(block,1);
y=Getint32OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{y[i]=(long)u[i];}
break;}
case 20:{
char *u = NULL;
short *y = NULL;
u=Getint8InPortPtrs(block,1);
y=Getint16OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{y[i]=(short)u[i];}
break;}
case 21:{
char *u = NULL;
unsigned long *y = NULL;
u=Getint8InPortPtrs(block,1);
y=Getuint32OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{y[i]=(unsigned long)u[i];}
break;}
case 22:{
char *u = NULL;
unsigned short *y = NULL;
u=Getint8InPortPtrs(block,1);
y=Getuint16OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{y[i]=(unsigned short)u[i];}
break;}
case 23:{
unsigned long *u = NULL;
double *y = NULL;
u=Getuint32InPortPtrs(block,1);
y=GetRealOutPortPtrs(block,1);
for (i=0;i<m*n;i++) y[i]=(double) u[i];
break;}
case 24:{
unsigned long *u = NULL;
short *y = NULL;
u=Getuint32InPortPtrs(block,1);
y=Getint16OutPortPtrs(block,1);
k=pow(2,16);
for (i=0;i<m*n;i++)
{v=(double)u[i];
w=v-(double)((int)(v/k))*k;
if ((w)>k/2-1)
{ w=(-k/2+fabs(w-(double)((int)(w/(k/2)))*(k/2)));
}
y[i]=(short)w;}
break;}
case 25:{
unsigned long *u = NULL;
char *y = NULL;
u=Getuint32InPortPtrs(block,1);
y=Getint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{v=(double)u[i];
w=v-(double)((int)(v/k))*k;
if ((w)>k/2-1)
{w=(-k/2+fabs(w-(double)((int)(w/(k/2)))*(k/2)));
}
y[i]=(char)w;}
break;}
case 26:{
unsigned long *u = NULL;
unsigned short *y = NULL;
u=Getuint32InPortPtrs(block,1);
y=Getuint16OutPortPtrs(block,1);
k=pow(2,16);
for (i=0;i<m*n;i++)
{v=(double)u[i];
w=v-(double)((int)(v/k))*k;
y[i]=(unsigned short)w;}
break;}
case 27:{
unsigned long *u = NULL;
unsigned char *y = NULL;
u=Getuint32InPortPtrs(block,1);
y=Getuint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{v=(double)u[i];
w=v-(double)((int)(v/k))*k;
y[i]=(unsigned char)w;}
break;}
case 28:{
unsigned short *u = NULL;
double *y = NULL;
u=Getuint16InPortPtrs(block,1);
y=GetRealOutPortPtrs(block,1);
for (i=0;i<m*n;i++) y[i]=(double) u[i];
break;}
case 29:{
unsigned short *u = NULL;
long *y = NULL;
u=Getuint16InPortPtrs(block,1);
y=Getint32OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{y[i]=(long)u[i];}
break;}
case 30:{
unsigned short *u = NULL;
char *y = NULL;
u=Getuint16InPortPtrs(block,1);
y=Getint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{v=(double)u[i];
w=v-(double)((int)(v/k))*k;
if (w>k/2-1)
{w=(-k/2+fabs(w-(double)((int)(w/(k/2)))*(k/2)));
}
y[i]=(char)w;}
break;}
case 31:{
unsigned short *u = NULL;
unsigned long *y = NULL;
u=Getuint16InPortPtrs(block,1);
y=Getuint32OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{y[i]=(unsigned long)u[i];}
break;}
case 32:{
unsigned short *u = NULL;
unsigned char *y = NULL;
u=Getuint16InPortPtrs(block,1);
y=Getuint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{v=(double)u[i];
w=v-(double)((int)(v/k))*k;
y[i]=(unsigned char)w;}
break;}
case 33:{
unsigned char *u = NULL;
double *y = NULL;
u=Getuint8InPortPtrs(block,1);
y=GetRealOutPortPtrs(block,1);
for (i=0;i<m*n;i++) y[i]=(double) u[i];
break;}
case 34:{
unsigned char *u = NULL;
long *y = NULL;
u=Getuint8InPortPtrs(block,1);
y=Getint32OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{y[i]=(long)u[i];}
break;}
case 35:{
unsigned char *u = NULL;
short *y = NULL;
u=Getuint8InPortPtrs(block,1);
y=Getint16OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{y[i]=(short)u[i];}
break;}
case 36:{
unsigned char *u = NULL;
unsigned long *y = NULL;
u=Getuint8InPortPtrs(block,1);
y=Getuint32OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{y[i]=(unsigned long)u[i];}
break;}
case 37:{
unsigned char *u = NULL;
unsigned short *y = NULL;
u=Getuint8InPortPtrs(block,1);
y=Getuint16OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{y[i]=(unsigned short)u[i];}
break;}
case 38:{
double *u = NULL;
long *y = NULL;
u=GetRealInPortPtrs(block,1);
y=Getint32OutPortPtrs(block,1);
k=pow(2,32);
for (i=0;i<m*n;i++)
{if (u[i]>k/2-1)
{y[i]=(long)(k/2-1);}
else if (u[i]<-(k/2))
{y[i]=-(long)(k/2);}
else {y[i]=(long)(u[i]);}
}
break;}
case 39:{
double *u = NULL;
short *y = NULL;
u=GetRealInPortPtrs(block,1);
y=Getint16OutPortPtrs(block,1);
k=pow(2,16);
for (i=0;i<m*n;i++)
{if (u[i]>k/2-1)
{y[i]=(short)(k/2-1);}
else if (u[i]<-(k/2))
{y[i]=-(short)(k/2);}
else {y[i]=(short)(u[i]);}
}
break;}
case 40:{
double *u = NULL;
char *y = NULL;
u=GetRealInPortPtrs(block,1);
y=Getint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{if (u[i]>k/2-1)
{y[i]=(char)(k/2-1);}
else if (u[i]<-(k/2))
{y[i]=-(char)(k/2);}
else {y[i]=(char)(u[i]);}
}
break;}
case 41:{
double *u = NULL;
unsigned long *y = NULL;
u=GetRealInPortPtrs(block,1);
y=Getuint32OutPortPtrs(block,1);
k=pow(2,32);
for (i=0;i<m*n;i++)
{if (u[i]>=k)
{y[i]=(unsigned long)(k-1);}
else if (u[i]<0)
{y[i]=0;}
else {y[i]=(unsigned long)(u[i]);}
}
break;}
case 42:{
double *u = NULL;
unsigned short *y = NULL;
u=GetRealInPortPtrs(block,1);
y=Getuint16OutPortPtrs(block,1);
k=pow(2,16);
for (i=0;i<m*n;i++)
{if (u[i]>=k)
{y[i]=(unsigned short)(k-1);}
else if (u[i]<0)
{y[i]=0;}
else {y[i]=(unsigned short)(u[i]);}
}
break;}
case 43:{
double *u = NULL;
unsigned char *y = NULL;
u=GetRealInPortPtrs(block,1);
y=Getuint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{if (u[i]>=k)
{y[i]=(unsigned char)(k-1);}
else if (u[i]<0)
{y[i]=0;}
else {y[i]=(unsigned char)(u[i]);}
}
break;}
case 44:{
long *u = NULL;
short *y = NULL;
u=Getint32InPortPtrs(block,1);
y=Getint16OutPortPtrs(block,1);
k=pow(2,16);
for (i=0;i<m*n;i++)
{if (u[i]>(long)(k/2-1))
{y[i]=(short)(k/2-1);}
else if (u[i]<-(long)(k/2))
{y[i]=-(short)(k/2);}
else {y[i]=(short)(u[i]);}
}
break;}
case 45:{
long *u = NULL;
char *y = NULL;
u=Getint32InPortPtrs(block,1);
y=Getint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{if (u[i]>(long)(k/2-1))
{y[i]=(char)(k/2-1);}
else if (u[i]<-(long)(k/2))
{y[i]=-(char)(k/2);}
else {y[i]=(char)(u[i]);}
}
break;}
case 46:{
long *u = NULL;
unsigned long *y = NULL;
u=Getint32InPortPtrs(block,1);
y=Getuint32OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{if (u[i]<0)
{y[i]=0;}
else {y[i]=(unsigned long)(u[i]);}
}
break;}
case 47:{
long *u = NULL;
unsigned short *y = NULL;
u=Getint32InPortPtrs(block,1);
y=Getuint16OutPortPtrs(block,1);
k=pow(2,16);
for (i=0;i<m*n;i++)
{if (u[i]>=(long)(k))
{y[i]=(unsigned short)(k-1);}
else if (u[i]<0)
{y[i]=0;}
else {y[i]=(unsigned short)(u[i]);}
}
break;}
case 48:{
long *u = NULL;
unsigned char *y = NULL;
u=Getint32InPortPtrs(block,1);
y=Getuint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{if (u[i]>=(long)k)
{y[i]=(unsigned char)(k-1);}
else if (u[i]<0)
{y[i]=0;}
else {y[i]=(unsigned char)(u[i]);}
}
break;}
case 49:{
short *u = NULL;
char *y = NULL;
u=Getint16InPortPtrs(block,1);
y=Getint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{if (u[i]>(short)(k/2-1))
{y[i]=(char)(k/2-1);}
else if (u[i]<-(short)(k/2))
{y[i]=-(char)(k/2);}
else {y[i]=(char)(u[i]);}
}
break;}
case 50:{
short *u = NULL;
unsigned long *y = NULL;
u=Getint16InPortPtrs(block,1);
y=Getuint32OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{if (u[i]<0) y[i]=0;
else y[i]=(unsigned long)u[i];}
break;}
case 51:{
short *u = NULL;
unsigned short *y = NULL;
u=Getint16InPortPtrs(block,1);
y=Getuint16OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{if (u[i]<0)
{y[i]=0;}
else {y[i]=(unsigned short)(u[i]);}
}
break;}
case 52:{
short *u = NULL;
unsigned char *y = NULL;
u=Getint16InPortPtrs(block,1);
y=Getuint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{if (u[i]>=(short)k)
{y[i]=(unsigned char)(k-1);}
else if (u[i]<0)
{y[i]=0;}
else {y[i]=(unsigned char)(u[i]);}
}
break;}
case 53:{
char *u = NULL;
unsigned long *y = NULL;
u=Getint8InPortPtrs(block,1);
y=Getuint32OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{if (u[i]<0) y[i]=0;
else y[i]=(unsigned long)u[i];}
break;}
case 54:{
char *u = NULL;
unsigned short *y = NULL;
u=Getint8InPortPtrs(block,1);
y=Getuint16OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{if (u[i]<0)
{y[i]=0;}
else {y[i]=(unsigned short)(u[i]);}
}
break;}
case 55:{
char *u = NULL;
unsigned char *y = NULL;
u=Getint8InPortPtrs(block,1);
y=Getuint8OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{if (u[i]<0)
{y[i]=0;}
else {y[i]=(unsigned char)(u[i]);}
}
break;}
case 56:{
long *y = NULL;
unsigned long *u = NULL;
u=Getuint32InPortPtrs(block,1);
y=Getint32OutPortPtrs(block,1);
k=pow(2,32);
for (i=0;i<m*n;i++)
{if (u[i]>(unsigned long)(k/2-1))
{y[i]=(long)(k/2-1);}
else {y[i]=(long)(u[i]);}
}
break;}
case 57:{
unsigned long *u = NULL;
short *y = NULL;
u=Getuint32InPortPtrs(block,1);
y=Getint16OutPortPtrs(block,1);
k=pow(2,16);
for (i=0;i<m*n;i++)
{if (u[i]>(unsigned long)(k/2-1))
{y[i]=(short)(k/2-1);}
else {y[i]=(short)(u[i]);}
}
break;}
case 58:{
unsigned long *u = NULL;
char *y = NULL;
u=Getuint32InPortPtrs(block,1);
y=Getint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{if (u[i]>(unsigned long)(k/2-1))
{y[i]=(char)(k/2-1);}
else {y[i]=(char)(u[i]);}
}
break;}
case 59:{
unsigned long *u = NULL;
unsigned short *y = NULL;
u=Getuint32InPortPtrs(block,1);
y=Getuint16OutPortPtrs(block,1);
k=pow(2,16);
for (i=0;i<m*n;i++)
{if (u[i]>(unsigned long)(k/2-1))
{y[i]=(unsigned short)(k/2-1);}
else {y[i]=(unsigned short)(u[i]);}
}
break;}
case 60:{
unsigned long *u = NULL;
unsigned char *y = NULL;
u=Getuint32InPortPtrs(block,1);
y=Getuint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{if (u[i]>(unsigned long)(k/2-1))
{y[i]=(unsigned char)(k/2-1);}
else {y[i]=(unsigned char)(u[i]);}
}
break;}
case 61:{
unsigned short *u = NULL;
short *y = NULL;
u=Getuint16InPortPtrs(block,1);
y=Getint16OutPortPtrs(block,1);
k=pow(2,16);
for (i=0;i<m*n;i++)
{if (u[i]>(unsigned short)(k/2-1))
{y[i]=(short)(k/2-1);}
else {y[i]=(short)(u[i]);}
}
break;}
case 62:{
unsigned short *u = NULL;
char *y = NULL;
u=Getuint16InPortPtrs(block,1);
y=Getint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{if (u[i]>(unsigned short)(k/2-1))
{y[i]=(char)(k/2-1);}
else {y[i]=(char)(u[i]);}
}
break;}
case 63:{
unsigned short *u = NULL;
unsigned char *y = NULL;
u=Getuint16InPortPtrs(block,1);
y=Getuint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{if (u[i]>(unsigned short)(k/2-1))
{y[i]=(unsigned char)k/2-1;}
else {y[i]=(unsigned char)(u[i]);}
}
break;}
case 64:{
unsigned char *u = NULL;
char *y = NULL;
u=Getuint8InPortPtrs(block,1);
y=Getint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{if (u[i]>(unsigned char)(k/2-1))
{y[i]=(char)(k/2-1);}
else {y[i]=(char)(u[i]);}
}
break;}
case 65:{
double *u = NULL;
long *y = NULL;
u=GetRealInPortPtrs(block,1);
y=Getint32OutPortPtrs(block,1);
k=pow(2,32);
for (i=0;i<m*n;i++)
{if ((u[i]>k/2-1) | (u[i]<-(k/2)))
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(long)(u[i]);}
}
break;}
case 66:{
double *u = NULL;
short *y = NULL;
u=GetRealInPortPtrs(block,1);
y=Getint16OutPortPtrs(block,1);
k=pow(2,16);
for (i=0;i<m*n;i++)
{if ((u[i]>k/2-1) | (u[i]<-(k/2)))
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(short)(u[i]);}
}
break;}
case 67:{
double *u = NULL;
char *y = NULL;
u=GetRealInPortPtrs(block,1);
y=Getint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{if ((u[i]>k/2-1) | (u[i]<-(k/2)))
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(char)(u[i]);}
}
break;}
case 68:{
double *u = NULL;
unsigned long *y = NULL;
u=GetRealInPortPtrs(block,1);
y=Getuint32OutPortPtrs(block,1);
k=pow(2,32);
for (i=0;i<m*n;i++)
{if ((u[i]>=k) | (u[i]<0))
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(unsigned long)(u[i]);}
}
break;}
case 69:{
double *u = NULL;
unsigned short *y = NULL;
u=GetRealInPortPtrs(block,1);
y=Getuint16OutPortPtrs(block,1);
k=pow(2,16);
for (i=0;i<m*n;i++)
{if ((u[i]>=k) | (u[i]<0))
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(unsigned short)(u[i]);}
}
break;}
case 70:{
double *u = NULL;
unsigned char *y = NULL;
u=GetRealInPortPtrs(block,1);
y=Getuint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{if ((u[i]>=k) | (u[i]<0))
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(unsigned char)(u[i]);}
}
break;}
case 71:{
long *u = NULL;
short *y = NULL;
u=Getint32InPortPtrs(block,1);
y=Getint16OutPortPtrs(block,1);
k=pow(2,16);
for (i=0;i<m*n;i++)
{if ((u[i]>k/2-1) | (u[i]<-(k/2)))
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(short)(u[i]);}
}
break;}
case 72:{
long *u = NULL;
char *y = NULL;
u=Getint32InPortPtrs(block,1);
y=Getint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{if ((u[i]>k/2-1) | (u[i]<-(k/2)))
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(char)(u[i]);}
}
break;}
case 73:{
long *u = NULL;
unsigned long *y = NULL;
u=Getint32InPortPtrs(block,1);
y=Getuint32OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{if (u[i]<0)
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(unsigned long)(u[i]);}
}
break;}
case 74:{
long *u = NULL;
unsigned short *y = NULL;
u=Getint32InPortPtrs(block,1);
y=Getuint16OutPortPtrs(block,1);
k=pow(2,16);
for (i=0;i<m*n;i++)
{if ((u[i]>=k) | (u[i]<0))
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(unsigned short)(u[i]);}
}
break;}
case 75:{
long *u = NULL;
unsigned char *y = NULL;
u=Getint32InPortPtrs(block,1);
y=Getuint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{if ((u[i]>=k) | (u[i]<0))
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(unsigned char)(u[i]);}
}
break;}
case 76:{
short *u = NULL;
char *y = NULL;
u=Getint16InPortPtrs(block,1);
y=Getint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{if ((u[i]>k/2-1) | (u[i]<-(k/2)))
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(char)(u[i]);}
}
break;}
case 77:{
short *u = NULL;
unsigned long *y = NULL;
u=Getint16InPortPtrs(block,1);
y=Getuint32OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{if (u[i]<0)
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else y[i]=(unsigned long)u[i];}
break;}
case 78:{
short *u = NULL;
unsigned short *y = NULL;
u=Getint16InPortPtrs(block,1);
y=Getuint16OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{if (u[i]<0)
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(unsigned short)(u[i]);}
}
break;}
case 79:{
short *u = NULL;
unsigned char *y = NULL;
u=Getint16InPortPtrs(block,1);
y=Getuint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{if (((u[i]>=k) | (u[i]<0))& (flag==1))
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(unsigned char)(u[i]);}
}
break;}
case 80:{
char *u = NULL;
unsigned long *y = NULL;
u=Getint8InPortPtrs(block,1);
y=Getuint32OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{if (u[i]<0)
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else y[i]=(unsigned long)u[i];}
break;}
case 81:{
char *u = NULL;
unsigned short *y = NULL;
u=Getint8InPortPtrs(block,1);
y=Getuint16OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{if (u[i]<0)
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(unsigned short)(u[i]);}
}
break;}
case 82:{
char *u = NULL;
unsigned char *y = NULL;
u=Getint8InPortPtrs(block,1);
y=Getuint8OutPortPtrs(block,1);
for (i=0;i<m*n;i++)
{if (u[i]<0)
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(unsigned char)(u[i]);}
}
break;}
case 83:{
long *y = NULL;
unsigned long *u = NULL;
u=Getuint32InPortPtrs(block,1);
y=Getint32OutPortPtrs(block,1);
k=pow(2,32);
for (i=0;i<m*n;i++)
{if (u[i]>(k/2-1))
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(long)(u[i]);}
}
break;}
case 84:{
unsigned long *u = NULL;
short *y = NULL;
u=Getuint32InPortPtrs(block,1);
y=Getint16OutPortPtrs(block,1);
k=pow(2,16);
for (i=0;i<m*n;i++)
{if (u[i]>(k/2-1))
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(short)(u[i]);}
}
break;}
case 85:{
unsigned long *u = NULL;
char *y = NULL;
u=Getuint32InPortPtrs(block,1);
y=Getint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{if (u[i]>(k/2-1))
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(char)(u[i]);}
}
break;}
case 86:{
unsigned long *u = NULL;
unsigned short *y = NULL;
u=Getuint32InPortPtrs(block,1);
y=Getuint16OutPortPtrs(block,1);
k=pow(2,16);
for (i=0;i<m*n;i++)
{if (u[i]>(k/2-1))
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(unsigned short)(u[i]);}
}
break;}
case 87:{
unsigned long *u = NULL;
unsigned char *y = NULL;
u=Getuint32InPortPtrs(block,1);
y=Getuint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{if (u[i]>(k/2-1))
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(unsigned char)(u[i]);}
}
break;}
case 88:{
unsigned short *u = NULL;
short *y = NULL;
u=Getuint16InPortPtrs(block,1);
y=Getint16OutPortPtrs(block,1);
k=pow(2,16);
for (i=0;i<m*n;i++)
{if (u[i]>(k/2-1))
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(short)(u[i]);}
}
break;}
case 89:{
unsigned short *u = NULL;
char *y = NULL;
u=Getuint16InPortPtrs(block,1);
y=Getint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{if (u[i]>(k/2-1))
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(char)(u[i]);}
}
break;}
case 90:{
unsigned short *u = NULL;
unsigned char *y = NULL;
u=Getuint16InPortPtrs(block,1);
y=Getuint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{if (u[i]>(k/2-1))
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(unsigned char)(u[i]);}
}
break;}
case 91:{
unsigned char *u = NULL;
char *y = NULL;
u=Getuint8InPortPtrs(block,1);
y=Getint8OutPortPtrs(block,1);
k=pow(2,8);
for (i=0;i<m*n;i++)
{if (u[i]>(k/2-1))
{if (flag==1)
{sciprint(_("overflow error"));
set_block_error(-4);
return;}}
else {y[i]=(char)(u[i]);}
}
break;}
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void mat_svd(scicos_block *block, int flag)
{
double *u = NULL;
double *y1 = NULL, *y2 = NULL, *y3 = NULL;
int nu = 0, mu = 0;
int info = 0;
int i = 0, j = 0, ij = 0, ji = 0, ii = 0, lwork = 0;
mat_sdv_struct** work = (mat_sdv_struct**) block->work;
mat_sdv_struct *ptr = NULL;
mu = GetInPortRows(block, 1);
nu = GetInPortCols(block, 1);
u = GetRealInPortPtrs(block, 1);
y1 = GetRealOutPortPtrs(block, 1);
y2 = GetRealOutPortPtrs(block, 2);
y3 = GetRealOutPortPtrs(block, 3);
/* for lapack 3.1 (2006)*/
lwork = Max(3 * Min(mu, nu) + Max(mu, nu), 5 * Min(mu, nu));
lwork = Max(1, lwork);
/*init : initialization*/
if (flag == 4)
{
if ((*work = (mat_sdv_struct*) scicos_malloc(sizeof(mat_sdv_struct))) == NULL)
{
set_block_error(-16);
return;
}
ptr = *work;
if ((ptr->l0 = (double*) scicos_malloc(sizeof(double))) == NULL)
{
set_block_error(-16);
scicos_free(ptr);
return;
}
if ((ptr->LA = (double*) scicos_malloc(sizeof(double) * (mu * nu))) == NULL)
{
set_block_error(-16);
scicos_free(ptr->l0);
scicos_free(ptr);
return;
}
if ((ptr->LSV = (double*) scicos_malloc(sizeof(double) * (Min(mu, nu)))) == NULL)
{
set_block_error(-16);
scicos_free(ptr->LA);
scicos_free(ptr->l0);
scicos_free(ptr);
return;
}
if ((ptr->LVT = (double*) scicos_malloc(sizeof(double) * (nu * nu))) == NULL)
{
set_block_error(-16);
scicos_free(ptr->LSV);
scicos_free(ptr->LA);
scicos_free(ptr->l0);
scicos_free(ptr);
return;
}
if ((ptr->dwork = (double*) scicos_malloc(sizeof(double) * lwork)) == NULL)
{
set_block_error(-16);
scicos_free(ptr->LVT);
scicos_free(ptr->LSV);
scicos_free(ptr->LA);
scicos_free(ptr->l0);
scicos_free(ptr);
return;
}
}
/* Terminaison */
else if (flag == 5)
{
ptr = *work;
if ((ptr->dwork) != NULL)
{
scicos_free(ptr->l0);
scicos_free(ptr->LA);
scicos_free(ptr->LSV);
scicos_free(ptr->LVT);
scicos_free(ptr->dwork);
scicos_free(ptr);
return;
}
}
else
{
ptr = *work;
C2F(dlacpy)("F", &mu, &nu, u, &mu, ptr->LA, &mu);
C2F(dgesvd)("A", "A", &mu, &nu, ptr->LA, &mu, ptr->LSV, y1, &mu, ptr->LVT, &nu, ptr->dwork, &lwork, &info);
if (info != 0)
{
if (flag != 6)
{
set_block_error(-7);
return;
}
}
*(ptr->l0) = 0;
C2F(dlaset)("F", &mu, &nu, ptr->l0, ptr->l0, y2, &mu);
for (i = 0; i < Min(mu, nu); i++)
{
ii = i + i * mu;
*(y2 + ii) = *(ptr->LSV + i);
}
for (j = 0; j < nu; j++)
{
for (i = j; i < nu; i++)
{
ij = i + j * nu;
ji = j + i * nu;
*(y3 + ij) = *(ptr->LVT + ji);
*(y3 + ji) = *(ptr->LVT + ij);
}
}
}
}
void sci_zeroOrderHold(scicos_block *block,scicos::enumScicosFlags flag)
{
// get block data pointers, etc
double *_z=GetDstate(block);
double *_u1=GetRealInPortPtrs(block,1);
double *_u2=GetRealInPortPtrs(block,2);
double *_y1=GetRealOutPortPtrs(block,1);
int *_ipar=GetIparPtrs(block);
int & evtFlag = GetNevIn(block);
int & evtPortTime = _ipar[0];
int & evtPortReset = _ipar[1];
// compute flags
int evtFlagTime = scicos::evtPortNumToFlag(evtPortTime);
int evtFlagReset = scicos::evtPortNumToFlag(evtPortReset);
// loop over all rows of data
int i,j;
int nRows = GetInPortRows(block,1);
int nCols = GetInPortCols(block,1);
size_t nBytes = sizeof(double)*nRows*nCols;
for(i=0; i<nRows; i++) {
for(j=0; j<nCols; j++) {
if (flag ==scicos::computeOutput || flag ==scicos::reinitialize || flag ==scicos::initialize)
memcpy(_y1,_z,nBytes);
else if (flag == scicos::updateState)
{
// bitwise comparison for flag
if(evtFlag & evtFlagReset && _u2)
{
memcpy(_z,_u2,nBytes);
}
else if(evtFlag & evtFlagTime && _u1)
{
memcpy(_z,_u1,nBytes);
}
else
{
printf("\nunhandled event flat %d\n",evtFlag);
printf("\nknown flags:\n");
printf("\ttime flag: %d\n",evtFlagTime);
printf("\ttime flag & event flag: %d\n",evtFlagTime & evtFlag);
printf("\treset flag: %d\n",evtFlagReset);
printf("\treset flag & event flag: %d\n",evtFlagReset & evtFlag);
}
}
else if (flag == scicos::terminate)
{
}
else
{
char msg[50];
sprintf(msg,"unhandled block flag %d\n",flag);
Coserror(msg);
}
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void mat_bksl(scicos_block *block,int flag)
{
double *u1 = NULL;
double *u2 = NULL;
double *y = NULL;
int mu = 0;
int nu1 = 0;
int nu2 = 0;
int info = 0;
int i = 0,l = 0,lw = 0,lu = 0;
mat_bksl_struct *ptr = NULL;
double rcond = 0., ANORM = 0., EPS = 0.;
mu =GetInPortRows(block,1);
nu1 =GetInPortCols(block,1);
nu2 =GetInPortCols(block,2);
u1=GetRealInPortPtrs(block,1);
u2=GetRealInPortPtrs(block,2);
y=GetRealOutPortPtrs(block,1);
l=Max(mu,nu1);
lu=Max(4*nu1,Min(mu,nu1)+3*nu1+1);
lw=Max(lu,2*Min(mu,nu1)+nu2);
/*init : initialization*/
if (flag==4)
{if((*(block->work)=(mat_bksl_struct*) scicos_malloc(sizeof(mat_bksl_struct)))==NULL)
{set_block_error(-16);
return;}
ptr=*(block->work);
if((ptr->ipiv=(int*) scicos_malloc(sizeof(int)*nu1))==NULL)
{set_block_error(-16);
scicos_free(ptr);
return;}
if((ptr->rank=(int*) scicos_malloc(sizeof(int)))==NULL)
{set_block_error(-16);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;}
if((ptr->jpvt=(int*) scicos_malloc(sizeof(int)*nu1))==NULL)
{set_block_error(-16);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;}
if((ptr->iwork=(int*) scicos_malloc(sizeof(int)*nu1))==NULL)
{set_block_error(-16);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;}
if((ptr->dwork=(double*) scicos_malloc(sizeof(double)*lw))==NULL)
{set_block_error(-16);
scicos_free(ptr->iwork);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;}
if((ptr->LAF=(double*) scicos_malloc(sizeof(double)*(mu*nu1)))==NULL)
{set_block_error(-16);
scicos_free(ptr->dwork);
scicos_free(ptr->iwork);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;}
if((ptr->LA=(double*) scicos_malloc(sizeof(double)*(mu*nu1)))==NULL)
{set_block_error(-16);
scicos_free(ptr->LAF);
scicos_free(ptr->dwork);
scicos_free(ptr->iwork);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;}
if((ptr->LXB=(double*) scicos_malloc(sizeof(double)*(l*nu2)))==NULL)
{set_block_error(-16);
scicos_free(ptr->LA);
scicos_free(ptr->LAF);
scicos_free(ptr->dwork);
scicos_free(ptr->iwork);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;}
}
/* Terminaison */
else if (flag==5)
{ptr=*(block->work);
if(ptr->LXB!=NULL){
scicos_free(ptr->ipiv);
scicos_free(ptr->rank);
scicos_free(ptr->jpvt);
scicos_free(ptr->iwork);
scicos_free(ptr->LAF);
scicos_free(ptr->LA);
scicos_free(ptr->LXB);
scicos_free(ptr->dwork);
scicos_free(ptr);
return;}
}
else
{
ptr=*(block->work);
EPS=C2F(dlamch)("e",1L);
ANORM=C2F(dlange)("1",&mu,&nu1,u1,&mu,ptr->dwork);
C2F(dlacpy)("F",&mu,&nu1,u1,&mu,ptr->LA,&mu);
if (mu==nu1)
{C2F(dlacpy)("F",&mu,&nu1,ptr->LA,&mu,ptr->LAF,&mu);
C2F(dgetrf)(&nu1,&nu1,ptr->LAF,&nu1,ptr->ipiv,&info);
rcond=0;
if (info==0)
{C2F(dgecon)("1",&nu1,ptr->LAF,&nu1,&ANORM,&rcond,ptr->dwork,ptr->iwork,&info);
if (rcond>pow(EPS,0.5))
{C2F(dlacpy)("F",&nu1,&nu2,u2,&nu1,ptr->LXB,&nu1);
C2F(dgetrs)("N",&nu1,&nu2,ptr->LAF,&nu1,ptr->ipiv,ptr->LXB,&nu1,&info);
C2F(dlacpy)("F",&nu1,&nu2,ptr->LXB,&nu1,y,&nu1);
return;
}
}
}
rcond=pow(EPS,0.5);
C2F(dlacpy)("F",&mu,&nu2,u2,&mu,ptr->LXB,&l);
for (i=0;i<nu1;i++) *(ptr->jpvt+i)=0;
C2F(dgelsy1)(&mu,&nu1,&nu2,ptr->LA,&mu,ptr->LXB,&l,ptr->jpvt,&rcond,ptr->rank,ptr->dwork,&lw,&info);
if (info!=0)
{if (flag!=6)
{set_block_error(-7);
return;
}
}
C2F(dlacpy)("F",&nu1,&nu2,ptr->LXB,&l,y,&nu1);
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void integralz_func(scicos_block *block, int flag)
{
int i = 0;
double *ur = NULL, *ui = NULL;
double *yr = NULL, *yi = NULL;
ur = GetRealInPortPtrs(block, 1);
ui = GetImagInPortPtrs(block, 1);
yr = GetRealOutPortPtrs(block, 1);
yi = GetImagOutPortPtrs(block, 1);
if (flag == 0)
{
if (block->ng > 0)
{
for (i = 0; i < (block->nx) / 2; ++i)
{
if (block->mode[i] == 3)
{
block->xd[i] = ur[i];
block->xd[i + (block->nx) / 2] = ui[i];
}
else
{
block->xd[i] = 0.0;
block->xd[i + (block->nx) / 2] = 0.0;
}
}
}
else
{
for (i = 0; i < (block->nx) / 2; ++i)
{
block->xd[i] = ur[i];
block->xd[i + (block->nx) / 2] = ui[i];
}
}
}
else if (flag == 1 || flag == 6)
{
for (i = 0; i < (block->nx) / 2; ++i)
{
yr[i] = block->x[i];
yi[i] = block->x[i + (block->nx) / 2];
}
}
else if (flag == 2 && block->nevprt == 1)
{
for (i = 0; i < (block->nx) / 2; ++i)
{
block->x[i] = ur[i];
block->x[i + (block->nx) / 2] = ui[i];
}
}
else if (flag == 9)
{
for (i = 0; i < (block->nx) / 2; ++i)
{
if (block->mode[i] == 3)
{
block->g[i] = (block->x[i] - (block->rpar[i])) * (block->x[i] - (block->rpar[(block->nx) / 2 + i]));
block->g[i + (block->nx) / 2] = (block->x[i + (block->nx) / 2] - (block->rpar[i + (block->nx)])) * (block->x[i + (block->nx) / 2] - (block->rpar[3 * (block->nx) / 2 + i]));
}
else
{
block->g[i] = ur[i];
block->g[i + (block->nx) / 2] = ui[i];
}
if (get_phase_simulation() == 1)
{
if ((ur[i] >= 0) && (block->x[i] >= block->rpar[i])
&& (fpclassify(ui[i >= 0]) != FP_ZERO)
&& (block->x[i + (block->nx) / 2] >= block->rpar[i + (block->nx)]))
{
block->mode[i] = 1;
}
else if (ur[i] <= 0 && block->x[i] <= block->rpar[(block->nx) / 2 + i] && ui[i] <= 0 && block->x[i + (block->nx) / 2] <= block->rpar[3 * (block->nx) / 2 + i])
{
block->mode[i] = 2;
}
else
{
block->mode[i] = 3;
}
}
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void matz_bksl(scicos_block *block,int flag)
{
double *u1r = NULL,*u1i = NULL;
double *u2r = NULL,*u2i = NULL;
double *yr = NULL,*yi = NULL;
int mu = 0,vu = 0,wu = 0;
int nu1 = 0;
int nu2 = 0;
int info = 0;
int i = 0,j = 0,l = 0,lw = 0,lu = 0,rw = 0,ij = 0,k = 0;
mat_bksl_struct *ptr = NULL;
double rcond = 0., ANORM = 0., EPS = 0.;
vu =GetOutPortRows(block,1);
wu =GetOutPortCols(block,1);
mu =GetInPortRows(block,1);
nu1 =GetInPortCols(block,1);
nu2 =GetInPortCols(block,2);
u1r=GetRealInPortPtrs(block,1);
u1i=GetImagInPortPtrs(block,1);
u2r=GetRealInPortPtrs(block,2);
u2i=GetImagInPortPtrs(block,2);
yr=GetRealOutPortPtrs(block,1);
yi=GetImagOutPortPtrs(block,1);
l=Max(mu,nu1);
lw=Max(2*Min(mu,nu1),nu1+1);
lu=Max(lw,Min(mu,nu1)+nu2);
lw=Max(2*nu1,Min(mu,nu1)+lu);
rw=2*nu1;
/*init : initialization*/
if (flag==4)
{
if((*(block->work)=(mat_bksl_struct*) scicos_malloc(sizeof(mat_bksl_struct)))==NULL)
{
set_block_error(-16);
return;
}
ptr=*(block->work);
if((ptr->ipiv=(int*) scicos_malloc(sizeof(int)*nu1))==NULL)
{
set_block_error(-16);
scicos_free(ptr);
return;
}
if((ptr->rank=(int*) scicos_malloc(sizeof(int)))==NULL)
{
set_block_error(-16);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
if((ptr->jpvt=(int*) scicos_malloc(sizeof(int)*nu1))==NULL)
{
set_block_error(-16);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
if((ptr->iwork=(double*) scicos_malloc(sizeof(double)*2*nu1))==NULL)
{set_block_error(-16);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
if((ptr->dwork=(double*) scicos_malloc(sizeof(double)*2*lw))==NULL)
{
set_block_error(-16);
scicos_free(ptr->iwork);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
if((ptr->IN1F=(double*) scicos_malloc(sizeof(double)*(2*mu*nu1)))==NULL)
{
set_block_error(-16);
scicos_free(ptr->dwork);
scicos_free(ptr->iwork);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
if((ptr->IN1=(double*) scicos_malloc(sizeof(double)*(2*mu*nu1)))==NULL)
{
set_block_error(-16);
scicos_free(ptr->IN1F);
scicos_free(ptr->dwork);
scicos_free(ptr->iwork);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
if((ptr->IN2X=(double*) scicos_malloc(sizeof(double)*(2*l*nu2)))==NULL)
{
set_block_error(-16);
scicos_free(ptr->IN1);
scicos_free(ptr->IN1F);
scicos_free(ptr->dwork);
scicos_free(ptr->iwork);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
if((ptr->IN2=(double*) scicos_malloc(sizeof(double)*(2*mu*nu2)))==NULL)
{
set_block_error(-16);
scicos_free(ptr->IN2);
scicos_free(ptr->IN1);
scicos_free(ptr->IN1F);
scicos_free(ptr->dwork);
scicos_free(ptr->iwork);
scicos_free(ptr->jpvt);
scicos_free(ptr->rank);
scicos_free(ptr->ipiv);
scicos_free(ptr);
return;
}
}
/* Terminaison */
else if (flag==5)
{
ptr=*(block->work);
if((ptr->IN2)!=NULL)
{
scicos_free(ptr->ipiv);
scicos_free(ptr->rank);
scicos_free(ptr->jpvt);
scicos_free(ptr->iwork);
scicos_free(ptr->IN1F);
scicos_free(ptr->IN1);
scicos_free(ptr->IN2X);
scicos_free(ptr->IN2);
scicos_free(ptr->dwork);
scicos_free(ptr);
return;
}
}
else
{
ptr=*(block->work);
for (i=0;i<(mu*nu1);i++)
{
ptr->IN1[2*i]=u1r[i];
ptr->IN1[2*i+1]=u1i[i];
}
for (i=0;i<(mu*nu2);i++)
{
ptr->IN2[2*i]=u2r[i];
ptr->IN2[2*i+1]=u2i[i];
}
EPS=C2F(dlamch)("e",1L);
ANORM=C2F(zlange)("1",&mu,&nu1,ptr->IN1,&mu,ptr->dwork);
if (mu==nu1)
{
C2F(zlacpy)("F",&mu,&nu1,ptr->IN1,&mu,ptr->IN1F,&mu);
C2F(zgetrf)(&nu1,&nu1,ptr->IN1F,&nu1,ptr->ipiv,&info);
rcond=0;
if (info==0)
{
C2F(zgecon)("1",&nu1,ptr->IN1F,&nu1,&ANORM,&rcond,ptr->dwork,ptr->iwork,&info);
if (rcond>pow(EPS,0.5))
{
C2F(zgetrs)("N",&nu1,&nu2,ptr->IN1F,&nu1,ptr->ipiv,ptr->IN2,&nu1,&info);
for (i=0;i<(mu*nu2);i++)
{*(yr+i)=*(ptr->IN2+2*i);
*(yi+i)=*(ptr->IN2+(2*i)+1);}
return;
}
}
}
rcond=pow(EPS,0.5);
for (i=0;i<nu1;i++) *(ptr->jpvt+i)=0;
C2F(zlacpy)("F",&mu,&nu2,ptr->IN2,&mu,ptr->IN2X,&l);
C2F(zgelsy1)(&mu,&nu1,&nu2,ptr->IN1,&mu,ptr->IN2X,&l,ptr->jpvt,&rcond,ptr->rank,ptr->dwork,&lw,ptr->iwork,&info);
if (info!=0)
{
if (flag!=6)
{
set_block_error(-7);
return;
}
}
k=0;
for(j=0;j<wu;j++)
{
for(i=0;i<vu;i++)
{
ij=i+j*l;
*(yr+k)=*(ptr->IN2X+2*ij);
*(yi+k)=*(ptr->IN2X+(2*ij)+1);
k++;
}
}
}
}
/*--------------------------------------------------------------------------*/
SCICOS_BLOCKS_IMPEXP void lookup_c(scicos_block *block, int flag)
{
double a = 0., b = 0., c = 0., y1 = 0., y2 = 0., t1 = 0., t2 = 0.;
int *ind = NULL, inow = 0, i = 0, ip1 = 0;
double *y = NULL, *u = NULL, u0 = 0.;
double d1 = 0., d2 = 0., h = 0., dh = 0., ddh = 0., dddh = 0.;
switch (flag)
{
/* init */
case 4 : /* the workspace is used to store discrete counter value */
{
if ((*block->work = scicos_malloc(1 * sizeof(int))) == NULL)
{
set_block_error(-16);
return;
}
ind = *block->work;
ind[0] = 0;
return;
}
/* event date computation */
case 1 :
{
y = GetRealOutPortPtrs(block, 1);
u = GetRealInPortPtrs(block, 1);
u0 = u[0];
ind = *block->work;
i = ind[0];
ip1 = i + 1;
if ((Extrapo == 0) || ((Extrapo == 1) && ((Order == 0) || (Order == 8) || (Order == 9))))
{
if (u0 < RPAR[0])
{
y[0] = RPAR[nPoints];
break;
}
if (u0 >= RPAR[nPoints - 1])
{
y[0] = RPAR[nPoints * 2 - 1];
break;
}
}
if (u0 < RPAR[i])
{
i = FindIndex(Order, u0, 0, i, RPAR, nPoints);
}
else if (u0 >= RPAR[ip1])
{
i = FindIndex(Order, u0, ip1, nPoints - 1, RPAR, nPoints);
}
ind[0] = i;
if (Order == 0) /* (METHOD=='zero order-below')*/
{
y[0] = RPAR[nPoints + i];
break;
}
if (Order == 8) /* (METHOD=='zero order-above')*/
{
y[0] = RPAR[nPoints + i + 1];
break;
}
if (Order == 9) /* (METHOD=='zero order-nearest')*/
{
if (u0 < (RPAR[i] + RPAR[i + 1]) / 2 )
{
y[0] = RPAR[nPoints + i];
}
else
{
y[0] = RPAR[nPoints + i + 1];
}
break;
}
if (Order == 1)
{
t1 = RPAR[i];
t2 = RPAR[i + 1];
y1 = RPAR[nPoints + i];
y2 = RPAR[nPoints + i + 1];
y[0] = (y2 - y1) * (u0 - t1) / (t2 - t1) + y1;
break;
}
if ((Order == 2) && (nPoints > 2))
{
t1 = RPAR[i];
a = RPAR[2 * nPoints + i];
b = RPAR[2 * nPoints + i + nPoints - 1];
c = RPAR[2 * nPoints + i + 2 * nPoints - 2];
y[0] = a * (u0 - t1) * (u0 - t1) + b * (u0 - t1) + c;
break;
}
if ((Order >= 3) && (Order <= 7))
{
t1 = RPAR[i];
t2 = RPAR[i + 1];
y1 = RPAR[nPoints + i];
y2 = RPAR[nPoints + i + 1];
d1 = RPAR[2 * nPoints + i];
d2 = RPAR[2 * nPoints + i + 1];
/*-- this function is defined in curve_c.c ---*/
scicos_evalhermite(&u0, &t1, &t2, &y1, &y2, &d1, &d2, &h, &dh, &ddh, &dddh, &inow);
y[0] = h;
break;
}
}
/* event date computation */
case 3 :
{
/* ind=*block->work;
i=ind[0];
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;
}
}
