00001
00002
00003
00004
00005
00006
00007
00008
00009 #include <stdlib.h>
00010 #include <string.h>
00011
00012 #include "machine.h"
00013
00014
00015 #include "link.h"
00016
00017
00018 #include "scicos.h"
00019 #include "import.h"
00020 #include "blocks.h"
00021 #include <math.h>
00022 #include "core_math.h"
00023 #include "sci_mem_alloc.h"
00024
00025
00026 #include "scicos-def.h"
00027 #include "sciprint.h"
00028
00029
00030 typedef struct {
00031 integer iero;
00032 } IERSCODE_struct;
00033
00034 IERSCODE_struct C2F(ierscode);
00035
00036
00037 #define abs(x) ((x) >= 0 ? (x) : -(x))
00038 #ifndef _MSC_VER
00039 #define max(a,b) ((a) >= (b) ? (a) : (b))
00040 #define min(a,b) ((a) <= (b) ? (a) : (b))
00041 #endif
00042 #define freeall \
00043 FREE(rhot);\
00044 FREE(ihot);\
00045 FREE(jroot);\
00046 FREE(zcros);
00047
00048 #define freeallx \
00049 FREE(rhot);\
00050 FREE(ihot);\
00051 FREE(jroot);\
00052 FREE(zcros);\
00053 FREE(scicos_xproperty);\
00054 FREE(Mode_save);
00055
00056 #define freeouttbptr \
00057 FREE(outtbd);\
00058 FREE(outtbc);\
00059 FREE(outtbs);\
00060 FREE(outtbl);\
00061 FREE(outtbuc);\
00062 FREE(outtbus);\
00063 FREE(outtbul);
00064
00065 void cosini(double *);
00066 void idoit(double *);
00067 void cosend(double *);
00068 void cdoit(double *);
00069 void doit(double *);
00070 void ddoit(double *);
00071 void edoit(double *,integer *);
00072 void odoit(double *,double *,double *,double *);
00073 void ozdoit(double *,double *,double *,integer *);
00074 void zdoit(double *,double *,double *,double *);
00075 void reinitdoit(double *,integer *);
00076 void cossimdaskr(double *);
00077 void cossim(double *);
00078 void callf(double *, double *, double *, double *,double *,integer *);
00079 int C2F(simblk)(integer *, double *, double *, double *);
00080 int C2F(simblkdaskr)(double *, double *, double *, double *, double *, integer *, double *, integer *);
00081 int C2F(grblk)(integer *, double *, double *, integer *, double *);
00082 int C2F(grblkdaskr)(integer *, double *, double *, double *, integer *, double *, double *, integer *);
00083 void addevs(double ,integer *,integer *);
00084 void putevs(double *,integer *,integer *);
00085 void FREE_blocks();
00086 int setmode(double *,double *,double *,integer *,double);
00087
00088
00089 void Jdoit(double *,double *,double *,double *,int *);
00090 int C2F(Jacobian)(double *, double *, double *, double *, double *, double *, int *);
00091 void Multp(double *, double *,double *, int, int, int ,int);
00092 void Set_Jacobian_flag(int flag);
00093 double Get_Jacobian_parameter(void);
00094 double Get_Scicos_SQUR(void);
00095
00096
00097
00098 extern void F2C(sciblk)();
00099 extern void sciblk2();
00100 extern void sciblk4();
00101 extern void GetDynFunc();
00102 extern void C2F(iislink)();
00103
00104
00105 extern integer C2F(cvstr)();
00106 extern integer C2F(dset)();
00107 extern integer C2F(dcopy)();
00108 extern integer C2F(iset)();
00109 extern integer C2F(realtime)();
00110 extern integer C2F(realtimeinit)();
00111 extern integer C2F(sxevents)();
00112 extern integer C2F(stimer)();
00113 extern integer C2F(xscion)();
00114 extern integer C2F(ddaskr)();
00115 extern integer C2F(lsodar2)();
00116
00117 extern int scilab_timer_check();
00118
00119 ScicosImport scicos_imp;
00120
00121 static integer nblk, nordptr, nlnk, ng, ncord, noord, nzord,niord,
00122 nclock,nordclk,niord,nmod;
00123
00124 static integer *neq;
00125
00126 static double Atol, rtol, ttol, deltat,hmax;
00127 static integer hot;
00128
00129
00130
00131 static integer c__90 = 90;
00132 static integer c__0 = 0;
00133 static integer c__91 = 91;
00134 static double c_b14 = 0.;
00135 static integer c__1 = 1;
00136 static integer panj=5;
00137
00138 static integer *iwa;
00139
00140 static integer *xptr,*modptr, *evtspt;
00141 static integer *funtyp, *inpptr, *outptr, *inplnk, *outlnk;
00142 static integer *clkptr, *ordptr, *ordclk, *cord,
00143 *iord, *oord, *zord, *critev, *zcptr;
00144 static integer *pointi;
00145 static integer *ierr;
00146
00147 static double *x,*xd,*tevts,*g;
00148 static integer *mod;
00149 static double *t0,*tf,scicos_time;
00150
00151
00152 static void **outtbptr;
00153 static integer *outtbsz;
00154 static integer *outtbtyp;
00155 double *outtbdptr;
00156 char *outtbcptr;
00157 short *outtbsptr;
00158 long *outtblptr;
00159 unsigned char *outtbucptr;
00160 unsigned short *outtbusptr;
00161 unsigned long *outtbulptr;
00162 static outtb_el *outtb_elem;
00163 static int nelem;
00164 static scicos_block *Blocks;
00165
00166 static integer phase;
00167
00168 integer *pointer_xproperty;
00169
00170 integer n_pointer_xproperty;
00171
00172 static integer *block_error;
00173
00174 static integer Jacobian_Flag;
00175 static double CJJ;
00176 static double SQuround;
00177
00178
00179 void call_debug_scicos(double *, double *, double *, double *,double *,integer
00180 *,integer,integer,integer);
00181
00182 static integer debug_block;
00183
00184
00185 int C2F(scicos)(
00186 double *x_in, integer *xptr_in, double *z__,
00187 void **work,integer *zptr,integer *modptr_in,
00188
00189 integer *iz,integer *izptr,
00190 double *t0_in,double *tf_in,double *tevts_in,
00191 integer *evtspt_in,integer *nevts,integer *pointi_in,
00192 void **outtbptr_in, integer *outtbsz_in,integer *outtbtyp_in,
00193 outtb_el *outtb_elem_in,integer *nelem1,integer *nlnk1,
00194 integer *funptr,integer *funtyp_in,integer *inpptr_in,integer *outptr_in,
00195 integer *inplnk_in,integer *outlnk_in,
00196 double *rpar,
00197 integer *rpptr,integer *ipar,integer *ipptr,integer *clkptr_in,integer *ordptr_in,integer *nordptr1,
00198 integer *ordclk_in,integer *cord_in,integer *ncord1,integer *iord_in,integer *niord1,integer *oord_in,
00199 integer *noord1,integer *zord_in,integer *nzord1,integer *critev_in,integer *nblk1,integer *ztyp,integer *zcptr_in,
00200 integer *subscr,integer *nsubs,
00201 double *simpar,
00202 integer *flag__,integer *ierr_out)
00203
00204 {
00205 integer i1,kf,lprt,in,out,job=1;
00206
00207 extern int C2F(msgs)();
00208 static integer mxtb, ierr0, kfun0, i, j, k;
00209 extern int C2F(makescicosimport)();
00210 extern int C2F(getscsmax)();
00211 static integer ni, no;
00212 extern int C2F(clearscicosimport)();
00213 static integer nx, nz;
00214 double *W;
00215
00216
00217
00218
00219 t0=t0_in;
00220 tf=tf_in;
00221 ierr=ierr_out;
00222
00223
00224 pointi=pointi_in;
00225 x=x_in;
00226 xptr=xptr_in-1;
00227 modptr=modptr_in-1;
00228 --zptr;
00229 --izptr;
00230 evtspt=evtspt_in-1;
00231 tevts=tevts_in-1;
00232 outtbptr=outtbptr_in;
00233 outtbsz=outtbsz_in;
00234 outtbtyp=outtbtyp_in;
00235 outtb_elem=outtb_elem_in;
00236 funtyp=funtyp_in-1;
00237 inpptr=inpptr_in-1;
00238 outptr=outptr_in-1;
00239 inplnk=inplnk_in-1;
00240 outlnk=outlnk_in-1;
00241 --rpptr;
00242 --ipptr;
00243 clkptr=clkptr_in-1;
00244 ordptr=ordptr_in-1;
00245 ordclk=ordclk_in-1;
00246 cord=cord_in-1;
00247 iord=iord_in-1;
00248 oord=oord_in-1;
00249 zord=zord_in-1;
00250
00251 critev=critev_in-1;
00252 --ztyp;
00253 zcptr=zcptr_in-1;
00254 --simpar;
00255
00256
00257 Atol = simpar[1];
00258 rtol = simpar[2];
00259 ttol = simpar[3];
00260 deltat = simpar[4];
00261 C2F(rtfactor).scale = simpar[5];
00262 C2F(cmsolver).solver = (integer) simpar[6];
00263 hmax=simpar[7];
00264
00265 nordptr = *nordptr1;
00266 nblk = *nblk1;
00267 ncord = *ncord1;
00268 noord = *noord1;
00269 nzord = *nzord1;
00270 niord = *niord1;
00271 nlnk = *nlnk1;
00272 nelem = *nelem1;
00273 *ierr = 0;
00274
00275 xd=&x[xptr[nblk+1]-1];
00276
00277 nordclk=ordptr[nordptr]-1;
00278
00279 ng = zcptr[nblk + 1] - 1;
00280 nmod = modptr[nblk + 1] - 1;
00281
00282
00283 nz = zptr[nblk + 1] - 1;
00284
00285 nx = xptr[nblk +1] - 1;
00286 neq=&nx;
00287
00288
00289
00290 for (i = 1; i <= nblk; ++i) {
00291 if (funtyp[i] < 10000) {
00292 funtyp[i] %= 1000;
00293 } else {
00294 funtyp[i] = funtyp[i] % 1000 + 10000;
00295 }
00296 ni = inpptr[i + 1] - inpptr[i];
00297 no = outptr[i + 1] - outptr[i];
00298 if (funtyp[i] == 1) {
00299 if (ni + no > 11) {
00300
00301 C2F(msgs)(&c__90, &c__0);
00302 C2F(curblk).kfun = i;
00303 *ierr = i + 1005;
00304 return 0;
00305 }
00306 } else if (funtyp[i] == 2 || funtyp[i] == 3) {
00307
00308 if (ni + no > SZ_SIZE) {
00309 C2F(msgs)(&c__90, &c__0);
00310 C2F(curblk).kfun = i;
00311 *ierr = i + 1005;
00312 return 0;
00313 }
00314 }
00315 mxtb = 0;
00316 if (funtyp[i] == 0) {
00317 if (ni > 1) {
00318 for (j = 1; j <= ni; ++j) {
00319 k = inplnk[inpptr[i] - 1 + j];
00320 mxtb = mxtb + (outtbsz[2*(k-1)]*outtbsz[2*(k-1)+1]);
00321 }
00322 }
00323 if (no > 1) {
00324 for (j = 1; j <= no; ++j) {
00325 k = outlnk[outptr[i] - 1 + j];
00326 mxtb = mxtb + (outtbsz[2*(k-1)]*outtbsz[2*(k-1)+1]);
00327 }
00328 }
00329 if (mxtb > TB_SIZE) {
00330 C2F(msgs)(&c__91, &c__0);
00331 C2F(curblk).kfun = i;
00332 *ierr = i + 1005;
00333 return 0;
00334 }
00335 }
00336 }
00337
00338 if((Blocks=MALLOC(sizeof(scicos_block)*nblk))== NULL ){
00339 *ierr =5;
00340 return 0;
00341 }
00342 if(nmod>0){
00343 if((mod=MALLOC(sizeof(int)*nmod))== NULL ){
00344 *ierr =5;
00345 FREE(Blocks);
00346 return 0;
00347 }
00348 }
00349 if(ng>0){
00350 if((g=MALLOC(sizeof(double)*ng))== NULL ){
00351 *ierr =5;
00352 if(nmod>0){
00353 FREE(mod);
00354 }
00355 FREE(Blocks);
00356 return 0;
00357 }
00358 }
00359 debug_block=-1;
00360 C2F(cosdebugcounter).counter=0;
00361
00362 for (kf = 0; kf < nblk; ++kf) {
00363 C2F(curblk).kfun = kf+1;
00364 i=funptr[kf];
00365 Blocks[kf].type=funtyp[kf+1];
00366 if (i<0) {
00367 switch (funtyp[kf+1]) {
00368 case 0:
00369 Blocks[kf].funpt=F2C(sciblk);
00370 break;
00371 case 1:
00372 sciprint("type 1 function not allowed for scilab blocks\r\n");
00373 *ierr =1000+kf+1;
00374 FREE_blocks();
00375 return 0;
00376 case 2:
00377 sciprint("type 2 function not allowed for scilab blocks\r\n");
00378 *ierr =1000+kf+1;
00379 FREE_blocks();
00380 return 0;
00381 case 3:
00382 Blocks[kf].funpt=sciblk2;
00383 Blocks[kf].type=2;
00384 break;
00385 case 5:
00386 Blocks[kf].funpt=sciblk4;
00387 Blocks[kf].type=4;
00388 break;
00389 case 99:
00390 Blocks[kf].funpt=sciblk4;
00391 Blocks[kf].type=4;
00392 debug_block=kf;
00393 break;
00394
00395 case 10005:
00396 Blocks[kf].funpt=sciblk4;
00397 Blocks[kf].type=10004;
00398 break;
00399 default :
00400 sciprint("Undefined Function type\r\n");
00401 *ierr =1000+kf+1;
00402 FREE_blocks();
00403 return 0;
00404 }
00405 Blocks[kf].scsptr=-i;
00406 }
00407 else if (i<=ntabsim){
00408 Blocks[kf].funpt=*(tabsim[i-1].fonc);
00409 Blocks[kf].scsptr=0;
00410
00411
00412 }
00413 else {
00414 i -= (ntabsim+1);
00415 GetDynFunc(i,&Blocks[kf].funpt);
00416 if ( Blocks[kf].funpt == (voidf) 0) {
00417 sciprint("Function not found\r\n");
00418 *ierr =1000+kf+1;
00419 FREE_blocks();
00420 return 0;
00421 }
00422 Blocks[kf].scsptr=0;
00423
00424
00425 }
00426 Blocks[kf].ztyp=ztyp[kf+1];
00427 Blocks[kf].nx=xptr[kf+2]-xptr[kf+1];
00428 Blocks[kf].ng=zcptr[kf+2]-zcptr[kf+1];
00429 Blocks[kf].nz=zptr[kf+2]-zptr[kf+1];
00430 Blocks[kf].nrpar=rpptr[kf+2]-rpptr[kf+1];
00431 Blocks[kf].nipar=ipptr[kf+2]-ipptr[kf+1];
00432 Blocks[kf].nin=inpptr[kf+2]-inpptr[kf+1];
00433 Blocks[kf].nout=outptr[kf+2]-outptr[kf+1];
00434
00435
00436
00437
00438
00439
00440 if ((Blocks[kf].insz=MALLOC(Blocks[kf].nin*3*sizeof(int)))== NULL ){
00441 FREE_blocks();
00442 *ierr =5;
00443 return 0;
00444 }
00445 if ((Blocks[kf].inptr=MALLOC(Blocks[kf].nin*sizeof(double*)))== NULL ){
00446 FREE_blocks();
00447 *ierr =5;
00448 return 0;
00449 }
00450 for(in=0;in<Blocks[kf].nin;in++) {
00451 lprt=inplnk[inpptr[kf+1]+in];
00452 Blocks[kf].inptr[in]=outtbptr[lprt-1];
00453 Blocks[kf].insz[in]=outtbsz[2*(lprt-1)];
00454 Blocks[kf].insz[Blocks[kf].nin+in]=outtbsz[2*(lprt-1)+1];
00455 Blocks[kf].insz[2*Blocks[kf].nin+in]=outtbtyp[lprt-1];
00456 }
00457
00458
00459
00460
00461
00462
00463 if ((Blocks[kf].outsz=MALLOC(Blocks[kf].nout*3*sizeof(int)))== NULL ){
00464 FREE_blocks();
00465 *ierr =5;
00466 return 0;
00467 }
00468 if ((Blocks[kf].outptr=MALLOC(Blocks[kf].nout*sizeof(double*)))== NULL ){
00469 FREE_blocks();
00470 *ierr =5;
00471 return 0;
00472 }
00473 for(out=0;out<Blocks[kf].nout;out++) {
00474 lprt=outlnk[outptr[kf+1]+out];
00475 Blocks[kf].outptr[out]=outtbptr[lprt-1];
00476 Blocks[kf].outsz[out]=outtbsz[2*(lprt-1)];
00477 Blocks[kf].outsz[Blocks[kf].nout+out]=outtbsz[2*(lprt-1)+1];
00478 Blocks[kf].outsz[2*Blocks[kf].nout+out]=outtbtyp[lprt-1];
00479 }
00480
00481 Blocks[kf].nevout=clkptr[kf+2] - clkptr[kf+1];
00482 if ((Blocks[kf].evout=CALLOC(Blocks[kf].nevout,sizeof(double)))== NULL ){
00483 FREE_blocks();
00484 *ierr =5;
00485 return 0;
00486 }
00487
00488 Blocks[kf].z=&(z__[zptr[kf+1]-1]);
00489 Blocks[kf].rpar=&(rpar[rpptr[kf+1]-1]);
00490 Blocks[kf].ipar=&(ipar[ipptr[kf+1]-1]);
00491
00492 if ((Blocks[kf].res=MALLOC(Blocks[kf].nx*sizeof(double)))== NULL ){
00493 FREE_blocks();
00494 *ierr =5;
00495 return 0;
00496 }
00497
00498 i1=izptr[kf+2]-izptr[kf+1];
00499 if ((Blocks[kf].label=MALLOC(sizeof(char)*(i1+1)))== NULL ){
00500 FREE_blocks();
00501 *ierr =5;
00502 return 0;
00503 }
00504 Blocks[kf].label[i1]='\0';
00505 C2F(cvstr)(&i1,&(iz[izptr[kf+1]-1]),Blocks[kf].label,&job,i1);
00506 if ((Blocks[kf].jroot=CALLOC(Blocks[kf].ng,sizeof(int)))== NULL ){
00507 FREE_blocks();
00508 *ierr =5;
00509 return 0;
00510 }
00511
00512 Blocks[kf].work=(void **)(((double *)work)+kf);
00513 Blocks[kf].nmode=modptr[kf+2]-modptr[kf+1];
00514 if ( Blocks[kf].nmode!=0){
00515 Blocks[kf].mode=&(mod[modptr[kf+1]-1]);
00516 }
00517 }
00518
00519
00520 if((iwa=MALLOC(sizeof(int)*(*nevts)))== NULL ){
00521 FREE_blocks();
00522 *ierr =5;
00523 return 0;
00524 }
00525
00526
00527 C2F(makescicosimport)(x, &nx, &xptr[1], &zcptr[1], z__, &nz, &zptr[1],g , &ng, mod, &nmod, &modptr[1],
00528 iz, &izptr[1], &inpptr[1], &inplnk[1], &outptr[1], &outlnk[1], outtbptr,
00529 outtbsz, outtbtyp, outtb_elem,&nelem,&nlnk, rpar, &rpptr[1], ipar, &ipptr[1],
00530 &nblk,
00531 subscr, nsubs, &tevts[1], &evtspt[1], nevts, pointi, &iord[1], &niord,
00532 &oord[1], &noord, &zord[1], &nzord, funptr, &funtyp[1],
00533 &ztyp[1], &cord[1], &ncord,
00534 &ordclk[1], &clkptr[1], &ordptr[1], &nordptr,
00535 &critev[1], iwa, Blocks,t0 ,tf , &Atol, &rtol, &ttol, &deltat, &hmax);
00536
00537 if (*flag__ == 1) {
00538
00539 for (kf = 0; kf < nblk; ++kf) {
00540 *(Blocks[kf].work)=NULL;
00541 }
00542 cosini(t0);
00543 if (*ierr != 0) {
00544 ierr0=*ierr;
00545 kfun0 = C2F(curblk).kfun;
00546 cosend(t0);
00547 *ierr=ierr0;
00548 C2F(curblk).kfun = kfun0;
00549 }
00550 } else if (*flag__ == 2) {
00551
00552 if (C2F(cmsolver).solver == 0) {
00553 cossim(t0);
00554 } else if (C2F(cmsolver).solver == 100) {
00555 cossimdaskr(t0);
00556 } else {
00557
00558 }
00559 if (*ierr != 0) {
00560 ierr0=*ierr;
00561 kfun0 = C2F(curblk).kfun;
00562 cosend(t0);
00563 *ierr=ierr0;
00564 C2F(curblk).kfun = kfun0;
00565 }
00566
00567 } else if (*flag__ == 3) {
00568
00569 cosend(t0);
00570 } else if (*flag__ == 4) {
00571 phase=1;
00572 idoit(t0);
00573 if (*ierr == 0) {
00574 if((W=MALLOC(sizeof(double)*nx))== NULL ){
00575 FREE(iwa);
00576 FREE_blocks();
00577 *ierr =5;
00578 return 0;
00579 }
00580
00581 C2F(simblk)(&nx, t0, x, W);
00582 for (i = 0; i < nx; ++i) {
00583 x[i] = W[i];
00584 }
00585 FREE(W);
00586 }
00587 }
00588 FREE(iwa);
00589 FREE_blocks();
00590
00591 C2F(clearscicosimport)();
00592 return 0;
00593 }
00594
00595 void cosini(told)
00596
00597 double *told;
00598
00599 {
00600 static integer flag__;
00601 static integer i;
00602
00603 static integer kfune;
00604 static integer jj;
00605
00606 double *outtbd=NULL;
00607 char *outtbc=NULL;
00608 short *outtbs=NULL;
00609 long *outtbl=NULL;
00610 unsigned char *outtbuc=NULL;
00611 unsigned short *outtbus=NULL;
00612 unsigned long *outtbul=NULL;
00613 int szouttbd=0;
00614 int szouttbc=0, szouttbs=0, szouttbl=0;
00615 int szouttbuc=0, szouttbus=0, szouttbul=0;
00616 int curouttbd=0;
00617 int curouttbc=0, curouttbs=0, curouttbl=0;
00618 int curouttbuc=0, curouttbus=0, curouttbul=0;
00619
00620 int ii,kk;
00621 int sszz;
00622
00623
00624 for (ii=0;ii<nlnk;ii++)
00625 {
00626 switch (outtbtyp[ii])
00627 {
00628 case 10 : szouttbd+=outtbsz[2*ii]*outtbsz[2*ii+1];
00629 outtbd=(double *) REALLOC (outtbd,szouttbd*sizeof(double));
00630 break;
00631
00632 case 11 : szouttbd+=2*outtbsz[2*ii]*outtbsz[2*ii+1];
00633 outtbd=(double *) REALLOC (outtbd,szouttbd*sizeof(double));
00634 break;
00635
00636 case 81 : szouttbc+=outtbsz[2*ii]*outtbsz[2*ii+1];
00637 outtbc=(char *) REALLOC (outtbc,szouttbc*sizeof(char));
00638 break;
00639
00640 case 82 : szouttbs+=outtbsz[2*ii]*outtbsz[2*ii+1];
00641 outtbs=(short *) REALLOC (outtbs,szouttbs*sizeof(short));
00642 break;
00643
00644 case 84 : szouttbl+=outtbsz[2*ii]*outtbsz[2*ii+1];
00645 outtbl=(long *) REALLOC (outtbl,szouttbl*sizeof(long));
00646 break;
00647
00648 case 811 : szouttbuc+=outtbsz[2*ii]*outtbsz[2*ii+1];
00649 outtbuc=(unsigned char *) REALLOC (outtbuc,szouttbuc*sizeof(unsigned char));
00650 break;
00651
00652 case 812 : szouttbus+=outtbsz[2*ii]*outtbsz[2*ii+1];
00653 outtbus=(unsigned short *) REALLOC (outtbus,szouttbus*sizeof(unsigned short));
00654 break;
00655
00656 case 814 : szouttbul+=outtbsz[2*ii]*outtbsz[2*ii+1];
00657 outtbul=(unsigned long *) REALLOC (outtbul,szouttbul*sizeof(unsigned long));
00658 break;
00659
00660 default :
00661 break;
00662 }
00663 }
00664
00665
00666 Jacobian_Flag=0;
00667
00668
00669
00670 *ierr = 0;
00671
00672
00673
00674 C2F(dset)(&ng, &c_b14, g, &c__1);
00675 nclock = 0;
00676 for (C2F(curblk).kfun = 1; C2F(curblk).kfun <= nblk; ++C2F(curblk).kfun) {
00677 if (funtyp[C2F(curblk).kfun] >= 0) {
00678 flag__ = 4;
00679 callf(told, xd, x, x,g,&flag__);
00680 if (flag__ < 0 && *ierr == 0) {
00681 *ierr = 5 - flag__;
00682 kfune = C2F(curblk).kfun;
00683 }
00684 }
00685 }
00686 if (*ierr != 0) {
00687 C2F(curblk).kfun = kfune;
00688 freeouttbptr;
00689 return;
00690 }
00691
00692 nclock = 0;
00693
00694 for (jj = 1; jj <= ncord; ++jj) {
00695 C2F(curblk).kfun = cord[jj];
00696 flag__ = 6;
00697 if (funtyp[C2F(curblk).kfun] >= 0) {
00698 callf(told, xd, x, x,g,&flag__);
00699 if (flag__ < 0) {
00700 *ierr = 5 - flag__;
00701 freeouttbptr;
00702 return;
00703 }
00704 }
00705 }
00706
00707 nclock =0;
00708 for (i = 1; i <= nblk + 1; ++i) {
00709
00710 for (C2F(curblk).kfun = 1; C2F(curblk).kfun <= nblk; ++C2F(curblk).kfun) {
00711 flag__ = 6;
00712 if (funtyp[C2F(curblk).kfun] >= 0) {
00713 callf(told, xd, x, x,g,&flag__);
00714 if (flag__ < 0) {
00715 *ierr = 5 - flag__;
00716 freeouttbptr;
00717 return;
00718 }
00719 }
00720 }
00721
00722 nclock = 0;
00723
00724 for (jj = 1; jj <= ncord; ++jj) {
00725 C2F(curblk).kfun = cord[jj];
00726 flag__ = 6;
00727 if (funtyp[C2F(curblk).kfun] >= 0) {
00728 callf(told, xd, x, x,g,&flag__);
00729 if (flag__ < 0) {
00730 *ierr = 5 - flag__;
00731 freeouttbptr;
00732 return;
00733 }
00734 }
00735 }
00736
00737
00738 curouttbd=0; curouttbc=0; curouttbs=0; curouttbl=0;
00739 curouttbuc=0; curouttbus=0; curouttbul=0;
00740 for (jj=0; jj<nlnk; jj++)
00741 {
00742 switch (outtbtyp[jj])
00743 {
00744 case 10 : outtbdptr=(double *)outtbptr[jj];
00745 sszz=outtbsz[2*jj]*outtbsz[2*jj+1];
00746 for(kk=0;kk<sszz;kk++)
00747 {
00748 if(outtbdptr[kk]!=(double)outtbd[curouttbd+kk]) goto L30;
00749 }
00750 curouttbd+=sszz;
00751 break;
00752
00753 case 11 : outtbdptr=(double *)outtbptr[jj];
00754 sszz=2*outtbsz[2*jj]*outtbsz[2*jj+1];
00755 for(kk=0;kk<sszz;kk++)
00756 {
00757 if(outtbdptr[kk]!=(double)outtbd[curouttbd+kk]) goto L30;
00758 }
00759 curouttbd+=sszz;
00760 break;
00761
00762 case 81 : outtbcptr=(char *)outtbptr[jj];
00763 sszz=outtbsz[2*jj]*outtbsz[2*jj+1];
00764 for(kk=0;kk<sszz;kk++)
00765 {
00766 if(outtbcptr[kk]!=(char)outtbc[curouttbc+kk]) goto L30;
00767 }
00768 curouttbc+=sszz;
00769 break;
00770
00771 case 82 : outtbsptr=(short *)outtbptr[jj];
00772 sszz=outtbsz[2*jj]*outtbsz[2*jj+1];
00773 for (kk=0;kk<sszz;kk++)
00774 {
00775 if(outtbsptr[kk]!=(short)outtbs[curouttbs+kk]) goto L30;
00776 }
00777 curouttbs+=sszz;
00778 break;
00779
00780 case 84 : outtblptr=(long *)outtbptr[jj];
00781 sszz=outtbsz[2*jj]*outtbsz[2*jj+1];
00782 for (kk=0;kk<sszz;kk++)
00783 {
00784 if(outtblptr[kk]!=(long)outtbl[curouttbl+kk]) goto L30;
00785 }
00786 curouttbl+=sszz;
00787 break;
00788
00789 case 811 : outtbucptr=(unsigned char *)outtbptr[jj];
00790 sszz=outtbsz[2*jj]*outtbsz[2*jj+1];
00791 for (kk=0;kk<sszz;kk++)
00792 {
00793 if(outtbucptr[kk]!=(unsigned char)outtbuc[curouttbuc+kk]) goto L30;
00794 }
00795 curouttbuc+=sszz;
00796 break;
00797
00798 case 812 : outtbusptr=(unsigned short *)outtbptr[jj];
00799 sszz=outtbsz[2*jj]*outtbsz[2*jj+1];
00800 for (kk=0;kk<sszz;kk++)
00801 {
00802 if(outtbusptr[kk]!=(unsigned short)outtbus[curouttbus+kk]) goto L30;
00803 }
00804 curouttbus+=sszz;
00805 break;
00806
00807 case 814 : outtbulptr=(unsigned long *)outtbptr[jj];
00808 sszz=outtbsz[2*jj]*outtbsz[2*jj+1];
00809 for (kk=0;kk<sszz;kk++)
00810 {
00811 if(outtbulptr[kk]!=(unsigned long)outtbul[curouttbul+kk]) goto L30;
00812 }
00813 curouttbul+=sszz;
00814 break;
00815
00816 default :
00817 break;
00818 }
00819 }
00820 freeouttbptr;
00821 return;
00822
00823 L30:
00824
00825 curouttbd=0;
00826 curouttbc=0; curouttbs=0; curouttbl=0;
00827 curouttbuc=0; curouttbus=0; curouttbul=0;
00828 for (ii=0;ii<nlnk;ii++)
00829 {
00830 switch (outtbtyp[ii])
00831 {
00832 case 10 : outtbdptr=(double *)outtbptr[ii];
00833 sszz=outtbsz[2*ii]*outtbsz[2*ii+1];
00834 C2F(dcopy)(&sszz, outtbdptr, &c__1, &outtbd[curouttbd], &c__1);
00835 curouttbd+=sszz;
00836 break;
00837
00838 case 11 : outtbdptr=(double *)outtbptr[ii];
00839 sszz=2*outtbsz[2*ii]*outtbsz[2*ii+1];
00840 C2F(dcopy)(&sszz, outtbdptr, &c__1, &outtbd[curouttbd], &c__1);
00841 curouttbd+=sszz;
00842 break;
00843
00844 case 81 : outtbcptr=(char *)outtbptr[ii];
00845 sszz=outtbsz[2*ii]*outtbsz[2*ii+1];
00846 for (kk=0;kk<sszz;kk++) outtbc[curouttbc+kk]=(char)outtbcptr[kk];
00847 curouttbc+=sszz;
00848 break;
00849
00850 case 82 : outtbsptr=(short *)outtbptr[ii];
00851 sszz=outtbsz[2*ii]*outtbsz[2*ii+1];
00852 for (kk=0;kk<sszz;kk++) outtbs[curouttbs+kk]=(short)outtbsptr[kk];
00853 curouttbs+=sszz;
00854 break;
00855
00856 case 84 : outtblptr=(long *)outtbptr[ii];
00857 sszz=outtbsz[2*ii]*outtbsz[2*ii+1];
00858 for (kk=0;kk<sszz;kk++) outtbl[curouttbl+kk]=(long)outtblptr[kk];
00859 curouttbl+=sszz;
00860 break;
00861
00862 case 811 : outtbucptr=(unsigned char *)outtbptr[ii];
00863 sszz=outtbsz[2*ii]*outtbsz[2*ii+1];
00864 for (kk=0;kk<sszz;kk++) outtbuc[curouttbuc+kk]=(unsigned char)outtbucptr[kk];
00865 curouttbuc+=sszz;
00866 break;
00867
00868 case 812 : outtbusptr=(unsigned short *)outtbptr[ii];
00869 sszz=outtbsz[2*ii]*outtbsz[2*ii+1];
00870 for (kk=0;kk<sszz;kk++) outtbus[curouttbus+kk]=(unsigned short)outtbusptr[kk];
00871 curouttbus+=sszz;
00872 break;
00873
00874 case 814 : outtbulptr=(unsigned long *)outtbptr[ii];
00875 sszz=outtbsz[2*ii]*outtbsz[2*ii+1];
00876 for (kk=0;kk<sszz;kk++) outtbul[curouttbul+kk]=(unsigned long)outtbulptr[kk];
00877 curouttbul+=sszz;
00878 break;
00879
00880 default :
00881 break;
00882 }
00883 }
00884 }
00885 *ierr = 20;
00886 freeouttbptr;
00887 }
00888
00889 void idoit(told)
00890 double *told;
00891 {
00892 static integer flag__;
00893 static integer i,jj;
00894 static integer ierr1;
00895 static integer i2;
00896
00897
00898
00899
00900
00901
00902
00903
00904
00905
00906
00907
00908
00909
00910 for (jj = 1; jj <= niord; ++jj) {
00911 C2F(curblk).kfun = iord[jj];
00912 if (outptr[C2F(curblk).kfun + 1] - outptr[C2F(curblk).kfun] > 0) {
00913 nclock = iord[jj + niord];
00914 flag__ = 1;
00915 callf(told, xd, x, x,x,&flag__);
00916
00917 if (flag__ < 0) {
00918 *ierr = 5 - flag__;
00919 return;
00920 }
00921 }
00922 if (Blocks[C2F(curblk).kfun - 1].nevout > 0) {
00923 if (funtyp[C2F(curblk).kfun] < 0) {
00924
00925 if (funtyp[C2F(curblk).kfun] == -1) {
00926 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
00927 if (outtbdptr[0] <= 0.) {
00928 i=2;
00929 } else {
00930 i=1;
00931 }
00932 } else if (funtyp[C2F(curblk).kfun] == -2) {
00933 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
00934 i=max(min((integer) outtbdptr[0],
00935 Blocks[C2F(curblk).kfun - 1].nevout),1);
00936 }
00937 i2 =i+ clkptr[C2F(curblk).kfun] - 1;
00938 putevs(told, &i2, &ierr1);
00939 if (ierr1 != 0) {
00940
00941 *ierr = 3;
00942 return;
00943 }
00944 doit(told);
00945 if (*ierr != 0) {
00946 return;
00947 }
00948 }
00949 }
00950 }
00951 }
00952
00953 void cossim(told)
00954 double *told;
00955
00956 {
00957
00958 static integer otimer = 0;
00959
00960 integer i3;
00961
00962
00963 static integer flag__, jdum;
00964 static integer iopt;
00965
00966 static integer ierr1;
00967 static integer j, k;
00968 static double t;
00969 static integer itask;
00970 static integer jj, jt;
00971 static integer istate, ntimer;
00972
00973 static double rhotmp;
00974 static integer inxsci;
00975
00976 static integer kpo, kev;
00977
00978 double *rhot;
00979 integer *ihot,niwp,nrwp;
00980 integer *jroot,*zcros;
00981
00982
00983 nrwp = (*neq) * max(16,*neq + 9) + 22 + ng * 3;
00984
00985 if((rhot=MALLOC(sizeof(double)*(nrwp+1)))== NULL ){
00986 *ierr =10000;
00987 return;
00988 }
00989 niwp = *neq + 20 + ng;
00990
00991
00992
00993 if((ihot=MALLOC(sizeof(int)*(niwp+1)))== NULL ){
00994 *ierr =10000;
00995 FREE(rhot);
00996 return;
00997 }
00998 if((jroot=MALLOC(sizeof(int)*ng))== NULL ){
00999 *ierr =10000;
01000 FREE(rhot);
01001 FREE(ihot);
01002 return;
01003 }
01004
01005
01006 for ( jj = 0 ; jj < ng ; jj++ )
01007 {
01008 jroot[jj] = 0 ;
01009 }
01010
01011 if((zcros=MALLOC(sizeof(int)*ng))== NULL ){
01012 *ierr =10000;
01013 FREE(rhot);
01014 FREE(ihot);
01015 FREE(jroot);
01016 return;
01017 }
01018
01019
01020
01021 C2F(coshlt).halt = 0;
01022 *ierr = 0;
01023
01024 C2F(xscion)(&inxsci);
01025
01026 C2F(iset)(&niwp, &c__0, &ihot[1], &c__1);
01027 C2F(dset)(&nrwp, &c_b14, &rhot[1], &c__1);
01028 C2F(realtimeinit)(told, &C2F(rtfactor).scale);
01029
01030 phase=1;
01031 hot = 0;
01032 itask = 4;
01033
01034 jt = 2;
01035
01036 jj = 0;
01037 for (C2F(curblk).kfun = 1; C2F(curblk).kfun <= nblk; ++C2F(curblk).kfun) {
01038 if (Blocks[C2F(curblk).kfun-1].ng >= 1) {
01039 zcros[jj] = C2F(curblk).kfun;
01040 ++jj;
01041 }
01042 }
01043
01044 if (jj != ng) {
01045 zcros[jj] = -1;
01046 }
01047
01048 idoit(told);
01049 if (*ierr != 0) {
01050 freeall;
01051 return;
01052 }
01053
01054
01055
01056
01057 while(*told < *tf) {
01058
01059 if (inxsci == 1 && scilab_timer_check() == 1) {
01060 C2F(sxevents)();
01061
01062 }
01063 if (C2F(coshlt).halt != 0) {
01064 C2F(coshlt).halt = 0;
01065 freeall;
01066 return;
01067 }
01068 if (*pointi == 0) {
01069 t = *tf;
01070 } else {
01071 t = tevts[*pointi];
01072 }
01073 if (abs(t - *told) < ttol) {
01074 t = *told;
01075
01076 }
01077 if (*told > t) {
01078
01079 *ierr = 1;
01080 freeall;
01081 return;
01082 }
01083 if (*told != t) {
01084 if (xptr[nblk+1] == 1) {
01085
01086 if (*told + deltat + ttol > t) {
01087 *told = t;
01088 } else {
01089 *told += deltat;
01090 }
01091
01092 if (*told >= *tf) {
01093
01094 if (ncord > 0) {
01095 cdoit(told);
01096 freeall;
01097 return;
01098 }
01099 }
01100 } else {
01101
01102 rhotmp = *tf + ttol;
01103 kpo = *pointi;
01104 L20:
01105 if (critev[kpo] == 1) {
01106 rhotmp = tevts[kpo];
01107 goto L30;
01108 }
01109 kpo = evtspt[kpo];
01110 if (kpo != 0) {
01111 goto L20;
01112 }
01113 L30:
01114 if (rhotmp < rhot[1]) {
01115 hot = 0;
01116 }
01117 rhot[1] = rhotmp;
01118 t = min(*told + deltat,min(t,*tf + ttol));
01119
01120 if (ng>0 && hot == 0 && nmod>0) {
01121 zdoit(g,x,x,told);
01122 if (*ierr != 0){
01123 freeall;
01124 return;
01125 }
01126 }
01127
01128
01129 if (hot){
01130 istate=2;
01131 }else{
01132 istate = 1;
01133 }
01134 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
01135 {
01136 sciprint("****lsodar from: %f to %f hot= %d \r\n", *told,t,hot);
01137 }
01138
01139 if(hmax==0){
01140 iopt = 0;
01141 }else{
01142 iopt=1;
01143 C2F(iset)(&panj, &c__0, &ihot[5], &c__1);
01144 C2F(dset)(&panj, &c_b14, &rhot[5], &c__1);
01145 rhot[6]=hmax;
01146 }
01147
01148
01149
01150 if (ng>0 && hot==0){
01151 zdoit(g, x, x,told);
01152 if (*ierr != 0) {freeall;return;}
01153 for (jj = 0; jj < ng; ++jj) {
01154 if((g[jj]>=0.0)&&(jroot[jj]==-5)) {istate=3;jroot[jj]=1;}
01155 else if((g[jj]<0.0)&&(jroot[jj]==5)) {istate=3;jroot[jj]=-1;}
01156 }
01157 }
01158
01159
01160 if (istate!=3){
01161 phase=2;
01162 C2F(lsodar2)(C2F(simblk), neq, x, told, &t, &c__1, &rtol,
01163 &Atol, &itask, &istate, &iopt, &rhot[1], &
01164 nrwp, &ihot[1], &niwp, &jdum, &jt,
01165 C2F(grblk), &ng, jroot);
01166 }
01167 phase=1;
01168
01169 if (*ierr > 5) {
01170
01171 freeall;
01172 return;
01173 }
01174 if (istate <= 0) {
01175
01176 *ierr = 100 - istate;
01177 freeall;
01178 return;
01179 } else {
01180 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
01181 {
01182 sciprint("****lsodar reached: %f\r\n",*told);
01183 }
01184 hot = 1;
01185 }
01186
01187
01188 if (*told >= *tf) {
01189 if (ncord > 0) {
01190 cdoit(told);
01191 freeall;
01192 return;
01193 }
01194 }
01195 if (istate == 4) hot=0;
01196 if (istate == 3) {
01197
01198 hot = 0;
01199 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
01200 {
01201 sciprint("root found at t=: %f\r\n",*told);
01202 }
01203
01204 zdoit(g, xd, x,told);
01205 if (*ierr != 0) {
01206 freeall;
01207 return;
01208 }
01209 for (jj = 0; jj < ng; ++jj) {
01210 C2F(curblk).kfun = zcros[ jj];
01211 if (C2F(curblk).kfun == -1) {
01212 break;
01213 }
01214 kev = 0;
01215
01216 for (j = zcptr[C2F(curblk).kfun]-1 ;
01217 j <zcptr[C2F(curblk).kfun+1]-1 ; ++j) {
01218 if(jroot[j]!=0){
01219 kev=1;
01220 break;
01221 }
01222 }
01223
01224 if (kev != 0) {
01225 Blocks[C2F(curblk).kfun-1].jroot=&jroot[zcptr[C2F(curblk).kfun]-1];
01226 if (funtyp[C2F(curblk).kfun] > 0) {
01227
01228 if (Blocks[C2F(curblk).kfun-1].nevout > 0) {
01229 flag__ = 3;
01230
01231 nclock = -kev;
01232 callf(told, xd, x, x,g,&flag__);
01233 if (flag__ < 0) {
01234 *ierr = 5 - flag__;
01235 freeall;
01236 return;
01237 }
01238
01239 for (k = 0; k < Blocks[C2F(curblk).kfun-1].nevout; ++k) {
01240 if (Blocks[C2F(curblk).kfun-1].evout[k] >= 0.) {
01241 i3 = k + clkptr[C2F(curblk).kfun] ;
01242 addevs(Blocks[C2F(curblk).kfun-1].evout[k]+(*told), &i3, &ierr1);
01243 if (ierr1 != 0) {
01244
01245 *ierr = 3;
01246 freeall;
01247 return;
01248 }
01249 }
01250 }
01251 }
01252
01253 if (Blocks[C2F(curblk).kfun-1].nx > 0) {
01254
01255 flag__ = 2;
01256 nclock = -kev;
01257 callf(told, xd, x, x,g,&flag__);
01258
01259 if (flag__ < 0) {
01260 *ierr = 5 - flag__;
01261 freeall;
01262 return;
01263 }
01264 }
01265 }
01266 }
01267 }
01268 }
01269 }
01270
01271 if (ng>0){
01272 zdoit(g, x, x, told); if (*ierr != 0) {freeall;return; }
01273 for (jj = 0; jj < ng; ++jj)
01274 if(g[jj]>=0)jroot[jj]=5;else jroot[jj]=-5;
01275 }
01276
01277
01278 C2F(realtime)(told);
01279 } else {
01280
01281 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
01282 {
01283 sciprint("Event: %d activated at t=%f\r\n",*pointi,*told);
01284 for(kev=0;kev<nblk;kev++){
01285 if (Blocks[kev].nmode>0){
01286 sciprint("mode of block %d=%d, ",kev,Blocks[kev].mode[0]);
01287 }
01288 }
01289 sciprint("**mod**\r\n");
01290 }
01291
01292 ddoit(told);
01293 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
01294 {
01295 sciprint("End of activation\r\n");
01296 }
01297 if (*ierr != 0) {
01298 freeall;
01299 return;
01300 }
01301
01302 }
01303
01304 }
01305 freeall;
01306 }
01307
01308
01309
01310 void cossimdaskr(told)
01311 double *told;
01312 {
01313
01314 static integer otimer = 0;
01315
01316 integer i3,*ipardummy=NULL;
01317 double *rpardummy=NULL;
01318
01319
01320 static integer flag__;
01321 static integer info[20];
01322
01323 static integer ierr1;
01324 static integer j, k;
01325 static double t;
01326 static integer jj, jt;
01327 static integer istate, ntimer;
01328 static double rhotmp;
01329 static integer inxsci;
01330 static integer kpo, kev;
01331
01332 double *rhot;
01333 integer *ihot,niwp,nrwp;
01334 integer *jroot,*zcros;
01335 integer maxord;
01336 integer *scicos_xproperty;
01337
01338 integer *Mode_save;
01339 integer Mode_change;
01340
01341 int Jn, Jnx, Jno, Jni, Jactaille;
01342 double uround;
01343 Jactaille=0;
01344 if(Jacobian_Flag==1){
01345 Jn=*neq;
01346 Jnx=Blocks[nblk-1].nx;
01347 Jno=Blocks[nblk-1].nout;
01348 Jni=Blocks[nblk-1].nin;
01349 Jactaille= 2+3*Jn+(Jn+Jni)*(Jn+Jno)+Jnx*(Jni+2*Jn+Jno)+(Jn-Jnx)*(2*(Jn-Jnx)+Jno+Jni)+2*Jni*Jno;}
01350
01351 maxord = 5;
01352 nrwp = max(maxord + 4,7) * (*neq) + 60 + (*neq)*(*neq) + ng * 3 + Jactaille;
01353 niwp = (*neq) + 40 + (*neq) +ng ;
01354
01355
01356 if((rhot=MALLOC(sizeof(double)*(nrwp+1)))== NULL ){
01357 *ierr =10000;
01358 return;
01359 }
01360
01361 if((ihot=MALLOC(sizeof(int)*(niwp+1)))== NULL ){
01362 FREE(rhot);
01363 *ierr =10000;
01364 return;
01365 }
01366 if((jroot=MALLOC(sizeof(int)*ng))== NULL ){
01367 *ierr =10000;
01368 FREE(rhot);
01369 FREE(ihot);
01370 return;
01371 }
01372 if((scicos_xproperty=MALLOC(sizeof(int)*(*neq)))== NULL ){
01373 *ierr =10000;
01374 FREE(rhot);
01375 FREE(ihot);
01376 FREE(jroot);
01377 return;
01378 }
01379 C2F(iset)(neq, &c__1, scicos_xproperty, &c__1);
01380 if((zcros=MALLOC(sizeof(int)*ng))== NULL ){
01381 *ierr =10000;
01382 FREE(rhot);
01383 FREE(ihot);
01384 FREE(jroot);
01385 FREE(scicos_xproperty);
01386 return;
01387 }
01388
01389 if((Mode_save=MALLOC(sizeof(double)*nmod))== NULL ){
01390 *ierr =10000;
01391 FREE(rhot);
01392 FREE(ihot);
01393 FREE(jroot);
01394 FREE(scicos_xproperty);
01395 FREE(zcros);
01396 return;
01397 }
01398
01399 uround = 1.0;
01400 do{
01401 uround = uround*0.5;
01402 }while ( 1.0 + uround != 1.0);
01403 uround = uround*2.0;
01404 SQuround=sqrt(uround);
01405
01406
01407 C2F(coshlt).halt = 0;
01408 *ierr = 0;
01409
01410 phase=1;
01411 hot = 0;
01412
01413 jt = 2;
01414
01415
01416 C2F(xscion)(&inxsci);
01417
01418 C2F(iset)(&niwp, &c__0, &ihot[1], &c__1);
01419 C2F(dset)(&nrwp, &c_b14, &rhot[1], &c__1);
01420 C2F(realtimeinit)(told, &C2F(rtfactor).scale);
01421
01422 info[1] = 0;
01423 info[2] = 0;
01424 info[3] = 1;
01425
01426
01427
01428 info[4] = 0;
01429
01430 info[5] = 0;
01431
01432 if(hmax==0){
01433 info[6] = 0;
01434 }else{
01435 info[6] = 1;
01436 rhot[2]=hmax;
01437 }
01438
01439
01440 info[7] = 0;
01441
01442 info[8] = 0;
01443
01444 info[9] = 0;
01445
01446 info[11] = 0;
01447 info[12] = 0;
01448 info[13] = 0;
01449 info[14] = 0;
01450 info[15] = 0;
01451 info[16] = 0;
01452 info[17] = 0;
01453
01454 jj = 0;
01455 for (C2F(curblk).kfun = 1; C2F(curblk).kfun <= nblk; ++C2F(curblk).kfun) {
01456 if (Blocks[C2F(curblk).kfun-1].ng >= 1) {
01457 zcros[jj] = C2F(curblk).kfun;
01458 ++jj;
01459 }
01460 }
01461
01462 if (jj != ng) {
01463 zcros[jj] = -1;
01464 }
01465
01466 idoit(told);
01467 if (*ierr != 0) {
01468 freeallx;
01469 return;
01470 }
01471
01472 while (*told < *tf) {
01473 if (inxsci == 1 && scilab_timer_check() == 1) {
01474 C2F(sxevents)();
01475
01476 }
01477 if (C2F(coshlt).halt != 0) {
01478 C2F(coshlt).halt = 0;
01479 freeallx;
01480 return;
01481 }
01482 if (*pointi == 0) {
01483 t = *tf;
01484 } else {
01485 t = tevts[*pointi];
01486 }
01487 if (abs(t - *told) < ttol) {
01488 t = *told;
01489
01490 }
01491 if (*told > t) {
01492
01493 *ierr = 1;
01494 freeallx;
01495 return;
01496 }
01497 if (*told != t) {
01498 if (xptr[nblk+1] == 1) {
01499
01500 if (*told + deltat + ttol > t) {
01501 *told = t;
01502 } else {
01503 *told += deltat;
01504 }
01505
01506 if (*told >= *tf) {
01507
01508 cdoit(told);
01509 freeallx;
01510 return;
01511 }
01512 } else {
01513 if (hot == 0) {
01514 reinitdoit(told,scicos_xproperty);
01515 if(*ierr >0){
01516 freeallx;
01517 return;
01518 }
01519 }
01520 rhotmp = *tf + ttol;
01521 kpo = *pointi;
01522 L20:
01523 if (critev[kpo] == 1) {
01524 rhotmp = tevts[kpo];
01525 goto L30;
01526 }
01527 kpo = evtspt[kpo];
01528 if (kpo != 0) {
01529 goto L20;
01530 }
01531 L30:
01532 if (rhotmp < rhot[1]) {
01533 hot = 0;
01534 }
01535 rhot[1] = rhotmp;
01536 t = min(*told + deltat,min(t,*tf + ttol));
01537
01538 if (hot == 0){
01539
01540 if (ng>0&&nmod>0){
01541 phase=1;
01542 zdoit(g, xd, x,told);
01543 if (*ierr != 0) {
01544 freeallx;
01545 return;
01546 }
01547 }
01548
01549
01550 for(j=0;j<=nmod;j++){
01551
01552
01553
01554
01555 info[0]=0;
01556 info[10]=1;
01557 info[13]=1;
01558 reinitdoit(told,scicos_xproperty);
01559 if(*ierr >0){
01560 freeallx;
01561 return;
01562 }
01563 for (jj = 1; jj <= *neq; ++jj) {
01564 ihot[jj + 40] = scicos_xproperty[jj-1];
01565 }
01566 phase=2;
01567
01568 info[4] =Jacobian_Flag;
01569
01570 C2F(ddaskr)(C2F(simblkdaskr), neq, told, x, xd, &t, info, &rtol,
01571 &Atol, &istate, &rhot[1],&nrwp, &ihot[1], &niwp,
01572 rpardummy, ipardummy, C2F(Jacobian), rpardummy,
01573 C2F(grblkdaskr), &ng, jroot);
01574
01575 if (*ierr > 5) {
01576 freeallx;
01577 return;
01578 }
01579 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
01580 {
01581 if (istate==4) {
01582 sciprint("**** daskr succesfully initialized *****/r/n" );
01583 }
01584 else{
01585 sciprint("**** daskr failed to initialize ->try again *****/r/n" );
01586 }
01587 }
01588
01589
01590 for (jj = 0; jj < nmod; ++jj) {
01591 Mode_save[jj] = mod[jj];
01592 }
01593 if (ng>0&&nmod>0){
01594 phase=1;
01595 zdoit(g, xd, x,told);
01596 if (*ierr != 0) {
01597 freeallx;
01598 return;
01599 }
01600 }
01601
01602 Mode_change=0;
01603 for (jj = 0; jj < nmod; ++jj) {
01604 if(Mode_save[jj] != mod[jj])
01605 {Mode_change=1;
01606 break;
01607 }
01608 }
01609 if ( Mode_change==0) break;
01610 }
01611 if(Mode_change==1){
01612 *ierr = 100 - (-17);
01613 freeallx;
01614 return;
01615 }
01616 info[0]=0;
01617 info[10]=1;
01618 info[13]=0;
01619 }
01620
01621 info[0]=hot;
01622
01623 phase=2;
01624 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
01625 {
01626 sciprint("****daskr from: %f to %f hot= %d \r\n", *told,t,hot);
01627 }
01628
01629
01630
01631 if (ng>0 && hot==0){
01632 zdoit(g, xd, x,told);
01633 if (*ierr != 0) {freeallx;return; }
01634 istate=0;
01635 for (jj = 0; jj < ng; ++jj) {
01636 if((g[jj]>=0.0)&&( jroot[jj]==-5)) {istate=5;jroot[jj]=1;}
01637 else if((g[jj]<0.0)&&( jroot[jj]==5)) {istate=5;jroot[jj]=-1;}
01638 else jroot[jj]=0;
01639 }
01640 }
01641
01642 if (istate!=5){
01643 C2F(ddaskr)(C2F(simblkdaskr), neq, told, x, xd, &t,
01644 info, &rtol, &Atol, &istate, &rhot[1], &
01645 nrwp, &ihot[1], &niwp, rpardummy, ipardummy
01646 ,C2F(Jacobian), rpardummy, C2F(grblkdaskr), &ng, jroot);
01647 }
01648
01649 if (istate == -1)
01650 sciprint("**** Stiffness at: %26.18f %d\r\n",*told,istate);
01651
01652 phase=1;
01653 if (*ierr > 5) {
01654 freeallx;
01655 return;
01656 }
01657
01658 if (istate <= -2) {
01659 *ierr = 100 - istate;
01660 freeallx;
01661 return;
01662 } else {
01663 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
01664 {
01665 sciprint("****daskr reached: %f\r\n",*told);
01666 }
01667 hot = 1;
01668 }
01669
01670
01671 if (*told >= *tf) {
01672 cdoit(told);
01673 freeallx;
01674 return;
01675 }
01676 if (istate == 6) hot=0;
01677 if (istate == 5) {
01678
01679 hot = 0;
01680 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
01681 {
01682 sciprint("root found at t=: %f\r\n",*told);
01683 }
01684
01685 zdoit(g, xd, x,told);
01686 if (*ierr != 0) {
01687 freeallx;
01688 return;
01689 }
01690 for (jj = 0; jj < ng; ++jj) {
01691 C2F(curblk).kfun = zcros[jj];
01692 if (C2F(curblk).kfun == -1) {
01693 break;
01694 }
01695 kev = 0;
01696 for (j = zcptr[C2F(curblk).kfun]-1 ;
01697 j <zcptr[C2F(curblk).kfun+1]-1 ; ++j) {
01698 if(jroot[j]!=0){
01699 kev=1;
01700 break;
01701 }
01702 }
01703 if (kev != 0) {
01704 Blocks[C2F(curblk).kfun-1].jroot=&jroot[zcptr[C2F(curblk).kfun]-1];
01705 if (funtyp[C2F(curblk).kfun] > 0) {
01706 if (Blocks[C2F(curblk).kfun-1].nevout > 0) {
01707 flag__ = 3;
01708
01709 nclock = -kev;
01710 callf(told, xd, x, x,g,&flag__);
01711 if (flag__ < 0) {
01712 *ierr = 5 - flag__;
01713 freeallx;
01714 return;
01715 }
01716
01717 for (k = 0; k < Blocks[C2F(curblk).kfun-1].nevout; ++k) {
01718 if (Blocks[C2F(curblk).kfun-1].evout[k] >= 0) {
01719 i3 = k + clkptr[C2F(curblk).kfun] ;
01720 addevs(Blocks[C2F(curblk).kfun-1].evout[k]+(*told), &i3, &ierr1);
01721 if (ierr1 != 0) {
01722
01723 *ierr = 3;
01724 freeallx;
01725 return;
01726 }
01727 }
01728 }
01729 }
01730
01731 if (Blocks[C2F(curblk).kfun-1].nx > 0) {
01732
01733 flag__ = 2;
01734 nclock = -kev;
01735 pointer_xproperty=
01736 &scicos_xproperty[-1+xptr[C2F(curblk).kfun]];
01737 n_pointer_xproperty=Blocks[C2F(curblk).kfun-1].nx;
01738 callf(told, xd, x, x,g,&flag__);
01739 if (flag__ < 0) {
01740 *ierr = 5 - flag__;
01741 freeallx;
01742 return;
01743 }
01744 }
01745 }
01746 }
01747 }
01748 }
01749 if (inxsci == 1 && scilab_timer_check() == 1) {
01750 C2F(sxevents)();
01751 otimer = ntimer;
01752
01753 }
01754 if (C2F(coshlt).halt != 0) {
01755 C2F(coshlt).halt = 0;
01756 freeallx;
01757 return;
01758 }
01759
01760
01761
01762
01763
01764
01765
01766
01767
01768
01769
01770
01771
01772
01773 if (ng>0){
01774 zdoit(g, xd, x, told); if (*ierr != 0) {freeallx;return; }
01775 for (jj = 0; jj < ng; ++jj)
01776 if(g[jj]>=0)jroot[jj]=5;else jroot[jj]=-5;
01777 }
01778
01779 }
01780 C2F(realtime)(told);
01781 } else {
01782
01783 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
01784 {
01785 sciprint("Event: %d activated at t=%f\r\n",*pointi,*told);
01786 }
01787
01788 ddoit(told);
01789 if ((C2F(cosdebug).cosd >= 1) && (C2F(cosdebug).cosd != 3))
01790 {
01791 sciprint("End of activation");
01792 }
01793 if (*ierr != 0) {
01794 freeallx;
01795 return;
01796 }
01797 }
01798
01799 }
01800 freeallx;
01801 }
01802
01803
01804
01805 void cosend(told)
01806 double *told;
01807 {
01808
01809 static integer flag__;
01810
01811 static integer kfune;
01812
01813
01814 *ierr = 0;
01815
01816 nclock=0;
01817 for (C2F(curblk).kfun = 1; C2F(curblk).kfun <= nblk; ++C2F(curblk).kfun) {
01818 flag__ = 5;
01819 if (funtyp[C2F(curblk).kfun] >= 0) {
01820 callf(told, xd, x, x,x,&flag__);
01821 if (flag__ < 0 && *ierr == 0) {
01822 *ierr = 5 - flag__;
01823 kfune = C2F(curblk).kfun;
01824 }
01825 }
01826 }
01827 if (*ierr != 0) {
01828 C2F(curblk).kfun = kfune;
01829 return;
01830 }
01831 }
01832
01833 void doit(told)
01834 double *told;
01835 {
01836
01837 integer i,i2;
01838
01839
01840 static integer flag__, nord;
01841
01842 static integer ierr1;
01843 integer ii, kever;
01844
01845
01846 kever = *pointi;
01847 *pointi = evtspt[kever];
01848 evtspt[kever] = -1;
01849
01850 nord = ordptr[kever + 1] - ordptr[kever];
01851 if (nord == 0) {
01852 return;
01853 }
01854
01855 for (ii = ordptr[kever]; ii <=ordptr[kever + 1] - 1 ; ++ii) {
01856 C2F(curblk).kfun = ordclk[ii];
01857 if (outptr[C2F(curblk).kfun + 1] - outptr[C2F(curblk).kfun] > 0) {
01858 nclock = abs(ordclk[ii + nordclk]);
01859 flag__ = 1;
01860 callf(told, xd, x, x,x,&flag__);
01861
01862 if (flag__ < 0) {
01863 *ierr = 5 - flag__;
01864 return;
01865 }
01866 }
01867
01868
01869 if (Blocks[C2F(curblk).kfun - 1].nevout > 0) {
01870 if (funtyp[C2F(curblk).kfun] < 0) {
01871 if (funtyp[C2F(curblk).kfun] == -1) {
01872 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
01873 if (outtbdptr[0] <= 0.) {
01874 i= 2;
01875 } else {
01876 i= 1;
01877 }
01878 } else if (funtyp[C2F(curblk).kfun] == -2) {
01879 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
01880 i=max(min((integer) outtbdptr[0],
01881 Blocks[C2F(curblk).kfun - 1].nevout),1);
01882 }
01883 i2 = i + clkptr[C2F(curblk).kfun] - 1;
01884 putevs(told, &i2, &ierr1);
01885 if (ierr1 != 0) {
01886
01887 *ierr = 3;
01888 return;
01889 }
01890 doit(told);
01891 if (*ierr != 0) {
01892 return;
01893 }
01894 }
01895 }
01896 }
01897 }
01898
01899 void cdoit(told)
01900 double *told;
01901 {
01902
01903 integer i2;
01904
01905
01906 static integer flag__;
01907 static integer ierr1;
01908 static integer i,jj;
01909
01910
01911 for (jj = 1; jj <= ncord; ++jj) {
01912 C2F(curblk).kfun = cord[jj];
01913 nclock = cord[jj + ncord];
01914 if (outptr[C2F(curblk).kfun + 1] - outptr[C2F(curblk).kfun] > 0) {
01915 flag__ = 1;
01916 callf(told, xd, x, x,x,&flag__);
01917
01918 if (flag__ < 0) {
01919 *ierr = 5 - flag__;
01920 return;
01921 }
01922 }
01923
01924 if (Blocks[C2F(curblk).kfun - 1].nevout > 0) {
01925 if (funtyp[C2F(curblk).kfun] < 0) {
01926
01927 if (funtyp[C2F(curblk).kfun] == -1) {
01928 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
01929 if (outtbdptr[0] <= 0.) {
01930 i = 2;
01931 } else {
01932 i = 1;
01933 }
01934 } else if (funtyp[C2F(curblk).kfun] == -2) {
01935 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
01936 i= max(min((integer) outtbdptr[0],
01937 Blocks[C2F(curblk).kfun - 1].nevout),1);
01938 }
01939 i2 = i + clkptr[C2F(curblk).kfun] - 1;
01940 putevs(told, &i2, &ierr1);
01941 if (ierr1 != 0) {
01942
01943 *ierr = 3;
01944 return;
01945 }
01946 doit(told);
01947 if (*ierr != 0) {
01948 return;
01949 }
01950 }
01951 }
01952 }
01953 }
01954
01955 void ddoit(told)
01956 double *told;
01957
01958 {
01959
01960 integer i2,j;
01961
01962
01963 static integer flag__, kiwa;
01964
01965 static integer i,i3,ierr1;
01966 static integer ii, keve;
01967 double d__1;
01968
01969
01970 kiwa = 0;
01971 edoit(told,&kiwa);
01972 if (*ierr != 0) {
01973 return;
01974 }
01975
01976
01977 if (kiwa == 0) {
01978 return;
01979 }
01980 for (i = 0; i < kiwa; ++i) {
01981 keve = iwa[i];
01982 if(critev[keve]!= 0){
01983 hot = 0;
01984 }
01985 i2 = ordptr[keve + 1] - 1;
01986 for (ii = ordptr[keve]; ii <= i2; ++ii) {
01987 C2F(curblk).kfun = ordclk[ii];
01988 nclock=ordclk[ii + nordclk];
01989
01990 if (Blocks[C2F(curblk).kfun - 1].nevout > 0) {
01991 if (funtyp[C2F(curblk).kfun] >= 0) {
01992 d__1 = - 1.;
01993 C2F(dset)(&Blocks[C2F(curblk).kfun - 1].nevout,
01994 &d__1, Blocks[C2F(curblk).kfun-1].evout, &c__1);
01995
01996 flag__ = 3;
01997
01998 if(nclock>0){
01999 callf(told, xd, x, x ,x,&flag__);
02000 if (flag__ < 0) {
02001 *ierr = 5 - flag__;
02002 return;
02003 }
02004 }
02005
02006 for (j = 0; j < Blocks[C2F(curblk).kfun - 1].nevout; ++j) {
02007 if (Blocks[C2F(curblk).kfun-1].evout[j] >= 0.) {
02008 i3 = j + clkptr[C2F(curblk).kfun] ;
02009 addevs(Blocks[C2F(curblk).kfun-1].evout[j]+(*told), &i3, &ierr1);
02010 if (ierr1 != 0) {
02011
02012 *ierr = 3;
02013 return;
02014 }
02015 }
02016 }
02017 }
02018 }
02019
02020 if(nclock> 0) {
02021 if (Blocks[C2F(curblk).kfun-1].nx+Blocks[C2F(curblk).kfun-1].nz > 0||
02022 *Blocks[C2F(curblk).kfun-1].work !=NULL) {
02023
02024
02025 flag__ = 2;
02026 callf(told, xd, x, x,x,&flag__);
02027 if (flag__ < 0) {
02028 *ierr = 5 - flag__;
02029 return;
02030 }
02031 }
02032 }else{
02033 if (*Blocks[C2F(curblk).kfun-1].work !=NULL) {
02034 flag__ = 2;
02035 nclock=0;
02036 callf(told, xd, x, x,x,&flag__);
02037 if (flag__ < 0) {
02038 *ierr = 5 - flag__;
02039 return;
02040 }
02041 }
02042 }
02043 }
02044 }
02045 }
02046
02047 void edoit(told,kiwa)
02048 double *told;
02049 integer *kiwa;
02050 {
02051
02052 integer i2;
02053
02054
02055 static integer flag__;
02056 static integer nord;
02057
02058 static integer ierr1, i;
02059 integer kever, ii;
02060
02061
02062 kever = *pointi;
02063 *pointi = evtspt[kever];
02064 evtspt[kever] = -1;
02065
02066 nord = ordptr[kever + 1] - ordptr[kever];
02067 if (nord == 0) {
02068 return;
02069 }
02070 iwa[*kiwa] = kever;
02071 ++(*kiwa);
02072 for (ii = ordptr[kever]; ii <= ordptr[kever + 1] - 1; ++ii) {
02073 C2F(curblk).kfun = ordclk[ii];
02074
02075 if (outptr[C2F(curblk).kfun + 1] - outptr[C2F(curblk).kfun] > 0) {
02076 nclock = abs(ordclk[ii + nordclk]);
02077 flag__ = 1;
02078 callf(told, xd, x, x,x,&flag__);
02079
02080 if (flag__ < 0) {
02081 *ierr = 5 - flag__;
02082 return;
02083 }
02084 }
02085
02086
02087 if (Blocks[C2F(curblk).kfun - 1].nevout > 0) {
02088 if (funtyp[C2F(curblk).kfun] < 0) {
02089 if (funtyp[C2F(curblk).kfun] == -1) {
02090 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
02091 if (outtbdptr[0] <= 0.) {
02092 i = 2;
02093 } else {
02094 i = 1;
02095 }
02096 } else if (funtyp[C2F(curblk).kfun] == -2) {
02097 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
02098 i= max(min((integer) outtbdptr[0],
02099 Blocks[C2F(curblk).kfun - 1].nevout),1);
02100 }
02101 i2 = i + clkptr[C2F(curblk).kfun] - 1;
02102 putevs(told, &i2, &ierr1);
02103 if (ierr1 != 0) {
02104
02105 *ierr = 3;
02106 return;
02107 }
02108 edoit(told,kiwa);
02109 }
02110 }
02111 }
02112 }
02113
02114 void odoit(residual, xt, xtd, told)
02115 double *residual, *xt, *xtd;
02116 double *told;
02117
02118 {
02119
02120 integer i2;
02121
02122
02123 static integer flag__, keve, kiwa;
02124 static integer ierr1, i;
02125 static integer ii, jj;
02126
02127
02128 kiwa = 0;
02129
02130 for (jj = 1; jj <= noord; ++jj) {
02131 C2F(curblk).kfun = oord[jj];
02132 nclock = oord[jj + noord];
02133 if (outptr[C2F(curblk).kfun + 1] - outptr[C2F(curblk).kfun] > 0) {
02134 flag__ = 1;
02135 callf(told, xtd, xt, residual,x,&flag__);
02136
02137 if (flag__ < 0) {
02138 *ierr = 5 - flag__;
02139 return;
02140 }
02141 }
02142
02143 if (Blocks[C2F(curblk).kfun - 1].nevout > 0) {
02144 if (funtyp[C2F(curblk).kfun] < 0) {
02145 if(Blocks[C2F(curblk).kfun - 1].nmode > 0){
02146 i2 = Blocks[C2F(curblk).kfun - 1].mode[0] +
02147 clkptr[C2F(curblk).kfun] - 1;
02148 } else{
02149 if (funtyp[C2F(curblk).kfun] == -1) {
02150 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
02151 if (outtbdptr[0] <= 0.) {
02152 i=2;
02153 } else {
02154 i=1;
02155 }
02156 } else if (funtyp[C2F(curblk).kfun] == -2) {
02157 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
02158 i=max(min((integer) outtbdptr[0],
02159 Blocks[C2F(curblk).kfun - 1].nevout),1);
02160 }
02161 i2 =i+ clkptr[C2F(curblk).kfun] - 1;
02162 }
02163 putevs(told, &i2, &ierr1);
02164 if (ierr1 != 0) {
02165
02166 *ierr = 3;
02167 return;
02168 }
02169 ozdoit(xtd, xt,told, &kiwa);
02170 }
02171 }
02172 }
02173
02174
02175 for (ii = 1; ii <= noord; ++ii) {
02176 C2F(curblk).kfun = oord[ii];
02177 if (Blocks[C2F(curblk).kfun-1].nx > 0||
02178 *Blocks[C2F(curblk).kfun-1].work !=NULL) {
02179
02180 flag__ = 0;
02181 nclock = oord[ii + noord];
02182 callf(told, xtd, xt, residual,xt,&flag__);
02183
02184 if (flag__ < 0) {
02185 *ierr = 5 - flag__;
02186 return;
02187 }
02188 }
02189 }
02190
02191 for (i = 0; i < kiwa; ++i) {
02192 keve = iwa[i];
02193 for (ii = ordptr[keve]; ii <= ordptr[keve + 1] - 1; ++ii) {
02194 C2F(curblk).kfun = ordclk[ii ];
02195 if (Blocks[C2F(curblk).kfun-1].nx > 0||
02196 *Blocks[C2F(curblk).kfun-1].work !=NULL) {
02197
02198 flag__ = 0;
02199 nclock = abs(ordclk[ii + nordclk]);
02200 callf(told, xtd, xt, residual,xt,&flag__);
02201
02202 if (flag__ < 0) {
02203 *ierr = 5 - flag__;
02204 return;
02205 }
02206 }
02207 }
02208 }
02209 }
02210
02211
02212 void reinitdoit(told,scicos_xproperty)
02213 double *told;
02214 integer *scicos_xproperty;
02215 {
02216
02217 integer i2;
02218
02219
02220 static integer flag__, keve, kiwa;
02221
02222 static integer ierr1, i;
02223 static integer ii, jj;
02224
02225
02226 kiwa = 0;
02227 for (jj = 1; jj <= noord; ++jj) {
02228 C2F(curblk).kfun = oord[jj];
02229 nclock = oord[jj + noord];
02230 if (outptr[C2F(curblk).kfun + 1] - outptr[C2F(curblk).kfun] > 0) {
02231 flag__ = 1;
02232 callf(told, xd, x, x,x,&flag__);
02233
02234 if (flag__ < 0) {
02235 *ierr = 5 - flag__;
02236 return;
02237 }
02238 }
02239
02240 if (Blocks[C2F(curblk).kfun - 1].nevout > 0) {
02241 if (funtyp[C2F(curblk).kfun] == -1) {
02242 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
02243 if (outtbdptr[0] <= 0.) {
02244 i=2;
02245 } else {
02246 i=1;
02247 }
02248 } else if (funtyp[C2F(curblk).kfun] == -2) {
02249 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
02250 i= max(min((integer) outtbdptr[0],
02251 Blocks[C2F(curblk).kfun - 1].nevout),1);
02252 }
02253 if(Blocks[C2F(curblk).kfun - 1].nmode>0){
02254 Blocks[C2F(curblk).kfun - 1].mode[0]=i;
02255 }
02256 i2 =i+ clkptr[C2F(curblk).kfun] - 1;
02257 putevs(told, &i2, &ierr1);
02258 if (ierr1 != 0) {
02259
02260 *ierr = 3;
02261 return;
02262 }
02263 doit(told);
02264 if (*ierr != 0) {
02265 return;
02266 }
02267 }
02268 }
02269
02270
02271 for (ii = 1; ii <= noord; ++ii) {
02272 C2F(curblk).kfun = oord[ii];
02273 if (Blocks[C2F(curblk).kfun-1].nx > 0) {
02274 flag__ = 7;
02275 nclock = oord[ii + noord];
02276 pointer_xproperty=&scicos_xproperty[-1+xptr[C2F(curblk).kfun]];
02277 n_pointer_xproperty=Blocks[C2F(curblk).kfun-1].nx;
02278 callf(told, xd, x, xd,x,&flag__);
02279
02280 if (flag__ < 0) {
02281 *ierr = 5 - flag__;
02282 return;
02283 }
02284 }
02285 }
02286
02287 for (i = 0; i < kiwa; ++i) {
02288 keve = iwa[i];
02289 for (ii = ordptr[keve]; ii <= ordptr[keve + 1] - 1; ++ii) {
02290 C2F(curblk).kfun = ordclk[ii ];
02291 if (Blocks[C2F(curblk).kfun-1].nx > 0) {
02292 flag__ = 7;
02293 nclock = abs(ordclk[ii + nordclk]);
02294 n_pointer_xproperty=Blocks[C2F(curblk).kfun-1].nx;
02295 pointer_xproperty=&scicos_xproperty[-1+xptr[C2F(curblk).kfun]];
02296 callf(told, xd, x, xd,x,&flag__);
02297
02298 if (flag__ < 0) {
02299 *ierr = 5 - flag__;
02300 return;
02301 }
02302 }
02303 }
02304 }
02305 }
02306
02307 void ozdoit(xtd, xt, told, kiwa)
02308 double *xtd, *xt;
02309 double *told;
02310 integer *kiwa;
02311 {
02312
02313 integer i2;
02314
02315
02316 static integer flag__, nord;
02317
02318 static integer ierr1, i;
02319 integer ii, kever;
02320
02321
02322 kever = *pointi;
02323 *pointi = evtspt[kever];
02324 evtspt[kever] = -1;
02325
02326 nord = ordptr[kever + 1] - ordptr[kever];
02327 if (nord == 0) {
02328 return;
02329 }
02330 iwa[*kiwa] = kever;
02331 ++(*kiwa);
02332
02333 for (ii = ordptr[kever]; ii <= ordptr[kever + 1] - 1; ++ii) {
02334 C2F(curblk).kfun = ordclk[ii];
02335 if (outptr[C2F(curblk).kfun + 1] - outptr[C2F(curblk).kfun] > 0) {
02336 nclock = abs(ordclk[ii + nordclk]);
02337 flag__ = 1;
02338 callf(told, xtd, xt, xt,x,&flag__);
02339
02340 if (flag__ < 0) {
02341 *ierr = 5 - flag__;
02342 return;
02343 }
02344 }
02345
02346
02347 if (Blocks[C2F(curblk).kfun - 1].nevout > 0) {
02348
02349 if (funtyp[C2F(curblk).kfun] < 0) {
02350
02351 if (funtyp[C2F(curblk).kfun] == -1) {
02352 if (phase==1 || Blocks[C2F(curblk).kfun - 1].nmode==0){
02353 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
02354 if (outtbdptr[0] <= 0.) {
02355 i=2;
02356 } else {
02357 i=1;
02358 }
02359 }else{
02360 i=Blocks[C2F(curblk).kfun - 1].mode[0];
02361 }
02362 } else if (funtyp[C2F(curblk).kfun] == -2) {
02363 if (phase==1 || Blocks[C2F(curblk).kfun - 1].nmode==0){
02364 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
02365 i= max(min((integer)
02366 outtbdptr[0],
02367 Blocks[C2F(curblk).kfun - 1].nevout),1);
02368 }else{
02369 i=Blocks[C2F(curblk).kfun - 1].mode[0];
02370
02371 }
02372 }
02373 i2 =i+clkptr[C2F(curblk).kfun] - 1;
02374 putevs(told, &i2, &ierr1);
02375 if (ierr1 != 0) {
02376
02377 *ierr = 3;
02378 return;
02379 }
02380 ozdoit(xtd, xt,told, kiwa);
02381 }
02382 }
02383 }
02384 }
02385
02386 void zdoit(g, xtd, xt,told)
02387 double *g;
02388 double *xtd, *xt;
02389 double *told;
02390 {
02391
02392 integer i2;
02393
02394
02395 static integer flag__, keve, kiwa;
02396 static integer ierr1, i,j;
02397 static integer ii, jj;
02398
02399
02400 C2F(dset)(&ng, &c_b14,g , &c__1);
02401
02402 kiwa = 0;
02403 for (jj = 1; jj <= nzord; ++jj) {
02404 C2F(curblk).kfun = zord[jj];
02405 nclock = zord[jj + nzord];
02406 if (outptr[C2F(curblk).kfun + 1] - outptr[C2F(curblk).kfun] > 0) {
02407 flag__ = 1;
02408 callf(told, xtd, xt, xt,xt,&flag__);
02409
02410 if (flag__ < 0) {
02411 *ierr = 5 - flag__;
02412 return;
02413 }
02414 }
02415
02416 if (Blocks[C2F(curblk).kfun - 1].nevout > 0) {
02417 if (funtyp[C2F(curblk).kfun] < 0) {
02418
02419
02420 if (funtyp[C2F(curblk).kfun] == -1) {
02421 if (phase==1|| Blocks[C2F(curblk).kfun - 1].nmode==0){
02422 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
02423 if (outtbdptr[0] <= 0.) {
02424 i=2;
02425 } else {
02426 i=1;
02427 }
02428 }else{
02429 i=Blocks[C2F(curblk).kfun - 1].mode[0];
02430 }
02431 } else if (funtyp[C2F(curblk).kfun] == -2) {
02432 if (phase==1|| Blocks[C2F(curblk).kfun - 1].nmode==0){
02433 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
02434 i=max(min((integer)
02435 outtbdptr[0],
02436 Blocks[C2F(curblk).kfun - 1].nevout),1);
02437 }else{
02438 i=Blocks[C2F(curblk).kfun - 1].mode[0];
02439 }
02440 }
02441 i2 =i+clkptr[C2F(curblk).kfun] - 1;
02442 putevs(told, &i2, &ierr1);
02443 if (ierr1 != 0) {
02444
02445 *ierr = 3;
02446 return;
02447 }
02448 ozdoit(xtd, xt,told, &kiwa);
02449 }
02450 }
02451 }
02452
02453
02454 for (ii = 1; ii <= nzord; ++ii) {
02455 C2F(curblk).kfun = zord[ii];
02456 if (Blocks[C2F(curblk).kfun-1].ng > 0) {
02457 if (funtyp[C2F(curblk).kfun] > 0) {
02458 flag__ = 9;
02459 nclock = zord[ii +nzord];
02460 callf(told, xtd, xt, xtd,g,&flag__);
02461 if (flag__ < 0) {
02462 *ierr = 5 - flag__;
02463 return;
02464 }
02465 }else{
02466 if (funtyp[C2F(curblk).kfun] == -1) {
02467 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
02468 g[zcptr[C2F(curblk).kfun]-1]=outtbdptr[0];
02469 if(phase==1&&Blocks[C2F(curblk).kfun - 1].nmode>0){
02470 if (g[zcptr[C2F(curblk).kfun]-1] <= 0.) {
02471 Blocks[C2F(curblk).kfun - 1].mode[0] = 2;
02472 }
02473 else {
02474 Blocks[C2F(curblk).kfun - 1].mode[0] = 1;
02475 }
02476 }
02477 } else if (funtyp[C2F(curblk).kfun] == -2) {
02478 for (jj=0;jj<Blocks[C2F(curblk).kfun - 1].nevout-1;++jj) {
02479 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
02480 g[zcptr[C2F(curblk).kfun]-1+jj]=outtbdptr[0]-(double)(jj+2);
02481 }
02482 if(phase==1&&Blocks[C2F(curblk).kfun - 1].nmode>0){
02483 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
02484 j=max(min((integer) outtbdptr[0],Blocks[C2F(curblk).kfun - 1].nevout),1);
02485 Blocks[C2F(curblk).kfun - 1].mode[0]= j;
02486 }
02487 }
02488 }
02489 }
02490 }
02491 for (i = 0; i < kiwa; ++i) {
02492 keve = iwa[i];
02493 for (ii = ordptr[keve]; ii <= ordptr[keve + 1] - 1; ++ii) {
02494 C2F(curblk).kfun = ordclk[ii ];
02495 if (Blocks[C2F(curblk).kfun-1].ng > 0) {
02496 if (funtyp[C2F(curblk).kfun] > 0) {
02497 flag__ = 9;
02498 nclock = abs(ordclk[ii + nordclk]);
02499 callf(told, xtd, xt, xtd,g,&flag__);
02500
02501 if (flag__ < 0) {
02502 *ierr = 5 - flag__;
02503 return;
02504 }
02505 }else{
02506 if (funtyp[C2F(curblk).kfun] == -1) {
02507 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
02508 g[zcptr[C2F(curblk).kfun]-1]=outtbdptr[0];
02509 if(phase==1&&Blocks[C2F(curblk).kfun - 1].nmode>0){
02510 if (g[zcptr[C2F(curblk).kfun]-1] <= 0.) {
02511 Blocks[C2F(curblk).kfun - 1].mode[0] = 2;
02512 } else {
02513 Blocks[C2F(curblk).kfun - 1].mode[0] = 1;
02514 }
02515 }
02516 } else if (funtyp[C2F(curblk).kfun] == -2) {
02517 for (jj=0;jj<Blocks[C2F(curblk).kfun - 1].nevout-1;++jj) {
02518 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
02519 g[zcptr[C2F(curblk).kfun]-1+jj]=outtbdptr[0]-(double)(jj+2);
02520 }
02521 if(phase==1&&Blocks[C2F(curblk).kfun - 1].nmode>0){
02522 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
02523 j=max(min((integer) outtbdptr[0],Blocks[C2F(curblk).kfun - 1].nevout),1);
02524 Blocks[C2F(curblk).kfun - 1].mode[0]= j;
02525 }
02526 }
02527 }
02528 }
02529 }
02530 }
02531 }
02532
02533
02534
02535
02536
02537 void
02538 callf(t,xtd,xt,residual,g,flag)
02539 integer *flag;
02540 double *t,*xtd,*xt,*residual,*g;
02541
02542 {
02543 voidf loc ;
02544 double* args[SZ_SIZE];
02545 integer sz[SZ_SIZE];
02546 double intabl[TB_SIZE],outabl[TB_SIZE];
02547 int ii,kf,in,out,ki,ko,no,k,j;
02548 int ni = 0 ;
02549 int lprt,szi,flagi;
02550 int solver=C2F(cmsolver).solver;
02551 int cosd=C2F(cosdebug).cosd;
02552 ScicosF0 loc0;
02553 ScicosF loc1;
02554
02555 ScicosF2 loc2;
02556 ScicosF2z loc2z;
02557 ScicosFi loci1;
02558 ScicosFi2 loci2;
02559 ScicosFi2z loci2z;
02560 ScicosF4 loc4;
02561
02562 kf=C2F(curblk).kfun;
02563
02564 if (kf==(debug_block+1)) return;
02565
02566 block_error=flag;
02567
02568 flagi=*flag;
02569 if(flagi==7 && Blocks[kf-1].type<10000) *flag=0;
02570
02571 if ( cosd > 1)
02572 {
02573 if (cosd != 3)
02574 {
02575 sciprint("block %d is called ",kf);
02576 sciprint("with flag %d ",*flag);
02577 sciprint("at time %f \r\n",*t);
02578 }
02579 if(debug_block>-1)
02580 {
02581 if (cosd != 3) sciprint("Entering the block \r\n");
02582 call_debug_scicos(t,xtd,xt,residual,g,flag,kf,flagi,debug_block);
02583 if (*flag<0) return;
02584 }
02585 }
02586
02587
02588 C2F(scsptr).ptr=Blocks[kf-1].scsptr;
02589
02590
02591 loc=Blocks[kf-1].funpt;
02592 if (Blocks[kf-1].type==4||Blocks[kf-1].type==10004) {
02593 scicos_time=*t;
02594 Blocks[kf-1].nevprt=nclock;
02595 loc4 = (ScicosF4) loc;
02596 if(Blocks[kf-1].ng>0){
02597 Blocks[kf-1].g=&g[zcptr[kf]-1];
02598 }
02599 if(Blocks[kf-1].nx==0){
02600 (*loc4)(&Blocks[kf-1],*flag);
02601 }
02602 else {
02603 Blocks[kf-1].x=&xt[xptr[kf]-1];
02604 if(Blocks[kf-1].type==4) {
02605 if(*flag==0 && solver==100) {
02606 Blocks[kf-1].res=&residual[xptr[kf]-1];
02607 Blocks[kf-1].xd=&residual[xptr[kf]-1];
02608 (*loc4)(&Blocks[kf-1],*flag);
02609 Blocks[kf-1].xd=&xtd[xptr[kf]-1];
02610 if(flagi!=7) {
02611 for (k=0;k<Blocks[kf-1].nx;k++) {
02612 Blocks[kf-1].res[k]=Blocks[kf-1].res[k]-Blocks[kf-1].xd[k];
02613 }
02614 }
02615 else {
02616 for (k=0;k<Blocks[kf-1].nx;k++) {
02617 Blocks[kf-1].xd[k]=Blocks[kf-1].res[k];
02618 }
02619 }
02620 }
02621 else {
02622 Blocks[kf-1].xd=&xtd[xptr[kf]-1];
02623 (*loc4)(&Blocks[kf-1],*flag);
02624 }
02625 }
02626 else {
02627 Blocks[kf-1].xd=&xtd[xptr[kf]-1];
02628 Blocks[kf-1].res=&residual[xptr[kf]-1];
02629 (*loc4)(&Blocks[kf-1],*flag);
02630 }
02631 }
02632 if ( cosd > 1){
02633 if(debug_block>-1){
02634 if (*flag<0) return;
02635 if (cosd != 3) sciprint("Leaving block %d \r\n",kf);
02636 call_debug_scicos(t,xtd,xt,residual,g,flag,kf,flagi,debug_block);
02637 }
02638 }
02639 return;
02640 }
02641
02642
02643 if(nclock<0){
02644 for (j =0;j<Blocks[kf-1].ng;++j){
02645 Blocks[kf-1].g[j]=(double)Blocks[kf-1].jroot[j];
02646 }
02647 }
02648
02649 if(Blocks[kf-1].ztyp>0){
02650 Blocks[kf-1].g=&g[zcptr[kf]-1];
02651 }
02652 if(Blocks[kf-1].nx>0){
02653 Blocks[kf-1].x=&xt[xptr[kf]-1];
02654 Blocks[kf-1].xd=&xtd[xptr[kf]-1];
02655 if(solver==100) {
02656 Blocks[kf-1].res=&residual[xptr[kf]-1];
02657 }
02658 }
02659 else
02660 {
02661 Blocks[kf-1].x = NULL ;
02662 Blocks[kf-1].xd = NULL ;
02663 }
02664
02665 switch (Blocks[kf-1].type) {
02666
02667 case 1 :
02668
02669 for (in = 0 ; in < Blocks[kf-1].nin ; in++)
02670 {
02671 args[in]=Blocks[kf-1].inptr[in];
02672 sz[in]=Blocks[kf-1].insz[in];
02673 }
02674 for (out=0;out<Blocks[kf-1].nout;out++) {
02675 args[in+out]=Blocks[kf-1].outptr[out];
02676 sz[in+out]=Blocks[kf-1].outsz[out];
02677 }
02678 if(Blocks[kf-1].ztyp>0){
02679 Blocks[kf-1].g=&g[zcptr[kf]-1];
02680 args[Blocks[kf-1].nin+Blocks[kf-1].nout]=Blocks[kf-1].g;
02681 sz[Blocks[kf-1].nin+Blocks[kf-1].nout]=Blocks[kf-1].ng;
02682 }
02683 loc1 = (ScicosF) loc;
02684 if (solver==100) {
02685 (*loc1)(flag,&nclock,t,Blocks[kf-1].res,Blocks[kf-1].x,&Blocks[kf-1].nx,
02686 Blocks[kf-1].z,&Blocks[kf-1].nz,
02687 Blocks[kf-1].evout,&Blocks[kf-1].nevout,Blocks[kf-1].rpar,&Blocks[kf-1].nrpar,
02688 Blocks[kf-1].ipar,&Blocks[kf-1].nipar,
02689 (double *)args[0],&sz[0],
02690 (double *)args[1],&sz[1],(double *)args[2],&sz[2],
02691 (double *)args[3],&sz[3],(double *)args[4],&sz[4],
02692 (double *)args[5],&sz[5],(double *)args[6],&sz[6],
02693 (double *)args[7],&sz[7],(double *)args[8],&sz[8],
02694 (double *)args[9],&sz[9],(double *)args[10],&sz[10],
02695 (double *)args[11],&sz[11],(double *)args[12],&sz[12],
02696 (double *)args[13],&sz[13],(double *)args[14],&sz[14],
02697 (double *)args[15],&sz[15],(double *)args[16],&sz[16],
02698 (double *)args[17],&sz[17]);
02699 }
02700 else {
02701 (*loc1)(flag,&nclock,t,Blocks[kf-1].xd,Blocks[kf-1].x,&Blocks[kf-1].nx,
02702 Blocks[kf-1].z,&Blocks[kf-1].nz,
02703 Blocks[kf-1].evout,&Blocks[kf-1].nevout,Blocks[kf-1].rpar,&Blocks[kf-1].nrpar,
02704 Blocks[kf-1].ipar,&Blocks[kf-1].nipar,
02705 (double *)args[0],&sz[0],
02706 (double *)args[1],&sz[1],(double *)args[2],&sz[2],
02707 (double *)args[3],&sz[3],(double *)args[4],&sz[4],
02708 (double *)args[5],&sz[5],(double *)args[6],&sz[6],
02709 (double *)args[7],&sz[7],(double *)args[8],&sz[8],
02710 (double *)args[9],&sz[9],(double *)args[10],&sz[10],
02711 (double *)args[11],&sz[11],(double *)args[12],&sz[12],
02712 (double *)args[13],&sz[13],(double *)args[14],&sz[14],
02713 (double *)args[15],&sz[15],(double *)args[16],&sz[16],
02714 (double *)args[17],&sz[17]);
02715 }
02716 break;
02717 case 0 :
02718
02719 ni=0;
02720
02721
02722 if (Blocks[kf-1].nin>1) {
02723 ki=0;
02724 for (in=0;in<Blocks[kf-1].nin;in++) {
02725 lprt=inplnk[inpptr[kf]+in];
02726 szi=outtbsz[2*(lprt-1)]*outtbsz[2*(lprt-1)+1];
02727 outtbdptr=(double *)outtbptr[lprt-1];
02728 for (ii=0;ii<szi;ii++) intabl[ki++]=outtbdptr[ii];
02729 ni=ni+szi;
02730 }
02731 args[0]=&(intabl[0]);
02732 }
02733 else {
02734 if (Blocks[kf-1].nin==0) {
02735 ni=0;
02736 outtbdptr=(double *)outtbptr[0];
02737 args[0]=&(outtbdptr[0]);
02738 }
02739 else {
02740 lprt=inplnk[inpptr[kf]];
02741 outtbdptr=(double *)outtbptr[lprt-1];
02742 args[0]=&(outtbdptr[0]);
02743 ni=outtbsz[2*(lprt-1)]*outtbsz[2*(lprt-1)+1];
02744 }
02745 }
02746 in=Blocks[kf-1].nin;
02747
02748
02749 no=0;
02750 if (Blocks[kf-1].nout>1) {
02751 ko=0;
02752 for (out=0;out<Blocks[kf-1].nout;out++) {
02753 lprt=outlnk[outptr[kf]+out];
02754 szi=outtbsz[2*(lprt-1)]*outtbsz[2*(lprt-1)+1];
02755 outtbdptr=(double *)outtbptr[lprt-1];
02756 for (ii=0;ii<szi;ii++) outabl[ko++]=outtbdptr[ii];
02757 no=no+szi;
02758 }
02759 args[1]=&(outabl[0]);
02760 }
02761 else {
02762 if (Blocks[kf-1].nout==0) {
02763 no=0;
02764 outtbdptr=(double *)outtbptr[0];
02765 args[1]=&(outtbdptr[0]);
02766 }
02767 else {
02768 lprt=outlnk[outptr[kf]];
02769 outtbdptr=(double *)outtbptr[lprt-1];
02770 args[1]=&(outtbdptr[0]);
02771 no=outtbsz[2*(lprt-1)]*outtbsz[2*(lprt-1)+1];
02772 }
02773 }
02774
02775 loc0 = (ScicosF0) loc;
02776 if (solver==100) {
02777 (*loc0)(flag,&nclock,t,Blocks[kf-1].res,Blocks[kf-1].x,&Blocks[kf-1].nx,
02778 Blocks[kf-1].z,&Blocks[kf-1].nz,
02779 Blocks[kf-1].evout,&Blocks[kf-1].nevout,Blocks[kf-1].rpar,&Blocks[kf-1].nrpar,
02780 Blocks[kf-1].ipar,&Blocks[kf-1].nipar,(double *)args[0],&ni,
02781 (double *)args[1],&no);
02782 }
02783 else {
02784 (*loc0)(flag,&nclock,t,Blocks[kf-1].xd,Blocks[kf-1].x,&Blocks[kf-1].nx,
02785 Blocks[kf-1].z,&Blocks[kf-1].nz,
02786 Blocks[kf-1].evout,&Blocks[kf-1].nevout,Blocks[kf-1].rpar,&Blocks[kf-1].nrpar,
02787 Blocks[kf-1].ipar,&Blocks[kf-1].nipar,(double *)args[0],&ni,
02788 (double *)args[1],&no);
02789 }
02790
02791
02792 if (Blocks[kf-1].nout>1) {
02793 ko=0;
02794 for (out=0;out<Blocks[kf-1].nout;out++) {
02795 lprt=outlnk[outptr[kf]+out];
02796 szi=outtbsz[2*(lprt-1)]*outtbsz[2*(lprt-1)+1];
02797 outtbdptr=(double *)outtbptr[lprt-1];
02798 for (ii=0;ii<szi;ii++) outtbdptr[ii]=outabl[ko++];
02799 }
02800 }
02801 break;
02802 case 2 :
02803
02804
02805 if (solver==100) {
02806 if (Blocks[kf-1].ztyp==0){
02807 loc2 = (ScicosF2) loc;
02808 (*loc2)(flag,&nclock,t,Blocks[kf-1].res,Blocks[kf-1].x,&Blocks[kf-1].nx,
02809 Blocks[kf-1].z,&Blocks[kf-1].nz,
02810 Blocks[kf-1].evout,&Blocks[kf-1].nevout,Blocks[kf-1].rpar,&Blocks[kf-1].nrpar,
02811 Blocks[kf-1].ipar,&Blocks[kf-1].nipar,(double **)Blocks[kf-1].inptr,
02812 Blocks[kf-1].insz,&Blocks[kf-1].nin,
02813 (double **)Blocks[kf-1].outptr,Blocks[kf-1].outsz,&Blocks[kf-1].nout);
02814 }
02815 else{
02816 loc2z = (ScicosF2z) loc;
02817 (*loc2z)(flag,&nclock,t,Blocks[kf-1].res,Blocks[kf-1].x,&Blocks[kf-1].nx,
02818 Blocks[kf-1].z,&Blocks[kf-1].nz,
02819 Blocks[kf-1].evout,&Blocks[kf-1].nevout,Blocks[kf-1].rpar,&Blocks[kf-1].nrpar,
02820 Blocks[kf-1].ipar,&Blocks[kf-1].nipar,
02821 (double **)Blocks[kf-1].inptr,Blocks[kf-1].insz,&Blocks[kf-1].nin,
02822 (double **)Blocks[kf-1].outptr,Blocks[kf-1].outsz,&Blocks[kf-1].nout,
02823 Blocks[kf-1].g,&Blocks[kf-1].ng);
02824 }
02825 }
02826 else {
02827 if (Blocks[kf-1].ztyp==0){
02828 loc2 = (ScicosF2) loc;
02829 (*loc2)(flag,&nclock,t,Blocks[kf-1].xd,Blocks[kf-1].x,&Blocks[kf-1].nx,
02830 Blocks[kf-1].z,&Blocks[kf-1].nz,
02831 Blocks[kf-1].evout,&Blocks[kf-1].nevout,Blocks[kf-1].rpar,&Blocks[kf-1].nrpar,
02832 Blocks[kf-1].ipar,&Blocks[kf-1].nipar,(double **)Blocks[kf-1].inptr,
02833 Blocks[kf-1].insz,&Blocks[kf-1].nin,
02834 (double **)Blocks[kf-1].outptr,Blocks[kf-1].outsz,&Blocks[kf-1].nout);
02835 }
02836 else{
02837 loc2z = (ScicosF2z) loc;
02838 (*loc2z)(flag,&nclock,t,Blocks[kf-1].xd,Blocks[kf-1].x,&Blocks[kf-1].nx,
02839 Blocks[kf-1].z,&Blocks[kf-1].nz,
02840 Blocks[kf-1].evout,&Blocks[kf-1].nevout,Blocks[kf-1].rpar,&Blocks[kf-1].nrpar,
02841 Blocks[kf-1].ipar,&Blocks[kf-1].nipar,(double **)Blocks[kf-1].inptr,
02842 Blocks[kf-1].insz,&Blocks[kf-1].nin,
02843 (double **)Blocks[kf-1].outptr,Blocks[kf-1].outsz,&Blocks[kf-1].nout,
02844 Blocks[kf-1].g,&Blocks[kf-1].ng);
02845 }
02846 }
02847 break;
02848 case 10001 :
02849
02850 for (in = 0 ; in < Blocks[kf-1].nin ; in++)
02851 {
02852 args[in]=Blocks[kf-1].inptr[in];
02853 sz[in]=Blocks[kf-1].insz[in];
02854 }
02855 for (out=0;out<Blocks[kf-1].nout;out++) {
02856 args[in+out]=Blocks[kf-1].outptr[out];
02857 sz[in+out]=Blocks[kf-1].outsz[out];
02858 }
02859 if(Blocks[kf-1].ztyp>0){
02860 Blocks[kf-1].g=&g[zcptr[kf]-1];
02861 args[Blocks[kf-1].nin+Blocks[kf-1].nout]=Blocks[kf-1].g;
02862 sz[Blocks[kf-1].nin+Blocks[kf-1].nout]=Blocks[kf-1].ng;
02863 }
02864 loci1 = (ScicosFi) loc;
02865
02866 (*loci1)(flag,&nclock,t,Blocks[kf-1].res,Blocks[kf-1].xd,Blocks[kf-1].x,
02867 &Blocks[kf-1].nx,Blocks[kf-1].z,&Blocks[kf-1].nz,
02868 Blocks[kf-1].evout,&Blocks[kf-1].nevout,Blocks[kf-1].rpar,&Blocks[kf-1].nrpar,
02869 Blocks[kf-1].ipar,&Blocks[kf-1].nipar,
02870 (double *)args[0],&sz[0],
02871 (double *)args[1],&sz[1],(double *)args[2],&sz[2],
02872 (double *)args[3],&sz[3],(double *)args[4],&sz[4],
02873 (double *)args[5],&sz[5],(double *)args[6],&sz[6],
02874 (double *)args[7],&sz[7],(double *)args[8],&sz[8],
02875 (double *)args[9],&sz[9],(double *)args[10],&sz[10],
02876 (double *)args[11],&sz[11],(double *)args[12],&sz[12],
02877 (double *)args[13],&sz[13],(double *)args[14],&sz[14],
02878 (double *)args[15],&sz[15],(double *)args[16],&sz[16],
02879 (double *)args[17],&sz[17]);
02880 break;
02881 case 10002 :
02882
02883
02884 if(Blocks[kf-1].ztyp==0) {
02885 loci2 = (ScicosFi2) loc;
02886
02887 (*loci2)(flag,&nclock,t,Blocks[kf-1].res,
02888 Blocks[kf-1].xd,Blocks[kf-1].x,&Blocks[kf-1].nx,
02889 Blocks[kf-1].z,&Blocks[kf-1].nz,
02890 Blocks[kf-1].evout,&Blocks[kf-1].nevout,Blocks[kf-1].rpar,&Blocks[kf-1].nrpar,
02891 Blocks[kf-1].ipar,&Blocks[kf-1].nipar,(double **)Blocks[kf-1].inptr,
02892 Blocks[kf-1].insz,&Blocks[kf-1].nin,
02893 (double **)Blocks[kf-1].outptr,Blocks[kf-1].outsz,&Blocks[kf-1].nout);
02894 }
02895 else {
02896 loci2z = (ScicosFi2z) loc;
02897
02898 (*loci2z)(flag,&nclock,t,Blocks[kf-1].res,
02899 Blocks[kf-1].xd,Blocks[kf-1].x,&Blocks[kf-1].nx,
02900 Blocks[kf-1].z,&Blocks[kf-1].nz,
02901 Blocks[kf-1].evout,&Blocks[kf-1].nevout,Blocks[kf-1].rpar,&Blocks[kf-1].nrpar,
02902 Blocks[kf-1].ipar,&Blocks[kf-1].nipar,
02903 (double **)Blocks[kf-1].inptr,Blocks[kf-1].insz,&Blocks[kf-1].nin,
02904 (double **)Blocks[kf-1].outptr,Blocks[kf-1].outsz,&Blocks[kf-1].nout,
02905 Blocks[kf-1].g,&Blocks[kf-1].ng);
02906 }
02907 break;
02908 default:
02909 sciprint("Undefined Function type\r\n");
02910 *flag=-1000;
02911 return;
02912 }
02913 if(solver==100 && Blocks[kf-1].type<10000 && *flag==0) {
02914
02915 if(flagi!=7) {
02916 for (k=0;k<Blocks[kf-1].nx;k++) {
02917 Blocks[kf-1].res[k]=Blocks[kf-1].res[k]-Blocks[kf-1].xd[k];
02918 }
02919 }
02920 else {
02921 for (k=0;k<Blocks[kf-1].nx;k++) {
02922 Blocks[kf-1].xd[k]=Blocks[kf-1].res[k];
02923 }
02924 }
02925 }
02926 for(in=0;in<Blocks[kf-1].nevout;++in){
02927 Blocks[kf-1].evout[in]=Blocks[kf-1].evout[in]-*t;
02928 }
02929 if ( cosd > 1){
02930 if(debug_block>-1){
02931 if (*flag<0) return;
02932 if (cosd != 3) sciprint("Leaving block %d \r\n",kf);
02933 call_debug_scicos(t,xtd,xt,residual,g,flag,kf,flagi,debug_block);
02934 }
02935 }
02936 }
02937
02938
02939 void call_debug_scicos(t,xtd,xt,residual,g,flag,kf,flagi,deb_blk)
02940 integer *flag,kf,flagi,deb_blk;
02941 double *t,*xtd,*xt,*residual,*g;
02942 {
02943 voidf loc ;
02944 int solver=C2F(cmsolver).solver,k;
02945 ScicosF4 loc4;
02946 C2F(cosdebugcounter).counter=C2F(cosdebugcounter).counter+1;
02947
02948 C2F(scsptr).ptr=Blocks[deb_blk].scsptr;
02949 loc=Blocks[deb_blk].funpt;
02950 scicos_time=*t;
02951 Blocks[kf-1].nevprt=nclock;
02952 loc4 = (ScicosF4) loc;
02953 if(Blocks[kf-1].ng>0){
02954 Blocks[kf-1].g=&g[zcptr[kf]-1];
02955 }
02956 if(Blocks[kf-1].nx==0){
02957 (*loc4)(&Blocks[kf-1],*flag);
02958 }
02959 else {
02960 Blocks[kf-1].x=&xt[xptr[kf]-1];
02961 if(*flag==0 && solver==100) {
02962 Blocks[kf-1].res=&residual[xptr[kf]-1];
02963 Blocks[kf-1].xd=&residual[xptr[kf]-1];
02964 (*loc4)(&Blocks[kf-1],*flag);
02965 Blocks[kf-1].xd=&xtd[xptr[kf]-1];
02966 if(flagi!=7) {
02967 for (k=0;k<Blocks[kf-1].nx;k++) {
02968 Blocks[kf-1].res[k]=Blocks[kf-1].res[k]-Blocks[kf-1].xd[k];
02969 }
02970 }
02971 else {
02972 for (k=0;k<Blocks[kf-1].nx;k++) {
02973 Blocks[kf-1].xd[k]=Blocks[kf-1].res[k];
02974 }
02975 }
02976 }
02977 else {
02978 Blocks[kf-1].xd=&xtd[xptr[kf]-1];
02979 (*loc4)(&Blocks[kf-1],*flag);
02980 }
02981 }
02982 if (*flag<0) sciprint("Error in the Debug block \r\n");
02983 }
02984
02985
02986
02987
02988
02989 integer C2F(funnum)(fname)
02990 char * fname;
02991 {
02992 int i=0,ln;
02993 integer loc=-1;
02994 while ( tabsim[i].name != (char *) NULL) {
02995 if ( strcmp(fname,tabsim[i].name) == 0 ) return(i+1);
02996 i++;
02997 }
02998 ln=(int)strlen(fname);
02999 C2F(iislink)(fname,&loc);C2F(iislink)(fname,&loc);
03000 if (loc >= 0) return(ntabsim+(int)loc+1);
03001 return(0);
03002 }
03003
03004
03005
03006 int C2F(simblk)(neq1, t, xc, xcdot)
03007 integer *neq1;
03008 double *t, *xc, *xcdot;
03009
03010
03011
03012
03013
03014
03015
03016
03017
03018
03019 {
03020 C2F(dset)(neq, &c_b14,xcdot , &c__1);
03021 C2F(ierscode).iero = 0;
03022 *ierr= 0;
03023 odoit(xcdot, xc,xcdot,t);
03024 C2F(ierscode).iero = *ierr;
03025 return 0;
03026 }
03027
03028 int C2F(simblkdaskr)(t,xc,xcdot,cj,residual,ires,rpar1,ipar1)
03029 integer *ires,*ipar1;
03030 double *t, *xc, *xcdot, *rpar1, *residual;
03031 double *cj;
03032
03033
03034
03035
03036
03037
03038
03039
03040
03041
03042 {
03043 CJJ=*cj;
03044 C2F(dcopy)(neq, xcdot, &c__1, residual, &c__1);
03045 *ires=0;
03046 *ierr= 0;
03047 C2F(ierscode).iero = 0;
03048 odoit(residual, xc, xcdot,t);
03049 C2F(ierscode).iero = *ierr;
03050 if(C2F(ierscode).iero != 0) *ires=-1;
03051 return 0;
03052 }
03053
03054
03055 int C2F(grblkdaskr)(neq1, t, xc, xtd,ng1, g,rpar1,ipar1)
03056 integer *neq1;
03057 double *t, *xc, *xtd;
03058 integer *ng1,*ipar1;
03059 double *g,*rpar1;
03060 {
03061 *ierr= 0;
03062 C2F(ierscode).iero = 0;
03063 zdoit(g, xtd, xc,t);
03064 C2F(ierscode).iero = *ierr;
03065 return 0;
03066 }
03067
03068
03069 int C2F(grblk)(neq1, t, xc, ng1, g)
03070 integer *neq1;
03071 double *t, *xc;
03072 integer *ng1;
03073 double *g;
03074
03075
03076
03077
03078
03079
03080
03081
03082
03083
03084
03085
03086
03087
03088
03089 {
03090 C2F(ierscode).iero = 0;
03091 *ierr= 0;
03092 zdoit(g,xc, xc,t);
03093 C2F(ierscode).iero = *ierr;
03094 return 0;
03095 }
03096
03097
03098 void addevs(t, evtnb, ierr1)
03099 double t;
03100 integer *evtnb, *ierr1;
03101 {
03102 static integer i, j;
03103
03104
03105 *ierr1 = 0;
03106 if (evtspt[*evtnb] != -1) {
03107 *ierr1 = 1;
03108 return;
03109 } else {
03110 evtspt[*evtnb] = 0;
03111 tevts[*evtnb] = t;
03112 }
03113 if (*pointi == 0) {
03114 *pointi = *evtnb;
03115 return;
03116 }
03117 if (t < tevts[*pointi]) {
03118 evtspt[*evtnb] = *pointi;
03119 *pointi = *evtnb;
03120 return;
03121 }
03122 i = *pointi;
03123
03124 L100:
03125 if (evtspt[i] == 0) {
03126 evtspt[i] = *evtnb;
03127 return;
03128 }
03129 if (t >= tevts[evtspt[i]]) {
03130 j = evtspt[i];
03131 if (evtspt[j] == 0) {
03132 evtspt[j] = *evtnb;
03133 return;
03134 }
03135 i = j;
03136 goto L100;
03137 } else {
03138 evtspt[*evtnb] = evtspt[i];
03139 evtspt[i] = *evtnb;
03140 }
03141 }
03142
03143 void putevs(t, evtnb, ierr1)
03144 double *t;
03145 integer *evtnb, *ierr1;
03146 {
03147
03148
03149 *ierr1 = 0;
03150 if (evtspt[*evtnb] != -1) {
03151 *ierr1 = 1;
03152 return;
03153 } else {
03154 evtspt[*evtnb] = 0;
03155 tevts[*evtnb] = *t;
03156 }
03157 if (*pointi == 0) {
03158 *pointi = *evtnb;
03159 return;
03160 }
03161 evtspt[*evtnb] = *pointi;
03162 *pointi = *evtnb;
03163 }
03164
03165
03166
03167 void FREE_blocks()
03168
03169 {
03170 int kf;
03171 for (kf = 0; kf < nblk; ++kf) {
03172 if (Blocks[kf].insz!=NULL) {
03173 FREE(Blocks[kf].insz);
03174 }else {
03175 break;
03176 }
03177 if (Blocks[kf].inptr!=NULL){
03178 FREE(Blocks[kf].inptr);
03179 }else {
03180 break;
03181 }
03182 if (Blocks[kf].outsz!=NULL){
03183 FREE(Blocks[kf].outsz);
03184 }else {
03185 break;
03186 }
03187 if (Blocks[kf].outptr!=NULL){
03188 FREE(Blocks[kf].outptr);
03189 }else {
03190 break;
03191 }
03192 if (Blocks[kf].label!=NULL){
03193 FREE(Blocks[kf].label);
03194 }else {
03195 break;
03196 }
03197 if (Blocks[kf].evout!=NULL){
03198 FREE(Blocks[kf].evout);
03199 }else {
03200 break;
03201 }
03202 }
03203 FREE(Blocks);
03204
03205 if(nmod>0) FREE(mod);
03206
03207 if(ng>0) FREE(g);
03208
03209 return;
03210 }
03211
03212
03213
03214 int setmode(W,x,told,jroot,ttol)
03215
03216 double *W,*x,*told,ttol;
03217 int *jroot;
03218 {
03219 int k,j,jj,diff;
03220 double ttmp;
03221
03222 ttmp=*told+ttol;
03223 zdoit(W,x,x,told);
03224 if (*ierr != 0) return 1;
03225 for(jj=0;jj<*neq;++jj){
03226 W[jj]=x[jj];
03227 }
03228 diff=1;
03229 k=0;
03230 while (diff!=0){
03231
03232 for(jj=0;jj<nmod;++jj){
03233 jroot[jj]=mod[jj];
03234 }
03235 for(j=0;j<=*neq;++j){
03236 C2F(simblk)(neq, &ttmp, W, &W[*neq]);
03237 if (*ierr != 0) return 1;
03238 for(jj=0;jj<*neq;++jj){
03239 W[jj]=x[jj]+ttol*W[jj+(*neq)];
03240 }
03241 }
03242
03243 zdoit(&W[2*(*neq)],W,W,&ttmp);
03244 if (*ierr != 0) return 1;
03245
03246 diff=0;
03247 for(jj=0;jj<nmod;++jj){
03248 if (jroot[jj]!=mod[jj]) {
03249 if(k>*neq) {
03250 *ierr=22;
03251 return 1;
03252 }
03253 k=k+1;
03254 diff=1;
03255 break;
03256 }
03257 }
03258 }
03259 return 0;
03260 }
03261
03262
03263 int get_phase_simulation()
03264
03265 {
03266 return phase;
03267 }
03268
03269
03270
03271 void do_cold_restart()
03272
03273 {
03274 hot=0;
03275 return;
03276 }
03277
03278 double get_scicos_time()
03279
03280 {
03281 return scicos_time;
03282 }
03283 int get_block_number()
03284
03285 {
03286 return C2F(curblk).kfun;
03287 }
03288
03289 void set_block_error(int err)
03290
03291 {
03292 *block_error=err;
03293 return;
03294 }
03295
03296
03297 void set_pointer_xproperty(int* pointer)
03298 {
03299 int i;
03300 for (i=0;i<n_pointer_xproperty;i++){
03301 pointer_xproperty[i]=pointer[i];
03302 }
03303 return;
03304 }
03305
03306
03307 void Set_Jacobian_flag(int flag)
03308 {
03309 Jacobian_Flag=flag;
03310 return;
03311 }
03312
03313 double Get_Jacobian_parameter(void)
03314 {
03315 return CJJ;
03316 }
03317
03318 double Get_Scicos_SQUR(void)
03319 {
03320 return SQuround;
03321 }
03322
03323 void Jdoit(residual, xt, xtd, told, job)
03324 double *residual, *xt, *xtd;
03325 double *told;
03326 int *job;
03327 {
03328
03329 integer i2;
03330
03331
03332 static integer flag__, keve, kiwa;
03333 static integer ierr1, i;
03334 static integer ii, jj;
03335
03336 kiwa = 0;
03337 for (jj = 1; jj <= noord; ++jj) {
03338 C2F(curblk).kfun = oord[jj];
03339 nclock = oord[jj + noord];
03340 if (outptr[C2F(curblk).kfun + 1] - outptr[C2F(curblk).kfun] > 0) {
03341 flag__ = 1;
03342
03343 if ((*job==2)&&(oord[jj]==nblk)) {
03344 }else
03345 callf(told, xtd, xt, residual,x,&flag__);
03346 if (flag__ < 0) {
03347 *ierr = 5 - flag__;
03348 return;
03349 }
03350 }
03351
03352 if (Blocks[C2F(curblk).kfun - 1].nevout > 0) {
03353 if (funtyp[C2F(curblk).kfun] < 0) {
03354 if(Blocks[C2F(curblk).kfun - 1].nmode > 0){
03355 i2 = Blocks[C2F(curblk).kfun - 1].mode[0] +
03356 clkptr[C2F(curblk).kfun] - 1;
03357 } else{
03358 if (funtyp[C2F(curblk).kfun] == -1) {
03359 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
03360 if (outtbdptr[0] <= 0.) {
03361 i=2;
03362 } else {
03363 i=1;
03364 }
03365 } else if (funtyp[C2F(curblk).kfun] == -2) {
03366 outtbdptr=(double *)outtbptr[-1+inplnk[inpptr[C2F(curblk).kfun]]];
03367 i=max(min((integer) outtbdptr[0],
03368 Blocks[C2F(curblk).kfun - 1].nevout),1);
03369 }
03370 i2 =i+ clkptr[C2F(curblk).kfun] - 1;
03371 }
03372 putevs(told, &i2, &ierr1);
03373 if (ierr1 != 0) {
03374
03375 *ierr = 3;
03376 return;
03377 }
03378 ozdoit(xtd, xt,told, &kiwa);
03379 }
03380 }
03381 }
03382
03383
03384 for (ii = 1; ii <= noord; ++ii) {
03385 C2F(curblk).kfun = oord[ii];
03386 if (Blocks[C2F(curblk).kfun-1].nx > 0||
03387 *Blocks[C2F(curblk).kfun-1].work !=NULL) {
03388
03389 flag__ = 0;
03390 if (((*job==1)&&(oord[ii]==nblk))||(*job!=1)){
03391 if (*job==1) flag__ = 10;
03392 nclock = oord[ii + noord];
03393 callf(told, xtd, xt, residual,xt,&flag__);
03394 };
03395 if (flag__ < 0) {
03396 *ierr = 5 - flag__;
03397 return;
03398 }
03399 }
03400 }
03401
03402 for (i = 0; i < kiwa; ++i) {
03403 keve = iwa[i];
03404 for (ii = ordptr[keve]; ii <= ordptr[keve + 1] - 1; ++ii) {
03405 C2F(curblk).kfun = ordclk[ii ];
03406 if (Blocks[C2F(curblk).kfun-1].nx > 0||
03407 *Blocks[C2F(curblk).kfun-1].work !=NULL) {
03408
03409
03410 flag__ = 0;
03411 if (((*job==1)&&(oord[ii]==nblk))||(*job!=1)){
03412 if (*job==1) flag__ = 10;
03413 nclock = abs(ordclk[ii + nordclk]);
03414 callf(told, xtd, xt, residual,xt,&flag__);
03415 }
03416 if (flag__ < 0) {
03417 *ierr = 5 - flag__;
03418 return;
03419 }
03420 }
03421 }
03422 }
03423 }
03424
03425
03426 int C2F(Jacobian)(t,xc, xcdot,residual,cj,rpar1,ipar1)
03427 int *ipar1;
03428 double *t, *xc, *xcdot, *rpar1, *residual;
03429 double *cj;
03430 {
03431 int i,j,n, nx,ni,no,nb,m;
03432 double *RX, *Fx, *Fu, *Gx, *Gu, *Ewt,*H,*SQUR,*ERR1,*ERR2;
03433 double *Hx, *Hu,*Kx,*Ku,*HuGx,*FuKx,*FuKuGx,*HuGuKx;
03434 double del,delinv,xsave,xdsave,ysave;
03435 double a,b;
03436 int job;
03437 double **y = (double **)Blocks[nblk-1].outptr;
03438 double **u = (double **)Blocks[nblk-1].inptr;
03439
03440 *ierr= 0;
03441 CJJ=*cj;
03442 n=*neq;
03443 nb=nblk;
03444 nx=Blocks[nblk-1].nx;m=n-nx;
03445 no=Blocks[nblk-1].nout;
03446 ni=Blocks[nblk-1].nin;
03447 H=residual+ n * n;
03448 SQUR=H+1;
03449 Ewt=SQUR+1;
03450 ERR1=Ewt+n;
03451 ERR2=ERR1+n;
03452 RX=ERR2+n;
03453 Fx=RX+(n+ni)*(n+no);
03454 Fu=Fx+nx*nx;
03455 Gx=Fu+nx*ni;
03456 Gu=Gx+no*nx;
03457 Hx=Gu+no*ni;
03458 Hu=Hx+m*m;
03459 Kx=Hu+m*no;
03460 Ku=Kx+ni*m;
03461 HuGx=Ku+ni*no;
03462 FuKx=HuGx+m*nx;
03463 FuKuGx=FuKx+nx*m;
03464 HuGuKx=FuKuGx+nx*nx;
03465
03466
03467 job=0;
03468 Jdoit(ERR1, xc, xcdot,t,&job);
03469 if (*ierr < 0) return -1;
03470
03471 for (i=0;i<m;i++)
03472 for (j=0;j<ni;j++)
03473 Kx[j+i*ni]=u[j][0];
03474
03475 for(i=0;i<m;i++){
03476 a= max(abs(H[0]*xcdot[i]),abs(1.0/Ewt[i]));
03477 b= max(1.0,abs(xc[i]));
03478 del=SQUR[0]*max(a,b);
03479 if (H[0]*xcdot[i]<0) del*=-1;
03480 del=(xc[i]+del)-xc[i];
03481 xsave=xc[i];
03482 xdsave=xcdot[i];
03483 xc[i]+=del;
03484 xcdot[i]+=CJJ*del;
03485 job=0;
03486 Jdoit(ERR2, xc, xcdot,t,&job);
03487 if (*ierr < 0) return -1;
03488 delinv=1.0/del;
03489 for(j=0;j<m;j++)
03490 Hx[m*i+j]=(ERR2[j]-ERR1[j])*delinv;
03491 for (j=0;j<ni;j++)
03492 Kx[j+i*ni]=(u[j][0]-Kx[j+i*ni])*delinv;
03493 xc[i]=xsave;
03494 xcdot[i]=xdsave;
03495 }
03496
03497 job=0;
03498 Jdoit(ERR1, xc, xcdot,t,&job);
03499 for (i=0;i<no;i++)
03500 for (j=0;j<ni;j++)
03501 Ku[j+i*ni]=u[j][0];
03502
03503 for(i=0;i<no;i++){
03504 del=SQUR[0]* max(1.0,abs(y[i][0]));
03505 del=(y[i][0]+del)-y[i][0];
03506 ysave=y[i][0];
03507 y[i][0]+=del;
03508 job=2;
03509 Jdoit(ERR2, xc, xcdot,t,&job);
03510 if (*ierr < 0) return -1;
03511 delinv=1.0/del;
03512 for(j=0;j<m;j++)
03513 Hu[m*i+j]=(ERR2[j]-ERR1[j])*delinv;
03514 for (j=0;j<ni;j++)
03515 Ku[j+i*ni]=(u[j][0]-Ku[j+i*ni])*delinv;
03516 y[i][0]=ysave;
03517 }
03518
03519 job=1;
03520 Jdoit(&Fx[-m], xc, xcdot,t,&job);
03521 if (*block_error!=0) sciprint("\n\r error in Jacobian");
03522
03523 Multp(Fu,Ku,RX,nx,ni,ni,no);Multp(RX,Gx,FuKuGx,nx,no,no,nx);
03524 for (i=0;i<nx;i++)
03525 for (j=0;j<nx;j++){
03526 residual[i+m+(j+m)*n]=Fx[i+j*nx]+FuKuGx[i+j*nx];
03527 }
03528
03529 Multp(Hu,Gx,HuGx,m, no, no,nx);
03530 for (i=0;i<nx;i++)
03531 for (j=0;j<m;j++){
03532 residual[j+(i+m)*n]=HuGx[j+i*m];
03533 }
03534
03535 Multp(Fu,Kx,FuKx,nx,ni,ni,m);
03536 for (i=0;i<m;i++)
03537 for (j=0;j<nx;j++){
03538 residual[(j+m)+i*n]=FuKx[j+i*nx];
03539 }
03540
03541 Multp(Hu,Gu,RX,m,no,no,ni); Multp(RX,Kx,HuGuKx,m,ni,ni,m);
03542 for (i=0;i<m;i++)
03543 for (j=0;j<m;j++){
03544 residual[i+(j)*n]=Hx[i+j*m]+HuGuKx[i+j*m];
03545 }
03546
03547
03548 C2F(ierscode).iero = *ierr;
03549 return 0;
03550
03551 }
03552
03553 void Multp(A,B,R,ra ,ca, rb,cb)
03554 double *A, *B, *R;
03555 int ra,rb,ca,cb;
03556 {
03557 int i,j,k;
03558 if (ca!=rb) sciprint("\n\r Error in matrix multiplication");
03559 for (i = 0; i<ra; i++)
03560 for (j = 0; j<cb; j++){
03561 R[i+ra*j]=0.0;
03562 for (k = 0; k<ca; k++)
03563 R[i+ra*j]+=A[i+k*ra]*B[k+j*rb];
03564 }
03565 return;
03566 }
03567
03568
03569
03570
03571
03572
03573
03574
03575
03576
03577
03578
03579
03580
03581
03582