Commit 87db2461 authored by tobias schroedter's avatar tobias schroedter

FF navigates people inside waiting area. people are waiting inside and after...

FF navigates people inside waiting area. people are waiting inside and after time going to next goal
parent 23d09e20
Pipeline #16725 failed with stages
in 15 seconds
......@@ -423,7 +423,7 @@ bool GeoFileParser::LoadRoutingInfo(Building* building)
Goal* wa = new WaitingArea();
WaitingArea* waitingArea = static_cast<WaitingArea*>(wa);
waitingArea->SetIsFinalGoal(1);
waitingArea->SetIsFinalGoal(0);
waitingArea->SetId(id);
waitingArea->SetCaption(caption);
waitingArea->setOpen(open);
......@@ -471,7 +471,7 @@ bool GeoFileParser::LoadRoutingInfo(Building* building)
}
}
if (!wa->ConvertLineToPoly()){
if (!waitingArea->ConvertLineToPoly()){
std::cout << "wa convertLineToPoly!";
return false;
}
......@@ -490,6 +490,8 @@ bool GeoFileParser::LoadRoutingInfo(Building* building)
building->AddGoal(wa);
_configuration->GetRoutingEngine()->AddFinalDestinationID(wa->GetId());
for (auto &itrGoal : building->GetAllGoals()) {
std::cout << "Goal ID: " << itrGoal.second->GetId() << std::endl;
}
......
......@@ -36,6 +36,8 @@
#include "math/GradientModel.h"
#include "pedestrian/AgentsQueue.h"
#include "pedestrian/AgentsSourcesManager.h"
#include "geometry/WaitingArea.h"
#ifdef _OPENMP
#else
......@@ -318,7 +320,7 @@ void Simulation::UpdateRoutesAndLocations()
pedsToRemove.insert(ped);
} else if ((ped->GetFinalDestination() != FINAL_DEST_OUT)
&& (goals.at(ped->GetFinalDestination())->Contains(
ped->GetPos()))) {
ped->GetPos()))&& (goals.at(ped->GetFinalDestination())->GetIsFinalGoal())) {
#pragma omp critical(Simulation_Update_pedsToRemove)
pedsToRemove.insert(ped);
}
......
......@@ -363,12 +363,44 @@ bool Building::InitGeometry()
if (s2) s2->AddNeighbor(s1);
}
InitInsideGoals();
Log->Write("INFO: \tInit Geometry successful!!!\n");
return true;
}
bool Building::InitInsideGoals()
{
for (auto& goalItr : _goals){
Goal* goal = goalItr.second;
for (auto& roomItr : _rooms){
Room* room = roomItr.second.get();
for (auto& subRoomItr : room->GetAllSubRooms()){
SubRoom* subRoom = subRoomItr.second.get();
if (subRoom->IsInSubRoom(goal->GetCentroid())){
std::cout << "Goal " << goal->GetId() << " is in subroom " << subRoom->GetUID() << std::endl;
Crossing* crossing = &(goal->GetCentreCrossing());
subRoom->AddCrossing(crossing);
crossing->SetRoom1(room);
crossing->SetSubRoom1(subRoom);
crossing->SetSubRoom2(subRoom);
AddCrossing(crossing);
for (auto& cross : subRoom->GetAllCrossings()){
std::cout << "Crossing Subroom: " << cross->GetUniqueID() << std::endl;
}
for (auto& cross: _crossings){
std::cout << "Crossing Building: " << cross.second->GetUniqueID() << std::endl;
}
}
}
}
}
return true;
}
bool Building::correct() const {
auto t_start = std::chrono::high_resolution_clock::now();
Log->Write("INFO:\tenter correct ...");
......
......@@ -187,6 +187,7 @@ public:
// convenience methods
bool InitGeometry();
void InitGrid();
//void InitRoomsAndSubroomsMap();
......@@ -254,7 +255,10 @@ public:
std::vector<Point> GetBoundaryVertices() const;
private:
void StringExplode(std::string str, std::string separator, std::vector<std::string>* results);
bool InitInsideGoals();
void StringExplode(std::string str, std::string separator, std::vector<std::string>* results);
/** @defgroup auto-correct-geometry
* functions used to auto-correct the geometry.
* Main function is correct()
......
......@@ -29,7 +29,7 @@
#include "Point.h"
#include "Goal.h"
#include "Wall.h"
#include "Crossing.h"
using namespace std;
......@@ -41,6 +41,7 @@ Goal::Goal()
_isFinalGoal=0;
_walls = vector<Wall > ();
_poly = vector<Point > ();
_crossing = Crossing();
}
Goal::~Goal()
......@@ -221,7 +222,29 @@ bool Goal::ConvertLineToPoly()
return false;
}
_poly = tmpPoly;
ComputeControid();
CreateBoostPoly();
ComputeCentroid();
//compute dummy crossing in the middle
Point point1, point2, tmp, diff, diff1, diff2;
if (_poly.size() %2 == 0){
point1 = _poly[0];
tmp = _poly[_poly.size()/2];
diff = point1 - tmp;
point1 = tmp + diff * 0.95;
point2 = tmp + diff * 0.05;
_crossing.SetPoint1(point1);
_crossing.SetPoint2(point2);
}else{
_crossing.SetPoint1(_poly[0]);
Line line(_poly[_poly.size()/2], _poly[(_poly.size()/2)+1], 0);
_crossing.SetPoint2(line.GetCentre());
}
std::cout << "Crossing goal: " << _crossing.GetUniqueID() << _crossing.toString() << std::endl;
return true;
}
......@@ -230,7 +253,7 @@ const Point& Goal::GetCentroid() const
return _centroid;
}
void Goal::ComputeControid()
void Goal::ComputeCentroid()
{
double px=0,py=0;
......@@ -271,3 +294,52 @@ void Goal::ComputeControid()
_centroid._x=px;
_centroid._y=py;
}
Crossing& Goal::GetCentreCrossing()
{
return _crossing;
}
//bool Goal::IsInsideGoal(Pedestrian* ped) const
//{
// return IsInsideGoal(ped->GetPos());
//}
bool Goal::IsInsideGoal(const Point& point) const
{
return boost::geometry::within(point, _boostPoly);
}
bool Goal::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);
return true;
}
bool Goal::IsClockwise()
{
//http://stackoverflow.com/questions/1165647/how-to-determine-if-a-list-of-polygon-points-are-in-clockwise-order
if(_poly.size()<3) {
Log->Write("ERROR:\tYou need at least 3 vertices to check for orientation. Obstacle ID [%d]",_id);
return false;
//exit(EXIT_FAILURE);
}
double sum = 0;
for (unsigned int i = 0; i < _poly.size() - 1; ++i) {
Point a = _poly[i];
Point b = _poly[i+1];
sum += (b._x - a._x) * (b._y + a._y);
}
Point first = _poly[0];
Point last = _poly[_poly.size()-1];
sum += (first._x - last._x) * (first._y + last._y);
return (sum > 0.);
}
......@@ -33,11 +33,17 @@
#include <vector>
#include "Point.h"
#include "Crossing.h"
#include <boost/polygon/polygon.hpp>
#include <boost/geometry.hpp>
//forward declarations
class Wall;
//class Point;
class Point;
//class Pedestrian;
namespace bg = boost::geometry;
typedef bg::model::polygon<Point, false, false> polygon_type;
class Goal {
......@@ -48,6 +54,10 @@ protected:
std::string _caption;
std::vector<Wall> _walls;
std::vector<Point> _poly;
Crossing _crossing;
polygon_type _boostPoly;
bool _inside;
public:
Goal();
......@@ -112,7 +122,7 @@ public:
* @return the centroid of the subroom
* @see http://en.wikipedia.org/wiki/Centroid
*/
void ComputeControid() ;
void ComputeCentroid() ;
/**
* @return the centroid of the goal
......@@ -125,8 +135,17 @@ public:
*/
std::string Write();
Crossing& GetCentreCrossing();
// bool IsInsideGoal(Pedestrian* ped) const;
bool IsInsideGoal(const Point& point) const;
private:
int WhichQuad(const Point& vertex, const Point& hitPos) const;
bool IsClockwise();
bool CreateBoostPoly();
int WhichQuad(const Point& vertex, const Point& hitPos) const;
// x-Koordinate der Linie von einer Eccke zur nächsten
double Xintercept(const Point& point1, const Point& point2,
......
......@@ -115,4 +115,40 @@ int WaitingArea::GetNextGoal()
}
}
}
\ No newline at end of file
}
void WaitingArea::addPed()
{
numPed++;
}
void WaitingArea::removePed()
{
numPed--;
}
void WaitingArea::startTimer(double time)
{
startTime = time;
std::cout << "Timer started at " << startTime << std::endl;
}
bool WaitingArea::isWaiting(double time)
{
if ((numPed >= minNumPed) && (startTime < 0. )){
startTimer(time);
}
if ((time < startTime + waitingTime) || (startTime < 0. )){
std::cout << "Waiting ..." << std::endl;
return true;
}else{
std::cout << "Waiting over!" << std::endl;
return false;
}
}
int WaitingArea::getNumPed()
{
return numPed;
}
......@@ -28,6 +28,9 @@ protected:
double waitingTime = -1;
double startTime = -1;
int numPed = 0;
protected:
/**
......@@ -68,6 +71,12 @@ public:
int GetNextGoal();
void addPed();
void removePed();
void startTimer(double time);
int getNumPed();
bool isWaiting(double time);
private:
void updateProbabilities();
bool checkProbabilities();
......
......@@ -368,38 +368,42 @@ void FloorfieldViaFMTrips::createMapEntryInLineToGoalID(const int goalID, bool i
long int dummykey;
if (isInside) {
for (const auto& loccross : crossings) {
std::cout << "Check crossing ID " << loccross.second->GetID() << " " << loccross.second->toString() << std::endl;
if ( !loccross.second->IsOpen()) {
continue;
}
dummykey = _grid->getKeyAtPoint(loccross.second->GetCentre());
if ((cost_of_MIN>localcostptr[dummykey]) && (localcostptr[dummykey]>=0.)) {
UID_of_MIN3 = UID_of_MIN2;
cost_of_MIN3 = cost_of_MIN2;
UID_of_MIN2 = UID_of_MIN;
cost_of_MIN2 = cost_of_MIN;
UID_of_MIN = loccross.second->GetUniqueID();
cost_of_MIN = localcostptr[dummykey];
std::cout << "Closer Line found: " << UID_of_MIN << std::endl;
continue;
}
if ((cost_of_MIN2>localcostptr[dummykey]) && (localcostptr[dummykey]>=0.)) {
UID_of_MIN3 = UID_of_MIN2;
cost_of_MIN3 = cost_of_MIN2;
UID_of_MIN2 = loccross.second->GetUniqueID();
cost_of_MIN2 = localcostptr[dummykey];
continue;
}
if ((cost_of_MIN3>localcostptr[dummykey]) && (localcostptr[dummykey]>=0.)) {
UID_of_MIN3 = loccross.second->GetUniqueID();
cost_of_MIN3 = localcostptr[dummykey];
continue;
}
}
// for (const auto& loccross : crossings) {
// std::cout << "Check crossing ID " << loccross.second->GetID() << " " << loccross.second->toString() << std::endl;
// if ( !loccross.second->IsOpen()) {
// continue;
// }
// dummykey = _grid->getKeyAtPoint(loccross.second->GetCentre());
// if ((cost_of_MIN>localcostptr[dummykey]) && (localcostptr[dummykey]>=0.)) {
// UID_of_MIN3 = UID_of_MIN2;
// cost_of_MIN3 = cost_of_MIN2;
//
// UID_of_MIN2 = UID_of_MIN;
// cost_of_MIN2 = cost_of_MIN;
//
// UID_of_MIN = loccross.second->GetUniqueID();
// cost_of_MIN = localcostptr[dummykey];
// std::cout << "Closer Line found: " << UID_of_MIN << " " << cost_of_MIN << std::endl;
// continue;
// }
// if ((cost_of_MIN2>localcostptr[dummykey]) && (localcostptr[dummykey]>=0.)) {
// UID_of_MIN3 = UID_of_MIN2;
// cost_of_MIN3 = cost_of_MIN2;
//
// UID_of_MIN2 = loccross.second->GetUniqueID();
// cost_of_MIN2 = localcostptr[dummykey];
// continue;
// }
// if ((cost_of_MIN3>localcostptr[dummykey]) && (localcostptr[dummykey]>=0.)) {
// UID_of_MIN3 = loccross.second->GetUniqueID();
// cost_of_MIN3 = localcostptr[dummykey];
// continue;
// }
//
// UID_of_MIN = 33;
//
// }
UID_of_MIN = allgoals.at(goalID)->GetCentreCrossing().GetUniqueID();
}
else {
for (const auto& loctrans : transitions) {
......
......@@ -114,20 +114,20 @@ UnivFFviaFMTrips::UnivFFviaFMTrips(Room* roomArg, Configuration* const confArg,
}
// //save all waiting area walls in tmpdoors
for (auto& goalMap : goals){
Goal* goal = goalMap.second;
if(WaitingArea* wa = dynamic_cast<WaitingArea*>(goal)) {
std::cout << "Testing for subroom " << subRoomPtr->GetSubRoomID() << std::endl;
if ((subRoomPtr->IsInSubRoom(wa->GetCentroid())) && (wa->isOpen())){
for (const Wall wall : wa->GetAllWalls()){
int uid = wall.GetUniqueID();
if (tmpDoors.count(uid) == 0) {
tmpDoors.emplace(std::make_pair(uid, (Line) wall));
}
}
}
}
}
// for (auto& goalMap : goals){
// Goal* goal = goalMap.second;
// if(WaitingArea* wa = dynamic_cast<WaitingArea*>(goal)) {
// std::cout << "Testing for subroom " << subRoomPtr->GetSubRoomID() << std::endl;
// if ((subRoomPtr->IsInSubRoom(wa->GetCentroid())) && (wa->isOpen())){
// for (const Wall wall : wa->GetAllWalls()){
// int uid = wall.GetUniqueID();
// if (tmpDoors.count(uid) == 0) {
// tmpDoors.emplace(std::make_pair(uid, (Line) wall));
// }
// }
// }
// }
// }
for (auto& door : tmpDoors){
std::cout << "uid: " << door.first << " door: " << door.second.toString() << std::endl;
......
......@@ -116,6 +116,11 @@ bool FFRouterTrips::Init(Building* building)
goalIDs.emplace_back(itrGoal.first);
}
_goalToLineUIDmap = _globalFF->getGoalToLineUIDmap();
for (auto& tmp : _goalToLineUIDmap) {
std::cout << "Goal: " << tmp.first << " Crossing: " << tmp.second << std::endl;
}
_goalToLineUIDmap2 = _globalFF->getGoalToLineUIDmap2();
_goalToLineUIDmap3 = _globalFF->getGoalToLineUIDmap3();
_globalFF->writeGoalFF("goal.vtk", goalIDs);
......@@ -153,34 +158,34 @@ bool FFRouterTrips::Init(Building* building)
if (room1) roomAndCroTrVector.emplace_back(std::make_pair(room1->GetID(), pair.second->GetUniqueID()));
}
}
for (auto& goalMap : allGoals) {
Goal* goal = goalMap.second;
if (WaitingArea* wa = dynamic_cast<WaitingArea*>(goal)) {
if (wa->isOpen()){
int roomID;
for (auto& room : _building->GetAllRooms()){
for (auto& subroom : room.second->GetAllSubRooms()){
if (subroom.second->IsInSubRoom(wa->GetCentroid())){
roomID = room.second->GetID();
}
}
}
for (const Wall wall : wa->GetAllWalls()) {
int uid = wall.GetUniqueID();
if (std::find(_allDoorUIDs.begin(), _allDoorUIDs.end(), uid) == _allDoorUIDs.end()) {
_allDoorUIDs.emplace_back(uid);
Crossing cross;
cross.SetPoint1(wall.GetPoint1());
cross.SetPoint2(wall.GetPoint2());
cross.SetCaption(wa->GetCaption());
_CroTrByUID.insert(std::make_pair(uid, &cross));
roomAndCroTrVector.emplace_back(roomID, uid);
}
}
}
}
}
// for (auto& goalMap : allGoals) {
// Goal* goal = goalMap.second;
// if (WaitingArea* wa = dynamic_cast<WaitingArea*>(goal)) {
// if (wa->isOpen()){
// int roomID;
// for (auto& room : _building->GetAllRooms()){
// for (auto& subroom : room.second->GetAllSubRooms()){
// if (subroom.second->IsInSubRoom(wa->GetCentroid())){
// roomID = room.second->GetID();
// }
// }
// }
//
// for (const Wall wall : wa->GetAllWalls()) {
// int uid = wall.GetUniqueID();
// if (std::find(_allDoorUIDs.begin(), _allDoorUIDs.end(), uid) == _allDoorUIDs.end()) {
// _allDoorUIDs.emplace_back(uid);
// Crossing cross;
// cross.SetPoint1(wall.GetPoint1());
// cross.SetPoint2(wall.GetPoint2());
// cross.SetCaption(wa->GetCaption());
// _CroTrByUID.insert(std::make_pair(uid, &cross));
// roomAndCroTrVector.emplace_back(roomID, uid);
// }
// }
// }
// }
// }
//make unique
std::sort(_allDoorUIDs.begin(), _allDoorUIDs.end());
......@@ -485,11 +490,18 @@ int FFRouterTrips::FindExit(Pedestrian* ped)
// Check if current position is already waiting area
// yes: set next goal and return findExit(p)
if (subroom->IsInSubRoom(goal->GetCentroid())){
if (goal->IsInsideGoal(ped->GetPos())){
std::cout << "Ped and Goal in same subroom: " << subroom->IsInSubRoom(goal->GetCentroid()) << std::endl;
std::cout << "Ped Final Destination before: " << ped->GetFinalDestination() << std::endl;
if(WaitingArea* wa = dynamic_cast<WaitingArea*>(goal)) {
ped->SetFinalDestination(wa->GetNextGoal());
//take the current time from the pedestrian
double t = Pedestrian::GetGlobalTime();
wa->addPed();
if (!wa->isWaiting(t)){
ped->SetFinalDestination(wa->GetNextGoal());
}
}
std::cout << "Ped Final Destination after: " << ped->GetFinalDestination() << std::endl;
ret = FindExit1(ped);
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment