Skip to content

GitLab

JPScore
JPScore
Commit 7fc0f133 authored by Mohcine Chraibi's avatar Mohcine Chraibi
Browse files
Options

Fising Strategy 4

parent 74bbbb99
Hide whitespace changes
Inline Side-by-side
Showing with 418 additions and 123 deletions
geometry/Line.cpp
View file @ 7fc0f133
......@@ -25,11 +25,12 @@
*
**/
#include "../math/Mathematics.h"
#include "Point.h"
//#include "SubRoom.h"
#include "../general/Macros.h"
#include "Line.h"
#include "Wall.h"
#include "../IO/OutputHandler.h"
......@@ -41,6 +42,9 @@ int Line::_static_UID=0;
using namespace std;
#define DEBUG 1
/************************************************************
Konstruktoren
************************************************************/
......@@ -521,7 +525,7 @@ std::string Line::toString() const
//insteed of a boolian
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();
......@@ -531,7 +535,8 @@ double Line::GetIntersectionDistance(const Line & l) const
double denominator = deltaBAx * deltaDCy - deltaBAy * deltaDCx;
double numerator = deltaACy * deltaDCx - deltaACx * deltaDCy;
double infinity =100000;
// double infinity =100000;
double infinity = std::numeric_limits<double>::infinity();
// the lines are parallel
if (denominator == 0.0) {
......@@ -568,7 +573,7 @@ double Line::GetIntersectionDistance(const Line & l) const
printf("Enter GetIntersection\n");
cout<< "\t" << l.toString() << " intersects with " << toString() <<endl;
cout<<"\t at point " << PointF.toString()<<endl;
cout << "\t\t --> distance is "<< sqrt(dist)<< "... return "<< dist<<endl;
cout << "\t\t --> distance is "<< sqrt(dist)<< "... return (squared) "<< dist<<endl;
printf("Leave GetIntersection\n");
#endif
return dist;
......@@ -576,7 +581,7 @@ double Line::GetIntersectionDistance(const Line & l) const
}
// calculates the angles QPF and QPL
// return the snagle of the point (F or L) which is nearer to the Goal
// return the angle of the point (F or L) which is nearer to the Goal
//the calling line: P->Q, Q is the crossing point
//
// o P
......@@ -591,7 +596,7 @@ double Line::GetIntersectionDistance(const Line & l) const
double Line::GetDeviationAngle(const Line & l) const
{
// const double PI = 3.14159258;
#define DEBUG 0
Point P = _point1;
Point Goal = _point2;
......@@ -624,9 +629,148 @@ double Line::GetDeviationAngle(const Line & l) const
}
// return the biggest angle between two lanes
double Line::GetAngle(const Line & l) const
{
Point P = _point1;
Point Goal = _point2;
Point L = l._point1;
Point R = l._point2;
double 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));
return (fabs(angleL)>fabs(angleR))?angleL:angleR;
}
// get the smallest angle that ensures a safe deviation from an
// obstacle. Safe means that the rotated line ped--->goal do not
// intersect with any line of the obstacle.
double Line::GetObstacleDeviationAngle(const std::vector<Wall>& owalls) const
{
#if DEBUG
printf("Enter GetObstacleDeviationAngle()\n");
#endif
Point P = _point1;
Point Goal = _point2;
Point GL, GR;
Point L, R;
double minAngle=std::numeric_limits<double>::infinity(),
angleL,
angleR,
angle;
Line l,
l_large,
tmpDirectionL,
tmpDirectionR;
bool visibleL=true,
visibleR=true;
// for (unsigned int i = 0; i < owalls.size(); i++) {
// l = owalls[i];
for(auto l:owalls){
visibleL = true;
visibleR = true;
l_large = l.enlarge(3); // 2*ped->GetLargerAxis()
L = l_large._point1;
R = l_large._point2;
angleL = atan((Goal - P).CrossProduct(L - P)/ (Goal - P).ScalarProduct(L - P));
angleR = atan((Goal - P).CrossProduct(R - P)/ (Goal - P).ScalarProduct(R - P));
GL = (Goal-P).Rotate(cos(angleL), sin(angleL))+P;
GR = (Goal-P).Rotate(cos(angleR), sin(angleR))+P;
tmpDirectionL = Line(P, GL);
tmpDirectionR = Line(P, GR);
for(auto l_other:owalls){
if (l_other == l) continue;
if(tmpDirectionL.IntersectionWith(l_other)){
visibleL = false;
break;
}
}
for(auto l_other:owalls){
if (l_other == l) continue;
if(tmpDirectionR.IntersectionWith(l_other)){
visibleR = false;
break;
}
}
if(visibleR && visibleL){ // both angles are OK. Get
// smallest deviation.
// prefer right
if (almostEqual (angleR, angleL, 0.001))
angle = angleR;
angle = (fabs(angleL) < fabs(angleR))?angleL:angleR;
}
else if(visibleR && !visibleL){
angle = angleR;
}
else if(!visibleR && visibleL){
angle = angleL;
}
else{
printf("continue ");
printf("VisibleL=%d, VisibleR=%d\n", visibleL, visibleR);
continue; // both angles are not OK. check next wall
}
#if DEBUG
printf("\tP=[%f,%f]\n",P.GetX(), P.GetY());
printf("\tGoal=[%f,%f]\n",Goal.GetX(), Goal.GetY());
printf("\tL=[%f,%f]\n",L.GetX(), L.GetY());
printf("\tR=[%f,%f]\n",R.GetX(), R.GetY());
// printf("\t\tdist_Goal_L=%f\n",dist_Goal_L);
// printf("\t\tdist_Goal_R=%f\n",dist_Goal_R);
printf("VisibleL=%d, VisibleR=%d\n", visibleL, visibleR);
printf("\t\t --> angleL=%f\n",angleL);
printf("\t\t --> angleR=%f\n",angleR);
printf("\t\t --> angle=%f\n",angle);
#endif
if(fabs(angle) < fabs(minAngle))
minAngle = angle;
#if DEBUG
printf("\t\t --> minAngle=%f\n", minAngle);
#endif
}// owalls
if(minAngle == std::numeric_limits<double>::infinity()){
printf("WARNING: minAngle ist infinity\n");
getc(stdin);
}
#if DEBUG
printf("Leave GetObstacleDeviationAngle() with angle=%f\n", minAngle);
#endif
return minAngle;
}
......
geometry/Line.h
View file @ 7fc0f133
......@@ -37,6 +37,7 @@
//forward declarations
class OutputHandler;
class Wall;
// external variables
extern OutputHandler* Log;
......@@ -235,6 +236,8 @@ public:
* @return the angle between two lines
*/
double GetDeviationAngle(const Line& l) const;
double GetAngle(const Line & l) const;
double GetObstacleDeviationAngle(const std::vector<Wall>& owalls) const;
// double GetAngle(SubRoom s) const;
Line enlarge(double d) const;
......
inputfiles/Corner/ini.xml
View file @ 7fc0f133
......@@ -63,7 +63,7 @@
<model_parameters>
<solver>euler</solver>
<stepsize>0.001</stepsize>
<exit_crossing_strategy>4</exit_crossing_strategy>
<exit_crossing_strategy>1</exit_crossing_strategy>
<linkedcells enabled="true" cell_size="2.2" />
<force_ped nu="0.3" dist_max="3" disteff_max="2" interpolation_width="0.1" />
<force_wall nu="0." dist_max="3" disteff_max="2" interpolation_width="0.1" />
......@@ -110,8 +110,8 @@
<route_choice_models>
<router router_id="1" description="global_shortest">
<parameters>
<navigation_mesh method="triangulation" use_for_local_planning="false" />
<navigation_lines file="routing.xml" />
<navigation_mesh method="triangulation" use_for_local_planning="true" />
<!-- <navigation_lines file="routing.xml" /> -->
</parameters>
</router>
</route_choice_models>
......
main.cpp
View file @ 7fc0f133
......@@ -64,7 +64,7 @@ int main(int argc, char **argv)
if (sim.GetPedsNumber())
{
Log->Write("WARNING: \nPedestrians not evacuated [%d] using [%d] threads",
Log->Write("WARNING: Pedestrians not evacuated [%d] using [%d] threads",
sim.GetPedsNumber(), args->GetMaxOpenMPThreads());
}
......
math/Mathematics.cpp
View file @ 7fc0f133
......@@ -34,6 +34,15 @@
using namespace std;
// ok that is not perfect. For a profound discussion see http://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/
bool almostEqual (double a, double b, double eps)
{
// std::cout<< "a=" << a << " b=" << b<< "diff= "<<std::fabs(a-b)<<std::endl;
return fabs(a - b) < eps;//std::numeric_limits<double>::epsilon();
}
/*
* Determines the sign of the number x.
* Returns -1 if x is negativ else 1.
......
math/Mathematics.h
View file @ 7fc0f133
......@@ -33,6 +33,8 @@
#include <complex>
#include <cmath>
bool almostEqual (double a, double b, double eps);
double sign(double x);
double sigmoid(double a, double b, double x);
......
pedestrian/Pedestrian.cpp
View file @ 7fc0f133
......@@ -174,7 +174,7 @@ void Pedestrian::SetV(const Point& v)
void Pedestrian::SetV0Norm(double v0,double v0UpStairs, double v0DownStairs)
{
_ellipse.SetV0(v0);
_ellipse.SetV0(v0);
_V0DownStairs=v0DownStairs;
_V0UpStairs=v0UpStairs;
}
......@@ -430,7 +430,7 @@ const Point& Pedestrian::GetV0(const Point& target)
//new_v0 = delta.NormalizedMolified();
new_v0 = delta.Normalized();
// -------------------------------------- Handover new target
t = _newOrientationDelay++ *_deltaT/(1.0+1000* _distToBlockade);
t = _newOrientationDelay++ *_deltaT/(1.0+100* _distToBlockade);
_V0 = _V0 + (new_v0 - _V0)*( 1 - exp(-t/_tau) );
#if DEBUG
......
routing/DirectionStrategy.cpp
View file @ 7fc0f133
......@@ -120,141 +120,278 @@ Point DirectionInRangeBottleneck::GetTarget(Room* room, Pedestrian* ped) const
}
/**
* this strategy is designed to work without Hlines for a general geometry.
* First tested for bottlenecks and corners.
* number 4
* @param room Pointer
* @param ped Pointer to Pedestrians
*
* @todo Need more tests e.g. for complex geometries.
* @todo Need refactoring: Put the WALL and OBS loops in appropriate functions
* @return Target (Point)
*/Point DirectionGeneral::GetTarget(Room* room, Pedestrian* ped) const
/// 4
Point DirectionGeneral::GetTarget(Room* room, Pedestrian* ped) const
{
#define DEBUG 0
using namespace std;
const Point& p1 = ped->GetExitLine()->GetPoint1();
const Point& p2 = ped->GetExitLine()->GetPoint2();
Line ExitLine = Line(p1, p2);
//Point Lot = ExitLine.LotPoint( ped->GetPos() );
double d = 0.2; //shorten the line by 20 cm
Point diff = (p1 - p2).Normalized() * d;
Line e_neu = Line(p1 - diff, p2 + diff);
Point NextPointOnLine = e_neu.ShortestPoint(ped->GetPos());
Line tmpDirection = Line(ped->GetPos(), NextPointOnLine );//This direction will be rotated if
// it intersects a wall || obstacle.
// check for intersection with walls
// @todo: make a FUNCTION of this
#define DEBUG 1
using namespace std;
const Point& p1 = ped->GetExitLine()->GetPoint1();
const Point& p2 = ped->GetExitLine()->GetPoint2();
Line ExitLine = Line(p1, p2);
//Point Lot = ExitLine.LotPoint( ped->GetPos() );
double d = 0.2; //shorten the line by 20 cm
Point diff = (p1 - p2).Normalized() * d;
Line e_neu = Line(p1 - diff, p2 + diff);
Point NextPointOnLine = e_neu.ShortestPoint(ped->GetPos());
Line tmpDirection = Line(ped->GetPos(), NextPointOnLine );//This direction will be rotated if
// it intersects a wall || obstacle.
// check for intersection with walls
// @todo: make a FUNCTION of this
#if DEBUG
printf("\n----------\nEnter GetTarget() with PED=%d\n----------\n",ped->GetID());
printf("nextPointOn Line: %f %f\n", NextPointOnLine.GetX(), NextPointOnLine.GetY());
printf("\n----------\nEnter GetTarget() with PED=%d\n----------\n",ped->GetID());
printf("nextPointOn Line: %f %f\n", NextPointOnLine.GetX(), NextPointOnLine.GetY());
#endif
double dist;
int inear = -1;
int iObs = -1;
double minDist = 20001;
int subroomId = ped->GetSubRoomID();
SubRoom * subroom = room->GetSubRoom(subroomId);
//============================ WALLS ===========================
const vector<Wall>& walls = subroom->GetAllWalls();
for (int i = 0; i < subroom->GetNumberOfWalls(); i++) {
dist = tmpDirection.GetIntersectionDistance(walls[i]);
if (dist < minDist) {
inear = i;
minDist = dist;
double dist;
int inear = -1;
int iObs = -1;
double minDist = 20001;
int subroomId = ped->GetSubRoomID();
SubRoom * subroom = room->GetSubRoom(subroomId);
//============================ WALLS ===========================
const vector<Wall>& walls = subroom->GetAllWalls();
printf("subroom <%d> has %d walls\n", subroomId , subroom->GetNumberOfWalls());
for (int i = 0; i < subroom->GetNumberOfWalls(); i++) {
dist = tmpDirection.GetIntersectionDistance(walls[i]);
printf("CHECK %f %f --- %f %f\n===========\n",walls[i].GetPoint1().GetX(),walls[i].GetPoint1().GetY(), walls[i].GetPoint2().GetX(),walls[i].GetPoint2().GetY());
if (dist < minDist) {
inear = i;
minDist = dist;
#if DEBUG
printf("Check wall %d. Dist = %f (%f)\n", i, dist, minDist);
printf("%f %f --- %f %f\n===========\n",walls[i].GetPoint1().GetX(),walls[i].GetPoint1().GetY(), walls[i].GetPoint2().GetX(),walls[i].GetPoint2().GetY());
printf("Check wall number %d. Dist = %f (%f)\n", i, dist, minDist);
printf("%f %f --- %f %f\n===========\n",walls[i].GetPoint1().GetX(),walls[i].GetPoint1().GetY(), walls[i].GetPoint2().GetX(),walls[i].GetPoint2().GetY());
#endif
}
}//walls
//============================ WALLS ===========================
//============================ OBST ===========================
const vector<Obstacle*>& obstacles = subroom->GetAllObstacles();
for(unsigned int obs=0; obs<obstacles.size(); ++obs) {
const vector<Wall>& owalls = obstacles[obs]->GetAllWalls();
for (unsigned int i = 0; i < owalls.size(); i++) {
dist = tmpDirection.GetIntersectionDistance(owalls[i]);
if (dist < minDist) {
inear = i;
minDist = dist;
iObs = obs;
// printf("Check OBS:obs=%d, i=%d Dist = %f (%f)\n", obs, i, dist, minDist);
// printf("%f %f --- %f %f\n===========\n",owalls[i].GetPoint1().GetX(),owalls[i].GetPoint1().GetY(), owalls[i].GetPoint2().GetX(),owalls[i].GetPoint2().GetY());
}
}//walls of obstacle
}// obstacles
//============================ OBST ===========================
double angle = 0;
if (inear >= 0) {
ped->SetNewOrientationFlag(true); //Mark this pedestrian for next target calculation
ped->SetDistToBlockade(minDist);
if(iObs >= 0) {
const vector<Wall>& owalls = obstacles[iObs]->GetAllWalls();
angle = tmpDirection.GetDeviationAngle(owalls[inear].enlarge(2*ped->GetLargerAxis()));
}
}//walls
//============================ WALLS ===========================
//============================ OBST ===========================
const vector<Obstacle*>& obstacles = subroom->GetAllObstacles();
for(unsigned int obs=0; obs<obstacles.size(); ++obs) {
const vector<Wall>& owalls = obstacles[obs]->GetAllWalls();
for (unsigned int i = 0; i < owalls.size(); i++) {
dist = tmpDirection.GetIntersectionDistance(owalls[i]);
if (dist < minDist) {
inear = i;
minDist = dist;
iObs = obs;
// printf("Check OBS:obs=%d, i=%d Dist = %f (%f)\n", obs, i, dist, minDist);
// printf("%f %f --- %f %f\n===========\n",owalls[i].GetPoint1().GetX(),owalls[i].GetPoint1().GetY(), owalls[i].GetPoint2().GetX(),owalls[i].GetPoint2().GetY());
}
}//walls of obstacle
}// obstacles
//============================ OBST ===========================
double angle = 0;
if (inear >= 0) {
ped->SetNewOrientationFlag(true); //Mark this pedestrian for next target calculation
ped->SetDistToBlockade(minDist);
if(iObs >= 0){ // obstacle is nearest
const vector<Wall>& owalls = obstacles[iObs]->GetAllWalls();
angle = tmpDirection.GetObstacleDeviationAngle(owalls);
// angle = tmpDirection.GetDeviationAngle(owalls[inear].enlarge(2*ped->GetLargerAxis()));
#if DEBUG
printf("COLLISION WITH %f %f --- %f %f\n===========\n",owalls[inear].GetPoint1().GetX(),owalls[inear].GetPoint1().GetY(), owalls[inear].GetPoint2().GetX(),owalls[inear].GetPoint2().GetY());
#endif
} else{
angle = tmpDirection.GetDeviationAngle(walls[inear].enlarge(2*ped->GetLargerAxis()));
printf("COLLISION WITH OBSTACLE %f %f --- %f %f\n===========\n",owalls[inear].GetPoint1().GetX(),owalls[inear].GetPoint1().GetY(), owalls[inear].GetPoint2().GetX(),owalls[inear].GetPoint2().GetY());
#endif
} //iObs
else{ // wall is nearest
angle = tmpDirection.GetDeviationAngle(walls[inear].enlarge(2*ped->GetLargerAxis()));
#if DEBUG
printf("COLLISION WITH %f %f --- %f %f\n===========\n",walls[inear].GetPoint1().GetX(),walls[inear].GetPoint1().GetY(), walls[inear].GetPoint2().GetX(),walls[inear].GetPoint2().GetY());
printf("COLLISION WITH WALL %f %f --- %f %f\n===========\n",walls[inear].GetPoint1().GetX(),walls[inear].GetPoint1().GetY(), walls[inear].GetPoint2().GetX(),walls[inear].GetPoint2().GetY());
#endif
}
}//inear
else{
if(ped->GetNewOrientationFlag()){ //this pedestrian could not see the target and now he can see it clearly.
// printf("ped->GetNewOrientationFlag()=%d\n",ped->GetNewOrientationFlag());getc(stdin);
ped->SetSmoothTurning(); // so the turning should be adapted accordingly.
ped->SetNewOrientationFlag(false);
}
}
} //else
}//inear
else{
if(ped->GetNewOrientationFlag()){ //this pedestrian could not see the target and now he can see it clearly.
// printf("ped->GetNewOrientationFlag()=%d\n",ped->GetNewOrientationFlag());getc(stdin);
ped->SetSmoothTurning(); // so the turning should be adapted accordingly.
ped->SetNewOrientationFlag(false);
}
}
////////////////////////////////////////////////////////////
Point G;
if (fabs(angle) > J_EPS)
//G = tmpDirection.GetPoint2().Rotate(cos(angle), sin(angle)) ;
G = (NextPointOnLine-ped->GetPos()).Rotate(cos(angle), sin(angle))+ped->GetPos() ;
else {
if(ped->GetNewOrientationFlag()) //this pedestrian could not see the target and now he can see it clearly.
ped->SetSmoothTurning(); // so the turning should be adapted accordingly.
Point G;
if (fabs(angle) > J_EPS)
//G = tmpDirection.GetPoint2().Rotate(cos(angle), sin(angle)) ;
G = (NextPointOnLine-ped->GetPos()).Rotate(cos(angle), sin(angle))+ped->GetPos() ;
else {
if(ped->GetNewOrientationFlag()) //this pedestrian could not see the target and now he can see it clearly.
ped->SetSmoothTurning(); // so the turning should be adapted accordingly.
G = NextPointOnLine;
}
G = NextPointOnLine;
}
#if DEBUG
printf("inear=%d, iObs=%d, minDist=%f\n", inear, iObs, minDist);
printf("PED=%d\n", ped->GetID());
printf ("MC Posx = %.2f, Posy=%.2f, Lot=[%.2f, %.2f]\n", ped->GetPos().GetX(), ped->GetPos().GetY(), NextPointOnLine.GetX(), NextPointOnLine.GetY());
printf("MC p1=[%.2f, %.2f] p2=[%.2f, %.2f]\n", p1.GetX(), p1.GetY(), p2.GetX(), p2.GetY());
printf("angle=%f, G=[%.2f, %.2f]\n", angle, G.GetX(), G.GetY());
printf("\n----------\nLEAVE function with PED=%d\n----------\n",ped->GetID());
printf("inear=%d, iObs=%d, minDist=%f\n", inear, iObs, minDist);
printf("PED=%d\n", ped->GetID());
printf ("MC Posx = %.2f, Posy=%.2f, Lot=[%.2f, %.2f]\n", ped->GetPos().GetX(), ped->GetPos().GetY(), NextPointOnLine.GetX(), NextPointOnLine.GetY());
printf("MC p1=[%.2f, %.2f] p2=[%.2f, %.2f]\n", p1.GetX(), p1.GetY(), p2.GetX(), p2.GetY());
printf("angle=%f, G=[%.2f, %.2f]\n", angle, G.GetX(), G.GetY());
printf("\n----------\nLEAVE function with PED=%d\n----------\n",ped->GetID());
#endif
// if( ped->GetID() == 12)
// fprintf(stderr, "%.2f %.2f %.2f %.2f %f %f %d %.2f %.2f %.2f\n", NextPointOnLine.GetX(), NextPointOnLine.GetY(),
// ped->GetPos().GetX(), ped->GetPos().GetY(), G.GetX(), G.GetY(), ped->GetID(), ped->GetV0().GetX(), ped->GetV0().GetY(), ped->GetGlobalTime());
if( ped->GetID() == 21)
fprintf(stderr, "%.2f %.2f %.2f %.2f %f %f %d %.2f %.2f %.2f\n", NextPointOnLine.GetX(), NextPointOnLine.GetY(),
ped->GetPos().GetX(), ped->GetPos().GetY(), G.GetX(), G.GetY(), ped->GetID(), ped->GetV0().GetX(), ped->GetV0().GetY(), ped->GetGlobalTime());
// this stderr output can be used with plot_desired_velocity.py
return G;
return G;
}
/**
* this strategy is designed to work without Hlines for a general geometry.
* First tested for bottlenecks and corners.
* number 4
* @param room Pointer
* @param ped Pointer to Pedestrians
*
* @todo Need more tests e.g. for complex geometries.
* @todo Need refactoring: Put the WALL and OBS loops in appropriate functions
* @return Target (Point)
**/
// */Point DirectionGeneral::GetTarget(Room* room, Pedestrian* ped) const
// {
// #define DEBUG 1
// using namespace std;
// const Point& p1 = ped->GetExitLine()->GetPoint1();
// const Point& p2 = ped->GetExitLine()->GetPoint2();
// Line ExitLine = Line(p1, p2);
// //Point Lot = ExitLine.LotPoint( ped->GetPos() );
// double d = 0.2; //shorten the line by 20 cm
// Point diff = (p1 - p2).Normalized() * d;
// Line e_neu = Line(p1 - diff, p2 + diff);
// Point NextPointOnLine = e_neu.ShortestPoint(ped->GetPos());
// Line tmpDirection = Line(ped->GetPos(), NextPointOnLine );//This direction will be rotated if
// // it intersects a wall || obstacle.
// // check for intersection with walls
// // @todo: make a FUNCTION of this
// #if DEBUG
// printf("\n----------\nEnter GetTarget() with PED=%d\n----------\n",ped->GetID());
// printf("nextPointOn Line: %f %f\n", NextPointOnLine.GetX(), NextPointOnLine.GetY());
// #endif
// double dist;
// int inear = -1;
// int iObs = -1;
// double minDist = 20001;
// int subroomId = ped->GetSubRoomID();
// SubRoom * subroom = room->GetSubRoom(subroomId);
// //============================ WALLS ===========================
// const vector<Wall>& walls = subroom->GetAllWalls();
// for (int i = 0; i < subroom->GetNumberOfWalls(); i++) {
// dist = tmpDirection.GetIntersectionDistance(walls[i]);
// if (dist < minDist) {
// inear = i;
// minDist = dist;
// #if DEBUG
// printf("Check wall %d. Dist = %f (%f)\n", i, dist, minDist);
// printf("%f %f --- %f %f\n===========\n",walls[i].GetPoint1().GetX(),walls[i].GetPoint1().GetY(), walls[i].GetPoint2().GetX(),walls[i].GetPoint2().GetY());
// #endif
// }
// }//walls
// //============================ WALLS ===========================
// //============================ OBST ===========================
// const vector<Obstacle*>& obstacles = subroom->GetAllObstacles();
// for(unsigned int obs=0; obs<obstacles.size(); ++obs) {
// const vector<Wall>& owalls = obstacles[obs]->GetAllWalls();
// for (unsigned int i = 0; i < owalls.size(); i++) {
// dist = tmpDirection.GetIntersectionDistance(owalls[i]);
// if (dist < minDist) {
// inear = i;
// minDist = dist;
// iObs = obs;
// // printf("Check OBS:obs=%d, i=%d Dist = %f (%f)\n", obs, i, dist, minDist);
// // printf("%f %f --- %f %f\n===========\n",owalls[i].GetPoint1().GetX(),owalls[i].GetPoint1().GetY(), owalls[i].GetPoint2().GetX(),owalls[i].GetPoint2().GetY());
// }
// }//walls of obstacle
// }// obstacles