diff --git a/CMakeLists.txt b/CMakeLists.txt
index ba10b6f055486d571f022d8745e6054745a9e1a4..ca53e22230047743ba3c6f75cd7a680c09aa564b 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -11,11 +11,13 @@
#--------------------------------------------------------------------------
cmake_minimum_required(VERSION 3.1 FATAL_ERROR)
-
set(CMAKE_LEGACY_CYGWIN_WIN32 0)
IF (POLICY CMP0054)
cmake_policy(SET CMP0054 NEW)
ENDIF (POLICY CMP0054)
+IF (POLICY CMP0077)
+ cmake_policy(SET CMP0077 OLD)
+ENDIF (POLICY CMP0077)
project(JPScore LANGUAGES CXX)
@@ -32,6 +34,8 @@ else()
set(JPSFIRE true)
endif()
+message(STATUS "BUILD_TESTING01: " ${BUILD_TESTING})
+
set(CMAKE_COLOR_MAKEFILE ON)
set(JPSCORE_MAJOR_VERSION 0)
set(JPSCORE_MINOR_VERSION 8)
@@ -42,7 +46,7 @@ message(STATUS "JPSCORE_VERSION: " ${JPSCORE_VERSION})
add_definitions("-DJPSCORE_VERSION=\"${JPSCORE_VERSION}\"")
if ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU" OR
"${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
- set(warnings "-Wall -Wextra -")
+ set(warnings "-Wall -Wextra")
# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
elseif ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC")
set(warnings "/W4 /WX /EHsc")
@@ -55,7 +59,7 @@ endif ()
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${warnings}")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${warnings}")
-message(STATUS "!BUILD_TESTING: " ${BUILD_TESTING})
+message(STATUS "BUILD_TESTING02: " ${BUILD_TESTING})
if (NOT BUILD_TESTING)
set(BUILD_TESTING OFF) # test units & python tests are not generated.
@@ -72,45 +76,28 @@ if (NOT CMAKE_EXPORT_COMPILE_COMMANDS)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
endif (NOT CMAKE_EXPORT_COMPILE_COMMANDS)
-# Get number of processors. Mac is not supported
-if (NOT DEFINED PROCESSOR_COUNT)
-# Unknown:
-set(PROCESSOR_COUNT 0)
-
-# Linux:
-set(cpuinfo_file "/proc/cpuinfo")
-if (EXISTS "${cpuinfo_file}")
-file(STRINGS "${cpuinfo_file}" procs REGEX "^processor.: [0-9]+$")
-list(LENGTH procs PROCESSOR_COUNT)
-endif (EXISTS "${cpuinfo_file}")
-
-# Windows:
-if (WIN32)
-set(PROCESSOR_COUNT "$ENV{NUMBER_OF_PROCESSORS}")
-endif (WIN32)
-endif (NOT DEFINED PROCESSOR_COUNT)
-
-if (PROCESSOR_COUNT)
-message( STATUS "PROCESSOR_COUNT: " ${PROCESSOR_COUNT})
-# set(CTEST_BUILD_FLAGS "-j${PROCESSOR_COUNT}")
-message(STATUS "PROCESSOR_COUNT: 1")
-
-set(CTEST_BUILD_FLAGS "-j0")
-endif (PROCESSOR_COUNT)
+set(CTEST_BUILD_FLAGS "-j")
if (NOT CMAKE_BUILD_TYPE)
-# set (CMAKE_BUILD_TYPE Release)
-set(CMAKE_BUILD_TYPE Debug)
+ # set (CMAKE_BUILD_TYPE Release)
+ set(CMAKE_BUILD_TYPE Debug)
endif (NOT CMAKE_BUILD_TYPE)
message(STATUS "CMAKE_BUILD_TYPE: " ${CMAKE_BUILD_TYPE})
if(NOT USE_DUAL_ABI)
-set (USE_DUAL_ABI FALSE)
+ set (USE_DUAL_ABI FALSE)
endif()
#------------------ set important directories --------------------
-set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin)
-set(EXECUTABLE_OUTPUT_PATH "${CMAKE_SOURCE_DIR}/bin")
+if (NOT CMAKE_RUNTIME_OUTPUT_DIRECTORY)
+ set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin)
+endif()
+
+if(NOT EXECUTABLE_OUTPUT_PATH)
+ set(EXECUTABLE_OUTPUT_PATH "${CMAKE_SOURCE_DIR}/bin")
+endif()
set(LIBRARY_OUTPUT_PATH ${CMAKE_SOURCE_DIR}/lib/${CMAKE_BUILD_TYPE})
set(CMAKE_TEST_DIR ${CMAKE_SOURCE_DIR}/Utest)
+message(STATUS "BUILD_TESTING03: " ${BUILD_TESTING})
+
# Debug messages
message(STATUS "CMAKE_BINARY_DIR: " ${CMAKE_BINARY_DIR})
message(STATUS "CMAKE_SOURCE_DIR: " ${CMAKE_SOURCE_DIR})
@@ -122,142 +109,134 @@ message(STATUS "")
message(STATUS "Platform: ")
message(STATUS " Host: " ${CMAKE_HOST_SYSTEM_NAME} ${CMAKE_HOST_SYSTEM_VERSION} ${CMAKE_HOST_SYSTEM_PROCESSOR})
if (CMAKE_CROSSCOMPILING)
-message(STATUS " Target: " ${CMAKE_SYSTEM_NAME} ${CMAKE_SYSTEM_VERSION} ${CMAKE_SYSTEM_PROCESSOR})
+ message(STATUS " Target: " ${CMAKE_SYSTEM_NAME} ${CMAKE_SYSTEM_VERSION} ${CMAKE_SYSTEM_PROCESSOR})
endif ()
message(STATUS " CMake: " ${CMAKE_VERSION})
message(STATUS " CMake generator: " ${CMAKE_GENERATOR})
message(STATUS " CMake build tool: " ${CMAKE_BUILD_TOOL})
if (MSVC)
-message(STATUS " MSVC: " ${MSVC_VERSION})
+ message(STATUS " MSVC: " ${MSVC_VERSION})
#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ZLIB_WINAPI)
endif ()
if (CMAKE_GENERATOR MATCHES Xcode)
-message(STATUS " Xcode: " ${XCODE_VERSION})
+ message(STATUS " Xcode: " ${XCODE_VERSION})
endif ()
if (NOT CMAKE_GENERATOR MATCHES "Xcode|Visual Studio")
-message(STATUS " Configuration: " ${CMAKE_BUILD_TYPE})
+ message(STATUS " Configuration: " ${CMAKE_BUILD_TYPE})
endif ()
message(STATUS "")
-# message( STATUS "CMAKE_CURRENT_SOURCE_DIR: " ${CMAKE_CURRENT_SOURCE_DIR} )
-# message( STATUS "CMAKE_RUNTIME_OUTPUT_DIRECTORY: " ${CMAKE_RUNTIME_OUTPUT_DIRECTORY} )
-# message( STATUS "EXECUTABLE_OUTPUT_PATH: " ${EXECUTABLE_OUTPUT_PATH} )
-# message( STATUS "CMAKE_VERBOSE_MAKEFILE: " ${CMAKE_VERBOSE_MAKEFILE} )
-
-find_package(Git REQUIRED) # no need for this msg. It comes from cmake.findgit()
+find_package(Git QUIET) # no need for this msg. It comes from cmake.findgit()
find_program(GIT_SCM git DOC "Git version control")
mark_as_advanced(GIT_SCM)
find_file(GITDIR NAMES .git PATHS ${CMAKE_SOURCE_DIR} NO_DEFAULT_PATH)
if (GIT_SCM AND GITDIR)
-# the commit's SHA1, and whether the building workspace was dirty or not
-# describe --match=NeVeRmAtCh --always --tags --abbrev=40 --dirty
-execute_process(COMMAND
-"${GIT_EXECUTABLE}" --no-pager describe --tags --always --dirty
-WORKING_DIRECTORY "${CMAKE_SOURCE_DIR}"
-OUTPUT_VARIABLE GIT_SHA1
-ERROR_QUIET OUTPUT_STRIP_TRAILING_WHITESPACE)
-
-# branch
-execute_process(
-COMMAND "${GIT_EXECUTABLE}" rev-parse --abbrev-ref HEAD
-WORKING_DIRECTORY "${CMAKE_SOURCE_DIR}"
-OUTPUT_VARIABLE GIT_BRANCH
-OUTPUT_STRIP_TRAILING_WHITESPACE
-)
-
-# the date of the commit
-execute_process(COMMAND
-"${GIT_EXECUTABLE}" log -1 --format=%ad --date=local
-WORKING_DIRECTORY "${CMAKE_SOURCE_DIR}"
-OUTPUT_VARIABLE GIT_DATE
-ERROR_QUIET OUTPUT_STRIP_TRAILING_WHITESPACE)
-
-# the subject of the commit
-execute_process(COMMAND
-"${GIT_EXECUTABLE}" log -1 --format=%s
-WORKING_DIRECTORY "${CMAKE_SOURCE_DIR}"
-OUTPUT_VARIABLE GIT_COMMIT_SUBJECT
-ERROR_QUIET OUTPUT_STRIP_TRAILING_WHITESPACE)
-
-
+ # the commit's SHA1, and whether the building workspace was dirty or not
+ # describe --match=NeVeRmAtCh --always --tags --abbrev=40 --dirty
+ execute_process(COMMAND
+ "${GIT_EXECUTABLE}" --no-pager describe --tags --always --dirty
+ WORKING_DIRECTORY "${CMAKE_SOURCE_DIR}"
+ OUTPUT_VARIABLE GIT_SHA1
+ ERROR_QUIET OUTPUT_STRIP_TRAILING_WHITESPACE)
+
+ # branch
+ execute_process(
+ COMMAND "${GIT_EXECUTABLE}" rev-parse --abbrev-ref HEAD
+ WORKING_DIRECTORY "${CMAKE_SOURCE_DIR}"
+ OUTPUT_VARIABLE GIT_BRANCH
+ OUTPUT_STRIP_TRAILING_WHITESPACE
+ )
+
+ # the date of the commit
+ execute_process(COMMAND
+ "${GIT_EXECUTABLE}" log -1 --format=%ad --date=local
+ WORKING_DIRECTORY "${CMAKE_SOURCE_DIR}"
+ OUTPUT_VARIABLE GIT_DATE
+ ERROR_QUIET OUTPUT_STRIP_TRAILING_WHITESPACE)
+
+ # the subject of the commit
+ execute_process(COMMAND
+ "${GIT_EXECUTABLE}" log -1 --format=%s
+ WORKING_DIRECTORY "${CMAKE_SOURCE_DIR}"
+ OUTPUT_VARIABLE GIT_COMMIT_SUBJECT
+ ERROR_QUIET OUTPUT_STRIP_TRAILING_WHITESPACE)
+else()
+ message(STATUS "Not in a git repo")
+ endif()
+ string(REGEX REPLACE "\#"
+ "" GIT_COMMIT_SUBJECT
+ ${GIT_COMMIT_SUBJECT})
+# even if not in a git repository we need to define these
add_definitions("-DGIT_COMMIT_HASH=\"${GIT_SHA1}\"")
add_definitions("-DGIT_COMMIT_DATE=\"${GIT_DATE}\"")
add_definitions("-DGIT_COMMIT_SUBJECT=\"${GIT_COMMIT_SUBJECT}\"")
add_definitions("-DGIT_BRANCH=\"${GIT_BRANCH}\"")
-else()
-message(STATUS "Not in a git repo")
-endif()
-
# add a target to generate API documentation with Doxygen
find_package(Doxygen)
if (DOXYGEN_FOUND)
-configure_file(${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile.in ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile @ONLY)
-add_custom_target(doc
-${DOXYGEN_EXECUTABLE} ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile
-WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
-COMMENT "Generating API documentation with Doxygen" VERBATIM
-)
+ configure_file(${CMAKE_CURRENT_SOURCE_DIR}/doc/Doxyfile.in ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile @ONLY)
+ add_custom_target(doc
+ ${DOXYGEN_EXECUTABLE} ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile
+ WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+ COMMENT "Generating API documentation with Doxygen" VERBATIM)
endif (DOXYGEN_FOUND)
#http://stackoverflow.com/questions/1487752/how-do-i-instruct-cmake-to-look-for-libraries-installed-by-macports
if (APPLE)
-# Detect if the "port" command is valid on this system; if so, return full path
-execute_process(COMMAND which port RESULT_VARIABLE DETECT_MACPORTS OUTPUT_VARIABLE MACPORTS_PREFIX ERROR_QUIET OUTPUT_STRIP_TRAILING_WHITESPACE)
-if (${DETECT_MACPORTS} EQUAL 0)
-# "/opt/local/bin/port" doesn't have libs, so we get the parent directory
-get_filename_component(MACPORTS_PREFIX ${MACPORTS_PREFIX} DIRECTORY)
-# "/opt/local/bin" doesn't have libs, so we get the parent directory
-get_filename_component(MACPORTS_PREFIX ${MACPORTS_PREFIX} DIRECTORY)
-# "/opt/local" is where MacPorts lives, add `/lib` suffix and link
-link_directories(${MACPORTS_PREFIX}/lib)
-message(STATUS "Macports detected: ${MACPORTS_PREFIX}/lib")
-# SET(CMAKE_SYSTEM_NAME Darwin)
-# # Add MacPorts
-# INCLUDE_DIRECTORIES(/opt/local/include)
-
-# LINK_DIRECTORIES(/opt/local/lib)
-
-else ()
-# Recommendation, also add a "brew --prefix" custom command to detect a homebrew build environment
-execute_process(COMMAND brew --prefix RESULT_VARIABLE DETECT_BREW OUTPUT_VARIABLE BREW_PREFIX ERROR_QUIET OUTPUT_STRIP_TRAILING_WHITESPACE)
-if (${DETECT_BREW} EQUAL 0)
-link_directories(${BREW_PREFIX}/lib)
-message(STATUS "Brew detected: ${BREW_PREFIX}")
-endif ()
-endif ()
-
-
+ # Detect if the "port" command is valid on this system; if so, return full path
+ execute_process(COMMAND which port RESULT_VARIABLE DETECT_MACPORTS OUTPUT_VARIABLE MACPORTS_PREFIX ERROR_QUIET OUTPUT_STRIP_TRAILING_WHITESPACE)
+ if (${DETECT_MACPORTS} EQUAL 0)
+ # "/opt/local/bin/port" doesn't have libs, so we get the parent directory
+ get_filename_component(MACPORTS_PREFIX ${MACPORTS_PREFIX} DIRECTORY)
+ # "/opt/local/bin" doesn't have libs, so we get the parent directory
+ get_filename_component(MACPORTS_PREFIX ${MACPORTS_PREFIX} DIRECTORY)
+ # "/opt/local" is where MacPorts lives, add `/lib` suffix and link
+ link_directories(${MACPORTS_PREFIX}/lib)
+ message(STATUS "Macports detected: ${MACPORTS_PREFIX}/lib")
+ # SET(CMAKE_SYSTEM_NAME Darwin)
+ # # Add MacPorts
+ # INCLUDE_DIRECTORIES(/opt/local/include)
+
+ # LINK_DIRECTORIES(/opt/local/lib)
+
+ else ()
+ # Recommendation, also add a "brew --prefix" custom command to detect a homebrew build environment
+ execute_process(COMMAND brew --prefix RESULT_VARIABLE DETECT_BREW OUTPUT_VARIABLE BREW_PREFIX ERROR_QUIET OUTPUT_STRIP_TRAILING_WHITESPACE)
+ if (${DETECT_BREW} EQUAL 0)
+ link_directories(${BREW_PREFIX}/lib)
+ message(STATUS "Brew detected: ${BREW_PREFIX}")
+ endif ()
+ endif ()
endif (APPLE)
if (Boost_NO_SYSTEM_PATHS)
-set(Boost_NO_SYSTEM_PATHS ON)
-set(BOOST_INCLUDE_DIRS "${BOOST_ROOT}/include")
-set(BOOST_LIBRARY_DIRS "${BOOST_ROOT}/stage/lib")
-set(CMAKE_INCLUDE_PATH ${CMAKE_INCLUDE_PATH} ${BOOST_ROOT})
-set(CMAKE_LIBRARY_PATH ${CMAKE_LIBRARY_PATH} ${BOOST_LIBRARY_DIRS})
+ set(Boost_NO_SYSTEM_PATHS ON)
+ set(BOOST_INCLUDE_DIRS "${BOOST_ROOT}/include")
+ set(BOOST_LIBRARY_DIRS "${BOOST_ROOT}/stage/lib")
+ set(CMAKE_INCLUDE_PATH ${CMAKE_INCLUDE_PATH} ${BOOST_ROOT})
+ set(CMAKE_LIBRARY_PATH ${CMAKE_LIBRARY_PATH} ${BOOST_LIBRARY_DIRS})
endif (Boost_NO_SYSTEM_PATHS)
# in case boost is a non-default location
# SET(CMAKE_INCLUDE_PATH ${CMAKE_INCLUDE_PATH} "C:/win32libs/boost")
# SET(CMAKE_LIBRARY_PATH ${CMAKE_LIBRARY_PATH} "C:/win32libs/boost/lib")
-#find_package(Boost REQUIRED)
# find the correct OpenMP flag
FIND_PACKAGE(OpenMP)
if (OPENMP_FOUND)
-set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
-set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
+ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+ set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${OpenMP_EXE_LINKER_FLAGS}")
else (OPENMP_FOUND)
-if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
-#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp")
-# somehow find_package(openmp) does not work properly with clang
-else (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
-message(STATUS "Disabling OpenMP support")
-endif (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+ if (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+ #set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fopenmp")
+ # somehow find_package(openmp) does not work properly with clang
+ else (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
+ message(STATUS "Disabling OpenMP support")
+ endif (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
endif (OPENMP_FOUND)
#statically link all gcc stuffs
@@ -265,41 +244,44 @@ endif (OPENMP_FOUND)
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS}")
#boost
-find_package(Boost COMPONENTS timer chrono system filesystem unit_test_framework REQUIRED)
+if(MSVC)
+ find_package(Boost COMPONENTS QUIET)
+else()
+ find_package(Boost COMPONENTS REQUIRED)
+endif()
+
+message(STATUS "BUILD_TESTING0: " ${BUILD_TESTING})
if(AIROUTER)
-#CGAL
-find_package(CGAL REQUIRED) # for AI router
+ #CGAL
+ find_package(CGAL REQUIRED) # for AI router
endif()
# test all cpp-files in Utest
if (BUILD_TESTING OR BUILD_CPPUNIT_TEST)
-
-find_package(Boost COMPONENTS timer chrono system filesystem unit_test_framework REQUIRED)
-IF (CMAKE_COMPILER_IS_GNUCXX)
-set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fprofile-arcs -ftest-coverage")
-IF (USE_DUAL_ABI)
-set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -D_GLIBCXX_USE_CXX11_ABI=0")
-ENDIF(USE_DUAL_ABI)
-set(CMAKE_EXE_LINKER_FLAGS "-fprofile-arcs -ftest-coverage")
-set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/cmake_modules/")
-INCLUDE(CodeCoverage)
-set(ENABLE_COVERAGE ON)
-SETUP_TARGET_FOR_COVERAGE(
-cov # Name for custom target.
-ctest # Name of the test driver executable that runs the tests.
-# NOTE! This should always have a ZERO as exit code
-# otherwise the coverage generation will not complete.
-coverage # Name of output directory.
-)
-
-endif (CMAKE_COMPILER_IS_GNUCXX)
-file(GLOB_RECURSE test_files "${CMAKE_TEST_DIR}/*.cpp")
-# file(GLOB test_py_files "${CMAKE_TEST_DIR}/*/runtest*.py")
+ IF (CMAKE_COMPILER_IS_GNUCXX)
+ set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fprofile-arcs -ftest-coverage")
+ IF (USE_DUAL_ABI)
+ set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -D_GLIBCXX_USE_CXX11_ABI=0")
+ ENDIF(USE_DUAL_ABI)
+ set(CMAKE_EXE_LINKER_FLAGS "-fprofile-arcs -ftest-coverage")
+ set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PROJECT_SOURCE_DIR}/cmake_modules/")
+ INCLUDE(CodeCoverage)
+ set(ENABLE_COVERAGE ON)
+ SETUP_TARGET_FOR_COVERAGE(
+ cov # Name for custom target.
+ ctest # Name of the test driver executable that runs the tests.
+ # NOTE! This should always have a ZERO as exit code
+ # otherwise the coverage generation will not complete.
+ coverage # Name of output directory.
+ )
+ endif (CMAKE_COMPILER_IS_GNUCXX)
+ file(GLOB_RECURSE test_files "${CMAKE_TEST_DIR}/*.cpp")
+ # file(GLOB test_py_files "${CMAKE_TEST_DIR}/*/runtest*.py")
endif (BUILD_TESTING OR BUILD_CPPUNIT_TEST)
if (BUILD_TESTING)
-file(GLOB_RECURSE test_py_files "${CMAKE_TEST_DIR}/*runtest_*.py")
+ file(GLOB_RECURSE test_py_files "${CMAKE_TEST_DIR}/*runtest_*.py")
endif (BUILD_TESTING)
# add sources and headers
@@ -307,10 +289,6 @@ set(source_files
Simulation.cpp
general/ArgumentParser.cpp
IO/progress_bar.cpp
-tinyxml/tinystr.cpp
-tinyxml/tinyxml.cpp
-tinyxml/tinyxmlerror.cpp
-tinyxml/tinyxmlparser.cpp
geometry/Building.cpp
geometry/Line.cpp
@@ -369,10 +347,8 @@ routing/ff_router/UnivFFviaFM.cpp
#floorfield
routing/ff_router_trips/ffRouterTrips.cpp
-#routing/ff_router/LocalFloorfieldViaFM.cpp
-routing/ff_router_trips/FloorfieldViaFM.cpp
-#routing/ff_router/FFKit.cpp
-routing/ff_router_trips/UnivFFviaFM.cpp
+routing/ff_router_trips/FloorfieldViaFMTrips.cpp
+routing/ff_router_trips/UnivFFviaFMTrips.cpp
#global_shortest
routing/global_shortest/AccessPoint.cpp
@@ -417,13 +393,8 @@ routing/smoke_router/cognitiveMap/cogmapoutputhandler.cpp
routing/smoke_router/cognitiveMap/landmarknetwork.cpp
routing/smoke_router/Brain.cpp
routing/smoke_router/cognitiveMap/internnavigationnetwork.cpp
-visiLibity/source_code/visilibity.cpp
-poly2tri/common/shapes.cpp
-poly2tri/sweep/sweep_context.cpp
-poly2tri/sweep/advancing_front.cpp
-poly2tri/sweep/sweep.cpp
-poly2tri/sweep/cdt.cpp
+
events/EventManager.cpp
events/Event.cpp
@@ -431,7 +402,7 @@ geometry/Trips.cpp
routing/trips_router/TripsRouter.cpp
forms/jpscore.rc
-math/KrauszModel.cpp )
+math/KrauszModel.cpp)
set(THIRD_PARTY_SRC
tinyxml/tinystr.cpp
@@ -471,7 +442,6 @@ JPSfire/B_walking_speed/WalkingSpeed.cpp
JPSfire/C_toxicity_analysis/ToxicityAnalysis.cpp
JPSfire/C_toxicity_analysis/ToxicityOutputhandler.cpp
)
-
set(header_files
#floorfield
@@ -487,10 +457,8 @@ routing/ff_router/UnivFFviaFM.h
routing/ff_router_trips/ffRouterTrips.h
routing/ff_router_trips/RectGrid.h
routing/ff_router_trips/Trial.h
-#routing/ff_router/LocalFloorfieldViaFM.h
-routing/ff_router_trips/FloorfieldViaFM.h
-#routing/ff_router/FFKit.h
-routing/ff_router_trips/UnivFFviaFM.h
+routing/ff_router_trips/FloorfieldViaFMTrips.h
+routing/ff_router_trips/UnivFFviaFMTrips.h
#general
routing/DirectionStrategy.h
@@ -538,7 +506,6 @@ routing/smoke_router/cognitiveMap/cogmapoutputhandler.h
routing/smoke_router/cognitiveMap/landmarknetwork.h
routing/smoke_router/Brain.h
routing/smoke_router/cognitiveMap/internnavigationnetwork.h
-visiLibity/source_code/visilibity.hpp
pedestrian/Pedestrian.h
pedestrian/PedDistributor.h
@@ -556,8 +523,6 @@ voronoi-boost/VoronoiPositionGenerator.h
mpi/LCGrid.h
-tinyxml/tinyxml.h
-tinyxml/tinystr.h
general/ArgumentParser.h
general/Configuration.h
@@ -594,15 +559,6 @@ math/GradientModel.h
math/VelocityModel.h
math/OperationalModel.h
-
-
-poly2tri/poly2tri.h
-poly2tri/common/shapes.h
-poly2tri/sweep/cdt.h
-poly2tri/common/utils.h
-poly2tri/sweep/sweep_context.h
-poly2tri/sweep/advancing_front.h
-poly2tri/sweep/sweep.h
events/EventManager.h
events/Event.h
@@ -619,8 +575,8 @@ JPSfire/A_smoke_sensor/SmokeSensor.h
JPSfire/B_walking_speed/WalkingSpeed.h
JPSfire/C_toxicity_analysis/ToxicityAnalysis.h
JPSfire/C_toxicity_analysis/ToxicityOutputhandler.h
-)
+)
SET(AIROUTER_SRC
#AI router
routing/ai_router/AIRouter.cpp
diff --git a/IO/GeoFileParser.cpp b/IO/GeoFileParser.cpp
index 42407da6c875f9ec731359771d6ffa1fc1f44581..b0bb0623def80d1e27f6b9ffa3030f63d63fc11d 100644
--- a/IO/GeoFileParser.cpp
+++ b/IO/GeoFileParser.cpp
@@ -476,8 +476,23 @@ bool GeoFileParser::LoadRoutingInfo(Building* building)
return false;
}
+
+// for (auto& room : building->GetAllRooms()){
+// for (auto& subroomMap : room.second->GetAllSubRooms()){
+// std::cout << "check if wa " << wa->GetId() << " is in subroom " << subroomMap.second->GetSubRoomID() << std::endl;
+// std::cout << "wa centroid " << wa->GetCentroid().toString() << std::endl;
+//
+// if (subroomMap.second->IsInSubRoom(wa->GetCentroid()) || subroomMap.second->GetSubRoomID() == 3){
+// subroomMap.second->AddGoalID(wa->GetId());
+// }
+// }
+// }
+
building->AddGoal(wa);
_configuration->GetRoutingEngine()->AddFinalDestinationID(wa->GetId());
+ for (auto &itrGoal : building->GetAllGoals()) {
+ std::cout << "Goal ID: " << itrGoal.second->GetId() << std::endl;
+ }
std::cout << waitingArea->toString() << std::endl;
diff --git a/IO/IniFileParser.cpp b/IO/IniFileParser.cpp
index ade857ea97c239f5acc7a37f596d404bdd0d7904..036573cbb51ad28c098c7ee3d79ff8e671df0a94 100644
--- a/IO/IniFileParser.cpp
+++ b/IO/IniFileParser.cpp
@@ -1179,21 +1179,21 @@ bool IniFileParser::ParseRoutingStrategies(TiXmlNode* routingNode, TiXmlNode* ag
exit(EXIT_FAILURE);
#endif
}
- else if ((strategy == "AI_trips") &&
- (std::find(usedRouter.begin(), usedRouter.end(), id) != usedRouter.end()) ) {
- #ifdef AIROUTER
- Router *r = new AIRouterTrips(id, ROUTING_AI_TRIPS);
- _config->GetRoutingEngine()->AddRouter(r);
-
- Log->Write("\nINFO: \tUsing AIRouter Trips");
- ///Parsing additional options
- if (!ParseAIOpts(e))
- return false;
- #else
- std::cerr << "\nCan not use AI Router. Rerun cmake with option -DAIROUTER=true and recompile.\n";
- exit(EXIT_FAILURE);
- #endif
- }
+// else if ((strategy == "AI_trips") &&
+// (std::find(usedRouter.begin(), usedRouter.end(), id) != usedRouter.end()) ) {
+// #ifdef AIROUTER
+// Router *r = new AIRouterTrips(id, ROUTING_AI_TRIPS);
+// _config->GetRoutingEngine()->AddRouter(r);
+//
+// Log->Write("\nINFO: \tUsing AIRouter Trips");
+// ///Parsing additional options
+// if (!ParseAIOpts(e))
+// return false;
+// #else
+// std::cerr << "\nCan not use AI Router. Rerun cmake with option -DAIROUTER=true and recompile.\n";
+// exit(EXIT_FAILURE);
+// #endif
+// }
else if ((strategy == "ff_global_shortest_trips") &&
(std::find(usedRouter.begin(), usedRouter.end(), id) != usedRouter.end()) ) {
diff --git a/events/EventManager.cpp b/events/EventManager.cpp
index e9191cfd9ac4d21addb047a21d61749da81f9246..9c00a1cbf76c4f15bfd46df3f4b789207e8e9e33 100644
--- a/events/EventManager.cpp
+++ b/events/EventManager.cpp
@@ -38,6 +38,7 @@
#include "../geometry/SubRoom.h"
#include "../tinyxml/tinyxml.h"
#include "../routing/global_shortest/GlobalRouter.h"
+#include "../routing/global_shortest_trips/GlobalRouterTrips.h"
#include "../routing/quickest/QuickestPathRouter.h"
#include "../routing/smoke_router/SmokeRouter.h"
#include "../routing/ff_router/ffRouter.h"
@@ -659,11 +660,11 @@ Router * EventManager::CreateRouter(const RoutingStrategy& strategy)
switch(strategy)
{
case ROUTING_LOCAL_SHORTEST:
- rout = new GlobalRouter(ROUTING_LOCAL_SHORTEST, ROUTING_LOCAL_SHORTEST);
+ rout = new GlobalRouterTrips(ROUTING_LOCAL_SHORTEST, ROUTING_LOCAL_SHORTEST);
break;
case ROUTING_GLOBAL_SHORTEST:
- rout = new GlobalRouter(ROUTING_GLOBAL_SHORTEST, ROUTING_GLOBAL_SHORTEST);
+ rout = new GlobalRouterTrips(ROUTING_GLOBAL_SHORTEST, ROUTING_GLOBAL_SHORTEST);
break;
case ROUTING_QUICKEST:
diff --git a/events/EventManager.h b/events/EventManager.h
index dd725fffda0ffe7daa646b9721e1f0aa47064c04..e0d063756e27174145898ab8c9efa6b332efc81e 100644
--- a/events/EventManager.h
+++ b/events/EventManager.h
@@ -31,7 +31,7 @@
class Building;
class Router;
-class GlobalRouter;
+class GlobalRouterTrips;
class QuickestPathRouter;
class RoutingEngine;
class Event;
diff --git a/general/Macros.h b/general/Macros.h
index 11ca4b777e50ae4dc5ef2d7e82f3be5d640c4561..17410f9cc7e2f18c13f893b0691377a8bb78b633 100644
--- a/general/Macros.h
+++ b/general/Macros.h
@@ -209,6 +209,22 @@ enum MagicNumbers{
TARGET_REGION
};
+enum SPEEDFIELD { //this enum is used as index in _speedFieldSelector
+ INITIAL_SPEED=0, //homogen speed in walkable area, nealy zero in walls
+ REDU_WALL_SPEED=1, //reduced wall speed
+ PED_SPEED=2 //standing agents reduce speed, so that jams will be considered in ff
+};
+
+enum TARGETMODE {
+ LINESEGMENT=0,
+ CENTERPOINT
+};
+
+enum USERMODE {
+ DISTANCE_MEASUREMENTS_ONLY,
+ DISTANCE_AND_DIRECTIONS_USED
+};
+
constexpr double magicnum(int i) {
return (i == UNKNOWN_DISTANCE) ? -3.0 : (i == UNKNOWN_COST) ? -2.0 : (i == WALL_ON_COSTARRAY) ? -7.0 : (i == TARGET_REGION) ? 0.0 : J_NAN;
// switch (i) {
diff --git a/geometry/Building.cpp b/geometry/Building.cpp
index d2dec196644824a41d0b4970f1b65346c48c8283..5a975e94a179a6ec69b20099edf6c59ba53d2a89 100644
--- a/geometry/Building.cpp
+++ b/geometry/Building.cpp
@@ -846,6 +846,8 @@ bool Building::AddGoal(Goal* goal)
exit(EXIT_FAILURE);
}
_goals[goal->GetId()] = goal;
+
+
return true;
}
diff --git a/geometry/Building.h b/geometry/Building.h
index 21690a36fe375f8bde36dd1e393ce739e1e929f7..519cb24ea5b7c4987e6a73a7423b25f9f9e92efd 100644
--- a/geometry/Building.h
+++ b/geometry/Building.h
@@ -59,6 +59,8 @@ class PedDistributor;
class Configuration;
+class WaitingArea;
+
class Building {
private:
Configuration* _configuration;
@@ -72,6 +74,7 @@ private:
std::map<int, Transition*> _transitions;
std::map<int, Hline*> _hLines;
std::map<int, Goal*> _goals;
+ std::map<int, std::vector<WaitingArea*>> _sr2wa;
/// pedestrians pathway
bool _savePathway;
diff --git a/geometry/SubRoom.cpp b/geometry/SubRoom.cpp
index cbcaf8dc54ee59b7656b7f8e3e9d121112a74e67..c80abc7254d207146facdc3c982d029143319bd6 100644
--- a/geometry/SubRoom.cpp
+++ b/geometry/SubRoom.cpp
@@ -33,6 +33,7 @@
#include "SubRoom.h"
#include "Transition.h"
#include "Hline.h"
+#include "WaitingArea.h"
#ifdef _SIMULATOR
@@ -1491,6 +1492,13 @@ std::vector<Point> SubRoom::StartLLCorner(const std::vector<Point> &polygon)
}
+std::vector<WaitingArea*> SubRoom::GetAllWaitingAreas()
+{
+
+
+ return vector<WaitingArea*>();
+}
+
/// Escalator
Escalator::Escalator(): Stair() {
diff --git a/geometry/SubRoom.h b/geometry/SubRoom.h
index 13a5d26b9b2879f90f51e724b068a1694aef2c15..ae4093a5f67ff719b75fb35239c3c481756e7288 100644
--- a/geometry/SubRoom.h
+++ b/geometry/SubRoom.h
@@ -56,6 +56,7 @@ class Crossing;
class Line;
class Point;
class Wall;
+class WaitingArea;
#ifdef _SIMULATOR
class Pedestrian;
@@ -101,6 +102,8 @@ protected:
polygon_type _boostPoly;
std::vector<polygon_type> _boostPolyObstacles;
+ std::vector<polygon_type> _boostPolyWall;
+
std::vector<double> _poly_help_constatnt; //for the function IsInsidePolygon, a.brkic
std::vector<double> _poly_help_multiple; //for the function IsInsidePolygon, a.brkic
std::vector<Obstacle*> _obstacles;
@@ -392,6 +395,8 @@ public:
///check whether the pedestrians is still in the subroom
virtual bool IsInSubRoom(const Point& ped) const = 0;
+ std::vector<WaitingArea*> GetAllWaitingAreas();
+
#ifdef _SIMULATOR
virtual bool IsInSubRoom(Pedestrian* ped) const;
diff --git a/geometry/WaitingArea.cpp b/geometry/WaitingArea.cpp
index e6172062c5d5a8d584d32ac5f59742fb302782e2..e82d277d483950cdb7c6832c86922d603fa25386 100644
--- a/geometry/WaitingArea.cpp
+++ b/geometry/WaitingArea.cpp
@@ -99,3 +99,7 @@ void WaitingArea::setWaitingTime(double waitingTime)
WaitingArea::waitingTime = waitingTime;
}
+std::vector<Line> WaitingArea::getAllLines()
+{
+
+}
diff --git a/geometry/WaitingArea.h b/geometry/WaitingArea.h
index 5e50a2736f14fcee91a338e7b26c5c6d87843a06..3070ad45de1de11be7fabdb5b1894822da93860d 100644
--- a/geometry/WaitingArea.h
+++ b/geometry/WaitingArea.h
@@ -6,9 +6,12 @@
#define _WAITINGAREA_H
#include "Goal.h"
+#include "Line.h"
+
#include <vector>
#include <map>
#include <stdio.h>
+
class WaitingArea : public Goal{
protected:
@@ -61,6 +64,8 @@ public:
void setWaitingTime(double waitingTime);
+ std::vector<Line> getAllLines();
+
private:
void updateProbabilities();
bool checkProbabilities();
diff --git a/routing/ff_router/UnivFFviaFM.cpp b/routing/ff_router/UnivFFviaFM.cpp
index 6a6e23b0fe32d9f71036754a6f1b33d98d530853..a116969e791e5c9bf173912f9e32e16824a1bff1 100644
--- a/routing/ff_router/UnivFFviaFM.cpp
+++ b/routing/ff_router/UnivFFviaFM.cpp
@@ -919,9 +919,9 @@ void UnivFFviaFM::drawLinesOnWall(Line& line, T* const target, const T value) {
} //drawLinesOnWall
void UnivFFviaFM::calcFF(double* costOutput, Point* directionOutput, const double *const speed) {
- //CompareCost comp = CompareCost(costOutput);
- std::priority_queue<long int, std::vector<long int>, CompareCost> trialfield(costOutput); //pass the argument for the constr of CompareCost
- //std::priority_queue<long int, std::vector<long int>, CompareCost> trialfield2(comp); //pass the CompareCost object directly
+ //CompareCostTrips comp = CompareCostTrips(costOutput);
+ std::priority_queue<long int, std::vector<long int>, CompareCostTrips> trialfield(costOutput); //pass the argument for the constr of CompareCostTrips
+ //std::priority_queue<long int, std::vector<long int>, CompareCostTrips> trialfield2(comp); //pass the CompareCostTrips object directly
directNeighbor local_neighbor = _grid->getNeighbors(0);
long int aux = 0;
@@ -1111,9 +1111,9 @@ void UnivFFviaFM::calcCost(const long int key, double* cost, Point* dir, const d
}
void UnivFFviaFM::calcDF(double* costOutput, Point* directionOutput, const double *const speed) {
- //CompareCost comp = CompareCost(costOutput);
- std::priority_queue<long int, std::vector<long int>, CompareCost> trialfield(costOutput); //pass the argument for the constr of CompareCost
- //std::priority_queue<long int, std::vector<long int>, CompareCost> trialfield2(comp); //pass the CompareCost object directly
+ //CompareCostTrips comp = CompareCostTrips(costOutput);
+ std::priority_queue<long int, std::vector<long int>, CompareCostTrips> trialfield(costOutput); //pass the argument for the constr of CompareCostTrips
+ //std::priority_queue<long int, std::vector<long int>, CompareCostTrips> trialfield2(comp); //pass the CompareCostTrips object directly
directNeighbor local_neighbor = _grid->getNeighbors(0);
long int aux = 0;
diff --git a/routing/ff_router/UnivFFviaFM.h b/routing/ff_router/UnivFFviaFM.h
index 53080ec1993fba5e32e6ae937c39868505dd41a3..7f22e9452f1a00c78833b58bfddbc90636e3f624 100644
--- a/routing/ff_router/UnivFFviaFM.h
+++ b/routing/ff_router/UnivFFviaFM.h
@@ -51,25 +51,25 @@ class Point;
class RectGrid;
class Line;
-enum SPEEDFIELD { //this enum is used as index in _speedFieldSelector
- INITIAL_SPEED=0, //homogen speed in walkable area, nealy zero in walls
- REDU_WALL_SPEED=1, //reduced wall speed
- PED_SPEED=2 //standing agents reduce speed, so that jams will be considered in ff
-};
-
-enum TARGETMODE {
- LINESEGMENT=0,
- CENTERPOINT
-};
-
-enum USERMODE {
- DISTANCE_MEASUREMENTS_ONLY,
- DISTANCE_AND_DIRECTIONS_USED
-};
+//enum SPEEDFIELD { //this enum is used as index in _speedFieldSelector
+// INITIAL_SPEED=0, //homogen speed in walkable area, nealy zero in walls
+// REDU_WALL_SPEED=1, //reduced wall speed
+// PED_SPEED=2 //standing agents reduce speed, so that jams will be considered in ff
+//};
+//
+//enum TARGETMODE {
+// LINESEGMENT=0,
+// CENTERPOINT
+//};
+//
+//enum USERMODE {
+// DISTANCE_MEASUREMENTS_ONLY,
+// DISTANCE_AND_DIRECTIONS_USED
+//};
-class CompareCost { //this class is used in std::priority_queue in UnivFFviaFM::calcFF
+class CompareCostTrips { //this class is used in std::priority_queue in UnivFFviaFM::calcFF
public:
- CompareCost(double* costarray) : _costarray(costarray) {}
+ CompareCostTrips(double* costarray) : _costarray(costarray) {}
bool operator() (const int a, const int b) const {
return _costarray[a] > _costarray[b];
}
diff --git a/routing/ff_router/ffRouter.cpp b/routing/ff_router/ffRouter.cpp
deleted file mode 100644
index 067c28f0dc292027030ce172b290e751a89b247b..0000000000000000000000000000000000000000
--- a/routing/ff_router/ffRouter.cpp
+++ /dev/null
@@ -1,637 +0,0 @@
-/**
- * \file ffRouter.h
- * \date Feb 19, 2016
- * \version v0.8
- * \copyright <2016-2022> Forschungszentrum Jülich GmbH. All rights reserved.
- *
- * \section License
- * This file is part of JuPedSim.
- *
- * JuPedSim is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Lesser General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * any later version.
- *
- * JuPedSim is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public License
- * along with JuPedSim. If not, see <http://www.gnu.org/licenses/>.
- *
- * \section Description
- * This router is an update of the former Router.{cpp, h} - Global-, Quickest
- * Router System. In the __former__ version, a graph was created with doors and
- * hlines as nodes and the distances of (doors, hlines), connected with a line-
- * of-sight, was used as edge-costs. If there was no line-of-sight, there was no
- * connecting edge. On the resulting graph, the Floyd-Warshall algorithm was
- * used to find any paths. In the "quickest-___" variants, the edge cost was not
- * determined by the distance, but by the distance multiplied by a speed-
- * estimate, to find the path with minimum travel times. This whole construct
- * worked pretty well, but dependend on hlines to create paths with line-of-
- * sights to the next target (hline/door).
- *
- * In the ffRouter, we want to overcome hlines by using floor fields to
- * determine the distances. A line of sight is not required any more. We hope to
- * reduce the graph complexity and the preparation-needs for new geometries.
- *
- * To find a counterpart for the "quickest-____" router, we can either use
- * __special__ floor fields, that respect the travel time in the input-speed map,
- * or take the distance-floor field and multiply it by a speed-estimate (analog
- * to the former construct.
- *
- * We will derive from the <Router> class to fit the interface.
- *
- **/
-
-#include <cfloat>
-#include <algorithm>
-#include "ffRouter.h"
-//#include "FloorfieldViaFM.h"
-//#include "../../geometry/Building.h"
-
-int FFRouter::_cnt = 0;
-
-FFRouter::FFRouter()
-{
-
-}
-
-FFRouter::FFRouter(int id, RoutingStrategy s, bool hasSpecificGoals, Configuration* config):Router(id,s) {
- _config = config;
- _building = nullptr;
- _hasSpecificGoals = hasSpecificGoals;
- _globalFF = nullptr;
- _targetWithinSubroom = true; //depending on exit_strat 8 => false, depending on exit_strat 9 => true;
- _targetWithinSubroom = (_config->get_exit_strat() == 9);
- if (s == ROUTING_FF_QUICKEST) {
- _mode = quickest;
- _recalc_interval = _config->get_recalc_interval();
- } else if (s == ROUTING_FF_LOCAL_SHORTEST) {
- _mode = local_shortest;
- _localShortestSafedPeds.clear();
- _localShortestSafedPeds.reserve(500);
- } else if (s == ROUTING_FF_GLOBAL_SHORTEST) {
- _mode = global_shortest;
- }
-}
-
-//FFRouter::FFRouter(const Building* const building)
-
-FFRouter::~FFRouter()
-{
- if (_globalFF) {
- delete _globalFF;
- }
- //delete localffs
- std::map<int, UnivFFviaFM*>::reverse_iterator delIter;
- for (delIter = _locffviafm.rbegin();
- delIter != _locffviafm.rend();
- ++delIter) {
- delete (*delIter).second;
- }
-}
-
-bool FFRouter::Init(Building* building)
-{
- _building = building;
- if (_hasSpecificGoals) {
- std::vector<int> goalIDs;
- goalIDs.clear();
- //get global field to manage goals (which are not in a subroom)
- _globalFF = new FloorfieldViaFM(building, 0.25, 0.25, 0.0, false, true);
- for (auto &itrGoal : building->GetAllGoals()) {
- _globalFF->createMapEntryInLineToGoalID(itrGoal.first);
- goalIDs.emplace_back(itrGoal.first);
- }
- _goalToLineUIDmap = _globalFF->getGoalToLineUIDmap();
- _goalToLineUIDmap2 = _globalFF->getGoalToLineUIDmap2();
- _goalToLineUIDmap3 = _globalFF->getGoalToLineUIDmap3();
- //_globalFF->writeGoalFF("goal.vtk", goalIDs);
- }
- //get all door UIDs
- _allDoorUIDs.clear();
- _TransByUID.clear();
- _ExitsByUID.clear();
- _CroTrByUID.clear();
- auto& allTrans = building->GetAllTransitions();
- auto& allCross = building->GetAllCrossings();
- std::vector<std::pair<int, int>> roomAndCroTrVector;
- roomAndCroTrVector.clear();
- for (auto& pair:allTrans) {
- 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()));
- Room* room2 = pair.second->GetRoom2();
- if (room2) roomAndCroTrVector.emplace_back(std::make_pair(room2->GetID(), pair.second->GetUniqueID()));
- }
- }
- for (auto& pair:allCross) {
- if (pair.second->IsOpen()) {
- _allDoorUIDs.emplace_back(pair.second->GetUniqueID());
- _CroTrByUID.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()));
- }
- }
- //make unique
- std::sort(_allDoorUIDs.begin(), _allDoorUIDs.end());
- _allDoorUIDs.erase( std::unique(_allDoorUIDs.begin(),_allDoorUIDs.end()), _allDoorUIDs.end());
-
- //cleanse maps
- _distMatrix.clear();
- _pathsMatrix.clear();
-
- //init, yet no distances, only create map entries
- for(auto& id1 : _allDoorUIDs) {
- for(auto& id2 : _allDoorUIDs){
- std::pair<int, int> key = std::make_pair(id1, id2);
- double value = (id1 == id2)? 0.0 : DBL_MAX;
- //distMatrix[i][j] = 0, if i==j
- //distMatrix[i][j] = max, else
- _distMatrix.insert(std::make_pair( key , value));
- //pathsMatrix[i][j] = i or j ? (follow wiki:path_reconstruction, it should be j)
- _pathsMatrix.insert(std::make_pair( key , id2 ));
- //_subroomMatrix.insert(std::make_pair(key, nullptr));
- }
- }
-
- //prepare all room-floor-fields-objects (one room = one instance)
- _locffviafm.clear();
- //type of allRooms: const std::map<int, std::unique_ptr<Room> >&
- const std::map<int, std::shared_ptr<Room> >& allRooms = _building->GetAllRooms();
-
-
- for (unsigned int i = 0; i < allRooms.size(); ++i) {
-
-#ifdef DEBUG
- std::cerr << "Creating Floorfield for Room: " << pair.first << std::endl;
-#endif
-
- auto pairRoomIt = allRooms.begin();
- std::advance(pairRoomIt, i);
- UnivFFviaFM *locffptr = nullptr;
- locffptr = new UnivFFviaFM(pairRoomIt->second.get(), building, 0.125, 0.0, false);
-
- locffptr->setUser(DISTANCE_MEASUREMENTS_ONLY);
- locffptr->setMode(CENTERPOINT);
- locffptr->setSpeedMode(FF_HOMO_SPEED);
- locffptr->addAllTargetsParallel();
- //locffptr->writeFF("UnivFF"+std::to_string(pairRoomIt->first)+".vtk", locffptr->getKnownDoorUIDs());
- Log->Write("INFO: \tAdding distances in Room %d to matrix", (*pairRoomIt).first);
-//#pragma omp critical(_locffviafm)
- _locffviafm.insert(std::make_pair((*pairRoomIt).first, locffptr));
- }
-
-
- // nowait, because the parallel region ends directly afterwards
-//#pragma omp for nowait
- //@todo: @ar.graf: it would be easier to browse thru doors of each field directly after "addAllTargetsParallel" as
- // we do only want doors of same subroom anyway. BUT the router would have to switch from room-scope
- // to subroom-scope. Nevertheless, we could omit the room info (used to acces correct field), if we
- // do it like in "ReInit()".
- for (unsigned int i = 0; i < roomAndCroTrVector.size(); ++i) {
- auto rctIt = roomAndCroTrVector.begin();
- std::advance(rctIt, i);
-
- ////loop over upper triangular matrice (i,j) and write to (j,i) as well
- for (auto otherDoor : roomAndCroTrVector) {
- if (otherDoor.first != rctIt->first) continue; // we only want doors with one room in common
- if (otherDoor.second <= rctIt->second) continue; // calculate every path only once
- // if we exclude otherDoor.second == rctIt->second, the program loops forever
-
- //if the door is closed, then don't calc distances
- //if (!_CroTrByUID.at(*otherDoor)->IsOpen()) {
- // continue;
- //}
-
- // if the two doors are not within the same subroom, do not consider (ar.graf)
- // should fix problems of oscillation caused by doorgaps in the distancegraph
- int thisUID1 = (_CroTrByUID.at(rctIt->second)->GetSubRoom1()) ? _CroTrByUID.at(rctIt->second)->GetSubRoom1()->GetUID() : -10 ;
- int thisUID2 = (_CroTrByUID.at(rctIt->second)->GetSubRoom2()) ? _CroTrByUID.at(rctIt->second)->GetSubRoom2()->GetUID() : -20 ;
- int otherUID1 = (_CroTrByUID.at(otherDoor.second)->GetSubRoom1()) ? _CroTrByUID.at(otherDoor.second)->GetSubRoom1()->GetUID() : -30 ;
- int otherUID2 = (_CroTrByUID.at(otherDoor.second)->GetSubRoom2()) ? _CroTrByUID.at(otherDoor.second)->GetSubRoom2()->GetUID() : -40 ;
-
- if (
- (thisUID1 != otherUID1) &&
- (thisUID1 != otherUID2) &&
- (thisUID2 != otherUID1) &&
- (thisUID2 != otherUID2) ) {
- continue;
- }
-
- UnivFFviaFM* locffptr = _locffviafm[rctIt->first];
- double tempDistance = locffptr->getDistanceBetweenDoors(rctIt->second, otherDoor.second);
-
- if (tempDistance < locffptr->getGrid()->Gethx()) {
- Log->Write("WARNING:\tIgnoring distance of doors %d and %d because it is too small: %f",rctIt->second, otherDoor.second, tempDistance);
- //Log->Write("^^^^^^^^\tIf there are scattered subrooms, which are not connected, this is ok.");
- continue;
- }
-//
- std::pair<int, int> key_ij = std::make_pair(otherDoor.second, rctIt->second);
- std::pair<int, int> key_ji = std::make_pair(rctIt->second, otherDoor.second);
-
-
-#pragma omp critical(_distMatrix)
- if (_distMatrix.at(key_ij) > tempDistance) {
- _distMatrix.erase(key_ij);
- _distMatrix.erase(key_ji);
- _distMatrix.insert(std::make_pair(key_ij, tempDistance));
- _distMatrix.insert(std::make_pair(key_ji, tempDistance));
- }
- } // otherDoor
- } // roomAndCroTrVector
-
- if (_config->get_has_directional_escalators()) {
- _directionalEscalatorsUID.clear();
- _penaltyList.clear();
- for (auto room : building->GetAllRooms()) {
- for (auto subroom : room.second->GetAllSubRooms()) {
- if ((subroom.second->GetType() == "escalator_up") || (subroom.second->GetType() == "escalator_down")) {
- _directionalEscalatorsUID.emplace_back(subroom.second->GetUID());
- }
- }
- }
- for (int subUID : _directionalEscalatorsUID) {
- Escalator* escalator = (Escalator*) building->GetSubRoomByUID(subUID);
- std::vector<int> lineUIDs = escalator->GetAllGoalIDs();
- assert(lineUIDs.size() == 2);
- if (escalator->IsEscalatorUp()) {
- if (_CroTrByUID[lineUIDs[0]]->IsInLineSegment(escalator->GetUp())) {
- _penaltyList.emplace_back(std::make_pair(lineUIDs[0], lineUIDs[1]));
- } else {
- _penaltyList.emplace_back(std::make_pair(lineUIDs[1], lineUIDs[0]));
- }
- } else { //IsEscalatorDown
- if (_CroTrByUID[lineUIDs[0]]->IsInLineSegment(escalator->GetUp())) {
- _penaltyList.emplace_back(std::make_pair(lineUIDs[1], lineUIDs[0]));
- } else {
- _penaltyList.emplace_back(std::make_pair(lineUIDs[0], lineUIDs[1]));
- }
- }
- }
- for (auto key : _penaltyList) {
- _distMatrix.erase(key);
- _distMatrix.insert(std::make_pair(key, DBL_MAX));
- }
- }
-
- FloydWarshall();
-
- //debug output in file
-// _locffviafm[4]->writeFF("ffTreppe.vtk", _allDoorUIDs);
-
- //int roomTest = (*(_locffviafm.begin())).first;
- //int transTest = (building->GetRoom(roomTest)->GetAllTransitionsIDs())[0];
- //auto test = _CroTrByUID.at(1253);
-
- if (_config->get_write_VTK_files()) {
- for (unsigned int i = 0; i < _locffviafm.size(); ++i) {
- auto iter = _locffviafm.begin();
- std::advance(iter, i);
- int roomNr = iter->first;
- iter->second->writeFF("ffrouterOfRoom" + std::to_string(roomNr) + ".vtk", _allDoorUIDs);
- }
- }
-
-// std::ofstream matrixfile;
-// matrixfile.open("Matrix.txt");
-//
-// 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() << std::endl;
-// }
-// matrixfile.close();
- Log->Write("INFO: \tFF Router Init done.");
- return true;
-}
-
-bool FFRouter::ReInit()
-{
- //cleanse maps
- _distMatrix.clear();
- _pathsMatrix.clear();
-
- //init, yet no distances, only create map entries
- for(auto& id1 : _allDoorUIDs) {
- for(auto& id2 : _allDoorUIDs){
- std::pair<int, int> key = std::make_pair(id1, id2);
- double value = (id1 == id2)? 0.0 : DBL_MAX;
- //distMatrix[i][j] = 0, if i==j
- //distMatrix[i][j] = max, else
- _distMatrix.insert(std::make_pair( key , value));
- //pathsMatrix[i][j] = i or j ? (follow wiki:path_reconstruction, it should be j)
- _pathsMatrix.insert(std::make_pair( key , id2 ));
- }
- }
-
- for (auto floorfield : _locffviafm) {
- floorfield.second->setSpeedMode(FF_PED_SPEED);
- //@todo: ar.graf: create a list of local ped-ptr instead of giving all peds-ptr
- floorfield.second->createPedSpeed(_building->GetAllPedestrians().data(), _building->GetAllPedestrians().size(), _mode, 1.);
- floorfield.second->recreateAllForQuickest();
- std::vector<int> allDoors(floorfield.second->getKnownDoorUIDs());
- for (auto firstDoor : allDoors) {
- for (auto secondDoor : allDoors) {
- if (secondDoor <= firstDoor) continue; // calculate every path only once
- // if the two doors are not within the same subroom, do not consider (ar.graf)
- // should fix problems of oscillation caused by doorgaps in the distancegraph
- int thisUID1 = (_CroTrByUID.at(firstDoor)->GetSubRoom1()) ? _CroTrByUID.at(firstDoor)->GetSubRoom1()->GetUID() : -10 ;
- int thisUID2 = (_CroTrByUID.at(firstDoor)->GetSubRoom2()) ? _CroTrByUID.at(firstDoor)->GetSubRoom2()->GetUID() : -20 ;
- int otherUID1 = (_CroTrByUID.at(secondDoor)->GetSubRoom1()) ? _CroTrByUID.at(secondDoor)->GetSubRoom1()->GetUID() : -30 ;
- int otherUID2 = (_CroTrByUID.at(secondDoor)->GetSubRoom2()) ? _CroTrByUID.at(secondDoor)->GetSubRoom2()->GetUID() : -40 ;
-
- if (
- (thisUID1 != otherUID1) &&
- (thisUID1 != otherUID2) &&
- (thisUID2 != otherUID1) &&
- (thisUID2 != otherUID2) ) {
- continue;
- }
-
- //double tempDistance = floorfield.second->getCostToDestination(firstDoor, _CroTrByUID.at(secondDoor)->GetCentre());
- double tempDistance = floorfield.second->getDistanceBetweenDoors(firstDoor, secondDoor);
- if (tempDistance < floorfield.second->getGrid()->Gethx()) {
- Log->Write("WARNING:\tDistance of doors %d and %d is too small: %f",firstDoor, secondDoor, tempDistance);
- //Log->Write("^^^^^^^^\tIf there are scattered subrooms, which are not connected, this is ok.");
- continue;
- }
- std::pair<int, int> key_ij = std::make_pair(secondDoor, firstDoor);
- std::pair<int, int> key_ji = std::make_pair(firstDoor, secondDoor);
- if (_distMatrix.at(key_ij) > tempDistance) {
- _distMatrix.erase(key_ij);
- _distMatrix.erase(key_ji);
- _distMatrix.insert(std::make_pair(key_ij, tempDistance));
- _distMatrix.insert(std::make_pair(key_ji, tempDistance));
- }
- } //secondDoor(s)
- } //firstDoor(s)
- } //allRooms
-
- if (_config->get_has_directional_escalators()) {
- _directionalEscalatorsUID.clear();
- _penaltyList.clear();
- for (auto room : _building->GetAllRooms()) {
- for (auto subroom : room.second->GetAllSubRooms()) {
- if ((subroom.second->GetType() == "escalator_up") || (subroom.second->GetType() == "escalator_down")) {
- _directionalEscalatorsUID.emplace_back(subroom.second->GetUID());
- }
- }
- }
- for (int subUID : _directionalEscalatorsUID) {
- Escalator* escalator = (Escalator*) _building->GetSubRoomByUID(subUID);
- std::vector<int> lineUIDs = escalator->GetAllGoalIDs();
- assert(lineUIDs.size() == 2);
- if (escalator->IsEscalatorUp()) {
- if (_CroTrByUID[lineUIDs[0]]->IsInLineSegment(escalator->GetUp())) {
- _penaltyList.emplace_back(std::make_pair(lineUIDs[0], lineUIDs[1]));
- } else {
- _penaltyList.emplace_back(std::make_pair(lineUIDs[1], lineUIDs[0]));
- }
- } else { //IsEscalatorDown
- if (_CroTrByUID[lineUIDs[0]]->IsInLineSegment(escalator->GetUp())) {
- _penaltyList.emplace_back(std::make_pair(lineUIDs[1], lineUIDs[0]));
- } else {
- _penaltyList.emplace_back(std::make_pair(lineUIDs[0], lineUIDs[1]));
- }
- }
- }
- for (auto key : _penaltyList) {
- _distMatrix.erase(key);
- _distMatrix.insert(std::make_pair(key, DBL_MAX));
- }
- }
-
- FloydWarshall();
- _plzReInit = false;
- return true;
-}
-
-
-
-int FFRouter::FindExit(Pedestrian* p)
-{
-// if (_mode == local_shortest) {
-// if ((_locffviafm.at(p->GetRoomID())->getGrid()->includesPoint(p->GetPos())) &&
-// (p->GetSubRoomUID() != _locffviafm.at(p->GetRoomID())->getSubroomUIDAt(p->GetPos()))) {
-// //pedestrian is still in the room, but changed subroom
-// _localShortestSafedPeds.emplace_back(p->GetID());
-// }
-//
-// //if needed: quickest-mechanic part 2 of 2
-// if (!(_locffviafm.at(p->GetRoomID())->getGrid()->includesPoint(p->GetPos()))) {
-// //pedestrian left the room and therefore changed subroom
-// _localShortestSafedPeds.emplace_back(p->GetID());
-// }
-// }
- if (_mode == quickest) {
- if (p->GetGlobalTime() > _recalc_interval && _building->GetRoom(p->GetRoomID())->GetSubRoom(p->GetSubRoomID())->IsInSubRoom(p)
- && _locffviafm[p->GetRoomID()]->getCostToDestination(p->GetExitIndex(), p->GetPos()) > 3.0
- && p->GetExitIndex() != -1) {
- //delay possible
- if ((int) p->GetGlobalTime() % 10 != p->GetID() % 10) {
- return p->GetExitIndex(); //stay with old target
- }
- }
- //new version: recalc densityspeed every x seconds
- if ((p->GetGlobalTime() > _timeToRecalc) && (p->GetGlobalTime() > Pedestrian::GetMinPremovementTime() + _recalc_interval)) {
- _plzReInit = true;
- }
- }
- double minDist = DBL_MAX;
- int bestDoor = -1;
-
- int goalID = p->GetFinalDestination();
- std::vector<int> validFinalDoor; //UIDs of doors
- validFinalDoor.clear();
- if (goalID == -1) {
- for (auto& pairDoor : _ExitsByUID) {
- //we add the all exits,
- validFinalDoor.emplace_back(pairDoor.first); //UID
- }
- } else { //only one specific goal, goalToLineUIDmap gets
- //populated in Init()
- if ((_goalToLineUIDmap.count(goalID) == 0) || (_goalToLineUIDmap[goalID] == -1)) {
- Log->Write("ERROR: \t ffRouter: unknown/unreachable goalID: %d in FindExit(Ped)",goalID);
- } else {
- validFinalDoor.emplace_back(_goalToLineUIDmap.at(goalID));
- }
- }
-
- std::vector<int> DoorUIDsOfRoom;
- DoorUIDsOfRoom.clear();
- if (_building->GetRoom(p->GetRoomID())->GetSubRoom(p->GetSubRoomID())->IsInSubRoom(p->GetPos())) {
- //ped is in the subroom, according to its member attribs
- } else {
- bool located = false;
- SubRoom* oldSubRoom = _building->GetRoom(p->GetRoomID())->GetSubRoom(p->GetSubRoomID());
- for (auto& room : _building->GetAllRooms()) {
- if (located) {break;}
- for (auto& subroom : room.second->GetAllSubRooms()) {
- if (subroom.second->IsInSubRoom(p->GetPos()) && subroom.second->IsDirectlyConnectedWith(oldSubRoom)) {
- //maybe room on wrong floor
- p->SetRoomID(room.second->GetID(), room.second->GetCaption());
- p->SetSubRoomID(subroom.second->GetSubRoomID());
- p->SetSubRoomUID(subroom.second->GetUID());
- located = true;
- break;
- }
- }
- }
- if (!located) { //ped is outside
- return -1;
- }
- }
- DoorUIDsOfRoom.clear();
- if (!_targetWithinSubroom) {
- //candidates of current room (ID) (provided by Room)
- for (auto transUID : _building->GetRoom(p->GetRoomID())->GetAllTransitionsIDs()) {
- if ((_CroTrByUID.count(transUID) != 0) && (_CroTrByUID[transUID]->IsOpen())) {
- DoorUIDsOfRoom.emplace_back(transUID);
- }
- }
- for (auto &subIPair : _building->GetRoom(p->GetRoomID())->GetAllSubRooms()) {
- for (auto &crossI : subIPair.second->GetAllCrossings()) {
- if (crossI->IsOpen()) {
- DoorUIDsOfRoom.emplace_back(crossI->GetUniqueID());
- }
- }
- }
- }
- else
- {
- //candidates of current subroom only
- for (auto &crossI : _building->GetRoom(p->GetRoomID())->GetSubRoom(p->GetSubRoomID())->GetAllCrossings()) {
- if (crossI->IsOpen()) {
- DoorUIDsOfRoom.emplace_back(crossI->GetUniqueID());
- }
- }
-
- for (auto &transI : _building->GetRoom(p->GetRoomID())->GetSubRoom(p->GetSubRoomID())->GetAllTransitions()) {
- if (transI->IsOpen()) {
- DoorUIDsOfRoom.emplace_back(transI->GetUniqueID());
- }
- }
- }
-
- int bestFinalDoor = -1; // to silence the compiler
- for(int finalDoor : validFinalDoor) {
- //with UIDs, we can ask for shortest path
- for (int doorUID : DoorUIDsOfRoom) {
- //double locDistToDoor = _locffviafm[p->GetRoomID()]->getCostToDestination(doorUID, p->GetPos(), _mode);
- double locDistToDoor = 0.;
- if (_targetWithinSubroom) {
- locDistToDoor = _config->get_dirSubLocal()->GetDistance2Target(p, doorUID);
- } else {
- locDistToDoor = _config->get_dirLocal()->GetDistance2Target(p, doorUID);
- }
-
- if (locDistToDoor < -J_EPS) { //for old ff: //this can happen, if the point is not reachable and therefore has init val -7
- continue;
- }
- std::pair<int, int> key = std::make_pair(doorUID, finalDoor);
- //auto subroomDoors = _building->GetSubRoomByUID(p->GetSubRoomUID())->GetAllGoalIDs();
- //only consider, if paths exists
- if (_pathsMatrix.count(key)==0) {
- Log->Write("no key for %d %d", key.first, key.second);
- continue;
- }
-
- if ((_distMatrix.count(key)!=0) && (_distMatrix.at(key) != DBL_MAX)) {
- if ((_distMatrix.at(key) + locDistToDoor) < minDist) {
- minDist = _distMatrix.at(key) + locDistToDoor;
- bestDoor = key.first; //doorUID
- //if (locDistToDoor == 0.) {
- if (true) {
- auto subroomDoors = _building->GetSubRoomByUID(p->GetSubRoomUID())->GetAllGoalIDs();
- if (std::find(subroomDoors.begin(), subroomDoors.end(), _pathsMatrix[key]) != subroomDoors.end()) {
- bestDoor = _pathsMatrix[key]; //@todo: @ar.graf: check this hack
- }
- }
- bestFinalDoor = key.second;
- }
- }
- }
- }
-
- //at this point, bestDoor is either a crossing or a transition
- if ((!_targetWithinSubroom) && (_CroTrByUID.count(bestDoor) != 0)) {
- while (!_CroTrByUID[bestDoor]->IsTransition()) {
- std::pair<int, int> key = std::make_pair(bestDoor, bestFinalDoor);
- bestDoor = _pathsMatrix[key];
- }
- }
-
-//#pragma omp critical(finalDoors)
-// _finalDoors.emplace(std::make_pair(p->GetID(), bestFinalDoor));
-
- if (_CroTrByUID.count(bestDoor)) {
- p->SetExitIndex(bestDoor);
- p->SetExitLine(_CroTrByUID.at(bestDoor));
- }
- return bestDoor; //-1 if no way was found, doorUID of best, if path found
-}
-
-void FFRouter::FloydWarshall()
-{
- bool change = false;
- double savedDistance = 0.;
- int totalnum = _allDoorUIDs.size();
- for(int k = 0; k<totalnum; ++k) {
- for(int i = 0; i<totalnum; ++i) {
- for(int j= 0; j<totalnum; ++j) {
- std::pair<int, int> key_ij = std::make_pair(_allDoorUIDs[i], _allDoorUIDs[j]);
- std::pair<int, int> key_ik = std::make_pair(_allDoorUIDs[i], _allDoorUIDs[k]);
- std::pair<int, int> key_kj = std::make_pair(_allDoorUIDs[k], _allDoorUIDs[j]);
- if ((_distMatrix[key_ik] < DBL_MAX) && (_distMatrix[key_kj] < DBL_MAX) &&
- (_distMatrix[key_ik] + _distMatrix[key_kj] < _distMatrix[key_ij]))
- {
- savedDistance = _distMatrix[key_ij] - _distMatrix[key_ik] - _distMatrix[key_kj];
- _distMatrix.erase(key_ij);
- _distMatrix.insert(std::make_pair(key_ij, _distMatrix[key_ik] + _distMatrix[key_kj]));
- _pathsMatrix.erase(key_ij);
- _pathsMatrix.insert(std::make_pair(key_ij, _pathsMatrix[key_ik]));
- change = true;
- }
- }
- }
- }
- if (change) {
- //Log->Write("Floyd nochmal!!! %f", savedDistance);
- FloydWarshall();
- } else {
- Log->Write("INFO:\t FloydWarshall done!");
- }
-}
-
-void FFRouter::SetMode(std::string s)
-{
- if (s == "global_shortest"){
- _mode = global_shortest;
- return;
- }
-
- if (s == "quickest") {
- _mode = quickest;
- return;
- }
-
- _mode = global_shortest;
- return;
-}
-
-bool FFRouter::MustReInit() {
- return _plzReInit;
-}
-
-void FFRouter::SetRecalc(double t) {
- _timeToRecalc = t + _recalc_interval;
-}
\ No newline at end of file
diff --git a/routing/ff_router_trips/FFKit.cpp b/routing/ff_router_trips/FFKit.cpp
deleted file mode 100644
index 2a22f377a21127acd4701792a02413d46375f52e..0000000000000000000000000000000000000000
--- a/routing/ff_router_trips/FFKit.cpp
+++ /dev/null
@@ -1,43 +0,0 @@
-//
-// Created by arne on 3/29/17.
-//
-/**
- * \file FFKit.h
- * \date Mar 29, 2017
- * \version
- * \copyright <2016-2022> Forschungszentrum Jülich GmbH. All rights reserved.
- *
- * \section License
- * This file is part of JuPedSim.
- *
- * JuPedSim is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Lesser General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * any later version.
- *
- * JuPedSim is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public License
- * along with JuPedSim. If not, see <http://www.gnu.org/licenses/>.
- *
- * \section Description
- * The Floorfield Kit is the rework of the prototype FloorfieldViaFM - class.
- * It will restructur the former version to avoid several downsides (diamond
- * inheritance, line/centerpoint version (DRY), overdoing the precalc, ...)
- * We try to build it in a cleaner, easier version.
- *
- * A floorfield will be calculated and stored. The user can access distances(cost)
- * values as well as the gradient of that.
- *
- * Cost Values are calculated via the fastmarching algorithm. Other algorithms might
- * follow, if they provide better abilities to work in paralell.
- *
- * The new grid-code/subroom version should be reused for all floorfields, that work
- * on the same part of the geometry (room/subroom e.g. floor/room)
- *
- **/
-
-#include "FFKit.h"
diff --git a/routing/ff_router_trips/FFKit.h b/routing/ff_router_trips/FFKit.h
deleted file mode 100644
index 557dcfe84e9ad3be5eca94545684ea45bc68f460..0000000000000000000000000000000000000000
--- a/routing/ff_router_trips/FFKit.h
+++ /dev/null
@@ -1,103 +0,0 @@
-//
-// Created by arne on 3/29/17.
-//
-/**
- * \file ffRouter.h
- * \date Feb 19, 2016
- * \version v0.8
- * \copyright <2016-2022> Forschungszentrum Jülich GmbH. All rights reserved.
- *
- * \section License
- * This file is part of JuPedSim.
- *
- * JuPedSim is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Lesser General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * any later version.
- *
- * JuPedSim is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public License
- * along with JuPedSim. If not, see <http://www.gnu.org/licenses/>.
- *
- * \section Description
- *
- * The Floorfield Kit is the rework of the prototype FloorfieldViaFM - class.
- * It will restructur the former version to avoid several downsides (diamond
- * inheritance, line/centerpoint version (DRY), overdoing the precalc, ...)
- * We try to build it in a cleaner, easier version.
- *
- * A floorfield will be calculated and stored. The user can access distances(cost)
- * values as well as the gradient of that.
- *
- * Cost Values are calculated via the fastmarching algorithm. Other algorithms might
- * follow, if they provide better abilities to work in paralell.
- *
- * The new grid-code/subroom version should be reused for all floorfields, that work
- * on the same part of the geometry (room/subroom e.g. floor/room)
- *
- * old ffrouter description:
- * This router is an update of the former Router.{cpp, h} - Global-, Quickest
- * Router System. In the __former__ version, a graph was created with doors and
- * hlines as nodes and the distances of (doors, hlines), connected with a line-
- * of-sight, was used as edge-costs. If there was no line-of-sight, there was no
- * connecting edge. On the resulting graph, the Floyd-Warshall algorithm was
- * used to find any paths. In the "quickest-___" variants, the edge cost was not
- * determined by the distance, but by the distance multiplied by a speed-
- * estimate, to find the path with minimum travel times. This whole construct
- * worked pretty well, but dependend on hlines to create paths with line-of-
- * sights to the next target (hline/door).
- *
- * In the ffRouter, we want to overcome hlines by using floor fields to
- * determine the distances. A line of sight is not required any more. We hope to
- * reduce the graph complexity and the preparation-needs for new geometries.
- *
- * To find a counterpart for the "quickest-____" router, we can either use
- * __special__ floor fields, that respect the travel time in the input-speed map,
- * or take the distance-floor field and multiply it by a speed-estimate (analog
- * to the former construct.
- *
- * We will derive from the <Router> class to fit the interface.
- *
- * Questions to solve: how to deal with goalID == doorID problem in matrix
- *
- * Restrictions/Requirements: Floorfields are not really 3D supporting:
- *
- * A room may not consist of subrooms which overlap in their projection (onto
- * x-y-plane). So subrooms, that are positioned on top of others (in stairways
- * for example), must be separated into different rooms.
- *
- * floorfields do not consider z-coordinates. Distances of two grid points are
- * functions of (x, y) and not (x, y, z). Any slope will be neglected.
- *
- **/
-
-#ifndef JPSCORE_FFKIT_H
-#define JPSCORE_FFKIT_H
-
-#include <vector>
-#include <unordered_set>
-#include <cmath>
-#include <functional>
-#include "mesh/RectGrid.h"
-#include "../../geometry/Wall.h"
-#include "../../geometry/Point.h"
-#include "../../geometry/Building.h"
-#include "../../geometry/SubRoom.h" //check: should Room.h include SubRoom.h??
-#include "./mesh/Trial.h"
-#include "../../pedestrian/Pedestrian.h"
-
-//class Building;
-//class Pedestrian;
-class OutputHandler;
-
-//log output
-extern OutputHandler* Log;
-
-
-
-
-#endif //JPSCORE_FFKIT_H
diff --git a/routing/ff_router_trips/FloorfieldViaFM.cpp b/routing/ff_router_trips/FloorfieldViaFM.cpp
deleted file mode 100644
index b25c1a52e8cfdb4d99068995a347eda794e0b13e..0000000000000000000000000000000000000000
--- a/routing/ff_router_trips/FloorfieldViaFM.cpp
+++ /dev/null
@@ -1,1785 +0,0 @@
-/**
- * \file FloorfieldViaFM.cpp
- * \date Mar 05, 2015
- * \version N/A (v0.6)
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
- *
- * \section License
- * This file is part of JuPedSim.
- *
- * JuPedSim is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Lesser General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * any later version.
- *
- * JuPedSim is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public License
- * along with JuPedSim. If not, see <http://www.gnu.org/licenses/>.
- *
- * \section Description
- * Implementation of classes for ...
- *
- *
- **/
-#define TESTING
-#define GEO_UP_SCALE 1
-#include "FloorfieldViaFM.h"
-//#include <iostream>
-//#include <string>
-//#include <sstream>
-//#include <fstream>
-//#include <cfloat>
-//#include <chrono>
-
-#ifdef _OPENMP
-#else
-#define omp_get_thread_num() 0
-#define omp_get_max_threads() 1
-#endif
-
-FloorfieldViaFM::FloorfieldViaFM()
-{
- //ctor (very ugly)
- //std::cerr << "The defaultconsturctor FloorfieldViaFM should not be called!" << std::endl;
-}
-
-FloorfieldViaFM::~FloorfieldViaFM()
-{
- //dtor
- delete _grid;
- if (_gcode) delete[] _gcode;
- if (_subrooms) delete[] _subrooms;
- if (_dist2Wall) delete[] _dist2Wall;
- if (_speedInitial) delete[] _speedInitial;
- if (_modifiedspeed) delete[] _modifiedspeed;
- if (_densityspeed) delete[] _densityspeed;
- //if (cost) delete[] cost;
- //if (neggrad) delete[] neggrad;
- if (_dirToWall) delete[] _dirToWall;
- //if (trialfield) delete[] trialfield;
- for ( const auto& goalid : _goalcostmap) {
- if (goalid.second) delete[] goalid.second;
- }
- for ( const auto& id : _costmap) {
- //if (id.first == -1) continue;
- if (id.second) delete[] id.second;
- if (_neggradmap.at(id.first)) delete[] _neggradmap.at(id.first);
- //map will be deleted by itself
- }
-
-}
-
-FloorfieldViaFM::FloorfieldViaFM(const Building* const buildingArg, const double hxArg, const double hyArg,
- const double wallAvoidDistance, const bool useDistancefield, const bool onlyRoomsWithExits) {
- //ctor
- //_threshold = -1; //negative value means: ignore threshold
- _threshold = wallAvoidDistance;
- _building = buildingArg;
- _useDistanceToWall = useDistancefield;
-
- if (hxArg != hyArg) {
- //std::cerr << "ERROR: hx != hy <=========";
- Log->Write("WARNING: \tFloor field: stepsize hx differs from hy! Taking hx = %d for both.", hxArg);
- }
- //parse building and create list of walls/obstacles (find xmin xmax, ymin, ymax, and add border?)
- //Log->Write("INFO: \tStart Parsing: Building");
- if (onlyRoomsWithExits) {
- parseBuildingForExits(buildingArg, hxArg, hyArg);
- } else {
- parseBuilding(buildingArg, hxArg, hyArg);
- }
- //Log->Write("INFO: \tFinished Parsing: Building");
- //testoutput("AALineScan.vtk", "AALineScan.txt", dist2Wall);
-
- prepareForDistanceFieldCalculation(onlyRoomsWithExits);
- //Log->Write("INFO: \tGrid initialized: Walls");
-
- calculateDistanceField(-1.); //negative threshold is ignored, so all distances get calculated. this is important since distances is used for slowdown/redirect
- //Log->Write("INFO: \tGrid initialized: Walldistances");
-
- setSpeed(useDistancefield); //use distance2Wall
- //Log->Write("INFO: \tGrid initialized: Speed");
-
- calculateFloorfield(_exitsFromScope, _cost, _neggrad);
- //writing FF-file disabled, we will not revive it ( @todo: argraf )
-}
-
-FloorfieldViaFM::FloorfieldViaFM(const FloorfieldViaFM &other) :
-_building(other._building)
-
-{
- _grid = other.getGrid();
- long int otherNumOfPoints = _grid->GetnPoints();
-
- _wall.clear();
- _wall.reserve(other._wall.size());
- std::copy(other._wall.begin(), other._wall.end(), _wall.begin());
-
- _exitsFromScope.clear();
- _exitsFromScope.reserve(other._exitsFromScope.size());
- std::copy(other._exitsFromScope.begin(), other._exitsFromScope.end(), _exitsFromScope.begin());
-
- _numOfExits = other._numOfExits;
-
- _gcode = new int[otherNumOfPoints];
- std::copy(other._gcode, other._gcode + otherNumOfPoints, _gcode);
-
- _dist2Wall = new double[otherNumOfPoints];
- std::copy(other._dist2Wall, other._dist2Wall + otherNumOfPoints, _dist2Wall);
-
- _speedInitial = new double[otherNumOfPoints];
- std::copy(other._speedInitial, other._speedInitial + otherNumOfPoints, _speedInitial);
-
- _modifiedspeed = new double[otherNumOfPoints];
- std::copy(other._modifiedspeed, other._modifiedspeed + otherNumOfPoints, _modifiedspeed);
-
- _densityspeed = new double[otherNumOfPoints];
- std::copy(other._densityspeed, other._densityspeed + otherNumOfPoints, _densityspeed);
-
- _cost = new double[otherNumOfPoints];
- std::copy(other._cost, other._cost + otherNumOfPoints, _cost);
-
- _neggrad = new Point[otherNumOfPoints]; //gradients
- std::copy(other._neggrad, other._neggrad + otherNumOfPoints, _neggrad);
-
- _dirToWall = new Point[otherNumOfPoints];
- std::copy(other._dirToWall, other._dirToWall + otherNumOfPoints, _dirToWall);
-
- _threshold = other._threshold;
- _useDistanceToWall = other._useDistanceToWall;
-}
-
-FloorfieldViaFM::FloorfieldViaFM(const std::string& filename) {
-
-
- Log->Write("ERROR: \tReading FF from file not supported!!");
- Log->Write(filename);
-
-}
-
-//void FloorfieldViaFM::getDirectionAt(const Point& position, Point& direction){
-// long int key = grid->getKeyAtPoint(position);
-// direction._x = (neggrad[key]._x);
-// direction._y = (neggrad[key]._y);
-//}
-
-void FloorfieldViaFM::getDirectionToDestination(Pedestrian* ped, Point& direction){
- const Point& position = ped->GetPos();
- int destID = ped->GetExitIndex();
- long int key = _grid->getKeyAtPoint(position);
- getDirectionToUID(destID, key, direction);
- if (direction._x == DBL_MAX && direction._y == DBL_MAX) {
- // This can be interpreted differently by the different operational models.
- // The inaccuracy introduced by this is negligible. --f.mack
- direction._x = 0;
- direction._y = 0;
- }
-}
-
-void FloorfieldViaFM::getDirectionToUID(int destID, const long int key, Point &direction) {
- getDirectionToUID(destID, key, direction, global_shortest);
-}
-
-void FloorfieldViaFM::getDirectionToUID(int destID, const long int key, Point& direction, int mode) {
- //what if goal == -1, meaning closest exit... is GetExitIndex then -1? NO... ExitIndex is UID, given by router
- //if (ped->GetFinalDestination() == -1) /*go to closest exit*/ destID != -1;
-
- if ((key < 0) || (key >= _grid->GetnPoints())) { // @todo: ar.graf: this check in a #ifdef-block?
- Log->Write("ERROR: \t Floorfield tried to access a key out of grid!");
- direction._x = 0.;
- direction._y = 0.;
- return;
- }
- Point* localneggradptr = nullptr;
- double* localcostptr = nullptr;
- {
- if (_neggradmap.count(destID) == 0) {
- //Log->Write("FF for destID %d does not exist (key is %d)", destID, key);
- //check, if distID is in this grid
- Hline* destLine = _building->GetTransOrCrossByUID(destID);
- Point A = destLine->GetPoint1();
- Point B = destLine->GetPoint2();
- if (!(_grid->includesPoint(A)) || !(_grid->includesPoint(B))) {
- Log->Write("ERROR: \t Destination ID %d is not in grid!", destID);
- direction._x = direction._y = 0.;
- //return;
- }
- }
- localneggradptr = (_neggradmap.count(destID) == 0) ? nullptr : _neggradmap.at(destID);
- localcostptr = (_costmap.count(destID) == 0) ? nullptr : _costmap.at(destID);
- if (localneggradptr == nullptr) {
- bool isBeingCalculated;
-#pragma omp critical(floorfieldsBeingCalculated)
- {
- if (!(isBeingCalculated = _floorfieldsBeingCalculated.count(destID) > 0)) {
- _floorfieldsBeingCalculated.insert(destID);
- }
- }
- if (isBeingCalculated) {
- // we do not want to wait until the other calculation has finished, so we return immediately
- // the values are corrected in getDirectionToDestination(), and getCostToDestination doesn't care about the direction
- direction._x = DBL_MAX;
- direction._y = DBL_MAX;
- return;
- }
-
- //create floorfield (remove mapentry with nullptr, allocate memory, add mapentry, create ff)
- localcostptr = new double[_grid->GetnPoints()];
- localneggradptr = new Point[_grid->GetnPoints()];
-#pragma omp critical(neggradmap)
- _neggradmap.erase(destID);
-#pragma omp critical(neggradmap)
- _neggradmap.emplace(destID, localneggradptr);
-#pragma omp critical(costmap)
- _costmap.erase(destID);
-#pragma omp critical(costmap)
- _costmap.emplace(destID, localcostptr);
-
- //create ff (prepare Trial-mechanic, then calc)
-// for (long int i = 0; i < grid->GetnPoints(); ++i) {
-// //set Trialptr to fieldelements
-// trialfield[i].cost = localcostptr + i;
-// trialfield[i].neggrad = localneggradptr + i;
-// trialfield[i].father = nullptr;
-// trialfield[i].child = nullptr;
-// }
-// clearAndPrepareForFloorfieldReCalc(localcostptr);
- std::vector<Line> localline = {Line((Line) *(_building->GetTransOrCrossByUID(destID)))};
-// setNewGoalAfterTheClear(localcostptr, localline);
- //Log->Write("Starting FF for UID %d (ID %d)", destID, dynamic_cast<Crossing*>(building->GetTransOrCrossByUID(destID))->GetID());
- //std::cerr << "\rW\tO\tR\tK\tI\tN\tG";
- if (mode == quickest) {
- calculateFloorfield(localline, localcostptr, localneggradptr, _densityspeed);
- } else {
- calculateFloorfield(localline, localcostptr, localneggradptr, _modifiedspeed);
- }
-#pragma omp critical(floorfieldsBeingCalculated)
- {
- if (_floorfieldsBeingCalculated.count(destID) != 1) {
- Log->Write("ERROR: FloorfieldViaFM::getDirectionToUID: key %d was calculating FF for destID %d, but it was removed from floorfieldsBeingCalculated meanwhile", key, destID);
- }
- _floorfieldsBeingCalculated.erase(destID);
- }
- //Log->Write("Ending FF for UID %d", destID);
- //std::cerr << "\r W\t O\t R\t K\t I\t N\t G";
- }
- }
- direction._x = (localneggradptr[key]._x);
- direction._y = (localneggradptr[key]._y);
-}
-
-void FloorfieldViaFM::createMapEntryInLineToGoalID(const int goalID)
-{
- Point* localneggradptr;
- double* localcostptr;
- if (goalID < 0) {
- Log->Write("WARNING: \t goalID was negative in FloorfieldViaFM::createMapEntryInLineToGoalID");
- return;
- }
- if (!_building->GetFinalGoal(goalID)) {
- Log->Write("WARNING: \t goalID was unknown in FloorfieldViaFM::createMapEntryInLineToGoalID");
- return;
- }
-
- // The scope of this critical section can probably be reduced (maybe use a GoalsBeingCalculated similar to FloorfieldViaFM::getDirectionToUID)
-#pragma omp critical(FloorfieldViaFM_maps)
- {
- if (_goalcostmap.count(goalID) == 0) { //no entry for goalcostmap, so we need to calc FF
- _goalcostmap.emplace(goalID, nullptr);
- _goalneggradmap.emplace(goalID, nullptr);
- _goalToLineUIDmap.emplace(goalID, -1);
- _goalToLineUIDmap2.emplace(goalID, -1);
- _goalToLineUIDmap3.emplace(goalID, -1);
- }
- localneggradptr = _goalneggradmap.at(goalID);
- localcostptr = _goalcostmap.at(goalID);
- if (localneggradptr == nullptr) {
- //create floorfield (remove mapentry with nullptr, allocate memory, add mapentry, create ff)
- localcostptr = new double[_grid->GetnPoints()];
- localneggradptr = new Point[_grid->GetnPoints()];
- _goalneggradmap.erase(goalID);
- //goalneggradmap.emplace(goalID, localneggradptr);
- _goalcostmap.erase(goalID);
- //goalcostmap.emplace(goalID, localcostptr);
- //create ff (prepare Trial-mechanic, then calc)
-// for (long int i = 0; i < grid->GetnPoints(); ++i) {
-// //set Trialptr to fieldelements
-// trialfield[i].cost = localcostptr + i;
-// trialfield[i].neggrad = localneggradptr + i;
-// trialfield[i].father = nullptr;
-// trialfield[i].child = nullptr;
-// }
-// clearAndPrepareForFloorfieldReCalc(localcostptr);
-
- //get all lines/walls of goalID
- vector<Line> localline;
- const std::map<int, Goal*>& allgoals = _building->GetAllGoals();
- vector<Wall> localwalls = allgoals.at(goalID)->GetAllWalls();
-
- double xMin = _grid->GetxMin();
- double xMax = _grid->GetxMax();
-
- double yMin = _grid->GetyMin();
- double yMax = _grid->GetyMax();
-
- for (const auto& iwall:localwalls) {
- const Point& a = iwall.GetPoint1();
- const Point& b = iwall.GetPoint2();
- if (
- (a._x >= xMin) && (a._x <= xMax)
- &&(a._y >= yMin) && (a._y <= yMax)
- &&(b._x >= xMin) && (b._x <= xMax)
- &&(b._y >= yMin) && (b._y <= yMax)
- )
- {
- localline.emplace_back( Line( (Line) iwall ) );
- } else {
- std::cerr << "GOAL " << goalID << " includes point out of grid!" << std::endl;
- std::cerr << "Point: " << a._x << ", " << a._y << std::endl;
- std::cerr << "Point: " << b._x << ", " << b._y << std::endl;
- }
- }
-
-// setNewGoalAfterTheClear(localcostptr, localline);
-
- //performance-measurement:
- //auto start = std::chrono::steady_clock::now();
-
- calculateFloorfield(localline, localcostptr, localneggradptr);
-
- //performance-measurement:
- //auto end = std::chrono::steady_clock::now();
- //auto diff = end - start;
- //std::cerr << std::chrono::duration_cast<std::chrono::seconds>(end - start).count() << std::endl;
- //std::cerr << "new GOALfield " << goalID << " : " << localline[0].GetPoint1().GetX() << " " << localline[0].GetPoint1().GetY() << " " << localline[0].GetPoint2().GetX() << " " << localline[0].GetPoint2().GetY() << std::endl;
- //Log->Write("new GOALfield " + std::to_string(goalID) + " : " + std::to_string(localline[0].GetPoint1().GetX()));
- //Log->Write("new GOALfield " + std::to_string(goalID) + " : " + std::to_string( std::chrono::duration_cast<std::chrono::seconds>(end - start).count() ) + " " + std::to_string(localline.size()) );
- //find closest door and add to cheatmap "goalToLineUID" map
-
- const std::map<int, Transition*>& transitions = _building->GetAllTransitions();
- int UID_of_MIN = -1;
- int UID_of_MIN2 = -1;
- int UID_of_MIN3 = -1;
- double cost_of_MIN = DBL_MAX;
- double cost_of_MIN2 = DBL_MAX;
- double cost_of_MIN3 = DBL_MAX;
- long int dummykey;
- for (const auto& loctrans : transitions) {
- if (!loctrans.second->IsExit() || !loctrans.second->IsOpen()) {
- continue;
- }
- dummykey = _grid->getKeyAtPoint(loctrans.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 = loctrans.second->GetUniqueID();
- cost_of_MIN = localcostptr[dummykey];
- //std::cerr << std::endl << "Closer Line found: " << UID_of_MIN ;
- 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 = loctrans.second->GetUniqueID();
- cost_of_MIN2 = localcostptr[dummykey];
- continue;
- }
- if ((cost_of_MIN3 > localcostptr[dummykey]) && (localcostptr[dummykey] >= 0.)) {
- UID_of_MIN3 = loctrans.second->GetUniqueID();
- cost_of_MIN3 = localcostptr[dummykey];
- continue;
- }
- }
- _goalToLineUIDmap.erase(goalID);
- _goalToLineUIDmap.emplace(goalID, UID_of_MIN);
- _goalToLineUIDmap2.erase(goalID);
- _goalToLineUIDmap2.emplace(goalID, UID_of_MIN2);
- _goalToLineUIDmap3.erase(goalID);
- _goalToLineUIDmap3.emplace(goalID, UID_of_MIN3);
- delete[] localcostptr;
- delete[] localneggradptr;
- }
- }
-}
-
-double FloorfieldViaFM::getCostToDestination(const int destID, const Point& position) {
- return getCostToDestination(destID, position, global_shortest);
-}
-
-double FloorfieldViaFM::getCostToDestination(const int destID, const Point& position, int mode) {
- if ((_costmap.count(destID) == 0) || (_costmap.at(destID) == nullptr)) {
- Point dummy;
- getDirectionToUID(destID, 0, dummy, mode); //this call induces the floorfieldcalculation
- }
- if ((_costmap.count(destID) == 0) || (_costmap.at(destID) == nullptr)) {
- Log->Write("ERROR: \tDestinationUID %d is invalid / out of grid.", destID);
- return DBL_MAX;
- }
- if (_grid->getKeyAtPoint(position) == -1) { //position is out of grid
- return -7;
- }
- return (_costmap.at(destID))[_grid->getKeyAtPoint(position)];
-}
-
-void FloorfieldViaFM::getDir2WallAt(const Point& position, Point& direction){
- long int key = _grid->getKeyAtPoint(position);
- //debug assert
- if (key < 0) {
- Log->Write("ERROR: \tgetDir2WallAt error");
- } else {
- direction._x = (_dirToWall[key]._x);
- direction._y = (_dirToWall[key]._y);
- }
-}
-
-double FloorfieldViaFM::getDistance2WallAt(const Point& position) {
- long int key = _grid->getKeyAtPoint(position);
- //debug assert
- if (key < 0) {
- Log->Write("ERROR: \tgetDistance2WallAt error");
- return 1.;
- } else {
- return _dist2Wall[key];
- }
-}
-
-//int FloorfieldViaFM::getSubroomUIDAt(const Point &position) {
-// long int key = grid->getKeyAtPoint(position);
-// return subroomUID[key];
-//}
-
-/*!
- * \brief Parsing geo-info but conflicts in multi-floor-buildings OBSOLETE
- *
- * When parsing a building with multiple floors, the projection of all floors onto the x-/y- plane leads to wrong
- * results.
- *
- * \param[in] buildingArg provides the handle to all information
- * \param[out] stepSizeX/-Y discretization of the grid, which will be created here
- * \return member attributes of FloorfieldViaFM class are allocated and initialized
- * \sa Macros.h, RectGrid.h
- * \note
- * \warning
- */
-void FloorfieldViaFM::parseBuilding(const Building* const buildingArg, const double stepSizeX, const double stepSizeY) {
- _building = buildingArg;
- //init min/max before parsing
- double xMin = DBL_MAX;
- double xMax = -DBL_MAX;
- double yMin = xMin;
- double yMax = xMax;
-
- if (stepSizeX != stepSizeY) Log->Write("ERROR: \tStepsizes in x- and y-direction must be identical!");
-
- _costmap.clear();
- _neggradmap.clear();
- _wall.clear();
- _exitsFromScope.clear();
-
- //create a list of walls
- const std::map<int, Transition*>& allTransitions = buildingArg->GetAllTransitions();
- for (auto& trans : allTransitions) {
- if (
- trans.second->IsExit() && trans.second->IsOpen()
- )
- {
- _exitsFromScope.emplace_back(Line ( (Line) *(trans.second)));
- }
- //populate both maps: costmap, neggradmap. These are the lookup maps for floorfields to specific transitions
- _costmap.emplace(trans.second->GetUniqueID(), nullptr);
- _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)));
- }
-
- }
- for (const auto& itRoom : buildingArg->GetAllRooms()) {
- for (const auto& itSubroom : itRoom.second->GetAllSubRooms()) {
- std::vector<Obstacle*> allObstacles = itSubroom.second->GetAllObstacles();
- for (std::vector<Obstacle*>::iterator itObstacles = allObstacles.begin(); itObstacles != allObstacles.end(); ++itObstacles) {
-
- std::vector<Wall> allObsWalls = (*itObstacles)->GetAllWalls();
- for (std::vector<Wall>::iterator itObsWall = allObsWalls.begin(); itObsWall != allObsWalls.end(); ++itObsWall) {
- _wall.emplace_back(Line( (Line) *itObsWall));
- // xMin xMax
- if ((*itObsWall).GetPoint1()._x < xMin) xMin = (*itObsWall).GetPoint1()._x;
- if ((*itObsWall).GetPoint2()._x < xMin) xMin = (*itObsWall).GetPoint2()._x;
- if ((*itObsWall).GetPoint1()._x > xMax) xMax = (*itObsWall).GetPoint1()._x;
- if ((*itObsWall).GetPoint2()._x > xMax) xMax = (*itObsWall).GetPoint2()._x;
-
- // yMin yMax
- if ((*itObsWall).GetPoint1()._y < yMin) yMin = (*itObsWall).GetPoint1()._y;
- if ((*itObsWall).GetPoint2()._y < yMin) yMin = (*itObsWall).GetPoint2()._y;
- if ((*itObsWall).GetPoint1()._y > yMax) yMax = (*itObsWall).GetPoint1()._y;
- if ((*itObsWall).GetPoint2()._y > yMax) yMax = (*itObsWall).GetPoint2()._y;
- }
- }
-
- std::vector<Wall> allWalls = itSubroom.second->GetAllWalls();
- for (std::vector<Wall>::iterator itWall = allWalls.begin(); itWall != allWalls.end(); ++itWall) {
- _wall.emplace_back( Line( (Line) *itWall));
-
- // xMin xMax
- if ((*itWall).GetPoint1()._x < xMin) xMin = (*itWall).GetPoint1()._x;
- if ((*itWall).GetPoint2()._x < xMin) xMin = (*itWall).GetPoint2()._x;
- if ((*itWall).GetPoint1()._x > xMax) xMax = (*itWall).GetPoint1()._x;
- if ((*itWall).GetPoint2()._x > xMax) xMax = (*itWall).GetPoint2()._x;
-
- // yMin yMax
- if ((*itWall).GetPoint1()._y < yMin) yMin = (*itWall).GetPoint1()._y;
- if ((*itWall).GetPoint2()._y < yMin) yMin = (*itWall).GetPoint2()._y;
- if ((*itWall).GetPoint1()._y > yMax) yMax = (*itWall).GetPoint1()._y;
- if ((*itWall).GetPoint2()._y > yMax) yMax = (*itWall).GetPoint2()._y;
- }
-
- const vector<Crossing*>& allCrossings = itSubroom.second->GetAllCrossings();
- for (Crossing* crossPtr : allCrossings) {
- if (!crossPtr->IsOpen()) {
- _wall.emplace_back( Line( (Line) *crossPtr));
-
- if (crossPtr->GetPoint1()._x < xMin) xMin = crossPtr->GetPoint1()._x;
- if (crossPtr->GetPoint2()._x < xMin) xMin = crossPtr->GetPoint2()._x;
- if (crossPtr->GetPoint1()._x > xMax) xMax = crossPtr->GetPoint1()._x;
- if (crossPtr->GetPoint2()._x > xMax) xMax = crossPtr->GetPoint2()._x;
-
- if (crossPtr->GetPoint1()._y < yMin) yMin = crossPtr->GetPoint1()._y;
- if (crossPtr->GetPoint2()._y < yMin) yMin = crossPtr->GetPoint2()._y;
- if (crossPtr->GetPoint1()._y > yMax) yMax = crossPtr->GetPoint1()._y;
- if (crossPtr->GetPoint2()._y > yMax) yMax = crossPtr->GetPoint2()._y;
- }
- }
- }
- }
-
- //all goals
- const std::map<int, Goal*>& allgoals = buildingArg->GetAllGoals();
- for (auto eachgoal:allgoals) {
- for (auto& eachwall:eachgoal.second->GetAllWalls() ) {
- if (eachwall.GetPoint1()._x < xMin) xMin = eachwall.GetPoint1()._x;
- if (eachwall.GetPoint2()._x < xMin) xMin = eachwall.GetPoint2()._x;
- if (eachwall.GetPoint1()._x > xMax) xMax = eachwall.GetPoint1()._x;
- if (eachwall.GetPoint2()._x > xMax) xMax = eachwall.GetPoint2()._x;
-
- if (eachwall.GetPoint1()._y < yMin) yMin = eachwall.GetPoint1()._y;
- if (eachwall.GetPoint2()._y < yMin) yMin = eachwall.GetPoint2()._y;
- if (eachwall.GetPoint1()._y > yMax) yMax = eachwall.GetPoint1()._y;
- if (eachwall.GetPoint2()._y > yMax) yMax = eachwall.GetPoint2()._y;
- }
- _goalcostmap.emplace(eachgoal.second->GetId(), nullptr);
- _goalneggradmap.emplace(eachgoal.second->GetId(), nullptr);
- }
-
- //create Rect Grid
- _grid = new RectGrid();
- _grid->setBoundaries(xMin, yMin, xMax, yMax);
- _grid->setSpacing(stepSizeX, stepSizeX);
- _grid->createGrid();
-
- //create arrays
- _subrooms = new SubRoom*[_grid->GetnPoints()]();
- _gcode = new int[_grid->GetnPoints()]; //gcode:
- // enum GridCode { //used in floor fields
- // WALL = 0,
- // INSIDE,
- // OUTSIDE,
- // OPEN_CROSSING,
- // OPEN_TRANSITION,
- // CLOSED_CROSSING,
- // CLOSED_TRANSITION
- // };
- _dist2Wall = new double[_grid->GetnPoints()];
- _speedInitial = new double[_grid->GetnPoints()];
- _modifiedspeed = new double[_grid->GetnPoints()];
- _densityspeed = new double[_grid->GetnPoints()];
- _cost = new double[_grid->GetnPoints()];
- _neggrad = new Point[_grid->GetnPoints()];
- _dirToWall = new Point[_grid->GetnPoints()];
-
- _costmap.emplace(-1 , _cost); // enable default ff (closest exit)
- _neggradmap.emplace(-1, _neggrad);
-
- //init grid with -3 as unknown distance to any wall
- for(long int i = 0; i < _grid->GetnPoints(); ++i) {
- _dist2Wall[i] = -3.;
- _cost[i] = -2.;
- _gcode[i] = OUTSIDE;
- //flag[i] = FM_UNKNOWN; //unknown
- }
- drawLinesOnGrid<double>(_wall, _dist2Wall, 0.);
- drawLinesOnGrid<double>(_wall, _cost, -7.);
- drawLinesOnGrid<int>(_wall, _gcode, WALL);
- drawLinesOnGrid<int>(_exitsFromScope, _gcode, OPEN_TRANSITION);
-}
-
-/*!
- * \brief Parsing geo-info ONLY considering rooms with EXITS to the outside to avoid conflicts in multi-floor-buildings
- *
- * When parsing a building with multiple floors, the projection of all floors onto the x-/y- plane leads to wrong
- * results. We then decided, to consider each floor separately and each staircase separately.
- * We still need to match the ouside goals to an exit (transition), so that the router can guide agents to that goals.
- * Next problem was in buildings, where exits to the outside would now be on different rooms. This is why we create this
- * function. We want to create one floorfield for all rooms, that lead outside. Reason: We want the router to lead agents
- * to the maybe second or third best exit-door, which might be in a different room.
- *
- * \param[in] buildingArg provides the handle to all information
- * \param[out] stepSizeX/-Y discretization of the grid, which will be created here
- * \return member attributes of FloorfieldViaFM class are allocated and initialized
- * \sa Macros.h, RectGrid.h
- * \note
- * \warning
- */
-void FloorfieldViaFM::parseBuildingForExits(const Building* const buildingArg, const double stepSizeX, const double stepSizeY) {
- _building = buildingArg;
- //init min/max before parsing
- double xMin = DBL_MAX;
- double xMax = -DBL_MAX;
- double yMin = xMin;
- double yMax = xMax;
-
- if (stepSizeX != stepSizeY) Log->Write("ERROR: \tStepsizes in x- and y-direction must be identical!");
-
- _costmap.clear();
- _neggradmap.clear();
- _wall.clear();
- _exitsFromScope.clear();
-
- std::vector<int> exitRoomIDs;
- exitRoomIDs.clear();
-
- //create a list of walls
- const std::map<int, Transition*>& allTransitions = buildingArg->GetAllTransitions();
- for (auto& trans : allTransitions) {
- if (
- trans.second->IsExit() && trans.second->IsOpen()
- )
- {
- _exitsFromScope.emplace_back(Line ( (Line) *(trans.second)));
- int roomID = -1;
- if (trans.second->GetRoom1()) {
- roomID = trans.second->GetRoom1()->GetID();
- }
- if (trans.second->GetRoom2()) {
- roomID = trans.second->GetRoom2()->GetID();
- }
- if (std::find(exitRoomIDs.begin(), exitRoomIDs.end(), roomID) == exitRoomIDs.end()) {
- exitRoomIDs.emplace_back(roomID);
- }
- }
- //populate both maps: costmap, neggradmap. These are the lookup maps for floorfields to specific transitions
- _costmap.emplace(trans.second->GetUniqueID(), nullptr);
- _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)));
- }
-
- }
- for (const auto& itRoom : buildingArg->GetAllRooms()) {
- if (std::find(exitRoomIDs.begin(), exitRoomIDs.end(), itRoom.second->GetID()) == exitRoomIDs.end()) { //room with no exit
- continue;
- }
- for (const auto& itSubroom : itRoom.second->GetAllSubRooms()) {
- std::vector<Obstacle*> allObstacles = itSubroom.second->GetAllObstacles();
- for (std::vector<Obstacle*>::iterator itObstacles = allObstacles.begin(); itObstacles != allObstacles.end(); ++itObstacles) {
-
- std::vector<Wall> allObsWalls = (*itObstacles)->GetAllWalls();
- for (std::vector<Wall>::iterator itObsWall = allObsWalls.begin(); itObsWall != allObsWalls.end(); ++itObsWall) {
- _wall.emplace_back(Line( (Line) *itObsWall));
- // xMin xMax
- if ((*itObsWall).GetPoint1()._x < xMin) xMin = (*itObsWall).GetPoint1()._x;
- if ((*itObsWall).GetPoint2()._x < xMin) xMin = (*itObsWall).GetPoint2()._x;
- if ((*itObsWall).GetPoint1()._x > xMax) xMax = (*itObsWall).GetPoint1()._x;
- if ((*itObsWall).GetPoint2()._x > xMax) xMax = (*itObsWall).GetPoint2()._x;
-
- // yMin yMax
- if ((*itObsWall).GetPoint1()._y < yMin) yMin = (*itObsWall).GetPoint1()._y;
- if ((*itObsWall).GetPoint2()._y < yMin) yMin = (*itObsWall).GetPoint2()._y;
- if ((*itObsWall).GetPoint1()._y > yMax) yMax = (*itObsWall).GetPoint1()._y;
- if ((*itObsWall).GetPoint2()._y > yMax) yMax = (*itObsWall).GetPoint2()._y;
- }
- }
-
- std::vector<Wall> allWalls = itSubroom.second->GetAllWalls();
- for (std::vector<Wall>::iterator itWall = allWalls.begin(); itWall != allWalls.end(); ++itWall) {
- _wall.emplace_back( Line( (Line) *itWall));
-
- // xMin xMax
- if ((*itWall).GetPoint1()._x < xMin) xMin = (*itWall).GetPoint1()._x;
- if ((*itWall).GetPoint2()._x < xMin) xMin = (*itWall).GetPoint2()._x;
- if ((*itWall).GetPoint1()._x > xMax) xMax = (*itWall).GetPoint1()._x;
- if ((*itWall).GetPoint2()._x > xMax) xMax = (*itWall).GetPoint2()._x;
-
- // yMin yMax
- if ((*itWall).GetPoint1()._y < yMin) yMin = (*itWall).GetPoint1()._y;
- if ((*itWall).GetPoint2()._y < yMin) yMin = (*itWall).GetPoint2()._y;
- if ((*itWall).GetPoint1()._y > yMax) yMax = (*itWall).GetPoint1()._y;
- if ((*itWall).GetPoint2()._y > yMax) yMax = (*itWall).GetPoint2()._y;
- }
- const vector<Crossing*>& allCrossings = itSubroom.second->GetAllCrossings();
- for (Crossing* crossPtr : allCrossings) {
- if (!crossPtr->IsOpen()) {
- _wall.emplace_back( Line( (Line) *crossPtr));
-
- if (crossPtr->GetPoint1()._x < xMin) xMin = crossPtr->GetPoint1()._x;
- if (crossPtr->GetPoint2()._x < xMin) xMin = crossPtr->GetPoint2()._x;
- if (crossPtr->GetPoint1()._x > xMax) xMax = crossPtr->GetPoint1()._x;
- if (crossPtr->GetPoint2()._x > xMax) xMax = crossPtr->GetPoint2()._x;
-
- if (crossPtr->GetPoint1()._y < yMin) yMin = crossPtr->GetPoint1()._y;
- if (crossPtr->GetPoint2()._y < yMin) yMin = crossPtr->GetPoint2()._y;
- if (crossPtr->GetPoint1()._y > yMax) yMax = crossPtr->GetPoint1()._y;
- if (crossPtr->GetPoint2()._y > yMax) yMax = crossPtr->GetPoint2()._y;
- }
- }
- }
- }
-
- //all goals
- const std::map<int, Goal*>& allgoals = buildingArg->GetAllGoals();
- for (auto eachgoal:allgoals) {
- for (auto& eachwall:eachgoal.second->GetAllWalls() ) {
- if (eachwall.GetPoint1()._x < xMin) xMin = eachwall.GetPoint1()._x;
- if (eachwall.GetPoint2()._x < xMin) xMin = eachwall.GetPoint2()._x;
- if (eachwall.GetPoint1()._x > xMax) xMax = eachwall.GetPoint1()._x;
- if (eachwall.GetPoint2()._x > xMax) xMax = eachwall.GetPoint2()._x;
-
- if (eachwall.GetPoint1()._y < yMin) yMin = eachwall.GetPoint1()._y;
- if (eachwall.GetPoint2()._y < yMin) yMin = eachwall.GetPoint2()._y;
- if (eachwall.GetPoint1()._y > yMax) yMax = eachwall.GetPoint1()._y;
- if (eachwall.GetPoint2()._y > yMax) yMax = eachwall.GetPoint2()._y;
- }
- //goalcostmap.emplace(eachgoal.second->GetId(), nullptr);
- //goalneggradmap.emplace(eachgoal.second->GetId(), nullptr);
- }
-
- //create Rect Grid
- _grid = new RectGrid();
- _grid->setBoundaries(xMin, yMin, xMax, yMax);
- _grid->setSpacing(stepSizeX, stepSizeX);
- _grid->createGrid();
-
- //create arrays
- _gcode = new int[_grid->GetnPoints()]; //see Macros.h: enum GridCode {...}
- _subrooms = new SubRoom*[_grid->GetnPoints()]();
- _dist2Wall = new double[_grid->GetnPoints()];
- _speedInitial = new double[_grid->GetnPoints()];
- _modifiedspeed = new double[_grid->GetnPoints()];
- _cost = new double[_grid->GetnPoints()];
- _neggrad = new Point[_grid->GetnPoints()];
- _dirToWall = new Point[_grid->GetnPoints()];
-
- _costmap.emplace(-1 , _cost); // enable default ff (closest exit)
- _neggradmap.emplace(-1, _neggrad);
-
- //init grid with -3 as unknown distance to any wall
- for(long int i = 0; i < _grid->GetnPoints(); ++i) {
- _dist2Wall[i] = -3.;
- _cost[i] = -2.;
- _gcode[i] = OUTSIDE; //unknown
- _subrooms[i] = nullptr;
- }
- drawLinesOnGrid<double>(_wall, _dist2Wall, 0.);
- drawLinesOnGrid<double>(_wall, _cost, -7.);
- drawLinesOnGrid<int>(_wall, _gcode, WALL);
- drawLinesOnGrid<int>(_exitsFromScope, _gcode, OPEN_TRANSITION);
-}
-
-//this function must only be used BEFORE calculateDistanceField(), because we set trialfield[].cost = dist2Wall AND we init dist2Wall with "-3"
-void FloorfieldViaFM::prepareForDistanceFieldCalculation(const bool onlyRoomsWithExits) { //onlyRoomsWithExits means, outside points must be considered
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
-
- switch (_gcode[i]) {
- //wall, closed_cross/_trans are all coded with "WALL" after prasing building
- case WALL:
- _speedInitial[i] = .001;
- _cost[i] = -7.;
- _dist2Wall[i] = 0.;
- break;
- case CLOSED_CROSSING:
- _speedInitial[i] = .001;
- _cost[i] = -7.;
- _dist2Wall[i] = 0.;
- break;
- case CLOSED_TRANSITION:
- _speedInitial[i] = .001;
- _cost[i] = -7.;
- _dist2Wall[i] = 0.;
- break;
- //open transitions are marked
- case OPEN_TRANSITION:
- _speedInitial[i] = 1.;
- _cost[i] = 0.;
- _neggrad[i]._x = (0.); //must be changed to costarray/neggradarray?
- _neggrad[i]._y = (0.); //we can leave it, if we agree on cost/neggrad being
- _dirToWall[i]._x = (0.); //default floorfield using all exits and have the
- _dirToWall[i]._y = (0.); //array mechanic on top
- break;
- //open crossings are not marked at all after parsing building
- case OPEN_CROSSING:
- _speedInitial[i] = 1.;
- _cost[i] = -2.;
- break;
- //after parsing, none is INSIDE, but for style reasons, I want switch cases to show all values
- case INSIDE:
- _speedInitial[i] = 1.;
- _cost[i] = -2.;
- break;
- //this is the main thing in this loop, we want to find and mark inside points (and it is costly!!)
- case OUTSIDE:
- {
- SubRoom* subroom = isInside(i);
- if (subroom || onlyRoomsWithExits) {
- _speedInitial[i] = 1.;
- _cost[i] = -2.;
- _gcode[i] = INSIDE;
- _subrooms[i] = subroom;
- }
- break;
- }
- } //switch
- } //for loop (points)
- // drawLinesOnGrid(exits, cost, 0.); //already mark targets/exits in cost array (for floorfieldcalc and crossout (LocalFF))
-}
-
-void FloorfieldViaFM::deleteAllFFs() {
- for (size_t i = 0; i < _costmap.size(); ++i) {
- auto costIter = _costmap.begin();
- auto negIter = _neggradmap.begin();
- std::advance(costIter, (_costmap.size() - (i+1)));
- std::advance(negIter, (_neggradmap.size() - (i+1)));
-
- if (costIter->second) delete[] costIter->second;
- if (negIter->second) delete[] negIter->second;
-
- costIter->second = nullptr;
- negIter->second = nullptr;
- }
-}
-
-template <typename T>
-void FloorfieldViaFM::drawLinesOnGrid(std::vector<Line>& wallArg, T* const target, const T value) { //no init, plz init elsewhere
-// i~x; j~y;
-//http://stackoverflow.com/questions/10060046/drawing-lines-with-bresenhams-line-algorithm
-//src in answer of "Avi"; adapted to fit this application
-
-// //init with inside value:
-// long int indexMax = grid->GetnPoints();
-// for (long int i = 0; i < indexMax; ++i) {
-// target[i] = inside;
-// }
-
- //grid handeling local vars:
- long int iMax = _grid->GetiMax();
-
- long int iStart, iEnd;
- long int jStart, jEnd;
- long int iDot, jDot;
- long int key;
- long int deltaX, deltaY, deltaX1, deltaY1, px, py, xe, ye, i; //Bresenham Algorithm
-
- for (auto& line : wallArg) {
- key = _grid->getKeyAtPoint(line.GetPoint1());
- iStart = (long) _grid->get_i_fromKey(key);
- jStart = (long) _grid->get_j_fromKey(key);
-
- key = _grid->getKeyAtPoint(line.GetPoint2());
- iEnd = (long) _grid->get_i_fromKey(key);
- jEnd = (long) _grid->get_j_fromKey(key);
-
- deltaX = (int) (iEnd - iStart);
- deltaY = (int) (jEnd - jStart);
- deltaX1 = abs( (int) (iEnd - iStart));
- deltaY1 = abs( (int) (jEnd - jStart));
-
- px = 2*deltaY1 - deltaX1;
- py = 2*deltaX1 - deltaY1;
-
- if(deltaY1<=deltaX1) {
- if(deltaX>=0) {
- iDot = iStart;
- jDot = jStart;
- xe = iEnd;
- } else {
- iDot = iEnd;
- jDot = jEnd;
- xe = iStart;
- }
- if ((_gcode[jDot*iMax + iDot] != WALL) && (_gcode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gcode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
- target[jDot * iMax + iDot] = value;
- }
- for (i=0; iDot < xe; ++i) {
- ++iDot;
- if(px<0) {
- px+=2*deltaY1;
- } else {
- if((deltaX<0 && deltaY<0) || (deltaX>0 && deltaY>0)) {
- ++jDot;
- } else {
- --jDot;
- }
- px+=2*(deltaY1-deltaX1);
- }
- if ((_gcode[jDot*iMax + iDot] != WALL) && (_gcode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gcode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
- target[jDot * iMax + iDot] = value;
- }
- }
- } else {
- if(deltaY>=0) {
- iDot = iStart;
- jDot = jStart;
- ye = jEnd;
- } else {
- iDot = iEnd;
- jDot = jEnd;
- ye = jStart;
- }
- if ((_gcode[jDot*iMax + iDot] != WALL) && (_gcode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gcode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
- target[jDot * iMax + iDot] = value;
- }
- for(i=0; jDot<ye; ++i) {
- ++jDot;
- if (py<=0) {
- py+=2*deltaX1;
- } else {
- if((deltaX<0 && deltaY<0) || (deltaX>0 && deltaY>0)) {
- ++iDot;
- } else {
- --iDot;
- }
- py+=2*(deltaX1-deltaY1);
- }
- if ((_gcode[jDot*iMax + iDot] != WALL) && (_gcode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gcode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
- target[jDot * iMax + iDot] = value;
- }
- }
- }
- } //loop over all walls
-
-} //drawLinesOnGrid
-
-void FloorfieldViaFM::setSpeed(bool useDistance2WallArg) {
- if (useDistance2WallArg && (_threshold > 0)) {
- double temp; //needed to only slowdown band of threshold. outside of band modifiedspeed should be 1
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- temp = (_dist2Wall[i] < _threshold) ? _dist2Wall[i] : _threshold;
- _modifiedspeed[i] = 0.001 + 0.999 * (temp/_threshold); //linear ramp from wall (0.001) to thresholddistance (1.000)
- }
- } else {
- if (useDistance2WallArg) { //favor middle of hallways/rooms
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- if (_threshold == 0.) { //special case if user set (use_wall_avoidance = true, wall_avoid_distance = 0.0) and thus wants a plain floorfield
- _modifiedspeed[i] = _speedInitial[i];
- } else { //this is the regular case for "favor middle of hallways/rooms
- _modifiedspeed[i] = 0.001 + 0.999 * (_dist2Wall[i]/10); // @todo: ar.graf (10 ist ein hardgecodeter wert.. sollte ggf. angepasst werden)
- }
- }
- } else { //do not use Distance2Wall
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- _modifiedspeed[i] = _speedInitial[i];
- }
- }
- }
- if (_densityspeed) {
- std::copy(_modifiedspeed, _modifiedspeed + _grid->GetnPoints(), _densityspeed);
- }
-}
-
-void FloorfieldViaFM::setSpeedThruPeds(Pedestrian* const * pedsArg, int nsize, int modechoice, double radius) {
-
- long int delta = radius / _grid->Gethx();
- long int posIndex = 0;
- long int pos_i = 0;
- long int pos_j = 0;
- long int i_start = 0;
- long int j_start = 0;
- long int i_end = 0;
- long int j_end = 0;
- double indexDistance = 0.0;
-
- if (nsize == 0) {
- Log->Write("WARNING: \tSetSpeedThruPeds: nsize is ZERO");
- } else {
- Log->Write("INFO: \t\tNumber of Peds used in setSpeedThruPeds: %d",nsize);
- }
-
- if ((modechoice == quickest) && (!_densityspeed)) {
- _densityspeed = new double[_grid->GetnPoints()];
- }
- //std::copy(modifiedspeed, modifiedspeed+(grid->GetnPoints()), densityspeed);
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- _densityspeed[i] = _speedInitial[i];
- }
-
- for (int i = 0; i < nsize; ++i) {
- //the following check is not 3D proof, we require the caller of this function to provide a list with "valid"
- //pedestrian-pointer
- if (!_grid->includesPoint(pedsArg[i]->GetPos())) {
- continue;
- }
- /*this value defines the jam-speed threshold*/
-// if (pedsArg[i]->GetEllipse().GetV().Norm() > 0.8*pedsArg[i]->GetEllipse().GetV0()) {
-// continue;
-// }
- posIndex = _grid->getKeyAtPoint(pedsArg[i]->GetPos());
- pos_i = _grid->get_i_fromKey(posIndex);
- pos_j = _grid->get_j_fromKey(posIndex);
-
- i_start = ((pos_i - delta) < 0) ? 0 : (pos_i - delta);
- i_end = ((pos_i + delta) >= _grid->GetiMax()) ? _grid->GetiMax()-1 : (pos_i + delta);
-
- j_start = ((pos_j - delta) < 0) ? 0 : (pos_j - delta);
- j_end = ((pos_j + delta) >= _grid->GetjMax()) ? _grid->GetjMax()-1 : (pos_j + delta);
-
- for (long int curr_i = i_start; curr_i < i_end; ++curr_i) {
- for (long int curr_j = j_start; curr_j < j_end; ++curr_j) {
- //indexDistance holds the square
- indexDistance = ( (curr_i - pos_i)*(curr_i - pos_i) + (curr_j - pos_j)*(curr_j - pos_j) );
- //now using indexDistance to store the (speed) reduction value
- //indexDistance = (delta*delta) - (indexDistance);
- //if (indexDistance < 0) { indexDistance = 0.;}
- //scale to [0 .. 1]
- //indexDistance = indexDistance/(delta*delta);
-
- //densityspeed[curr_j*grid->GetiMax() + curr_i] = (indexDistance > (delta*delta)) ? densityspeed[curr_j*grid->GetiMax() + curr_i] : .001;
- if (indexDistance < (delta*delta)) {
- //std::cout << "c h a n g i n g ";
- _densityspeed[curr_j*_grid->GetiMax() + curr_i] = 0.07;
- }
- }
- }
- }
-
-}
-
-void FloorfieldViaFM::calculateFloorfield(std::vector<Line>& targetlines, double* costarray, Point* neggradarray) {
- calculateFloorfield(targetlines, costarray, neggradarray, _modifiedspeed);
-}
-
-void FloorfieldViaFM::calculateFloorfield(std::vector<Line>& targetlines, double* costarray, Point* neggradarray, double* speedarray) {
-
- std::priority_queue<TrialP, std::vector<TrialP>, std::greater<TrialP>> trialqueue;
- int* flag = new int[_grid->GetnPoints()];
-
- //re-init memory
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- if ((_gcode[i] == INSIDE) || (_gcode[i] == OPEN_TRANSITION) || (_gcode[i] == OPEN_CROSSING)) {
- flag[i] = FM_UNKNOWN; //inside
- } else {
- flag[i] = FM_OUTSIDE; //wall, outside or closed
- }
-
- costarray[i] = -7.;
- }
- drawLinesOnGrid<double>(targetlines, costarray, 0.);
- drawLinesOnGrid<int>(targetlines, flag, FM_FINAL);
-
- //init narrowband
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- if (flag[i] == FM_FINAL) {
- TrialP newTrialP = TrialP(i, costarray, speedarray, flag, neggradarray);
- checkNeighborsAndAddToNarrowband(trialqueue, newTrialP,
- [&] (TrialP trialPArg) {this->calcFloorfield(trialPArg);});
- }
- }
-
- // initial narrowband done, now loop (while not empty: remove smallest, add neighbors of removed)
- while (!trialqueue.empty()) {
- long int keyOfSmallest = trialqueue.top().key;
- flag[keyOfSmallest] = FM_FINAL;
- trialqueue.pop();
- TrialP newTrialP = TrialP(keyOfSmallest, costarray, speedarray, flag, neggradarray);
- checkNeighborsAndAddToNarrowband(trialqueue, newTrialP,
- [&] (TrialP trialPArg) { this->calcFloorfield(trialPArg);});
- }
- delete[] flag;
-}
-
-void FloorfieldViaFM::calculateDistanceField(const double thresholdArg) { //if threshold negative, then ignore it
-
- std::priority_queue<TrialP, std::vector<TrialP>, std::greater<TrialP>> trialqueue;
- int* flag = new int[_grid->GetnPoints()];
-#ifdef TESTING
- //sanity check (fields <> 0)
- if (flag == nullptr) return;
- if (_dist2Wall == 0) return;
- if (_speedInitial == 0) return;
- if (_cost == 0) return;
- if (_neggrad == 0) return;
-#endif //TESTING
-
- //re-init memory
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- if ((_gcode[i] == INSIDE) || (_gcode[i] == OPEN_TRANSITION) || (_gcode[i] == OPEN_CROSSING)) {
- flag[i] = FM_UNKNOWN; //inside
- } else if (_gcode[i] == OUTSIDE) {
- flag[i] = FM_OUTSIDE; //outside
- } else {
- flag[i] = FM_FINAL; //wall or closed
- }
- }
-
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- //if ((dist2Wall[i] == 0.) && (flag[i] == -7.)) {
- if ((_gcode[i] == WALL) || (_gcode[i] == CLOSED_TRANSITION) || (_gcode[i] == CLOSED_CROSSING)) {
- TrialP newP = TrialP(i, _dist2Wall, _speedInitial, flag, _dirToWall);
- checkNeighborsAndAddToNarrowband(trialqueue, newP,
- [&] (TrialP trialPArg) { this->calcDist2Wall(trialPArg);} );
- }
- }
-
- while (!trialqueue.empty()) {
- long int keyOfSmallest = trialqueue.top().key;
- flag[keyOfSmallest] = FM_FINAL;
- trialqueue.pop();
- if ((thresholdArg > 0) && (_dist2Wall[keyOfSmallest] > thresholdArg)) { //set rest of nearfield and rest of unknown to this max value:
-
- //rest of nearfield
- while (!trialqueue.empty()) {
- long int currKey = trialqueue.top().key;
- _dist2Wall[currKey] = _dist2Wall[keyOfSmallest];
- flag[currKey] = FM_FINAL;
- _dirToWall[currKey]._x = (0.);
- _dirToWall[currKey]._y = (0.);
- trialqueue.pop();
- }
-
- //rest of unknown
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- if (flag[i] == FM_UNKNOWN) {
- flag[i] = FM_FINAL;
- _dist2Wall[i] = _dist2Wall[keyOfSmallest];
- _dirToWall[i]._x = (0.);
- _dirToWall[i]._y = (0.);
- }
- }
- } else {
- //Log->Write(std::to_string(debugcounter++) + " " + std::to_string(grid->GetnPoints()));
- TrialP smallestP = TrialP(keyOfSmallest, _dist2Wall, _speedInitial, flag, _dirToWall);
- checkNeighborsAndAddToNarrowband(trialqueue, smallestP,
- [&] (TrialP trialPArg) { this->calcDist2Wall(trialPArg);} );
- }
- }
- delete[] flag;
-} //calculateDistancField
-
-void FloorfieldViaFM::checkNeighborsAndAddToNarrowband(std::priority_queue<TrialP, std::vector<TrialP>, std::greater<TrialP>>& trialqueue, TrialP keyP,
- std::function<void (TrialP)> calc) {
- int* flag = keyP.flag;
-
- directNeighbor dNeigh = _grid->getNeighbors(keyP.key);
-
- //check for valid neigh
- long int aux = dNeigh.key[0];
- //hint: trialfield[i].cost = dist2Wall + i; <<< set in prepareForDistanceFieldCalc
- if ((aux != -2) && (flag[aux] == FM_UNKNOWN)) {
- flag[aux] = FM_ADDED; // added to trial but not calculated
- TrialP currNeighP = TrialP(aux, keyP.cost, keyP.speed, flag, keyP.neggrad);
- calc(currNeighP);
- trialqueue.emplace(currNeighP);
- }
- aux = dNeigh.key[1];
- if ((aux != -2) && (flag[aux] == FM_UNKNOWN)) {
- flag[aux] = FM_ADDED; // added to trial but not calculated
- TrialP currNeighP = TrialP(aux, keyP.cost, keyP.speed, flag, keyP.neggrad);
- calc(currNeighP);
- trialqueue.emplace(currNeighP);
- }
- aux = dNeigh.key[2];
- if ((aux != -2) && (flag[aux] == FM_UNKNOWN)) {
- flag[aux] = FM_ADDED; // added to trial but not calculated
- TrialP currNeighP = TrialP(aux, keyP.cost, keyP.speed, flag, keyP.neggrad);
- calc(currNeighP);
- trialqueue.emplace(currNeighP);
- }
- aux = dNeigh.key[3];
- if ((aux != -2) && (flag[aux] == FM_UNKNOWN)) {
- flag[aux] = FM_ADDED; // added to trial but not calculated
- TrialP currNeighP = TrialP(aux, keyP.cost, keyP.speed, flag, keyP.neggrad);
- calc(currNeighP);
- trialqueue.emplace(currNeighP);
- }
-}
-
-void FloorfieldViaFM::calcDist2Wall(TrialP newPoint) {
- double row;
- double col;
- long int aux;
- bool pointsUp;
- bool pointsRight;
-
- int* flag = newPoint.flag;
- double* cost = newPoint.cost;
- long int key = newPoint.key;
-
- row = 100000.;
- col = 100000.;
- //aux = -1;
-
- directNeighbor dNeigh = _grid->getNeighbors(key);
-
- aux = dNeigh.key[0];
- //hint: trialfield[i].cost = dist2Wall + i; <<< set in resetGoalAndCosts
- if ((aux != -2) && //neighbor is a gridpoint
- (flag[aux] != FM_UNKNOWN) && //gridpoint holds a calculated value
- (_gcode[aux] != OUTSIDE)) //gridpoint holds a calculated value
- {
- row = cost[aux];
- pointsRight = true;
- if (row < 0) {
- std::cerr << "hier ist was schief " << row << " " << aux << " " << flag[aux] << std::endl;
- row = 100000;
- }
- }
- aux = dNeigh.key[2];
- if ((aux != -2) && //neighbor is a gridpoint
- (flag[aux] != FM_UNKNOWN) && //gridpoint holds a calculated value
- (_gcode[aux] != OUTSIDE) &&
- (cost[aux] < row)) //calculated value promises smaller cost
- {
- row = cost[aux];
- pointsRight = false;
- }
-
- aux = dNeigh.key[1];
- //hint: trialfield[i].cost = dist2Wall + i; <<< set in parseBuilding after linescan call
- if ((aux != -2) && //neighbor is a gridpoint
- (flag[aux] != FM_UNKNOWN) && //gridpoint holds a calculated value
- (_gcode[aux] != OUTSIDE))
- {
- col = cost[aux];
- pointsUp = true;
- if (col < 0) {
- std::cerr << "hier ist was schief " << col << " " << aux << " " << flag[aux] << std::endl;
- col = 100000;
- }
- }
- aux = dNeigh.key[3];
- if ((aux != -2) && //neighbor is a gridpoint
- (flag[aux] != FM_UNKNOWN) && //gridpoint holds a calculated value
- (_gcode[aux] != OUTSIDE) &&
- (cost[aux] < col)) //calculated value promises smaller cost
- {
- col = cost[aux];
- pointsUp = false;
- }
- if (col == 100000.) { //one sided update with row
- cost[key] = onesidedCalc(row, _grid->Gethx());
- flag[key] = FM_SINGLE;
- if (pointsRight) {
- _dirToWall[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
- _dirToWall[key]._y = (0.);
- } else {
- _dirToWall[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
- _dirToWall[key]._y = (0.);
- }
- _dirToWall[key] = _dirToWall[key].Normalized(); //@todo: ar.graf: what yields better performance? scale every point here or scale each read value? more points or more calls to any element of dir2Wall
- return;
- }
-
- if (row == 100000.) { //one sided update with col
- cost[key] = onesidedCalc(col, _grid->Gethy());
- flag[key] = FM_SINGLE;
- if (pointsUp) {
- _dirToWall[key]._x = (0.);
- _dirToWall[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
- } else {
- _dirToWall[key]._x = (0.);
- _dirToWall[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
- }
- _dirToWall[key] = _dirToWall[key].Normalized();
- return;
- }
-
- //two sided update
- double precheck = twosidedCalc(row, col, _grid->Gethx());
- if (precheck >= 0) {
- cost[key] = precheck;
- flag[key] = FM_DOUBLE;
- if (pointsUp && pointsRight) {
- _dirToWall[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
- _dirToWall[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
- }
- if (pointsUp && !pointsRight) {
- _dirToWall[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
- _dirToWall[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
- }
- if (!pointsUp && pointsRight) {
- _dirToWall[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
- _dirToWall[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
- }
- if (!pointsUp && !pointsRight) {
- _dirToWall[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
- _dirToWall[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
- }
- } else {
- std::cerr << "else in twosided Dist " << std::endl;
- }
- _dirToWall[key] = _dirToWall[key].Normalized();
-}
-
-void FloorfieldViaFM::calcFloorfield(TrialP newPoint) {
- double row;
- double col;
- long int aux;
- bool pointsUp = false;
- bool pointsRight = false;
-
- int* flag = newPoint.flag;
- double* cost = newPoint.cost;
- long int key = newPoint.key;
- double* speed = newPoint.speed;
- Point* neggrad = newPoint.neggrad;
-
- row = DBL_MAX;
- col = DBL_MAX;
- //aux = -1;
-
- directNeighbor dNeigh = _grid->getNeighbors(key);
-
- aux = dNeigh.key[0];
- //hint: trialfield[i].cost = costarray + i; <<< set in calculateFloorfield
- if ((aux != -2) && //neighbor is a gridpoint
- (flag[aux] != FM_UNKNOWN) &&
- //((gcode[aux] == INSIDE) || (gcode[aux] == OPEN_CROSSING) || (gcode[aux] == OPEN_TRANSITION)) ) //gridpoint holds a calculated value
- (flag[aux] != FM_OUTSIDE))
- {
- row = cost[aux];
- pointsRight = true;
- }
- aux = dNeigh.key[2];
- if ((aux != -2) && //neighbor is a gridpoint
- (flag[aux] != FM_UNKNOWN) &&
- (flag[aux] != FM_OUTSIDE) && //gridpoint holds a calculated value
- (cost[aux] < row)) //calculated value promises smaller cost
- {
- row = cost[aux];
- pointsRight = false;
- }
-
- aux = dNeigh.key[1];
- //hint: trialfield[i].cost = cost + i; <<< set in calculateFloorfield
- if ((aux != -2) && //neighbor is a gridpoint
- (flag[aux] != FM_UNKNOWN) &&
- (flag[aux] != FM_OUTSIDE)) //gridpoint holds a calculated value
- {
- col = cost[aux];
- pointsUp = true;
- }
- aux = dNeigh.key[3];
- if ((aux != -2) && //neighbor is a gridpoint
- (flag[aux] != FM_UNKNOWN) &&
- (flag[aux] != FM_OUTSIDE) && //gridpoint holds a calculated value
- (cost[aux] < col)) //calculated value promises smaller cost
- {
- col = cost[aux];
- pointsUp = false;
- }
- if ((col == DBL_MAX) && (row == DBL_MAX)) {
- std::cerr << "Issue 175 in FloorfieldViaFM: invalid combination of row,col (both on max)" <<std::endl;
- return;
- }
- if (col == DBL_MAX) { //one sided update with row
- cost[key] = onesidedCalc(row, _grid->Gethx()/speed[key]);
- flag[key] = FM_SINGLE;
- if (pointsRight && (dNeigh.key[0] != -2)) {
- neggrad[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
- neggrad[key]._y = (0.);
- } else if (dNeigh.key[2] != -2) {
- neggrad[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
- neggrad[key]._y = (0.);
- }
- return;
- }
-
- if (row == DBL_MAX) { //one sided update with col
- cost[key] = onesidedCalc(col, _grid->Gethy()/speed[key]);
- flag[key] = FM_SINGLE;
- if ((pointsUp) && (dNeigh.key[1] != -2)) {
- neggrad[key]._x = (0.);
- neggrad[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
- } else if (dNeigh.key[3] != -2){
- neggrad[key]._x = (0.);
- neggrad[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
- }
- return;
- }
-
- //two sided update
- double precheck = twosidedCalc(row, col, _grid->Gethx()/speed[key]);
-// if (precheck > 10000) {
-// Log->Write("ERROR \t\t\t is in twosided");
-// }
- if (precheck >= 0) {
- cost[key] = precheck;
- flag[key] = FM_DOUBLE;
- if (pointsUp && pointsRight) {
- neggrad[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
- neggrad[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
- }
- if (pointsUp && !pointsRight) {
- neggrad[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
- neggrad[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
- }
- if (!pointsUp && pointsRight) {
- neggrad[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
- neggrad[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
- }
- if (!pointsUp && !pointsRight) {
- neggrad[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
- neggrad[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
- }
- } else {
- std::cerr << "else in twosided Floor " << precheck << " " << row << " " << col << std::endl;
- }
-}
-
-inline double FloorfieldViaFM::onesidedCalc(double xy, double hDivF) {
- //if ( (xy+hDivF) > 10000) std::cerr << "error in onesided " << xy << std::endl;
- return xy + hDivF;
-}
-
-inline double FloorfieldViaFM::twosidedCalc(double x, double y, double hDivF) { //on error return -2
- double determinante = (2*hDivF*hDivF - (x-y)*(x-y));
- if (determinante >= 0) {
- return (x + y + sqrt(determinante))/2;
- } else {
- return (x < y) ? (x + hDivF) : (y + hDivF);
- }
- std::cerr << "error in two-sided 2!!!!!!!!!!!!!!!!!!!!!!! o_O??" << std::endl;
- return -2.; //this line should never execute
-} //twosidedCalc
-
-void FloorfieldViaFM::testoutput(const char* filename1, const char* filename2, const double* target) {
-//security debug check
- std::ofstream file;
- std::ofstream file2;
- int numX = (int) ((_grid->GetxMax()-_grid->GetxMin())/_grid->Gethx());
- int numY = (int) ((_grid->GetyMax()-_grid->GetyMin())/_grid->Gethy());
- int numTotal = numX * numY;
- //std::cerr << numTotal << " numTotal" << std::endl;
- //std::cerr << grid->GetnPoints() << " grid" << std::endl;
- file.open(filename1);
- file2.open(filename2);
- file << "# vtk DataFile Version 3.0" << std::endl;
- file << "Testdata: Fast Marching: Test: " << std::endl;
- file << "ASCII" << std::endl;
- file << "DATASET STRUCTURED_POINTS" << std::endl;
- file << "DIMENSIONS " <<
- std::to_string(_grid->GetiMax()) <<
- " " <<
- std::to_string(_grid->GetjMax()) <<
- " 1" << std::endl;
- file << "ORIGIN " << _grid->GetxMin() << " " << _grid->GetyMin() << " 0" << std::endl;
- file << "SPACING " << std::to_string(_grid->Gethx()) << " " << std::to_string(_grid->Gethy()) << " 1" << std::endl;
- file << "POINT_DATA " << std::to_string(numTotal) << std::endl;
- file << "SCALARS Cost float 1" << std::endl;
- file << "LOOKUP_TABLE default" << std::endl;
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- file << target[i] << std::endl;
- Point iPoint = _grid->getPointFromKey(i);
- file2 << iPoint._x /*- grid->GetxMin()*/ << " " << iPoint._y /*- grid->GetyMin()*/ << " " << target[i] << std::endl;
- }
-
- if (target == _cost) {
- file << "VECTORS Gradient float" << std::endl;
- for (int i = 0; i < _grid->GetnPoints(); ++i) {
- file << _neggrad[i]._x << " " << _neggrad[i]._y << " 0.0" << std::endl;
- }
- }
-
- file.close();
- file2.close();
-
- //std::cerr << "INFO: \tFile closed: " << filename1 << std::endl;
-}
-
-void FloorfieldViaFM::writeFF(const std::string& filename, std::vector<int> targetID) {
- Log->Write("INFO: \tWrite Floorfield to file");
- Log->Write(filename);
- std::ofstream file;
-
- Log->Write("FloorfieldViaFM::writeFF(): writing to file %s: There are %d targets.", filename.c_str(), targetID.size());
-
- int numX = (int) ((_grid->GetxMax()-_grid->GetxMin())/_grid->Gethx());
- int numY = (int) ((_grid->GetyMax()-_grid->GetyMin())/_grid->Gethy());
- int numTotal = numX * numY;
- //std::cerr << numTotal << " numTotal" << std::endl;
- //std::cerr << grid->GetnPoints() << " grid" << std::endl;
- file.open(filename);
-
- file << "# vtk DataFile Version 3.0" << std::endl;
- file << "Testdata: Fast Marching: Test: " << std::endl;
- file << "ASCII" << std::endl;
- file << "DATASET STRUCTURED_POINTS" << std::endl;
- file << "DIMENSIONS " <<
- std::to_string(_grid->GetiMax()) <<
- " " <<
- std::to_string(_grid->GetjMax()) <<
- " 1" << std::endl;
- file << "ORIGIN " << _grid->GetxMin() << " " << _grid->GetyMin() << " 0" << std::endl;
- file << "SPACING " << std::to_string(_grid->Gethx()) << " " << std::to_string(_grid->Gethy()) << " 1" << std::endl;
- file << "POINT_DATA " << std::to_string(numTotal) << std::endl;
- file << "SCALARS Dist2Wall float 1" << std::endl;
- file << "LOOKUP_TABLE default" << std::endl;
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- file << _dist2Wall[i] << std::endl; //@todo: change target to all dist2wall
- //Point iPoint = grid->getPointFromKey(i);
- //file2 << iPoint._x /*- grid->GetxMin()*/ << " " << iPoint._y /*- grid->GetyMin()*/ << " " << target[i] << std::endl;
- }
-
- file << "VECTORS Dir2Wall float" << std::endl;
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- file << _dirToWall[i]._x << " " << _dirToWall[i]._y << " 0.0" << std::endl;
- }
-
- file << "SCALARS SubroomPtr float 1" << std::endl;
- file << "LOOKUP_TABLE default" << std::endl;
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- if (_subrooms[i]) {
- file << _subrooms[i]->GetUID() << std::endl;
- } else {
- file << 0.0 << std::endl;
- }
- }
-
- for (unsigned int iTarget = 0; iTarget < targetID.size(); ++iTarget) {
- Log->Write("%s: target number %d: UID %d", filename.c_str(), iTarget, targetID[iTarget]);
- if (_neggradmap.count(targetID[iTarget]) == 0) {
- continue;
- }
-
- Point *gradarray = _neggradmap[targetID[iTarget]];
- if (gradarray == nullptr) {
- continue;
- }
-
- std::string name = _building->GetTransOrCrossByUID(targetID[iTarget])->GetCaption() + "-" + std::to_string(targetID[iTarget]);
- std::replace(name.begin(), name.end(), ' ', '_');
- file << "VECTORS GradientTarget" << name << " float" << std::endl;
- for (int i = 0; i < _grid->GetnPoints(); ++i) {
- file << gradarray[i]._x << " " << gradarray[i]._y << " 0.0" << std::endl;
- }
-
- double *costarray = _costmap[targetID[iTarget]];
- file << "SCALARS CostTarget" << name << " float 1" << std::endl;
- file << "LOOKUP_TABLE default" << std::endl;
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- file << costarray[i] << std::endl;
- }
- }
- file << "SCALARS GCode float 1" << std::endl;
- file << "LOOKUP_TABLE default" << std::endl;
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- file << _gcode[i] << std::endl;
- }
- file.close();
-}
-
-void FloorfieldViaFM::writeGoalFF(const std::string& filename, std::vector<int> targetID) {
- Log->Write("INFO: \tWrite Floorfield to file");
- Log->Write(filename);
- std::ofstream file;
-
- int numX = (int) ((_grid->GetxMax()-_grid->GetxMin())/_grid->Gethx());
- int numY = (int) ((_grid->GetyMax()-_grid->GetyMin())/_grid->Gethy());
- int numTotal = numX * numY;
- //std::cerr << numTotal << " numTotal" << std::endl;
- //std::cerr << grid->GetnPoints() << " grid" << std::endl;
- file.open(filename);
-
- file << "# vtk DataFile Version 3.0" << std::endl;
- file << "Testdata: Fast Marching: Test: " << std::endl;
- file << "ASCII" << std::endl;
- file << "DATASET STRUCTURED_POINTS" << std::endl;
- file << "DIMENSIONS " <<
- std::to_string(_grid->GetiMax()) <<
- " " <<
- std::to_string(_grid->GetjMax()) <<
- " 1" << std::endl;
- file << "ORIGIN " << _grid->GetxMin() << " " << _grid->GetyMin() << " 0" << std::endl;
- file << "SPACING " << std::to_string(_grid->Gethx()) << " " << std::to_string(_grid->Gethy()) << " 1" << std::endl;
- file << "POINT_DATA " << std::to_string(numTotal) << std::endl;
- //file << "SCALARS Dist2Wall float 1" << std::endl;
- //file << "LOOKUP_TABLE default" << std::endl;
- //for (long int i = 0; i < grid->GetnPoints(); ++i) {
- // file << dist2Wall[i] << std::endl; //@todo: change target to all dist2wall
- //Point iPoint = grid->getPointFromKey(i);
- //file2 << iPoint._x /*- grid->GetxMin()*/ << " " << iPoint._y /*- grid->GetyMin()*/ << " " << target[i] << std::endl;
- //}
-
- //file << "VECTORS Dir2Wall float" << std::endl;
- //for (long int i = 0; i < grid->GetnPoints(); ++i) {
- // file << dirToWall[i]._x << " " << dirToWall[i]._y << " 0.0" << std::endl;
- //}
-
- for (unsigned int iTarget = 0; iTarget < targetID.size(); ++iTarget) {
- if (_goalneggradmap.count(targetID[iTarget]) == 0) {
- continue;
- }
-
- Point *gradarray = _goalneggradmap[targetID[iTarget]];
- if (gradarray == nullptr) {
- continue;
- }
-
- file << "VECTORS GradientTarget" << targetID[iTarget] << " float" << std::endl;
- for (int i = 0; i < _grid->GetnPoints(); ++i) {
- file << gradarray[i]._x << " " << gradarray[i]._y << " 0.0" << std::endl;
- }
-
- double *costarray = _goalcostmap[targetID[iTarget]];
- file << "SCALARS CostTarget" << targetID[iTarget] << " float 1" << std::endl;
- file << "LOOKUP_TABLE default" << std::endl;
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- file << costarray[i] << std::endl;
- }
- }
- file << "SCALARS GCode float 1" << std::endl;
- file << "LOOKUP_TABLE default" << std::endl;
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- file << _gcode[i] << std::endl;
- }
- file.close();
-}
-
-SubRoom* FloorfieldViaFM::isInside(const long int key) {
-// Point probe = _grid->getPointFromKey(key);
-
-// const std::map<int, std::shared_ptr<Room>>& roomMap = _building->GetAllRooms();
-//
-// for (auto& roomPair : roomMap) {
-//
-// Room* roomPtr = roomPair.second.get();
-// const std::map<int, std::shared_ptr<SubRoom>>& subRoomMap = roomPtr->GetAllSubRooms();
-//
-// for (auto& subRoomPair : subRoomMap) {
-//
-// SubRoom* subRoomPtr = subRoomPair.second.get();
-//
-// if (subRoomPtr->IsInSubRoom(probe)) {
-// return subRoomPtr;
-// }
-// }
-
- //FloorfieldViaFM does not support isInside because of unresolved problem of ambiguous projection of xy plane in multi-
- //storage buildings
-
- return nullptr;
-}
-
-void CentrePointFFViaFM::getDirectionToUID(int destID, const long int key, Point& direction, int mode) {
- //what if goal == -1, meaning closest exit... is GetExitIndex then -1? NO... ExitIndex is UID, given by router
- //if (ped->GetFinalDestination() == -1) /*go to closest exit*/ destID != -1;
-
- if ((key < 0) || (key >= _grid->GetnPoints())) { // @todo: ar.graf: this check in a #ifdef-block?
- Log->Write("ERROR: \t Floorfield tried to access a key out of grid!");
- direction._x = 0.;
- direction._y = 0.;
- return;
- }
- Point* localneggradptr = nullptr;
- double* localcostptr = nullptr;
- {
- if (_neggradmap.count(destID) == 0) {
- //Log->Write("FF for destID %d does not exist (key is %d)", destID, key);
- //check, if distID is in this grid
- Hline* destLine = _building->GetTransOrCrossByUID(destID);
- Point A = destLine->GetCentre();
- //Point B = destLine->GetPoint2();
- if (!(_grid->includesPoint(A))) {
- Log->Write("ERROR: \t Destination ID %d is not in grid!", destID);
- direction._x = direction._y = 0.;
- //return;
- }
- }
- localneggradptr = (_neggradmap.count(destID) == 0) ? nullptr : _neggradmap.at(destID);
- localcostptr = (_costmap.count(destID) == 0) ? nullptr : _costmap.at(destID);
- if (localneggradptr == nullptr) {
- bool isBeingCalculated;
-#pragma omp critical(floorfieldsBeingCalculated)
- {
- if (!(isBeingCalculated = _floorfieldsBeingCalculated.count(destID) > 0)) {
- _floorfieldsBeingCalculated.insert(destID);
- }
- }
- if (isBeingCalculated) {
- // we do not want to wait until the other calculation has finished, so we return immediately
- // the values are corrected in getDirectionToDestination(), and getCostToDestination doesn't care about the direction
- direction._x = DBL_MAX;
- direction._y = DBL_MAX;
- return;
- }
-
- //create floorfield (remove mapentry with nullptr, allocate memory, add mapentry, create ff)
- localcostptr = new double[_grid->GetnPoints()];
- localneggradptr = new Point[_grid->GetnPoints()];
-#pragma omp critical(neggradmap)
- _neggradmap.erase(destID);
-#pragma omp critical(neggradmap)
- _neggradmap.emplace(destID, localneggradptr);
-#pragma omp critical(costmap)
- _costmap.erase(destID);
-#pragma omp critical(costmap)
- _costmap.emplace(destID, localcostptr);
-
-
- //create ff (prepare Trial-mechanic, then calc)
-// for (long int i = 0; i < grid->GetnPoints(); ++i) {
-// //set Trialptr to fieldelements
-// trialfield[i].cost = localcostptr + i;
-// trialfield[i].neggrad = localneggradptr + i;
-// trialfield[i].father = nullptr;
-// trialfield[i].child = nullptr;
-// }
-// clearAndPrepareForFloorfieldReCalc(localcostptr);
- //std::vector<Line> localline = {Line((Line) *(building->GetTransOrCrossByUID(destID)))};
- Point centre = _building->GetTransOrCrossByUID(destID)->GetCentre();
- std::vector<Line> localline = {Line(centre, centre, 0)}; // only one point
-// setNewGoalAfterTheClear(localcostptr, localline);
- //Log->Write("Starting FF for UID %d (ID %d)", destID, dynamic_cast<Crossing*>(building->GetTransOrCrossByUID(destID))->GetID());
- //std::cerr << "\rW\tO\tR\tK\tI\tN\tG";
- if (mode == quickest) {
- calculateFloorfield(localline, localcostptr, localneggradptr, _densityspeed);
- } else {
- calculateFloorfield(localline, localcostptr, localneggradptr, _modifiedspeed);
- }
- //when using CentralPoint, the rest of destID Line would not be calculated. We set that line to zero in the lines below.
- //this way, the ffrouter understands, if a pedestrian reached the line, but is next to central point.
- Point a = _building->GetTransOrCrossByUID(destID)->GetPoint1();
- Point b = _building->GetTransOrCrossByUID(destID)->GetPoint2();
- localline.emplace_back(Line(a, b, 0));
- drawLinesOnGrid<double>(localline, localcostptr, 0.);
-#pragma omp critical(floorfieldsBeingCalculated)
- {
- if (_floorfieldsBeingCalculated.count(destID) != 1) {
- Log->Write("ERROR: FloorfieldViaFM::getDirectionToUID: key %d was calculating FF for destID %d, but it was removed from floorfieldsBeingCalculated meanwhile", key, destID);
- }
- _floorfieldsBeingCalculated.erase(destID);
- }
- //Log->Write("Ending FF for UID %d", destID);
- //std::cerr << "\r W\t O\t R\t K\t I\t N\t G";
- }
- }
- direction._x = (localneggradptr[key]._x);
- direction._y = (localneggradptr[key]._y);
-}
-
-SubRoom* FloorfieldViaFM::GetSubroom(Pedestrian* ped) {
- long int key = _grid->getKeyAtPoint(ped->GetPos());
- assert(key >= 0 && key <= _grid->GetnPoints());
- return _subrooms[key];
-}
\ No newline at end of file
diff --git a/routing/ff_router_trips/FloorfieldViaFM.h b/routing/ff_router_trips/FloorfieldViaFM.h
deleted file mode 100644
index c61eea13696aad6e947d9075120224172fcf2516..0000000000000000000000000000000000000000
--- a/routing/ff_router_trips/FloorfieldViaFM.h
+++ /dev/null
@@ -1,226 +0,0 @@
-/**
- * \file FloorfieldViaFM.h
- * \date Mar 05, 2015
- * \version N/A (v0.6)
- * \copyright <2009-2014> Forschungszentrum Jülich GmbH. All rights reserved.
- *
- * \section License
- * This file is part of JuPedSim.
- *
- * JuPedSim is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Lesser General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * any later version.
- *
- * JuPedSim is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public License
- * along with JuPedSim. If not, see <http://www.gnu.org/licenses/>.
- *
- * \section Description
- * Implementation of classes for ...
- *
- *
- **/
-
- //remark:
- //refac the code to use enums instead of integer values where integer values code sth
- //was considered, but enum classes do not implicitly cast to int
- //rather use makros/masks like in plain C? or just makros (defines)?
- //this would make it easier to read
-
-#ifndef FLOORFIELDVIAFM_H
-#define FLOORFIELDVIAFM_H
-
-#include <vector>
-#include <unordered_set>
-#include <cmath>
-#include <functional>
-#include "mesh/RectGrid.h"
-#include "../../geometry/Wall.h"
-#include "../../geometry/Point.h"
-#include "../../geometry/Building.h"
-#include "../../geometry/SubRoom.h" //check: should Room.h include SubRoom.h??
-#include "./mesh/Trial.h"
-#include "../../pedestrian/Pedestrian.h"
-
-//maybe put following in macros.h
-#define LOWSPEED 0.001
-
-class TrialP
-{
-public:
- long int key;
- int* flag;
- double* cost;
- double* speed;
- Point* neggrad;
-
- TrialP() {
- key = 0;
- flag = nullptr;
- cost = nullptr;
- speed = nullptr;
- neggrad = nullptr;
- }
-
- TrialP(long int keyArg, double* t, double* f, int* flagArg, Point* neggradArg) {
- key = keyArg;
- cost = t;
- speed = f;
- flag = flagArg;
- neggrad = neggradArg;
- }
-
- ~TrialP(){}
-
- bool operator <(const TrialP& rhs) const
- {
- return this->cost[this->key] < rhs.cost[rhs.key];
- }
-
- bool operator >(const TrialP& rhs) const
- {
- return this->cost[this->key] > rhs.cost[rhs.key];
- }
-
- bool operator ==(const TrialP& rhs) const
- {
- return this->cost[this->key] == rhs.cost[rhs.key];
- }
-};
-
-class FloorfieldViaFM
-{
-public:
- FloorfieldViaFM();
- FloorfieldViaFM(const std::string&);
- FloorfieldViaFM(const Building* const buildingArg, const double hxArg, const double hyArg,
- const double wallAvoidDistance, const bool useDistancefield, const bool onlyRoomsWithExits);
- //FloorfieldViaFM(const FloorfieldViaFM* const refFM);
- virtual ~FloorfieldViaFM();
- FloorfieldViaFM(const FloorfieldViaFM& other); //will not make a copy; only takes geometry info
- //FloorfieldViaFM& operator=(const FloorfieldViaFM& other);
-
- //void getDirectionAt(const Point& position, Point& direction); //obsolete
- //void getDirectionToDestination (const int destID, const Point& position, Point& direction); //obsolete
- void getDirectionToUID(int destID, const long int key, Point& direction); // shall not be used any more, therefore not virtual
- virtual void getDirectionToUID(int destID, const long int key, Point& direction, int mode);
- //void getDirectionToUIDParallel(int destID, const long int key, Point& direction);
- virtual void getDirectionToDestination (Pedestrian* ped, Point& direction);
- //void getDirectionToFinalDestination(Pedestrian* ped, Point& direction); //this is router buissness! problem in multi-storage buildings
-
- void createMapEntryInLineToGoalID(const int goalID);
-
- double getCostToDestination(const int destID, const Point& position);
- double getCostToDestination(const int destID, const Point& position, int mode);
- //double getCostToDestinationParallel(const int destID, const Point& position);
-
- void getDir2WallAt(const Point& position, Point& direction);
- double getDistance2WallAt(const Point& position);
-
- int getSubroomUIDAt(const Point& position);
-
- void parseBuilding(const Building* const buildingArg, const double stepSizeX, const double stepSizeY);
- void parseBuildingForExits(const Building* const buildingArg, const double stepSizeX, const double stepSizeY);
- void prepareForDistanceFieldCalculation(const bool withExits);
- template <typename T>
- void drawLinesOnGrid(std::vector<Line>& wallArg, T* const target, const T value);
- void setSpeed(bool useDistance2WallArg);
- void setSpeedThruPeds(Pedestrian* const* pedsArg, int nPeds, int modechoice, double radius);
- void deleteAllFFs();
- void clearAndPrepareForFloorfieldReCalc(double* costarray);
- void setNewGoalAfterTheClear(double* costarray, std::vector<Line>& GoalWallArg);
- void calculateFloorfield(std::vector<Line>& wallArg, double* costarray, Point* neggradarray); //make private
- void calculateFloorfield(std::vector<Line>& wallArg, double* costarray, Point* neggradarray, double* speedarray);
- void calculateDistanceField(const double thresholdArg); //make private
-
- void checkNeighborsAndAddToNarrowband(std::priority_queue<TrialP, std::vector<TrialP>, std::greater<TrialP> >& trialfield, TrialP key,
- std::function<void (TrialP)> calc);
-
- void calcDist2Wall(TrialP);
- void calcFloorfield(TrialP);
- //void (*checkNeighborsAndCalc)(const long int key);
-
- inline double onesidedCalc(double xy, double hDivF);
- inline double twosidedCalc(double x, double y, double hDivF);
-
- void testoutput(const char*, const char*, const double*);
- void writeFF(const std::string&, std::vector<int> targetID);
- void writeGoalFF(const std::string&, std::vector<int> targetID);
-
- virtual SubRoom* isInside(const long int key);
- SubRoom* GetSubroom(Pedestrian* p);
-
- std::map<int, int> getGoalToLineUIDmap() const
- {
- return _goalToLineUIDmap;
- }
-
- std::map<int, int> getGoalToLineUIDmap2() const
- {
- return _goalToLineUIDmap2;
- }
-
- std::map<int, int> getGoalToLineUIDmap3() const
- {
- return _goalToLineUIDmap3;
- }
-
- RectGrid* getGrid() const
- {
- return _grid;
- }
-
-#ifdef TESTING
- void setGrid(RectGrid* gridArg) {_grid = gridArg;}
-#endif //TESTING
-
-protected:
- RectGrid* _grid = nullptr;
- std::vector<Line> _wall;
- std::vector<Line> _exitsFromScope;
- unsigned int _numOfExits;
-
- const Building* _building;
-
- //GridPoint Data in independant arrays (shared primary key)
- // changed to threadsafe creation when needed: int* flag;
- int* _gcode = nullptr; //gridcode (see Macros.h)
- SubRoom* * _subrooms = nullptr; // this is an array (first asterisk) of pointers (second asterisk)
- double* _dist2Wall = nullptr;
- double* _speedInitial = nullptr;
- double* _modifiedspeed = nullptr;
- double* _densityspeed = nullptr;
- double* _cost = nullptr;
- //long int* secKey; //secondary key to address ... not used yet
- Point* _neggrad = nullptr; //gradients
- Point* _dirToWall = nullptr;
- // changed to threadsafe creation when needed: Trial* trialfield;
-
- std::map<int, double*> _goalcostmap;
- std::map<int, int> _goalToLineUIDmap; //key is the goalID and value is the UID of closest transition -> it maps goal to LineUID
- std::map<int, int> _goalToLineUIDmap2;
- std::map<int, int> _goalToLineUIDmap3;
- std::map<int, Point*> _goalneggradmap;
- std::map<int, double*> _costmap;
- std::map<int, Point*> _neggradmap;
- // use an unordered_set for faster access (it is accessed within a critical region)
- std::unordered_set<int> _floorfieldsBeingCalculated;
- bool maps_deleted = false; // @todo f.mack remove
-
- double _threshold;
- bool _useDistanceToWall;
-};
-
-// very similar to FloorfieldViaFM, but the calculation of floorfields starts at the center of the door only, not on the whole line
-// this happens in order to avoid "door hopping" (for a pedestrian, it is shorter to go to a nearby door and skip half the door width)
-class CentrePointFFViaFM : public virtual FloorfieldViaFM {
-public:
- virtual void getDirectionToUID(int destID, const long int key, Point& direction, int mode);
-};
-
-#endif // FLOORFIELDVIAFM_H
diff --git a/routing/ff_router_trips/LocalFloorfieldViaFM.cpp b/routing/ff_router_trips/LocalFloorfieldViaFM.cpp
deleted file mode 100644
index 145c764e5a5a618068d644d5c8bcf5c6c547a2b2..0000000000000000000000000000000000000000
--- a/routing/ff_router_trips/LocalFloorfieldViaFM.cpp
+++ /dev/null
@@ -1,519 +0,0 @@
-//
-// Created by ar.graf on 1/28/16.
-//
-
-#include "LocalFloorfieldViaFM.h"
-
-LocalFloorfieldViaFM::LocalFloorfieldViaFM(){};
-LocalFloorfieldViaFM::LocalFloorfieldViaFM(const Room* const roomArg,
- const Building* buildingArg,
- const double hxArg, const double hyArg,
- const double wallAvoidDistance, const bool useDistancefield) {
- //ctor
- //_threshold = -1; //negative value means: ignore threshold
- _threshold = wallAvoidDistance;
- _building = buildingArg;
- _room = roomArg;
-
- if (hxArg != hyArg) std::cerr << "ERROR: hx != hy <=========";
- //parse building and create list of walls/obstacles (find xmin xmax, ymin, ymax, and add border?)
- //Log->Write("INFO: \tStart Parsing: Room %d", roomArg->GetID());
- parseRoom(roomArg, hxArg, hyArg);
- //Log->Write("INFO: \tFinished Parsing: Room %d", roomArg->GetID());
- //testoutput("AALineScan.vtk", "AALineScan.txt", dist2Wall);
-
- prepareForDistanceFieldCalculation(false);
- //here we need to draw blocker lines @todo: ar.graf
- //drawBlockerLines();
- //Log->Write("INFO: \tGrid initialized: Walls in room %d", roomArg->GetID());
-
- calculateDistanceField(-1.); //negative threshold is ignored, so all distances get calculated. this is important since distances is used for slowdown/redirect
- //Log->Write("INFO: \tGrid initialized: Walldistances in room %d", roomArg->GetID());
-
- setSpeed(useDistancefield); //use distance2Wall
- //Log->Write("INFO: \tGrid initialized: Speed in room %d", roomArg->GetID());
- calculateFloorfield(_exitsFromScope, _cost, _neggrad);
- //Log->Write("INFO: \tFloor field for \"goal -1\" done in room %d", roomArg->GetID());
-};
-
-//void LocalFloorfieldViaFM::getDirectionToGoalID(const int goalID){
-// std::cerr << "invalid call to LocalFloorfieldViaFM::getDirectionToGoalID!" << std::endl;
-//};
-
-void LocalFloorfieldViaFM::parseRoom(const Room* const roomArg,
- const double hxArg, const double hyArg)
-{
- _room = roomArg;
- //init min/max before parsing
- double xMin = DBL_MAX;
- double xMax = -DBL_MAX;
- double yMin = xMin;
- double yMax = xMax;
- _costmap.clear();
- _neggradmap.clear();
-
- //create a list of walls
- //add all transition and put open doors at the beginning of "wall"
- std::map<int, Transition*> allTransitions;
- for (auto& itSubroom : _room->GetAllSubRooms()) {
- for (auto itTrans : itSubroom.second->GetAllTransitions()) {
- if (!allTransitions.count(itTrans->GetUniqueID())) {
- allTransitions[itTrans->GetUniqueID()] = &(*itTrans);
- if (itTrans->IsOpen()) {
- _exitsFromScope.emplace_back(Line((Line) *itTrans));
- _costmap.emplace(itTrans->GetUniqueID(), nullptr);
- _neggradmap.emplace(itTrans->GetUniqueID(), nullptr);
- }
- }
- }
- }
- _numOfExits = (unsigned int) _exitsFromScope.size();
- //put closed doors next, they are considered as walls later (index >= numOfExits)
- for (auto& trans : allTransitions) {
- if (!trans.second->IsOpen()) {
- _wall.emplace_back(Line ( (Line) *(trans.second)));
- }
- }
-
- for (const auto& itSubroom : _room->GetAllSubRooms()) {
- std::vector<Obstacle*> allObstacles = itSubroom.second->GetAllObstacles();
- for (std::vector<Obstacle*>::iterator itObstacles = allObstacles.begin(); itObstacles != allObstacles.end(); ++itObstacles) {
-
- std::vector<Wall> allObsWalls = (*itObstacles)->GetAllWalls();
- for (std::vector<Wall>::iterator itObsWall = allObsWalls.begin(); itObsWall != allObsWalls.end(); ++itObsWall) {
- _wall.emplace_back(Line( (Line) *itObsWall));
- // xMin xMax
- if ((*itObsWall).GetPoint1()._x < xMin) xMin = (*itObsWall).GetPoint1()._x;
- if ((*itObsWall).GetPoint2()._x < xMin) xMin = (*itObsWall).GetPoint2()._x;
- if ((*itObsWall).GetPoint1()._x > xMax) xMax = (*itObsWall).GetPoint1()._x;
- if ((*itObsWall).GetPoint2()._x > xMax) xMax = (*itObsWall).GetPoint2()._x;
- // yMin yMax
- if ((*itObsWall).GetPoint1()._y < yMin) yMin = (*itObsWall).GetPoint1()._y;
- if ((*itObsWall).GetPoint2()._y < yMin) yMin = (*itObsWall).GetPoint2()._y;
- if ((*itObsWall).GetPoint1()._y > yMax) yMax = (*itObsWall).GetPoint1()._y;
- if ((*itObsWall).GetPoint2()._y > yMax) yMax = (*itObsWall).GetPoint2()._y;
- }
- }
-
- std::vector<Wall> allWalls = itSubroom.second->GetAllWalls();
- for (std::vector<Wall>::iterator itWall = allWalls.begin(); itWall != allWalls.end(); ++itWall) {
- _wall.emplace_back( Line( (Line) *itWall));
- // xMin xMax
- if ((*itWall).GetPoint1()._x < xMin) xMin = (*itWall).GetPoint1()._x;
- if ((*itWall).GetPoint2()._x < xMin) xMin = (*itWall).GetPoint2()._x;
- if ((*itWall).GetPoint1()._x > xMax) xMax = (*itWall).GetPoint1()._x;
- if ((*itWall).GetPoint2()._x > xMax) xMax = (*itWall).GetPoint2()._x;
- // yMin yMax
- if ((*itWall).GetPoint1()._y < yMin) yMin = (*itWall).GetPoint1()._y;
- if ((*itWall).GetPoint2()._y < yMin) yMin = (*itWall).GetPoint2()._y;
- if ((*itWall).GetPoint1()._y > yMax) yMax = (*itWall).GetPoint1()._y;
- if ((*itWall).GetPoint2()._y > yMax) yMax = (*itWall).GetPoint2()._y;
- }
- const vector<Crossing*>& allCrossings = itSubroom.second->GetAllCrossings();
- for (Crossing* crossPtr : allCrossings) {
- if (!crossPtr->IsOpen()) {
- _wall.emplace_back( Line( (Line) *crossPtr));
-
- if (crossPtr->GetPoint1()._x < xMin) xMin = crossPtr->GetPoint1()._x;
- if (crossPtr->GetPoint2()._x < xMin) xMin = crossPtr->GetPoint2()._x;
- if (crossPtr->GetPoint1()._x > xMax) xMax = crossPtr->GetPoint1()._x;
- if (crossPtr->GetPoint2()._x > xMax) xMax = crossPtr->GetPoint2()._x;
-
- if (crossPtr->GetPoint1()._y < yMin) yMin = crossPtr->GetPoint1()._y;
- if (crossPtr->GetPoint2()._y < yMin) yMin = crossPtr->GetPoint2()._y;
- if (crossPtr->GetPoint1()._y > yMax) yMax = crossPtr->GetPoint1()._y;
- if (crossPtr->GetPoint2()._y > yMax) yMax = crossPtr->GetPoint2()._y;
- }
- }
- }
-
- //create Rect Grid
- _grid = new RectGrid();
- _grid->setBoundaries(xMin, yMin, xMax, yMax);
- _grid->setSpacing(hxArg, hyArg);
- _grid->createGrid();
-
- //create arrays
- //flag = new int[grid->GetnPoints()];
- _gcode = new int[_grid->GetnPoints()];
- _subrooms = new SubRoom*[_grid->GetnPoints()](); // initializes with nullptr
- _dist2Wall = new double[_grid->GetnPoints()];
- _speedInitial = new double[_grid->GetnPoints()];
- _modifiedspeed = new double[_grid->GetnPoints()];
- _densityspeed = new double[_grid->GetnPoints()];
- _cost = new double[_grid->GetnPoints()];
- _neggrad = new Point[_grid->GetnPoints()];
- _dirToWall = new Point[_grid->GetnPoints()];
- //trialfield = new Trial[grid->GetnPoints()]; //created with other arrays, but not initialized yet
-
- _costmap.emplace(-1 , _cost); // enable default ff (closest exit)
- _neggradmap.emplace(-1, _neggrad);
-
- //init grid with -3 as unknown distance to any wall
- for(long int i = 0; i < _grid->GetnPoints(); ++i) {
- _dist2Wall[i] = -3.;
- _cost[i] = -2.;
- _gcode[i] = OUTSIDE; //unknown
- }
-
- drawLinesOnGrid<double>(_wall, _dist2Wall, 0.);
- drawLinesOnGrid<double>(_wall, _cost, -7.);
- drawLinesOnGrid<int>(_wall, _gcode, WALL);
- drawLinesOnGrid<int>(_exitsFromScope, _gcode, OPEN_TRANSITION);
-}
-
-void LocalFloorfieldViaFM::drawBlockerLines() {
- //std::vector<Line> exits(wall.begin(), wall.begin()+numOfExits);
-
- //grid handeling local vars:
- //long int iMax = grid->GetiMax();
-
- long int iStart, iEnd;
- long int jStart, jEnd;
- long int iDot, jDot;
- long int key;
- long int deltaX, deltaY, deltaX1, deltaY1, px, py, xe, ye, i; //Bresenham Algorithm
-
- for(auto& line : _wall) {
- key = _grid->getKeyAtPoint(line.GetPoint1());
- iStart = (long) _grid->get_i_fromKey(key);
- jStart = (long) _grid->get_j_fromKey(key);
-
- key = _grid->getKeyAtPoint(line.GetPoint2());
- iEnd = (long) _grid->get_i_fromKey(key);
- jEnd = (long) _grid->get_j_fromKey(key);
-
- deltaX = (int) (iEnd - iStart);
- deltaY = (int) (jEnd - jStart);
- deltaX1 = abs( (int) (iEnd - iStart));
- deltaY1 = abs( (int) (jEnd - jStart));
-
- px = 2*deltaY1 - deltaX1;
- py = 2*deltaX1 - deltaY1;
-
- if(deltaY1<=deltaX1) {
- if(deltaX>=0) {
- iDot = iStart;
- jDot = jStart;
- xe = iEnd;
- } else {
- iDot = iEnd;
- jDot = jEnd;
- xe = iStart;
- }
- //crossOutOutsideNeighbors(jDot*iMax + iDot);
- for (i=0; iDot < xe; ++i) {
- ++iDot;
- if(px<0) {
- px+=2*deltaY1;
- } else {
- if((deltaX<0 && deltaY<0) || (deltaX>0 && deltaY>0)) {
- ++jDot;
- } else {
- --jDot;
- }
- px+=2*(deltaY1-deltaX1);
- }
- //crossOutOutsideNeighbors(jDot*iMax + iDot);
- }
- } else {
- if(deltaY>=0) {
- iDot = iStart;
- jDot = jStart;
- ye = jEnd;
- } else {
- iDot = iEnd;
- jDot = jEnd;
- ye = jStart;
- }
- //crossOutOutsideNeighbors(jDot*iMax + iDot);
- for(i=0; jDot<ye; ++i) {
- ++jDot;
- if (py<=0) {
- py+=2*deltaX1;
- } else {
- if((deltaX<0 && deltaY<0) || (deltaX>0 && deltaY>0)) {
- ++iDot;
- } else {
- --iDot;
- }
- py+=2*(deltaX1-deltaY1);
- }
- // crossOutOutsideNeighbors(jDot*iMax + iDot);
- }
- }
- } //loop over all walls
-
- for(auto& line : _exitsFromScope) {
- key = _grid->getKeyAtPoint(line.GetPoint1());
- iStart = (long) _grid->get_i_fromKey(key);
- jStart = (long) _grid->get_j_fromKey(key);
-
- key = _grid->getKeyAtPoint(line.GetPoint2());
- iEnd = (long) _grid->get_i_fromKey(key);
- jEnd = (long) _grid->get_j_fromKey(key);
-
- deltaX = (int) (iEnd - iStart);
- deltaY = (int) (jEnd - jStart);
- deltaX1 = abs( (int) (iEnd - iStart));
- deltaY1 = abs( (int) (jEnd - jStart));
-
- px = 2*deltaY1 - deltaX1;
- py = 2*deltaX1 - deltaY1;
-
- if(deltaY1<=deltaX1) {
- if(deltaX>=0) {
- iDot = iStart;
- jDot = jStart;
- xe = iEnd;
- } else {
- iDot = iEnd;
- jDot = jEnd;
- xe = iStart;
- }
- //crossOutOutsideNeighbors(jDot*iMax + iDot);
- for (i=0; iDot < xe; ++i) {
- ++iDot;
- if(px<0) {
- px+=2*deltaY1;
- } else {
- if((deltaX<0 && deltaY<0) || (deltaX>0 && deltaY>0)) {
- ++jDot;
- } else {
- --jDot;
- }
- px+=2*(deltaY1-deltaX1);
- }
- //crossOutOutsideNeighbors(jDot*iMax + iDot);
- }
- } else {
- if(deltaY>=0) {
- iDot = iStart;
- jDot = jStart;
- ye = jEnd;
- } else {
- iDot = iEnd;
- jDot = jEnd;
- ye = jStart;
- }
- //crossOutOutsideNeighbors(jDot*iMax + iDot);
- for(i=0; jDot<ye; ++i) {
- ++jDot;
- if (py<=0) {
- py+=2*deltaX1;
- } else {
- if((deltaX<0 && deltaY<0) || (deltaX>0 && deltaY>0)) {
- ++iDot;
- } else {
- --iDot;
- }
- py+=2*(deltaX1-deltaY1);
- }
- // crossOutOutsideNeighbors(jDot*iMax + iDot);
- }
- }
- } //loop over all exits
-
-}
-
-SubRoom* LocalFloorfieldViaFM::isInside(const long int key) {
- Point probe = _grid->getPointFromKey(key);
- auto neighbors = _grid->getNeighbors(key);
-
- for (auto& neighbor : neighbors.key) {
- if (neighbor == -2) continue; // -2 is returned by getNeighbors() for invalid points
- SubRoom* subroom = _subrooms[neighbor];
- if (subroom && subroom->IsInSubRoom(probe)) return subroom;
- }
-
- // If we weren't successful so far, we have to search the whole list.
- const std::map<int, std::shared_ptr<SubRoom>>& subRoomMap = _room->GetAllSubRooms();
-
- for (auto& subRoomPair : subRoomMap) {
- SubRoom* subRoomPtr = subRoomPair.second.get();
- if (subRoomPtr->IsInSubRoom(probe)) {
- return subRoomPtr;
- }
- }
-
- return nullptr;
-}
-
-SubLocalFloorfieldViaFM::SubLocalFloorfieldViaFM(){};
-SubLocalFloorfieldViaFM::SubLocalFloorfieldViaFM(SubRoom* const roomArg,
- const Building* buildingArg,
- const double hxArg, const double hyArg,
- const double wallAvoidDistance, const bool useDistancefield) {
- //ctor
- //_threshold = -1; //negative value means: ignore threshold
- _threshold = wallAvoidDistance;
- _building = buildingArg;
- _subroom = roomArg;
-
- if (hxArg != hyArg) std::cerr << "ERROR: hx != hy <=========";
- //parse building and create list of walls/obstacles (find xmin xmax, ymin, ymax, and add border?)
- //Log->Write("INFO: \tStart Parsing: Room %d" , roomArg->GetUID());
- parseRoom(roomArg, hxArg, hyArg);
- //Log->Write("INFO: \tFinished Parsing: Room %d" , roomArg->GetUID());
- //testoutput("AALineScan.vtk", "AALineScan.txt", dist2Wall);
-
- prepareForDistanceFieldCalculation(false);
- //here we need to draw blocker lines @todo: ar.graf
- //Log->Write("INFO: \tGrid initialized: Walls");
-
- calculateDistanceField(-1.); //negative threshold is ignored, so all distances get calculated. this is important since distances is used for slowdown/redirect
- //Log->Write("INFO: \tGrid initialized: Walldistances");
-
- setSpeed(useDistancefield); //use distance2Wall
- //Log->Write("INFO: \tGrid initialized: Speed");
- calculateFloorfield(_exitsFromScope, _cost, _neggrad);
-};
-
-void SubLocalFloorfieldViaFM::getDirectionToDestination(Pedestrian* ped,
- Point& direction)
-{
- FloorfieldViaFM::getDirectionToDestination(ped, direction);
- return;
-}
-
-void SubLocalFloorfieldViaFM::getDirectionToGoalID(const int goalID){
- std::cerr << "invalid call to SubLocalFloorfieldViaFM::getDirectionToGoalID with goalID: " << goalID << std::endl;
-};
-
-
-void SubLocalFloorfieldViaFM::parseRoom(SubRoom* const roomArg,
- const double hxArg, const double hyArg)
-{
- _subroom = roomArg;
- //init min/max before parsing
- double xMin = DBL_MAX;
- double xMax = -DBL_MAX;
- double yMin = xMin;
- double yMax = xMax;
- _costmap.clear();
- _neggradmap.clear();
-
- //create a list of walls
- //add all transition and put open doors at the beginning of "wall"
- std::map<int, Transition*> allTransitions;
-
- for (auto itTrans : _subroom->GetAllTransitions()) {
- if (!allTransitions.count(itTrans->GetUniqueID())) {
- allTransitions[itTrans->GetUniqueID()] = &(*itTrans);
- if (itTrans->IsOpen()) {
- _exitsFromScope.emplace_back(Line((Line) *itTrans));
- _costmap.emplace(itTrans->GetUniqueID(), nullptr);
- _neggradmap.emplace(itTrans->GetUniqueID(), nullptr);
- }
- }
- }
-
- for (auto itCross : _subroom->GetAllCrossings()) {
- if (itCross->IsOpen()) {
- _exitsFromScope.emplace_back(Line((Line) *itCross));
- _costmap.emplace(itCross->GetUniqueID(), nullptr);
- _neggradmap.emplace(itCross->GetUniqueID(), nullptr);
- } else {
- _wall.emplace_back(Line( (Line) *itCross));
- }
- }
-
- _numOfExits = _exitsFromScope.size();
- //put closed doors next, they are considered as walls later (index >= numOfExits)
- for (auto& trans : allTransitions) {
- if (!trans.second->IsOpen()) {
- _wall.emplace_back(Line ( (Line) *(trans.second)));
- }
- }
-
-
- std::vector<Obstacle*> allObstacles = _subroom->GetAllObstacles();
- for (std::vector<Obstacle*>::iterator itObstacles = allObstacles.begin(); itObstacles != allObstacles.end(); ++itObstacles) {
-
- std::vector<Wall> allObsWalls = (*itObstacles)->GetAllWalls();
- for (std::vector<Wall>::iterator itObsWall = allObsWalls.begin(); itObsWall != allObsWalls.end(); ++itObsWall) {
- _wall.emplace_back(Line( (Line) *itObsWall));
- // xMin xMax
- if ((*itObsWall).GetPoint1()._x < xMin) xMin = (*itObsWall).GetPoint1()._x;
- if ((*itObsWall).GetPoint2()._x < xMin) xMin = (*itObsWall).GetPoint2()._x;
- if ((*itObsWall).GetPoint1()._x > xMax) xMax = (*itObsWall).GetPoint1()._x;
- if ((*itObsWall).GetPoint2()._x > xMax) xMax = (*itObsWall).GetPoint2()._x;
- // yMin yMax
- if ((*itObsWall).GetPoint1()._y < yMin) yMin = (*itObsWall).GetPoint1()._y;
- if ((*itObsWall).GetPoint2()._y < yMin) yMin = (*itObsWall).GetPoint2()._y;
- if ((*itObsWall).GetPoint1()._y > yMax) yMax = (*itObsWall).GetPoint1()._y;
- if ((*itObsWall).GetPoint2()._y > yMax) yMax = (*itObsWall).GetPoint2()._y;
- }
- }
-
- std::vector<Wall> allWalls = _subroom->GetAllWalls();
- for (std::vector<Wall>::iterator itWall = allWalls.begin(); itWall != allWalls.end(); ++itWall) {
- _wall.emplace_back( Line( (Line) *itWall));
- // xMin xMax
- if ((*itWall).GetPoint1()._x < xMin) xMin = (*itWall).GetPoint1()._x;
- if ((*itWall).GetPoint2()._x < xMin) xMin = (*itWall).GetPoint2()._x;
- if ((*itWall).GetPoint1()._x > xMax) xMax = (*itWall).GetPoint1()._x;
- if ((*itWall).GetPoint2()._x > xMax) xMax = (*itWall).GetPoint2()._x;
- // yMin yMax
- if ((*itWall).GetPoint1()._y < yMin) yMin = (*itWall).GetPoint1()._y;
- if ((*itWall).GetPoint2()._y < yMin) yMin = (*itWall).GetPoint2()._y;
- if ((*itWall).GetPoint1()._y > yMax) yMax = (*itWall).GetPoint1()._y;
- if ((*itWall).GetPoint2()._y > yMax) yMax = (*itWall).GetPoint2()._y;
- }
-
- const vector<Crossing*>& allCrossings = _subroom->GetAllCrossings();
- for (Crossing* crossPtr : allCrossings) {
- if (!crossPtr->IsOpen()) {
- _wall.emplace_back( Line( (Line) *crossPtr));
-
- if (crossPtr->GetPoint1()._x < xMin) xMin = crossPtr->GetPoint1()._x;
- if (crossPtr->GetPoint2()._x < xMin) xMin = crossPtr->GetPoint2()._x;
- if (crossPtr->GetPoint1()._x > xMax) xMax = crossPtr->GetPoint1()._x;
- if (crossPtr->GetPoint2()._x > xMax) xMax = crossPtr->GetPoint2()._x;
-
- if (crossPtr->GetPoint1()._y < yMin) yMin = crossPtr->GetPoint1()._y;
- if (crossPtr->GetPoint2()._y < yMin) yMin = crossPtr->GetPoint2()._y;
- if (crossPtr->GetPoint1()._y > yMax) yMax = crossPtr->GetPoint1()._y;
- if (crossPtr->GetPoint2()._y > yMax) yMax = crossPtr->GetPoint2()._y;
- }
- }
-
-
- //create Rect Grid
- _grid = new RectGrid();
- _grid->setBoundaries(xMin, yMin, xMax, yMax);
- _grid->setSpacing(hxArg, hyArg);
- _grid->createGrid();
-
- //create arrays
- _gcode = new int[_grid->GetnPoints()];
- _subrooms = new SubRoom*[_grid->GetnPoints()](); // initializes with nullptr
- _dist2Wall = new double[_grid->GetnPoints()];
- _speedInitial = new double[_grid->GetnPoints()];
- _modifiedspeed = new double[_grid->GetnPoints()];
- _densityspeed = new double[_grid->GetnPoints()];
- _cost = new double[_grid->GetnPoints()];
- _neggrad = new Point[_grid->GetnPoints()];
- _dirToWall = new Point[_grid->GetnPoints()];
- //trialfield = new Trial[grid->GetnPoints()]; //created with other arrays, but not initialized yet
-
- _costmap.emplace(-1 , _cost); // enable default ff (closest exit)
- _neggradmap.emplace(-1, _neggrad);
-
- //init grid with -3 as unknown distance to any wall
- for(long int i = 0; i < _grid->GetnPoints(); ++i) {
- _dist2Wall[i] = -3.;
- _cost[i] = -2.;
- _gcode[i] = OUTSIDE;
- }
-
- drawLinesOnGrid<double>(_wall, _dist2Wall, 0.);
- drawLinesOnGrid<double>(_wall, _cost, -7.);
- drawLinesOnGrid<int>(_wall, _gcode, WALL);
- drawLinesOnGrid<int>(_exitsFromScope, _gcode, OPEN_TRANSITION);
-}
-
-SubRoom* SubLocalFloorfieldViaFM::isInside(const long int key) {
- Point probe = _grid->getPointFromKey(key);
- return _subroom->IsInSubRoom(probe) ? _subroom : nullptr;
-}
\ No newline at end of file
diff --git a/routing/ff_router_trips/LocalFloorfieldViaFM.h b/routing/ff_router_trips/LocalFloorfieldViaFM.h
deleted file mode 100644
index 02a3d6ccd0691e2dbc55679f25d8729bfd8f2c90..0000000000000000000000000000000000000000
--- a/routing/ff_router_trips/LocalFloorfieldViaFM.h
+++ /dev/null
@@ -1,86 +0,0 @@
-/**
- * \file LocalFloorfieldViaFM.h
- * \date Feb 1, 2016
- * \version v0.8 +
- * \copyright <2009-2015> Forschungszentrum Jülich GmbH. All rights reserved.
- *
- * \section License
- * This file is part of JuPedSim.
- *
- * JuPedSim is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Lesser General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * any later version.
- *
- * JuPedSim is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public License
- * along with JuPedSim. If not, see <http://www.gnu.org/licenses/>.
- *
- * \section Description
- * The local FloorfieldViaFM is derived from the FloorfieldViaFM class. It will
- * create Floorfields in a room (room consisting of one or more subrooms) for
- * each exit. It is to be used in conjunction with a router, yielding intermediate
- * goals which are a door in the same room. It uses all the calculations and the
- * working on grid form the motherclass, yet it has a different constructor.
- **/
-
-
-#ifndef JPSCORE_LOCALFLOORFIELDVIAFM_H
-#define JPSCORE_LOCALFLOORFIELDVIAFM_H
-
-//#include <vector>
-#include <string>
-#include "FloorfieldViaFM.h"
-
-class LocalFloorfieldViaFM : public virtual FloorfieldViaFM {
-public:
- LocalFloorfieldViaFM();
- LocalFloorfieldViaFM(const Room* const room, const Building* buildingArg,
- const double hxArg, const double hyArg,
- const double wallAvoidDistance,
- const bool useDistancefield);
-
- void parseRoom(const Room* const roomArg, const double hxArg, const double hyArg);
-// void getDirectionToGoalID(const int goalID);
- void drawBlockerLines();
- void crossOutOutsideNeighbors(const long int key);
- // returns the UID of a subroom for a key of th grid, using the information from _subroomMap
- virtual SubRoom* isInside(const long int key);
-protected:
- const Room* _room;
-};
-
-/*
- NOTE: both parents are derived from FloorfieldViaFM with "public virtual" (it is called "Virtual Inheritance"). This
- means that there is only one instance of FloorfieldViaFM in CentrePointLocalFFViaFM, so calls to FlorfieldViaFM's member
- variables and functions are not ambiguous. See http://www.programering.com/a/MDOxITMwATk.html for some nice diagrams.
- */
-class CentrePointLocalFFViaFM : public CentrePointFFViaFM, public LocalFloorfieldViaFM {
-public:
- CentrePointLocalFFViaFM(const Room* const room, const Building* buildingArg,
- const double hxArg, const double hyArg,
- const double wallAvoidDistance,
- const bool useDistancefield) : LocalFloorfieldViaFM(room, buildingArg, hxArg, hyArg, wallAvoidDistance, useDistancefield) {};
-};
-
-class SubLocalFloorfieldViaFM : public FloorfieldViaFM {
-public:
- SubLocalFloorfieldViaFM();
- SubLocalFloorfieldViaFM(SubRoom* const subroom, const Building* buildingArg,
- const double hxArg, const double hyArg,
- const double wallAvoidDistance,
- const bool useDistancefield);
-
- void parseRoom(SubRoom* const subroomArg, const double hxArg, const double hyArg);
- void getDirectionToDestination (Pedestrian* ped, Point& direction);
- void getDirectionToGoalID(const int goalID);
- virtual SubRoom* isInside(const long int key);
-protected:
- SubRoom* _subroom;
-};
-
-#endif //JPSCORE_LOCALFLOORFIELDVIAFM_H
diff --git a/routing/ff_router_trips/UnivFFviaFM.cpp b/routing/ff_router_trips/UnivFFviaFM.cpp
deleted file mode 100644
index 6a6e23b0fe32d9f71036754a6f1b33d98d530853..0000000000000000000000000000000000000000
--- a/routing/ff_router_trips/UnivFFviaFM.cpp
+++ /dev/null
@@ -1,1881 +0,0 @@
-//
-// Created by arne on 5/9/17.
-//
-#define NOMINMAX
-#include <unordered_set>
-#include "UnivFFviaFM.h"
-#include "../../geometry/Line.h"
-#include "../../geometry/Building.h"
-#include "../../geometry/SubRoom.h"
-#include "../../pedestrian/Pedestrian.h"
-#include "mesh/RectGrid.h"
-
-
-UnivFFviaFM::~UnivFFviaFM() {
- if (_grid) delete _grid;
- //size_t speedsize = _speedFieldSelector.size();
- for (auto speedPtr : _speedFieldSelector) {
- if (speedPtr) delete[] speedPtr;
- }
- if (_gridCode) delete[] _gridCode;
- if (_subrooms) delete[] _subrooms;
-
- std::map<int, double*>::reverse_iterator delIterCost;
-
- for (delIterCost = _costFieldWithKey.rbegin();
- delIterCost != _costFieldWithKey.rend();
- ++delIterCost) {
- delete[] delIterCost->second;
- }
- std::map<int, Point*>::reverse_iterator delIterDir;
- for (delIterDir = _directionFieldWithKey.rbegin();
- delIterDir != _directionFieldWithKey.rend();
- ++delIterDir) {
- delete[] delIterDir->second;
- }
-}
-
-UnivFFviaFM::UnivFFviaFM(Room* r, Building* b, double hx, double wallAvoid, bool useWallDistances)
- : UnivFFviaFM(r, b->GetConfig(), hx, wallAvoid, useWallDistances) {
- _building = b;
-}
-
-UnivFFviaFM::UnivFFviaFM(SubRoom* sr, Building* b, double hx, double wallAvoid, bool useWallDistances)
- : UnivFFviaFM(sr, b->GetConfig(), hx, wallAvoid, useWallDistances) {
- _building = b;
-}
-
-UnivFFviaFM::UnivFFviaFM(Room* r, Configuration* const conf, double hx, double wallAvoid, bool useWallDistances)
- : UnivFFviaFM(r, conf, hx, wallAvoid, useWallDistances, std::vector<int>()){
-}
-
-UnivFFviaFM::UnivFFviaFM(Room* roomArg, Configuration* const confArg, double hx, double wallAvoid, bool useWallDistances, std::vector<int> wantedDoors) {
- //build the vector with walls(wall or obstacle), the map with <UID, Door(Cross or Trans)>, the vector with targets(UIDs)
- //then call other constructor including the mode
-
- _configuration = confArg;
- _scope = FF_ROOM_SCALE;
- _room = roomArg->GetID();
- std::vector<Line> lines;
- std::map<int, Line> tmpDoors;
- Line anyDoor = Line{};
- for (auto& subroomMap : roomArg->GetAllSubRooms()) {
- SubRoom* subRoomPtr = subroomMap.second.get();
- std::vector<Wall> walls = std::vector<Wall>(subRoomPtr->GetAllWalls());
- for (auto& wall : walls) {
- lines.emplace_back((Line)wall);
- }
-
- std::vector<Obstacle*> tmpObsPtrVec = subRoomPtr->GetAllObstacles();
- for (Obstacle* ptrObs : tmpObsPtrVec) {
- const std::vector<Wall> obsWalls = ptrObs->GetAllWalls();
- for (auto& owall : obsWalls) {
- lines.emplace_back((Line)owall);
- }
- }
-
- const std::vector<Crossing*> tmpCross = subRoomPtr->GetAllCrossings();
- const std::vector<Transition*> tmpTrans = subRoomPtr->GetAllTransitions();
-
- int uidNotConst = 0;
- bool isOpen = false;
- for (auto& cross : tmpCross) {
- uidNotConst = cross->GetUniqueID();
- isOpen = cross->IsOpen();
- if (!isOpen) {
- //will be added twice! is it a problem?
- lines.emplace_back((Line)*cross);
- } else {
- anyDoor = Line{*cross};
- if (tmpDoors.count(uidNotConst) == 0) {
- tmpDoors.emplace(std::make_pair(uidNotConst, (Line) *cross));
- }
- }
- }
- 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 {
- anyDoor = Line{*trans};
- if (tmpDoors.count(uidNotConst) == 0) {
- tmpDoors.emplace(std::make_pair(uidNotConst, (Line) *trans));
- }
- }
- }
- //find insidePoint and save it, together with UID
- Point normalVec = anyDoor.NormalVec();
- double length = normalVec.Norm();
- Point midPoint = anyDoor.GetCentre();
- Point candidate01 = midPoint + (normalVec * 0.25);
- Point candidate02 = midPoint - (normalVec * 0.25);
- if (subRoomPtr->IsInSubRoom(candidate01)) {
- _subRoomPtrTOinsidePoint.emplace(std::make_pair(subRoomPtr, candidate01));
- } else {
- //candidate = candidate - (normalVec * 0.25);
- if (subRoomPtr->IsInSubRoom(candidate02)) {
- _subRoomPtrTOinsidePoint.emplace(std::make_pair(subRoomPtr, candidate02));
- } else {
- Log->Write("ERROR:\t In UnivFF InsidePoint Analysis");
- bool a = subRoomPtr->IsInSubRoom(candidate01);
- bool b = subRoomPtr->IsInSubRoom(candidate02);
- a = b && a; //ignore this line. only to have a codeline after initialization of bools (to place a breakpoint)
- }
- }
- //_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) {
- create(lines, tmpDoors, wantedDoors, FF_WALL_AVOID, hx, wallAvoid, useWallDistances);
- } else {
- create(lines, tmpDoors, wantedDoors, FF_HOMO_SPEED, hx, wallAvoid, useWallDistances);
- }
-}
-
-UnivFFviaFM::UnivFFviaFM(SubRoom* sr, Configuration* const conf, double hx, double wallAvoid, bool useWallDistances)
- : UnivFFviaFM(sr, conf, hx, wallAvoid, useWallDistances, std::vector<int>()){
-}
-
-UnivFFviaFM::UnivFFviaFM(SubRoom* subRoomArg, Configuration* const confArg, double hx, double wallAvoid, bool useWallDistances, std::vector<int> wantedDoors) {
- //build the vector with walls(wall or obstacle), the map with <UID, Door(Cross or Trans)>, the vector with targets(UIDs)
- //then call other constructor including the mode
- _configuration = confArg;
- _scope = FF_SUBROOM_SCALE;
- _room = subRoomArg->GetRoomID();
- std::vector<Line> lines;
- std::map<int, Line> tmpDoors;
-
- std::vector<Wall> walls = std::vector<Wall> (subRoomArg->GetAllWalls());
- for (auto& wall : walls) {
- lines.emplace_back((Line)wall);
- }
-
- std::vector<Obstacle*> tmpObsPtrVec = subRoomArg->GetAllObstacles();
- for (Obstacle* ptrObs : tmpObsPtrVec) {
- const std::vector<Wall> obsWalls = ptrObs->GetAllWalls();
- for (auto& owall : obsWalls) {
- lines.emplace_back((Line)owall);
- }
- }
-
- const std::vector<Crossing*> tmpCross = subRoomArg->GetAllCrossings();
- const std::vector<Transition*> tmpTrans = subRoomArg->GetAllTransitions();
-
- int uidNotConst = 0;
- bool isOpen = false;
- for (auto& cross : tmpCross) {
- uidNotConst = cross->GetUniqueID();
- isOpen = cross->IsOpen();
- if (!isOpen) {
- lines.emplace_back((Line)*cross);
- } else {
- tmpDoors.emplace(std::make_pair(uidNotConst, (Line) *cross));
- }
- }
- for (auto& trans : tmpTrans) {
- uidNotConst = trans->GetUniqueID();
- isOpen = trans->IsOpen();
- if (!isOpen) {
- lines.emplace_back((Line)*trans);
- } else {
- tmpDoors.emplace(std::make_pair(uidNotConst, (Line) *trans));
- }
- }
-
- //find insidePoint and save it, together with UID
- Line anyDoor = Line{(--tmpDoors.end())->second};
- Point normalVec = anyDoor.NormalVec();
- double length = normalVec.Norm();
- Point midPoint = anyDoor.GetCentre();
- Point candidate01 = midPoint + (normalVec * 0.25);
- Point candidate02 = midPoint - (normalVec * 0.25);
- if (subRoomArg->IsInSubRoom(candidate01)) {
- _subRoomPtrTOinsidePoint.emplace(std::make_pair(subRoomArg, candidate01));
- } else {
- //candidate = candidate - (normalVec * 0.25);
- if (subRoomArg->IsInSubRoom(candidate02)) {
- _subRoomPtrTOinsidePoint.emplace(std::make_pair(subRoomArg, candidate02));
- } else {
- Log->Write("ERROR:\t In UnivFF InsidePoint Analysis");
- bool a = subRoomArg->IsInSubRoom(candidate01);
- bool b = subRoomArg->IsInSubRoom(candidate02);
- a = b && a;
- }
- }
-
- //_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);
- } else {
- create(lines, tmpDoors, wantedDoors, FF_HOMO_SPEED, hx, wallAvoid, useWallDistances);
- }
-}
-
-void UnivFFviaFM::create(std::vector<Line>& walls, std::map<int, Line>& doors, std::vector<int> targetUIDs, int mode,
- double spacing, double wallAvoid, bool useWallDistances) {
-
- _wallAvoidDistance = wallAvoid;
- _useWallDistances = useWallDistances;
- _speedmode = mode;
-
- //find circumscribing rectangle (x_min/max, y_min/max) //create RectGrid
- createRectGrid(walls, doors, spacing);
- _nPoints = _grid->GetnPoints();
-
- //allocate _gridCode and _speedFieldSelector and initialize them ("draw" walls and doors)
- _gridCode = new int[_nPoints];
- processGeometry(walls, doors);
- _speedFieldSelector.emplace(_speedFieldSelector.begin()+INITIAL_SPEED, new double[_nPoints]);
- std::fill(_speedFieldSelector[INITIAL_SPEED], _speedFieldSelector[INITIAL_SPEED]+_nPoints, 1.0);
-
- _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];
- _costFieldWithKey[0] = cost_alias_walldistance;
- Point* gradient_alias_walldirection = new Point[_nPoints];
- _directionFieldWithKey[0] = gradient_alias_walldirection;
-
- //create wall distance field
- //init costarray
- for (int i = 0; i < _nPoints; ++i) {
- if (_gridCode[i] == WALL) {
- cost_alias_walldistance[i] = magicnum(WALL_ON_COSTARRAY);
- } else {
- cost_alias_walldistance[i] = magicnum(UNKNOWN_COST);
- }
- }
- drawLinesOnWall(walls, cost_alias_walldistance, magicnum(TARGET_REGION));
- calcDF(cost_alias_walldistance, gradient_alias_walldirection, _speedFieldSelector[INITIAL_SPEED]);
- //_uids.emplace_back(0);
-
- double* temp_reduWallSpeed = new double[_nPoints];
- if (_speedFieldSelector[REDU_WALL_SPEED]) { //free memory before overwriting
- delete[] _speedFieldSelector[REDU_WALL_SPEED];
- }
- _speedFieldSelector[REDU_WALL_SPEED] = temp_reduWallSpeed;
- //init _reducedWallSpeed by using distance field
- createReduWallSpeed(temp_reduWallSpeed);
- }
-
- // parallel call
- if (!targetUIDs.empty()) {
- addTargetsParallel(targetUIDs);
- }
-}
-
-void UnivFFviaFM::createRectGrid(std::vector<Line>& walls, std::map<int, Line>& doors, double spacing) {
- double x_min = DBL_MAX; double x_max = DBL_MIN;
- double y_min = DBL_MAX; double y_max = DBL_MIN;
-
- for(auto& wall : walls) {
- if (wall.GetPoint1()._x < x_min) x_min = wall.GetPoint1()._x;
- if (wall.GetPoint1()._y < y_min) y_min = wall.GetPoint1()._y;
- if (wall.GetPoint2()._x < x_min) x_min = wall.GetPoint2()._x;
- if (wall.GetPoint2()._y < y_min) y_min = wall.GetPoint2()._y;
-
- if (wall.GetPoint1()._x > x_max) x_max = wall.GetPoint1()._x;
- if (wall.GetPoint1()._y > y_max) y_max = wall.GetPoint1()._y;
- if (wall.GetPoint2()._x > x_max) x_max = wall.GetPoint2()._x;
- if (wall.GetPoint2()._y > y_max) y_max = wall.GetPoint2()._y;
- }
-
- for(auto& doorPair:doors) {
- Line& door = doorPair.second;
- if (door.GetPoint1()._x < x_min) x_min = door.GetPoint1()._x;
- if (door.GetPoint1()._y < y_min) y_min = door.GetPoint1()._y;
- if (door.GetPoint2()._x < x_min) x_min = door.GetPoint2()._x;
- if (door.GetPoint2()._y < y_min) y_min = door.GetPoint2()._y;
-
- if (door.GetPoint1()._x > x_max) x_max = door.GetPoint1()._x;
- if (door.GetPoint1()._y > y_max) y_max = door.GetPoint1()._y;
- if (door.GetPoint2()._x > x_max) x_max = door.GetPoint2()._x;
- if (door.GetPoint2()._y > y_max) y_max = door.GetPoint2()._y;
- }
-
- //create Rect Grid
- _grid = new RectGrid();
- _grid->setBoundaries(x_min, y_min, x_max, y_max);
- _grid->setSpacing(spacing, spacing);
- _grid->createGrid();
-}
-
-void UnivFFviaFM::processGeometry(std::vector<Line>&walls, std::map<int, Line>& doors) {
- for (int i = 0; i < _nPoints; ++i) {
- _gridCode[i] = OUTSIDE;
- }
-
- for (auto mapentry : doors) {
- _doors.insert(mapentry);
- }
- //_doors = doors;
-
- drawLinesOnGrid<int>(walls, _gridCode, WALL);
- drawLinesOnGrid(doors, _gridCode); //UIDs of doors will be drawn on _gridCode
-}
-
-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];
- for (long i = 0; i < _nPoints; ++i) {
- _subrooms[i] = nullptr;
- }
- }
-
- //init start
- _subrooms[_grid->getKeyAtPoint(insidePoint)] = value;
- _gridCode[_grid->getKeyAtPoint(insidePoint)] = INSIDE;
-
- std::unordered_set<long> wavefront;
- wavefront.reserve(_nPoints);
-
- directNeighbor _neigh = _grid->getNeighbors(_grid->getKeyAtPoint(insidePoint));
- long aux = _neigh.key[0];
- if ((aux != -2) && (_gridCode[aux] == INSIDE || _gridCode[aux] == OUTSIDE)) {
- wavefront.insert(aux);
- _subrooms[aux] = value;
- }
-
- aux = _neigh.key[1];
- if ((aux != -2) && (_gridCode[aux] == INSIDE || _gridCode[aux] == OUTSIDE)) {
- wavefront.insert(aux);
- _subrooms[aux] = value;
- }
-
- aux = _neigh.key[2];
- if ((aux != -2) && (_gridCode[aux] == INSIDE || _gridCode[aux] == OUTSIDE)) {
- wavefront.insert(aux);
- _subrooms[aux] = value;
- }
-
- aux = _neigh.key[3];
- if ((aux != -2) && (_gridCode[aux] == INSIDE || _gridCode[aux] == OUTSIDE)) {
- wavefront.insert(aux);
- _subrooms[aux] = value;
- }
-
- while(!wavefront.empty()) {
- long current = *(wavefront.begin());
- wavefront.erase(current);
- _gridCode[current] = INSIDE;
-
- _neigh = _grid->getNeighbors(current);
- aux = _neigh.key[0];
- if ((aux != -2) && (_gridCode[aux] == INSIDE || _gridCode[aux] == OUTSIDE) && _subrooms[aux] == nullptr) {
- wavefront.insert(aux);
- _subrooms[aux] = value;
- }
-
- aux = _neigh.key[1];
- if ((aux != -2) && (_gridCode[aux] == INSIDE || _gridCode[aux] == OUTSIDE) && _subrooms[aux] == nullptr) {
- wavefront.insert(aux);
- _subrooms[aux] = value;
- }
-
- aux = _neigh.key[2];
- if ((aux != -2) && (_gridCode[aux] == INSIDE || _gridCode[aux] == OUTSIDE) && _subrooms[aux] == nullptr) {
- wavefront.insert(aux);
- _subrooms[aux] = value;
- }
-
- aux = _neigh.key[3];
- if ((aux != -2) && (_gridCode[aux] == INSIDE || _gridCode[aux] == OUTSIDE) && _subrooms[aux] == nullptr) {
- wavefront.insert(aux);
- _subrooms[aux] = value;
- }
- }
-}
-
-void UnivFFviaFM::createReduWallSpeed(double* reduWallSpeed){
- double factor = 1/_wallAvoidDistance;
- double* wallDstAlias = _costFieldWithKey[0];
-
- for (long int i = 0; i < _nPoints; ++i) {
- if (wallDstAlias[i] > 0.) {
- reduWallSpeed[i] = (wallDstAlias[i] > _wallAvoidDistance) ? 1.0 : (factor * wallDstAlias[i]);
- }
- }
-}
-
-void UnivFFviaFM::recreateAllForQuickest() {
- //allocate if neccessary (should not be!)
- for (int doorUID : _uids) {
- if (!_costFieldWithKey[doorUID]) {
- _costFieldWithKey[doorUID] = new double[_nPoints];
- }
- if (_user == DISTANCE_MEASUREMENTS_ONLY) {
- if (_directionFieldWithKey.count(doorUID) != 0 && _directionFieldWithKey[doorUID]){
- delete[] _directionFieldWithKey[doorUID];
- }
- _directionFieldWithKey[doorUID] = nullptr;
- }
- if (_user == DISTANCE_AND_DIRECTIONS_USED) {
- //check if not in map OR (in map but) nullptr)
- if ((_directionFieldWithKey.count(doorUID) == 0) || (!_directionFieldWithKey[doorUID])) {
- _directionFieldWithKey[doorUID] = new Point[_nPoints];
- }
- }
- }
-
- //parallel region
-#pragma omp parallel
- {
-#pragma omp for
- for (signed int i = 0; i < _doors.size(); ++i) {
- auto doorPair = _doors.begin();
- std::advance(doorPair, i);
- addTarget(doorPair->first, _costFieldWithKey[doorPair->first], _directionFieldWithKey[doorPair->first]);
- }
- };
-}
-
-void UnivFFviaFM::createPedSpeed(Pedestrian *const *pedsArg, int nsize, int modechoice, double radius) {
- long int delta = radius / _grid->Gethx();
- long int posIndex = 0;
- long int pos_i = 0;
- long int pos_j = 0;
- long int i_start = 0;
- long int j_start = 0;
- long int i_end = 0;
- long int j_end = 0;
- long int iStride = _grid->GetiMax();
- double indexDistance = 0.0;
- double newWaveSpeed = 0.0;
-
-// if (nsize == 0) {
-// Log->Write("WARNING: \tcreatePedSpeed: nsize is ZERO");
-// } else {
-// Log->Write("INFO: \t\tNumber of Peds used in createPedSpeed: %d",nsize);
-// }
-
- if ((modechoice == quickest) && (!_speedFieldSelector[PED_SPEED])) {
- _speedFieldSelector[PED_SPEED] = new double[_grid->GetnPoints()];
- }
-
- //we assume, that this function is only used by router and is not using REDU_WALL_SPEED
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- _speedFieldSelector[PED_SPEED][i] = _speedFieldSelector[INITIAL_SPEED][i];
- }
- if (_speedFieldSelector[REDU_WALL_SPEED] && _mode == LINESEGMENT) { //indicates, that the direction strat is using it
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- _speedFieldSelector[PED_SPEED][i] = _speedFieldSelector[REDU_WALL_SPEED][i];
- }
- }
-
- for (int i = 0; i < nsize; ++i) {
- //the following check is not 3D proof, we require the caller of this function to provide a list with "valid"
- //pedestrian-pointer
- if (!_grid->includesPoint(pedsArg[i]->GetPos())) {
- continue;
- }
-
- newWaveSpeed = pedsArg[i]->GetEllipse().GetV().Norm()/pedsArg[i]->GetEllipse().GetV0(); // (current speed)/(desired speed)
- newWaveSpeed *= 0.8;
- if (newWaveSpeed < 0.1) {
- newWaveSpeed = 0.1;
- }
- posIndex = _grid->getKeyAtPoint(pedsArg[i]->GetPos());
- pos_i = _grid->get_i_fromKey(posIndex);
- pos_j = _grid->get_j_fromKey(posIndex);
-
- i_start = ((pos_i - delta) < 0) ? 0 : (pos_i - delta);
- i_end = ((pos_i + delta) >= iStride) ? iStride-1 : (pos_i + delta);
-
- j_start = ((pos_j - delta) < 0) ? 0 : (pos_j - delta);
- j_end = ((pos_j + delta) >= _grid->GetjMax()) ? _grid->GetjMax()-1 : (pos_j + delta);
-
- for (long int curr_i = i_start; curr_i < i_end; ++curr_i) {
- for (long int curr_j = j_start; curr_j < j_end; ++curr_j) {
- //indexDistance holds the square
- indexDistance = ( (curr_i - pos_i)*(curr_i - pos_i) + (curr_j - pos_j)*(curr_j - pos_j) );
-
- if (indexDistance < (delta*delta)) {
- //_speedFieldSelector[PED_SPEED][curr_j*iStride + curr_i] = 0.2;
- _speedFieldSelector[PED_SPEED][curr_j*iStride + curr_i] = std::min(newWaveSpeed, _speedFieldSelector[PED_SPEED][curr_j*iStride + curr_i]);
- }
- }
- }
- }
-}
-
-void UnivFFviaFM::finalizeTargetLine(const int uid, const Line& line, Point* target, Point& value) {
- // i~x; j~y;
-//http://stackoverflow.com/questions/10060046/drawing-lines-with-bresenhams-line-algorithm
-//src in answer of "Avi"; adapted to fit this application
-
- //grid handeling local vars:
- long int iMax = _grid->GetiMax();
-
- long int iStart, iEnd;
- long int jStart, jEnd;
- long int iDot, jDot;
- long int key;
- long int deltaX, deltaY, deltaX1, deltaY1, px, py, xe, ye, i; //Bresenham Algorithm
-
- long int goodneighbor;
- directNeighbor neigh;
-
-
- key = _grid->getKeyAtPoint(line.GetPoint1());
- iStart = (long) _grid->get_i_fromKey(key);
- jStart = (long) _grid->get_j_fromKey(key);
-
- key = _grid->getKeyAtPoint(line.GetPoint2());
- iEnd = (long) _grid->get_i_fromKey(key);
- jEnd = (long) _grid->get_j_fromKey(key);
-
- deltaX = (int) (iEnd - iStart);
- deltaY = (int) (jEnd - jStart);
- deltaX1 = abs( (int) (iEnd - iStart));
- deltaY1 = abs( (int) (jEnd - jStart));
-
- px = 2*deltaY1 - deltaX1;
- py = 2*deltaX1 - deltaY1;
-
- if(deltaY1<=deltaX1) {
- if(deltaX>=0) {
- iDot = iStart;
- jDot = jStart;
- xe = iEnd;
- } else {
- iDot = iEnd;
- jDot = jEnd;
- xe = iStart;
- }
- if (_gridCode[jDot*iMax + iDot] == uid) {
- /* //find a good neighborvalue
- neigh = _grid->getNeighbors(jDot*iMax + iDot);
- if ((neigh.key[0] >= 0) && (_gridCode[neigh.key[0]] == INSIDE)) {
- goodneighbor = neigh.key[0];
- } else if ((neigh.key[1] >= 0) && (_gridCode[neigh.key[1]] == INSIDE)) {
- goodneighbor = neigh.key[1];
- } else if ((neigh.key[2] >= 0) && (_gridCode[neigh.key[2]] == INSIDE)) {
- goodneighbor = neigh.key[2];
- } else if ((neigh.key[3] >= 0) && (_gridCode[neigh.key[3]] == INSIDE)) {
- goodneighbor = neigh.key[3];
- } else {
- //ERROR - should have an inside neighbor
- Log->Write("ERROR:\t In finalizeTargetLine");
- }
- //write the value on targetline
- if ((target[goodneighbor]._x == 0.) && (target[goodneighbor]._y == 0.)) {
- //ERROR - should have a true vector
- Log->Write("ERROR:\t (0;0) In finalizeTargetLine");
- }*/
- target[jDot * iMax + iDot] = value;
- } else if (_gridCode[jDot*iMax + iDot] == WALL) {
- //do nothing
- } else {
- target[jDot * iMax + iDot] = value;
- //Log->Write("ERROR:\t in finalizingTargetLine");
- }
- for (i=0; iDot < xe; ++i) {
- ++iDot;
- if (px < 0) {
- px += 2 * deltaY1;
- } else {
- if ((deltaX < 0 && deltaY < 0) || (deltaX > 0 && deltaY > 0)) {
- ++jDot;
- } else {
- --jDot;
- }
- px += 2 * (deltaY1 - deltaX1);
- }
- if (_gridCode[jDot * iMax + iDot] == uid) {
- /*//find a good neighborvalue
- neigh = _grid->getNeighbors(jDot*iMax + iDot);
- if ((neigh.key[0] >= 0) && (_gridCode[neigh.key[0]] == INSIDE)) {
- goodneighbor = neigh.key[0];
- } else if ((neigh.key[1] >= 0) && (_gridCode[neigh.key[1]] == INSIDE)) {
- goodneighbor = neigh.key[1];
- } else if ((neigh.key[2] >= 0) && (_gridCode[neigh.key[2]] == INSIDE)) {
- goodneighbor = neigh.key[2];
- } else if ((neigh.key[3] >= 0) && (_gridCode[neigh.key[3]] == INSIDE)) {
- goodneighbor = neigh.key[3];
- } else {
- //ERROR - should have an inside neighbor
- Log->Write("ERROR:\t In finalizeTargetLine");
- }
- //write the value on targetline
- if ((target[goodneighbor]._x == 0.) && (target[goodneighbor]._y == 0.)) {
- //ERROR - should have a true vector
- Log->Write("ERROR:\t (0;0) In finalizeTargetLine");
- }*/
- target[jDot * iMax + iDot] = value;
- } else if (_gridCode[jDot*iMax + iDot] == WALL) {
- //do nothing
- } else {
- target[jDot * iMax + iDot] = value;
- //Log->Write("ERROR:\t in finalizingTargetLine");
- }
- }
- } else {
- if(deltaY>=0) {
- iDot = iStart;
- jDot = jStart;
- ye = jEnd;
- } else {
- iDot = iEnd;
- jDot = jEnd;
- ye = jStart;
- }
- if (_gridCode[jDot*iMax + iDot] == uid) {
- /*//find a good neighborvalue
- neigh = _grid->getNeighbors(jDot*iMax + iDot);
- if ((neigh.key[0] >= 0) && (_gridCode[neigh.key[0]] == INSIDE)) {
- goodneighbor = neigh.key[0];
- } else if ((neigh.key[1] >= 0) && (_gridCode[neigh.key[1]] == INSIDE)) {
- goodneighbor = neigh.key[1];
- } else if ((neigh.key[2] >= 0) && (_gridCode[neigh.key[2]] == INSIDE)) {
- goodneighbor = neigh.key[2];
- } else if ((neigh.key[3] >= 0) && (_gridCode[neigh.key[3]] == INSIDE)) {
- goodneighbor = neigh.key[3];
- } else {
- //ERROR - should have an inside neighbor
- Log->Write("ERROR:\t In finalizeTargetLine");
- }
- //write the value on targetline
- if ((target[goodneighbor]._x == 0.) && (target[goodneighbor]._y == 0.)) {
- //ERROR - should have a true vector
- Log->Write("ERROR:\t (0;0) In finalizeTargetLine");
- }*/
- target[jDot * iMax + iDot] = value;
- } else if (_gridCode[jDot*iMax + iDot] == WALL) {
- //do nothing
- } else {
- target[jDot * iMax + iDot] = value;
- //Log->Write("ERROR:\t in finalizingTargetLine");
- }
- for(i=0; jDot<ye; ++i) {
- ++jDot;
- if (py<=0) {
- py+=2*deltaX1;
- } else {
- if((deltaX<0 && deltaY<0) || (deltaX>0 && deltaY>0)) {
- ++iDot;
- } else {
- --iDot;
- }
- py+=2*(deltaX1-deltaY1);
- }
- if (_gridCode[jDot*iMax + iDot] == uid) {
- /*//find a good neighborvalue
- neigh = _grid->getNeighbors(jDot*iMax + iDot);
- if ((neigh.key[0] >= 0) && (_gridCode[neigh.key[0]] == INSIDE)) {
- goodneighbor = neigh.key[0];
- } else if ((neigh.key[1] >= 0) && (_gridCode[neigh.key[1]] == INSIDE)) {
- goodneighbor = neigh.key[1];
- } else if ((neigh.key[2] >= 0) && (_gridCode[neigh.key[2]] == INSIDE)) {
- goodneighbor = neigh.key[2];
- } else if ((neigh.key[3] >= 0) && (_gridCode[neigh.key[3]] == INSIDE)) {
- goodneighbor = neigh.key[3];
- } else {
- //ERROR - should have an inside neighbor
- Log->Write("ERROR:\t In finalizeTargetLine");
- }
- //write the value on targetline
- if ((target[goodneighbor]._x == 0.) && (target[goodneighbor]._y == 0.)) {
- //ERROR - should have a true vector
- Log->Write("ERROR:\t (0;0) In finalizeTargetLine");
- }*/
- target[jDot * iMax + iDot] = value;
- } else if (_gridCode[jDot*iMax + iDot] == WALL) {
- //do nothing
- } else {
- target[jDot * iMax + iDot] = value;
- //Log->Write("ERROR:\t in finalizingTargetLine");
- }
- }
- }
-}
-
-void UnivFFviaFM::drawLinesOnGrid(std::map<int, Line>& doors, int *const grid) {
- for (auto&& doorPair : doors) {
- int tempUID = doorPair.first;
- Line tempDoorLine = Line(doorPair.second);
- drawLinesOnGrid(tempDoorLine, grid, tempUID);
- }
-}
-
-template <typename T>
-void UnivFFviaFM::drawLinesOnGrid(std::vector<Line>& wallArg, T* const target, const T value) { //no init, plz init elsewhere
-
- for (auto& line : wallArg) {
- drawLinesOnGrid(line, target, value);
- } //loop over all walls
-
-} //drawLinesOnGrid
-
-template <typename T>
-void UnivFFviaFM::drawLinesOnGrid(Line& line, T* const target, const T value) { //no init, plz init elsewhere
-// i~x; j~y;
-//http://stackoverflow.com/questions/10060046/drawing-lines-with-bresenhams-line-algorithm
-//src in answer of "Avi"; adapted to fit this application
-
- //grid handeling local vars:
- long int iMax = _grid->GetiMax();
-
- long int iStart, iEnd;
- long int jStart, jEnd;
- long int iDot, jDot;
- long int key;
- long int deltaX, deltaY, deltaX1, deltaY1, px, py, xe, ye, i; //Bresenham Algorithm
-
-
- key = _grid->getKeyAtPoint(line.GetPoint1());
- iStart = (long) _grid->get_i_fromKey(key);
- jStart = (long) _grid->get_j_fromKey(key);
-
- key = _grid->getKeyAtPoint(line.GetPoint2());
- iEnd = (long) _grid->get_i_fromKey(key);
- jEnd = (long) _grid->get_j_fromKey(key);
-
- deltaX = (int) (iEnd - iStart);
- deltaY = (int) (jEnd - jStart);
- deltaX1 = abs( (int) (iEnd - iStart));
- deltaY1 = abs( (int) (jEnd - jStart));
-
- px = 2*deltaY1 - deltaX1;
- py = 2*deltaX1 - deltaY1;
-
- if(deltaY1<=deltaX1) {
- if(deltaX>=0) {
- iDot = iStart;
- jDot = jStart;
- xe = iEnd;
- } else {
- iDot = iEnd;
- jDot = jEnd;
- xe = iStart;
- }
- if ((_gridCode[jDot*iMax + iDot] != WALL) && (_gridCode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gridCode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
- target[jDot * iMax + iDot] = value;
- }
- for (i=0; iDot < xe; ++i) {
- ++iDot;
- if(px<0) {
- px+=2*deltaY1;
- } else {
- if((deltaX<0 && deltaY<0) || (deltaX>0 && deltaY>0)) {
- ++jDot;
- } else {
- --jDot;
- }
- px+=2*(deltaY1-deltaX1);
- }
- if ((_gridCode[jDot*iMax + iDot] != WALL) && (_gridCode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gridCode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
- target[jDot * iMax + iDot] = value;
- }
- }
- } else {
- if(deltaY>=0) {
- iDot = iStart;
- jDot = jStart;
- ye = jEnd;
- } else {
- iDot = iEnd;
- jDot = jEnd;
- ye = jStart;
- }
- if ((_gridCode[jDot*iMax + iDot] != WALL) && (_gridCode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gridCode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
- target[jDot * iMax + iDot] = value;
- }
- for(i=0; jDot<ye; ++i) {
- ++jDot;
- if (py<=0) {
- py+=2*deltaX1;
- } else {
- if((deltaX<0 && deltaY<0) || (deltaX>0 && deltaY>0)) {
- ++iDot;
- } else {
- --iDot;
- }
- py+=2*(deltaX1-deltaY1);
- }
- if ((_gridCode[jDot*iMax + iDot] != WALL) && (_gridCode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gridCode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
- target[jDot * iMax + iDot] = value;
- }
- }
- }
-} //drawLinesOnGrid
-
-template <typename T>
-void UnivFFviaFM::drawLinesOnWall(std::vector<Line>& wallArg, T* const target, const T value) { //no init, plz init elsewhere
-
- for (auto& line : wallArg) {
- drawLinesOnWall(line, target, value);
- } //loop over all walls
-
-} //drawLinesOnWall
-
-template <typename T>
-void UnivFFviaFM::drawLinesOnWall(Line& line, T* const target, const T value) { //no init, plz init elsewhere
-// i~x; j~y;
-//http://stackoverflow.com/questions/10060046/drawing-lines-with-bresenhams-line-algorithm
-//src in answer of "Avi"; adapted to fit this application
-
- //grid handeling local vars:
- long int iMax = _grid->GetiMax();
-
- long int iStart, iEnd;
- long int jStart, jEnd;
- long int iDot, jDot;
- long int key;
- long int deltaX, deltaY, deltaX1, deltaY1, px, py, xe, ye, i; //Bresenham Algorithm
-
-
- key = _grid->getKeyAtPoint(line.GetPoint1());
- iStart = (long) _grid->get_i_fromKey(key);
- jStart = (long) _grid->get_j_fromKey(key);
-
- key = _grid->getKeyAtPoint(line.GetPoint2());
- iEnd = (long) _grid->get_i_fromKey(key);
- jEnd = (long) _grid->get_j_fromKey(key);
-
- deltaX = (int) (iEnd - iStart);
- deltaY = (int) (jEnd - jStart);
- deltaX1 = abs( (int) (iEnd - iStart));
- deltaY1 = abs( (int) (jEnd - jStart));
-
- px = 2*deltaY1 - deltaX1;
- py = 2*deltaX1 - deltaY1;
-
- if(deltaY1<=deltaX1) {
- if(deltaX>=0) {
- iDot = iStart;
- jDot = jStart;
- xe = iEnd;
- } else {
- iDot = iEnd;
- jDot = jEnd;
- xe = iStart;
- }
- if ((_gridCode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gridCode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
- target[jDot * iMax + iDot] = value;
- }
- for (i=0; iDot < xe; ++i) {
- ++iDot;
- if(px<0) {
- px+=2*deltaY1;
- } else {
- if((deltaX<0 && deltaY<0) || (deltaX>0 && deltaY>0)) {
- ++jDot;
- } else {
- --jDot;
- }
- px+=2*(deltaY1-deltaX1);
- }
- if ((_gridCode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gridCode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
- target[jDot * iMax + iDot] = value;
- }
- }
- } else {
- if(deltaY>=0) {
- iDot = iStart;
- jDot = jStart;
- ye = jEnd;
- } else {
- iDot = iEnd;
- jDot = jEnd;
- ye = jStart;
- }
- if ((_gridCode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gridCode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
- target[jDot * iMax + iDot] = value;
- }
- for(i=0; jDot<ye; ++i) {
- ++jDot;
- if (py<=0) {
- py+=2*deltaX1;
- } else {
- if((deltaX<0 && deltaY<0) || (deltaX>0 && deltaY>0)) {
- ++iDot;
- } else {
- --iDot;
- }
- py+=2*(deltaX1-deltaY1);
- }
- if ((_gridCode[jDot*iMax + iDot] != CLOSED_CROSSING) && (_gridCode[jDot*iMax + iDot] != CLOSED_TRANSITION)) {
- target[jDot * iMax + iDot] = value;
- }
- }
- }
-} //drawLinesOnWall
-
-void UnivFFviaFM::calcFF(double* costOutput, Point* directionOutput, const double *const speed) {
- //CompareCost comp = CompareCost(costOutput);
- std::priority_queue<long int, std::vector<long int>, CompareCost> trialfield(costOutput); //pass the argument for the constr of CompareCost
- //std::priority_queue<long int, std::vector<long int>, CompareCost> trialfield2(comp); //pass the CompareCost object directly
-
- directNeighbor local_neighbor = _grid->getNeighbors(0);
- long int aux = 0;
- //init trial field
- for (long int i = 0; i < _nPoints; ++i) {
- if (costOutput[i] == 0.0) {
- //check for negative neighbours, calc that ones and add to queue trialfield
- local_neighbor = _grid->getNeighbors(i);
-
- //check for valid neigh
- aux = local_neighbor.key[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) && (_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) && (_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) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
- calcCost(aux, costOutput, directionOutput, speed);
- trialfield.emplace(aux);
- //trialfield2.emplace(aux);
- }
- }
- }
-
- while(!trialfield.empty()) {
- local_neighbor = _grid->getNeighbors(trialfield.top());
- trialfield.pop();
-
- //check for valid neigh
- aux = local_neighbor.key[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) && (_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) && (_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) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
- calcCost(aux, costOutput, directionOutput, speed);
- trialfield.emplace(aux);
- //trialfield2.emplace(aux);
- }
- }
-}
-
-void UnivFFviaFM::calcCost(const long int key, double* cost, Point* dir, const double* const speed) {
- //adapt from calcFloorfield
- double row = DBL_MAX;
- double col = DBL_MAX;
- long int aux = -1; //will be set below
- bool pointsUp = false;
- bool pointsRight = false;
-
- directNeighbor dNeigh = _grid->getNeighbors(key);
-
- aux = dNeigh.key[0];
- //hint: trialfield[i].cost = dist2Wall + i; <<< set in resetGoalAndCosts
- if ((aux != -2) && //neighbor is a gridpoint
- (cost[aux] != magicnum(UNKNOWN_COST)) && (cost[aux] != magicnum(UNKNOWN_DISTANCE)) && //gridpoint holds a calculated value
- (_gridCode[aux] != WALL)) //gridpoint holds a calculated value
- {
- row = cost[aux];
- pointsRight = true;
- if (row < 0) {
- std::cerr << "hier ist was schief " << row << " " << aux << " " << std::endl;
- row = DBL_MAX;
- }
- }
- aux = dNeigh.key[2];
- if ((aux != -2) && //neighbor is a gridpoint
- (cost[aux] != magicnum(UNKNOWN_COST)) && (cost[aux] != magicnum(UNKNOWN_DISTANCE)) && //gridpoint holds a calculated value
- (_gridCode[aux] != WALL) &&
- (cost[aux] < row)) //calculated value promises smaller cost
- {
- row = cost[aux];
- pointsRight = false;
- }
-
- aux = dNeigh.key[1];
- //hint: trialfield[i].cost = dist2Wall + i; <<< set in parseBuilding after linescan call
- if ((aux != -2) && //neighbor is a gridpoint
- (cost[aux] != magicnum(UNKNOWN_COST)) && (cost[aux] != magicnum(UNKNOWN_DISTANCE)) && //gridpoint holds a calculated value
- (_gridCode[aux] != WALL))
- {
- col = cost[aux];
- pointsUp = true;
- if (col < 0) {
- std::cerr << "hier ist was schief " << col << " " << aux << " " << std::endl;
- col = DBL_MAX;
- }
- }
- aux = dNeigh.key[3];
- if ((aux != -2) && //neighbor is a gridpoint
- (cost[aux] != magicnum(UNKNOWN_COST)) && (cost[aux] != magicnum(UNKNOWN_DISTANCE)) && //gridpoint holds a calculated value
- (_gridCode[aux] != WALL) &&
- (cost[aux] < col)) //calculated value promises smaller cost
- {
- col = cost[aux];
- pointsUp = false;
- }
- if (col == DBL_MAX) { //one sided update with row
- cost[key] = onesidedCalc(row, _grid->Gethx()/speed[key]);
- //flag[key] = FM_SINGLE;
- if (!dir) {
- return;
- }
- if (pointsRight) {
- dir[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
- dir[key]._y = (0.);
- } else {
- dir[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
- dir[key]._y = (0.);
- }
- dir[key] = dir[key].Normalized();
- return;
- }
-
- if (row == DBL_MAX) { //one sided update with col
- cost[key] = onesidedCalc(col, _grid->Gethy()/speed[key]);
- //flag[key] = FM_SINGLE;
- if (!dir) {
- return;
- }
- if (pointsUp) {
- dir[key]._x = (0.);
- dir[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
- } else {
- dir[key]._x = (0.);
- dir[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
- }
- dir[key] = dir[key].Normalized();
- return;
- }
-
- //two sided update
- double precheck = twosidedCalc(row, col, _grid->Gethx()/speed[key]);
- if (precheck >= 0) {
- cost[key] = precheck;
- //flag[key] = FM_DOUBLE;
- if (!dir) {
- return;
- }
- if (pointsUp && pointsRight) {
- dir[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
- dir[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
- }
- if (pointsUp && !pointsRight) {
- dir[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
- dir[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
- }
- if (!pointsUp && pointsRight) {
- dir[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
- dir[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
- }
- if (!pointsUp && !pointsRight) {
- dir[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
- dir[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
- }
- } else {
- std::cerr << "else in twosided Dist " << std::endl;
- }
- dir[key] = dir[key].Normalized();
-}
-
-void UnivFFviaFM::calcDF(double* costOutput, Point* directionOutput, const double *const speed) {
- //CompareCost comp = CompareCost(costOutput);
- std::priority_queue<long int, std::vector<long int>, CompareCost> trialfield(costOutput); //pass the argument for the constr of CompareCost
- //std::priority_queue<long int, std::vector<long int>, CompareCost> trialfield2(comp); //pass the CompareCost object directly
-
- directNeighbor local_neighbor = _grid->getNeighbors(0);
- long int aux = 0;
- //init trial field
- for (long int i = 0; i < _nPoints; ++i) {
- if (costOutput[i] == 0.0) {
- //check for negative neighbours, calc that ones and add to queue trialfield
- local_neighbor = _grid->getNeighbors(i);
-
- //check for valid neigh
- aux = local_neighbor.key[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) && (_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) && (_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) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
- calcDist(aux, costOutput, directionOutput, speed);
- trialfield.emplace(aux);
- //trialfield2.emplace(aux);
- }
- }
- }
-
- while(!trialfield.empty()) {
- local_neighbor = _grid->getNeighbors(trialfield.top());
- trialfield.pop();
-
- //check for valid neigh
- aux = local_neighbor.key[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) && (_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) && (_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) && (_gridCode[aux] != OUTSIDE) && (costOutput[aux] < 0.0)) {
- calcDist(aux, costOutput, directionOutput, speed);
- trialfield.emplace(aux);
- //trialfield2.emplace(aux);
- }
- }
-}
-
-void UnivFFviaFM::calcDist(const long int key, double* cost, Point* dir, const double* const speed) {
- //adapt from calcFloorfield
- double row = DBL_MAX;
- double col = DBL_MAX;
- long int aux = -1; //will be set below
- bool pointsUp = false;
- bool pointsRight = false;
-
- directNeighbor dNeigh = _grid->getNeighbors(key);
-
- aux = dNeigh.key[0];
- //hint: trialfield[i].cost = dist2Wall + i; <<< set in resetGoalAndCosts
- if ((aux != -2) && //neighbor is a gridpoint
- (cost[aux] != magicnum(UNKNOWN_COST)) && (cost[aux] != magicnum(UNKNOWN_DISTANCE)) //gridpoint holds a calculated value
- ) //gridpoint holds a calculated value
- {
- row = cost[aux];
- pointsRight = true;
- if (row < 0) {
- std::cerr << "hier ist was schief " << row << " " << aux << " " << std::endl;
- row = DBL_MAX;
- }
- }
- aux = dNeigh.key[2];
- if ((aux != -2) && //neighbor is a gridpoint
- (cost[aux] != magicnum(UNKNOWN_COST)) && (cost[aux] != magicnum(UNKNOWN_DISTANCE)) //gridpoint holds a calculated value
- &&
- (cost[aux] < row)) //calculated value promises smaller cost
- {
- row = cost[aux];
- pointsRight = false;
- }
-
- aux = dNeigh.key[1];
- //hint: trialfield[i].cost = dist2Wall + i; <<< set in parseBuilding after linescan call
- if ((aux != -2) && //neighbor is a gridpoint
- (cost[aux] != magicnum(UNKNOWN_COST)) && (cost[aux] != magicnum(UNKNOWN_DISTANCE)) //gridpoint holds a calculated value
- )
- {
- col = cost[aux];
- pointsUp = true;
- if (col < 0) {
- std::cerr << "hier ist was schief " << col << " " << aux << " " << std::endl;
- col = DBL_MAX;
- }
- }
- aux = dNeigh.key[3];
- if ((aux != -2) && //neighbor is a gridpoint
- (cost[aux] != magicnum(UNKNOWN_COST)) && (cost[aux] != magicnum(UNKNOWN_DISTANCE)) && //gridpoint holds a calculated value
-
- (cost[aux] < col)) //calculated value promises smaller cost
- {
- col = cost[aux];
- pointsUp = false;
- }
- if (col == DBL_MAX) { //one sided update with row
- cost[key] = onesidedCalc(row, _grid->Gethx()/speed[key]);
- //flag[key] = FM_SINGLE;
- if (!dir) {
- return;
- }
- if (pointsRight) {
- dir[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
- dir[key]._y = (0.);
- } else {
- dir[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
- dir[key]._y = (0.);
- }
- dir[key] = dir[key].Normalized();
- return;
- }
-
- if (row == DBL_MAX) { //one sided update with col
- cost[key] = onesidedCalc(col, _grid->Gethy()/speed[key]);
- //flag[key] = FM_SINGLE;
- if (!dir) {
- return;
- }
- if (pointsUp) {
- dir[key]._x = (0.);
- dir[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
- } else {
- dir[key]._x = (0.);
- dir[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
- }
- dir[key] = dir[key].Normalized();
- return;
- }
-
- //two sided update
- double precheck = twosidedCalc(row, col, _grid->Gethx()/speed[key]);
- if (precheck >= 0) {
- cost[key] = precheck;
- //flag[key] = FM_DOUBLE;
- if (!dir) {
- return;
- }
- if (pointsUp && pointsRight) {
- dir[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
- dir[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
- }
- if (pointsUp && !pointsRight) {
- dir[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
- dir[key]._y = (-(cost[key+(_grid->GetiMax())]-cost[key])/_grid->Gethy());
- }
- if (!pointsUp && pointsRight) {
- dir[key]._x = (-(cost[key+1]-cost[key])/_grid->Gethx());
- dir[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
- }
- if (!pointsUp && !pointsRight) {
- dir[key]._x = (-(cost[key]-cost[key-1])/_grid->Gethx());
- dir[key]._y = (-(cost[key]-cost[key-(_grid->GetiMax())])/_grid->Gethy());
- }
- } else {
- std::cerr << "else in twosided Dist " << std::endl;
- }
- dir[key] = dir[key].Normalized();
-}
-
-inline double UnivFFviaFM::onesidedCalc(double xy, double hDivF) {
- //if ( (xy+hDivF) > 10000) std::cerr << "error in onesided " << xy << std::endl;
- return xy + hDivF;
-}
-
-inline double UnivFFviaFM::twosidedCalc(double x, double y, double hDivF) { //on error return -2
- double determinante = (2*hDivF*hDivF - (x-y)*(x-y));
- if (determinante >= 0) {
- return (x + y + sqrt(determinante))/2;
- } else {
- return (x < y) ? (x + hDivF) : (y + hDivF);
- }
- std::cerr << "error in two-sided 2!!!!!!!!!!!!!!!!!!!!!!! o_O??" << std::endl;
- return -2.; //this line should never execute
-} //twosidedCalc
-
-void UnivFFviaFM::addTarget(const int uid, double* costarrayDBL, Point* gradarrayPt) {
- if (_doors.count(uid) == 0) {
- Log->Write("ERROR: \tCould not find door with uid %d in Room %d", uid, _room);
- return;
- }
- Line tempTargetLine = Line(_doors[uid]);
- Point tempCenterPoint = Point(tempTargetLine.GetCentre());
- if (_mode == LINESEGMENT) {
- if (tempTargetLine.GetLength() > 0.6) { //shorten line from both Points to avoid targeting edges of real door
- const Point &p1 = tempTargetLine.GetPoint1();
- const Point &p2 = tempTargetLine.GetPoint2();
- double length = tempTargetLine.GetLength();
- double u = 0.2 / length;
- tempTargetLine = Line(p1 + (p2 - p1) * u, p1 + (p2 - p1) * (1 - u), 0);
- } else if (tempTargetLine.GetLength() > 0.2) {
- const Point &p1 = tempTargetLine.GetPoint1();
- const Point &p2 = tempTargetLine.GetPoint2();
- double length = tempTargetLine.GetLength();
- double u = 0.05 / length;
- tempTargetLine = Line(p1 + (p2 - p1) * u, p1 + (p2 - p1) * (1 - u), 0);
- }
- }
-
- //this allocation must be on shared heap! to be accessible by any thread later (should be shared in openmp)
- double* newArrayDBL = (costarrayDBL)? costarrayDBL : new double[_nPoints];
- Point* newArrayPt = nullptr;
- if (_user == DISTANCE_AND_DIRECTIONS_USED) {
- newArrayPt = (gradarrayPt)? gradarrayPt : new Point[_nPoints];
- }
-
- if ((_costFieldWithKey[uid]) && (_costFieldWithKey[uid] != costarrayDBL))
- delete[] _costFieldWithKey[uid];
- _costFieldWithKey[uid] = newArrayDBL;
-
- //init costarray
- for (int i = 0; i < _nPoints; ++i) {
- if (_gridCode[i] == WALL) {
- newArrayDBL[i] = magicnum(WALL_ON_COSTARRAY);
- } else {
- newArrayDBL[i] = magicnum(UNKNOWN_COST);
- }
- }
-
- if ((_directionFieldWithKey[uid]) && (_directionFieldWithKey[uid] != gradarrayPt))
- delete[] _directionFieldWithKey[uid];
- if (newArrayPt)
- _directionFieldWithKey[uid] = newArrayPt;
-
- //initialize start area
- if (_mode == LINESEGMENT) {
- drawLinesOnGrid(tempTargetLine, newArrayDBL, magicnum(TARGET_REGION));
- }
- if (_mode == CENTERPOINT) {
- newArrayDBL[_grid->getKeyAtPoint(tempCenterPoint)] = magicnum(TARGET_REGION);
- }
-
- if (_speedmode == FF_WALL_AVOID) {
- calcFF(newArrayDBL, newArrayPt, _speedFieldSelector[REDU_WALL_SPEED]);
- } else if (_speedmode == FF_HOMO_SPEED) {
- calcFF(newArrayDBL, newArrayPt, _speedFieldSelector[INITIAL_SPEED]);
- } else if (_speedmode == FF_PED_SPEED) {
- calcFF(newArrayDBL, newArrayPt, _speedFieldSelector[PED_SPEED]);
- }
-
- //the rest of the door must be initialized if centerpoint was used. else ff_router will have probs getting localDist
- if (_mode == CENTERPOINT) {
- drawLinesOnGrid(tempTargetLine, newArrayDBL, magicnum(TARGET_REGION));
- }
- //the directional field is yet undefined on the target line itself. we will use neighboring vector to help agents
- //to cross the line
- if (newArrayPt) {
- Point passvector = tempTargetLine.NormalVec();
- Point trial = tempTargetLine.GetCentre() - passvector * 0.25;
- Point trial2 = tempTargetLine.GetCentre() + passvector * 0.25;
- if ((_grid->includesPoint(trial)) && (_gridCode[_grid->getKeyAtPoint(trial)] == INSIDE)) {
- finalizeTargetLine(uid, _doors[uid], newArrayPt, passvector);
- finalizeTargetLine(uid, tempTargetLine, newArrayPt, passvector);
- } else if ((_grid->includesPoint(trial2)) && (_gridCode[_grid->getKeyAtPoint(trial2)] == INSIDE)) {
- passvector = passvector * -1.0;
- finalizeTargetLine(uid, _doors[uid], newArrayPt, passvector);
- finalizeTargetLine(uid, tempTargetLine, newArrayPt, passvector);
-
- } else {
- Log->Write("ERROR:\t in addTarget: calling finalizeTargetLine");
- }
-// for (long int i = 0; i < _grid->GetnPoints(); ++i) {
-// if ((_gridCode[i] != OUTSIDE) && (_gridCode[i] != WALL) && (newArrayPt[i] == Point(0.0, 0.0) )) {
-// Log->Write("Mist");
-// }
-// }
- }
-#pragma omp critical(_uids)
- _uids.emplace_back(uid);
-}
-
-void UnivFFviaFM::addTarget(const int uid, Line* door, double* costarray, Point* gradarray){
- if (_doors.count(uid) == 0) {
- _doors.emplace(std::make_pair(uid, *door));
- }
- addTarget(uid, costarray, gradarray);
-}
-
-void UnivFFviaFM::addAllTargets() {
- for (auto uidmap : _doors) {
- addTarget(uidmap.first);
- }
-}
-
-void UnivFFviaFM::addAllTargetsParallel() {
- //Reason: freeing and reallocating takes time. We do not use already allocated memory, because we do not know if it
- // is shared memory. Maybe this is not neccessary - maybe reconsider. This way, it is safe. If this function
- // is called from a parallel region, we all go to hell.
- //free old memory
- for (auto memoryDBL : _costFieldWithKey) {
- if (memoryDBL.first == 0) continue; //do not free distancemap
- if (memoryDBL.second) delete[](memoryDBL.second);
- }
- for (auto memoryPt : _directionFieldWithKey) {
- if (memoryPt.first == 0) continue; //do not free walldirectionmap
- if (memoryPt.second) delete[](memoryPt.second);
- }
- //allocate new memory
- for (auto uidmap : _doors) {
- _costFieldWithKey[uidmap.first] = new double[_nPoints];
- if (_user == DISTANCE_MEASUREMENTS_ONLY) {
- _directionFieldWithKey[uidmap.first] = nullptr;
- }
- if (_user == DISTANCE_AND_DIRECTIONS_USED) {
- _directionFieldWithKey[uidmap.first] = new Point[_nPoints];
- }
- }
-
- //parallel region
-#pragma omp parallel
- {
-#pragma omp for
- for (signed int i = 0; i < _doors.size(); ++i) {
- auto doorPair = _doors.begin();
- std::advance(doorPair, i);
- addTarget(doorPair->first, _costFieldWithKey[doorPair->first], _directionFieldWithKey[doorPair->first]);
- }
- };
-}
-
-void UnivFFviaFM::addTargetsParallel(std::vector<int> wantedDoors) {
- //free old memory (but not the distancemap with key == 0)
- for (int targetUID : wantedDoors) {
- if ((targetUID != 0) && _costFieldWithKey.count(targetUID) && _costFieldWithKey[targetUID]) {
- delete[] _costFieldWithKey[targetUID];
- }
- if ((targetUID != 0) && _directionFieldWithKey.count(targetUID) && _directionFieldWithKey[targetUID]) {
- delete[] _directionFieldWithKey[targetUID];
- }
- }
- //allocate new memory
- for (int targetUID : wantedDoors) {
- _costFieldWithKey[targetUID] = new double[_nPoints];
- if (_user == DISTANCE_MEASUREMENTS_ONLY) {
- _directionFieldWithKey[targetUID] = nullptr;
- }
- if (_user == DISTANCE_AND_DIRECTIONS_USED) {
- _directionFieldWithKey[targetUID] = new Point[_nPoints];
- }
- }
-
- //parallel region
-#pragma omp parallel
- {
-#pragma omp for
- for (signed int i = 0; i < wantedDoors.size(); ++i) {
- auto doorUID = wantedDoors.begin();
- std::advance(doorUID, i);
- addTarget(*doorUID, _costFieldWithKey[*doorUID], _directionFieldWithKey[*doorUID]);
- }
- };
-}
-
-SubRoom** UnivFFviaFM::getSubRoomFF(){
- return _subrooms;
-}
-
-SubRoom* UnivFFviaFM::getSubRoom(const Point& pos){
- if (!_subrooms) return nullptr;
- if (!_grid->includesPoint(pos)) return nullptr;
-
- long key = _grid->getKeyAtPoint(pos);
- return _subrooms[key];
-}
-
-std::vector<int> UnivFFviaFM::getKnownDoorUIDs(){
- return _uids;
-}
-
-void UnivFFviaFM::setUser(int userArg) {
- _user=userArg;
-}
-
-void UnivFFviaFM::setMode(int modeArg) {
- _mode=modeArg;
-}
-
-void UnivFFviaFM::setSpeedMode(int speedModeArg) {
- _speedmode = speedModeArg;
- if (_speedmode == FF_PED_SPEED && !_speedFieldSelector[PED_SPEED]) {
- _speedFieldSelector[PED_SPEED] = new double[_nPoints];
- }
-}
-
-
-void UnivFFviaFM::writeFF(const std::string& filename, std::vector<int> targetID) {
- Log->Write("INFO: \tWrite Floorfield to file");
- Log->Write(filename);
- std::ofstream file;
-
- Log->Write("FloorfieldViaFM::writeFF(): writing to file %s: There are %d targets.", filename.c_str(), targetID.size());
-
- int numX = (int) ((_grid->GetxMax()-_grid->GetxMin())/_grid->Gethx());
- int numY = (int) ((_grid->GetyMax()-_grid->GetyMin())/_grid->Gethy());
- //int numTotal = numX * numY;
- //std::cerr << numTotal << " numTotal" << std::endl;
- //std::cerr << grid->GetnPoints() << " grid" << std::endl;
- file.open(_configuration->GetProjectRootDir()+filename);
-
- file << "# vtk DataFile Version 3.0" << std::endl;
- file << "Testdata: Fast Marching: Test: " << std::endl;
- file << "ASCII" << std::endl;
- file << "DATASET STRUCTURED_POINTS" << std::endl;
- file << "DIMENSIONS " <<
- std::to_string(_grid->GetiMax()) <<
- " " <<
- std::to_string(_grid->GetjMax()) <<
- " 1" << std::endl;
- file << "ORIGIN " << _grid->GetxMin() << " " << _grid->GetyMin() << " 0" << std::endl;
- file << "SPACING " << std::to_string(_grid->Gethx()) << " " << std::to_string(_grid->Gethy()) << " 1" << std::endl;
- file << "POINT_DATA " << std::to_string(_grid->GetnPoints()) << std::endl;
- file << "SCALARS GCode float 1" << std::endl;
- file << "LOOKUP_TABLE default" << std::endl;
- if (!_gridCode) {
- return;
- }
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- file << _gridCode[i] << std::endl;
- }
-
- if (_directionFieldWithKey[0]) {
- file << "VECTORS Dir2Wall float" << std::endl;
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- file << _directionFieldWithKey[0][i]._x << " " << _directionFieldWithKey[0][i]._y << " 0.0" << std::endl;
- }
-
- file << "SCALARS Dist2Wall float 1" << std::endl;
- file << "LOOKUP_TABLE default" << std::endl;
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- file << _costFieldWithKey[0][i] << std::endl; //@todo: change target to all dist2wall
- }
- }
-
- if (_subrooms) {
- file << "SCALARS SubroomPtr float 1" << std::endl;
- file << "LOOKUP_TABLE default" << std::endl;
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- if (_subrooms[i]) {
- file << _subrooms[i]->GetUID() << std::endl;
- } else {
- file << 0.0 << std::endl;
- }
- }
- }
-
- if (!targetID.empty()) {
- for (unsigned int iTarget = 0; iTarget < targetID.size(); ++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(), ' ', '_');
-
- if (!costarray) {
- continue;
- }
-
- file << "SCALARS CostTarget" << name << " float 1" << std::endl;
- file << "LOOKUP_TABLE default" << std::endl;
- for (long int i = 0; i < _grid->GetnPoints(); ++i) {
- file << costarray[i] << std::endl;
- }
-
- if (_directionFieldWithKey.count(targetID[iTarget]) == 0) {
- continue;
- }
-
- Point *gradarray = _directionFieldWithKey[targetID[iTarget]];
- if (gradarray == nullptr) {
- continue;
- }
-
-
- file << "VECTORS GradientTarget" << name << " float" << std::endl;
- for (int i = 0; i < _grid->GetnPoints(); ++i) {
- file << gradarray[i]._x << " " << gradarray[i]._y << " 0.0" << std::endl;
- }
-
-
- }
- }
- file.close();
-}
-
-//mode is argument, which should not be needed, the info is stored in members like speedmode, ...
-double UnivFFviaFM::getCostToDestination(const int destID, const Point& position, int mode) {
- assert(_grid->includesPoint(position));
- long int key = _grid->getKeyAtPoint(position);
- if ((_gridCode[key] == OUTSIDE) || (_gridCode[key] == WALL)) {
- //bresenham line (treppenstruktur) at middle and calculated centre of line are on different gridpoints
- //find a key that belongs domain (must be one left or right and second one below or above)
- if ((key+1 <= _grid->GetnPoints()) && (_gridCode[key+1] != OUTSIDE) && (_gridCode[key+1] != WALL)) {
- key = key+1;
- } else if ((key-1 >= 0) && (_gridCode[key-1] != OUTSIDE) && (_gridCode[key-1] != WALL)) {
- key = key - 1;
- } else if ((key >= _grid->GetiMax()) && (_gridCode[key-_grid->GetiMax()] != OUTSIDE) && (_gridCode[key-_grid->GetiMax()] != WALL)) {
- key = key - _grid->GetiMax();
- } else if ((key < _grid->GetnPoints()-_grid->GetiMax()) && (_gridCode[key+_grid->GetiMax()] != OUTSIDE) && (_gridCode[key+_grid->GetiMax()] != WALL)) {
- key = key + _grid->GetiMax();
- } else {
- Log->Write("ERROR:\t In getCostToDestination(3 args)");
- }
- }
- if (_costFieldWithKey.count(destID)==1 && _costFieldWithKey[destID]) {
- return _costFieldWithKey[destID][key];
- } else if (_directCalculation && _doors.count(destID) > 0) {
- _costFieldWithKey[destID] = new double[_nPoints];
- if (_user == DISTANCE_AND_DIRECTIONS_USED) {
- _directionFieldWithKey[destID] = new Point[_nPoints];
- } else {
- _directionFieldWithKey[destID] = nullptr;
- }
-
- addTarget(destID, _costFieldWithKey[destID], _directionFieldWithKey[destID]);
- return getCostToDestination(destID, position, mode);
- } else if (!_directCalculation && _doors.count(destID) > 0) {
- //omp critical
-#pragma omp critical(UnivFFviaFM_toDo)
- _toDo.emplace_back(destID);
- }
- return DBL_MAX;
-}
-
-double UnivFFviaFM::getCostToDestination(const int destID, const Point& position) {
- assert(_grid->includesPoint(position));
- long int key = _grid->getKeyAtPoint(position);
- if ((_gridCode[key] == OUTSIDE) || (_gridCode[key] == WALL)) {
- //bresenham line (treppenstruktur) getKeyAtPoint yields gridpoint next to edge, although position is on edge
- //find a key that belongs domain (must be one left or right and second one below or above)
- if ((key+1 <= _grid->GetnPoints()) && (_gridCode[key+1] != OUTSIDE) && (_gridCode[key+1] != WALL)) {
- key = key+1;
- } else if ((key-1 >= 0) && (_gridCode[key-1] != OUTSIDE) && (_gridCode[key-1] != WALL)) {
- key = key - 1;
- } else if ((key >= _grid->GetiMax()) && (_gridCode[key-_grid->GetiMax()] != OUTSIDE) && (_gridCode[key-_grid->GetiMax()] != WALL)) {
- key = key - _grid->GetiMax();
- } else if ((key < _grid->GetnPoints()-_grid->GetiMax()) && (_gridCode[key+_grid->GetiMax()] != OUTSIDE) && (_gridCode[key+_grid->GetiMax()] != WALL)) {
- key = key + _grid->GetiMax();
- } else {
- Log->Write("ERROR:\t In getCostToDestination(2 args)");
- }
- }
- if (_costFieldWithKey.count(destID)==1 && _costFieldWithKey[destID]) {
- return _costFieldWithKey[destID][key];
- } else if (_directCalculation && _doors.count(destID) > 0) {
- _costFieldWithKey[destID] = new double[_nPoints];
- if (_user == DISTANCE_AND_DIRECTIONS_USED) {
- _directionFieldWithKey[destID] = new Point[_nPoints];
- } else {
- _directionFieldWithKey[destID] = nullptr;
- }
-
- addTarget(destID, _costFieldWithKey[destID], _directionFieldWithKey[destID]);
- return getCostToDestination(destID, position);
- } else if (!_directCalculation && _doors.count(destID) > 0) {
-//omp critical
-#pragma omp critical(UnivFFviaFM_toDo)
- _toDo.emplace_back(destID);
- }
- return DBL_MAX;
-}
-
-double UnivFFviaFM::getDistanceBetweenDoors(const int door1_ID, const int door2_ID) {
- assert(_doors.count(door1_ID) != 0);
- assert(_doors.count(door2_ID) != 0);
-
- if (_costFieldWithKey.count(door1_ID)==1 && _costFieldWithKey[door1_ID]) {
- long int key = _grid->getKeyAtPoint(_doors.at(door2_ID).GetCentre());
- if (_gridCode[key] != door2_ID) {
- //bresenham line (treppenstruktur) getKeyAtPoint yields gridpoint next to edge, although position is on edge
- //find a key that belongs to door (must be one left or right and second one below or above)
- if (_gridCode[key+1] == door2_ID) {
- key = key+1;
- } else if (_gridCode[key-1] == door2_ID){
- key = key-1;
- } else {
- Log->Write("ERROR:\t In DistanceBetweenDoors");
- }
- }
- return _costFieldWithKey[door1_ID][key];
- } else if (_directCalculation && _doors.count(door1_ID) > 0) {
- _costFieldWithKey[door1_ID] = new double[_nPoints];
- if (_user == DISTANCE_AND_DIRECTIONS_USED) {
- _directionFieldWithKey[door1_ID] = new Point[_nPoints];
- } else {
- _directionFieldWithKey[door1_ID] = nullptr;
- }
-
- addTarget(door1_ID, _costFieldWithKey[door1_ID], _directionFieldWithKey[door1_ID]);
- return getDistanceBetweenDoors(door1_ID, door2_ID);
- } else if (!_directCalculation && _doors.count(door1_ID) > 0) {
-//omp critical
-#pragma omp critical(UnivFFviaFM_toDo)
- _toDo.emplace_back(door1_ID);
- }
- return DBL_MAX;
-}
-
-RectGrid* UnivFFviaFM::getGrid(){
- return _grid;
-}
-
-void UnivFFviaFM::getDirectionToUID(int destID, long int key, Point& direction, int mode){
- assert(key > 0 && key < _nPoints);
- if ((_gridCode[key] == OUTSIDE) || (_gridCode[key] == WALL)) {
- //bresenham line (treppenstruktur) getKeyAtPoint yields gridpoint next to edge, although position is on edge
- //find a key that belongs domain (must be one left or right and second one below or above)
- if ((key+1 <= _grid->GetnPoints()) && (_gridCode[key+1] != OUTSIDE) && (_gridCode[key+1] != WALL)) {
- key = key+1;
- } else if ((key-1 >= 0) && (_gridCode[key-1] != OUTSIDE) && (_gridCode[key-1] != WALL)) {
- key = key - 1;
- } else if ((key >= _grid->GetiMax()) && (_gridCode[key-_grid->GetiMax()] != OUTSIDE) && (_gridCode[key-_grid->GetiMax()] != WALL)) {
- key = key - _grid->GetiMax();
- } else if ((key < _grid->GetnPoints()-_grid->GetiMax()) && (_gridCode[key+_grid->GetiMax()] != OUTSIDE) && (_gridCode[key+_grid->GetiMax()] != WALL)) {
- key = key + _grid->GetiMax();
- } else {
- Log->Write("ERROR:\t In getDirectionToUID (4 args)");
- }
- }
- if (_directionFieldWithKey.count(destID)==1 && _directionFieldWithKey[destID]) {
- direction = _directionFieldWithKey[destID][key];
- } else if (_directCalculation && _doors.count(destID) > 0) {
- //free memory if needed
- if (_costFieldWithKey.count(destID) == 1 && _costFieldWithKey[destID]) {
- delete[] _costFieldWithKey[destID];
- }
- //allocate memory
- _costFieldWithKey[destID] = new double[_nPoints];
- if (_user == DISTANCE_AND_DIRECTIONS_USED) {
- _directionFieldWithKey[destID] = new Point[_nPoints];
- } else {
- _directionFieldWithKey[destID] = nullptr;
- }
-
- //calculate destID's fields and call function
- addTarget(destID, _costFieldWithKey[destID], _directionFieldWithKey[destID]);
- getDirectionToUID(destID, key, direction, mode);
- } else if (!_directCalculation && _doors.count(destID) > 0) {
-//omp critical
-#pragma omp critical(UnivFFviaFM_toDo)
- _toDo.emplace_back(destID);
- direction._x = 0.;
- direction._y = 0.;
- }
- return;
-}
-
-void UnivFFviaFM::getDirectionToUID(int destID, long int key, Point& direction){
- //assert(key > 0 && key < _nPoints);
- if(key <=0 || key>=_nPoints)
- {
- direction._x = 0.;
- direction._y = 0.;
- return;
- }
- if ((_gridCode[key] == OUTSIDE) || (_gridCode[key] == WALL)) {
- //bresenham line (treppenstruktur) getKeyAtPoint yields gridpoint next to edge, although position is on edge
- //find a key that belongs domain (must be one left or right and second one below or above)
- if ((key+1 <= _grid->GetnPoints()) && (_gridCode[key+1] != OUTSIDE) && (_gridCode[key+1] != WALL)) {
- key = key+1;
- } else if ((key-1 >= 0) && (_gridCode[key-1] != OUTSIDE) && (_gridCode[key-1] != WALL)) {
- key = key - 1;
- } else if ((key >= _grid->GetiMax()) && (_gridCode[key-_grid->GetiMax()] != OUTSIDE) && (_gridCode[key-_grid->GetiMax()] != WALL)) {
- key = key - _grid->GetiMax();
- } else if ((key < _grid->GetnPoints()-_grid->GetiMax()) && (_gridCode[key+_grid->GetiMax()] != OUTSIDE) && (_gridCode[key+_grid->GetiMax()] != WALL)) {
- key = key + _grid->GetiMax();
- } else {
- Log->Write("ERROR:\t In getDirectionToUID (3 args)");
- }
- }
- if (_directionFieldWithKey.count(destID)==1 && _directionFieldWithKey[destID]) {
- direction = _directionFieldWithKey[destID][key];
- } else if (_directCalculation && _doors.count(destID) > 0) {
- //free memory if needed
- if (_costFieldWithKey.count(destID) == 1 && _costFieldWithKey[destID]) {
- delete[] _costFieldWithKey[destID];
- }
- //allocate memory
- _costFieldWithKey[destID] = new double[_nPoints];
- if (_user == DISTANCE_AND_DIRECTIONS_USED) {
- _directionFieldWithKey[destID] = new Point[_nPoints];
- } else {
- _directionFieldWithKey[destID] = nullptr;
- }
-
- //calculate destID's fields and call function
- addTarget(destID, _costFieldWithKey[destID], _directionFieldWithKey[destID]);
- getDirectionToUID(destID, key, direction);
- } else if (!_directCalculation && _doors.count(destID) > 0) {
-//omp critical
-#pragma omp critical(UnivFFviaFM_toDo)
- _toDo.emplace_back(destID);
- direction._x = 0.;
- direction._y = 0.;
- }
- return;
-}
-
-void UnivFFviaFM::getDirectionToUID(int destID, const Point& pos, Point& direction, int mode) {
- getDirectionToUID(destID, _grid->getKeyAtPoint(pos), direction, mode);
-}
-
-void UnivFFviaFM::getDirectionToUID(int destID, const Point& pos,Point& direction) {
- getDirectionToUID(destID, _grid->getKeyAtPoint(pos), direction);
-}
-
-double UnivFFviaFM::getDistance2WallAt(const Point &pos) {
- if (_useWallDistances || (_speedmode == FF_WALL_AVOID)) {
- if (_costFieldWithKey[0]) {
- return _costFieldWithKey[0][_grid->getKeyAtPoint(pos)];
- }
- }
- return DBL_MAX;
-}
-
-void UnivFFviaFM::getDir2WallAt(const Point &pos, Point &p) {
- if (_useWallDistances || (_speedmode == FF_WALL_AVOID)) {
- if (_directionFieldWithKey[0]) {
- p = _directionFieldWithKey[0][_grid->getKeyAtPoint(pos)];
- }
- } else {
- p = Point(0.0, 0.0);
- }
-}
-
-/* Log:
- * todo:
- * - implement error treatment: extend fctns to throw errors and handle them
- * - error treatment will be advantageous, if calculation of FFs can be postponed
- * to be done in Simulation::RunBody, where
- * all cores are available
- * - (WIP) fill subroom* array with correct values
- * */
diff --git a/routing/ff_router_trips/UnivFFviaFM.h b/routing/ff_router_trips/UnivFFviaFM.h
deleted file mode 100644
index 53080ec1993fba5e32e6ae937c39868505dd41a3..0000000000000000000000000000000000000000
--- a/routing/ff_router_trips/UnivFFviaFM.h
+++ /dev/null
@@ -1,179 +0,0 @@
-//
-// Created by arne on 5/9/17.
-//
-/**
- * \file UnivFFviaFM.h
- * \date May 09, 2017
- * \version N/A (v0.8.x)
- * \copyright <2017-2020> Forschungszentrum Jülich GmbH. All rights reserved.
- *
- * \section License
- * This file is part of JuPedSim.
- *
- * JuPedSim is free software: you can redistribute it and/or modify
- * it under the terms of the GNU Lesser General Public License as published by
- * the Free Software Foundation, either version 3 of the License, or
- * any later version.
- *
- * JuPedSim is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU Lesser General Public License
- * along with JuPedSim. If not, see <http://www.gnu.org/licenses/>.
- *
- * \section Description
- * Implementation of classes for a reworked floorfield. A floorfield in general
- * yields a cost field to a specific goal and a correlated vectorfield for the
- * optimal path in terms of the cost value.
- *
- * Rework focused on a cleaner structure and less inheritance (no diamond) and
- * less workarounds.
- *
- *
- **/
-#ifndef JPSCORE_UNIVFFVIAFM_H
-#define JPSCORE_UNIVFFVIAFM_H
-
-#include <string>
-#include <vector>
-#include <map>
-#include <float.h>
-#include "../../general/Macros.h"
-
-class Pedestrian;
-class Room;
-class SubRoom;
-class Building;
-class Configuration;
-class Point;
-class RectGrid;
-class Line;
-
-enum SPEEDFIELD { //this enum is used as index in _speedFieldSelector
- INITIAL_SPEED=0, //homogen speed in walkable area, nealy zero in walls
- REDU_WALL_SPEED=1, //reduced wall speed
- PED_SPEED=2 //standing agents reduce speed, so that jams will be considered in ff
-};
-
-enum TARGETMODE {
- LINESEGMENT=0,
- CENTERPOINT
-};
-
-enum USERMODE {
- DISTANCE_MEASUREMENTS_ONLY,
- DISTANCE_AND_DIRECTIONS_USED
-};
-
-class CompareCost { //this class is used in std::priority_queue in UnivFFviaFM::calcFF
-public:
- CompareCost(double* costarray) : _costarray(costarray) {}
- bool operator() (const int a, const int b) const {
- return _costarray[a] > _costarray[b];
- }
-
-private:
- double* _costarray = nullptr;
-};
-
-class UnivFFviaFM {
-public:
- UnivFFviaFM(Room* a, Building* b, double c, double e, bool f);
- UnivFFviaFM(SubRoom* a, Building* b, double c, double e, bool f);
- UnivFFviaFM(Room* a, Configuration* const b, double hx, double wallAvoid, bool useWallDistances);
- UnivFFviaFM(Room* a, Configuration* const b, double hx, double wallAvoid, bool useWallDistances, std::vector<int> wantedDoors);
- UnivFFviaFM(SubRoom* sr, Configuration* const conf, double hx, double wallAvoid, bool useWallDistances);
- UnivFFviaFM(SubRoom* subRoomArg, Configuration* const confArg, double hx, double wallAvoid, bool useWallDistances, std::vector<int> wantedDoors);
- void create(std::vector<Line>& walls, std::map<int, Line>& doors, std::vector<int> targetUIDs, int mode,
- double spacing, double wallAvoidDist, bool useWallDistances);
- void recreateAllForQuickest();
- UnivFFviaFM() {};
- UnivFFviaFM(UnivFFviaFM&){};
- virtual ~UnivFFviaFM();
-
- void addTarget(const int uid, Line* door, double* costarray = nullptr, Point* gradarray = nullptr);
- void addTarget(const int uid, double* costarray = nullptr, Point* gradarray = nullptr);
- void addAllTargets();
- void addAllTargetsParallel();
- void addTargetsParallel(std::vector<int> wantedDoors);
- std::vector<int> getKnownDoorUIDs();
- void setUser(int userArg);
- void setMode(int modeArg);
- void setSpeedMode(int speedModeArg);
- SubRoom** getSubRoomFF();
- SubRoom* getSubRoom(const Point& pos);
-
- double getCostToDestination(const int destID, const Point& position, int mode);
- double getCostToDestination(const int destID, const Point& position);
- double getDistanceBetweenDoors(const int door1_ID, const int door2_ID);
- RectGrid* getGrid();
- virtual void getDirectionToUID(int destID, long int key, Point& direction, int mode);
- void getDirectionToUID(int destID, long int key, Point& direction);
- virtual void getDirectionToUID(int destID, const Point& pos, Point& direction, int mode);
- void getDirectionToUID(int destID, const Point& pos, Point& direction);
- double getDistance2WallAt(const Point& pos);
- void getDir2WallAt(const Point& pos, Point& p);
-
- void writeFF(const std::string&, std::vector<int> targetID);
-
- void createRectGrid(std::vector<Line>& walls, std::map<int, Line>& doors, double spacing);
- void processGeometry(std::vector<Line>&walls, std::map<int, Line>& doors);
- void markSubroom(const Point& insidePoint, SubRoom* const value);
- void createReduWallSpeed(double* reduWallSpeed);
- void createPedSpeed(Pedestrian* const * pedsArg, int nsize, int modechoice, double radius);
- void finalizeTargetLine(const int uid, const Line& tempTargetLine, Point* newArrayPt, Point& passvector);
-
- void drawLinesOnGrid(std::map<int, Line>& doors, int *const grid);
- template <typename T>
- void drawLinesOnGrid(std::vector<Line>& wallArg, T* const target, const T value);
- template <typename T>
- void drawLinesOnGrid(Line& line, T* const target, const T value);
-
- template <typename T>
- void drawLinesOnWall(std::vector<Line>& wallArg, T* const target, const T value);
- template <typename T>
- void drawLinesOnWall(Line& line, T* const target, const T value);
-
- void calcFF(double*, Point*, const double* const);
- void calcCost(const long int key, double* cost, Point* dir, const double* const speed);
- void calcDF(double*, Point*, const double* const);
- void calcDist(const long int key, double* cost, Point* dir, const double* const speed);
- inline double onesidedCalc(double xy, double hDivF);
- inline double twosidedCalc(double x, double y, double hDivF);
-
-private:
- Building* _building = nullptr;
- Configuration* _configuration = nullptr;
- int _room = -1; //not set
- int _mode = LINESEGMENT; //default
- int _user = DISTANCE_AND_DIRECTIONS_USED; //default
- int _speedmode = FF_HOMO_SPEED; //default
- int _scope = 0; //not set / unknown
- bool _directCalculation = true;
- RectGrid* _grid = nullptr;
- long int _nPoints = 0;
- std::vector<double*> _speedFieldSelector;
- int* _gridCode = nullptr;
- SubRoom* * _subrooms = nullptr; // this is an array (first asterisk) of pointers (second asterisk)
-
- double _wallAvoidDistance = 0.;
- bool _useWallDistances = false; //could be used in DirectionStrategy even if mode _speedmode is FF_HOMO_SPEED
-
- //the following maps are responsible for dealloc the arrays
- std::map<int, double*> _costFieldWithKey;
- std::map<int, Point*> _directionFieldWithKey;
-
- std::vector<int> _uids;
- std::map<int, Line> _doors;
- std::vector<int> _toDo;
-
- std::map<int, Point> _subroomUIDtoInsidePoint;
- std::map<int, SubRoom*> _subroomUIDtoSubRoomPtr;
- std::map<SubRoom*, Point> _subRoomPtrTOinsidePoint;
-
-};
-
-
-#endif //JPSCORE_UNIVFFVIAFM_H
diff --git a/routing/ff_router_trips/ffRouterTrips.cpp b/routing/ff_router_trips/ffRouterTrips.cpp
index dcecf4067b1b9a0736fe015f742124463c6fa5ed..36faf37433ae9b263e238642b03221f2300c1bba 100644
--- a/routing/ff_router_trips/ffRouterTrips.cpp
+++ b/routing/ff_router_trips/ffRouterTrips.cpp
@@ -48,8 +48,10 @@
#include <cfloat>
#include <algorithm>
#include "ffRouterTrips.h"
-//#include "FloorfieldViaFM.h"
+#include "FloorfieldViaFMTrips.h"
+#include "UnivFFviaFMTrips.h"
//#include "../../geometry/Building.h"
+#include "../../geometry/WaitingArea.h"
int FFRouterTrips::_cnt = 0;
@@ -85,7 +87,7 @@ FFRouterTrips::~FFRouterTrips()
delete _globalFF;
}
//delete localffs
- std::map<int, UnivFFviaFM*>::reverse_iterator delIter;
+ std::map<int, UnivFFviaFMTrips*>::reverse_iterator delIter;
for (delIter = _locffviafm.rbegin();
delIter != _locffviafm.rend();
++delIter) {
@@ -97,12 +99,15 @@ bool FFRouterTrips::Init(Building* building)
{
std::cout << "bool FFRouterTrips::Init(Building* building)" << std::endl;
_building = building;
+
if (_hasSpecificGoals) {
std::vector<int> goalIDs;
goalIDs.clear();
//get global field to manage goals (which are not in a subroom)
- _globalFF = new FloorfieldViaFM(building, 0.25, 0.25, 0.0, false, true);
+ _globalFF = new FloorfieldViaFMTrips(building, 0.25, 0.25, 0.0, false, true);
+ std::cout << std::endl;
for (auto &itrGoal : building->GetAllGoals()) {
+ std::cout << "Goal ID: " << itrGoal.second->GetId() << std::endl;
_globalFF->createMapEntryInLineToGoalID(itrGoal.first);
goalIDs.emplace_back(itrGoal.first);
}
@@ -119,6 +124,8 @@ bool FFRouterTrips::Init(Building* building)
_CroTrByUID.clear();
auto& allTrans = building->GetAllTransitions();
auto& allCross = building->GetAllCrossings();
+ auto& allGoals = building->GetAllGoals();
+
std::vector<std::pair<int, int>> roomAndCroTrVector;
roomAndCroTrVector.clear();
for (auto& pair:allTrans) {
@@ -142,6 +149,35 @@ bool FFRouterTrips::Init(Building* building)
if (room1) roomAndCroTrVector.emplace_back(std::make_pair(room1->GetID(), pair.second->GetUniqueID()));
}
}
+ for (auto& goalMap : allGoals) {
+ Goal* goal = goalMap.second;
+ if (WaitingArea* wa = dynamic_cast<WaitingArea*>(goal)) {
+ if (wa->isOpen()){
+ int roomID;
+ for (auto& room : _building->GetAllRooms()){
+ for (auto& subroom : room.second->GetAllSubRooms()){
+ if (subroom.second->IsInSubRoom(wa->GetCentroid())){
+ roomID = room.second->GetID();
+ }
+ }
+ }
+
+ for (const Wall wall : wa->GetAllWalls()) {
+ int uid = wall.GetUniqueID();
+ if (std::find(_allDoorUIDs.begin(), _allDoorUIDs.end(), uid) == _allDoorUIDs.end()) {
+ _allDoorUIDs.emplace_back(uid);
+ Crossing cross;
+ cross.SetPoint1(wall.GetPoint1());
+ cross.SetPoint2(wall.GetPoint2());
+ cross.SetCaption(wa->GetCaption());
+ _CroTrByUID.insert(std::make_pair(uid, &cross));
+ roomAndCroTrVector.emplace_back(roomID, uid);
+ }
+ }
+ }
+ }
+ }
+
//make unique
std::sort(_allDoorUIDs.begin(), _allDoorUIDs.end());
_allDoorUIDs.erase( std::unique(_allDoorUIDs.begin(),_allDoorUIDs.end()), _allDoorUIDs.end());
@@ -178,8 +214,8 @@ bool FFRouterTrips::Init(Building* building)
auto pairRoomIt = allRooms.begin();
std::advance(pairRoomIt, i);
- UnivFFviaFM *locffptr = nullptr;
- locffptr = new UnivFFviaFM(pairRoomIt->second.get(), building, 0.125, 0.0, false);
+ UnivFFviaFMTrips *locffptr = nullptr;
+ locffptr = new UnivFFviaFMTrips(pairRoomIt->second.get(), building, 0.125, 0.0, false);
locffptr->setUser(DISTANCE_MEASUREMENTS_ONLY);
locffptr->setMode(CENTERPOINT);
@@ -228,7 +264,7 @@ bool FFRouterTrips::Init(Building* building)
continue;
}
- UnivFFviaFM* locffptr = _locffviafm[rctIt->first];
+ UnivFFviaFMTrips* locffptr = _locffviafm[rctIt->first];
double tempDistance = locffptr->getDistanceBetweenDoors(rctIt->second, otherDoor.second);
if (tempDistance < locffptr->getGrid()->Gethx()) {
@@ -427,6 +463,7 @@ bool FFRouterTrips::ReInit()
int FFRouterTrips::FindExit(Pedestrian* ped)
{
+ std::cout << std::endl;
std::cout << "Ped[" << ped->GetID() << "] in (" << ped->GetRoomID() << ", " << ped->GetSubRoomID()
<< "/" << ped->GetSubRoomUID() << "): " << std::endl;
std::cout << "FinalDestination: " << ped->GetFinalDestination() << std::endl;
diff --git a/routing/ff_router_trips/ffRouterTrips.h b/routing/ff_router_trips/ffRouterTrips.h
index 5b4396bb8d07b5507c953913d88f8dd6e54d40df..6917125a7305d58cf10f53cfbe092133ce0fd7b8 100644
--- a/routing/ff_router_trips/ffRouterTrips.h
+++ b/routing/ff_router_trips/ffRouterTrips.h
@@ -63,8 +63,8 @@
#include "../Router.h"
#include "../../general/Macros.h"
#include "../../geometry/Building.h"
-#include "FloorfieldViaFM.h"
-#include "UnivFFviaFM.h"
+#include "./FloorfieldViaFMTrips.h"
+#include "./UnivFFviaFMTrips.h"
class Building;
class Pedestrian;
@@ -205,8 +205,8 @@ protected:
std::vector<int> _directionalEscalatorsUID;
std::vector<std::pair<int, int>> _penaltyList;
const Building* _building;
- std::map<int, UnivFFviaFM*> _locffviafm; // the actual type might be CentrePointLocalFFViaFM
- FloorfieldViaFM* _globalFF;
+ std::map<int, UnivFFviaFMTrips*> _locffviafm; // the actual type might be CentrePointLocalFFViaFM
+ FloorfieldViaFMTrips* _globalFF;
std::map<int, Transition*> _TransByUID;
std::map<int, Transition*> _ExitsByUID;
std::map<int, Crossing*> _CroTrByUID;