diff --git a/IO/GeoFileParser.cpp b/IO/GeoFileParser.cpp
index fb19f3587f2a49fd495a0d6c7022fed055537578..bc62f97dae56393d7df7cabcba1907bbe6e415d9 100644
--- a/IO/GeoFileParser.cpp
+++ b/IO/GeoFileParser.cpp
@@ -211,6 +211,7 @@ bool GeoFileParser::LoadGeometry(Building* building)
subroom->AddObstacle(obstacle);
}
room->AddSubRoom(subroom);
+
}
//parsing the crossings
TiXmlNode* xCrossingsNode = xRoom->FirstChild("crossings");
diff --git a/demos/scenario_4_stairs/stairs_ini.xml b/demos/scenario_4_stairs/stairs_ini.xml
index c6aaac7f7ce563569743478c7a80f80c9a0370d8..2ebcbb2c246ac4a1ad4d83a317819762bd110e5d 100644
--- a/demos/scenario_4_stairs/stairs_ini.xml
+++ b/demos/scenario_4_stairs/stairs_ini.xml
@@ -10,7 +10,7 @@ xsi:noNamespaceSchemaLocation="../../xsd/jps_ini_core.xsd">
<max_sim_time>300</max_sim_time>
<!-- geometry file -->
<geometry>stairs_geo.xml</geometry>
- <num_threads>6</num_threads>
+ <num_threads>1</num_threads>
<!-- traectories file and format -->
<trajectories format="xml-plain" fps="8">
<file location="stairs_traj.xml" />
@@ -44,7 +44,7 @@ xsi:noNamespaceSchemaLocation="../../xsd/jps_ini_core.xsd">
<!--persons information and distribution -->
<agents operational_model_id="2">
<agents_distribution>
- <group group_id="1" agent_parameter_id="1" room_id="0" subroom_id="0" number="100" router_id="2"/>
+ <group group_id="1" agent_parameter_id="1" room_id="0" subroom_id="0" number="7" router_id="2" x_min="1.0" x_max="5.0"/>
</agents_distribution>
</agents>
diff --git a/geometry/Building.cpp b/geometry/Building.cpp
index 72561e47005cc999ab3167de4dcddeee875277bd..c6abcb59b54ac06f05c082eea42da804fa754596 100644
--- a/geometry/Building.cpp
+++ b/geometry/Building.cpp
@@ -319,6 +319,9 @@ bool Building::InitGeometry()
return false;
}
+ //here we can create a boost::geometry::model::polygon out of the vector<Point> objects created above
+ itr_subroom.second->CreateBoostPoly();
+
double minElevation = FLT_MAX;
double maxElevation = -FLT_MAX;
for (auto&& wall:itr_subroom.second->GetAllWalls()) {
diff --git a/geometry/SubRoom.cpp b/geometry/SubRoom.cpp
index f22732204cd53ed83e9b0fd2756d9b35589eae3d..f8a5b3e8a1dbef070901b2b4156f5cb3f4c4c4bd 100644
--- a/geometry/SubRoom.cpp
+++ b/geometry/SubRoom.cpp
@@ -76,6 +76,7 @@ SubRoom::SubRoom()
_goalIDs = vector<int> ();
_area = 0.0;
_uid = _static_uid++;
+ _boostPoly = polygon_type();
}
SubRoom::~SubRoom()
@@ -185,6 +186,11 @@ bool SubRoom::AddWall(const Wall& w)
return false;
}
_walls.push_back(w);
+
+ //not a good idea to add points, as we need points in clockwise rotation and each point only once. we need to process all walls
+ //boost::geometry::append(_boostPoly, w.GetPoint1());
+ //boost::geometry::append(_boostPoly, w.GetPoint2());
+
return true;
}
@@ -784,13 +790,39 @@ bool SubRoom::IsPartOfPolygon(const Point& ptw)
bool SubRoom::IsInObstacle(const Point& pt)
{
//write the obstacles
- for (auto&& obst : _obstacles)
- {
- if(obst->Contains(pt)) return true;
+// for (auto&& obst : _obstacles)
+// {
+// if(obst->Contains(pt)) return true;
+// }
+// return false;
+ for (polygon_type obs:_boostPolyObstacles) {
+ if (boost::geometry::within(pt, obs)) {
+ return true;
+ }
}
return false;
}
+bool SubRoom::CreateBoostPoly() {
+ std::vector<Point> copyPts;
+ copyPts.insert(copyPts.begin(), _poly.begin(), _poly.end());
+ if(IsClockwise())
+ std::reverse(copyPts.begin(), copyPts.end());
+
+ boost::geometry::assign_points(_boostPoly, _poly);
+
+ for (auto obsPtr:_obstacles) {
+ std::vector<Point> obsPoints;
+ obsPoints.insert(obsPoints.begin(), obsPtr->GetPolygon().begin(), obsPtr->GetPolygon().end());
+ if (obsPtr->IsClockwise()) {
+ std::reverse(obsPoints.begin(), obsPoints.end());
+ }
+ polygon_type newObstacle;
+ boost::geometry::assign_points(newObstacle, obsPoints);
+ _boostPolyObstacles.emplace_back(newObstacle);
+ }
+}
+
/************************************************************
NormalSubRoom
************************************************************/
@@ -932,11 +964,11 @@ bool NormalSubRoom::ConvertLineToPoly(const vector<Line*>& goals)
return false;
}
else
- ++j;
+ ++j; //number of lines, that share endpoints with wall "it"
}
}
if (j <= 2)
- j = 0;
+ j = 0; //all good, set j back to 0 for next iteration
else {
char tmp[CLENGTH];
sprintf(tmp, "WARNING: \tNormanSubRoom::ConvertLineToPoly(): SubRoom %d Room %d !!\n", GetSubRoomID(), GetRoomID());
@@ -944,7 +976,7 @@ bool NormalSubRoom::ConvertLineToPoly(const vector<Line*>& goals)
sprintf(tmp, "WARNING: \tWall %s shares edge with multiple walls!!! j=%d\n", it.toString().c_str(), j);
Log->Write(tmp); // why should this return false?
}
- ++itr;
+ ++itr; // only check lines that have greater index than current "it" (inner loop goes from itr to size)
}
@@ -1035,50 +1067,61 @@ double NormalSubRoom::Xintercept(const Point& point1, const Point& point2, doubl
bool NormalSubRoom::IsInSubRoom(const Point& ped) const
{
-
- //case when the point is on an edge
- // todo: this affect the runtime, and do we really need that
- // If we do not do this check, then for a square for instance, half the points located on the edge will be inside and
- // the other half will be outside the polygon.
- for(auto& w: _walls)
- {
- if(w.IsInLineSegment(ped)) return true;
- }
-
- auto next_corner = _poly.begin();
- short quad = WhichQuad(*next_corner, ped);
- short next_quad;
- short total = 0; // count of absolute sectors crossed
- for (auto& corner : _poly) { // _poly contains all corner points of the walls
- ++next_corner;
- if (next_corner == _poly.end()) next_corner = _poly.begin();
- next_quad = WhichQuad(*next_corner, ped);
- short delta = next_quad - quad;
-
- switch (delta) {
- case 2:
- case -2:
- // If we crossed the middle, figure out if it
- // was clockwise or counter-clockwise by using
- // the x position of the ped
- if (Xintercept(corner, *next_corner, ped._y) > ped._x)
- delta = -delta;
- break;
- case 3: // moving 3 quads is like moving back 1
- delta = -1;
- break;
- case -3: // moving back 3 is like moving forward 1
- delta = 1;
- break;
- default:
- break;
+ for (polygon_type obs:_boostPolyObstacles) {
+ // if pedestrian is stuck in obstacle, please return false
+ if (boost::geometry::within(ped, obs)) {
+ return false;
}
-
- total += delta;
- quad = next_quad;
}
+ // pedestrian is not in obstacle, so we can use within(...) on _boostPoly
+ return boost::geometry::within(ped, _boostPoly);
+
+ // the code below is old and only works in manhatten-polygons (horizontal and vertical lines only)
- return abs(total) == 4;
+// //case when the point is on an edge
+// // todo: this affect the runtime, and do we really need that
+// // If we do not do this check, then for a square for instance, half the points located on the edge will be inside and
+// // the other half will be outside the polygon.
+// // We should also consider crossings and transitions - not only walls
+// for(auto& w: _walls)
+// {
+// if(w.IsInLineSegment(ped)) return true;
+// }
+//
+// auto next_corner = _poly.begin();
+// short quad = WhichQuad(*next_corner, ped);
+// short next_quad;
+// short total = 0; // count of absolute sectors crossed
+// for (auto& corner : _poly) { // _poly contains all corner points of the walls
+// ++next_corner;
+// if (next_corner == _poly.end()) next_corner = _poly.begin();
+// next_quad = WhichQuad(*next_corner, ped);
+// short delta = next_quad - quad;
+//
+// switch (delta) {
+// case 2:
+// case -2:
+// // If we crossed the middle, figure out if it
+// // was clockwise or counter-clockwise by using
+// // the x position of the ped
+// if (Xintercept(corner, *next_corner, ped._y) > ped._x)
+// delta = -delta;
+// break;
+// case 3: // moving 3 quads is like moving back 1
+// delta = -1;
+// break;
+// case -3: // moving back 3 is like moving forward 1
+// delta = 1;
+// break;
+// default:
+// break;
+// }
+//
+// total += delta;
+// quad = next_quad;
+// }
+//
+// return abs(total) == 4;
}
/************************************************************
diff --git a/geometry/SubRoom.h b/geometry/SubRoom.h
index 89abc9b96e8244f5a23dcba5cad63dc9cdb78a54..11779d29900f7576d41e61a892d8095734bc8739 100644
--- a/geometry/SubRoom.h
+++ b/geometry/SubRoom.h
@@ -35,6 +35,11 @@
#include <vector>
#include <string>
+#include <boost/polygon/polygon.hpp>
+
+//typedef boost::geometry::model::d2::point_xy<double> point_type;
+//typedef boost::geometry::model::polygon<point_type> polygon_type;
+typedef boost::geometry::model::polygon<Point> polygon_type;
//forward declarations
class Transition;
@@ -86,6 +91,9 @@ private:
protected:
std::vector<Wall> _walls;
std::vector<Point> _poly; // Polygonal representation of the subroom
+
+ polygon_type _boostPoly;
+ std::vector<polygon_type> _boostPolyObstacles;
std::vector<double> _poly_help_constatnt; //for the function IsInsidePolygon, a.brkic
std::vector<double> _poly_help_multiple; //for the function IsInsidePolygon, a.brkic
std::vector<Obstacle*> _obstacles;
@@ -365,6 +373,7 @@ public:
/// convert all walls and transitions(doors) into a polygon representing the subroom
virtual bool ConvertLineToPoly(const std::vector<Line*>& goals) = 0;
+ bool CreateBoostPoly();
///check whether the pedestrians is still in the subroom
virtual bool IsInSubRoom(const Point& ped) const = 0;
diff --git a/routing/ff_router/FloorfieldViaFM.cpp b/routing/ff_router/FloorfieldViaFM.cpp
index 0b45d9c05427210981a8b0691c5e857ee12ff58c..1fe30791d023602da064d2a665f7a6edef7c61ef 100644
--- a/routing/ff_router/FloorfieldViaFM.cpp
+++ b/routing/ff_router/FloorfieldViaFM.cpp
@@ -1709,6 +1709,16 @@ void FloorfieldViaFM::writeFF(const std::string& filename, std::vector<int> targ
file << _dirToWall[i]._x << " " << _dirToWall[i]._y << " 0.0" << std::endl;
}
+ file << "SCALARS SubroomPtr float 1" << std::endl;
+ file << "LOOKUP_TABLE default" << std::endl;
+ for (long int i = 0; i < _grid->GetnPoints(); ++i) {
+ if (_subrooms[i]) {
+ file << _subrooms[i]->GetUID() << std::endl;
+ } else {
+ file << 0.0 << std::endl;
+ }
+ }
+
for (unsigned int iTarget = 0; iTarget < targetID.size(); ++iTarget) {
Log->Write("%s: target number %d: UID %d", filename.c_str(), iTarget, targetID[iTarget]);
if (_neggradmap.count(targetID[iTarget]) == 0) {
@@ -1921,4 +1931,10 @@ void CentrePointFFViaFM::getDirectionToUID(int destID, const long int key, Point
}
direction._x = (localneggradptr[key]._x);
direction._y = (localneggradptr[key]._y);
+}
+
+SubRoom* FloorfieldViaFM::GetSubroom(Pedestrian* ped) {
+ long int key = _grid->getKeyAtPoint(ped->GetPos());
+ assert(key >= 0 && key <= _grid->GetnPoints());
+ return _subrooms[key];
}
\ No newline at end of file
diff --git a/routing/ff_router/FloorfieldViaFM.h b/routing/ff_router/FloorfieldViaFM.h
index cc2239a5d74a3c7bd84cba702c2af6d2a17e6bc0..655a47c3c5f9467fa81f7ccf4307b3c38445e482 100644
--- a/routing/ff_router/FloorfieldViaFM.h
+++ b/routing/ff_router/FloorfieldViaFM.h
@@ -153,6 +153,7 @@ public:
void writeGoalFF(const std::string&, std::vector<int> targetID);
virtual SubRoom* isInside(const long int key);
+ SubRoom* GetSubroom(Pedestrian* p);
std::map<int, int> getGoalToLineUIDmap() const
{
diff --git a/routing/ff_router/ffRouter.cpp b/routing/ff_router/ffRouter.cpp
index 76cc29947cef585653d31aacb5ae7400328117dd..58008e5e4252bfbcf037b2c5e1f95581e3f2173e 100644
--- a/routing/ff_router/ffRouter.cpp
+++ b/routing/ff_router/ffRouter.cpp
@@ -398,13 +398,13 @@ bool FFRouter::Init(Building* building)
//int roomTest = (*(_locffviafm.begin())).first;
//int transTest = (building->GetRoom(roomTest)->GetAllTransitionsIDs())[0];
- /*
- for (unsigned int i = 0; i < _locffviafm.size(); ++i) {
- auto iter = _locffviafm.begin();
- std::advance(iter, i);
- int roomNr = iter->first;
- iter->second->writeFF("testFF" + std::to_string(roomNr) + ".vtk", _allDoorUIDs);
- }//*/
+
+// for (unsigned int i = 0; i < _locffviafm.size(); ++i) {
+// auto iter = _locffviafm.begin();
+// std::advance(iter, i);
+// int roomNr = iter->first;
+// iter->second->writeFF("testFF" + std::to_string(roomNr) + ".vtk", _allDoorUIDs);
+// }
std::ofstream matrixfile;
matrixfile.open("Matrix.txt");
@@ -717,6 +717,10 @@ int FFRouter::FindExit(Pedestrian* p)
std::vector<int> DoorUIDsOfRoom;
DoorUIDsOfRoom.clear();
if (_building->GetRoom(p->GetRoomID())->GetSubRoom(p->GetSubRoomID())->IsInSubRoom(p->GetPos())) {
+ //SubRoom* exp1 = _building->GetRoom(p->GetRoomID())->GetSubRoom(p->GetSubRoomID());
+ //SubRoom* exp2 = _locffviafm[p->GetRoomID()]->GetSubroom(p);
+ //if (exp1 == exp2) {
+ //if (_building->GetRoom(p->GetRoomID())->GetSubRoom(p->GetSubRoomID()) == _locffviafm[p->GetRoomID()]->GetSubroom(p)) {
//ped is in the subroom, according to its member attribs
} else {
// find next crossing / transition and set new room to the OTHER