From e3ec6ecd38343329cc1bb742938c2432e314f779 Mon Sep 17 00:00:00 2001 From: Mohcine Chraibi Date: Mon, 22 Apr 2024 20:23:47 +0200 Subject: [PATCH] fix repulsive forces --- .../src/GeneralizedCentrifugalForceModel.cpp | 33 ++++++++++--------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/libsimulator/src/GeneralizedCentrifugalForceModel.cpp b/libsimulator/src/GeneralizedCentrifugalForceModel.cpp index 723d508f7..f5256fb8a 100644 --- a/libsimulator/src/GeneralizedCentrifugalForceModel.cpp +++ b/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,7 +218,7 @@ 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; } @@ -224,11 +226,11 @@ Point GeneralizedCentrifugalForceModel::ForceRepPed( 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;