Commit bdfb5d01 authored by tobias schroedter's avatar tobias schroedter

WIP commit

parent 6fd2a795
Pipeline #17430 failed with stages
in 15 seconds
......@@ -372,7 +372,7 @@ Point VelocityModel::e0(Pedestrian* ped, Room* room) const
if (desired_direction.NormSquare() < 0.25) {
desired_direction = lastE0;
ped->SetLastE0(lastE0);
Log->Write("desired_direction: %f %f", desired_direction._x, desired_direction._y);
// Log->Write("desired_direction: %f %f", desired_direction._x, desired_direction._y);
//_direction->GetTarget(room, ped);
}
// if (dist > 1*J_EPS_GOAL) {
......
......@@ -710,7 +710,7 @@ Point DirectionSubLocalFloorfieldTripsVoronoi::GetTarget(Room* room, Pedestrian*
{
Goal* goal = ped->GetBuilding()->GetFinalGoal(ped->GetFinalDestination());
// Pedestrian is inside a waiting area
if (goal->IsInsideGoal(ped->GetPos())){
if ((goal!=nullptr) && (goal->IsInsideGoal(ped->GetPos()))){
std::vector<Point> polygon(goal->GetPolygon());
std::set<Point> triangle;
......
......@@ -368,47 +368,13 @@ void FloorfieldViaFMTrips::createMapEntryInLineToGoalID(const int goalID, bool i
long int dummykey;
if (isInside) {
// for (const auto& loccross : crossings) {
// std::cout << "Check crossing ID " << loccross.second->GetID() << " " << loccross.second->toString() << std::endl;
// if ( !loccross.second->IsOpen()) {
// continue;
// }
// dummykey = _grid->getKeyAtPoint(loccross.second->GetCentre());
// if ((cost_of_MIN>localcostptr[dummykey]) && (localcostptr[dummykey]>=0.)) {
// UID_of_MIN3 = UID_of_MIN2;
// cost_of_MIN3 = cost_of_MIN2;
//
// UID_of_MIN2 = UID_of_MIN;
// cost_of_MIN2 = cost_of_MIN;
//
// UID_of_MIN = loccross.second->GetUniqueID();
// cost_of_MIN = localcostptr[dummykey];
// std::cout << "Closer Line found: " << UID_of_MIN << " " << cost_of_MIN << std::endl;
// continue;
// }
// if ((cost_of_MIN2>localcostptr[dummykey]) && (localcostptr[dummykey]>=0.)) {
// UID_of_MIN3 = UID_of_MIN2;
// cost_of_MIN3 = cost_of_MIN2;
//
// UID_of_MIN2 = loccross.second->GetUniqueID();
// cost_of_MIN2 = localcostptr[dummykey];
// continue;
// }
// if ((cost_of_MIN3>localcostptr[dummykey]) && (localcostptr[dummykey]>=0.)) {
// UID_of_MIN3 = loccross.second->GetUniqueID();
// cost_of_MIN3 = localcostptr[dummykey];
// continue;
// }
//
// UID_of_MIN = 33;
//
// }
UID_of_MIN = allgoals.at(goalID)->GetCentreCrossing()->GetUniqueID();
}
else {
} else {
for (const auto& loctrans : transitions) {
if (!loctrans.second->IsExit() || !loctrans.second->IsOpen()) {
continue;
// if (!loctrans.second->IsExit() || !loctrans.second->IsOpen()) {
if (!loctrans.second->IsExit()) {
continue;
}
dummykey = _grid->getKeyAtPoint(loctrans.second->GetCentre());
if ((cost_of_MIN>localcostptr[dummykey]) && (localcostptr[dummykey]>=0.)) {
......@@ -528,10 +494,9 @@ void FloorfieldViaFMTrips::parseBuilding(const Building* const buildingArg, cons
//create a list of walls
const std::map<int, Transition*>& allTransitions = buildingArg->GetAllTransitions();
for (auto& trans : allTransitions) {
if (
trans.second->IsExit() && trans.second->IsOpen()
)
{
// TODO temp_close
// if (trans.second->IsExit() && trans.second->IsOpen())
if (trans.second->IsExit() ) {
_exitsFromScope.emplace_back(Line ( (Line) *(trans.second)));
}
//populate both maps: costmap, neggradmap. These are the lookup maps for floorfields to specific transitions
......@@ -539,12 +504,15 @@ void FloorfieldViaFMTrips::parseBuilding(const Building* const buildingArg, cons
_neggradmap.emplace(trans.second->GetUniqueID(), nullptr);
}
_numOfExits = (unsigned int) _exitsFromScope.size();
for (auto& trans : allTransitions) {
if (!trans.second->IsOpen()) {
_wall.emplace_back(Line ( (Line) *(trans.second)));
}
}
// TODO temp_close
// for (auto& trans : allTransitions) {
// if (!trans.second->IsOpen()) {
// _wall.emplace_back(Line ( (Line) *(trans.second)));
// }
//
// }
for (const auto& itRoom : buildingArg->GetAllRooms()) {
for (const auto& itSubroom : itRoom.second->GetAllSubRooms()) {
std::vector<Obstacle*> allObstacles = itSubroom.second->GetAllObstacles();
......@@ -701,10 +669,9 @@ void FloorfieldViaFMTrips::parseBuildingForExits(const Building* const buildingA
//create a list of walls
const std::map<int, Transition*>& allTransitions = buildingArg->GetAllTransitions();
for (auto& trans : allTransitions) {
if (
trans.second->IsExit() && trans.second->IsOpen()
)
{
// TODO temp_close
// if ( trans.second->IsExit() && trans.second->IsOpen() ) {
if ( trans.second->IsExit() ) {
_exitsFromScope.emplace_back(Line ( (Line) *(trans.second)));
int roomID = -1;
if (trans.second->GetRoom1()) {
......@@ -722,12 +689,13 @@ void FloorfieldViaFMTrips::parseBuildingForExits(const Building* const buildingA
_neggradmap.emplace(trans.second->GetUniqueID(), nullptr);
}
_numOfExits = (unsigned int) _exitsFromScope.size();
for (auto& trans : allTransitions) {
if (!trans.second->IsOpen()) {
_wall.emplace_back(Line ( (Line) *(trans.second)));
}
}
// TODO temp_close
// for (auto& trans : allTransitions) {
// if (!trans.second->IsOpen()) {
// _wall.emplace_back(Line ( (Line) *(trans.second)));
// }
//
// }
for (const auto& itRoom : buildingArg->GetAllRooms()) {
if (std::find(exitRoomIDs.begin(), exitRoomIDs.end(), itRoom.second->GetID()) == exitRoomIDs.end()) { //room with no exit
continue;
......
......@@ -101,36 +101,17 @@ UnivFFviaFMTrips::UnivFFviaFMTrips(Room* roomArg, Configuration* const confArg,
for (auto& trans : tmpTrans) {
uidNotConst = trans->GetUniqueID();
isOpen = trans->IsOpen();
if (!isOpen) {
//will be added twice! is it a problem?
lines.emplace_back((Line)*trans);
} else {
// TODO temp_close
// if (!isOpen) {
// //will be added twice! is it a problem?
// lines.emplace_back((Line)*trans);
// } else {
anyDoor = Line{*trans};
if (tmpDoors.count(uidNotConst) == 0) {
tmpDoors.emplace(std::make_pair(uidNotConst, (Line) *trans));
}
}
}
// //save all waiting area walls in tmpdoors
// for (auto& goalMap : goals){
// Goal* goal = goalMap.second;
// if(WaitingArea* wa = dynamic_cast<WaitingArea*>(goal)) {
// std::cout << "Testing for subroom " << subRoomPtr->GetSubRoomID() << std::endl;
// if ((subRoomPtr->IsInSubRoom(wa->GetCentroid())) && (wa->isOpen())){
// for (const Wall wall : wa->GetAllWalls()){
// int uid = wall.GetUniqueID();
// if (tmpDoors.count(uid) == 0) {
// tmpDoors.emplace(std::make_pair(uid, (Line) wall));
// }
// }
// }
// }
// }
// for (auto& door : tmpDoors){
// std::cout << "uid: " << door.first << " door: " << door.second.toString() << std::endl;
// }
}
//find insidePoint and save it, together with UID
Point normalVec = anyDoor.NormalVec();
......@@ -204,12 +185,13 @@ UnivFFviaFMTrips::UnivFFviaFMTrips(SubRoom* subRoomArg, Configuration* const con
}
for (auto& trans : tmpTrans) {
uidNotConst = trans->GetUniqueID();
// TODO temp_close
isOpen = trans->IsOpen();
if (!isOpen) {
lines.emplace_back((Line)*trans);
} else {
// if (!isOpen) {
// lines.emplace_back((Line)*trans);
// } else {
tmpDoors.emplace(std::make_pair(uidNotConst, (Line) *trans));
}
// }
}
_building->GetAllGoals();
......@@ -347,6 +329,7 @@ void UnivFFviaFMTrips::processGeometry(std::vector<Line>&walls, std::map<int, Li
for (auto mapentry : doors) {
_doors.insert(mapentry);
std::cout << "Added: " << mapentry.first << std::endl;
}
//_doors = doors;
......@@ -1741,6 +1724,10 @@ double UnivFFviaFMTrips::getCostToDestination(const int destID, const Point& pos
}
double UnivFFviaFMTrips::getDistanceBetweenDoors(const int door1_ID, const int door2_ID) {
std::cout << " ------------ " << std::endl;
for (auto door : _doors){
std::cout << door.first << " " << door.second.toString() << std::endl;
}
assert(_doors.count(door1_ID) != 0);
assert(_doors.count(door2_ID) != 0);
......
......@@ -137,17 +137,20 @@ bool FFRouterTrips::Init(Building* building)
roomAndCroTrVector.clear();
for (auto& pair:allTrans) {
if (pair.second->IsOpen()) {
// TODO temp_close
// if (pair.second->IsOpen()) {
_allDoorUIDs.emplace_back(pair.second->GetUniqueID());
_CroTrByUID.insert(std::make_pair(pair.second->GetUniqueID(), pair.second));
if (pair.second->IsExit()) {
_ExitsByUID.insert(std::make_pair(pair.second->GetUniqueID(), pair.second));
}
Room* room1 = pair.second->GetRoom1();
if (room1) roomAndCroTrVector.emplace_back(std::make_pair(room1->GetID(), pair.second->GetUniqueID()));
if (room1)
roomAndCroTrVector.emplace_back(std::make_pair(room1->GetID(), pair.second->GetUniqueID()));
Room* room2 = pair.second->GetRoom2();
if (room2) roomAndCroTrVector.emplace_back(std::make_pair(room2->GetID(), pair.second->GetUniqueID()));
}
if (room2)
roomAndCroTrVector.emplace_back(std::make_pair(room2->GetID(), pair.second->GetUniqueID()));
// }
}
for (auto& pair:allCross) {
......@@ -164,9 +167,9 @@ bool FFRouterTrips::Init(Building* building)
std::sort(_allDoorUIDs.begin(), _allDoorUIDs.end());
_allDoorUIDs.erase( std::unique(_allDoorUIDs.begin(),_allDoorUIDs.end()), _allDoorUIDs.end());
// for (auto& door : _allDoorUIDs){
// std::cout << "Door: " << door << std::endl;
// }
for (auto& door : _allDoorUIDs){
std::cout << "Door: " << door << std::endl;
}
//cleanse maps
_distMatrix.clear();
......@@ -553,6 +556,7 @@ int FFRouterTrips::FindExit1(Pedestrian* p)
if (!_targetWithinSubroom) {
//candidates of current room (ID) (provided by Room)
for (auto transUID : _building->GetRoom(p->GetRoomID())->GetAllTransitionsIDs()) {
// TODO temp_close
if ((_CroTrByUID.count(transUID) != 0) && (_CroTrByUID[transUID]->IsOpen())) {
DoorUIDsOfRoom.emplace_back(transUID);
}
......@@ -575,6 +579,7 @@ int FFRouterTrips::FindExit1(Pedestrian* p)
}
for (auto &transI : _building->GetRoom(p->GetRoomID())->GetSubRoom(p->GetSubRoomID())->GetAllTransitions()) {
// TODO temp_close
if (transI->IsOpen()) {
DoorUIDsOfRoom.emplace_back(transI->GetUniqueID());
}
......
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