fix repulsive forces

libsimulator/src/GeneralizedCentrifugalForceModel.cpp

@@ -143,10 +143,12 @@ void GeneralizedCentrifugalForceModel::CheckModelConstraint(
         if(contanctDist >= distance) {
             throw SimulationError(
                 "Model constraint violation: Agent {} too close to agent {}: distance {}, "
+                "contactDist {}, "
                 "effective distance {}",
                 agent.pos,
                 neighbor.pos,
                 distance,
+                contanctDist,
                 distance - contanctDist);
         }
     }
@@ -216,19 +218,19 @@ Point GeneralizedCentrifugalForceModel::ForceRepPed(
     //       5   |     4       |            3             |      2       | 1
 
     // If the pedestrian is outside the cutoff distance, the force is zero.
-    if(dist_eff >= maxNeighborRepulsionForce) {
+    if(dist_eff >= maxNeighborInteractionDistance) {
         F_rep = Point(0.0, 0.0);
         return F_rep;
     }
 
     const double mindist =
         0.5; // for performance reasons, it is assumed that this distance is about 50 cm
     const double dist_intpol_left =
-        mindist + maxNeighborInteractionDistance; // lower cut-off for Frep (modCFM)
+        mindist + maxNeighborInterpolationDistance; // lower cut-off for Frep (modCFM)
     const double dist_intpol_right =
-        maxNeighborRepulsionForce -
-        maxNeighborInteractionDistance; // upper cut-off for Frep (modCFM)
-    const double smax = mindist - maxNeighborInteractionDistance; // max overlapping
+     maxNeighborInteractionDistance -
+        maxNeighborInterpolationDistance; // upper cut-off for Frep (modCFM)
+    const double smax = mindist - maxNeighborInterpolationDistance; // max overlapping
     double f = 0.0f; // fuction value
     double f1 = 0.0f; // derivative of function value
 
@@ -269,18 +271,17 @@ Point GeneralizedCentrifugalForceModel::ForceRepPed(
     K_ij = sqrt(K_ij);
     if(dist_eff <= smax) { // 5
         f = -agent1_mass * K_ij * nom / dist_intpol_left;
-        F_rep = ep12 * maxNeighborInterpolationDistance * f;
+        F_rep = ep12 * maxNeighborRepulsionForce * f;
         return F_rep;
     }
 
     //          smax    dist_intpol_left           dist_intpol_right       dist_eff_max
     //           ----|-------------|--------------------------|--------------|----
     //           5   |     4       |            3             |      2       | 1
-
     if(dist_eff >= dist_intpol_right) { // 2
         f = -agent1_mass * K_ij * nom / dist_intpol_right; // abs(NR-Dv(i)+Sa)
         f1 = -f / dist_intpol_right;
-        px = hermite_interp(dist_eff, dist_intpol_right, maxNeighborRepulsionForce, f, 0, f1, 0);
+        px = hermite_interp(dist_eff, dist_intpol_right, maxNeighborInteractionDistance, f, 0, f1, 0);
         F_rep = ep12 * px;
     } else if(dist_eff >= dist_intpol_left) { // 3
         f = -agent1_mass * K_ij * nom / fabs(dist_eff); // abs(NR-Dv(i)+Sa)
@@ -289,7 +290,7 @@ Point GeneralizedCentrifugalForceModel::ForceRepPed(
         f = -agent1_mass * K_ij * nom / dist_intpol_left;
         f1 = -f / dist_intpol_left;
         px = hermite_interp(
-            dist_eff, smax, dist_intpol_left, maxNeighborInterpolationDistance * f, f, 0, f1);
+            dist_eff, smax, dist_intpol_left, maxNeighborRepulsionForce * f, f, 0, f1);
         F_rep = ep12 * px;
     }
     if(F_rep.x != F_rep.x || F_rep.y != F_rep.y) {
@@ -417,9 +418,9 @@ Point GeneralizedCentrifugalForceModel::ForceInterpolation(
     double nominator = strengthGeometryRepulsion * v0 + vn;
     nominator *= nominator * K_ij;
     double f = 0, f1 = 0; // function value and its derivative at the interpolation point
-    double smax = l - maxGeometryInteractionDistance; // max overlapping radius
-    double dist_intpol_left = l + maxGeometryInteractionDistance; // r_eps
-    double dist_intpol_right = maxGeometryRepulsionForce - maxGeometryInteractionDistance;
+    double smax = l - maxGeometryInterpolationDistance; // max overlapping radius
+    double dist_intpol_left = l + maxGeometryInterpolationDistance; // r_eps
+    double dist_intpol_right = maxGeometryInteractionDistance - maxGeometryInterpolationDistance;
 
     double dist_eff = d - r;
 
@@ -428,7 +429,7 @@ Point GeneralizedCentrifugalForceModel::ForceInterpolation(
     //       5   |     4       |            3             |      2       | 1
 
     double px = 0; // value of the interpolated function
-    double tmp1 = maxGeometryRepulsionForce;
+    double tmp1 = maxGeometryInteractionDistance;
     double tmp2 = dist_intpol_right;
     double tmp3 = dist_intpol_left;
     double tmp5 = smax + r;
@@ -439,14 +440,14 @@ Point GeneralizedCentrifugalForceModel::ForceInterpolation(
     }
 
     if(dist_eff <= tmp5) { // 5
-        F_rep = e * (-maxGeometryInterpolationDistance);
+        F_rep = e * (-maxGeometryRepulsionForce);
         return F_rep;
     }
 
     if(dist_eff > tmp2) { // 2
         f = -nominator / dist_intpol_right;
         f1 = -f / dist_intpol_right; // nominator / (dist_intpol_right^2) = derivativ of f
-        px = hermite_interp(dist_eff, dist_intpol_right, maxGeometryRepulsionForce, f, 0, f1, 0);
+        px = hermite_interp(dist_eff, dist_intpol_right, maxGeometryInteractionDistance, f, 0, f1, 0);
         F_rep = e * px;
     } else if(dist_eff >= tmp3) { // 3
         f = -nominator / fabs(dist_eff); // devided by abs f the effective distance
@@ -455,7 +456,7 @@ Point GeneralizedCentrifugalForceModel::ForceInterpolation(
         f = -nominator / dist_intpol_left;
         f1 = -f / dist_intpol_left;
         px = hermite_interp(
-            dist_eff, smax, dist_intpol_left, maxGeometryInterpolationDistance * f, f, 0, f1);
+            dist_eff, smax, dist_intpol_left, maxGeometryRepulsionForce * f, f, 0, f1);
         F_rep = e * px;
     }
     return F_rep;