diff --git a/geometry/SubRoom.cpp b/geometry/SubRoom.cpp
index 7296664b166bc15c11eb14bd8778182abe1912c1..ede9a6c82ca87e89b31839efe88272b46d83a48f 100644
--- a/geometry/SubRoom.cpp
+++ b/geometry/SubRoom.cpp
@@ -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) {
diff --git a/routing/DirectionStrategy.cpp b/routing/DirectionStrategy.cpp
index 0254a85aa6603d8fe6fa4b0d0a70700591447f46..2db97090a793e1e28f8c3bfbabc9f93905290a68 100644
--- a/routing/DirectionStrategy.cpp
+++ b/routing/DirectionStrategy.cpp
@@ -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;
diff --git a/routing/ff_router/UnivFFviaFM.cpp b/routing/ff_router/UnivFFviaFM.cpp
index 1798a8cdd2cbe06fcb65e2da35b8519a3a633a9f..3dce5bb1b3f02070c3fbf0cc476c29219a8933e0 100644
--- a/routing/ff_router/UnivFFviaFM.cpp
+++ b/routing/ff_router/UnivFFviaFM.cpp
@@ -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(), ' ', '_');
diff --git a/routing/ff_router/UnivFFviaFM.h b/routing/ff_router/UnivFFviaFM.h
index f4424f9a797403eceee66c1c2b208d8488207f11..e69520a15ef34fcad508f6a9c59827210713becd 100644
--- a/routing/ff_router/UnivFFviaFM.h
+++ b/routing/ff_router/UnivFFviaFM.h
@@ -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;
+
};
diff --git a/routing/ff_router/ffRouter.cpp b/routing/ff_router/ffRouter.cpp
index 8b555c0ee9cf034816d08b030a97288e669557c6..9be107666c042e29e19e9242760b316794130dbe 100644
--- a/routing/ff_router/ffRouter.cpp
+++ b/routing/ff_router/ffRouter.cpp
@@ -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;