diff --git a/pedestrian/Pedestrian.cpp b/pedestrian/Pedestrian.cpp
index c945270e57cbbdeac0464595944da442ec182a91..e7d46fd461e1c6d724c88415af49431e097b41d5 100644
--- a/pedestrian/Pedestrian.cpp
+++ b/pedestrian/Pedestrian.cpp
@@ -1293,8 +1293,8 @@ void Pedestrian::LeaveGoal()
bool Pedestrian::IsWaiting() const
{
-// return _waiting;
- return true;
+ return _waiting;
+// return true;
}
const Point& Pedestrian::GetWaitingPos() const
diff --git a/routing/direction/waiting/WaitingProbability.cpp b/routing/direction/waiting/WaitingProbability.cpp
index 36b10b622c213ee3015b4b40423d31b484a7c673..440a16aee04fc0d32efde2ab2c0261f3302c8575 100644
--- a/routing/direction/waiting/WaitingProbability.cpp
+++ b/routing/direction/waiting/WaitingProbability.cpp
@@ -32,8 +32,8 @@ void WaitingProbability::Init(Building* building){
void WaitingProbability::parseBuilding(Building* building){
Log->Write("Start parseBuilding");
- double stepSizeX = 2.*0.125;
- double stepSizeY = 2.*0.125;
+ double stepSizeX = 0.5*0.125;
+ double stepSizeY = 0.5*0.125;
std::vector<Line> _wall;
std::vector<Line> _exitsFromScope;
@@ -112,9 +112,12 @@ void WaitingProbability::parseBuilding(Building* building){
}
Point WaitingProbability::GetWaitingPosition(Room* room, Pedestrian* ped){
- SubRoom* subRoom = room->GetSubRoom(ped->GetSubRoomID());
- int uid = subRoom->GetUID();
+// int uid = subRoom->GetUID();
+ int uid = 1;
+
+ SubRoom* subRoom = room->GetSubRoom(uid);
+
double x=0., y=0.;
int gridSize = _gridMap.at(uid)->GetnPoints();
@@ -155,7 +158,7 @@ Point WaitingProbability::GetWaitingPosition(Room* room, Pedestrian* ped){
x = _gridMap.at(uid)->get_x_fromKey(i);
y = _gridMap.at(uid)->get_y_fromKey(i);
- if (subRoom->IsInSubRoom(Point(x,y)) && distanceField.at(i) > 0.75 ){
+ if (subRoom->IsInSubRoom(Point(x,y)) && distanceField.at(i) > 0.75){
bool inObs = false;
for (auto obs : subRoom->GetAllObstacles()){
if (obs->Contains(Point(x,y))){
@@ -492,8 +495,8 @@ void WaitingProbability::computeDynamicDistance(const SubRoom* subroom, std::vec
double minDist = DBL_MAX;
int step = 1;
-// for (auto trans : subroom->GetAllTransitions()){
- for (auto trans : _transitions){
+ for (auto trans : subroom->GetAllTransitions()){
+// for (auto trans : _transitions){
Point p1 = trans->GetPoint1();
Point p2 = trans->GetPoint2();
Point direction = p2 -p1;
@@ -663,11 +666,12 @@ void WaitingProbability::combineAll(const SubRoom* subroom,
for (int i = 0; i<_gridMap.at(uid)->GetnPoints(); ++i) {
double value =
- 10.*_flowMap.at(uid).at(i)+
+ 1000.*_flowMap.at(uid).at(i)+
1.*_angleMap.at(uid).at(i)+
- 50.*_attractionRepulsionMap.at(uid).at(i)+
- 50.*_wallPreferenceMap.at(uid).at(i) +
- 100.*distDyn.at(i) ;
+ 1000.*_attractionRepulsionMap.at(uid).at(i)+
+ 5.*_wallPreferenceMap.at(uid).at(i) +
+ 1.*distDyn.at(i) ;
+ 1.*distDyn.at(i) ;
double filter = distProb.at(i) * _forbiddenMap.at(uid).at(i);
_valueMap.at(uid).at(i) = value;
@@ -753,8 +757,8 @@ void WaitingProbability::computeFlowAvoidance(const SubRoom* subroom){
Point xAxis(1., 0.);
double flow = 0.;
-// for (auto trans : subroom->GetAllTransitions()) {
- for (auto trans : _transitions){
+ for (auto trans : subroom->GetAllTransitions()) {
+// for (auto trans : _transitions){
flow = 0.;
Point centre = trans->GetCentre();
@@ -872,12 +876,12 @@ void WaitingProbability::computeWallDistance(const SubRoom* subroom){
if (subroom->IsInSubRoom(p)){
-// for (auto line : subroom->GetAllTransitions()) {
-// minDist = std::min(minDist, line->DistTo(p));
-// }
-// for (auto line : subroom->GetAllWalls()){
-// minDist = std::min(minDist, line.DistTo(p));
-// }
+ for (auto line : subroom->GetAllTransitions()) {
+ minDist = std::min(minDist, line->DistTo(p));
+ }
+ for (auto line : subroom->GetAllWalls()){
+ minDist = std::min(minDist, line.DistTo(p));
+ }
for (auto obs : subroom->GetAllObstacles()){
for (auto line : obs->GetAllWalls()){
@@ -903,7 +907,7 @@ void WaitingProbability::computeWallPreference(const SubRoom* subroom){
if (subroom->IsInSubRoom(p)) {
double dist = _wallDistanceMap.at(uid).at(i);
- _wallPreferenceMap.at(uid).at(i) = 1. * exp(-1* dist *dist);
+ _wallPreferenceMap.at(uid).at(i) = 1. * exp(-1* dist *dist/0.5);
}
}
@@ -916,20 +920,31 @@ void WaitingProbability::computeAttractionRepulsionZones(const SubRoom* subroom)
int uid = subroom->GetUID();
//TODO read from file
- double xMinAtt1 = 1;
- double xMaxAtt1 = 4;
- double yMinAtt1 = -2;
- double yMaxAtt1 = 2;
+// double xMinAtt1 = 1;
+// double xMaxAtt1 = 4;
+// double yMinAtt1 = -2;
+// double yMaxAtt1 = 2;
+//
+// double xMinAtt2 = 20;
+// double xMaxAtt2 = 24;
+// double yMinAtt2 = -0.5;
+// double yMaxAtt2 = 0.5;
+//
+// double xMinAtt3 = 36;
+// double xMaxAtt3 = 39;
+// double yMinAtt3 = -2;
+// double yMaxAtt3 = 2;
+
+ double xMinAtt1 = 0.5;
+ double xMaxAtt1 = 1.5;
+ double yMinAtt1 = 3.5;
+ double yMaxAtt1 = 4.5;
- double xMinAtt2 = 20;
- double xMaxAtt2 = 24;
- double yMinAtt2 = -0.5;
- double yMaxAtt2 = 0.5;
+ double xMinAtt2 = 4.5;
+ double xMaxAtt2 = 5.5;
+ double yMinAtt2 = 3.5;
+ double yMaxAtt2 = 4.5;
- double xMinAtt3 = 36;
- double xMaxAtt3 = 39;
- double yMinAtt3 = -2;
- double yMaxAtt3 = 2;
for (int i=0; i< _gridMap.at(uid)->GetnPoints(); ++i) {
double x = _gridMap.at(uid)->get_x_fromKey(i);
@@ -941,12 +956,13 @@ void WaitingProbability::computeAttractionRepulsionZones(const SubRoom* subroom)
_attractionRepulsionMap.at(uid).at(i) = 1.;
}
if ( xMinAtt2 <= x && x <= xMaxAtt2 && yMinAtt2 <= y && y <= yMaxAtt2){
- _attractionRepulsionMap.at(uid).at(i) = 1.;
- }
- if ( xMinAtt3 <= x && x <= xMaxAtt3 && yMinAtt3 <= y && y <= yMaxAtt3){
- _attractionRepulsionMap.at(uid).at(i) = 1.;
+ _attractionRepulsionMap.at(uid).at(i) = -1.;
}
+// if ( xMinAtt3 <= x && x <= xMaxAtt3 && yMinAtt3 <= y && y <= yMaxAtt3){
+// _attractionRepulsionMap.at(uid).at(i) = 1.;
+// }
+
// if ( xMinRep <= x && x <= xMaxRep && yMinRep <= y && y <= yMaxRep){
// _attractionRepulsionMap.at(uid).at(i) = 1.;
// }
@@ -962,20 +978,20 @@ void WaitingProbability::computeForbiddenZones(const SubRoom* subroom){
int uid = subroom->GetUID();
//TODO read from file
- double xMin1 = 0;
- double xMax1 = 40;
- double yMin1 = 4.5;
- double yMax1 = 5;
-
- double xMin2 = 0;
- double xMax2 = 40;
- double yMin2 = -5;
- double yMax2 = -4.5;
-
- double xMin3 = 5;
- double xMax3 = 12;
- double yMin3 = -2;
- double yMax3 = 2;
+// double xMin1 = 0;
+// double xMax1 = 40;
+// double yMin1 = 4.5;
+// double yMax1 = 5;
+//
+// double xMin2 = 0;
+// double xMax2 = 40;
+// double yMin2 = -5;
+// double yMax2 = -4.5;
+//
+// double xMin3 = 5;
+// double xMax3 = 12;
+// double yMin3 = -2;
+// double yMax3 = 2;
for (int i=0; i< _gridMap.at(uid)->GetnPoints(); ++i) {
double x = _gridMap.at(uid)->get_x_fromKey(i);
@@ -983,15 +999,15 @@ void WaitingProbability::computeForbiddenZones(const SubRoom* subroom){
Point p(x, y);
if (subroom->IsInSubRoom(p)) {
- if ( xMin1 <= x && x <= xMax1 && yMin1 <= y && y <= yMax1){
- _forbiddenMap.at(uid).at(i) = 0.;
- } else if ( xMin2 <= x && x <= xMax2 && yMin2 <= y && y <= yMax2){
- _forbiddenMap.at(uid).at(i) = 0.;
- } else if ( xMin3 <= x && x <= xMax3 && yMin3 <= y && y <= yMax3){
- _forbiddenMap.at(uid).at(i) = 0.;
- } else {
+// if ( xMin1 <= x && x <= xMax1 && yMin1 <= y && y <= yMax1){
+// _forbiddenMap.at(uid).at(i) = 0.;
+// } else if ( xMin2 <= x && x <= xMax2 && yMin2 <= y && y <= yMax2){
+// _forbiddenMap.at(uid).at(i) = 0.;
+// } else if ( xMin3 <= x && x <= xMax3 && yMin3 <= y && y <= yMax3){
+// _forbiddenMap.at(uid).at(i) = 0.;
+// } else {
_forbiddenMap.at(uid).at(i) = 1.;
- }
+// }
}
}
}
@@ -1015,8 +1031,8 @@ void WaitingProbability::computeAngleCost(const SubRoom* subroom){
Point p(x, y);
if (subroom->IsInSubRoom(p)) {
-// for (auto trans : subroom->GetAllTransitions()) {
- for (auto trans : _transitions) {
+ for (auto trans : subroom->GetAllTransitions()) {
+// for (auto trans : _transitions) {
Transition normal;
normal.SetPoint1(trans->GetCentre());
normal.SetPoint2(trans->GetCentre()+trans->NormalVec());
@@ -1031,14 +1047,14 @@ void WaitingProbability::computeAngleCost(const SubRoom* subroom){
minAngle = 0.5 * pi;
}
- if (x < 12){
+ if (y > 3){
minAngle = 0.5 * pi;
}
// minAngle = std::max(minAngle, tmpAngle);
}
- _angleMap.at(uid).at(i) = -1. * a * std::pow(minAngle, alpha);
-// _angleMap.at(uid).at(i) = minAngle;
+// _angleMap.at(uid).at(i) = -1. * a * std::pow(minAngle, alpha);
+ _angleMap.at(uid).at(i) = minAngle;
}
// Point p0 = trans->GetPoint2() - trans->GetPoint1();
//
@@ -1092,7 +1108,7 @@ void WaitingProbability::computeAngleCost(const SubRoom* subroom){
// std::cout << "min value: " << *std::min_element(_angleMap.at(uid).begin(), _angleMap.at(uid).end()) << std::endl;
// markOutside(_angleMap.at(uid), subroom);
- bool foo = true;
+// bool foo = true;
}