diff --git a/CMakeLists.txt b/CMakeLists.txt
index 5b44a8de88987fb9de25dbafa1eb936634613b0a..f58b3197965f59f8745ef8b7c0c9a976ed4e2432 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -320,7 +320,7 @@ math/VelocityModel.cpp
math/OperationalModel.cpp
math/GCVMModel.cpp
math/SimplestModel.cpp
-
+math/AGCVMModel.cpp
mpi/LCGrid.cpp
pedestrian/Ellipse.cpp
@@ -550,7 +550,7 @@ math/VelocityModel.h
math/OperationalModel.h
math/GCVMModel.h
math/SimplestModel.h
-
+math/AGCVMModel.h
events/EventManager.h
events/Event.h
diff --git a/IO/IniFileParser.cpp b/IO/IniFileParser.cpp
index 42e47576647441ac13c78ef99c501b3d6bf74616..e716bdc37977066e3f7a1655f177771487da9fb0 100644
--- a/IO/IniFileParser.cpp
+++ b/IO/IniFileParser.cpp
@@ -41,6 +41,7 @@
#include "../math/VelocityModel.h"
#include "../math/GCVMModel.h"
#include "../math/SimplestModel.h"
+#include "../math/AGCVMModel.h"
#include "../routing/global_shortest/GlobalRouter.h"
#include "../routing/quickest/QuickestPathRouter.h"
#include "../routing/smoke_router/SmokeRouter.h"
@@ -557,10 +558,21 @@ bool IniFileParser::Parse(std::string iniFile)
//only parsing one model
break;
}
+ if ((_model == MODEL_AGCVM) && (model_id == MODEL_AGCVM)) {
+ if (modelName != "agcvm") {
+ Log->Write("ERROR: \t mismatch model ID and description. Did you mean agcvm?");
+ return false;
+ }
+ if (!ParseGCVMModel(xModel, xMainNode))
+ return false;
+ parsingModelSuccessful = true;
+ //only parsing one model
+ break;
+ }
}
if (!parsingModelSuccessful) {
- Log->Write("ERROR: \tWrong model id [%d]. Choose 1 (GCFM), 2 (Gompertz), 3 (Tordeux2015), 5 (Krausz), 6 (GCVM) or 7 (Simplest)", _model);
+ Log->Write("ERROR: \tWrong model id [%d]. Choose 1 (GCFM), 2 (Gompertz), 3 (Tordeux2015), 5 (Krausz), 6 (GCVM), 7 (Simplest), 8 (AGCVM)", _model);
Log->Write("ERROR: \tPlease make sure that all models are specified in the operational_models section");
Log->Write("ERROR: \tand make sure to use the same ID in the agent section");
return false;
@@ -1290,6 +1302,12 @@ void IniFileParser::ParseAgentParameters(TiXmlElement* operativModel, TiXmlNode*
_config->SetDistEffMaxPed(max_Eb + _config->GetTs()*agentParameters->GetV0());
_config->SetDistEffMaxWall(_config->GetDistEffMaxPed());
}
+
+ if (_model == 8) { // AGCVM
+ double max_Eb = 2 * agentParameters->GetBmax();
+ _config->SetDistEffMaxPed(max_Eb + _config->GetTs()*agentParameters->GetV0());
+ _config->SetDistEffMaxWall(_config->GetDistEffMaxPed());
+ }
}
}
}
@@ -2000,7 +2018,7 @@ bool IniFileParser::ParseGCVMModel(TiXmlElement* xGCVM, TiXmlElement* xMainNode)
if (xModelPara->FirstChild("GCVM")) {
if (!xModelPara->FirstChildElement("GCVM")->Attribute("using"))
- _config->SetGCVMUsing(0);
+ _config->SetGCVMUsing(1);
else {
string GCVMUsing = xModelPara->FirstChildElement("GCVM")->Attribute("using");
_config->SetGCVMUsing(atoi(GCVMUsing.c_str()));
@@ -2177,13 +2195,145 @@ bool IniFileParser::ParseSimplestModel(TiXmlElement* xSimplest, TiXmlElement* xM
Log->Write("INFO: \twaiting_time Tw=%0.2f",_config->GetWaitingTime());
}
+ if (xModelPara->FirstChild("clogging_area")) {
+
+ if (!xModelPara->FirstChildElement("clogging_area")->Attribute("size"))
+ _config->SetAreaSize(1); // default value
+ else {
+ string AreaSize = xModelPara->FirstChildElement("clogging_area")->Attribute("size");
+ _config->SetAreaSize(atof(AreaSize.c_str()));
+ }
+ Log->Write("INFO: \tclogging_area size=%0.2f", _config->GetAreaSize());
+ }
+
//Parsing the agent parameters
TiXmlNode* xAgentDistri = xMainNode->FirstChild("agents")->FirstChild("agents_distribution");
ParseAgentParameters(xSimplest, xAgentDistri);
_config->SetModel(std::shared_ptr<OperationalModel>(new SimplestModel(_exit_strategy, _config->GetaPed(),
_config->GetDPed(), _config->GetaWall(),
- _config->GetDWall(), _config->GetTs(), _config->GetTd(), _config->GetUpdate(), _config->GetWaitingTime(), _config->GetSubmodelDirection(), _config->GetSubmodelSpeed(), _config->GetGCVMUsing())));
+ _config->GetDWall(), _config->GetTs(), _config->GetTd(), _config->GetUpdate(), _config->GetWaitingTime(), _config->GetAreaSize(), _config->GetSubmodelDirection(), _config->GetSubmodelSpeed(), _config->GetGCVMUsing())));
+
+ return true;
+}
+
+bool IniFileParser::ParseAGCVMModel(TiXmlElement* xGCVM, TiXmlElement* xMainNode)
+{
+ //parsing the model parameters
+ Log->Write("\nINFO:\tUsing AGCVM model");
+ Log->Write("INFO:\tParsing the model parameters");
+
+ TiXmlNode* xModelPara = xGCVM->FirstChild("model_parameters");
+
+ if (!xModelPara) {
+ Log->Write("ERROR: \t !!!! Changes in the operational model section !!!");
+ Log->Write("ERROR: \t !!!! The new version is in inputfiles/ship_msw/ini_ship3.xml !!!");
+ return false;
+ }
+
+ // For convenience. This moved to the header as it is not model specific
+ if (xModelPara->FirstChild("tmax")) {
+ Log->Write("ERROR: \tthe maximal simulation time section moved to the header!!!");
+ Log->Write("ERROR: \t\t <max_sim_time> </max_sim_time>\n");
+ return false;
+ }
+
+ //solver
+ if (!ParseNodeToSolver(*xModelPara))
+ return false;
+
+ //stepsize
+ if (!ParseStepSize(*xModelPara))
+ return false;
+
+ //exit crossing strategy
+ if (!ParseStrategyNodeToObject(*xModelPara))
+ return false;
+
+ //linked-cells
+ if (!ParseLinkedCells(*xModelPara))
+ return false;
+
+ //periodic
+ if (!ParsePeriodic(*xModelPara))
+ return false;
+
+ //force_ped
+ if (xModelPara->FirstChild("force_ped")) {
+
+ if (!xModelPara->FirstChildElement("force_ped")->Attribute("a"))
+ _config->SetaPed(3.0); // default value
+ else {
+ string a = xModelPara->FirstChildElement("force_ped")->Attribute("a");
+ _config->SetaPed(atof(a.c_str()));
+ }
+
+ if (!xModelPara->FirstChildElement("force_ped")->Attribute("D"))
+ _config->SetDPed(0.1); // default value in [m]
+ else {
+ string D = xModelPara->FirstChildElement("force_ped")->Attribute("D");
+ _config->SetDPed(atof(D.c_str()));
+ }
+ Log->Write("INFO: \tfrep_ped a=%0.2f, D=%0.2f", _config->GetaPed(), _config->GetDPed());
+
+ }
+ //force_wall
+ if (xModelPara->FirstChild("force_wall")) {
+
+ if (!xModelPara->FirstChildElement("force_wall")->Attribute("a"))
+ _config->SetaWall(6.0); // default value
+ else {
+ string a = xModelPara->FirstChildElement("force_wall")->Attribute("a");
+ _config->SetaWall(atof(a.c_str()));
+ }
+
+ if (!xModelPara->FirstChildElement("force_wall")->Attribute("D"))
+ _config->SetDWall(0.05); // default value in [m]
+ else {
+ string D = xModelPara->FirstChildElement("force_wall")->Attribute("D");
+ _config->SetDWall(atof(D.c_str()));
+ }
+ Log->Write("INFO: \tfrep_wall a=%0.2f, D=%0.2f", _config->GetaWall(), _config->GetDWall());
+ }
+
+ //time parameters
+ if (xModelPara->FirstChild("time_parameters")) {
+
+ if (!xModelPara->FirstChildElement("time_parameters")->Attribute("Ts"))
+ _config->SetTs(0.5); // default value
+ else {
+ string Ts = xModelPara->FirstChildElement("time_parameters")->Attribute("Ts");
+ _config->SetTs(atof(Ts.c_str()));
+ }
+
+ if (!xModelPara->FirstChildElement("time_parameters")->Attribute("Td"))
+ _config->SetTd(0.3); // default value in [m]
+ else {
+ string Td = xModelPara->FirstChildElement("time_parameters")->Attribute("Td");
+ _config->SetTd(atof(Td.c_str()));
+ }
+ Log->Write("INFO: \ttime_parameters Ts=%0.2f, Td=%0.2f", _config->GetTs(), _config->GetTd());
+
+ if (xModelPara->FirstChild("GCVM")) {
+ if (!xModelPara->FirstChildElement("GCVM")->Attribute("using"))
+ _config->SetGCVMUsing(1);
+ else {
+ string GCVMUsing = xModelPara->FirstChildElement("GCVM")->Attribute("using");
+ _config->SetGCVMUsing(atoi(GCVMUsing.c_str()));
+ }
+ _config->GetGCVMUsing() == 1 ?
+ Log->Write("INFO:\tUsing GCVM model") :
+ Log->Write("INFO:\tUsing CVM model");
+ }
+
+
+ }
+ //Parsing the agent parameters
+ TiXmlNode* xAgentDistri = xMainNode->FirstChild("agents")->FirstChild("agents_distribution");
+ ParseAgentParameters(xGCVM, xAgentDistri);
+ _config->SetModel(std::shared_ptr<OperationalModel>(new GCVMModel(_exit_strategy, _config->GetaPed(),
+ _config->GetDPed(), _config->GetaWall(),
+ _config->GetDWall(), _config->GetTs(), _config->GetTd(), _config->GetGCVMUsing())));
return true;
}
\ No newline at end of file
diff --git a/IO/IniFileParser.h b/IO/IniFileParser.h
index 98216579f462d0ca9e18f9c471ec2693b5bb6068..0a81d9c007d28076f880839fd6fa0a16696be532 100644
--- a/IO/IniFileParser.h
+++ b/IO/IniFileParser.h
@@ -65,6 +65,8 @@ private:
bool ParseGCVMModel(TiXmlElement* xGCVM, TiXmlElement* xMain);
bool ParseSimplestModel(TiXmlElement* xSimplest, TiXmlElement* xMain);
+
+ bool ParseAGCVMModel(TiXmlElement* xGCVM, TiXmlElement* xMain);
#ifdef AIROUTER
bool ParseAIOpts(TiXmlNode* routingNode);
#endif
diff --git a/Simulation.cpp b/Simulation.cpp
index bd9e48bd0d21dc9cfefc9e787da5d0a35a05bba1..7e60e48ab33a431de31ec8e914107f467aa4d099 100644
--- a/Simulation.cpp
+++ b/Simulation.cpp
@@ -36,6 +36,7 @@
#include "math/GradientModel.h"
#include "math/GCVMModel.h"
#include "math/SimplestModel.h"
+#include "math/AGCVMModel.h"
#include "pedestrian/AgentsQueue.h"
#include "pedestrian/AgentsSourcesManager.h"
#include "geometry/WaitingArea.h"
diff --git a/general/Configuration.h b/general/Configuration.h
index d11bc5d89f1753436a4006796e76737214cb0fa2..e3965ec1ce747e417888b3b1078a3c7ca3e507d3 100644
--- a/general/Configuration.h
+++ b/general/Configuration.h
@@ -104,6 +104,7 @@ public:
_SubmodelDirection = 0;
_SubmodelSpeed = 0;
_GCVMUsing = 0;
+ _AreaSize = 1;
// ----------------
_hostname = "localhost";
@@ -381,7 +382,11 @@ public:
void SetGCVMUsing(int gu) { _GCVMUsing=gu; };
- int GetGCVMUsing() const { return _GCVMUsing; }
+ int GetGCVMUsing() const { return _GCVMUsing; };
+
+ void SetAreaSize(double as) { _AreaSize = as; };
+
+ double GetAreaSize() const { return _AreaSize; };
#ifdef _JPS_AS_A_SERVICE
@@ -456,6 +461,7 @@ private:
int _SubmodelDirection;
int _SubmodelSpeed;
int _GCVMUsing;
+ double _AreaSize;
//ff router quickest
double _recalc_interval;
diff --git a/general/Macros.h b/general/Macros.h
index b080cda5c401135634153782a13c40a224914de5..0e57067e15da5916d439688b99b6e25ffad536ef 100644
--- a/general/Macros.h
+++ b/general/Macros.h
@@ -144,7 +144,8 @@ enum OperativModels {
MODEL_GRADIENT,
MODEL_KRAUSZ,
MODEL_GCVM,
- MODEL_SIMPLEST
+ MODEL_SIMPLEST,
+ MODEL_AGCVM
// MODEL_ORCA,
// MODEL_CFM,
// MODEL_GNM
diff --git a/math/AGCVMModel.cpp b/math/AGCVMModel.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1622874d8e0a3f01b585b93d3955a973c014f114
--- /dev/null
+++ b/math/AGCVMModel.cpp
@@ -0,0 +1,902 @@
+/**
+* \file AGCVM.cpp
+* \date Jun 26, 2019
+* \version v0.8
+* \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
+*
+* \section License
+* This file is part of JuPedSim.
+*
+* JuPedSim is free software: you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* any later version.
+*
+* JuPedSim is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with JuPedSim. If not, see <http://www.gnu.org/licenses/>.
+*
+* \section Description
+* Implementation of first-order model
+* Anticipation generalized collision-free velocity model: Qiancheng (8)
+*
+*
+**/
+
+# define NOMINMAX
+#include "../pedestrian/Pedestrian.h"
+//#include "../routing/DirectionStrategy.h"
+#include "../mpi/LCGrid.h"
+#include "../geometry/Wall.h"
+#include "../geometry/SubRoom.h"
+
+
+#include "AGCVMModel.h"
+#ifdef _OPENMP
+#include <omp.h>
+#else
+#define omp_get_thread_num() 0
+#define omp_get_max_threads() 1
+#endif
+
+double xRight_agcvm = 26.0;
+double xLeft_agcvm = 0.0;
+double cutoff_agcvm = 2.0;
+
+//-------------------------------
+
+using std::vector;
+using std::string;
+
+AGCVMModel::AGCVMModel(std::shared_ptr<DirectionStrategy> dir, double aped, double Dped,
+ double awall, double Dwall, double Ts, double Td, int GCVM)
+{
+ _direction = dir;
+ // Force_rep_PED Parameter
+ _aPed = aped;
+ _DPed = Dped;
+ // Force_rep_WALL Parameter
+ _aWall = awall;
+ _DWall = Dwall;
+ // GCVM Parameter
+ _Ts = Ts; // Speed module
+ _Td = Td; // Direction module
+ _GCVMUsing = GCVM; // If using GCVM
+}
+
+
+AGCVMModel::~AGCVMModel()
+{
+
+}
+
+bool AGCVMModel::Init(Building* building)
+{
+
+ if (auto dirff = dynamic_cast<DirectionFloorfield*>(_direction.get())) {
+ Log->Write("INFO:\t Init DirectionFloorfield starting ...");
+ double _deltaH = building->GetConfig()->get_deltaH();
+ double _wallAvoidDistance = building->GetConfig()->get_wall_avoid_distance();
+ bool _useWallAvoidance = building->GetConfig()->get_use_wall_avoidance();
+ dirff->Init(building, _deltaH, _wallAvoidDistance, _useWallAvoidance);
+ Log->Write("INFO:\t Init DirectionFloorfield done");
+ }
+
+ if (auto dirlocff = dynamic_cast<DirectionLocalFloorfield*>(_direction.get())) {
+ Log->Write("INFO:\t Init DirectionLOCALFloorfield starting ...");
+ double _deltaH = building->GetConfig()->get_deltaH();
+ double _wallAvoidDistance = building->GetConfig()->get_wall_avoid_distance();
+ bool _useWallAvoidance = building->GetConfig()->get_use_wall_avoidance();
+ dirlocff->Init(building, _deltaH, _wallAvoidDistance, _useWallAvoidance);
+ Log->Write("INFO:\t Init DirectionLOCALFloorfield done");
+ }
+
+ if (auto dirsublocff = dynamic_cast<DirectionSubLocalFloorfield*>(_direction.get())) {
+ Log->Write("INFO:\t Init DirectionSubLOCALFloorfield starting ...");
+ double _deltaH = building->GetConfig()->get_deltaH();
+ double _wallAvoidDistance = building->GetConfig()->get_wall_avoid_distance();
+ bool _useWallAvoidance = building->GetConfig()->get_use_wall_avoidance();
+ dirsublocff->Init(building, _deltaH, _wallAvoidDistance, _useWallAvoidance);
+ Log->Write("INFO:\t Init DirectionSubLOCALFloorfield done");
+ }
+
+ const vector< Pedestrian* >& allPeds = building->GetAllPedestrians();
+ size_t peds_size = allPeds.size();
+ for (unsigned int p = 0; p < peds_size; p++)
+ {
+ Pedestrian* ped = allPeds[p];
+ double cosPhi, sinPhi;
+ //a destination could not be found for that pedestrian
+ if (ped->FindRoute() == -1) {
+ Log->Write(
+ "ERROR:\tGCVMModel::Init() cannot initialise route. ped %d is deleted in Room %d %d.\n", ped->GetID(), ped->GetRoomID(), ped->GetSubRoomID());
+ building->DeletePedestrian(ped);
+ p--;
+ peds_size--;
+ continue;
+ }
+
+
+
+
+ Point target = ped->GetExitLine()->ShortestPoint(ped->GetPos());
+ Point d = target - ped->GetPos();
+ double dist = d.Norm();
+ if (dist != 0.0) {
+ cosPhi = d._x / dist;
+ sinPhi = d._y / dist;
+ }
+ else {
+ Log->Write(
+ "ERROR: \tallPeds::Init() cannot initialise phi! "
+ "dist to target is 0\n");
+ return false;
+ }
+
+ ped->InitV0(target);
+
+ JEllipse E = ped->GetEllipse();
+ E.SetCosPhi(cosPhi);
+ E.SetSinPhi(sinPhi);
+ ped->SetEllipse(E);
+ }
+ return true;
+}
+
+void AGCVMModel::ComputeNextTimeStep(double current, double deltaT, Building* building, int periodic)
+{
+ // collect all pedestrians in the simulation.
+ const vector< Pedestrian* >& allPeds = building->GetAllPedestrians();
+ vector<Pedestrian*> pedsToRemove;
+ pedsToRemove.reserve(500);
+ unsigned long nSize;
+ nSize = allPeds.size();
+ int nThreads = omp_get_max_threads();
+ nThreads = 1; //debug only, I have to use other pedestrian's information
+ int partSize;
+ partSize = ((int)nSize > nThreads) ? (int)(nSize / nThreads) : (int)nSize;
+ if (partSize == (int)nSize)
+ nThreads = 1; // not worthy to parallelize
+
+#pragma omp parallel default(shared) num_threads(nThreads)
+ {
+ vector< Point > result_acc = vector<Point >();
+ result_acc.reserve(nSize);
+ vector< Point > result_dir = vector<Point >();
+ result_dir.reserve(nSize);
+ vector< my_pair > spacings = vector<my_pair >();
+ spacings.reserve(nSize); // larger than needed
+ const int threadID = omp_get_thread_num();
+
+ int start = threadID * partSize;
+ int end;
+ end = (threadID < nThreads - 1) ? (threadID + 1) * partSize - 1 : (int)(nSize - 1);
+ for (int p = start; p <= end; ++p) {
+ Pedestrian* ped = allPeds[p];
+ Room* room = building->GetRoom(ped->GetRoomID());
+ SubRoom* subroom = room->GetSubRoom(ped->GetSubRoomID());
+ Point repPed = Point(0, 0);
+ vector<Pedestrian*> neighbours;
+ building->GetGrid()->GetNeighbourhood(ped, neighbours);
+ Point inid_direction = e0(ped, room);
+
+ //using a new method calculate the influence of pedestrian (the value of influence id decided by distance and the direction is vertical with desired direction)
+ Point inf_direction;// left side of pedestrian
+ inf_direction._x = -inid_direction._y;
+ inf_direction._y = inid_direction._x;
+ inf_direction = inf_direction.Normalized();
+ //--------------------------------------------------------------------------------------------------------------------------------------------------------------
+
+ int size = (int)neighbours.size();
+ for (int i = 0; i < size; i++)
+ {
+ Pedestrian* ped1 = neighbours[i];
+ //if they are in the same subroom
+ Point p1 = ped->GetPos();
+ Point p2 = ped1->GetPos();
+ Point ep12 = p2 - p1;
+
+ //Deciding the influence direction--------------------------------------------------------------------
+ double result1 = inid_direction.CrossProduct(ep12);
+ Point zero = Point(0, 0);
+ if (bool equal = almostEqual(result1, 0, 0.00001))//if neighbour is in front or behind pedestrian
+ {
+ int random = rand() % 1000;//choose one direciton bu random
+ if (random < 500)//when random is larger than 50, influence's direction is right, otherwise is left
+ {
+ inf_direction = zero - inf_direction;
+ }
+ }
+ else
+ {
+ double result2 = inid_direction.CrossProduct(inf_direction);
+ if (result1*result2 > 0)//is ep12 and inf_direction in the same side of inid_direction, change the direction of inf_direction
+ {
+ inf_direction = zero - inf_direction;
+ }
+ }
+
+ //subrooms to consider when looking for neighbour for the 3d visibility
+ vector<SubRoom*> emptyVector;
+ emptyVector.push_back(subroom);
+ emptyVector.push_back(building->GetRoom(ped1->GetRoomID())->GetSubRoom(ped1->GetSubRoomID()));
+ bool isVisible = building->IsVisible(p1, p2, emptyVector, false);
+ if (!isVisible)
+ {
+ continue;
+ }
+ if (ped->GetUniqueRoomID() == ped1->GetUniqueRoomID()) {
+ if (GetGCVMU() == 1)
+ {
+ repPed += inf_direction * ForceRepPed(ped, ped1, inid_direction, periodic).Norm();//new method
+ }
+ else
+ {
+ repPed += ForceRepPed(ped, ped1, inid_direction, periodic);//original method
+ }
+ }
+ else {
+ // or in neighbour subrooms
+ SubRoom* sb2 = building->GetRoom(ped1->GetRoomID())->GetSubRoom(ped1->GetSubRoomID());
+ if (subroom->IsDirectlyConnectedWith(sb2)) {
+ if (GetGCVMU() == 1)
+ {
+ repPed += inf_direction * ForceRepPed(ped, ped1, inid_direction, periodic).Norm();//new method
+ }
+ else
+ {
+ repPed += ForceRepPed(ped, ped1, inid_direction, periodic);//original method
+ }
+ }
+ }
+ } //for i
+
+ // Calculating influence of walls-------------------------------------------------------------------------
+ Point repWall = ForceRepRoom(allPeds[p], subroom, inid_direction);
+
+ //Caluculating desired direcition----------------------------------------------------------------------------------------------
+ Point d_direction;
+ d_direction = inid_direction + repPed + repWall;//new method
+
+ //Calculating the actual direction of pedestrian at next timestep------------------------------------------------------------------
+ Point a_direction;
+ a_direction._x = ped->GetEllipse().GetCosPhi();
+ a_direction._y = ped->GetEllipse().GetSinPhi();
+ double angle_tau = _Td;
+
+ //case 1 (vector)
+ Point angle_v = (d_direction.Normalized()-a_direction)/ angle_tau;
+ Point direction = a_direction + angle_v * deltaT;
+
+ //When the angle between desired mocing direction and actual direction is bigger than 90 degree. turning to the desired moving direction directly.
+ if (d_direction.ScalarProduct(a_direction) < 0) {
+ direction = d_direction;
+ }
+
+ if (GetGCVMU() == 0) {
+ direction = d_direction;//original method
+ }
+ result_dir.push_back(direction);
+ //------------------------------------------------------------------------------------------------------------------------------------
+
+
+ //Calculating spacing in front -------------------------------------------------------------------------------------------------------
+ for (int i = 0; i < size; i++) {
+ Pedestrian* ped1 = neighbours[i];
+ // calculate spacing
+ if (ped->GetUniqueRoomID() == ped1->GetUniqueRoomID()) {
+ spacings.push_back(GetSpacing(ped, ped1, direction, periodic));
+ }
+ else {
+ // or in neighbour subrooms
+ SubRoom* sb2 = building->GetRoom(ped1->GetRoomID())->GetSubRoom(ped1->GetSubRoomID());
+ if (subroom->IsDirectlyConnectedWith(sb2)) {
+ spacings.push_back(GetSpacing(ped, ped1, direction, periodic));
+ }
+ }
+ }//for i
+ //------------------------------------------------------------------------------------------------------------------------------------
+
+ // Calculate min spacing, and save all spacing=0 to vector relation--------------------------------------------------------------------
+ std::sort(spacings.begin(), spacings.end(), sort_pred_agcvm());
+ double spacing = spacings.size() == 0 ? 100 : spacings[0].first;
+ double first_ID = spacings.size() == 0 ? -1 : spacings[0].second;
+ double sec_spacing = spacings.size() <= 1 ? 100 : spacings[1].first;
+
+ // add this part to avoid pedestrian cross the wall directly
+ spacing = spacing > GetSpacingRoom(ped, subroom, direction) ? GetSpacingRoom(ped, subroom, direction) : spacing;
+ Point speed;
+ speed = direction.NormalizedMolified() *OptimalSpeed(ped, spacing);
+ result_acc.push_back(speed);
+ spacings.clear(); //clear for ped p
+ //-----------------------------------------------------------------------------------------------------------------------------------------
+ } // for p
+
+
+#pragma omp barrier
+ // update
+ for (int p = start; p <= end; ++p) {
+ Pedestrian* ped = allPeds[p];
+
+ Point v_neu = result_acc[p - start];
+ Point dir_neu = result_dir[p - start];
+ Point pos_neu = ped->GetPos() + v_neu * deltaT;
+ //Jam is based on the current velocity
+ if (v_neu.Norm() >= ped->GetV0Norm()*0.5) {
+ ped->ResetTimeInJam();
+ }
+ else {
+ ped->UpdateTimeInJam();
+ }
+ //calculate ellipse orientation
+ double normdir = dir_neu.Norm();
+ double cosPhi = dir_neu._x / normdir;
+ double sinPhi = dir_neu._y / normdir;
+ JEllipse e = ped->GetEllipse();
+ e.SetCosPhi(cosPhi);
+ e.SetSinPhi(sinPhi);
+ ped->SetEllipse(e);
+
+ ped->SetV(v_neu);
+ if (periodic) {
+ if ((ped->GetPos()._x < xRight_agcvm)&&(pos_neu._x>=xRight_agcvm)) {
+ ped->SetPos(Point(pos_neu._x - (xRight_agcvm - xLeft_agcvm), pos_neu._y));
+ }
+ else if ((ped->GetPos()._x > xLeft_agcvm) && (pos_neu._x <= xLeft_agcvm)) {
+ ped->SetPos(Point(pos_neu._x + (xRight_agcvm - xLeft_agcvm), pos_neu._y));
+ }
+ else {
+ ped->SetPos(pos_neu);
+ }
+ }
+ else {
+ ped->SetPos(pos_neu);
+ }
+ }
+ }//end parallel
+
+ // remove the pedestrians that have left the building
+ for (unsigned int p = 0; p<pedsToRemove.size(); p++) {
+ building->DeletePedestrian(pedsToRemove[p]);
+ }
+ pedsToRemove.clear();
+}
+
+Point AGCVMModel::e0(Pedestrian* ped, Room* room) const
+{
+ const Point target = _direction->GetTarget(room, ped); // target is where the ped wants to be after the next timestep
+ Point desired_direction;
+ const Point pos = ped->GetPos();
+ double dist = ped->GetExitLine()->DistTo(pos);
+ // check if the molified version works
+ Point lastE0 = ped->GetLastE0();
+ ped->SetLastE0(target - pos);
+
+ if ((dynamic_cast<DirectionFloorfield*>(_direction.get())) ||
+ (dynamic_cast<DirectionLocalFloorfield*>(_direction.get())) ||
+ (dynamic_cast<DirectionSubLocalFloorfield*>(_direction.get()))) {
+ if (dist > 50 * J_EPS_GOAL) {
+ desired_direction = target - pos; //ped->GetV0(target);
+ }
+ else {
+ desired_direction = lastE0;
+ ped->SetLastE0(lastE0); //keep old vector (revert set operation done 9 lines above)
+ }
+ }
+ else if (dist > J_EPS_GOAL) {
+ desired_direction = ped->GetV0(target);
+ }
+ else {
+ ped->SetSmoothTurning();
+ desired_direction = ped->GetV0();
+ }
+ //test result
+ //fprintf(stderr,"\n %d %f %f %f %f",ped->GetID(),ped->GetPos()._x,ped->GetPos()._y,target._x,target._y);
+ //test result
+ return desired_direction;
+}
+
+
+double AGCVMModel::OptimalSpeed(Pedestrian* ped, double spacing) const
+{
+ double v0 = ped->GetV0Norm();
+ double T = _Ts;
+ double speed = (spacing) / T;
+ speed = (speed>0) ? speed : 0;
+ speed = (speed<v0) ? speed : v0;
+ return speed;
+}
+
+// return spacing and id of the nearest pedestrian
+my_pair AGCVMModel::GetSpacing(Pedestrian* ped1, Pedestrian* ped2, Point ei, int periodic) const
+{
+ double x1 = ped1->GetPos()._x;
+ double x2_real = ped2->GetPos()._x;
+ double y2 = ped2->GetPos()._y;
+ Point ped2_current = ped2->GetPos();
+
+ if (periodic) {
+ if ((xRight_agcvm - x1) + (x2_real - xLeft_agcvm) <= cutoff_agcvm) {
+ double x2_periodic = x2_real + xRight_agcvm - xLeft_agcvm;
+ ped2->SetPos(Point(x2_periodic, y2));
+ }
+ if ((x1 - xLeft_agcvm) + (xRight_agcvm - x2_real) <= cutoff_agcvm) {
+ double x2_periodic = xLeft_agcvm - xRight_agcvm + x2_real;
+ ped2->SetPos(Point(x2_periodic, y2));
+ }
+ }
+
+ Point distp12 = ped2->GetPos() - ped1->GetPos(); // inversed sign
+ double Distance = distp12.Norm();
+ Point ep12;
+ if (Distance >= J_EPS) {
+ ep12 = distp12.Normalized();
+ }
+ else {
+ //printf("ERROR: \tin VelocityModel::forcePedPed() ep12 can not be calculated!!!\n");
+ Log->Write("WARNING: \tin VelocityModel::GetSPacing() ep12 can not be calculated!!!\n");
+ Log->Write("\t\t Pedestrians are too near to each other (%f).", Distance);
+ exit(EXIT_FAILURE);
+ }
+ //calculate effective distance
+ JEllipse eped1 = ped1->GetEllipse();
+ JEllipse eped2 = ped2->GetEllipse();
+ double dist;
+ double eff_dist = eped1.EffectiveDistanceToEllipse(eped2, &dist);
+
+ double condition1 = ei.ScalarProduct(ep12); // < e_i , e_ij > should be positive
+ if (condition1 < 0) {
+ ped2->SetPos(ped2_current);
+ return my_pair(FLT_MAX, -1);
+ }
+ //When condition1==0,should no influence on velocity
+ bool parallel = almostEqual(condition1, 0.0, J_EPS);
+ if (parallel)
+ {
+ ped2->SetPos(ped2_current);
+ return my_pair(FLT_MAX, -1);
+ }
+ //Judge conllision
+ //Obtain parameters
+ double a1 = ped1->GetLargerAxis();
+ Point v = ped1->GetV();
+ double b1 = ped1->GetSmallerAxis();
+
+ //Avoid block B(drill,small tricks)-----------------------------------------------------
+ /*
+ if (fabs(v._x) < J_EPS && fabs(v._y) < J_EPS) // v==0
+ {
+ const Point& pos = ped1->GetPos();
+ double distGoal = ped1->GetExitLine()->DistToSquare(pos);
+ if (distGoal < 0.2)
+ {
+ b1 = 0.1;
+ b1 = ped1->GetEllipse().GetBmin();
+ }
+ }
+ */
+ //-----------------------------------------------------------------------------------
+
+ double a2 = ped2->GetLargerAxis();
+ double b2 = ped2->GetSmallerAxis();
+ double x2 = ped2->GetPos()._x;
+ double y1 = ped1->GetPos()._y;
+ double cosphi1 = ei.Normalized()._x;
+ double sinphi1 = ei.Normalized()._y;
+ double cosphi2 = ped2->GetEllipse().GetCosPhi();
+ double sinphi2 = ped2->GetEllipse().GetSinPhi();
+ //Judge the position of the center of ped2
+ double d1 = -sinphi1 * (x2 - x1) + cosphi1 * (y2 - y1) + b1;
+ double d2 = -sinphi1 * (x2 - x1) + cosphi1 * (y2 - y1) - b1;
+ if (d1*d2 <= 0) {
+ //if the center between two lines, collision
+ ped2->SetPos(ped2_current);
+ return my_pair(eff_dist, ped2->GetID());
+ }
+ //If the center not between two lines, Judge if ped2 contact with two lines
+ Point D;
+ D._x = cosphi1;
+ D._y = sinphi1;
+ Point De;
+ De._x = cosphi1 * cosphi2 + sinphi1 * sinphi2;
+ De._y = sinphi1 * cosphi2 - sinphi2 * cosphi1;
+ Point Ne;
+ Ne._x = -De._y;
+ Ne._y = De._x;
+ Point A1;
+ A1._x = x1 + b1 * sinphi1;
+ A1._y = y1 - b1 * cosphi1;
+ Point A2;
+ A2._x = x1 - b1 * sinphi1;
+ A2._y = y1 + b1 * cosphi1;
+ //Transfer A1 and A2 to ped2 coordinate
+ //Point A1e = ((A1._x - x2)*cosphi2 + (A1._y - y2)*sinphi2, (A1._y - y2)*cosphi2 - (A1._x - x2)*sinphi2);
+ Point A1e = A1.TransformToEllipseCoordinates(ped2->GetPos(), cosphi2, sinphi2);
+ //Point A2e = ((A2._x - x2)*cosphi2 + (A2._y - y2)*sinphi2, (A2._y - y2)*cosphi2 - (A2._x - x2)*sinphi2);
+ Point A2e = A2.TransformToEllipseCoordinates(ped2->GetPos(), cosphi2, sinphi2);
+ // Judge if the direction of De is right (ellipse2 coordinate)
+ double J1 = Ne.ScalarProduct(A1e);
+ double J2 = Ne.ScalarProduct(A2e);
+ Point De1 = (J1 >= 0) ? De * (-1, -1) : De;
+ Point De2 = (J2 >= 0) ? De * (-1, -1) : De;
+ //Calculate point R (ellipse2 coordinate)
+ Point Ne1;
+ Ne1._x = -De1._y;
+ Ne1._y = De1._x;
+ Point Ne2;
+ Ne2._x = -De2._y;
+ Ne2._y = De2._x;
+ Point Te1;
+ Te1._x = De1._y / b2;
+ Te1._y = -De1._x / a2;
+ Point Te2;
+ Te2._x = De2._y / b2;
+ Te2._y = -De2._x / a2;
+ Point Te1n = Te1.Normalized();
+ Point Te2n = Te2.Normalized();
+ Point Re1;
+ Re1._x = a2 * Te1n._x;
+ Re1._y = b2 * Te1n._y;
+ Point Re2;
+ Re2._x = a2 * Te2n._x;
+ Re2._y = b2 * Te2n._y;
+ //Transfer R to global coordinate
+ /*
+ Point R1 = (Re1._x*cosphi2 - Re1._y*sinphi2 + x2, Re1._y*cosphi2 + Re1._x*sinphi2 + y2);
+ Point R1 = Re1.TransformToCartesianCoordinates(ped2->GetPos(), cosphi2, sinphi2);
+ Point R2 = (Re2._x*cosphi2 - Re2._y*sinphi2 + x2, Re2._y*cosphi2 + Re2._x*sinphi2 + y2);
+ Point R2 = Re2.TransformToCartesianCoordinates(ped2->GetPos(), cosphi2, sinphi2);
+ */
+ //Calculate distance between point R and line
+ double Dis1 = Ne1.ScalarProduct(Re1) - Ne1.ScalarProduct(A1e);
+ double Dis2 = Ne2.ScalarProduct(Re2) - Ne2.ScalarProduct(A2e);
+
+ //Judge if the line contact with ellipse2
+ ped2->SetPos(ped2_current);
+ if (Dis1 >= 0 && Dis2 >= 0)
+ return my_pair(FLT_MAX, -1);
+ else
+ {
+ return my_pair(eff_dist, ped2->GetID());
+ }
+}
+Point AGCVMModel::ForceRepPed(Pedestrian* ped1, Pedestrian* ped2, Point e0, int periodic) const
+{
+ Point F_rep(0.0, 0.0);
+ // x- and y-coordinate of the distance between p1 and p2
+
+ double x_j = ped2->GetPos()._x;
+ double y_j = ped2->GetPos()._y;
+
+ if (periodic) {
+ double x = ped1->GetPos()._x;
+ if ((xRight_agcvm - x) + (x_j - xLeft_agcvm) <= cutoff_agcvm) {
+ double x2_periodic = x_j + xRight_agcvm - xLeft_agcvm;
+ ped2->SetPos(Point(x2_periodic, y_j));
+ }
+ if ((x - xLeft_agcvm) + (xRight_agcvm - x_j) <= cutoff_agcvm) {
+ double x2_periodic = xLeft_agcvm - xRight_agcvm + x_j;
+ ped2->SetPos(Point(x2_periodic, y_j));
+ }
+ }
+
+ Point distp12 = ped2->GetPos() - ped1->GetPos();
+ double Distance = distp12.Norm();
+ Point ep12; // x- and y-coordinate of the normalized vector between p1 and p2
+ double R_ij;
+
+ if (Distance >= J_EPS) {
+ ep12 = distp12.Normalized();
+ }
+ else {
+ //printf("ERROR: \tin VelocityModel::forcePedPed() ep12 can not be calculated!!!\n");
+ Log->Write(KRED "\nWARNING: \tin VelocityModel::forcePedPed() ep12 can not be calculated!!!" RESET);
+ Log->Write("\t\t Pedestrians are too near to each other (dist=%f).", Distance);
+ Log->Write("\t\t Maybe the value of <a> in force_ped should be increased. Going to exit.\n");
+ printf("ped1 %d ped2 %d\n", ped1->GetID(), ped2->GetID());
+ printf("ped1 at (%f, %f), ped2 at (%f, %f)\n", ped1->GetPos()._x, ped1->GetPos()._y, ped2->GetPos()._x, ped2->GetPos()._y);
+ exit(EXIT_FAILURE);
+ }
+ Point ei;
+ JEllipse Eped1 = ped1->GetEllipse();
+ ei._x = Eped1.GetCosPhi();
+ ei._y = Eped1.GetSinPhi();
+
+ //Version area-----------------------------------
+ double condition1 = e0.ScalarProduct(ep12); // < e_i , e_ij > should be positive
+ double condition2 = ei.ScalarProduct(ep12);
+ if (GetGCVMU() == 0)//all
+ {
+ condition1 = 1;
+ condition2 = 1;
+ }
+ //-----------------------------------------------
+
+ //rule:pedestrian's direction only influenced by pedestrian in version area
+ if (condition1 > 0 || condition2 > 0)
+ {
+ double dist;
+ JEllipse Eped2 = ped2->GetEllipse();
+ dist = Eped1.EffectiveDistanceToEllipse(Eped2, &dist);
+ dist = dist > 0 ? dist : 0;
+ R_ij = -_aPed * exp((-dist) / _DPed);
+ F_rep = ep12 * R_ij;
+ }
+ ped2->SetPos(Point(x_j, y_j));
+ return F_rep;
+}//END Velocity:ForceRepPed()
+
+Point AGCVMModel::ForceRepRoom(Pedestrian* ped, SubRoom* subroom, Point e0) const
+{
+ Point f(0., 0.);
+ const Point& centroid = subroom->GetCentroid();
+ bool inside = subroom->IsInSubRoom(centroid);
+ //first the walls
+ for (const auto & wall : subroom->GetAllWalls())
+ {
+ f += ForceRepWall(ped, wall, centroid, inside, e0);
+ }
+
+ //then the obstacles
+ for (const auto & obst : subroom->GetAllObstacles())
+ {
+ if (obst->Contains(ped->GetPos()))
+ {
+ Log->Write("ERROR:\t Agent [%d] is trapped in obstacle in room/subroom [%d/%d]", ped->GetID(), subroom->GetRoomID(), subroom->GetSubRoomID());
+ exit(EXIT_FAILURE);
+ }
+ else
+ for (const auto & wall : obst->GetAllWalls())
+ {
+ f += ForceRepWall(ped, wall, centroid, inside, e0);
+ }
+ }
+
+ // and finally the closed doors
+ for (const auto & goal : subroom->GetAllTransitions())
+ {
+ if (!goal->IsOpen())
+ {
+ f += ForceRepWall(ped, *(static_cast<Line*>(goal)), centroid, inside, e0);
+ }
+ int uid1 = goal->GetUniqueID();
+ int uid2 = ped->GetExitIndex();
+ // ignore my transition consider closed doors
+ //closed doors are considered as wall
+ //door is open, but it's not my door (has influence)
+ /*
+ if((uid1 != uid2) && (goal->IsOpen()==true ))
+ {
+ f += ForceRepWall(ped,*(static_cast<Line*>(goal)), centroid, inside, e0, pdesire);
+ }
+ */
+ }
+ return f;
+}
+
+Point AGCVMModel::ForceRepWall(Pedestrian* ped, const Line& w, const Point& centroid, bool inside, Point e0) const
+{
+ Point F_wrep = Point(0.0, 0.0);
+ Point pt = w.ShortestPoint(ped->GetPos());
+ Point dist = pt - ped->GetPos(); // x- and y-coordinate of the distance between ped and p
+ const double EPS = 0.000; // molified see Koester2013
+ double Distance = dist.Norm() + EPS; // distance between the centre of ped and point p
+ //double vn = w.NormalComp(ped->GetV()); //normal component of the velocity on the wall
+ Point e_iw; // x- and y-coordinate of the normalized vector between ped and pt
+ //double K_iw;
+ double R_iw;
+ double min_distance_to_wall = 0.001; // 10 cm
+ if (Distance > min_distance_to_wall) {
+ e_iw = dist / Distance;
+ }
+ else {
+ Log->Write("WARNING:\t Velocity: forceRepWall() ped %d [%f, %f] is too near to the wall [%f, %f]-[%f, %f] (dist=%f)", ped->GetID(), ped->GetPos()._y, ped->GetPos()._y, w.GetPoint1()._x, w.GetPoint1()._y, w.GetPoint2()._x, w.GetPoint2()._y, Distance);
+ Point new_dist = centroid - ped->GetPos();
+ new_dist = new_dist / new_dist.Norm();
+ //printf("new distance = (%f, %f) inside=%d\n", new_dist._x, new_dist._y, inside);
+ e_iw = (inside ? new_dist * -1 : new_dist);
+ }
+
+ //rules--------------------------------------
+
+ //rule: wall in behind has no influence
+ Point pt1 = w.GetPoint1();
+ Point pt2 = w.GetPoint2();
+ Point dist_pt1 = pt1 - ped->GetPos();
+ Point dist_pt2 = pt2 - ped->GetPos();
+ Point e_iw1 = dist_pt1 / dist_pt1.Norm();
+ Point e_iw2 = dist_pt2 / dist_pt2.Norm();
+ Point ei;
+ JEllipse Eped1 = ped->GetEllipse();
+ ei._x = Eped1.GetCosPhi();
+ ei._y = Eped1.GetSinPhi();
+ //version area----------------------------------
+ double result_e01 = e0.ScalarProduct(e_iw1);
+ double result_e02 = e0.ScalarProduct(e_iw2);
+ double result_ei1 = ei.ScalarProduct(e_iw1);
+ double result_ei2 = ei.ScalarProduct(e_iw2);
+ //----------------------------------------------
+ if (GetGCVMU() == 0)//all
+ {
+ result_e01 = 1;
+ result_e02 = 1;
+ result_ei1 = 1;
+ result_ei2 = 1;
+ }
+ if (result_e01 < 0 && result_e02 < 0 && result_ei1 < 0 && result_ei1 < 0)
+ {
+ return F_wrep;
+ }
+
+ // using new method calculate influence direciton----------------------------------------
+ Point inf_direction;// left side f pedestrian
+ inf_direction._x = -e0._y;
+ inf_direction._y = e0._x;
+ inf_direction = inf_direction.Normalized();
+ double result1 = e0.CrossProduct(e_iw);
+ double result2 = e0.CrossProduct(inf_direction);
+ if (bool equal = almostEqual(result1, 0, 0.00001))//Is there any possible that desired direction towards the wall?
+ {
+ //double alpha = ped->GetAlpha();
+ Point zero = Point(0.0, 0.0);
+ int random = rand() % 1000;//choose one direciton by random
+ if (random > 500)//when random is larger than 50, influence's direction is right, otherwise is left
+ {
+ inf_direction = zero - inf_direction;
+ }
+ }
+ else
+ {
+ double result2 = e0.CrossProduct(inf_direction);
+ Point zero = Point(0.0, 0.0);
+ if (result1*result2 < 0)
+ {
+ inf_direction = zero - inf_direction;
+ }
+ }
+ //-----------------------------------------------------------------------------------------
+
+ //use effective distance to calculate ForceRepWall
+ double effdis = Eped1.EffectiveDistanceToLine(w);
+ effdis = effdis > 0 ? effdis : 0;
+ R_iw = -_aWall * exp((-effdis) / _DWall);
+ if (GetGCVMU() == 1)
+ {
+ F_wrep = inf_direction * R_iw;//new method
+ }
+ else
+ {
+ F_wrep = e_iw * R_iw;//original method
+ }
+ return F_wrep;
+}
+
+double AGCVMModel::GetSpacingRoom(Pedestrian* ped, SubRoom* subroom, Point ei) const
+{
+ double spacing = FLT_MAX;
+ double distance = FLT_MAX;
+ //first the walls
+ for (const auto & wall : subroom->GetAllWalls())
+ {
+ distance = GetSpacingWall(ped, wall, ei);
+ spacing = spacing > distance ? distance : spacing;
+ }
+ //then the obstacles
+ for (const auto & obst : subroom->GetAllObstacles())
+ {
+ for (const auto & wall : obst->GetAllWalls())
+ {
+ distance = GetSpacingWall(ped, wall, ei);
+ spacing = spacing > distance ? distance : spacing;
+ }
+ }
+ //and finally the closed doors
+ for (const auto & goal : subroom->GetAllTransitions())
+ {
+ if (!goal->IsOpen())
+ {
+ distance = GetSpacingWall(ped, *(static_cast<Line*>(goal)), ei);
+ spacing = spacing > distance ? distance : spacing;
+ }
+ int uid1 = goal->GetUniqueID();
+ int uid2 = ped->GetExitIndex();
+ //door is open, bur not my door
+ if ((uid1 != uid2) && (goal->IsOpen() == true))
+ {
+ distance = GetSpacingWall(ped, *(static_cast<Line*>(goal)), ei);
+ spacing = spacing > distance ? distance : spacing;
+ }
+ }
+ return spacing;
+
+}
+
+double AGCVMModel::GetSpacingWall(Pedestrian* ped, const Line& l, Point ei) const
+{
+ double spacing = FLT_MAX;
+ Point pp = ped->GetPos();
+ Point pt = l.ShortestPoint(ped->GetPos());
+ Point p1 = l.GetPoint1();
+ Point p2 = l.GetPoint2();
+ Point dist = pt - pp;
+ Point ei_vertical;
+ ei_vertical._x = -ei.Normalized()._y;
+ ei_vertical._y = ei.Normalized()._x;
+ double b = ped->GetSmallerAxis();
+ Point A1 = pp + ei_vertical * b;
+ Point A2 = pp - ei_vertical * b;
+ Point p1_A1 = p1 - A1;
+ Point p2_A1 = p2 - A1;
+ Point p12 = p1 - p2;
+ double A1_result1 = ei.CrossProduct(p1_A1);
+ double A1_result2 = ei.CrossProduct(p2_A1);
+ double result3 = ei.ScalarProduct(dist);
+ Point p1_A2 = p1 - A2;
+ Point p2_A2 = p2 - A2;
+ double A2_result1 = ei.CrossProduct(p1_A2);
+ double A2_result2 = ei.CrossProduct(p2_A2);
+ if (result3 <= 0)
+ {
+ return spacing;
+ }
+ if (A1_result1*A1_result2 > 0 && A2_result1*A2_result2 > 0 && A1_result1*A2_result1 > 0 && A1_result2*A2_result2 > 0)
+ {
+ return spacing;
+ }
+ double effdis = ped->GetEllipse().EffectiveDistanceToLine(l);
+ double cosangle = dist.ScalarProduct(ei) / (dist.Norm()*ei.Norm());
+ if (cosangle < 0.00001)
+ {
+ return spacing;
+ }
+ spacing = effdis / cosangle;
+ return spacing;
+}
+
+string AGCVMModel::GetDescription()
+{
+ string rueck;
+ char tmp[CLENGTH];
+
+ sprintf(tmp, "\t\ta: \t\tPed: %f \tWall: %f\n", _aPed, _aWall);
+ rueck.append(tmp);
+ sprintf(tmp, "\t\tD: \t\tPed: %f \tWall: %f\n", _DPed, _DWall);
+ rueck.append(tmp);
+ return rueck;
+}
+
+std::shared_ptr<DirectionStrategy> AGCVMModel::GetDirection() const
+{
+ return _direction;
+}
+
+
+double AGCVMModel::GetaPed() const
+{
+ return _aPed;
+}
+
+double AGCVMModel::GetDPed() const
+{
+ return _DPed;
+}
+
+
+double AGCVMModel::GetaWall() const
+{
+ return _aWall;
+}
+
+double AGCVMModel::GetDWall() const
+{
+ return _DWall;
+}
+
+int AGCVMModel::GetGCVMU() const
+{
+ return _GCVMUsing;
+}
diff --git a/math/AGCVMModel.h b/math/AGCVMModel.h
new file mode 100644
index 0000000000000000000000000000000000000000..adfea0e8cecf64d4e672c14b2762b6b756e1e25d
--- /dev/null
+++ b/math/AGCVMModel.h
@@ -0,0 +1,189 @@
+/**
+* \file AGCVM.h
+* \date Jun 26, 2019
+* \version v0.8
+* \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
+*
+* \section License
+* This file is part of JuPedSim.
+*
+* JuPedSim is free software: you can redistribute it and/or modify
+* it under the terms of the GNU Lesser General Public License as published by
+* the Free Software Foundation, either version 3 of the License, or
+* any later version.
+*
+* JuPedSim is distributed in the hope that it will be useful,
+* but WITHOUT ANY WARRANTY; without even the implied warranty of
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+* GNU General Public License for more details.
+*
+* You should have received a copy of the GNU Lesser General Public License
+* along with JuPedSim. If not, see <http://www.gnu.org/licenses/>.
+*
+* \section Description
+* Implementation of first-order model
+* Anticipation generalized collision-free velocity model: Qiancheng (8)
+*
+*
+**/
+
+
+#ifndef AGCVMMODEL_H_
+#define AGCVMMODEL_H_
+
+#include <vector>
+
+#include "../geometry/Building.h"
+#include "OperationalModel.h"
+
+typedef std::pair<double, double> my_pair;
+// sort with respect to first element (ascending).
+// In case of equality sort with respect to second element (descending)
+
+struct sort_pred_agcvm
+{
+ bool operator () (const my_pair& left, const my_pair& right)
+ {
+ return (left.first == right.first) ?
+ (left.second > right.second) :
+ (left.first < right.first);
+ }
+};
+
+
+
+//forward declaration
+class Pedestrian;
+class DirectionStrategy;
+
+
+class AGCVMModel : public OperationalModel {
+private:
+
+ /// Modellparameter
+ double _aPed;
+ double _DPed;
+
+ double _aWall;
+ double _DWall;
+
+ double _Ts;
+ double _Td;
+
+ int _GCVMUsing=1;// Keep it for incase
+
+ double OptimalSpeed(Pedestrian* ped, double spacing) const;
+
+ /**
+ * The desired direction of pedestrian
+ *
+ * @param ped: Pointer to Pedestrians
+ * @param room: Pointer to room
+ *
+ * @return Point
+ */
+ Point e0(Pedestrian *ped, Room* room) const;
+ /**
+ * Get the spacing between ped1 and ped2
+ *
+ * @param ped1 Pointer to Pedestrian: First pedestrian
+ * @param ped2 Pointer to Pedestrian: Second pedestrian
+ * @param ei the direction of pedestrian.
+ *
+ * @return Point
+ */
+ my_pair GetSpacing(Pedestrian* ped1, Pedestrian* ped2, Point ei, int periodic) const;
+ /**
+ * Repulsive force between two pedestrians ped1 and ped2
+ * @param ped1 Pointer to Pedestrian: First pedestrian
+ * @param ped2 Pointer to Pedestrian: Second pedestrian
+ *
+ * @return Point
+ */
+ Point ForceRepPed(Pedestrian* ped1, Pedestrian* ped2, Point e0, int periodic) const;
+ /**
+ * Repulsive force acting on pedestrian <ped> from the walls in
+ * <subroom>. The sum of all repulsive forces of the walls in <subroom> is calculated
+ * @see ForceRepWall
+ * @param ped Pointer to Pedestrian
+ * @param subroom Pointer to SubRoom
+ *
+ * @return Point
+ */
+ Point ForceRepRoom(Pedestrian* ped, SubRoom* subroom, Point e0) const;
+ /**
+ * Repulsive force between pedestrian <ped> and wall <l>
+ *
+ * @param ped Pointer to Pedestrian
+ * @param l reference to Wall
+ *
+ * @return Point
+ */
+ Point ForceRepWall(Pedestrian* ped, const Line& l, const Point& centroid, bool inside, Point e0) const;
+ double GetSpacingRoom(Pedestrian* ped, SubRoom* subroom, Point ei) const;
+ double GetSpacingWall(Pedestrian* ped, const Line& l, Point ei) const;
+
+public:
+
+ AGCVMModel(std::shared_ptr<DirectionStrategy> dir, double aped, double Dped,
+ double awall, double Dwall, double Ts, double Td, int GCVM);
+ virtual ~AGCVMModel(void);
+
+
+ std::shared_ptr<DirectionStrategy> GetDirection() const;
+
+ /**
+ * ToDO: What is this parameter doing?
+ *
+ * @return double
+ *
+ */
+ double GetaPed() const;
+
+ /**
+ * ToDO: What is this parameter doing?
+ *
+ * @return double
+ */
+ double GetDPed() const;
+
+ /**
+ * ToDO: What is this parameter doing?
+ *
+ * @return double
+ */
+ double GetaWall() const;
+
+ /**
+ * ToDO: What is this parameter doing?
+ *
+ * @return double
+ */
+ double GetDWall() const;
+
+ /**
+ * @return all model parameters in a nicely formatted string
+ */
+ virtual std::string GetDescription();
+
+ /**
+ * initialize the phi angle
+ * @param building
+ */
+ virtual bool Init(Building* building);
+
+ /**
+ * Compute the next simulation step
+ * Solve the differential equations and update the positions and velocities
+ * @param current the actual time
+ * @param deltaT the next timestep
+ * @param building the geometry object
+ * @param periodic: used in some utests for periodic scenarios (very specific)
+ */
+ virtual void ComputeNextTimeStep(double current, double deltaT, Building* building, int periodic);
+
+ int GetGCVMU() const;
+};
+
+
+#endif
diff --git a/math/SimplestModel.cpp b/math/SimplestModel.cpp
index bcf54952a7d3f5346031b8817df02c34122c651d..65fdba942ef17aa1f8628fb00395f0cf7ac11d6a 100644
--- a/math/SimplestModel.cpp
+++ b/math/SimplestModel.cpp
@@ -61,7 +61,7 @@ using std::vector;
using std::string;
SimplestModel::SimplestModel(std::shared_ptr<DirectionStrategy> dir, double aped, double Dped,
- double awall, double Dwall, double Ts, double Td, int Parallel, double waitingTime, int sDirection, int sSpeed, int GCVMU)
+ double awall, double Dwall, double Ts, double Td, int Parallel, double waitingTime, double areasize, int sDirection, int sSpeed, int GCVMU)
{
_direction = dir;
// Force_rep_PED Parameter
@@ -77,6 +77,7 @@ SimplestModel::SimplestModel(std::shared_ptr<DirectionStrategy> dir, double aped
_SubmodelDirection = sDirection;
_SubmodelSpeed = sSpeed;
_GCVMUsing = GCVMU;
+ _AreaSize = areasize;
}
@@ -326,7 +327,7 @@ void SimplestModel::ComputeNextTimeStep(double current, double deltaT, Building*
my_pair relation = my_pair(ped->GetID(), first_ID);
const Point& pos = ped->GetPos();
double distGoal = ped->GetExitLine()->DistTo(pos);
- double DRange=1;
+ double DRange=GetAreaSize();
if (UDirection==0||distGoal<DRange)
{
relations.push_back(relation);
@@ -1121,4 +1122,9 @@ int SimplestModel::GetSSpeed() const
int SimplestModel::GetGCVMU() const
{
return _GCVMUsing;
+}
+
+double SimplestModel::GetAreaSize() const
+{
+ return _AreaSize;
}
\ No newline at end of file
diff --git a/math/SimplestModel.h b/math/SimplestModel.h
index 999d696b02eef001f21046a650aa8f455460d611..94126507177eef59e009b4723355cadf5728a354 100644
--- a/math/SimplestModel.h
+++ b/math/SimplestModel.h
@@ -73,6 +73,7 @@ private:
int _Parallel;
double _WaitingTime;
+ double _AreaSize;
int _SubmodelDirection;
int _SubmodelSpeed;
@@ -133,7 +134,7 @@ private:
public:
SimplestModel(std::shared_ptr<DirectionStrategy> dir, double aped, double Dped,
- double awall, double Dwall, double Ts, double Td, int Parallel, double waitingTime, int sDirection, int sSpeed, int GCVMU);
+ double awall, double Dwall, double Ts, double Td, int Parallel, double waitingTime, double areasize, int sDirection, int sSpeed, int GCVMU);
virtual ~SimplestModel(void);
@@ -198,6 +199,8 @@ public:
int GetSSpeed() const;
int GetGCVMU() const;
+
+ double GetAreaSize() const;
};