From 96b1a674905a6a7f003f415928d32367c3779f12 Mon Sep 17 00:00:00 2001 From: Torsten Sommer Date: Fri, 27 Sep 2024 13:33:01 +0200 Subject: [PATCH] Use FMIIsClose() to compare time values (#594) --- fmusim/FMI1CSSimulation.c | 8 ++++---- fmusim/FMI1MESimulation.c | 32 ++++++++++++++------------------ fmusim/FMI2CSSimulation.c | 7 +++---- fmusim/FMI2MESimulation.c | 29 ++++++++++++----------------- fmusim/FMI3CSSimulation.c | 18 +++++------------- fmusim/FMI3MESimulation.c | 27 ++++++++++++--------------- fmusim/FMIStaticInput.c | 14 ++++++++------ fmusim/FMIUtil.c | 16 ++++++++++++++++ fmusim/FMIUtil.h | 2 ++ 9 files changed, 76 insertions(+), 77 deletions(-) diff --git a/fmusim/FMI1CSSimulation.c b/fmusim/FMI1CSSimulation.c index 9c4a3f30..8a14b427 100644 --- a/fmusim/FMI1CSSimulation.c +++ b/fmusim/FMI1CSSimulation.c @@ -1,3 +1,4 @@ +#include "FMIUtil.h" #include "FMI1.h" #include "FMI1CSSimulation.h" @@ -5,7 +6,6 @@ #define CALL(f) do { status = f; if (status > FMIOK) goto TERMINATE; } while (0) - FMIStatus FMI1CSSimulate(const FMISimulationSettings* s) { FMIStatus status = FMIOK; @@ -42,12 +42,12 @@ FMIStatus FMI1CSSimulate(const FMISimulationSettings* s) { CALL(FMISample(S, time, s->recorder)); - CALL(FMIApplyInput(S, s->input, time, true, true, false)); - - if (time >= s->stopTime) { + if (time > s->stopTime || FMIIsClose(time, s->stopTime)) { break; } + CALL(FMIApplyInput(S, s->input, time, true, true, false)); + const FMIStatus doStepStatus = FMI1DoStep(S, time, s->outputInterval, fmi1True); if (doStepStatus == fmi1Discard) { diff --git a/fmusim/FMI1MESimulation.c b/fmusim/FMI1MESimulation.c index 89b4c474..4f35eb95 100644 --- a/fmusim/FMI1MESimulation.c +++ b/fmusim/FMI1MESimulation.c @@ -1,13 +1,12 @@ #include #include +#include "FMIUtil.h" #include "FMI1.h" #include "FMI1MESimulation.h" - #define CALL(f) do { status = f; if (status > FMIOK) goto TERMINATE; } while (0) - FMIStatus FMI1MESimulate(const FMISimulationSettings* s) { FMIStatus status = FMIOK; @@ -57,10 +56,6 @@ FMIStatus FMI1MESimulate(const FMISimulationSettings* s) { // initialize CALL(FMI1Initialize(S, s->tolerance > 0, s->tolerance, &eventInfo)); - if (!eventInfo.upcomingTimeEvent) { - eventInfo.nextEventTime = INFINITY; - } - const FMISolverParameters solverFunctions = { .modelInstance = S, .input = s->input, @@ -91,7 +86,7 @@ FMIStatus FMI1MESimulate(const FMISimulationSettings* s) { CALL(FMISample(S, time, s->recorder)); - if (time >= s->stopTime) { + if (time > s->stopTime || FMIIsClose(time, s->stopTime)) { break; } @@ -101,14 +96,19 @@ FMIStatus FMI1MESimulate(const FMISimulationSettings* s) { nextInputEventTime = FMINextInputEvent(s->input, time); - inputEvent = nextCommunicationPoint >= nextInputEventTime; - - timeEvent = nextCommunicationPoint >= eventInfo.nextEventTime; + if (nextCommunicationPoint > nextInputEventTime && !FMIIsClose(nextCommunicationPoint, nextInputEventTime)) { + nextCommunicationPoint = nextInputEventTime; + } - if (inputEvent || timeEvent) { - nextCommunicationPoint = fmin(nextInputEventTime, eventInfo.nextEventTime); + if (eventInfo.upcomingTimeEvent && nextCommunicationPoint > eventInfo.nextEventTime && !FMIIsClose(nextCommunicationPoint, eventInfo.nextEventTime)) { + nextCommunicationPoint = eventInfo.nextEventTime; } + inputEvent = FMIIsClose(nextCommunicationPoint, nextInputEventTime); + + timeEvent = eventInfo.upcomingTimeEvent && FMIIsClose(nextCommunicationPoint, eventInfo.nextEventTime); + + CALL(s->solverStep(solver, nextCommunicationPoint, &time, &stateEvent)); CALL(FMI1SetTime(S, time)); @@ -119,7 +119,7 @@ FMIStatus FMI1MESimulate(const FMISimulationSettings* s) { false // after event )); - if (time == nextRegularPoint) { + if (FMIIsClose(time, nextRegularPoint)) { nSteps++; } @@ -144,8 +144,8 @@ FMIStatus FMI1MESimulate(const FMISimulationSettings* s) { resetSolver = fmi1False; - // event iteration do { + CALL(FMI1EventUpdate(S, fmi1True, &eventInfo)); if (eventInfo.terminateSimulation) { @@ -157,10 +157,6 @@ FMIStatus FMI1MESimulate(const FMISimulationSettings* s) { } while (!eventInfo.iterationConverged); - if (!eventInfo.upcomingTimeEvent) { - eventInfo.nextEventTime = INFINITY; - } - if (resetSolver) { s->solverReset(solver, time); } diff --git a/fmusim/FMI2CSSimulation.c b/fmusim/FMI2CSSimulation.c index 3eafbce7..460abe17 100644 --- a/fmusim/FMI2CSSimulation.c +++ b/fmusim/FMI2CSSimulation.c @@ -2,7 +2,6 @@ #include "FMI2.h" #include "FMI2CSSimulation.h" - #define FMI_PATH_MAX 4096 #define CALL(f) do { status = f; if (status > FMIOK) goto TERMINATE; } while (0) @@ -53,12 +52,12 @@ FMIStatus FMI2CSSimulate(const FMISimulationSettings* s) { CALL(FMISample(S, time, s->recorder)); - CALL(FMIApplyInput(S, s->input, time, true, true, false)); - - if (time >= s->stopTime) { + if (time > s->stopTime || FMIIsClose(time, s->stopTime)) { break; } + CALL(FMIApplyInput(S, s->input, time, true, true, false)); + const FMIStatus doStepStatus = FMI2DoStep(S, time, s->outputInterval, fmi2True); if (doStepStatus == fmi2Discard) { diff --git a/fmusim/FMI2MESimulation.c b/fmusim/FMI2MESimulation.c index 86a5747c..89478e26 100644 --- a/fmusim/FMI2MESimulation.c +++ b/fmusim/FMI2MESimulation.c @@ -91,10 +91,6 @@ FMIStatus FMI2MESimulate(const FMISimulationSettings* s) { } while (eventInfo.newDiscreteStatesNeeded); - if (!eventInfo.nextEventTimeDefined) { - eventInfo.nextEventTime = INFINITY; - } - CALL(FMI2EnterContinuousTimeMode(S)); } @@ -130,7 +126,7 @@ FMIStatus FMI2MESimulate(const FMISimulationSettings* s) { CALL(FMISample(S, time, s->recorder)); - if (time >= s->stopTime) { + if (time > s->stopTime || FMIIsClose(time, s->stopTime)) { break; } @@ -140,14 +136,18 @@ FMIStatus FMI2MESimulate(const FMISimulationSettings* s) { nextInputEventTime = FMINextInputEvent(s->input, time); - inputEvent = nextCommunicationPoint >= nextInputEventTime; - - timeEvent = nextCommunicationPoint >= eventInfo.nextEventTime; + if (nextCommunicationPoint > nextInputEventTime && !FMIIsClose(nextCommunicationPoint, nextInputEventTime)) { + nextCommunicationPoint = nextInputEventTime; + } - if (inputEvent || timeEvent) { - nextCommunicationPoint = fmin(nextInputEventTime, eventInfo.nextEventTime); + if (eventInfo.nextEventTimeDefined && nextCommunicationPoint > eventInfo.nextEventTime && !FMIIsClose(nextCommunicationPoint, eventInfo.nextEventTime)) { + nextCommunicationPoint = eventInfo.nextEventTime; } + inputEvent = FMIIsClose(nextCommunicationPoint, nextInputEventTime); + + timeEvent = eventInfo.nextEventTimeDefined && FMIIsClose(nextCommunicationPoint, eventInfo.nextEventTime); + CALL(s->solverStep(solver, nextCommunicationPoint, &time, &stateEvent)); CALL(FMI2SetTime(S, time)); @@ -158,7 +158,7 @@ FMIStatus FMI2MESimulate(const FMISimulationSettings* s) { false // after event )); - if (time == nextRegularPoint) { + if (FMIIsClose(time, nextRegularPoint)) { nSteps++; } @@ -188,8 +188,8 @@ FMIStatus FMI2MESimulate(const FMISimulationSettings* s) { resetSolver = fmi2False; - // event iteration do { + CALL(FMI2NewDiscreteStates(S, &eventInfo)); if (eventInfo.terminateSimulation) { @@ -201,11 +201,6 @@ FMIStatus FMI2MESimulate(const FMISimulationSettings* s) { } while (eventInfo.newDiscreteStatesNeeded); - if (!eventInfo.nextEventTimeDefined) { - eventInfo.nextEventTime = INFINITY; - } - - // enter Continuous-Time Mode CALL(FMI2EnterContinuousTimeMode(S)); if (resetSolver) { diff --git a/fmusim/FMI3CSSimulation.c b/fmusim/FMI3CSSimulation.c index 969d19ef..c5bd9559 100644 --- a/fmusim/FMI3CSSimulation.c +++ b/fmusim/FMI3CSSimulation.c @@ -117,10 +117,6 @@ FMIStatus FMI3CSSimulate(const FMISimulationSettings* s) { } while (discreteStatesNeedUpdate); - if (!nextEventTimeDefined) { - nextEventTime = INFINITY; - } - CALL(FMI3EnterStepMode(S)); } } @@ -135,7 +131,7 @@ FMIStatus FMI3CSSimulate(const FMISimulationSettings* s) { for (;;) { - if (time >= s->stopTime) { + if (time > s->stopTime || FMIIsClose(time, s->stopTime)) { break; } @@ -145,12 +141,12 @@ FMIStatus FMI3CSSimulate(const FMISimulationSettings* s) { nextInputEventTime = FMINextInputEvent(s->input, time); - inputEvent = nextCommunicationPoint >= nextInputEventTime; - - if (inputEvent) { + if (nextCommunicationPoint > nextInputEventTime && !FMIIsClose(nextCommunicationPoint, nextInputEventTime)) { nextCommunicationPoint = nextInputEventTime; } + inputEvent = FMIIsClose(nextCommunicationPoint, nextInputEventTime); + stepSize = nextCommunicationPoint - time; CALL(FMIApplyInput(S, s->input, time, @@ -181,7 +177,7 @@ FMIStatus FMI3CSSimulate(const FMISimulationSettings* s) { time = nextCommunicationPoint; } - if (time == nextRegularPoint) { + if (FMIIsClose(time, nextRegularPoint)) { nSteps++; } @@ -220,10 +216,6 @@ FMIStatus FMI3CSSimulate(const FMISimulationSettings* s) { } while (discreteStatesNeedUpdate); - if (!nextEventTimeDefined) { - nextEventTime = INFINITY; - } - CALL(FMI3EnterStepMode(S)); CALL(FMISample(S, time, s->recorder)); diff --git a/fmusim/FMI3MESimulation.c b/fmusim/FMI3MESimulation.c index 293cb6ce..340b266d 100644 --- a/fmusim/FMI3MESimulation.c +++ b/fmusim/FMI3MESimulation.c @@ -95,10 +95,6 @@ FMIStatus FMI3MESimulate(const FMISimulationSettings* s) { } while (discreteStatesNeedUpdate); - if (!nextEventTimeDefined) { - nextEventTime = INFINITY; - } - CALL(FMI3EnterContinuousTimeMode(S)); } @@ -138,7 +134,7 @@ FMIStatus FMI3MESimulate(const FMISimulationSettings* s) { CALL(FMISample(S, time, s->recorder)); - if (time >= s->stopTime) { + if (time > s->stopTime || FMIIsClose(time, s->stopTime)) { break; } @@ -148,14 +144,18 @@ FMIStatus FMI3MESimulate(const FMISimulationSettings* s) { nextInputEventTime = FMINextInputEvent(s->input, time); - inputEvent = nextCommunicationPoint >= nextInputEventTime; - - timeEvent = nextCommunicationPoint >= nextEventTime; + if (nextCommunicationPoint > nextInputEventTime && !FMIIsClose(nextCommunicationPoint, nextInputEventTime)) { + nextCommunicationPoint = nextInputEventTime; + } - if (inputEvent || timeEvent) { - nextCommunicationPoint = fmin(nextInputEventTime, nextEventTime); + if (nextEventTimeDefined && nextCommunicationPoint > nextEventTime && !FMIIsClose(nextCommunicationPoint, nextEventTime)) { + nextCommunicationPoint = nextEventTime; } + inputEvent = FMIIsClose(nextCommunicationPoint, nextInputEventTime); + + timeEvent = nextEventTimeDefined && FMIIsClose(nextCommunicationPoint, nextEventTime); + CALL(s->solverStep(solver, nextCommunicationPoint, &time, &stateEvent)); CALL(FMI3SetTime(S, time)); @@ -166,7 +166,7 @@ FMIStatus FMI3MESimulate(const FMISimulationSettings* s) { false // after event )); - if (time == nextRegularPoint) { + if (FMIIsClose(time, nextRegularPoint)) { nSteps++; } @@ -181,6 +181,7 @@ FMIStatus FMI3MESimulate(const FMISimulationSettings* s) { if (inputEvent || timeEvent || stateEvent || stepEvent) { + // record the values before the event CALL(FMISample(S, time, s->recorder)); CALL(FMI3EnterEventMode(S)); @@ -214,10 +215,6 @@ FMIStatus FMI3MESimulate(const FMISimulationSettings* s) { } while (discreteStatesNeedUpdate); - if (!nextEventTimeDefined) { - nextEventTime = INFINITY; - } - CALL(FMI3EnterContinuousTimeMode(S)); if (resetSolver) { diff --git a/fmusim/FMIStaticInput.c b/fmusim/FMIStaticInput.c index feed8f38..f1cd250c 100644 --- a/fmusim/FMIStaticInput.c +++ b/fmusim/FMIStaticInput.c @@ -137,7 +137,7 @@ double FMINextInputEvent(const FMIStaticInput* input, double time) { const double t0 = input->time[i]; const double t1 = input->time[i + 1]; - if (time >= t1) { + if (time > t1 || FMIIsClose(time, t1)) { continue; } @@ -266,22 +266,24 @@ FMIStatus FMIApplyInput(FMIInstance* instance, const FMIStaticInput* input, doub size_t row = 0; - for (size_t i = 1; i < input->nRows; i++) { + for (size_t j = 1; j < input->nRows; j++) { - const double t = input->time[i]; + const double t = input->time[j]; - if (t >= time) { + if (t > time || FMIIsClose(t, time)) { break; } - row = i; + row = j; } if (afterEvent) { while (row < input->nRows - 1) { - if (input->time[row + 1] > time) { + const double t = input->time[row + 1]; + + if (t > time && !FMIIsClose(t, time)) { break; } diff --git a/fmusim/FMIUtil.c b/fmusim/FMIUtil.c index 7e6cd107..6d43f7be 100644 --- a/fmusim/FMIUtil.c +++ b/fmusim/FMIUtil.c @@ -1,4 +1,5 @@ #include +#include #include "FMI1.h" #include "FMI2.h" @@ -732,3 +733,18 @@ FMIStatus FMIDuplicateBuffer(const void* source, void** destination, size_t size return FMIOK; } + +#define EPSILON (1.0e-5) + +bool FMIIsClose(double a, double b) { + + if (!isfinite(a) || !isfinite(b)) { + return false; + } + + if (fabs(a - b) <= EPSILON) { + return true; + } + + return fabs(a - b) <= EPSILON * fmax(fabs(a), fabs(b)); +} diff --git a/fmusim/FMIUtil.h b/fmusim/FMIUtil.h index 21855963..77715a43 100644 --- a/fmusim/FMIUtil.h +++ b/fmusim/FMIUtil.h @@ -48,3 +48,5 @@ FMIStatus FMISaveFMUStateToFile(FMIInstance* S, const char* filename); FMIStatus FMIDuplicateString(const char* source, char** destination); FMIStatus FMIDuplicateBuffer(const void* source, void** destination, size_t size); + +bool FMIIsClose(double a, double b);