diff --git a/src/general/Macros.h b/src/general/Macros.h
index c8d457c63fd6d882af2f3d0a3beb82d810f3c622..3fcfd0d231002a97544d43609dcd3ec5b8c5d287 100644
--- a/src/general/Macros.h
+++ b/src/general/Macros.h
@@ -1,8 +1,8 @@
/**
* \file Macros.h
* \date Jun 16, 2010
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
@@ -36,19 +36,23 @@
#include <string.h>
#include <algorithm>
#include <sstream>
+#include <iostream>
+
#ifndef M_PI
-#define M_PI 3.14159265358979323846
-#endif
+#define M_PI 3.14159265358979323846
+#endif
+
-#define _isnan(x) std::isnan(x)
// should be true only when using this file in the simulation core
//#define _SIMULATOR 1
+//#define _USE_PROTOCOL_BUFFER 1
+#define JPS_OLD_VERSION "0.5" // this version is still supported
+#define JPS_VERSION_MINOR "6"
+#define JPS_VERSION_MAJOR "0"
-#define JPS_VERSION "0.5"
-#define JPS_VERSION_MINOR 5
-#define JPS_VERSION_MAJOR 0
+#define JPS_VERSION JPS_VERSION_MAJOR "." JPS_VERSION_MINOR
// disable openmp in debug mode
#ifdef _NDEBUG
@@ -74,13 +78,13 @@
#define CLENGTH 1000
// conversion (cm <-> m)
-#define FAKTOR 100
+#define FAKTOR 1
// default final destination for the pedestrians
#define FINAL_DEST_OUT -1
// Linked cells
-#define LIST_EMPTY -1
+#define LIST_EMPTY -1
enum RoomState {
@@ -128,7 +132,11 @@ enum AgentColorMode {
BY_VELOCITY=1,
BY_KNOWLEDGE,
BY_ROUTE,
- BY_SPOTLIGHT
+ BY_ROUTER,
+ BY_SPOTLIGHT,
+ BY_GROUP,
+ BY_FINAL_GOAL,
+ BY_INTERMEDIATE_GOAL
};
//global functions for convenience
@@ -167,7 +175,7 @@ inline char xmltoc(const char * t, const char v = '\0')
* @return true if the element is present in the vector
*/
template<typename A>
-inline bool IsElementInVector(const std::vector<A> &vec, A& el) {
+inline bool IsElementInVector(const std::vector<A> &vec, const A& el) {
typename std::vector<A>::const_iterator it;
it = std::find (vec.begin(), vec.end(), el);
if(it==vec.end()) {
diff --git a/src/geometry/Building.cpp b/src/geometry/Building.cpp
index ea9c3e00a7aa61fe30292524ff51660cbe60f43f..89400ce07315130e0c4cc5f877b8d4768b913703 100644
--- a/src/geometry/Building.cpp
+++ b/src/geometry/Building.cpp
@@ -1,8 +1,8 @@
/**
* \file Building.cpp
* \date Oct 1, 2014
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
@@ -72,7 +72,7 @@ Building::Building(const std::string& filename, const std::string& rootDir, Rout
//todo: what happens if any of these methods failed (return false)? throw exception ?
this->LoadGeometry();
this->LoadRoutingInfo(filename);
- this->AddSurroundingRoom();
+ //this->AddSurroundingRoom();
this->InitGeometry();
this->LoadTrafficInfo();
distributor.Distribute(this);
@@ -156,7 +156,7 @@ int Building::GetNumberOfGoals() const
return _transitions.size() + _hLines.size() + _crossings.size();
}
-const std::map<int, std::unique_ptr<Room>>& Building::GetAllRooms() const
+const std::map<int, std::unique_ptr<Room> >& Building::GetAllRooms() const
{
return _rooms;
}
@@ -291,13 +291,32 @@ bool Building::InitGeometry()
//do the same for the obstacles that are closed
for(auto&& obst:itr_subroom.second->GetAllObstacles())
{
- if (obst->GetClosed() == 1)
- if(!obst->ConvertLineToPoly())
- return false;
+ //if (obst->GetClosed() == 1)
+ if(!obst->ConvertLineToPoly())
+ return false;
}
}
}
+ // look and save the neighbor subroom for improving the runtime
+ // that information is already present in the crossing/transitions
+
+ for(const auto & cross: _crossings)
+ {
+ SubRoom* s1=cross.second->GetSubRoom1();
+ SubRoom* s2=cross.second->GetSubRoom2();
+ if(s1) s1->AddNeighbor(s2);
+ if(s2) s2->AddNeighbor(s1);
+ }
+
+ for(const auto & trans: _transitions)
+ {
+ SubRoom* s1=trans.second->GetSubRoom1();
+ SubRoom* s2=trans.second->GetSubRoom2();
+ if(s1) s1->AddNeighbor(s2);
+ if(s2) s2->AddNeighbor(s1);
+ }
+
Log->Write("INFO: \tInit Geometry successful!!!\n");
return true;
@@ -377,7 +396,7 @@ bool Building::LoadGeometry(const std::string &geometryfile)
double version = xmltof(xRootNode->Attribute("version"), -1);
- if (version != std::stod(JPS_VERSION)) {
+ if (version != std::stod(JPS_VERSION) && version != std::stod(JPS_OLD_VERSION)) {
Log->Write(" \tWrong geometry version!");
Log->Write(" \tOnly version >= %s supported",JPS_VERSION);
Log->Write(" \tPlease update the version of your geometry file to %s",JPS_VERSION);
@@ -485,13 +504,13 @@ bool Building::LoadGeometry(const std::string &geometryfile)
int id = xmltof(xObstacle->Attribute("id"), -1);
int height = xmltof(xObstacle->Attribute("height"), 0);
- double ObstClosed = xmltof(xObstacle->Attribute("closed"), 0);
+ //double ObstClosed = xmltof(xObstacle->Attribute("closed"), 0);
string ObstCaption = xmltoa(xObstacle->Attribute("caption"),"-1");
Obstacle* obstacle = new Obstacle();
obstacle->SetId(id);
obstacle->SetCaption(ObstCaption);
- obstacle->SetClosed(ObstClosed);
+ //obstacle->SetClosed(ObstClosed);
obstacle->SetHeight(height);
//looking for polygons (walls)
@@ -843,26 +862,37 @@ SubRoom* Building::GetSubRoomByUID( int uid)
return NULL;
}
-bool Building::IsVisible(Line* l1, Line* l2, bool considerHlines)
+//bool Building::IsVisible(Line* l1, Line* l2, bool considerHlines)
+//{
+//
+// for(auto&& itr_room: _rooms)
+// {
+// for(auto&& itr_subroom: itr_room.second->GetAllSubRooms())
+// {
+// if(itr_subroom.second->IsVisible(l1,l2,considerHlines)==false) return false;
+// }
+// }
+// return true;
+//}
+
+bool Building::IsVisible(const Point& p1, const Point& p2, const std::vector<SubRoom*>& subrooms, bool considerHlines)
{
-
- for(auto&& itr_room: _rooms)
+ //loop over all subrooms if none is provided
+ if (subrooms.empty())
{
- for(auto&& itr_subroom: itr_room.second->GetAllSubRooms())
+ for(auto&& itr_room: _rooms)
{
- if(itr_subroom.second->IsVisible(l1,l2,considerHlines)==false) return false;
+ for(auto&& itr_subroom: itr_room.second->GetAllSubRooms())
+ {
+ if(itr_subroom.second->IsVisible(p1,p2,considerHlines)==false) return false;
+ }
}
}
- return true;
-}
-
-bool Building::IsVisible(const Point& p1, const Point& p2, bool considerHlines)
-{
- for(auto&& itr_room: _rooms)
+ else
{
- for(auto&& itr_subroom: itr_room.second->GetAllSubRooms())
+ for(auto&& sub: subrooms)
{
- if(itr_subroom.second->IsVisible(p1,p2,considerHlines)==false) return false;
+ if(sub and sub->IsVisible(p1,p2,considerHlines)==false) return false;
}
}
@@ -937,7 +967,6 @@ void Building::InitGrid(double cellSize)
y_max = y_max + 1*cellSize;
double boundaries[4] = { x_min, x_max, y_min, y_max };
- int pedsCount = _allPedestians.size();
//no algorithms
// the domain is made of a single cell
@@ -953,7 +982,8 @@ void Building::InitGrid(double cellSize)
Log->Write("INFO: \tInitializing the grid with cell size: %f ", cellSize);
}
- _linkedCellGrid = new LCGrid(boundaries, cellSize, pedsCount);
+ //_linkedCellGrid = new LCGrid(boundaries, cellSize, _allPedestians.size());
+ _linkedCellGrid = new LCGrid(boundaries, cellSize, Pedestrian::GetAgentsCreated());
_linkedCellGrid->ShallowCopy(_allPedestians);
Log->Write("INFO: \tDone with Initializing the grid ");
@@ -980,6 +1010,10 @@ bool Building::LoadRoutingInfo(const string &filename)
return false;
}
+ if (! xRootNode->FirstChild("routing"))
+ {
+ return true; // no extra routing information
+ }
//load goals and routes
TiXmlNode* xGoalsNode = xRootNode->FirstChild("routing")->FirstChild("goals");
@@ -1135,16 +1169,12 @@ void Building::DeletePedestrian(Pedestrian* &ped)
_allPedestians.erase(it);
int nowPeds= _allPedestians.size();
- //if((*it)->GetID()==69){
- //cout << "rescued agent: " << (*it)->GetID()<<endl;
- //cout << "want to rescue agent: " << ped->GetID()<<endl<<endl;
- // exit(0);
- // }
Log->ProgressBar(totalPeds, totalPeds-nowPeds);
}
//update the stats before deleting
Transition* trans =GetTransitionByUID(ped->GetExitIndex());
- if(trans) {
+ if(trans)
+ {
trans->IncreaseDoorUsage(1, ped->GetGlobalTime());
}
delete ped;
@@ -1169,9 +1199,12 @@ void Building::AddPedestrian(Pedestrian* ped)
void Building::GetPedestrians(int room, int subroom, std::vector<Pedestrian*>& peds) const
{
- for(unsigned int p = 0;p<_allPedestians.size();p++){
- Pedestrian* ped=_allPedestians[p];
- if(room==ped->GetRoomID() && subroom==ped->GetSubRoomID())
+ //for(unsigned int p = 0;p<_allPedestians.size();p++){
+ // Pedestrian* ped=_allPedestians[p];
+
+ for (auto&& ped : _allPedestians)
+ {
+ if ((room == ped->GetRoomID()) && (subroom == ped->GetSubRoomID()))
{
peds.push_back(ped);
}
@@ -1271,7 +1304,7 @@ bool Building::SaveGeometry(const std::string &filename)
auto&& sub=itr_sub.second;
const double* plane=sub->GetPlaneEquation();
geometry<<"\t\t<subroom id =\""<<sub->GetSubRoomID()
- <<"\" closed=\""<<sub->GetClosed()
+ <<"\" closed=\""<<0
<<"\" class=\""<<sub->GetType()
<<"\" A_x=\""<<plane[0]
<<"\" B_y=\""<<plane[1]
diff --git a/src/geometry/Building.h b/src/geometry/Building.h
index 9e876244283bff38fc960fdd8e239fd52d80f3b1..109d79866c9a867e2e0939d01d95c3649d593b1d 100644
--- a/src/geometry/Building.h
+++ b/src/geometry/Building.h
@@ -1,8 +1,8 @@
/**
* \file Building.h
* \date Oct 1, 2010
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
@@ -89,7 +89,7 @@ public:
std::string GetCaption() const;
RoutingEngine* GetRoutingEngine() const;
- const std::map<int, std::unique_ptr<Room>>& GetAllRooms() const;
+ const std::map<int, std::unique_ptr<Room> >& GetAllRooms() const;
const std::vector<Pedestrian*>& GetAllPedestrians() const;
Pedestrian* GetPedestrian( int pedID) const;
int GetNumberOfRooms() const;
@@ -122,14 +122,14 @@ public:
* Alls walls and transitions and crossings are used in this check.
* The use of hlines is optional, because they are not real, can can be considered transparent
*/
- bool IsVisible(Line* l1, Line* l2, bool considerHlines=false);
+ //bool IsVisible(Line* l1, Line* l2, bool considerHlines=false);
/**
* @return true if the two points are visible from each other.
* Alls walls and transitions and crossings are used in this check.
* The use of hlines is optional, because they are not real, can be considered transparent
*/
- bool IsVisible(const Point& p1, const Point& p2, bool considerHlines=false);
+ bool IsVisible(const Point& p1, const Point& p2, const std::vector<SubRoom*>& subrooms, bool considerHlines=false);
/**
* @return a crossing or a transition matching the given caption.
diff --git a/src/geometry/Crossing.cpp b/src/geometry/Crossing.cpp
index f4ce45a108009cbdf2a99601c5527ad0442b35b2..58b50716c4085d1ee396aa61533d3bfa2795b20a 100644
--- a/src/geometry/Crossing.cpp
+++ b/src/geometry/Crossing.cpp
@@ -1,8 +1,8 @@
/**
* \file Crossing.cpp
* \date Nov 16, 2010
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
@@ -36,9 +36,6 @@ using namespace std;
Crossing::Crossing()
{
_id = -1;
- _room1 = NULL;
- _subRoom1 = NULL;
- _subRoom2 = NULL;
}
Crossing::~Crossing()
@@ -50,10 +47,10 @@ void Crossing::SetID(int ID)
_id = ID;
}
-void Crossing::SetSubRoom2(SubRoom* r2)
-{
- _subRoom2 = r2;
-}
+//void Crossing::SetSubRoom2(SubRoom* r2)
+//{
+// _subRoom2 = r2;
+//}
// Getter-Funktionen
@@ -62,10 +59,10 @@ int Crossing::GetID() const
return _id;
}
-SubRoom* Crossing::GetSubRoom2() const
-{
- return _subRoom2;
-}
+//SubRoom* Crossing::GetSubRoom2() const
+//{
+// return _subRoom2;
+//}
// Sonstiges
diff --git a/src/geometry/Crossing.h b/src/geometry/Crossing.h
index a2802b32be74fe270f57c5118ebc0e3c55e8cc10..3dc404650cd3895762d98fc4f192c7cb3a974a32 100644
--- a/src/geometry/Crossing.h
+++ b/src/geometry/Crossing.h
@@ -1,8 +1,8 @@
/**
* \file Crossing.h
* \date Nov 16, 2010
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
@@ -41,33 +41,42 @@ private:
/// TODO ? unique between crossings and transitions ?
int _id;
/// only one room needed, since a crossing only separates 2 subrooms
- Room* _room1;
- SubRoom* _subRoom1;
- SubRoom* _subRoom2;
+ //Room* _room1;
+ //SubRoom* _subRoom1;
+ //SubRoom* _subRoom2;
public:
+ /**
+ * Constructor
+ */
Crossing();
+
+ /**
+ * Destructor
+ */
virtual ~Crossing();
/**
* Set/Get the Id of the crossing
+ * TODO: do you really want to shadow ?
*/
void SetID(int ID);
/**
* Set/Get the Id of the crossing
+ * TODO: do you really want to shadow ?
*/
int GetID () const;
/**
* Set/Get the second subroom
*/
- void SetSubRoom2(SubRoom* r2);
+ //void SetSubRoom2(SubRoom* r2);
/**
* Set/Get the second subroom
*/
- SubRoom* GetSubRoom2() const;
+ //SubRoom* GetSubRoom2() const;
/**
* \return true if the subroomID is associated with the current crossing
diff --git a/src/geometry/Goal.cpp b/src/geometry/Goal.cpp
index ff6c2705b03062d4478569e2fdcf2e99163f0fe5..77b80b78dd5dfa1827e6e22c06c30853185b7951 100644
--- a/src/geometry/Goal.cpp
+++ b/src/geometry/Goal.cpp
@@ -1,8 +1,8 @@
/**
* \file Goal.cpp
* \date Spe 12, 2013
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
diff --git a/src/geometry/Goal.h b/src/geometry/Goal.h
index cdd335e8105d71dec242538267508c33dd5914ba..9aceda34bb7698db2d0f2d220088a5b94be4c436 100644
--- a/src/geometry/Goal.h
+++ b/src/geometry/Goal.h
@@ -1,8 +1,8 @@
/**
* \file Goal.h
* \date Sep 12, 2013
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
diff --git a/src/geometry/Hline.cpp b/src/geometry/Hline.cpp
index 795d685220ade115fd8db8e96b888f361cc89718..dc530d507fb5da01b348d88f044561e805381e0e 100644
--- a/src/geometry/Hline.cpp
+++ b/src/geometry/Hline.cpp
@@ -1,8 +1,8 @@
/**
* \file Hline.cpp
* \date Aug 1, 2012
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
@@ -33,8 +33,6 @@ using namespace std;
Hline::Hline()
{
- _room1=NULL;
- _subRoom1=NULL;
_id=-1;
}
@@ -82,6 +80,17 @@ SubRoom* Hline::GetSubRoom1() const
return _subRoom1;
}
+void Hline::SetSubRoom2(SubRoom* r2)
+{
+ _subRoom2 = r2;
+}
+
+
+SubRoom* Hline::GetSubRoom2() const
+{
+ return _subRoom2;
+}
+
bool Hline::IsInSubRoom(int subroomID) const
{
return _subRoom1->GetSubRoomID() == subroomID;
@@ -110,7 +119,7 @@ string Hline::GetDescription() const
{
string geometry;
char tmp[CLENGTH] = "";
- sprintf(tmp,"\t\t<hline ID=\"%d\" color = \"250\" caption=\"h_%d_%d\">\n",GetUniqueID(),GetID(),GetUniqueID());
+ sprintf(tmp,"\t\t<hline ID=\"%d\" room_id=\"%d\" subroom_id=\"%d\" color = \"250\" caption=\"h_%d_%d\">\n",GetUniqueID(),_room1->GetID(),_subRoom1->GetSubRoomID(),GetID(),GetUniqueID());
geometry.append(tmp);
//geometry.append("\t\t<door color=\"250\">\n");
sprintf(tmp, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\" zPos=\"%.2f\"/>\n",
diff --git a/src/geometry/Hline.h b/src/geometry/Hline.h
index e2c2b8cf0a11bbf54020720cc41a027fde020272..500361032691c3ea7bbfa83ec4c1799f66e1bad4 100644
--- a/src/geometry/Hline.h
+++ b/src/geometry/Hline.h
@@ -1,8 +1,8 @@
/**
* \file Hline.h
* \date Aug 1, 2012
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
@@ -40,12 +40,24 @@ class Hline: public NavLine {
private:
int _id;
- Room* _room1;
std::string _caption;
- SubRoom* _subRoom1;
+
+//TODO: these should be redesigned as private
+//and accessed via functions in the derived classes.
+protected:
+ Room* _room1=nullptr;
+ SubRoom* _subRoom1=nullptr;
+ SubRoom* _subRoom2=nullptr;
public:
+ /**
+ * Constructor
+ */
Hline();
+
+ /**
+ * Destructor
+ */
virtual ~Hline();
/**
@@ -67,6 +79,10 @@ public:
* Set/Get the subroom containing this line
*/
void SetSubRoom1(SubRoom* r);
+ /**
+ * Set/Get the second subroom
+ */
+ void SetSubRoom2(SubRoom* r2);
/**
* Set/Get the id of the line
@@ -88,6 +104,11 @@ public:
*/
SubRoom* GetSubRoom1() const;
+ /**
+ * Set/Get the second subroom
+ */
+ SubRoom* GetSubRoom2() const;
+
/**
* @return true if the line is in the given subroom
*/
diff --git a/src/geometry/Line.cpp b/src/geometry/Line.cpp
index e6383fdd0cf04d3b814348a9a8f9d1f30f751772..1df68720eaedde5e6bcd6df33c7a72aa3ae16cb2 100644
--- a/src/geometry/Line.cpp
+++ b/src/geometry/Line.cpp
@@ -1,8 +1,8 @@
/**
* \file Line.cpp
* \date Sep 30, 2010
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
@@ -33,99 +33,87 @@
#include "../IO/OutputHandler.h"
-
#include <cmath>
#include <sstream>
-int Line::_static_UID=0;
+int Line::_static_UID = 0;
using namespace std;
/************************************************************
Konstruktoren
************************************************************/
-Line::Line()
-{
- SetPoint1(Point()); //Default-Constructor (0.0,0.0)
- SetPoint2(Point());
- _uid = _static_UID++;
+Line::Line() {
+ SetPoint1(Point()); //Default-Constructor (0.0,0.0)
+ SetPoint2(Point());
+ _uid = _static_UID++;
}
-Line::Line(const Point& p1, const Point& p2)
-{
- SetPoint1(p1);
- SetPoint2(p2);
- _uid = _static_UID++;
+Line::Line(const Point &p1, const Point &p2) {
+ SetPoint1(p1);
+ SetPoint2(p2);
+ _uid = _static_UID++;
}
-int Line::GetUniqueID() const
-{
- return _uid;
+int Line::GetUniqueID() const {
+ return _uid;
}
-Line::Line(const Line& orig)
-{
- _point1 = orig.GetPoint1();
- _point2 = orig.GetPoint2();
- _centre = orig.GetCentre();
- _uid = orig.GetUniqueID();
+Line::Line(const Line &orig) {
+ _point1 = orig.GetPoint1();
+ _point2 = orig.GetPoint2();
+ _centre = orig.GetCentre();
+ _uid = orig.GetUniqueID();
}
-Line::~Line()
-{
+Line::~Line() {
}
/*************************************************************
Setter-Funktionen
************************************************************/
-void Line::SetPoint1(const Point& p)
-{
- _point1 = p;
- _centre = (_point1+_point2)*0.5;
+void Line::SetPoint1(const Point &p) {
+ _point1 = p;
+ _centre = (_point1 + _point2) * 0.5;
}
-void Line::SetPoint2(const Point& p)
-{
- _point2 = p;
- _centre = (_point1+_point2)*0.5;
+void Line::SetPoint2(const Point &p) {
+ _point2 = p;
+ _centre = (_point1 + _point2) * 0.5;
}
/*************************************************************
Getter-Funktionen
************************************************************/
-const Point& Line::GetPoint1() const
-{
- return _point1;
+const Point &Line::GetPoint1() const {
+ return _point1;
}
-const Point& Line::GetPoint2() const
-{
- return _point2;
+const Point &Line::GetPoint2() const {
+ return _point2;
}
-const Point& Line::GetCentre()const
-{
- return _centre;
+const Point &Line::GetCentre() const {
+ return _centre;
}
/*************************************************************
Ausgabe
************************************************************/
-string Line::Write() const
-{
- string geometry;
- char wall[500] = "";
- geometry.append("\t\t<wall color=\"100\">\n");
- sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\"/>\n",
- (GetPoint1().GetX()) * FAKTOR,
- (GetPoint1().GetY()) * FAKTOR);
- geometry.append(wall);
- sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\"/>\n",
- (GetPoint2().GetX()) * FAKTOR,
- (GetPoint2().GetY()) * FAKTOR);
- geometry.append(wall);
- geometry.append("\t\t</wall>\n");
- return geometry;
+string Line::Write() const {
+ string geometry;
+ char wall[500] = "";
+ geometry.append("\t\t<wall color=\"100\">\n");
+ sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\"/>\n",
+ (GetPoint1().GetX()) * FAKTOR,
+ (GetPoint1().GetY()) * FAKTOR);
+ geometry.append(wall);
+ sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\"/>\n",
+ (GetPoint2().GetX()) * FAKTOR,
+ (GetPoint2().GetY()) * FAKTOR);
+ geometry.append(wall);
+ geometry.append("\t\t</wall>\n");
+ return geometry;
}
@@ -134,160 +122,111 @@ string Line::Write() const
************************************************************/
// Normalen vector zur Linie
-Point Line::NormalVec() const
-{
- double nx, ny, norm;
- Point r = GetPoint2() - GetPoint1();
-
- if (r.GetX() == 0.0) {
- nx = 1;
- ny = 0;
- } else {
- nx = -r.GetY() / r.GetX();
- ny = 1;
- /* Normieren */
- norm = sqrt(nx * nx + ny * ny);
- if (fabs(norm) < J_EPS) {
- Log->Write("ERROR: \tLine::NormalVec() norm==0\n");
- exit(0);
- }
- nx /= norm;
- ny /= norm;
- }
- return Point(nx, ny);
+Point Line::NormalVec() const {
+ double nx, ny, norm;
+ Point r = GetPoint2() - GetPoint1();
+
+ if (r.GetX() == 0.0) {
+ nx = 1;
+ ny = 0;
+ } else {
+ nx = -r.GetY() / r.GetX();
+ ny = 1;
+ /* Normieren */
+ norm = sqrt(nx * nx + ny * ny);
+ if (fabs(norm) < J_EPS) {
+ Log->Write("ERROR: \tLine::NormalVec() norm==0\n");
+ exit(0);
+ }
+ nx /= norm;
+ ny /= norm;
+ }
+ return Point(nx, ny);
}
// Normale Komponente von v auf l
-double Line::NormalComp(const Point& v) const
-{
- // Normierte Vectoren
- Point l = (GetPoint2() - GetPoint1()).Normalized();
- const Point& n = NormalVec();
-
-
- double lx = l.GetX();
- double ly = l.GetY();
- double nx = n.GetX();
- double ny = n.GetY();
- double alpha;
-
- if (fabs(lx) < J_EPS) {
- alpha = v.GetX() / nx;
- } else if (fabs(ly) < J_EPS) {
- alpha = v.GetY() / ny;
- } else {
- alpha = (v.GetY() * lx - v.GetX() * ly) / (nx * ly - ny * lx);
- }
+double Line::NormalComp(const Point &v) const {
+ // Normierte Vectoren
+ Point l = (GetPoint2() - GetPoint1()).Normalized();
+ const Point &n = NormalVec();
+
+
+ double lx = l.GetX();
+ double ly = l.GetY();
+ double nx = n.GetX();
+ double ny = n.GetY();
+ double alpha;
- return fabs(alpha);
+ if (fabs(lx) < J_EPS) {
+ alpha = v.GetX() / nx;
+ } else if (fabs(ly) < J_EPS) {
+ alpha = v.GetY() / ny;
+ } else {
+ alpha = (v.GetY() * lx - v.GetX() * ly) / (nx * ly - ny * lx);
+ }
+
+ return fabs(alpha);
}
// Lotfußpunkt zur Geraden Line
// Muss nicht im Segment liegen
-Point Line::LotPoint(const Point& p) const
-{
- const Point& r = GetPoint1();
- const Point& s = GetPoint2();
- const Point& t = r - s;
- Point tmp;
- double lambda;
+Point Line::LotPoint(const Point &p) const {
+ const Point &r = GetPoint1();
+ const Point &s = GetPoint2();
+ const Point &t = r - s;
+ Point tmp;
+ double lambda;
- tmp = p - s;
- lambda = tmp.ScalarP(t) / t.ScalarP(t);
- Point f = s + t*lambda;
- return f;
+ tmp = p - s;
+ lambda = tmp.ScalarProduct(t) / t.ScalarProduct(t);
+ Point f = s + t * lambda;
+ return f;
}
/* Punkt auf der Linie mit kürzestem Abstand zu p
* In der Regel Lotfußpunkt, Ist der Lotfußpunkt nicht im Segment
* wird der entsprechende Eckpunkt der Line genommen
* */
-Point Line::ShortestPoint(const Point& p) const
-{
+Point Line::ShortestPoint(const Point &p) const {
- const Point& t = _point1 - _point2;
- if(_point1 == _point2)
- return _point1;
- Point tmp = p - _point2;
- double lambda = tmp.ScalarP(t) / t.ScalarP(t);
- Point f = _point2 + t*lambda;
+ const Point &t = _point1 - _point2;
+ if (_point1 == _point2)
+ return _point1;
+ Point tmp = p - _point2;
+ double lambda = tmp.ScalarProduct(t) / t.ScalarProduct(t);
+ Point f = _point2 + t * lambda;
- /* Prüfen ob Punkt in der Linie,sonst entsprechenden Eckpunkt zurückgeben */
- if (lambda < 0)
- f = _point2;
- if (lambda > 1)
- f = _point1;
+ /* Prüfen ob Punkt in der Linie,sonst entsprechenden Eckpunkt zurückgeben */
+ if (lambda < 0)
+ f = _point2;
+ if (lambda > 1)
+ f = _point1;
- return f;
+ return f;
}
-/* Prüft, ob Punkt p im Liniensegment enthalten ist
- * Verfahren wie bei Line::ShortestPoint(), d. h,
- * lambda berechnen und prüfen ob zwischen 0 und 1
- * */
-//bool Line::IsInLine(const Point& p) const {
-// double ax, ay, bx, by, px, py;
-// const Point& a = GetPoint1();
-// const Point& b = GetPoint2();
-// double lambda;
-// ax = a.GetX();
-// ay = a.GetY();
-// bx = b.GetX();
-// by = b.GetY();
-// px = p.GetX();
-// py = p.GetY();
-// if (fabs(ax - bx) > J_EPS_DIST) {
-// lambda = (px - ax) / (bx - ax);
-// } else if (fabs(ay - by) > J_EPS_DIST) {
-// lambda = (py - ay) / (by - ay);
-// } else {
-// Log->Write("ERROR: \tIsInLine: Endpunkt = Startpunkt!!!");
-// exit(0);
-// }
-// return (0 <= lambda) && (lambda <= 1);
-//}
-
/*
* Prüft, ob Punkt p im Liniensegment enthalten ist
* algorithm from:
* http://stackoverflow.com/questions/328107/how-can-you-determine-a-point-is-between-two-other-points-on-a-line-segment
+ *
* */
-bool Line::IsInLineSegment(const Point& p) const
+bool Line::IsInLineSegment(const Point &p) const
{
- //const Point& _point1 = GetPoint1();
- //const Point& _point2 = GetPoint2();
- double ax = _point1.GetX();
- double ay = _point1.GetY();
- double bx = _point2.GetX();
- double by = _point2.GetY();
- double px = p._x;
- double py = p._y;
-
- // cross product to check if point i colinear
- double crossp = (py-ay)*(bx-ax)-(px-ax)*(by-ay);
- if(fabs(crossp) > J_EPS) return false;
-
- // dotproduct and distSquared to check if point is in segment and not just in line
- double dotp = (px-ax)*(bx-ax)+(py-ay)*(by-ay);
- if(dotp < 0 || (_point1-_point2).NormSquare() < dotp) return false;
-
- return true;
-
+ return fabs( (_point1-p ).Norm() + (_point2-p ).Norm() - (_point2-_point1 ).Norm() )<J_EPS;
}
/* Berechnet direkt den Abstand von p zum Segment l
* dazu wird die Funktion Line::ShortestPoint()
* benuzt
* */
-double Line::DistTo(const Point& p) const
-{
- return (p - ShortestPoint(p)).Norm();
+double Line::DistTo(const Point &p) const {
+ return (p - ShortestPoint(p)).Norm();
}
-double Line::DistToSquare(const Point& p) const
-{
- return (p - ShortestPoint(p)).NormSquare();
+double Line::DistToSquare(const Point &p) const {
+ return DistTo(p) * DistTo(p);
}
// bool Line::operator*(const Line& l) const {
@@ -299,271 +238,262 @@ double Line::DistToSquare(const Point& p) const
/* Zwei Linien sind gleich, wenn ihre beiden Punkte
* gleich sind
* */
-bool Line::operator==(const Line& l) const
-{
- return ((_point1 == l.GetPoint1() && _point2 == l.GetPoint2()) ||
- (_point2 == l.GetPoint1() && _point1 == l.GetPoint2()));
+bool Line::operator==(const Line &l) const {
+ return ((_point1 == l.GetPoint1() && _point2 == l.GetPoint2()) ||
+ (_point2 == l.GetPoint1() && _point1 == l.GetPoint2()));
}
/* Zwei Linien sind ungleich, wenn ihre beiden Punkte
* ungleich sind.
* */
-bool Line::operator!=(const Line& l) const
-{
- return (! this->operator ==(l) );
+bool Line::operator!=(const Line &l) const {
+ return (!(*this==l));
}
-double Line::Length() const
-{
- return (_point1 - _point2).Norm();
+double Line::Length() const {
+ return (_point1 - _point2).Norm();
}
-double Line::LengthSquare() const
-{
- return (_point1 - _point2).NormSquare();
+double Line::LengthSquare() const {
+ return (_point1 - _point2).NormSquare();
}
-bool Line::IntersectionWith(const Point &p1, const Point &p2) const
+//TODO unit test
+bool Line::Overlapp(const Line& l) const
{
- double deltaACy = _point1.GetY() - p1.GetY();
- double deltaDCx = p2.GetX() - p1.GetX();
- double deltaACx = _point1.GetX() - p1.GetX();
- double deltaDCy = p2.GetY() - p1.GetY();
- double deltaBAx = _point2.GetX() - _point1.GetX();
- double deltaBAy = _point2.GetY() - _point1.GetY();
- double denominator = deltaBAx * deltaDCy - deltaBAy * deltaDCx;
- double numerator = deltaACy * deltaDCx - deltaACx * deltaDCy;
-
- double r = numerator / denominator;
- if (r < 0.0 || r > 1.0) {
- return false;
- }
-
- double s = (deltaACy * deltaBAx - deltaACx * deltaBAy) / denominator;
- if (s < 0.0 || s > 1.0) {
- return false;
- }
-
+ //first check if they are colinear
+ Point vecAB=l.GetPoint2()-l.GetPoint1();
+ Point vecDC=_point1-_point2;
+ if(fabs(vecAB.Determinant(vecDC))<J_EPS)
+ {
+
+ if( IsInLineSegment(l.GetPoint1()) and not HasEndPoint(l.GetPoint1()))
+ {
+ //Log->Write("ERROR: 1. Overlapping walls %s and %s ", toString().c_str(),l.toString().c_str());
+ return true;
+ }
+
+ if( IsInLineSegment(l.GetPoint2()) and not HasEndPoint(l.GetPoint2()))
+ {
+ //Log->Write("ERROR: 2. Overlapping walls %s and %s ", toString().c_str(),l.toString().c_str());
+ return true;
+ }
+ }
+ return false;
+}
+
+//FIXME no equals check with == on double or float bring in an epsilon
+bool Line::IntersectionWith(const Point &p1, const Point &p2) const {
+ Point AC = _point1 - p1;
+ Point DC = p2 - p1;
+ Point BA = _point2 - _point1;
+ double denominator = BA.CrossProduct(DC);
+ double numerator = DC.CrossProduct(AC);
+
if (denominator == 0.0) {
-
+
// the lines are superposed
if (numerator == 0.0) {
-
+
// the segment are superposed
- if(IsInLineSegment(p1) ||
- IsInLineSegment(p2) ) return true;
- else return false;
-
+ return IsInLineSegment(p1) || IsInLineSegment(p2);
+
} else { // the lines are just parallel and do not share a common point
-
+
return false;
}
}
-
+
+ double r = numerator / denominator;
+ if (r < 0.0 || r > 1.0) {
+ return false;
+ }
+
+ double s = (BA.CrossProduct(AC)) / denominator;
+ if (s < 0.0 || s > 1.0) {
+ return false;
+ }
+
return true;
-
+
}
-bool Line::IntersectionWith(const Line& l) const
-{
- return this->IntersectionWith(l._point1, l._point2);
+bool Line::IntersectionWith(const Line &l) const {
+ return IntersectionWith(l._point1, l._point2);
}
-Line Line::enlarge(double d) const
-{
- const Point& p1 = _point1;
- const Point& p2 = _point2;
- Point diff = (p1 - p2).Normalized() * d;
+Line Line::Enlarge(double d) const {
+ const Point &p1 = _point1;
+ const Point &p2 = _point2;
+ Point diff = (p1 - p2).Normalized() * d;
- return Line(p1 + diff, p2 - diff);
+ return Line(p1 + diff, p2 - diff);
}
-bool Line::IsHorizontal()
-{
- return fabs (_point1._y-_point2._y ) <= J_EPS;
+bool Line::IsHorizontal() {
+ return fabs(_point1._y - _point2._y) <= J_EPS;
}
-bool Line::IsVertical()
-{
- return fabs (_point1._x-_point2._x ) <= J_EPS;
+bool Line::IsVertical() {
+ return fabs(_point1._x - _point2._x) <= J_EPS;
}
-int Line::WichSide(const Point& pt)
-{
- //special case for horizontal lines
- if (IsVertical()) {
- //left
- if (pt._x < _point1._x)
- return 0;
- //right or colinear
- if (pt._x >= _point1._x)
- return 1;
- }
+int Line::WichSide(const Point &pt) {
- return ((_point2._x - _point1._x) * (pt._y - _point1._y)
- - (_point2._y - _point1._y) * (pt._x - _point1._x)) > 0;
+ if(IsLeft(pt)) return 0;
+ return 1;
}
-bool Line::ShareCommonPointWith(const Line& line) const
-{
- if(line.GetPoint1()==_point1) return true;
- if(line.GetPoint2()==_point1) return true;
+bool Line::ShareCommonPointWith(const Line &line) const {
+ if (line.GetPoint1() == _point1) return true;
+ if (line.GetPoint2() == _point1) return true;
- if(line.GetPoint1()==_point2) return true;
- if(line.GetPoint2()==_point2) return true;
+ if (line.GetPoint1() == _point2) return true;
+ return line.GetPoint2() == _point2;
- return false;
}
-bool Line::HasEndPoint(const Point& point) const
-{
- if (_point1==point) return true;
- if (_point2==point) return true;
- return false;
+bool Line::HasEndPoint(const Point &point) const {
+ if (_point1 == point) return true;
+ return _point2 == point;
}
-bool Line::IntersectionWithCircle(const Point& centre, double radius /*cm for pedestrians*/)
-{
+bool Line::IntersectionWithCircle(const Point ¢re, double radius /*cm for pedestrians*/) {
- double r=radius;
- double x1=_point1.GetX();
- double y1=_point1.GetY();
+ double r = radius;
+ double x1 = _point1.GetX();
+ double y1 = _point1.GetY();
- double x2=_point2.GetX();
- double y2=_point2.GetY();
+ double x2 = _point2.GetX();
+ double y2 = _point2.GetY();
- double xc=centre.GetX();
- double yc=centre.GetY();
+ double xc = centre.GetX();
+ double yc = centre.GetY();
- //this formula assumes that the circle is centered the origin.
- // so we translate the complete stuff such that the circle ends up at the origin
- x1=x1-xc;
- y1=y1-yc;
- x2=x2-xc;
- y2=y2-yc;
- //xc=xc-xc;yc=yc-yc; to make it perfect
+ //this formula assumes that the circle is centered the origin.
+ // so we translate the complete stuff such that the circle ends up at the origin
+ x1 = x1 - xc;
+ y1 = y1 - yc;
+ x2 = x2 - xc;
+ y2 = y2 - yc;
+ //xc=xc-xc;yc=yc-yc; to make it perfect
- // we first check the intersection of the circle and the infinite line defined by the segment
- double dr2=((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1));
- double D2=(x1*y2-x2*y1)*(x1*y2-x2*y1);
- double r2=radius*radius;
+ // we first check the intersection of the circle and the infinite line defined by the segment
+ double dr2 = ((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
+ double D2 = (x1 * y2 - x2 * y1) * (x1 * y2 - x2 * y1);
+ double r2 = radius * radius;
- double delta=r2*dr2-D2;
- if(delta<=0.0) return false;
+ double delta = r2 * dr2 - D2;
+ if (delta <= 0.0) return false;
- double a=(x2-x1)*(x2-x1)+(y2-y1)*(y2-y1);
- double b=2*((x1*(x2-x1))+y1*(y2-y1));
- double c=x1*x1+y1*y1-r*r;
+ double a = (x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1);
+ double b = 2 * ((x1 * (x2 - x1)) + y1 * (y2 - y1));
+ double c = x1 * x1 + y1 * y1 - r * r;
- delta=b*b-4*a*c;
+ delta = b * b - 4 * a * c;
- if((x1==x2)&&(y1==y2)) {
- Log->Write("isLineCrossingCircle: Your line is a point");
- return false;
- }
- if(delta<0.0) {
- char tmp[CLENGTH];
- sprintf(tmp,"there is a bug in 'isLineCrossingCircle', delta(%f) can t be <0 at this point.",delta);
- Log->Write(tmp);
- Log->Write("press ENTER");
- return false; //fixme
- //getc(stdin);
- }
+ if ((x1 == x2) && (y1 == y2)) {
+ Log->Write("isLineCrossingCircle: Your line is a point");
+ return false;
+ }
+ if (delta < 0.0) {
+ char tmp[CLENGTH];
+ sprintf(tmp, "there is a bug in 'isLineCrossingCircle', delta(%f) can t be <0 at this point.", delta);
+ Log->Write(tmp);
+ Log->Write("press ENTER");
+ return false; //fixme
+ //getc(stdin);
+ }
- double t1= (-b + sqrt(delta))/(2*a);
- double t2= (-b - sqrt(delta))/(2*a);
- if((t1<0.0) || (t1>1.0)) return false;
- if((t2<0.0) || (t2>1.0)) return false;
- return true;
+ double t1 = (-b + sqrt(delta)) / (2 * a);
+ double t2 = (-b - sqrt(delta)) / (2 * a);
+ if ((t1 < 0.0) || (t1 > 1.0)) return false;
+ if ((t2 < 0.0) || (t2 > 1.0)) return false;
+ return true;
}
//TODO: Consider numerical stability and special case pt is on line
// Returns true if pt is on the left side ( from point1 toward point2)
-bool Line::IsLeft(const Point& pt)
-{
- double test=(_point2._x-_point1._x)*(pt.GetY()-_point1._y)-(_point2._y-_point1._y)*(pt.GetX()-_point1._x);
- if (test>0.0)
- return true;
- else
- return false;
+bool Line::IsLeft(const Point &pt) {
+ double test =
+ (_point2._x - _point1._x) * (pt.GetY() - _point1._y) - (_point2._y - _point1._y) * (pt.GetX() - _point1._x);
+ return test > 0.0;
}
-const Point& Line::GetLeft(const Point& pt)
-{
- if (IsLeft(pt)) {
- return _point2;
- } else {
- return _point1;
- }
+const Point &Line::GetLeft(const Point &pt) {
+ if (IsLeft(pt)) {
+ return _point2;
+ } else {
+ return _point1;
+ }
}
-const Point& Line::GetRight(const Point& pt)
-{
- if (!IsLeft(pt)) {
- return _point2;
- } else {
- return _point1;
- }
+const Point &Line::GetRight(const Point &pt) {
+ if (!IsLeft(pt)) {
+ return _point2;
+ } else {
+ return _point1;
+ }
}
-std::string Line::toString() const
-{
- std::stringstream tmp;
- tmp<<_point1.toString()<<"--"<<_point2.toString();
- return tmp.str();
+std::string Line::toString() const {
+ std::stringstream tmp;
+ tmp << _point1.toString() << "--" << _point2.toString();
+ return tmp.str();
}
+
// get distance between first point of line with the intersection point.
//if no intersection return infinity
// this function is exactly the same as GetIntersection(), but returns the distance squared
//insteed of a boolian
-double Line::GetIntersectionDistance(const Line & l) const
-{
+double Line::GetIntersectionDistance(const Line &l) const {
#define DEBUG 0
- double deltaACy = _point1.GetY() - l.GetPoint1().GetY();
- double deltaDCx = l.GetPoint2().GetX() - l.GetPoint1().GetX();
- double deltaACx = _point1.GetX() - l.GetPoint1().GetX();
- double deltaDCy = l.GetPoint2().GetY() - l.GetPoint1().GetY();
- double deltaBAx = _point2.GetX() - _point1.GetX();
- double deltaBAy = _point2.GetY() - _point1.GetY();
-
- double denominator = deltaBAx * deltaDCy - deltaBAy * deltaDCx;
- double numerator = deltaACy * deltaDCx - deltaACx * deltaDCy;
- double infinity =100000;
- // the lines are parallel
- if (denominator == 0.0) {
-
- // the lines are superposed
- if (numerator == 0.0) {
-
- // the segment are superposed
- if(IsInLineSegment(l.GetPoint1()) ||
- IsInLineSegment(l.GetPoint2()) ) return infinity;//really?
- else return infinity;
-
- } else { // the lines are just parallel and do not share a common point
-
- return infinity;
- }
- }
+ double deltaACy = _point1.GetY() - l.GetPoint1().GetY();
+ double deltaDCx = l.GetPoint2().GetX() - l.GetPoint1().GetX();
+ double deltaACx = _point1.GetX() - l.GetPoint1().GetX();
+ double deltaDCy = l.GetPoint2().GetY() - l.GetPoint1().GetY();
+ double deltaBAx = _point2.GetX() - _point1.GetX();
+ double deltaBAy = _point2.GetY() - _point1.GetY();
- // the lines intersect
- double r = numerator / denominator;
- if (r < 0.0 || r > 1.0) {
- return infinity;
- }
+ double denominator = deltaBAx * deltaDCy - deltaBAy * deltaDCx;
+ double numerator = deltaACy * deltaDCx - deltaACx * deltaDCy;
+ double infinity = 100000;
+ // the lines are parallel
+ if (denominator == 0.0) {
- double s = (deltaACy * deltaBAx - deltaACx * deltaBAy) / denominator;
- if (s < 0.0 || s > 1.0) {
- return infinity;
- }
+ // the lines are superposed
+ if (numerator == 0.0) {
+
+ // the segment are superposed
+ if (IsInLineSegment(l.GetPoint1()) ||
+ IsInLineSegment(l.GetPoint2()))
+ return infinity;//really?
+ else return infinity;
+
+ } else { // the lines are just parallel and do not share a common point
- Point PointF = Point ((float) (_point1._x + r * deltaBAx), (float) (_point1._y + r * deltaBAy));
- if(!IsInLineSegment(PointF)) //is point on the line?
- return infinity;
- double dist = (_point1-PointF).NormSquare();
+ return infinity;
+ }
+ }
+
+ // the lines intersect
+ double r = numerator / denominator;
+ if (r < 0.0 || r > 1.0) {
+ return infinity;
+ }
+
+ double s = (deltaACy * deltaBAx - deltaACx * deltaBAy) / denominator;
+ if (s < 0.0 || s > 1.0) {
+ return infinity;
+ }
+
+ Point PointF = Point((float) (_point1._x + r * deltaBAx), (float) (_point1._y + r * deltaBAy));
+ if (!IsInLineSegment(PointF)) //is point on the line?
+ return infinity;
+ double dist = (_point1 - PointF).NormSquare();
#if DEBUG
printf("Enter GetIntersection\n");
cout<< "\t" << l.toString() << " intersects with " << toString() <<endl;
@@ -571,7 +501,7 @@ double Line::GetIntersectionDistance(const Line & l) const
cout << "\t\t --> distance is "<< sqrt(dist)<< "... return "<< dist<<endl;
printf("Leave GetIntersection\n");
#endif
- return dist;
+ return dist;
}
@@ -588,25 +518,24 @@ double Line::GetIntersectionDistance(const Line & l) const
// /
// o Goal
-double Line::GetDeviationAngle(const Line & l) const
-{
- // const double PI = 3.14159258;
+double Line::GetDeviationAngle(const Line &l) const {
+ // const double PI = 3.14159258;
#define DEBUG 0
- Point P = _point1;
- Point Goal = _point2;
-
- Point L = l._point1;
- Point R = l._point2;
-
- double dist_Goal_L = (Goal-L).NormSquare();
- double dist_Goal_R = (Goal-R).NormSquare();
-
- double angle, angleL, angleR;
- // we don't need to calculate both angles, but for debugging purposes we do it.
- angleL = atan((Goal-P).CrossP(L-P)/(Goal-P).ScalarP(L-P));
- angleR = atan((Goal-P).CrossP(R-P)/(Goal-P).ScalarP(R-P));
-
- angle = (dist_Goal_L<dist_Goal_R)?angleL:angleR;
+ Point P = _point1;
+ Point Goal = _point2;
+
+ Point L = l._point1;
+ Point R = l._point2;
+
+ double dist_Goal_L = (Goal - L).NormSquare();
+ double dist_Goal_R = (Goal - R).NormSquare();
+
+ double angle, angleL, angleR;
+ // we don't need to calculate both angles, but for debugging purposes we do it.
+ angleL = atan((Goal - P).CrossProduct(L - P) / (Goal - P).ScalarProduct(L - P));
+ angleR = atan((Goal - P).CrossProduct(R - P) / (Goal - P).ScalarProduct(R - P));
+
+ angle = (dist_Goal_L < dist_Goal_R) ? angleL : angleR;
#if DEBUG
printf("Enter GetAngel()\n");
printf("\tP=[%f,%f]\n",P.GetX(), P.GetY());
@@ -620,7 +549,7 @@ double Line::GetDeviationAngle(const Line & l) const
printf("\t\t --> angle=%f\n",angle);
printf("Leave GetAngel()\n");
#endif
- return angle;
+ return angle;
}
diff --git a/src/geometry/Line.h b/src/geometry/Line.h
index 8467c13cf6b476046793916d05922e970bd4f580..5b8dac97a6cb6a7df83a193613465915bb584017 100644
--- a/src/geometry/Line.h
+++ b/src/geometry/Line.h
@@ -1,8 +1,8 @@
/**
* \file Line.h
* \date Sep 30, 2010
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
@@ -42,7 +42,8 @@ class OutputHandler;
extern OutputHandler* Log;
-class Line {
+class Line
+{
private:
Point _point1;
Point _point2;
@@ -142,9 +143,11 @@ public:
*/
double LengthSquare() const;
- //
- // @return dot product of two lines
- // bool operator*(const Line& l) const;
+ /**
+ *
+ * @return true if both lines overlapp
+ */
+ bool Overlapp(const Line& l) const;
/**
* @return true if both segments are equal. The end points must be in the range of J_EPS.
@@ -164,6 +167,12 @@ public:
* @return true if both segments intersect
*/
bool IntersectionWith(const Line& l) const; // check two segments for intersections
+
+ /**
+ * @see http://alienryderflex.com/intersect/
+ * @see http://social.msdn.microsoft.com/Forums/en-US/csharpgeneral/thread/e5993847-c7a9-46ec-8edc-bfb86bd689e3/
+ * @return true if both segments intersect
+ */
bool IntersectionWith(const Point& p1, const Point&p2) const;
/**
@@ -190,14 +199,14 @@ public:
bool HasEndPoint(const Point& point) const;
/**
- * return the same value if the checked points are all situated on the same side.
- * @return 0 or 1 depending on which side of the line the point is located.
+ * Determine on which side the point is located on of the line directed from (_point1 to _point2).
+ * @return 0 (Left) or 1 (Right) depending on which side of the line the point is located.
+ * The return value is undefined if the points are colinear.
*/
int WichSide (const Point& pt);
/**
- * @return true if the point is located in the left hand side of the line.
- * For horizontal lines return true if the point is above the line.
+ * @return true if the point is located in the left hand side of the line directed from (_point1 to _point2).
*/
bool IsLeft (const Point& pt);
@@ -235,8 +244,13 @@ public:
* @return the angle between two lines
*/
double GetDeviationAngle(const Line& l) const;
- // double GetAngle(SubRoom s) const;
- Line enlarge(double d) const;
+
+ /**
+ * ???
+ * @param d
+ * @return
+ */
+ Line Enlarge(double d) const;
};
diff --git a/src/geometry/NavLine.cpp b/src/geometry/NavLine.cpp
index bc5d4982052301ddd275324fdf9458cd089a7c81..a5356fc276cb79ff1369256386115778ddf9cb13 100644
--- a/src/geometry/NavLine.cpp
+++ b/src/geometry/NavLine.cpp
@@ -1,8 +1,8 @@
/**
* \file NavLine.cpp
* \date Aug 30, 2012
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
diff --git a/src/geometry/NavLine.h b/src/geometry/NavLine.h
index 0b3c7dc993de26f97d7828776fd1177f299ff54c..ce14e3ee7fb23ac76225ca320c2ef9e14b2038bb 100644
--- a/src/geometry/NavLine.h
+++ b/src/geometry/NavLine.h
@@ -1,8 +1,8 @@
/**
* \file NavLine.h
* \date Aug 30, 2012
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
diff --git a/src/geometry/Obstacle.cpp b/src/geometry/Obstacle.cpp
index 7daa9ae11e7874d13040d40def5aceb54ee184e6..d5960b597553cdc878a55d384d065a71ad843e32 100644
--- a/src/geometry/Obstacle.cpp
+++ b/src/geometry/Obstacle.cpp
@@ -1,8 +1,8 @@
/**
* \file Obstacle.cpp
* \date Jul 31, 2012
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
@@ -46,7 +46,6 @@ using namespace std;
Obstacle::Obstacle()
{
- _isClosed=0.0;
_height=0.0;
_id=-1;
_caption="obstacle";
@@ -72,16 +71,6 @@ void Obstacle::SetCaption(string caption)
_caption = caption;
}
-double Obstacle::GetClosed() const
-{
- return _isClosed;
-}
-
-void Obstacle::SetClosed(double closed)
-{
- _isClosed = closed;
-}
-
double Obstacle::GetHeight() const
{
return _height;
@@ -110,7 +99,6 @@ const vector<Point>& Obstacle::GetPolygon() const
string Obstacle::Write()
{
string s;
- //Point pos;
for (unsigned int j = 0; j < _walls.size(); j++) {
const Wall& w = _walls[j];
@@ -140,8 +128,30 @@ const vector<Wall>& Obstacle::GetAllWalls() const
int Obstacle::WhichQuad(const Point& vertex, const Point& hitPos) const
{
- return (vertex.GetX() > hitPos.GetX()) ? ((vertex.GetY() > hitPos.GetY()) ? 1 : 4) :
- ((vertex.GetY() > hitPos.GetY()) ? 2 : 3);
+ return (vertex.GetX() > hitPos.GetX()) ?
+ ((vertex.GetY() > hitPos.GetY()) ? 1 : 4) :
+ ((vertex.GetY() > hitPos.GetY()) ? 2 : 3);
+
+// if ((vertex.GetX() - hitPos.GetX())>J_EPS)
+// {
+// if ((vertex.GetY() - hitPos.GetY())>J_EPS)
+// {
+// return 1;
+// } else
+// {
+// return 4;
+// }
+// } else
+// {
+// if ((vertex.GetY() - hitPos.GetY())>J_EPS)
+// {
+// return 2;
+// } else
+// {
+// return 3;
+// }
+//
+// }
}
@@ -155,15 +165,16 @@ double Obstacle::Xintercept(const Point& point1, const Point& point2, double hit
bool Obstacle::Contains(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 d othis 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;
+ }
// in the case the obstacle is not a close surface, allow
// pedestrians distribution 'inside'
- if(_isClosed==0.0) {
- char tmp[CLENGTH];
- sprintf(tmp, "ERROR: \tObstacle::Contains(): the obstacle [%d] is open!!!\n", _id);
- Log->Write(tmp);
- exit(EXIT_FAILURE);
- }
short edge, first, next;
short quad, next_quad, delta, total;
@@ -209,23 +220,17 @@ bool Obstacle::Contains(const Point& ped) const
return false;
}
+
bool Obstacle::ConvertLineToPoly()
{
- if(_isClosed==0.0)
- {
- char tmp[CLENGTH];
- sprintf(tmp, "INFO: \tObstacle [%d] is not closed. Not converting to polyline.\n", _id);
- Log->Write(tmp);
- return true;
- }
-
vector<Line*> copy;
vector<Point> tmpPoly;
Point point;
Line* line;
// Alle Linienelemente in copy speichern
- for (unsigned int i = 0; i < _walls.size(); i++) {
- copy.push_back(&_walls[i]);
+ for(auto& w: _walls)
+ {
+ copy.push_back(&w);
}
line = copy[0];
@@ -253,9 +258,25 @@ bool Obstacle::ConvertLineToPoly()
char tmp[CLENGTH];
sprintf(tmp, "ERROR: \tObstacle::ConvertLineToPoly(): ID %d !!!\n", _id);
Log->Write(tmp);
+ sprintf(tmp, "ERROR: \tDistance between the points: %lf !!!\n", (tmpPoly[0] - point).Norm());
+ Log->Write(tmp);
return false;
}
_poly = tmpPoly;
+
+
+ //check if all walls and goals were used in the polygon
+ for (const auto& w: _walls)
+ {
+ for (const auto & ptw: {w.GetPoint1(),w.GetPoint2()})
+ {
+ if(IsPartOfPolygon(ptw)==false)
+ {
+ Log->Write("ERROR:\t Edge was not used during polygon creation for obstacle: %s",w.toString().c_str());
+ return false;
+ }
+ }
+ }
return true;
}
@@ -311,7 +332,7 @@ bool Obstacle::IsClockwise() const
Point vecAB= _poly[1]-_poly[0];
Point vecBC= _poly[2]-_poly[1];
- double det=vecAB.Det(vecBC);
+ double det= vecAB.Determinant(vecBC);
if(fabs(det)<J_EPS) det=0.0;
return ( det<=0.0 );
@@ -327,3 +348,27 @@ bool Obstacle::IntersectWithLine(const Line& line) const
return false;
}
+
+bool Obstacle::IsPartOfPolygon(const Point& ptw)
+{
+ if ( false == IsElementInVector(_poly, ptw))
+ {
+ //maybe the point was too closed to other points and got replaced
+ //check that eventuality
+ bool near = false;
+ for (const auto & pt : _poly)
+ {
+ if ((pt - ptw).Norm() < J_TOLERANZ)
+ {
+ near = true;
+ break;
+ }
+ }
+
+ if(near==false)
+ {
+ return false;
+ }
+ }
+ return true;
+}
diff --git a/src/geometry/Obstacle.h b/src/geometry/Obstacle.h
index fdc9fee3e00ba1e8cf0032b93a9f8164efb3065a..2ce02e66dd56b3aa425fc88aa10607a60f32ba8c 100644
--- a/src/geometry/Obstacle.h
+++ b/src/geometry/Obstacle.h
@@ -1,8 +1,8 @@
/**
* \file Obstacle.h
* \date Jul 31, 2012
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
@@ -40,7 +40,6 @@ class Line;
class Obstacle {
private:
- double _isClosed;
double _height;
int _id;
std::string _caption;
@@ -48,7 +47,14 @@ private:
std::vector<Point> _poly;
public:
+ /**
+ * Constructor
+ */
Obstacle();
+
+ /**
+ * Destructor
+ */
virtual ~Obstacle();
/**
@@ -61,16 +67,6 @@ public:
*/
void SetCaption(std::string caption);
- /**
- * Set/Get the close state of the obstacle
- */
- double GetClosed() const;
-
- /**
- * Set/Get the close state of the obstacle
- */
- void SetClosed(double closed);
-
/**
* Set/Get the height of the obstacle.
* Is used for computing visibility
@@ -140,6 +136,11 @@ public:
*/
bool IsClockwise() const;
+ /**
+ * @return true if the point is part of the polygon, also considering the geometry precision.
+ */
+ bool IsPartOfPolygon(const Point& ptw);
+
private:
int WhichQuad(const Point& vertex, const Point& hitPos) const;
diff --git a/src/geometry/Point.cpp b/src/geometry/Point.cpp
index c8fa246b403cee18c43d67dd6cdec724f47fd1e1..12d51c0dfaf2b8ca23c00bbd0fa6f9328bfdf0c4 100644
--- a/src/geometry/Point.cpp
+++ b/src/geometry/Point.cpp
@@ -1,8 +1,8 @@
/**
* \file Point.cpp
* \date Sep 30, 2010
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
@@ -40,22 +40,18 @@
/************************************************************
Konstruktoren
************************************************************/
-Point::Point()
+Point::Point() : _x(0), _y(0)
{
- _x = 0.0;
- _y = 0.0;
}
-Point::Point(double x, double y)
+Point::Point(double x, double y) : _x(x), _y(y)
{
- _x = x;
- _y = y;
}
Point::Point(const Point& orig)
{
- _x = orig.GetX();
- _y = orig.GetY();
+ _x = orig._x;
+ _y = orig._y;
}
std::string Point::toString() const
@@ -87,18 +83,13 @@ double Point::GetY() const
double Point::Norm() const
{
- return sqrt(_x * _x + _y * _y);
+ return sqrt(NormSquare());
}
double Point::NormMolified() const
{
double const eps_sq = 0.1;
- return sqrt(_x * _x + _y * _y + eps_sq);
-}
-
-double Point::NormSquare() const
-{
- return (_x * _x + _y * _y);
+ return sqrt(NormSquare() + eps_sq);
}
Point Point::NormalizedMolified() const
@@ -118,28 +109,8 @@ Point Point::Normalized() const
else return Point(0.0, 0.0);
}
-// scalar product
-double Point::ScalarP(const Point& v) const
-{
- //return _x * v.GetX() + _y * v.GetY();
- return _x * v._x + _y * v._y;
-}
-
-// since we have only 2D vectors (may be changed in the future), this function returns a scalar
-// (basically the third component of the vector (0,0,z) )
-double Point::CrossP(const Point& p) const
-{
- return _x*p.GetY() - _y*p.GetX();
-}
-
-/// determinant of the square matrix formed by the vectors [ this, v]
-double Point::Det(const Point& v) const
-{
- return _x * v._y - _y * v._x;
-}
-
/* Transformiert die "normalen" Koordinaten in Koordinaten der Ellipse
* dazu verschieben des Koordinaten Ursprungs in Center und anschliessend drehen um phi
* alle Punkte müssen in "normalen" Koordinaten gegeben sein
@@ -199,14 +170,14 @@ OC = -O1O +O1C
xnew = -xc + x
*/
-Point Point::CoordTransToEllipse(const Point& center, double cphi, double sphi) const
+Point Point::TransformToEllipseCoordinates(const Point ¢er, double cphi, double sphi) const
{
Point p = Point(_x, _y);
return (p - center).Rotate(cphi, -sphi);
}
/*
-This is the reverse funktion of CoordTransToEllipse(),
+This is the reverse funktion of TransformToEllipseCoordinates(),
where the coord. of a point are transformated to cart. coord.
input:
@@ -224,7 +195,7 @@ where the coord. of a point are transformated to cart. coord.
*/
-Point Point::CoordTransToCart(const Point& center, double cphi, double sphi) const
+Point Point::TransformToCartesianCoordinates(const Point ¢er, double cphi, double sphi) const
{
Point p = Point(_x, _y);
return (p.Rotate(cphi, sphi) + center);
diff --git a/src/geometry/Point.h b/src/geometry/Point.h
index df332e2eb7148fa1402b361a50fd9a375d2ec540..ecec992ce68cb0ca790166f5e8f83b4d0c48db3a 100644
--- a/src/geometry/Point.h
+++ b/src/geometry/Point.h
@@ -1,8 +1,8 @@
/**
* \file Point.h
* \date Sep 30, 2010
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
@@ -70,20 +70,39 @@ public:
double NormMolified() const;
/// Norm square
- double NormSquare() const;
+ inline double NormSquare() const
+ {
+ return ScalarProduct(*this);
+ }
/// normalized vector
Point Normalized() const;
/// normalized vector usinf NormMolified
Point NormalizedMolified() const;
/// dot product
- double ScalarP(const Point& v) const;
- double CrossP(const Point& p) const;
+ inline double ScalarProduct(const Point &v) const
+ {
+ return _x * v._x + _y * v._y;
+ }
+
+ // since we have only 2D vectors (may be changed in the future), this function returns a scalar
+ // (basically the third component of the vector (0,0,z) )
+ inline double CrossProduct(const Point &p) const
+ {
+ return Determinant(p);
+ }
+
+
+
/// determinant of the square matrix formed by the vectors [ this, v]
- double Det(const Point& v) const;
+ inline double Determinant(const Point &v) const
+ {
+ return _x*v._y - _y*v._x;
+ }
+
/// translation and rotation in Ellipse coordinate system
- Point CoordTransToEllipse(const Point& center, double cphi, double sphi) const;
+ Point TransformToEllipseCoordinates(const Point ¢er, double cphi, double sphi) const;
/// translation and rotation in cartesian system
- Point CoordTransToCart(const Point& center, double cphi, double sphi) const;
+ Point TransformToCartesianCoordinates(const Point ¢er, double cphi, double sphi) const;
/// rotate the vector by theta
Point Rotate(double ctheta, double stheta) const;
diff --git a/src/geometry/Room.cpp b/src/geometry/Room.cpp
index 340156c844ac9af327af991d2d9c48036999f49d..d6c4514e19237735710da91eccedfb5b8b9bc240 100644
--- a/src/geometry/Room.cpp
+++ b/src/geometry/Room.cpp
@@ -1,8 +1,8 @@
/**
* \file Room.cpp
* \date Sep 30, 2010
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
@@ -88,6 +88,10 @@ void Room::SetState(RoomState state)
_state=state;
}
+void Room::SetEgressTime(double time)
+{
+ _egressTime=time;
+}
/*************************************************************
Getter-Functions
@@ -113,17 +117,12 @@ double Room::GetEgressTime() const
return _egressTime;
}
-void Room::SetEgressTime(double time)
-{
- _egressTime=time;
-}
-
int Room::GetNumberOfSubRooms() const
{
return _subRooms.size();
}
-const std::map<int, std::unique_ptr<SubRoom>>& Room::GetAllSubRooms() const
+const std::map<int, std::unique_ptr<SubRoom> >& Room::GetAllSubRooms() const
{
return _subRooms;
}
diff --git a/src/geometry/Room.h b/src/geometry/Room.h
index 2ffa5b97fc2e0060958a8e200d53eab106243b1d..7abe606eb14e32783f588e0c3675469e8b093a34 100644
--- a/src/geometry/Room.h
+++ b/src/geometry/Room.h
@@ -1,8 +1,8 @@
/**
* \file Room.h
* \date Sep 30, 2010
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
@@ -53,7 +53,7 @@ private:
/// room elevation
double _zPos;
/// all subrooms/partitions of the room
- std::map<int, std::unique_ptr<SubRoom>> _subRooms;
+ std::map<int, std::unique_ptr<SubRoom> > _subRooms;
/// all transitions ids
std::vector<int> _transitionsIDs;
/// needed if the trajectories for this room are to be write in a special way
@@ -120,7 +120,7 @@ public:
/**
* @return a vector containing all subrooms
*/
- const std::map<int, std::unique_ptr<SubRoom>>& GetAllSubRooms() const;
+ const std::map<int, std::unique_ptr<SubRoom> >& GetAllSubRooms() const;
/**
* @return a vector containing all transitions Ids
diff --git a/src/geometry/SubRoom.cpp b/src/geometry/SubRoom.cpp
index e0f4a32ecc6521c77c57454f3e73d61378440be9..0b289cf90d84cca7ce0fb52854c435adef4edc53 100644
--- a/src/geometry/SubRoom.cpp
+++ b/src/geometry/SubRoom.cpp
@@ -1,8 +1,8 @@
/**
* \file SubRoom.cpp
* \date Oct 8, 2010
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
@@ -68,15 +68,13 @@ SubRoom::SubRoom()
_goalIDs = vector<int> ();
_area = 0.0;
- _closed=false;
_uid = _static_uid++;
}
SubRoom::~SubRoom()
{
- if (_walls.size() > 0) _walls.clear();
- if (_poly.size() > 0) _poly.clear();
- for (unsigned int i = 0; i < _obstacles.size(); i++) {
+ for (unsigned int i = 0; i < _obstacles.size(); i++)
+ {
delete _obstacles[i];
}
_obstacles.clear();
@@ -87,10 +85,6 @@ void SubRoom::SetSubRoomID(int ID)
{
_id = ID;
}
-void SubRoom::SetClosed(double closed)
-{
- _closed = closed;
-}
void SubRoom::SetRoomID(int ID)
{
@@ -102,11 +96,6 @@ int SubRoom::GetSubRoomID() const
return _id;
}
-double SubRoom::GetClosed() const
-{
- return _closed;
-}
-
// unique identifier for this subroom
int SubRoom::GetUID() const
{
@@ -124,26 +113,11 @@ int SubRoom::GetRoomID() const
return _roomID;
}
-int SubRoom::GetNumberOfWalls() const
-{
- return (int)_walls.size();
-}
-
const vector<Wall>& SubRoom::GetAllWalls() const
{
return _walls;
}
-const Wall& SubRoom::GetWall(int index) const
-{
- if ((index >= 0) && (index < GetNumberOfWalls()))
- return _walls[index];
- else {
- Log->Write("ERROR: Wrong 'index' in SubRoom::GetWall()");
- exit(0);
- }
-}
-
const vector<Point>& SubRoom::GetPolygon() const
{
return _poly;
@@ -165,9 +139,29 @@ const vector<int>& SubRoom::GetAllGoalIDs() const
}
-void SubRoom::AddWall(const Wall& w)
+bool SubRoom::AddWall(const Wall& w)
{
+ //check for duplicated walls
+ for(auto&& w1: _walls)
+ {
+ if(w==w1)
+ {
+ Log->Write("ERROR:\t Duplicate wall found in Room/Subroom %d/%d %s",_roomID,_id ,w.toString().c_str());
+ Log->Write("ERROR:\t will not be added");
+ //exit(EXIT_FAILURE);
+ return false;
+ }
+ }
+ //checking for wall chunks.
+ if(w.Length()<J_TOLERANZ)
+ {
+ Log->Write("ERROR:\t Wall too small (length = %lf) found in Room/Subroom %d/%d %s",w.Length(),_roomID,_id ,w.toString().c_str());
+ Log->Write("ERROR:\t will not be added");
+ //exit(EXIT_FAILURE);
+ return false;
+ }
_walls.push_back(w);
+ return true;
}
@@ -195,17 +189,27 @@ void SubRoom::AddTransition(Transition* line)
_goalIDs.push_back(line->GetUniqueID());
}
+void SubRoom::AddNeighbor(SubRoom* sub)
+{
+ if(sub and (IsElementInVector(_neighbors, sub)==false))
+ {
+ _neighbors.push_back(sub);
+ }
+}
+
void SubRoom::AddHline(Hline* line)
{
- for(unsigned int i=0;i<_hlines.size();i++){
- if (line->GetID()==_hlines[i]->GetID()){
- Log->Write("INFO:\tskipping duplicate hline [%d] with id [%d]",_id,line->GetID());
- return;
- }
- }
+ for(unsigned int i=0;i<_hlines.size();i++)
+ {
+ if (line->GetID()==_hlines[i]->GetID())
+ {
+ Log->Write("INFO:\tskipping duplicate hline [%d] with id [%d]",_id,line->GetID());
+ return;
+ }
+ }
- _hlines.push_back(line);
- _goalIDs.push_back(line->GetUniqueID());
+ _hlines.push_back(line);
+ _goalIDs.push_back(line->GetUniqueID());
}
const vector<Crossing*>& SubRoom::GetAllCrossings() const
@@ -223,6 +227,11 @@ const vector<Hline*>& SubRoom::GetAllHlines() const
return _hlines;
}
+const std::vector<SubRoom*>& SubRoom::GetNeighbors() const
+{
+ return _neighbors;
+}
+
const Crossing* SubRoom::GetCrossing(int i) const
{
return _crossings[i];
@@ -302,7 +311,8 @@ Point SubRoom::GetCentroid() const
return Point(px,py);
}
-std::vector<Wall> SubRoom::GetVisibleWalls(const Point & position){
+vector<Wall> SubRoom::GetVisibleWalls(const Point & position)
+{
#define DEBUG 0
std::vector<Wall> visible_walls;
bool wall_is_vis;
@@ -324,7 +334,7 @@ std::vector<Wall> SubRoom::GetVisibleWalls(const Point & position){
}
#if DEBUG
printf("Leave GetVisiblewalls with %d visible walls\n------------------------------\n",visible_walls.size());
- #endif
+#endif
return visible_walls;
}
@@ -339,27 +349,28 @@ bool SubRoom::IsVisible(const Line &wall, const Point &position)
// Point nearest_point = wall.ShortestPoint(position);
// in cases where nearest_point is endPoint of Wall, the wall becomes visible even if it is not..
// try with the center. If it is not visible then the wall is definitly not.
- Point nearest_point = wall.GetCentre();
+ const Point& nearest_point = wall.GetCentre();
+
// printf("\t\t center of wall %f, %f\n",nearest_point.GetX(), nearest_point.GetY());
Line ped_wall = Line(position, nearest_point);
- for (auto w:_walls){
+ for (auto& w:_walls)
+ {
if(w == wall) //ignore wall
continue;
- if(wall_is_vis && ped_wall.IntersectionWith(w)){
+ if(wall_is_vis && ped_wall.IntersectionWith(w))
+ {
// fprintf (stdout, "\t\t Wall_is_visible: INTERSECTION WALL L1_P1(%.2f, %.2f), L1_P2(%.2f, %.2f), WALL(%.2f, %.2f)---(%.2f, %.2f)\n", ped_wall.GetPoint1().GetX(),ped_wall.GetPoint1().GetY(), ped_wall.GetPoint2().GetX(), ped_wall.GetPoint2().GetY(), w.GetPoint1().GetX(),w.GetPoint1().GetY(),w.GetPoint2().GetX(),w.GetPoint2().GetY());
wall_is_vis = false;
}
}
- //check intersection with obstacles
- for(unsigned int i = 0; i < _obstacles.size(); i++) {
- Obstacle * obs = _obstacles[i];
- for(unsigned int k = 0; k<obs->GetAllWalls().size(); k++) {
- const Wall& w = obs->GetAllWalls()[k];
+ for(const auto& obst: _obstacles)
+ {
+ for(const auto& w: obst->GetAllWalls())
+ {
if(wall_is_vis && ped_wall.IntersectionWith(w)){
// fprintf (stdout, "\t\t Wall_is_visible INTERSECTION OBS; L1_P1(%.2f, %.2f), L1_P2(%.2f, %.2f), L2_P1(%.2f, %.2f) L2_P2(%.2f, %.2f)\n", w.GetPoint1().GetX(), w.GetPoint1().GetY(), w.GetPoint2().GetX(), w.GetPoint2().GetY(), ped_wall.GetPoint1().GetX(), ped_wall.GetPoint1().GetY(), ped_wall.GetPoint2().GetX(), ped_wall.GetPoint2().GetY());
wall_is_vis = false;
-
}
}
}
@@ -373,116 +384,49 @@ bool SubRoom::IsVisible(const Line &wall, const Point &position)
// with the nearest point on the wall IS intersecting with the wall.
bool SubRoom::IsVisible(const Point& p1, const Point& p2, bool considerHlines)
{
- // printf("\t\tEnter ped_is_visible\n");
- // generate certain connection lines
- // connecting p1 with p2
- //Line cl(p1,p2);
- //Line L2;
- bool temp = true;
- //check intersection with Walls
- for(const auto& wall : _walls) {
- if (wall.IntersectionWith(p1, p2)) {
- return false;
- }
- }
-
- // printf("\t\t -- ped_is_visible; check obstacles\n");
- //check intersection with obstacles
- for ( const auto obstacle : _obstacles) {
- for ( const auto& wall : obstacle->GetAllWalls()) {
- if (wall.IntersectionWith(p1, p2)) {
- return false;
- }
- }
- }
-
-
- // check intersection with other hlines in room
- if(considerHlines)
- for(unsigned int i = 0; i < _hlines.size(); i++) {
- if(_hlines[i]->IsInLineSegment(p1)|| _hlines[i]->IsInLineSegment(p2)) continue;
- if(temp && (Line*)_hlines[i]->IntersectionWith(p1, p2))
- temp = false;
- }
-
- // printf("\t\tLeave ped_is_visible with %d\n",temp);
- return temp;
-}
-
-bool SubRoom::IsVisible(Line* l1, Line* l2, bool considerHlines)
-{
- // generate certain connection lines
- // connecting p1 mit p1, p1 mit p2, p2 mit p1, p2 mit p2 und center mit center
- Line cl[5];
- cl[0] = Line(l1->GetPoint1(), l2->GetPoint1());
- cl[1] = Line(l1->GetPoint1(), l2->GetPoint2());
- cl[2] = Line(l1->GetPoint2(), l2->GetPoint1());
- cl[3] = Line(l1->GetPoint2(), l2->GetPoint2());
- cl[4] = Line(l1->GetCentre(), l2->GetCentre());
- bool temp[5] = {true, true, true, true, true};
//check intersection with Walls
- for(unsigned int i = 0; i < GetAllWalls().size(); i++) {
- for(int k = 0; k < 5; k++) {
- if(temp[k] && cl[k].IntersectionWith(_walls[i]) && (cl[k].NormalVec() != _walls[i].NormalVec() || l1->NormalVec() != l2->NormalVec()))
- temp[k] = false;
+ for(const auto& wall : _walls)
+ {
+ if (wall.IntersectionWith(p1, p2))
+ {
+ return false;
}
}
+ // printf("\t\t -- ped_is_visible; check obstacles\n");
//check intersection with obstacles
- for(unsigned int i = 0; i < GetAllObstacles().size(); i++) {
- Obstacle * obs = GetAllObstacles()[i];
- for(unsigned int k = 0; k<obs->GetAllWalls().size(); k++) {
- const Wall& w = obs->GetAllWalls()[k];
- if((w.operator !=(*l1)) && (w.operator !=(*l2)))
- for(int j = 0; j < 5; j++) {
- if(temp[j] && cl[j].IntersectionWith(w))
- temp[j] = false;
- }
+ for ( const auto& obstacle : _obstacles)
+ {
+ for ( const auto& wall : obstacle->GetAllWalls())
+ {
+ if (wall.IntersectionWith(p1, p2))
+ {
+ return false;
+ }
}
}
- // check intersection with other hlines in room
if(considerHlines)
- for(unsigned int i = 0; i < _hlines.size(); i++) {
- if ( (l1->operator !=(*(Line*)_hlines[i])) && (l2->operator !=(*(Line*)_hlines[i])) ) {
- for(int k = 0; k < 5; k++) {
- if(temp[k] && cl[k].IntersectionWith(*(Line*)_hlines[i]))
- temp[k] = false;
- }
+ {
+ for(const auto& hline: _hlines)
+ {
+
+ if(hline->IsInLineSegment(p1)|| hline->IsInLineSegment(p2)) continue;
+ if(hline->IntersectionWith(p1, p2))
+ {
+ return false;
}
}
- return temp[0] || temp[1] || temp[2] || temp[3] || temp[4];
-}
-
-// this is the case if they share a transition or crossing
-bool SubRoom::IsDirectlyConnectedWith(const SubRoom* sub) const
-{
-
- //check the crossings
- const vector<Crossing*>& crossings = sub->GetAllCrossings();
- for (unsigned int i = 0; i < crossings.size(); i++) {
- for (unsigned int j = 0; j < _crossings.size(); j++) {
- int uid1 = crossings[i]->GetUniqueID();
- int uid2 = _crossings[j]->GetUniqueID();
- // ignore my transition
- if (uid1 == uid2)
- return true;
- }
}
- // and finally the transitions
- const vector<Transition*>& transitions = sub->GetAllTransitions();
- for (unsigned int i = 0; i < transitions.size(); i++) {
- for (unsigned int j = 0; j < _transitions.size(); j++) {
- int uid1 = transitions[i]->GetUniqueID();
- int uid2 = _transitions[j]->GetUniqueID();
- // ignore my transition
- if (uid1 == uid2)
- return true;
- }
- }
+ // printf("\t\tLeave ped_is_visible with %d\n",temp);
+ return true;
+}
- return false;
+// this is the case if they share a transition or crossing
+bool SubRoom::IsDirectlyConnectedWith(SubRoom* sub) const
+{
+ return IsElementInVector(_neighbors, sub);
}
void SubRoom::SetPlanEquation(double A, double B, double C)
@@ -510,36 +454,154 @@ double SubRoom::GetCosAngleWithHorizontal() const
}
-void SubRoom::CheckObstacles()
+bool SubRoom::CheckObstacles()
{
- for(unsigned int i = 0; i<_walls.size(); i++) {
- for(unsigned int j = 0; j<_obstacles.size(); j++) {
- if(_obstacles[j]->IntersectWithLine(_walls[i])) {
- Log->Write("INFO: \tthe obstacle id [%d] is intersection with subroom [%d]",_obstacles[j]->GetId(),_id);
- Log->Write("INFO: \tthe triangulation will not work.");
- exit(EXIT_FAILURE);
+ for(const auto& wall: _walls)
+ {
+ for(const auto& obst: _obstacles)
+ {
+ if(obst->IntersectWithLine(wall))
+ {
+ Log->Write("ERROR: \tthe obstacle id [%d] is intersection with subroom [%d]",obst->GetId(),_id);
+ Log->Write(" : \tthe triangulation will not work.");
+ return false;
}
}
}
+
+ return true;
+}
+
+bool SubRoom::Overlapp(const std::vector<Line*>& goals) const
+{
+ for(const auto& wall: _walls)
+ {
+ for(const auto& goal:goals)
+ {
+ if (wall.Overlapp(*goal)) return true;
+ }
+ }
+
+ return false;
}
bool SubRoom::SanityCheck()
{
- if(_obstacles.size()==0) {
- if((IsConvex()==false) && (_hlines.size()==0)) {
+ if(_obstacles.size()==0)
+ {
+ if((IsConvex()==false) && (_hlines.size()==0))
+ {
Log->Write("WARNING:\t Room [%d] Subroom [%d] is not convex!",_roomID,_id);
Log->Write("\t\t you might consider adding extra hlines in your routing.xml file");
} else {
// everything is fine
}
} else {
- if(_hlines.size()==0) {
+ if(_hlines.size()==0)
+ {
Log->Write("WARNING:\t you have obstacles in room [%d] Subroom [%d]!",_roomID,_id);
Log->Write("\t\t you might consider adding extra hlines in your routing.xml file");
} else {
// everything is fine
}
}
+ //check if there are overlapping walls
+ for (auto&& w1: _walls)
+ {
+ bool connected=false;
+
+ for (auto&& w2: _walls)
+ {
+ if (w1==w2) continue;
+ if(w1.Overlapp(w2))
+ {
+ Log->Write("ERROR: Overlapping between walls %s and %s ",w1.toString().c_str(),w2.toString().c_str());
+ exit(EXIT_FAILURE);
+ //return false;
+ }
+ connected=connected or w1.ShareCommonPointWith(w2);
+ }
+ //overlapping with lines
+ for(auto&& hline: _hlines)
+ {
+ if(w1.Overlapp(*hline))
+ {
+ Log->Write("ERROR: Overlapping between wall %s and Hline %s ",w1.toString().c_str(),hline->toString().c_str());
+ exit(EXIT_FAILURE);
+ //return false;
+ }
+ }
+ //overlaping with crossings
+ for(auto&& c: _crossings)
+ {
+ if(w1.Overlapp(*c))
+ {
+ Log->Write("ERROR: Overlapping between wall %s and crossing %s ",w1.toString().c_str(),c->toString().c_str());
+ exit(EXIT_FAILURE);
+ //return false;
+ }
+ connected=connected or w1.ShareCommonPointWith(*c);
+ }
+ //overlaping with transitions
+ for(auto&& t: _transitions)
+ {
+ if(w1.Overlapp(*t))
+ {
+ Log->Write("ERROR: Overlapping between wall %s and transition %s ",w1.toString().c_str(),t->toString().c_str());
+ exit(EXIT_FAILURE);
+ //return false;
+ }
+ connected=connected or w1.ShareCommonPointWith(*t);
+ }
+
+ if(not connected)
+ {
+ Log->Write("ERROR: loose wall found %s in Room/Subroom %d/%d",w1.toString().c_str(),_roomID,_id);
+ exit(EXIT_FAILURE);
+ //return false;
+ }
+ }
+
+ //check overlaping between hline and crossings
+ for(auto&& h: _hlines)
+ {
+ for(auto&& c: _crossings)
+ {
+ if(h->Overlapp(*c))
+ {
+ Log->Write("ERROR: Overlapping between hline %s and crossing %s ",h->toString().c_str(),c->toString().c_str());
+ exit(EXIT_FAILURE);
+ //return false;
+ }
+ }
+ }
+ //check overlaping between hlines and transitions
+ for(auto&& h: _hlines)
+ {
+ for(auto&& t: _transitions)
+ {
+ if(h->Overlapp(*t))
+ {
+ Log->Write("ERROR: Overlapping between hline %s and transition %s ",h->toString().c_str(),t->toString().c_str());
+ exit(EXIT_FAILURE);
+ //return false;
+ }
+ }
+ }
+ //check overlaping between transitions and crossings
+ for(auto&& c: _crossings)
+ {
+ for(auto&& t: _transitions)
+ {
+ if(c->Overlapp(*t))
+ {
+ Log->Write("ERROR: Overlapping between crossing %s and transition %s ",c->toString().c_str(),t->toString().c_str());
+ exit(EXIT_FAILURE);
+ //return false;
+ }
+ }
+ }
+
return true;
}
@@ -559,7 +621,7 @@ bool SubRoom::IsConvex()
for(unsigned int i=0; i<hsize; i++) {
Point vecAB= _poly[(i+1)%hsize]-_poly[i%hsize];
Point vecBC= _poly[(i+2)%hsize]-_poly[(i+1)%hsize];
- double det= vecAB.Det(vecBC);
+ double det= vecAB.Determinant(vecBC);
if(fabs(det)<J_EPS) det=0.0;
if(det<0.0) {
@@ -591,19 +653,39 @@ bool SubRoom::IsClockwise()
Point vecAB= _poly[1]-_poly[0];
Point vecBC= _poly[2]-_poly[1];
- double det=vecAB.Det(vecBC);
- if(fabs(det)<J_EPS) det=0.0;
-
- return ( det<=0.0 );
+ double det= vecAB.Determinant(vecBC);
+ return (fabs(det)<J_EPS);
}
+bool SubRoom::IsPartOfPolygon(const Point& ptw)
+{
+ if ( false == IsElementInVector(_poly, ptw))
+ {
+ //maybe the point was too closed to other points and got replaced
+ //check that eventuality
+ bool near = false;
+ for (const auto & pt : _poly)
+ {
+ if ((pt - ptw).Norm() < J_TOLERANZ)
+ {
+ near = true;
+ break;
+ }
+ }
+
+ if(near==false)
+ {
+ return false;
+ }
+ }
+ return true;
+}
/************************************************************
NormalSubRoom
************************************************************/
NormalSubRoom::NormalSubRoom() : SubRoom()
{
-
}
@@ -614,37 +696,61 @@ NormalSubRoom::~NormalSubRoom()
string NormalSubRoom::WriteSubRoom() const
{
string s;
- for (int j = 0; j < GetNumberOfWalls(); j++) {
-
- const Wall& w = GetWall(j);
+ for(auto&& w: _walls)
+ {
string geometry;
char wall[CLENGTH] = "";
geometry.append("\t\t<wall>\n");
sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\" zPos=\"%.2f\"/>\n",
- (w.GetPoint1().GetX()) * FAKTOR,
- (w.GetPoint1().GetY()) * FAKTOR,
- GetElevation(w.GetPoint1())*FAKTOR);
+ (w.GetPoint1().GetX()) * FAKTOR,
+ (w.GetPoint1().GetY()) * FAKTOR,
+ GetElevation(w.GetPoint1())*FAKTOR);
geometry.append(wall);
sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\" zPos=\"%.2f\"/>\n",
- (w.GetPoint2().GetX()) * FAKTOR,
- (w.GetPoint2().GetY()) * FAKTOR,
- GetElevation(w.GetPoint2())*FAKTOR);
+ (w.GetPoint2().GetX()) * FAKTOR,
+ (w.GetPoint2().GetY()) * FAKTOR,
+ GetElevation(w.GetPoint2())*FAKTOR);
geometry.append(wall);
geometry.append("\t\t</wall>\n");
s.append(geometry);
//s.append(GetWall(j).Write());
}
+
//add the subroom caption
- Point pos = GetCentroid();
+ const Point& pos = GetCentroid();
char tmp[CLENGTH];
- sprintf(tmp, "\t\t<label centerX=\"%.2f\" centerY=\"%.2f\" centerZ=\"0\" text=\"%d\" color=\"100\" />\n"
- , pos.GetX() * FAKTOR, pos.GetY() * FAKTOR, GetSubRoomID());
+ sprintf(tmp, "\t\t<label centerX=\"%.2f\" centerY=\"%.2f\" centerZ=\"%.2f\" text=\"%d\" color=\"100\" />\n"
+ , pos.GetX() * FAKTOR, pos.GetY() * FAKTOR, GetElevation(pos)*FAKTOR,GetSubRoomID());
s.append(tmp);
//write the obstacles
- for( unsigned int j=0; j<GetAllObstacles().size(); j++) {
- s.append(GetAllObstacles()[j]->Write());
+ for (auto&& obst : GetAllObstacles())
+ {
+ for (auto&& w: obst->GetAllWalls())
+ {
+ char wall[CLENGTH] = "";
+ s.append("\t\t<wall>\n");
+ sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\" zPos=\"%.2f\"/>\n",
+ (w.GetPoint1().GetX()) * FAKTOR,
+ (w.GetPoint1().GetY()) * FAKTOR,
+ GetElevation(w.GetPoint1())*FAKTOR);
+ s.append(wall);
+ sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\" zPos=\"%.2f\"/>\n",
+ (w.GetPoint2().GetX()) * FAKTOR,
+ (w.GetPoint2().GetY()) * FAKTOR,
+ GetElevation(w.GetPoint2())*FAKTOR);
+ s.append(wall);
+ s.append("\t\t</wall>\n");
+ }
+
+ const Point& pos = obst->GetCentroid();
+
+ //add the obstacle caption
+ char tmp[CLENGTH];
+ sprintf(tmp, "\t\t<label centerX=\"%.2f\" centerY=\"%.2f\" centerZ=\"%.2f\" text=\"%d\" color=\"100\" />\n"
+ , pos.GetX() * FAKTOR, pos.GetY() * FAKTOR,GetElevation(pos)*FAKTOR ,obst->GetId());
+ s.append(tmp);
}
return s;
@@ -652,47 +758,55 @@ string NormalSubRoom::WriteSubRoom() const
string NormalSubRoom::WritePolyLine() const
{
-
string s;
char tmp[CLENGTH];
s.append("\t<Obstacle closed=\"1\" boundingbox=\"0\" class=\"1\">\n");
- for (unsigned int j = 0; j < _poly.size(); j++) {
- sprintf(tmp, "\t\t<Vertex p_x = \"%.2lf\" p_y = \"%.2lf\"/>\n",_poly[j].GetX()* FAKTOR,_poly[j].GetY()* FAKTOR);
+ for(const auto& p: _poly)
+ {
+ sprintf(tmp, "\t\t<Vertex p_x = \"%.2lf\" p_y = \"%.2lf\"/>\n",p.GetX()* FAKTOR,p.GetY()* FAKTOR);
s.append(tmp);
}
s.append("\t</Obstacle>\n");
//write the obstacles
- for( unsigned int j=0; j<GetAllObstacles().size(); j++) {
- s.append(GetAllObstacles()[j]->Write());
+ for(auto&& obst: _obstacles)
+ {
+ s.append(obst->Write());
}
-
return s;
}
void NormalSubRoom::WriteToErrorLog() const
{
Log->Write("\t\tNormal SubRoom:\n");
- for (int i = 0; i < GetNumberOfWalls(); i++) {
- Wall w = GetWall(i);
+ for(auto&& w: _walls)
+ {
w.WriteToErrorLog();
}
}
-bool NormalSubRoom::ConvertLineToPoly(vector<Line*> goals)
+bool NormalSubRoom::ConvertLineToPoly(const vector<Line*>& goals)
{
vector<Line*> copy;
vector<Point> tmpPoly;
Point point;
Line* line;
// Alle Linienelemente in copy speichern
- for (int i = 0; i < GetNumberOfWalls(); i++) {
- copy.push_back(&_walls[i]);
+ for(auto& w: _walls)
+ {
+ copy.push_back(&w);
}
+
// Transitions und Crossings sind in goal abgespeichert
copy.insert(copy.end(), goals.begin(), goals.end());
+ if(Overlapp(goals))
+ {
+ Log->Write("ERROR:\t Overlapping between walls and goals");
+ return false;
+ }
+
line = copy[0];
tmpPoly.push_back(line->GetPoint1());
point = line->GetPoint2();
@@ -717,26 +831,53 @@ bool NormalSubRoom::ConvertLineToPoly(vector<Line*> goals)
if ((tmpPoly[0] - point).Norm() > J_TOLERANZ) {
char tmp[CLENGTH];
sprintf(tmp, "ERROR: \tNormalSubRoom::ConvertLineToPoly(): SubRoom %d Room %d Anfangspunkt ungleich Endpunkt!!!\n"
- "\t(%f, %f) != (%f, %f)\n", GetSubRoomID(), GetRoomID(), tmpPoly[0].GetX(), tmpPoly[0].GetY(), point.GetX(),
- point.GetY());
+ "\t(%f, %f) != (%f, %f)\n", GetSubRoomID(), GetRoomID(), tmpPoly[0].GetX(), tmpPoly[0].GetY(), point.GetX(),
+ point.GetY());
Log->Write(tmp);
sprintf(tmp, "ERROR: \tDistance between the points: %lf !!!\n", (tmpPoly[0] - point).Norm());
Log->Write(tmp);
return false;
}
_poly = tmpPoly;
+
+
+ //check if all walls and goals were used in the polygon
+ for (const auto& w: _walls)
+ {
+ for (const auto & ptw: {w.GetPoint1(),w.GetPoint2()})
+ {
+ if(IsPartOfPolygon(ptw)==false)
+ {
+ Log->Write("ERROR:\t Wall was not used during polygon creation for subroom: %s",w.toString().c_str());
+ return false;
+ }
+ }
+ }
+
+ for (const auto& g: goals)
+ {
+
+ for (const auto & ptw: {g->GetPoint1(),g->GetPoint2()})
+ {
+ if(IsPartOfPolygon(ptw)==false)
+ {
+ Log->Write("ERROR:\t goal was not used during polygon creation for subroom: %s",g->toString().c_str());
+ return false;
+ }
+ }
+ }
return true;
}
// private Funktionen
-// gibt zuruck in welchen Quadranten vertex liegt, wobei hitPos der Koordinatenursprung ist
+// gibt zurück in welchen Quadranten vertex liegt, wobei hitPos der Koordinatenursprung ist
int NormalSubRoom::WhichQuad(const Point& vertex, const Point& hitPos) const
{
return (vertex.GetX() > hitPos.GetX()) ? ((vertex.GetY() > hitPos.GetY()) ? 1 : 4) :
- ((vertex.GetY() > hitPos.GetY()) ? 2 : 3);
+ ((vertex.GetY() > hitPos.GetY()) ? 2 : 3);
}
@@ -745,7 +886,7 @@ int NormalSubRoom::WhichQuad(const Point& vertex, const Point& hitPos) const
double NormalSubRoom::Xintercept(const Point& point1, const Point& point2, double hitY) const
{
return (point2.GetX() - (((point2.GetY() - hitY) * (point1.GetX() - point2.GetX())) /
- (point1.GetY() - point2.GetY())));
+ (point1.GetY() - point2.GetY())));
}
@@ -753,6 +894,17 @@ 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 d othis 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;
+ }
+
+
short edge, first, next;
short quad, next_quad, delta, total;
@@ -783,7 +935,7 @@ bool NormalSubRoom::IsInSubRoom(const Point& ped) const
case -3: // MOVING BACK 3 IS LIKE MOVING FORWARD 1
delta = 1;
break;
- default:break;
+ default:break;
}
/* ADD IN THE DELTA */
total += delta;
@@ -840,22 +992,20 @@ const Point & Stair::GetDown() const
string Stair::WriteSubRoom() const
{
string s;
-
- for (int j = 0; j < GetNumberOfWalls(); j++) {
- const Wall& w = GetWall(j);
-
+ for(auto&& w: _walls)
+ {
string geometry;
char wall[CLENGTH] = "";
geometry.append("\t\t<wall>\n");
sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\" zPos=\"%.2f\"/>\n",
- (w.GetPoint1().GetX()) * FAKTOR,
- (w.GetPoint1().GetY()) * FAKTOR,
- GetElevation(w.GetPoint1())*FAKTOR);
+ (w.GetPoint1().GetX()) * FAKTOR,
+ (w.GetPoint1().GetY()) * FAKTOR,
+ GetElevation(w.GetPoint1())*FAKTOR);
geometry.append(wall);
sprintf(wall, "\t\t\t<point xPos=\"%.2f\" yPos=\"%.2f\" zPos=\"%.2f\"/>\n",
- (w.GetPoint2().GetX()) * FAKTOR,
- (w.GetPoint2().GetY()) * FAKTOR,
- GetElevation(w.GetPoint2())*FAKTOR);
+ (w.GetPoint2().GetX()) * FAKTOR,
+ (w.GetPoint2().GetY()) * FAKTOR,
+ GetElevation(w.GetPoint2())*FAKTOR);
geometry.append(wall);
geometry.append("\t\t</wall>\n");
@@ -867,12 +1017,12 @@ string Stair::WriteSubRoom() const
Point pos = GetCentroid();
char tmp_c[CLENGTH];
sprintf(tmp_c, "\t\t<sphere centerX=\"%.2f\" centerY=\"%.2f\" centerZ=\"%.2f\" radius=\"%.2f\" color=\"100\" />\n"
- , GetUp().GetX() * FAKTOR, GetUp().GetY() * FAKTOR,GetElevation(GetUp())*FAKTOR, 0.2*FAKTOR);
+ , GetUp().GetX() * FAKTOR, GetUp().GetY() * FAKTOR,GetElevation(GetUp())*FAKTOR, 0.2*FAKTOR);
s.append(tmp_c);
//add the subroom caption
sprintf(tmp_c, "\t\t<label centerX=\"%.2f\" centerY=\"%.2f\" centerZ=\"%.2f\" text=\"%d\" color=\"100\" />\n"
- , pos.GetX() * FAKTOR, pos.GetY() * FAKTOR,GetElevation(pos)*FAKTOR ,GetSubRoomID());
+ , pos.GetX() * FAKTOR, pos.GetY() * FAKTOR,GetElevation(pos)*FAKTOR ,GetSubRoomID());
s.append(tmp_c);
return s;
@@ -901,8 +1051,8 @@ string Stair::WritePolyLine() const
void Stair::WriteToErrorLog() const
{
Log->Write("\t\tStair:\n");
- for (int i = 0; i < GetNumberOfWalls(); i++) {
- Wall w = GetWall(i);
+ for(auto&& w: _walls)
+ {
w.WriteToErrorLog();
}
}
@@ -916,7 +1066,7 @@ const Point* Stair::CheckCorner(const Point** otherPoint, const Point** aktPoint
Point l2 = *nextPoint - **aktPoint;
const Point* rueck = NULL;
// Punkte bilden eine Linie
- if (fabs(fabs(l1.ScalarP(l2) / (l1.Norm() * l2.Norm())) - 1) < 0.1) {
+ if (fabs(fabs(l1.ScalarProduct(l2) / (l1.Norm() * l2.Norm())) - 1) < 0.1) {
*aktPoint = nextPoint;
} else { // aktPoint/p2 ist eine Ecke
rueck = *aktPoint;
@@ -926,9 +1076,8 @@ const Point* Stair::CheckCorner(const Point** otherPoint, const Point** aktPoint
return rueck;
}
-bool Stair::ConvertLineToPoly(vector<Line*> goals)
+bool Stair::ConvertLineToPoly(const vector<Line*>& goals)
{
-
//return NormalSubRoom::ConvertLineToPoly(goals);
vector<Line*> copy;
vector<Point> orgPoly = vector<Point > ();
@@ -940,12 +1089,20 @@ bool Stair::ConvertLineToPoly(vector<Line*> goals)
Line *nextLine;
// Alle Linienelemente in copy speichern
- for (int i = 0; i < GetNumberOfWalls(); i++) {
- copy.push_back(&_walls[i]);
+ for(auto& w: _walls)
+ {
+ copy.push_back(&w);
}
+
// Transitions und Crossings sind in goal abgespeichert
copy.insert(copy.end(), goals.begin(), goals.end());
+ if(Overlapp(goals))
+ {
+ Log->Write("ERROR:\t Overlapping between walls and goals");
+ return false;
+ }
+
aktPoint = ©[0]->GetPoint1();
firstAktPoint = aktPoint;
otherPoint = ©[0]->GetPoint2();
@@ -981,7 +1138,7 @@ bool Stair::ConvertLineToPoly(vector<Line*> goals)
x2 = aktPoint->GetX();
y2 = aktPoint->GetY();
sprintf(tmp, "ERROR: \tStair::ConvertLineToPoly(): SubRoom %d Room %d Anfangspunkt ungleich Endpunkt!!!\n"
- "\t(%f, %f) != (%f, %f)\n", GetSubRoomID(), GetRoomID(), x1, y1, x2, y2);
+ "\t(%f, %f) != (%f, %f)\n", GetSubRoomID(), GetRoomID(), x1, y1, x2, y2);
Log->Write(tmp);
return false;
}
@@ -989,14 +1146,16 @@ bool Stair::ConvertLineToPoly(vector<Line*> goals)
if (orgPoly.size() != 4) {
char tmp[CLENGTH];
sprintf(tmp, "ERROR: \tStair::ConvertLineToPoly(): Stair %d Room %d ist kein Viereck!!!\n"
- "Anzahl Ecken: %d\n", GetSubRoomID(), (int)GetRoomID(), (int)orgPoly.size());
+ "Anzahl Ecken: %d\n", GetSubRoomID(), (int)GetRoomID(), (int)orgPoly.size());
Log->Write(tmp);
return false;
}
vector<Point> neuPoly = (orgPoly);
// ganz kleine Treppen (nur eine Stufe) nicht
- if ((neuPoly[0] - neuPoly[1]).Norm() > 0.9 && (neuPoly[1] - neuPoly[2]).Norm() > 0.9) {
- for (int i1 = 0; i1 < (int) orgPoly.size(); i1++) {
+ if ((neuPoly[0] - neuPoly[1]).Norm() > 0.9 && (neuPoly[1] - neuPoly[2]).Norm() > 0.9)
+ {
+ for (int i1 = 0; i1 < (int) orgPoly.size(); i1++)
+ {
unsigned long i2 = (i1 + 1) % orgPoly.size();
unsigned long i3 = (i2 + 1) % orgPoly.size();
unsigned long i4 = (i3 + 1) % orgPoly.size();
@@ -1004,11 +1163,11 @@ bool Stair::ConvertLineToPoly(vector<Line*> goals)
Point p2 = neuPoly[i2];
Point p3 = neuPoly[i3];
Point p4 = neuPoly[i4];
-
Point l1 = p2 - p1;
Point l2 = p3 - p2;
- if (l1.Norm() < l2.Norm()) {
+ if (l1.Norm() < l2.Norm())
+ {
neuPoly[i2] = neuPoly[i2] + l1.Normalized() * 2 * J_EPS_GOAL;
l2 = p3 - p4;
neuPoly[i3] = neuPoly[i3] + l2.Normalized() * 2 * J_EPS_GOAL;
@@ -1017,6 +1176,34 @@ bool Stair::ConvertLineToPoly(vector<Line*> goals)
}
_poly = neuPoly;
+ //check if all walls and goals were used in the polygon
+ // will not work here, since the stairs only consist of 4 vertices
+
+// for (const auto& w: _walls)
+// {
+// for (const auto & ptw: {w.GetPoint1(),w.GetPoint2()})
+// {
+// if(IsPartOfPolygon(ptw)==false)
+// {
+// Log->Write("ERROR:\t Wall was not used during polygon creation for subroom: %s",w.toString().c_str());
+// return false;
+// }
+// }
+// }
+//
+// for (const auto& g: goals)
+// {
+//
+// for (const auto & ptw: {g->GetPoint1(),g->GetPoint2()})
+// {
+// if(IsPartOfPolygon(ptw)==false)
+// {
+// Log->Write("ERROR:\t goal was not used during polygon creation for subroom: %s",g->toString().c_str());
+// return false;
+// }
+// }
+// }
+
return true;
}
@@ -1026,7 +1213,8 @@ bool Stair::IsInSubRoom(const Point& ped) const
int N = (int) _poly.size();
int sum = 0;
- for (int i = 0; i < N; i++) {
+ for (int i = 0; i < N; i++)
+ {
Line l = Line(_poly[i], _poly[(i + 1) % N]);
Point s = l.LotPoint(ped);
if (l.IsInLineSegment(s))
diff --git a/src/geometry/SubRoom.h b/src/geometry/SubRoom.h
index 01b2f5c7b10f7c09df0541f352cb9c49d8fc4cb9..8ee6788380bccc552f75a30d171c1b09546335dd 100644
--- a/src/geometry/SubRoom.h
+++ b/src/geometry/SubRoom.h
@@ -1,8 +1,8 @@
/**
* \file SubRoom.h
* \date Oct 8, 2010
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
@@ -30,9 +30,7 @@
#define _SUBROOM_H
-//#include "Line.h"
#include "Wall.h"
-//#include "Point.h"
#include <vector>
#include <string>
@@ -64,18 +62,17 @@ private:
int _roomID;
std::vector<int> _goalIDs; // all navigation lines contained in this subroom
double _area;
- double _closed;
//defined by: Z = Ax + By + C
double _planeEquation[3];
double _cosAngleWithHorizontalPlane;
std::string _type;
- std::vector<Obstacle*> _obstacles; // obstacles
//different types of navigation lines
std::vector<Crossing*> _crossings;
std::vector<Transition*> _transitions;
std::vector<Hline*> _hlines;
+ std::vector<SubRoom*>_neighbors;
/// storing and incrementing the total number of subrooms
static int _static_uid;
@@ -83,11 +80,18 @@ private:
protected:
std::vector<Wall> _walls;
std::vector<Point> _poly; // Polygonal representation of the subroom
+ std::vector<Obstacle*> _obstacles;
public:
- // constructors
+ /**
+ * Constructor
+ */
SubRoom();
+
+ /**
+ * Destructor
+ */
virtual ~SubRoom();
/**
@@ -100,8 +104,6 @@ public:
*/
void SetRoomID(int ID);
- void SetClosed(double c);
-
/**
* Set the plane equation for this subroom.
* defined by: Z = Ax + By + C
@@ -113,11 +115,6 @@ public:
*/
int GetSubRoomID() const;
- /**
- * @return the number of walls forming this subroom
- */
- int GetNumberOfWalls() const;
-
/**
* @return all walls
*/
@@ -128,11 +125,6 @@ public:
*/
std::vector<Wall> GetVisibleWalls(const Point & position);
- /**
- * @return a reference to the wall at position index
- */
- const Wall& GetWall(int index) const;
-
/**
* @return the polygonal representation of the subroom
* counterclockwise
@@ -178,7 +170,6 @@ public:
*/
void SetType(const std::string& type);
-
/**
* @return the status
*/
@@ -236,7 +227,7 @@ public:
* e.g. simple polygons
* no intersection between the walls and the obstacles.
*/
- void CheckObstacles();
+ bool CheckObstacles();
/**
* Check the subroom for possible errors and
@@ -248,6 +239,7 @@ public:
void AddCrossing(Crossing* line);
void AddTransition(Transition* line);
void AddHline(Hline* line);
+ void AddNeighbor(SubRoom* sub);
const std::vector<Crossing*>& GetAllCrossings() const;
const std::vector<Transition*>& GetAllTransitions() const;
@@ -256,10 +248,26 @@ public:
const Transition* GetTransition(int i) const;
const Hline* GetHline(int i) const;
+ /**
+ * @return true if the point is part of the polygon, also considering the geometry precision.
+ */
+ bool IsPartOfPolygon(const Point& pt);
+
+ /**
+ * @return true if there is an overlapp between the walls of the subrooms and the
+ * supplied set of lines.
+ */
+ bool Overlapp(const std::vector<Line*>& goals) const;
+
+ /**
+ * @return the adjacent subrooms
+ */
+ const std::vector<SubRoom*>& GetNeighbors() const ;
+
/**
* Add a wall to the subroom
*/
- void AddWall(const Wall& w);
+ bool AddWall(const Wall& w);
/**
* Adds an obstacle to the subroom.
@@ -267,20 +275,20 @@ public:
*/
void AddObstacle(Obstacle* obs);
+ /**
+ * Add/remove a goal Id
+ */
void AddGoalID(int ID);
- void RemoveGoalID(int ID);
/**
- * @return true if the two subrooms share a common walkable Edge (crossing or transition)
+ * Add/remove a goal Id
*/
- bool IsDirectlyConnectedWith(const SubRoom* sub) const;
+ void RemoveGoalID(int ID);
/**
- * @return true if the two segments are visible from each other.
- * Alls walls and transitions and crossings are used in this check.
- * The use of hlines is optional, because they are not real and can be considered transparent
+ * @return true if the two subrooms share a common walkable Edge (crossing or transition)
*/
- bool IsVisible(Line* l1, Line* l2, bool considerHlines=false);
+ bool IsDirectlyConnectedWith(SubRoom* sub) const;
/**
* @return true if the two points are visible from each other.
@@ -302,14 +310,11 @@ public:
virtual std::string WritePolyLine() const=0;
/// convert all walls and transitions(doors) into a polygon representing the subroom
- virtual bool ConvertLineToPoly(std::vector<Line*> goals) = 0;
+ virtual bool ConvertLineToPoly(const std::vector<Line*>& goals) = 0;
///check whether the pedestrians is still in the subroom
virtual bool IsInSubRoom(const Point& ped) const = 0;
- // MPI:
- void ClearAllPedestrians();
-
#ifdef _SIMULATOR
virtual bool IsInSubRoom(Pedestrian* ped) const;
@@ -338,7 +343,7 @@ public:
std::string WritePolyLine() const;
void WriteToErrorLog() const;
- bool ConvertLineToPoly(std::vector<Line*> goals);
+ bool ConvertLineToPoly(const std::vector<Line*>& goals);
bool IsInSubRoom(const Point& ped) const;
};
@@ -373,7 +378,7 @@ public:
std::string WriteSubRoom() const;
std::string WritePolyLine() const;
virtual void WriteToErrorLog() const;
- virtual bool ConvertLineToPoly(std::vector<Line*> goals);
+ virtual bool ConvertLineToPoly(const std::vector<Line*>& goals);
bool IsInSubRoom(const Point& ped) const;
};
diff --git a/src/geometry/Transition.cpp b/src/geometry/Transition.cpp
index 1d285afeee5e385238e5ade2cabddd522f442a79..3dc322d7a7310f00c5208218fb57b882a8ac69a7 100644
--- a/src/geometry/Transition.cpp
+++ b/src/geometry/Transition.cpp
@@ -1,8 +1,8 @@
/**
* \file Transition.cpp
* \date Nov 16, 2010
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
@@ -42,14 +42,13 @@ Transition::Transition() : Crossing()
_isOpen = true;
_doorUsage=0;
_lastPassingTime=0;
- _room2 = NULL;
+ _room2 = nullptr;
}
Transition::~Transition()
{
}
-// Setter-Funktionen
void Transition::Close()
{
@@ -71,8 +70,6 @@ void Transition::SetRoom2(Room* r)
_room2 = r;
}
-// Getter-Funktionen
-
bool Transition::IsOpen() const
{
return _isOpen;
@@ -203,6 +200,7 @@ void Transition::IncreaseDoorUsage(int number, double time)
{
_doorUsage+=number;
_lastPassingTime=time;
+ _flowAtExit+=to_string(time)+" "+to_string(_doorUsage)+"\n";
}
int Transition::GetDoorUsage() const
@@ -214,3 +212,8 @@ double Transition::GetLastPassingTime() const
{
return _lastPassingTime;
}
+
+const std::string & Transition::GetFlowCurve() const
+{
+ return _flowAtExit;
+}
diff --git a/src/geometry/Transition.h b/src/geometry/Transition.h
index a19854540e69e1743b7b762f45a93e48f7889c16..936f22d45d01adebee8f290bcc705e57694a6a24 100644
--- a/src/geometry/Transition.h
+++ b/src/geometry/Transition.h
@@ -1,8 +1,8 @@
/**
* \file Transition.h
* \date Nov 16, 2010
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
@@ -43,10 +43,18 @@ private:
// number of agents that passed that exit
int _doorUsage;
double _lastPassingTime;
+ std::string _flowAtExit;
public:
+ /**
+ * Cosntructor
+ */
Transition();
+
+ /**
+ * Destructor
+ */
virtual ~Transition();
/**
@@ -82,6 +90,12 @@ public:
*/
int GetDoorUsage() const;
+
+ /**
+ * @return the flow curve for this exit
+ */
+ const std::string & GetFlowCurve() const;
+
/**
* @return the last time this door was crossed
*/
diff --git a/src/geometry/Wall.cpp b/src/geometry/Wall.cpp
index fa5ba4dd8b3f151af5cedf8bcce0421ccf3a1f67..4b9506b62039a56a3ac0cfaa8b8a0b1d487baade 100644
--- a/src/geometry/Wall.cpp
+++ b/src/geometry/Wall.cpp
@@ -1,8 +1,8 @@
/**
* \file Wall.cpp
* \date Nov 16, 2010
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
diff --git a/src/geometry/Wall.h b/src/geometry/Wall.h
index 6babac37e0b971c037748e87e688352a2fc517f7..b57b4da0423120dc4d24cb7ac86a6003998d66f4 100644
--- a/src/geometry/Wall.h
+++ b/src/geometry/Wall.h
@@ -1,8 +1,8 @@
/**
* \file Wall.h
* \date Nov 16, 2010
- * \version v0.6
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
+ * \version v0.7
+ * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
*
* \section License
* This file is part of JuPedSim.
@@ -34,8 +34,19 @@
class Wall : public Line {
public:
+ /**
+ * Constructor
+ */
Wall();
+
+ /**
+ * Constructor
+ */
Wall(const Point& p1, const Point& p2, const std::string& type="internal");
+
+ /**
+ * Constructor
+ */
Wall(const Wall& orig);
/**