Use isClose() to check next communication point (#602)

fixes #507

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);

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);

src/cosimulation.c

@@ -1,6 +1,5 @@
 #include <stdlib.h>  // for calloc(), free()
 #include <float.h>   // for DBL_EPSILON
-#include <math.h>    // for fabs()
 #include <stdio.h>
 #include <stdarg.h>
 #include <string.h>
@@ -190,6 +189,25 @@ Status reset(ModelInstance* comp) {
     return OK;
 }
 
+#define EPSILON (1.0e-5)
+
+static double fmiAbs(double v) {
+    return v >= 0 ? v : -v;
+}
+
+static double fmiMax(double a, double b) {
+    return (a < b) ? b : a;
+}
+
+bool isClose(double a, double b) {
+
+    if (fmiAbs(a - b) <= EPSILON) {
+        return true;
+    }
+
+    return fmiAbs(a - b) <= EPSILON * fmiMax(fmiAbs(a), fmiAbs(b));
+}
+
 bool invalidNumber(ModelInstance *comp, const char *f, const char *arg, size_t actual, size_t expected) {
 
     if (actual != expected) {

src/fmi1Functions.c

@@ -304,39 +304,42 @@ 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) {
+        const fmiReal nextSolverStepTime = instance->time + FIXED_SOLVER_STEP;
+
+        if (nextSolverStepTime > nextCommunicationPoint && !isClose(nextSolverStepTime, 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;
 }

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,19 @@ 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) {
+        const double nextSolverStepTime = S->time + FIXED_SOLVER_STEP;
+
+        if (nextSolverStepTime > nextCommunicationPoint && !isClose(nextSolverStepTime, nextCommunicationPoint)) {
             break;  // next communcation point reached
         }
 
@@ -667,7 +669,7 @@ fmi2Status fmi2DoStep(fmi2Component c, fmi2Real currentCommunicationPoint,
         }
     }
 
-    S->nextCommunicationPoint = currentCommunicationPoint + communicationStepSize;
+    S->nextCommunicationPoint = nextCommunicationPoint;
 
     END_FUNCTION();
 }

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;