Commit e31ae853 authored by tobias schroedter's avatar tobias schroedter

TGF simple, some hard coded stuff

parent 729548fa
Pipeline #22069 failed with stages
in 170 minutes and 54 seconds
......@@ -1293,8 +1293,8 @@ void Pedestrian::LeaveGoal()
bool Pedestrian::IsWaiting() const
{
// return _waiting;
return true;
return _waiting;
// return true;
}
const Point& Pedestrian::GetWaitingPos() const
......
......@@ -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;
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment