From 183a850c7383ea0ac12c92e8913c0972a8ef3e51 Mon Sep 17 00:00:00 2001 From: Torsten Sommer Date: Mon, 7 Oct 2024 10:48:12 +0200 Subject: [PATCH] Use isClose() to check next communication point fixes #507 --- include/cosimulation.h | 2 -- include/model.h | 1 + src/cosimulation.c | 15 +++++++++++++++ src/fmi1Functions.c | 13 +++++++------ src/fmi2Functions.c | 12 ++++++------ src/fmi3Functions.c | 21 +++++++++------------ 6 files changed, 38 insertions(+), 26 deletions(-) diff --git a/include/cosimulation.h b/include/cosimulation.h index 82187ec9..fda1ebf9 100644 --- a/include/cosimulation.h +++ b/include/cosimulation.h @@ -2,6 +2,4 @@ #include "model.h" -#define EPSILON (FIXED_SOLVER_STEP * 1e-6) - Status doFixedStep(ModelInstance *comp, bool* stateEvent, bool* timeEvent); diff --git a/include/model.h b/include/model.h index 21d5bec3..8dabb0ca 100644 --- a/include/model.h +++ b/include/model.h @@ -221,6 +221,7 @@ Status getPartialDerivative(ModelInstance *comp, ValueReference unknown, ValueRe Status getEventIndicators(ModelInstance *comp, double z[], size_t nz); Status eventUpdate(ModelInstance *comp); +bool isClose(double a, double b); bool invalidNumber(ModelInstance *comp, const char *f, const char *arg, size_t actual, size_t expected); bool invalidState(ModelInstance *comp, const char *f, int statesExpected); bool nullPointer(ModelInstance* comp, const char *f, const char *arg, const void *p); diff --git a/src/cosimulation.c b/src/cosimulation.c index 9e417201..66703227 100644 --- a/src/cosimulation.c +++ b/src/cosimulation.c @@ -190,6 +190,21 @@ Status reset(ModelInstance* comp) { return OK; } +#define EPSILON (1.0e-5) + +bool isClose(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)); +} + bool invalidNumber(ModelInstance *comp, const char *f, const char *arg, size_t actual, size_t expected) { if (actual != expected) { diff --git a/src/fmi1Functions.c b/src/fmi1Functions.c index eb09a90b..00ffba94 100644 --- a/src/fmi1Functions.c +++ b/src/fmi1Functions.c @@ -304,31 +304,32 @@ fmiStatus fmiDoStep(fmiComponent c, fmiReal currentCommunicationPoint, fmiReal c ModelInstance* instance = (ModelInstance *)c; - if (fabs(currentCommunicationPoint - instance->nextCommunicationPoint) > EPSILON) { + if (!isClose(currentCommunicationPoint, instance->nextCommunicationPoint)) { logError(instance, "Expected currentCommunicationPoint = %.16g but was %.16g.", instance->nextCommunicationPoint, currentCommunicationPoint); instance->state = modelError; return fmiError; } - if (currentCommunicationPoint + communicationStepSize > instance->stopTime + EPSILON) { + const fmiReal nextCommunicationPoint = currentCommunicationPoint + communicationStepSize; + + if (nextCommunicationPoint > instance->stopTime && !isClose(nextCommunicationPoint, instance->stopTime)) { logError(instance, "At communication point %.16g a step size of %.16g was requested but stop time is %.16g.", currentCommunicationPoint, communicationStepSize, instance->stopTime); instance->state = modelError; return fmiError; } - const fmiReal nextCommunicationPoint = currentCommunicationPoint + communicationStepSize + EPSILON; - while (true) { - if (instance->time + FIXED_SOLVER_STEP > nextCommunicationPoint) { + if (instance->time > nextCommunicationPoint || isClose(instance->time, nextCommunicationPoint)) { break; // next communcation point reached } bool stateEvent, timeEvent; doFixedStep(instance, &stateEvent, &timeEvent); + #ifdef EVENT_UPDATE if (stateEvent || timeEvent) { eventUpdate(instance); @@ -336,7 +337,7 @@ fmiStatus fmiDoStep(fmiComponent c, fmiReal currentCommunicationPoint, fmiReal c #endif } - instance->nextCommunicationPoint = currentCommunicationPoint + communicationStepSize; + instance->nextCommunicationPoint = nextCommunicationPoint; return fmiOK; } diff --git a/src/fmi2Functions.c b/src/fmi2Functions.c index 234a1c03..7fc0a3c5 100644 --- a/src/fmi2Functions.c +++ b/src/fmi2Functions.c @@ -625,7 +625,7 @@ fmi2Status fmi2DoStep(fmi2Component c, fmi2Real currentCommunicationPoint, BEGIN_FUNCTION(DoStep); - if (fabs(currentCommunicationPoint - S->nextCommunicationPoint) > EPSILON) { + if (!isClose(currentCommunicationPoint, S->nextCommunicationPoint)) { logError(S, "Expected currentCommunicationPoint = %.16g but was %.16g.", S->nextCommunicationPoint, currentCommunicationPoint); S->state = Terminated; @@ -637,17 +637,17 @@ fmi2Status fmi2DoStep(fmi2Component c, fmi2Real currentCommunicationPoint, CALL(Error); } - if (currentCommunicationPoint + communicationStepSize > S->stopTime + EPSILON) { + const fmi2Real nextCommunicationPoint = currentCommunicationPoint + communicationStepSize; + + if (nextCommunicationPoint > S->stopTime && !isClose(nextCommunicationPoint, S->stopTime)) { logError(S, "At communication point %.16g a step size of %.16g was requested but stop time is %.16g.", currentCommunicationPoint, communicationStepSize, S->stopTime); CALL(Error); } - const fmi2Real nextCommunicationPoint = currentCommunicationPoint + communicationStepSize + EPSILON; - while (true) { - if (S->time + FIXED_SOLVER_STEP > nextCommunicationPoint) { + if (S->time > nextCommunicationPoint || isClose(S->time, nextCommunicationPoint)) { break; // next communcation point reached } @@ -667,7 +667,7 @@ fmi2Status fmi2DoStep(fmi2Component c, fmi2Real currentCommunicationPoint, } } - S->nextCommunicationPoint = currentCommunicationPoint + communicationStepSize; + S->nextCommunicationPoint = nextCommunicationPoint; END_FUNCTION(); } diff --git a/src/fmi3Functions.c b/src/fmi3Functions.c index 96eefe2a..bfb4bca3 100644 --- a/src/fmi3Functions.c +++ b/src/fmi3Functions.c @@ -1352,28 +1352,25 @@ fmi3Status fmi3DoStep(fmi3Instance instance, BEGIN_FUNCTION(DoStep); - if (fabs(currentCommunicationPoint - S->nextCommunicationPoint) > EPSILON) { + if (!isClose(currentCommunicationPoint, S->nextCommunicationPoint)) { logError(S, "Expected currentCommunicationPoint = %.16g but was %.16g.", S->nextCommunicationPoint, currentCommunicationPoint); - S->state = Terminated; - return fmi3Error; + CALL(Error); } if (communicationStepSize <= 0) { - logError(S, "Communication step size must be > 0 but was %g.", communicationStepSize); - S->state = Terminated; - return fmi3Error; + logError(S, "Communication step size must be > 0 but was %.16g.", communicationStepSize); + CALL(Error); } - if (currentCommunicationPoint + communicationStepSize > S->stopTime + EPSILON) { + const fmi3Float64 nextCommunicationPoint = currentCommunicationPoint + communicationStepSize; + + if (nextCommunicationPoint > S->stopTime && !isClose(nextCommunicationPoint, S->stopTime)) { logError(S, "At communication point %.16g a step size of %.16g was requested but stop time is %.16g.", currentCommunicationPoint, communicationStepSize, S->stopTime); - S->state = Terminated; - return fmi3Error; + CALL(Error); } - const fmi3Float64 nextCommunicationPoint = currentCommunicationPoint + communicationStepSize + EPSILON; - bool nextCommunicationPointReached; *eventHandlingNeeded = fmi3False; @@ -1384,7 +1381,7 @@ fmi3Status fmi3DoStep(fmi3Instance instance, const fmi3Float64 nextSolverStepTime = S->time + FIXED_SOLVER_STEP; - nextCommunicationPointReached = nextSolverStepTime > nextCommunicationPoint; + nextCommunicationPointReached = nextSolverStepTime > nextCommunicationPoint || isClose(nextSolverStepTime, nextCommunicationPoint); if (nextCommunicationPointReached || (*eventHandlingNeeded && S->earlyReturnAllowed)) { break;