Commit 746bd5b8 authored by Arne Graf's avatar Arne Graf

UnivFF knows subrooms and in-/outside

parent f02f1a8a
Pipeline #8272 failed with stages
in 14 seconds
......@@ -1068,6 +1068,7 @@ double NormalSubRoom::Xintercept(const Point& point1, const Point& point2, doubl
// This method is called very often in DirectionFloorField, so it should be fast.
// we ignore
//@todo: ar.graf: UnivFF have subroomPtr Info for every gridpoint. Info should be used in DirectionFF instead of this
bool NormalSubRoom::IsInSubRoom(const Point& ped) const
{
for (polygon_type obs:_boostPolyObstacles) {
......
......@@ -404,7 +404,7 @@ void DirectionLocalFloorfield::Init(Building* buildingArg, double stepsize,
newfield->setSpeedMode(FF_HOMO_SPEED);
}
newfield->addAllTargetsParallel();
//newfield->writeFF("ffrouterOfRoom" + std::to_string(roomPair.first) + ".vtk", newfield->getKnownDoorUIDs());
//newfield->writeFF("directionsOfRoom" + std::to_string(roomPair.first) + ".vtk", newfield->getKnownDoorUIDs());
}
end = std::chrono::system_clock::now();
std::chrono::duration<double> elapsed_seconds = end-start;
......
......@@ -99,6 +99,24 @@ UnivFFviaFM::UnivFFviaFM(Room* roomArg, Configuration* const confArg, double hx,
tmpDoors.emplace(std::make_pair(uidNotConst, (Line) *trans));
}
}
//find insidePoint and save it, together with UID
Line anyDoor = Line{tmpDoors.begin()->second};
Point normalVec = anyDoor.NormalVec();
Point midPoint = anyDoor.GetCentre();
Point candidate = midPoint + normalVec * 0.5;
if (subRoomPtr->IsInSubRoom(candidate)) {
//_subroomUIDtoInsidePoint.emplace(std::make_pair(subRoomPtr->GetUID(), candidate));
_subRoomPtrTOinsidePoint.emplace(std::make_pair(subRoomPtr, candidate));
} else {
candidate = candidate - normalVec;
if (subRoomPtr->IsInSubRoom(candidate)) {
//_subroomUIDtoInsidePoint.emplace(std::make_pair(subRoomPtr->GetUID(), candidate));
_subRoomPtrTOinsidePoint.emplace(std::make_pair(subRoomPtr, candidate));
} else {
Log->Write("ERROR:\t In UnivFF InsidePoint Analysis");
}
}
//_subroomUIDtoSubRoomPtr.emplace(std::make_pair(subRoomPtr->GetUID(), subRoomPtr));
}
//this will interpret "useWallDistances" as best as possible. Users should clearify with "setSpeedMode" before calling "AddTarget"
if (useWallDistances) {
......@@ -158,6 +176,25 @@ UnivFFviaFM::UnivFFviaFM(SubRoom* subRoomArg, Configuration* const confArg, doub
}
}
//find insidePoint and save it, together with UID
Line anyDoor = Line{tmpDoors.begin()->second};
Point normalVec = anyDoor.NormalVec();
Point midPoint = anyDoor.GetCentre();
Point candidate = midPoint + normalVec * 0.5;
if (subRoomArg->IsInSubRoom(candidate)) {
//_subroomUIDtoInsidePoint.emplace(std::make_pair(subRoomArg->GetUID(), candidate));
_subRoomPtrTOinsidePoint.emplace(std::make_pair(subRoomArg, candidate));
} else {
candidate = candidate - normalVec;
if (subRoomArg->IsInSubRoom(candidate)) {
//_subroomUIDtoInsidePoint.emplace(std::make_pair(subRoomArg->GetUID(), candidate));
_subRoomPtrTOinsidePoint.emplace(std::make_pair(subRoomArg, candidate));
} else {
Log->Write("ERROR:\t In UnivFF InsidePoint Analysis");
}
}
//_subroomUIDtoSubRoomPtr.emplace(std::make_pair(subRoomArg->GetUID(), subRoomArg));
//this will interpret "useWallDistances" as best as possible. Users should clearify with "setSpeedMode" before calling "AddTarget"
if (useWallDistances) {
create(lines, tmpDoors, wantedDoors, FF_WALL_AVOID, hx, wallAvoid, useWallDistances);
......@@ -186,6 +223,11 @@ void UnivFFviaFM::create(std::vector<Line>& walls, std::map<int, Line>& doors, s
_speedFieldSelector.emplace(_speedFieldSelector.begin()+REDU_WALL_SPEED, nullptr);
_speedFieldSelector.emplace(_speedFieldSelector.begin()+PED_SPEED, nullptr);
//mark Inside areas (dont write outside areas)
for (auto subRoomPointPair : _subRoomPtrTOinsidePoint) {
markSubroom(subRoomPointPair.second, subRoomPointPair.first);
}
//allocate _modifiedSpeed
if ((_speedmode == FF_WALL_AVOID) || (useWallDistances)) {
double* cost_alias_walldistance = new double[_nPoints];
......@@ -274,6 +316,9 @@ void UnivFFviaFM::processGeometry(std::vector<Line>&walls, std::map<int, Line>&
void UnivFFviaFM::markSubroom(const Point& insidePoint, SubRoom* const value) {
//value must not be nullptr. it would lead to infinite loop
if (!value) return;
if(!_grid->includesPoint(insidePoint)) return;
//alloc mem if needed
if (!_subrooms) {
_subrooms = new SubRoom*[_nPoints];
......@@ -685,25 +730,25 @@ void UnivFFviaFM::calcFF(double* costOutput, Point* directionOutput, const doubl
//check for valid neigh
aux = local_neighbor.key[0];
if ((aux != -2) && (_gridCode[aux] != WALL) && (costOutput[aux] < 0.0)) {
if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
calcCost(aux, costOutput, directionOutput, speed);
trialfield.emplace(aux);
//trialfield2.emplace(aux);
}
aux = local_neighbor.key[1];
if ((aux != -2) && (_gridCode[aux] != WALL) && (costOutput[aux] < 0.0)) {
if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
calcCost(aux, costOutput, directionOutput, speed);
trialfield.emplace(aux);
//trialfield2.emplace(aux);
}
aux = local_neighbor.key[2];
if ((aux != -2) && (_gridCode[aux] != WALL) && (costOutput[aux] < 0.0)) {
if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
calcCost(aux, costOutput, directionOutput, speed);
trialfield.emplace(aux);
//trialfield2.emplace(aux);
}
aux = local_neighbor.key[3];
if ((aux != -2) && (_gridCode[aux] != WALL) && (costOutput[aux] < 0.0)) {
if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
calcCost(aux, costOutput, directionOutput, speed);
trialfield.emplace(aux);
//trialfield2.emplace(aux);
......@@ -717,25 +762,25 @@ void UnivFFviaFM::calcFF(double* costOutput, Point* directionOutput, const doubl
//check for valid neigh
aux = local_neighbor.key[0];
if ((aux != -2) && (_gridCode[aux] != WALL) && (costOutput[aux] < 0.0)) {
if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
calcCost(aux, costOutput, directionOutput, speed);
trialfield.emplace(aux);
//trialfield2.emplace(aux);
}
aux = local_neighbor.key[1];
if ((aux != -2) && (_gridCode[aux] != WALL) && (costOutput[aux] < 0.0)) {
if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
calcCost(aux, costOutput, directionOutput, speed);
trialfield.emplace(aux);
//trialfield2.emplace(aux);
}
aux = local_neighbor.key[2];
if ((aux != -2) && (_gridCode[aux] != WALL) && (costOutput[aux] < 0.0)) {
if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
calcCost(aux, costOutput, directionOutput, speed);
trialfield.emplace(aux);
//trialfield2.emplace(aux);
}
aux = local_neighbor.key[3];
if ((aux != -2) && (_gridCode[aux] != WALL) && (costOutput[aux] < 0.0)) {
if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
calcCost(aux, costOutput, directionOutput, speed);
trialfield.emplace(aux);
//trialfield2.emplace(aux);
......@@ -877,25 +922,25 @@ void UnivFFviaFM::calcDF(double* costOutput, Point* directionOutput, const doubl
//check for valid neigh
aux = local_neighbor.key[0];
if ((aux != -2) && (_gridCode[aux] != WALL) && (costOutput[aux] < 0.0)) {
if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
calcDist(aux, costOutput, directionOutput, speed);
trialfield.emplace(aux);
//trialfield2.emplace(aux);
}
aux = local_neighbor.key[1];
if ((aux != -2) && (_gridCode[aux] != WALL) && (costOutput[aux] < 0.0)) {
if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
calcDist(aux, costOutput, directionOutput, speed);
trialfield.emplace(aux);
//trialfield2.emplace(aux);
}
aux = local_neighbor.key[2];
if ((aux != -2) && (_gridCode[aux] != WALL) && (costOutput[aux] < 0.0)) {
if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
calcDist(aux, costOutput, directionOutput, speed);
trialfield.emplace(aux);
//trialfield2.emplace(aux);
}
aux = local_neighbor.key[3];
if ((aux != -2) && (_gridCode[aux] != WALL) && (costOutput[aux] < 0.0)) {
if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
calcDist(aux, costOutput, directionOutput, speed);
trialfield.emplace(aux);
//trialfield2.emplace(aux);
......@@ -909,25 +954,25 @@ void UnivFFviaFM::calcDF(double* costOutput, Point* directionOutput, const doubl
//check for valid neigh
aux = local_neighbor.key[0];
if ((aux != -2) && (_gridCode[aux] != WALL) && (costOutput[aux] < 0.0)) {
if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
calcDist(aux, costOutput, directionOutput, speed);
trialfield.emplace(aux);
//trialfield2.emplace(aux);
}
aux = local_neighbor.key[1];
if ((aux != -2) && (_gridCode[aux] != WALL) && (costOutput[aux] < 0.0)) {
if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
calcDist(aux, costOutput, directionOutput, speed);
trialfield.emplace(aux);
//trialfield2.emplace(aux);
}
aux = local_neighbor.key[2];
if ((aux != -2) && (_gridCode[aux] != WALL) && (costOutput[aux] < 0.0)) {
if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
calcDist(aux, costOutput, directionOutput, speed);
trialfield.emplace(aux);
//trialfield2.emplace(aux);
}
aux = local_neighbor.key[3];
if ((aux != -2) && (_gridCode[aux] != WALL) && (costOutput[aux] < 0.0)) {
if ((aux != -2) && (_gridCode[aux] != WALL) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
calcDist(aux, costOutput, directionOutput, speed);
trialfield.emplace(aux);
//trialfield2.emplace(aux);
......@@ -1313,13 +1358,13 @@ void UnivFFviaFM::writeFF(const std::string& filename, std::vector<int> targetID
if (!targetID.empty()) {
for (unsigned int iTarget = 0; iTarget < targetID.size(); ++iTarget) {
Log->Write("%s: target number %d: UID %d", filename.c_str(), iTarget, targetID[iTarget]);
if (_costFieldWithKey.count(targetID[iTarget]) == 0) {
continue;
}
double *costarray = _costFieldWithKey[targetID[iTarget]];
Log->Write("%s: target number %d: UID %d", filename.c_str(), iTarget, targetID[iTarget]);
std::string name = _building->GetTransOrCrossByUID(targetID[iTarget])->GetCaption() + "-" +
std::to_string(targetID[iTarget]);
std::replace(name.begin(), name.end(), ' ', '_');
......
......@@ -167,6 +167,10 @@ private:
std::map<int, Line> _doors;
std::vector<int> _toDo;
std::map<int, Point> _subroomUIDtoInsidePoint;
std::map<int, SubRoom*> _subroomUIDtoSubRoomPtr;
std::map<SubRoom*, Point> _subRoomPtrTOinsidePoint;
};
......
......@@ -325,7 +325,7 @@ bool FFRouter::Init(Building* building)
// for (auto mapItem : _distMatrix) {
// matrixfile << mapItem.first.first << " to " << mapItem.first.second << " : " << mapItem.second << "\t via \t" << _pathsMatrix[mapItem.first];
// matrixfile << "\t" << _CroTrByUID.at(mapItem.first.first)->GetID() << " to " << _CroTrByUID.at(mapItem.first.second)->GetID() << "\t via \t";
// matrixfile << _CroTrByUID.at(_pathsMatrix[mapItem.first])->GetID();
// matrixfile << _CroTrByUID.at(_pathsMatrix[mapItem.first])->GetID() << std::endl;
//// auto sub = _subroomMatrix.at(mapItem.first);
//// if (sub) {
//// matrixfile << std::string("\tSubroom: UID ") << sub->GetUID() << " (room: " << sub->GetRoomID() << " subroom ID: " << sub->GetSubRoomID() << ")" << std::endl;
......
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