Use isClose() to check next communication point

fixes #507
modelica · Oct 7, 2024 · 183a850 · 183a850
1 parent 6b2a24b
commit 183a850
Show file tree

Hide file tree

Showing 6 changed files with 38 additions and 26 deletions.
diff --git 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
@@ -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
@@ -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
@@ -304,39 +304,40 @@ 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);
         }
 #endif
     }
 
-    instance->nextCommunicationPoint = currentCommunicationPoint + communicationStepSize;
+    instance->nextCommunicationPoint = nextCommunicationPoint;
 
     return fmiOK;
 }

diff --git 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
@@ -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;