Skip to content

GitLab

JPScore
JPScore
Commit 1e191222 authored by Ulrich Kemloh's avatar Ulrich Kemloh
Browse files
Options

Cleaning and refactoring

parent 0390a0a5
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 1028 additions and 976 deletions
general/Macros.h
View file @ 1e191222
......@@ -177,7 +177,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()) {
......
geometry/Building.cpp
View file @ 1e191222
......@@ -298,6 +298,25 @@ bool Building::InitGeometry()
}
}
// 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;
......
geometry/SubRoom.cpp
View file @ 1e191222
......@@ -74,9 +74,8 @@ SubRoom::SubRoom()
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();
......@@ -124,26 +123,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;
......@@ -215,10 +199,20 @@ 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()){
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;
}
......@@ -243,6 +237,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];
......@@ -322,7 +321,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;
......@@ -350,7 +350,6 @@ std::vector<Wall> SubRoom::GetVisibleWalls(const Point & position){
// like ped_is_visible() but here we can exclude checking intersection
// with the same wall. This function should check if <position> can see the <Wall>
//TODO: optimize
bool SubRoom::IsVisible(const Line &wall, const Point &position)
{
// printf("\tEnter wall_is_visible\n");
......@@ -360,27 +359,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;
}
}
}
......@@ -433,81 +433,10 @@ bool SubRoom::IsVisible(const Point& p1, const Point& p2, bool considerHlines)
return true;
}
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;
}
}
//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;
}
}
}
// 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;
}
}
}
return temp[0] || temp[1] || temp[2] || temp[3] || temp[4];
}
// this is the case if they share a transition or crossing
//TODO collect and store the neighbors
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;
}
}
return false;
bool SubRoom::IsDirectlyConnectedWith(SubRoom* sub) const
{
return IsElementInVector(_neighbors, sub);
}
void SubRoom::SetPlanEquation(double A, double B, double C)
......@@ -537,28 +466,49 @@ double SubRoom::GetCosAngleWithHorizontal() const
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("ERROR: \tthe obstacle id [%d] is intersection with subroom [%d]",_obstacles[j]->GetId(),_id);
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 {
......@@ -617,7 +567,7 @@ bool SubRoom::SanityCheck()
if(not connected)
{
Log->Write("ERROR: loose wall found %s in Room/Subroom %d/%d",w1.toString().c_str(),_roomID,_id);
//exit(EXIT_FAILURE);
exit(EXIT_FAILURE);
//return false;
}
}
......@@ -714,9 +664,7 @@ bool SubRoom::IsClockwise()
Point vecBC= _poly[2]-_poly[1];
double det= vecAB.Determinant(vecBC);
if(fabs(det)<J_EPS) det=0.0;
return ( det<=0.0 );
return (fabs(det)<J_EPS);
}
......@@ -725,7 +673,6 @@ bool SubRoom::IsClockwise()
************************************************************/
NormalSubRoom::NormalSubRoom() : SubRoom()
{
}
......@@ -736,9 +683,8 @@ 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");
......@@ -799,47 +745,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();
......@@ -872,6 +826,28 @@ bool NormalSubRoom::ConvertLineToPoly(vector<Line*> goals)
return false;
}
_poly = tmpPoly;
//check if all walls and goals were used in the polygon
for (const auto& w: _walls)
{
if( ! ( IsElementInVector(_poly,w.GetPoint1()) and
IsElementInVector(_poly,w.GetPoint2())) )
{
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)
{
if( ! ( IsElementInVector(_poly,g->GetPoint1()) and
IsElementInVector(_poly,g->GetPoint2())) )
{
Log->Write("ERROR:\t goal was not used during polygon creation for subroom: %s",g->toString().c_str());
return false;
}
}
return true;
}
......@@ -987,10 +963,8 @@ 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");
......@@ -1048,8 +1022,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();
}
}
......@@ -1073,9 +1047,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 > ();
......@@ -1087,12 +1060,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 = &copy[0]->GetPoint1();
firstAktPoint = aktPoint;
otherPoint = &copy[0]->GetPoint2();
......@@ -1142,8 +1123,10 @@ bool Stair::ConvertLineToPoly(vector<Line*> goals)
}
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();
......@@ -1151,11 +1134,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;
......@@ -1164,6 +1147,27 @@ bool Stair::ConvertLineToPoly(vector<Line*> goals)
}
_poly = neuPoly;
//check if all walls and goals were used in the polygon
// for (const auto& w: _walls)
// {
// if( ! ( IsElementInVector(_poly,w.GetPoint1()) and
// IsElementInVector(_poly,w.GetPoint2())) )
// {
// Log->Write("ERROR:\t Wall was not used during polygon creation for stair: %s",w.toString().c_str());
// return false;
// }
// }
//
// for (const auto& g: goals)
// {
// if( ! ( IsElementInVector(_poly,g->GetPoint1()) and
// IsElementInVector(_poly,g->GetPoint2())) )
// {
// Log->Write("ERROR:\t goal was not used during polygon creation for stair: %s",g->toString().c_str());
// return false;
// }
// }
return true;
}
......@@ -1173,7 +1177,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))
......
geometry/SubRoom.h
View file @ 1e191222
......@@ -30,9 +30,7 @@
#define _SUBROOM_H
//#include "Line.h"
#include "Wall.h"
//#include "Point.h"
#include <vector>
#include <string>
......@@ -70,12 +68,12 @@ private:
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 +81,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();
/**
......@@ -113,11 +118,6 @@ public:
*/
int GetSubRoomID() const;
/**
* @return the number of walls forming this subroom
*/
int GetNumberOfWalls() const;
/**
* @return all walls
*/
......@@ -128,11 +128,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 +173,6 @@ public:
*/
void SetType(const std::string& type);
/**
* @return the status
*/
......@@ -248,6 +242,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,6 +251,17 @@ public: