123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806 |
- /* Begin dsblock5.c */
- /* File version: 1.4, 1998-03-20 */
- /*
- * Copyright (C) 1997-2001 Dynasim AB.
- * All rights reserved.
- *
- */
- }
- #undef externalTable_
- #ifdef NBR_TASKS
- DYMOLA_STATIC int nbrTasks_=NBR_TASKS;
- #else
- DYMOLA_STATIC int nbrTasks_=0;
- #endif
- #if !defined(DYMOLA_DSPACE)
- DYMOLA_STATIC double
- DymolaStartTimers_[
- #ifdef NrDymolaTimers_
- NrDymolaTimers_ ? NrDymolaTimers_ : 1
- #else
- 1
- #endif
- ];
- DYMOLA_STATIC double DymolaTimeZero[100000]={0};
- DYMOLA_STATIC int DymolaTimeZeroLength=100000;
- #endif
- #if !defined(DymolaHaveUpdateInitVars)
- DYMOLA_STATIC void UpdateInitVars(double *time, double X_[], double XD_[], double U_[], \
- double DP_[], int IP_[], Dymola_bool LP_[], double F_[], double Y_[], double W_[], double QZ_[], double duser_[], int iuser_[], void*cuser_[], struct DYNInstanceData*did_)
- {
- return;
- }
- #endif
- /* Must be initialized (and thus defined) because moutil is included first*/
- static int DYNStrInit(struct DYNInstanceData*did_) {
- if (DYNX(DYNAuxStr_,0)==0) {
- int j;
- for(j=0;j<sizeof(DYNAuxStr_)/sizeof(*DYNAuxStr_);++j) DYNAuxStr_[j]=did_->DYNAuxStrBuff_vec+j*
- #if defined(MAXAuxStrLen_) && MAXAuxStrLen_>10
- MAXAuxStrLen_
- #else
- 10
- #endif
- ;
- }
- return 0;
- }
- DYMOLA_STATIC void DYNSetAuxString(struct DYNInstanceData*did_,const char*s,int i) {
- DYNStrInit(did_);
- if (i>=0 && i<sizeof(DYNAuxStr_)/sizeof(*DYNAuxStr_)) {
- int j,mlen=
- #if defined(MAXAuxStrLen_) && MAXAuxStrLen_>10
- MAXAuxStrLen_
- #else
- 10
- #endif
- ;
- for(j=0;j<mlen-1 && s[j];++j) DYNAuxStr_[i][j]=s[j];
- DYNAuxStr_[i][j]=0;
- if (s[j]) {DymosimMessage("Truncated string variable to");DymosimMessage(DYNAuxStr_[i]);}
- } else DymosimMessage("Internal error in String handling.");
- }
- DYMOLA_STATIC void DYNSetAuxStringArray(struct DYNInstanceData*did_,struct StringArray s,int i) {
- int nrElem,j;
- nrElem=StringNrElements(s);
- if (i>=0 && i+nrElem<=sizeof(DYNAuxStr_)/sizeof(*DYNAuxStr_)) {
- for(j=0;j<nrElem;++j) {
- DYNSetAuxString(did_,s.data[j],i+j);
- }
- } else DymosimMessage("Internal error in String array handling.");
- }
- DYMOLA_STATIC const char*DYNGetAuxStr(struct DYNInstanceData*did_,int i) {
- DYNStrInit(did_);
- if (i>=0 && i<sizeof(DYNAuxStr_)/sizeof(*DYNAuxStr_)) {
- return DYNAuxStr_[i];
- }
- return "";
- }
- static int QNLfunc_vec[QNLmax_ ? QNLmax_ : 1] = {0};
- DYMOLA_STATIC int* QNLfunc = QNLfunc_vec;
- static int QNLjac_vec[QNLmax_ ? QNLmax_ :1] = {0};
- DYMOLA_STATIC int* QNLjac = QNLjac_vec;
- DYMOLA_STATIC int QNLmax=QNLmax_;
- #if !defined(NExternalObject_)
- #define NExternalObject_ 10
- #endif
-
- static double delayID_vec[SizeDelay_?SizeDelay_:1] = {0};
- DYMOLA_STATIC double* delayID = delayID_vec;
- DYMOLA_STATIC double Buffersize = 20000;
- DYMOLA_STATIC int setDefault_=0;
- DYMOLA_STATIC int setDefaultX_=0,setDefaultU_=0,setDefaultY_=0,setDefaultP_=0,setDefaultDX_=0,setDefaultW_=0;
- DYMOLA_STATIC LIBDS_API_AFTER void delayDerivativeClose(void);
- DYMOLA_STATIC void delayBuffersCloseNew(struct DYNInstanceData*did_) {
- int i;
- for(i=0;i<SizeDelay_;++i) delayID[i]=0;
- for(i=0;i<MAXAux+10000;++i) Aux_[i]=0;
- for(i=0;i<SizePre_;++i) QPre_[i]=0;
- for(i=0;i<SizePre_;++i) RefPre_[i]=0;
- for(i=0;i<SizeEq_;++i) EqRemember2_[i]=EqRemember1_[i]=0;
- for(i=0;i<NWhen_;++i) QEvaluateNew_[i]=QEvaluate_[i]=0;
- for(i=0;i<NGlobalHelp_;++i) DYNhelp[i]=0;
- for(i=0;i<NGlobalHelpI_;++i) did_->helpvari_vec[i]=0;
- for(i=0;i<2*NRel_+1;++i)
- oldQZ2_[i]=oldQZ3_[i] = QZold_[i]=oldQZ_[i]=oldQZDummy_[i]=0;
- for(i=0;i<NRel_+1;++i) QRel_[i]=QM_[i]=Qn_[i] = Qp_[i]=Qscaled_[i]=0.0;
- for(i=0;i<NSamp_;++i) {NextSampleTime_[i]=NextSampleTimeNew_[i]=0;NextSampleAct_[i]=NextSampleActNew_[i]=0;}
- for(i=0;i<NRel_+1;++i) QL_[i]=Qenable_[i]=0;
- for(i=0;i<NTim_+1;++i) QTimed_[i]=0;
- EqRemember1Time_=-1e33;
- EqRemember2Time_=-1e33;
- for(i=NExternalObject_-1;i>=0;--i) {
- /* Reverse order in case of dependencies */
- void*x=did_->externalTable_vec[i].obj_;
- did_->externalTable_vec[i].obj_=0;
- if (x && did_->externalTable_vec[i].destructor_)
- (*(did_->externalTable_vec[i].destructor_))(x);
- did_->externalTable_vec[i].destructor_=0;
- }
- for(i=0;i<did_->DymolaTimerStructsLen_var;i++) {
- did_->DymolaTimerStructs_vec[i].num=0;
- }
- delayDerivativeClose();
- }
- DYMOLA_STATIC void delayBuffersClose(void) {
- delayBuffersCloseNew(&tempData);
- }
- DYMOLA_STATIC int dynInstanceDataSize() {
- return sizeof(struct DYNInstanceData);
- }
- DYMOLA_STATIC void CheckForEvents(struct DYNInstanceData*did_,double Time, int Init, int Event,
- double QZ_[], int nrel_, double F_[], int nx_,double*duser_,int*iuser_)
- /* SCRAMBLE ON */
- {
- #define DebugCheckForEvents 0
-
- #define OvershootFactor 1.2
- /* */
- #define FindLastEvent 1
- /* */
- #define CheckForEventsEps 1e-10
- /* */
- #define SecondDegreeOvershootFactor 1.04
- #define SecondDegreeUncertainty 0.4
- #define SecondDegreeUncertainty2 0.7
- int ZZZ715,ZZZ39; static double oldTime,oldDummyTime=-1e30;
- static double oldTime2, oldDummyTime2, oldstepSizeRatio;
- static double c1, c2, c1start; static double T1end=-1e30; static double T2end=-1e30, stepSizeRatio=1; double ZZZ8329, ZZZ7652;
- #ifdef InterpolateStatesForInline
- static const double CheckForEventsMinStep=0.2;
- #else
- static const double CheckForEventsMinStep=0;
- #endif
- int ZZZ5998; if (Init) {
- #if defined(FindEvent_)
- DymosimMessage("");
- DymosimMessage("Approximative event finder used. Must be used with Euler method."); DymosimMessage("");
- #endif
- StepSize = 0; LastTime = 1E30; T1end = -1E30; T2end = -1E30; oldTime = Time; oldDummyTime = -1e30;
- #if SecondDegree
- oldTime2 = oldDummyTime2 = Time; oldstepSizeRatio = 1.0;
- #endif
- c1=1; c2=1; c1start=1; stepSizeRatio=1; } if (StepSize == 0 && Time > LastTime) StepSize = Time - LastTime;
- if (Event) LastTime = Time; ZZZ5998 = Time>oldDummyTime; if (ZZZ5998) {
- #if SecondDegree
- oldTime2=oldTime; oldDummyTime2=oldDummyTime; oldstepSizeRatio=stepSizeRatio;
- if (StepSize!=0) { if (Time>=T1end && Time<T2end) stepSizeRatio = (T1end-oldTime)/StepSize; else stepSizeRatio = (Time-oldTime)/StepSize; } for (ZZZ715 = 0; ZZZ715 < 2*nrel_;ZZZ715++) {oldQZ3_[ZZZ715]=oldQZ2_[ZZZ715];oldQZ2_[ZZZ715]=oldQZ_[ZZZ715];}
- #endif
- for(ZZZ715=0;ZZZ715<2*nrel_;ZZZ715++) oldQZ_[ZZZ715]=oldQZDummy_[ZZZ715]; }
- { for(ZZZ715=0;ZZZ715<2*nrel_;ZZZ715++) oldQZDummy_[ZZZ715]=Qenable_[ZZZ715/2+1] ? QZ_[ZZZ715] : 0; } if (StepSize!=0 && ZZZ5998) { double ZZZ1317=0;
- #if DebugCheckForEvents
- for (ZZZ715 = 0; ZZZ715 < 2*nrel_; ZZZ715++) { if (Qenable_[ZZZ715/2+1]) { if (oldQZ_[ZZZ715]*QZ_[ZZZ715]<0) {
- double ZZZ8860; ZZZ8860=QZ_[ZZZ715]/(QZ_[ZZZ715]-oldQZ_[ZZZ715]); if (ZZZ8860>ZZZ1317) {ZZZ1317=ZZZ8860;ZZZ39=ZZZ715/2;} } } } if (ZZZ1317>0) { char ZZZ732[200]; if (Time<T2end) { sprintf(ZZZ732,"Event at projected time %.10g overshoot %.10g",T1end,c1*ZZZ1317+1);
- } else if (stepSizeRatio>1+CheckForEventsEps || stepSizeRatio<1-CheckForEventsEps) { sprintf(ZZZ732,"Missed event at time %.10g interpolated at %.10g",Time,Time-ZZZ1317*stepSizeRatio*StepSize); } else { sprintf(ZZZ732,"Event at time %.10g interpolated at %.10g",Time,Time-ZZZ1317*StepSize); } DymosimMessage(ZZZ732);
- #if SecondDegree
- sprintf(ZZZ732,"Relation %d QZ=%.10g %.10g oldQZ=%.10g oldQZ2=%.10g oldQZ3=%.10g",ZZZ39,QZ_[2*ZZZ39],QZ_[2*ZZZ39+1],oldQZ_[2*ZZZ39],oldQZ2_[2*ZZZ39],oldQZ3_[2*ZZZ39]);
- #else
- sprintf(ZZZ732,"Relation %d QZ=%.10g %.10g oldQZ=%.10g",ZZZ39,QZ_[2*ZZZ39],QZ_[2*ZZZ39+1],oldQZ_[2*ZZZ39]);
- #endif
- DymosimMessage(ZZZ732); }
- #endif
- } if (StepSize != 0 && Time >= T2end) { c1 = c1start = FindLastEvent ? 0 :2; ZZZ7652 = (NextTimeEvent-Time)/StepSize; /* */
- ZZZ39=-1; if (ZZZ7652>0 && ZZZ7652<2 && (FindLastEvent ? ZZZ7652>c1: ZZZ7652<c1)) { c1=ZZZ7652; } for (ZZZ715 = 0; ZZZ715 < 2*nrel_; ZZZ715++) { ZZZ8329 = (QZ_[ZZZ715] - oldQZ_[ZZZ715])/stepSizeRatio; if (QZ_[ZZZ715] * (OvershootFactor*ZZZ8329*2 + QZ_[ZZZ715]) < 0 && Qenable_[ZZZ715/2+1] && QZ_[2*(ZZZ715/2)]*QZ_[2*(ZZZ715/2)+1]>0 ) { /* */ ZZZ7652 = -QZ_[ZZZ715]/ZZZ8329+(OvershootFactor-1); /* */
- #if SecondDegree
- if (oldDummyTime2>-1e30 && (oldQZ_[ZZZ715]>0 ? oldQZ2_[ZZZ715]>oldQZ_[ZZZ715] : oldQZ2_[ZZZ715]<oldQZ_[ZZZ715])) { /* */ double ZZZ3419, ZZZ8687, ZZZ4213, ZZZ2231, ZZZ4006, ZZZ6591, ZZZ5281, ZZZ8430, ZZZ7134; ZZZ3419=QZ_[ZZZ715]; ZZZ8687=(stepSizeRatio+oldstepSizeRatio); ZZZ4213=stepSizeRatio*ZZZ8687*oldstepSizeRatio; ZZZ2231=(ZZZ8687*ZZZ8687*(oldQZ_[ZZZ715]-ZZZ3419)-stepSizeRatio*stepSizeRatio*(oldQZ2_[ZZZ715]-ZZZ3419))/ZZZ4213; ZZZ4006=(-ZZZ8687*(oldQZ_[ZZZ715]-ZZZ3419)+stepSizeRatio*(oldQZ2_[ZZZ715]-ZZZ3419))/ZZZ4213; ZZZ6591=4*ZZZ3419*ZZZ4006; ZZZ5281=ZZZ2231*ZZZ2231;
- ZZZ8430=(oldQZ_[ZZZ715]>0 ? oldQZ3_[ZZZ715]>oldQZ2_[ZZZ715] : oldQZ3_[ZZZ715]<oldQZ2_[ZZZ715]) ? SecondDegreeUncertainty : SecondDegreeUncertainty2; ZZZ7134=ZZZ5281-(ZZZ6591>0 ? ZZZ6591*(1+ZZZ8430) : ZZZ6591*(1-ZZZ8430)); if (ZZZ7134>=0) { double ZZZ5803; ZZZ5803=-(2*ZZZ3419/(-ZZZ2231-(ZZZ2231>0?1:-1)*sqrt(ZZZ7134)))+(SecondDegreeOvershootFactor-1);
- #if DebugCheckForEvents
- {char ZZZ732[200]; sprintf(ZZZ732,"%d ZZZ3419 %.10g ZZZ8687 %.10g ZZZ4213 %.10g ZZZ2231 %.10g ZZZ7652 %.10g ZZZ7134 %.10g",ZZZ715,ZZZ3419,ZZZ8687,ZZZ4213,ZZZ2231,ZZZ4006,ZZZ7134); DymosimMessage(ZZZ732);
- sprintf(ZZZ732,"C1: %g C2: %g beta=-%g alpha=-%g QZ=%.10g oldQZ=%.10g oldQZ2=%.10g",ZZZ7652,ZZZ5803,stepSizeRatio+oldstepSizeRatio,stepSizeRatio, QZ_[ZZZ715],oldQZ_[ZZZ715],oldQZ2_[ZZZ715]); DymosimMessage(ZZZ732); }
- #endif
- if (ZZZ5803>-0.5 && ZZZ5803 < 2.5) ZZZ7652=ZZZ5803; } }
- #endif
- if (ZZZ7652 > 0 && ZZZ7652<2 && (FindLastEvent ? ZZZ7652>c1: ZZZ7652<c1)) { /* */
- c1 = ZZZ7652; /* */ZZZ39=ZZZ715/2; } } } if (c1 != 1E30 && c1 != c1start && c1<1+CheckForEventsMinStep) { /* */ if (c1<CheckForEventsMinStep) c1=CheckForEventsMinStep; c2 = 2 - c1;
- T1end = Time + (1-CheckForEventsEps)*StepSize; T2end = Time + (2-CheckForEventsEps)*StepSize; /* */
- #if DebugCheckForEvents
- {char ZZZ732[200]; sprintf(ZZZ732,"Project at %.10g to %.10g Short %.10g Long %.10g",Time,T1end,c1*StepSize,c2*StepSize); DymosimMessage(ZZZ732);} {char ZZZ732[200];
- #if SecondDegree
- sprintf(ZZZ732,"Relation %d QZ=%.10g %.10g oldQZ=%.10g oldQZ2=%.10g oldQZ3=%.10g",ZZZ39,QZ_[2*ZZZ39],QZ_[2*ZZZ39+1],oldQZ_[2*ZZZ39],oldQZ2_[2*ZZZ39],oldQZ3_[2*ZZZ39]);
- #else
- sprintf(ZZZ732,"Relation %d QZ=%.10g %.10g oldQZ=%.10g",ZZZ39,QZ_[2*ZZZ39],QZ_[2*ZZZ39+1],oldQZ_[2*ZZZ39]);
- #endif
- DymosimMessage(ZZZ732); }
- #endif
- } else { c2=1; T2end=Time; T1end=Time; }
- } oldTime=oldDummyTime=Time; currentStepSize_ = StepSize;
- #ifdef InterpolateStatesForInline
- currentStepSizeRatio_ = 1;
- #endif
- currentStepSizeRatio2_ = 1; if (Time < T1end) {
- currentStepSizeRatio2_ = c1;
- #ifdef InterpolateStatesForInline
- currentStepSizeRatio_ = c1;
- #else
- currentStepSize_ = c1*StepSize;
- #endif
- for (ZZZ715 = 0; ZZZ715 < nx_; ZZZ715++) { F_[ZZZ715] = F_[ZZZ715]*c1; } /* */
- } else if (Time < T2end) { currentStepSizeRatio2_ = c2;
- #ifdef InterpolateStatesForInline
- currentStepSizeRatio_ = c2;
- #else
- currentStepSize_ = c2*StepSize;
- #endif
- for (ZZZ715 = 0; ZZZ715 < nx_; ZZZ715++) { F_[ZZZ715] = F_[ZZZ715]*c2; }
- /* */ oldTime=T1end; }}
- /* SCRAMBLE OFF */
- DYMOLA_STATIC Dymola_bool sampleFunction(struct DYNInstanceData*did_,double Time, double start, double interval, int counter,
- Dymola_bool Init, Dymola_bool Event) {
- struct BasicDDymosimStruct*basicD=getBasicDDymosimStruct();
- Dymola_bool samp = false;
- if (Init || (Event && NextSampleAct_[counter]==0)) {
- double x;
- basicD->mOrigTimeError=Dymola_max(basicD->mOrigTimeError,fabs(start)); /* Collect them */
- x=findCounter(Dymola_max(Time,start),start,interval);
- if (Init || x>NextSampleTime_[counter])
- NextSampleTime_[counter]=x;
- /* Samples at start,start+interval,...*/
- /* Replace Dymola_max(Time,start) by Time to sample at ...,start-interval,start,start+interval */
- };
- if (Event) {
- double eventTime=start+(NextSampleTime_[counter]-1)*interval;
- const double eventAccuracy=
- #ifndef DynSimStruct
- 5e-14
- #else
- 1e-7
- #endif
- *(fabs(Time)+basicD->mOrigTimeError);
- /* 5*eps to guard against different times */
- /*DymosimMessageDouble("Event at time: ",Time);*/
- /*DymosimMessageDouble("Event Time:",eventTime);*/
- while (eventTime<=Time+eventAccuracy) {
- NextSampleTime_[counter]+=1;
- eventTime=start+(NextSampleTime_[counter]-1)*interval;
- /*DymosimMessageDouble("Sampling at time: ", Time);*/
- /*DymosimMessageDouble("Next sampling",eventTime);*/
- samp = true;
- }
- NextSampleTimeNew_[counter]=NextSampleTime_[counter];
- NextSampleActNew_[counter]=1;
- registerTimeEventNew(eventTime, did_); /* The next event for this sampler */
- }
- return samp;
- }
- DYMOLA_STATIC Dymola_bool sampleFunctionM(struct DYNInstanceData*did_,double Time, double start, double interval, int counter,
- Dymola_bool Init, Dymola_bool Event) {
- struct BasicDDymosimStruct*basicD=getBasicDDymosimStruct();
- Dymola_bool samp = false;
- if (interval<=0) DymosimError("Sample did not have positive sample interval");
- if (Init|| (Event && NextSampleTime_[counter]==0)) {
- double x;
- if (Init==2) return false;
- basicD->mOrigTimeError=Dymola_max(basicD->mOrigTimeError,fabs(start)); /* Collect them */
- x=findCounter(Dymola_max(Time,start),start,interval);
- if (Init || x>NextSampleTime_[counter])
- NextSampleTime_[counter]=x;
- /* Samples at start,start+interval,...*/
- /* Replace Dymola_max(Time,start) by Time to sample at ...,start-interval,start,start+interval */
- };
- if (Event) {
- double eventTime=start+(NextSampleTime_[counter]-1)*interval;
- const double eventAccuracy=
- #ifndef DynSimStruct
- 5e-14
- #else
- 1e-7
- #endif
- *(fabs(Time)+basicD->mOrigTimeError);
- /* 5*eps to guard against different times */
- /*DymosimMessageDouble("Event at time: ",Time);*/
- /*DymosimMessageDouble("Event Time:",eventTime);*/
- NextSampleTimeNew_[counter]=NextSampleTime_[counter];
- while (eventTime<=Time+eventAccuracy) {
- NextSampleTimeNew_[counter]+=1;
- eventTime=start+(NextSampleTimeNew_[counter]-1)*interval;
- /*DymosimMessageDouble("Sampling at time: ", Time);*/
- /*DymosimMessageDouble("Next sampling",eventTime);*/
- samp = true;
- }
- NextSampleActNew_[counter]=1;
- registerTimeEventNew(eventTime, did_); /* The next event for this sampler */
- }
- return samp;
- }
- DYMOLA_STATIC Dymola_bool sampleFunctionM3(struct DYNInstanceData*did_,double Time, double start, double interval, int phase, int maxVal, int counter,
- Dymola_bool Init, Dymola_bool Event) {
- struct BasicDDymosimStruct*basicD=getBasicDDymosimStruct();
- double x,x2;
- Dymola_bool samp = false;
- if (interval<=0 || maxVal<=0) DymosimError("Sample did not have positive sample interval");
- if (Init|| (Event && NextSampleTime_[counter]==0)) {
- if (Init==2) return false;
- basicD->mOrigTimeError=Dymola_max(basicD->mOrigTimeError,fabs(start)); /* Collect them */
- x=findCounter(Dymola_max(Time,start),start,interval);
- if (Init || x>NextSampleTime_[counter])
- NextSampleTime_[counter]=x;
- /* Samples at start,start+interval,...*/
- /* Replace Dymola_max(Time,start) by Time to sample at ...,start-interval,start,start+interval */
- };
- if (Event) {
- double eventTime=start+(NextSampleTime_[counter]-1)*interval;
- const double eventAccuracy=
- #ifndef DynSimStruct
- 5e-14
- #else
- 1e-7
- #endif
- *(fabs(Time)+basicD->mOrigTimeError);
- /* 5*eps to guard against different times */
- /*DymosimMessageDouble("Event at time: ",Time);*/
- /*DymosimMessageDouble("Event Time:",eventTime);*/
- NextSampleTimeNew_[counter]=NextSampleTime_[counter];
- while (eventTime<=Time+eventAccuracy) {
- NextSampleTimeNew_[counter]+=1;
- eventTime=start+(NextSampleTimeNew_[counter]-1)*interval;
- /*DymosimMessageDouble("Sampling at time: ", Time);*/
- /*DymosimMessageDouble("Next sampling",eventTime);*/
- samp=true;
- }
- x=floor((0.1+NextSampleTimeNew_[counter]-2-(maxVal-1-phase))/maxVal); /* the passed point */
- x2=x*maxVal+(maxVal-1-phase);
- samp=samp && (x2==(NextSampleTimeNew_[counter]-2)) && x>=0; /* Handle negative phase */
- eventTime=start+(x2+maxVal)*interval;
- NextSampleActNew_[counter]=1;
- registerTimeEventNew(eventTime, did_); /* The next event for this sampler */
- }
- return samp;
- }
- #if defined(DYMOSIM) && defined(NI_)
- LIBDS_API void InitI2(int, int, double*,int*);
- static void InitI(struct DYNInstanceData* did_,int n,int d) {
- InitI2(n, d, QImd_, QIml_);
- }
- #else
- static void InitI(struct DYNInstanceData* did_,int n,int d) {
- ;
- }
- #endif
- DYMOLA_STATIC void ClearNextSample(struct DYNInstanceData* did_) {
- int i;
- for(i=0;i<NSamp_;++i) NextSampleActNew_[i]=0;
- }
- #if defined(DIRECT_FEED_THROUGH)
- DYMOLA_STATIC int DirectFeedThrough_=1;
- #else
- DYMOLA_STATIC int DirectFeedThrough_=0;
- #endif
- #if DymolaUseRDTSC_
- static double rtdrealFrequency=1.0e9;
- static double rtdinvFreq=1.0/1.0e9;
- struct MyLargeInteger {
- unsigned int LowPart;
- unsigned int HighPart;
- };
- static const double MInt=4294967296.0;
- static double DymolaPerformance(double*d,int i) {
- struct MyLargeInteger count={0,0};
- if (sizeof(struct MyLargeInteger)!=sizeof(*d))
- return -1;
- {
- #if defined(_MSC_VER) && defined(_M_IX86)
- __asm {
- rdtsc
- mov count.LowPart, eax
- mov count.HighPart, edx
- }
- #elif defined(__GNUC__)
- /* Gnu assembler: other names of registers, declare that rdtsc clobbers registers
- and different order of operands */
- __asm("rdtsc" : /* none */ : : "eax", "edx" );
- __asm("mov %%eax, %0" : "=g" (count.LowPart));
- __asm("mov %%edx, %0" : "=g" (count.HighPart));
- #elif defined(__LCC__)
- /* Lcc, default in Matlab, has it as an intrinsic function */
- extern long long _stdcall _rdtsc(void);
- {
- *(long long*)(&count)=_rdtsc();
- }
- #endif
- }
- if (i==0) {
- if (d) *(struct MyLargeInteger*)(d)=count;
- return count.LowPart+count.HighPart*MInt;
- } else {
- struct MyLargeInteger*a=(struct MyLargeInteger*)(d);
- return (
- (count.HighPart*1.0-a->HighPart)*MInt+count.LowPart-a->LowPart)*rtdinvFreq;
- }
- }
- struct RegisterReturn {
- int EAXV1,EBXV1,ECXV1,EDXV1;
- int EAXV2,EBXV2,ECXV2,EDXV2;
- int EAXV3,EBXV3,ECXV3,EDXV3;
- };
- static void InitializeFrequency(double d) {
- static int firstCall=1;
- if (!firstCall) return;
- firstCall=0;
- if (d==0) {
- unsigned int x=0x80000000UL;
- union {
- struct RegisterReturn registerReturn;
- char ch[48];
- } myValues;
- myValues.ch[0]='\0';
- #if defined(_MSC_VER) && defined(_M_IX86)
- _asm {
- mov eax, x
- cpuid
- mov x, eax
- }
- #elif defined(__GNUC__) && defined(i386)
- __asm("mov %0, %%eax": /*none */: "g" (x));
- __asm("cpuid");
- __asm("mov %%eax, %0" : "=g" (x));
- #endif
- if (x>=0x80000004UL) {
- x=0x80000002UL;
- #if defined(_MSC_VER) && defined(_M_IX86)
- _asm {
- mov eax, x
- cpuid
- mov myValues.registerReturn.EAXV1, eax
- mov myValues.registerReturn.EBXV1, ebx
- mov myValues.registerReturn.ECXV1, ecx
- mov myValues.registerReturn.EDXV1, edx
- }
- #elif defined(__GNUC__) && defined(i386)
- __asm("mov %0, %%eax": : "g" (x));
- __asm("cpuid" : : : "eax", "ebx", "ecx", "edx");
- __asm("mov %%eax, %0": "=g" (myValues.registerReturn.EAXV1));
- __asm("mov %%ebx, %0": "=g" (myValues.registerReturn.EBXV1));
- __asm("mov %%ecx, %0": "=g" (myValues.registerReturn.ECXV1));
- __asm("mov %%edx, %0": "=g" (myValues.registerReturn.EDXV1));
- #endif
- x=0x80000003UL;
- #if defined(_MSC_VER) && defined(_M_IX86)
- _asm {
- mov eax, x
- cpuid
- mov myValues.registerReturn.EAXV2, eax
- mov myValues.registerReturn.EBXV2, ebx
- mov myValues.registerReturn.ECXV2, ecx
- mov myValues.registerReturn.EDXV2, edx
- }
- #elif defined(__GNUC__) && defined(i386)
- __asm("mov %0, %%eax": : "g" (x));
- __asm("cpuid" : : : "eax", "ebx", "ecx", "edx");
- __asm("mov %%eax, %0": "=g" (myValues.registerReturn.EAXV2));
- __asm("mov %%ebx, %0": "=g" (myValues.registerReturn.EBXV2));
- __asm("mov %%ecx, %0": "=g" (myValues.registerReturn.ECXV2));
- __asm("mov %%edx, %0": "=g" (myValues.registerReturn.EDXV2));
- #endif
- x=0x80000004UL;
- #if defined(_MSC_VER) && defined(_M_IX86)
- _asm {
- mov eax, x
- cpuid
- mov myValues.registerReturn.EAXV3, eax
- mov myValues.registerReturn.EBXV3, ebx
- mov myValues.registerReturn.ECXV3, ecx
- mov myValues.registerReturn.EDXV3, edx
- }
- #elif defined(__GNUC__) && defined(i386)
- __asm("mov %0, %%eax": : "g" (x));
- __asm("cpuid" : : : "eax", "ebx", "ecx", "edx");
- __asm("mov %%eax, %0": "=g" (myValues.registerReturn.EAXV3));
- __asm("mov %%ebx, %0": "=g" (myValues.registerReturn.EBXV3));
- __asm("mov %%ecx, %0": "=g" (myValues.registerReturn.ECXV3));
- __asm("mov %%edx, %0": "=g" (myValues.registerReturn.EDXV3));
- #endif
- {
- double dMult=1e6;
- char*lastM=strrchr(myValues.ch,'M');
- if (lastM!=0 && lastM[1]=='H' && lastM[2]=='z') {
- } else {
- lastM=strrchr(myValues.ch,'G');
- if (lastM!=0 && lastM[1]=='H' && lastM[2]=='z') {
- dMult=1e9;
- } else lastM=0;
- }
- if (lastM!=0) {
- for(;lastM>myValues.ch && lastM[-1]==' ';lastM--);
- for(;lastM>myValues.ch && (lastM[-1]>='0' && lastM[-1]<='9' || lastM[-1]=='.');lastM--);
- if (sscanf(lastM,"%lg",&d)!=1) {
- d=0;
- } else d*=dMult;
- }
- }
-
- }
- if (d==0) {
- char str[200];
- sprintf(str,"Could not determine speed of processor. Assuming 1GHz\nCPU reported: %s\n",myValues.ch);
- DymosimMessage(str);
- d=1e9;
- } else {
- char str[200];
- sprintf(str,"Determined processor speed to %lg MHz\nCPU reported: %s\n",d/1e6,myValues.ch);
- DymosimMessage(str);
- }
- }
- rtdrealFrequency=d;
- rtdinvFreq=1.0/rtdrealFrequency;
- {
- extern double (*DymolaTimerCounterCallback)(double*,int);
- DymolaTimerCounterCallback=&DymolaPerformance;
- }
- }
- #if defined(DymolaUseRDTSCFrequency_)
- #define SetupProcessorCounter() InitializeFrequency(DymolaUseRDTSCFrequency_)
- #else
- #define SetupProcessorCounter() InitializeFrequency(0.0)
- #endif
- #else
- #define SetupProcessorCounter()
- #endif
- DYMOLA_STATIC void GetDimensions(int *nx_, int *nx2_, int *nu_, int *ny_, int *nw_, int *np_,
- int *nrel_, int *ncons_, int *dae_)
- {
- *nx_ = NX_;
- *nx2_ = NX2_;
- *nu_ = NU_;
- *ny_ = NY_;
- *nw_ = NW_;
- *np_ = NP_;
- *nrel_ = NRel_;
- *ncons_ = NCons_;
- *dae_ = DAEsolver_;
- #if defined(DynSimStruct)
- SetupProcessorCounter();
- #endif
- }
- DYMOLA_STATIC void GetDimensions2(int *nx_, int *nx2_, int *nu_, int *ny_, int *nw_, int *np_, int* nsp_,
- int*nrel2_,int *nrel_, int *ncons_, int *dae_)
- {
- *nx_ = NX_;
- *nx2_ = NX2_;
- *nu_ = NU_;
- *ny_ = NY_;
- *nw_ = NW_;
- #ifdef NPS_
- *nsp_ = NPS_;
- #else
- *nsp_ = 0;
- #endif
- *np_ = NP_;
- *nrel_ = NRel_;
- #ifdef NRelF_
- *nrel2_ = NRelF_;
- #else
- *nrel2_ = NRel_;
- #endif
- *ncons_ = NCons_;
- *dae_ = DAEsolver_;
- #if defined(DynSimStruct)
- SetupProcessorCounter();
- #endif
- }
-
- static int nx_=NX_;
- static int nx2_=NX2_;
- static int nu_=NU_;
- static int ny_=NY_;
- static int nw_=NW_;
- static int np_=NP_;
- #ifdef NPS_
- static int nsp_=NPS_;
- #else
- static int nsp_=0;
- #endif
- #ifdef NRelF_
- static int nrel2_=NRelF_;
- #else
- static int nrel2_=NRel_;
- #endif
- static int nrel_=NRel_;
- static int ncons_=NCons_;
- static int dae_=DAEsolver_;
- DYMOLA_STATIC void GetDimensions3(int *nrel_, int *ntim_, int *ncheckif_, int *nsamp_, int *nwhen_, int *nglobalhelp_,
- int *maxaux, int *qnlmax_, int *sizepre_, int *sizeeq_)
- {
- *nrel_ = NRel_;
- *ntim_ = NTim_;
- *ncheckif_ = NCheckIf_;
- *nsamp_ = NSamp_;
- *nwhen_ = NWhen_;
- *nglobalhelp_ = NGlobalHelp_;
- *maxaux = MAXAux;
- *qnlmax_ = QNLmax_;
- *sizepre_ = SizePre_;
- *sizeeq_ = SizeEq_;
- }
- DYMOLA_STATIC void UpdateDaeF_(Dymola_bool Init, double F_[], double XD_[])
- /* Calculate residue = f(x) - der(x)
- When intializing also: der(x) = f(x) */
- {
- int i_;
- double derx_;
-
- if (DAEsolver_)
- for (i_=0; i_<NX_; i_++) {
- derx_ = F_[i_];
- F_[i_] = derx_ - XD_[i_];
- if (Init)
- XD_[i_] = derx_;
- }
- }
- DYMOLA_STATIC void InitializeDymosimStruct(struct BasicDDymosimStruct*basicD,struct BasicIDymosimStruct*basicI) {
- #if INLINE_INTEGRATION
- basicI->mInlineIntegration=INLINE_INTEGRATION;
- #else
- basicI->mInlineIntegration=0;
- #endif
- #if defined(DymolaGeneratedFixedStepSize_)
- basicD->mDymolaFixedStepSize=DymolaGeneratedFixedStepSize_;
- #else
- basicD->mDymolaFixedStepSize=0.0;
- #endif
- basicD->mCurrentStepSizeRatio2 = 1.0;
- basicD->mOrigTimeError=0;
- }
- #if defined(RT) || defined(NRT)
- #else
- DYMOLA_STATIC int dsblockb_(const int *iopt_, int info_[], int sig_[], int dim_[],
- double *t0_, double x0_[], double xd0_[],
- double dp_[], int ip_[], Dymola_bool lp_[],
- double duser_[], int iuser_[], void*cuser_[],
- int *QiErr)
- {
- int c1_, c2_, c3_, i_, nx2_;
- double d1_;
- *(GlobalErrorPointer())=0;
- if (DAEsolver_)
- nx2_ = NX2_;
- else
- nx2_ = 0;
- if (*iopt_ == 1) {
- } else if (*iopt_ == 2) {
-
- SetupProcessorCounter();
- if (NCons_ > 0)
- info_[0] = 2;
- else if (DAEsolver_)
- info_[0] = 1;
- #if defined(HaveDummyDerivative_)
- info_[1] = 1;
- #endif
- #if defined(AnalyticJacobian_)
- info_[2] = AnalyticJacobianElements_;
- #endif
- /* if (NRel_ > 0 && NX_ + nx2_ == 0) */
- if (NX_ + nx2_ == 0)
- sig_[0] = 1; /* To enable handling of "state events" without states. */
- else
- sig_[0] = NX_ + nx2_;
- sig_[1] = NU_;
- sig_[2] = NY_;
- if (NRel_ > 0 && RootFinder_)
- sig_[3] = 1;
- sig_[4] = NW_;
- sig_[5] = NP_;
- sig_[6] = NA_; /* Number of alias signal matrices or scalars */
- sig_[7] = NA_; /* Total number of alias elements */
-
- /* if (NRel_ > 0 && NX_ + nx2_ == 0) */
- if (NX_ + nx2_ == 0)
- dim_[0] = 1;
- else
- dim_[0] = NX_ + nx2_;
- dim_[1] = NU_;
- dim_[2] = NY_;
- dim_[3] = 2 * NRel_;
- dim_[4] = NW_;
- dim_[5] = NP_;
- dim_[6] = NRelF_;
- dim_[7] = SizeEq_;
- dim_[9] = NHash1_;
- dim_[10] = NHash2_;
- dim_[11] = NX_;
- dim_[12] = nx2_;
- dim_[13] = NGlobalHelp_;
- dim_[14] = NHash3_;
- #ifdef NPS_
- dim_[15] = NPS_;
- #endif
- dim_[8] = dim_[0] + dim_[2] + dim_[3]*sig_[3] + dim_[4] + sizeof(struct BasicDDymosimStruct)/sizeof(doublereal);
-
- } else if (*iopt_ == 3) {
- iuser_[0] = NX_ + nx2_ + NCons_;
- iuser_[1] = NY_;
- iuser_[2] = NW_;
- iuser_[3] = 2 * NRel_;
- InitializeDymosimStruct((struct BasicDDymosimStruct*)(duser_+
- iuser_[0]+iuser_[1]+iuser_[2]+iuser_[3]),(struct BasicIDymosimStruct*)(iuser_+4));
-
- /* if (NRel_ > 0 && NX_ + nx2_ == 0) */
-
-
- declareNew_(x0_, dp_, 0, cuser_, QiErr, 0, (struct DeclarePhase*)0);
- }
- else if (*iopt_ == 4) {
- declareNew_(x0_,dp_,0,cuser_,QiErr, 1, (struct DeclarePhase*)0);
- } else if (*iopt_ == 5) {
- InitializeDymosimStruct((struct BasicDDymosimStruct*)(duser_),(struct BasicIDymosimStruct*)(iuser_));
- }
- leave:
- if (*(GlobalErrorPointer()) != 0)
- *QiErr = *(GlobalErrorPointer());
- return 0;
- }
- #endif
- #if defined(AnalyticJacobian_) && defined(AnalyticJacobianBCD_)
- DYMOLA_STATIC int QJacobianDefined_=1;
- #else
- DYMOLA_STATIC int QJacobianDefined_=0;
- #endif
- #if !defined(QJacobianCGDef_)
- DYMOLA_STATIC int QJacobianCG_[1]={0};
- DYMOLA_STATIC int QJacobianGC_[1]={0};
- DYMOLA_STATIC double QJacobianCD_[1]={0};
- #endif
- /* vector to hold FMI value references for possible continuous time states*/
- #if !defined(FMIStateValueReferencesDef_)
- DYMOLA_STATIC unsigned int FMIStateValueReferences_[1]={~0};
- #endif
- /* End dsblock5.c */
|