diff --git a/CMakeLists.txt b/CMakeLists.txt
index 340148b033f34a2309a9f5982682f9bf20532253..ba10b6f055486d571f022d8745e6054745a9e1a4 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -11,6 +11,7 @@
#--------------------------------------------------------------------------
cmake_minimum_required(VERSION 3.1 FATAL_ERROR)
+
set(CMAKE_LEGACY_CYGWIN_WIN32 0)
IF (POLICY CMP0054)
cmake_policy(SET CMP0054 NEW)
@@ -41,7 +42,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")
@@ -54,6 +55,8 @@ endif ()
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${warnings}")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${warnings}")
+message(STATUS "!BUILD_TESTING: " ${BUILD_TESTING})
+
if (NOT BUILD_TESTING)
set(BUILD_TESTING OFF) # test units & python tests are not generated.
endif (NOT BUILD_TESTING)
@@ -69,24 +72,43 @@ if (NOT CMAKE_EXPORT_COMPILE_COMMANDS)
set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
endif (NOT CMAKE_EXPORT_COMPILE_COMMANDS)
-set(CTEST_BUILD_FLAGS "-j")
+# 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)
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 --------------------
-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(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin)
+set(EXECUTABLE_OUTPUT_PATH "${CMAKE_SOURCE_DIR}/bin")
set(LIBRARY_OUTPUT_PATH ${CMAKE_SOURCE_DIR}/lib/${CMAKE_BUILD_TYPE})
set(CMAKE_TEST_DIR ${CMAKE_SOURCE_DIR}/Utest)
# Debug messages
@@ -100,134 +122,142 @@ 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 "")
-find_package(Git QUIET) # no need for this msg. It comes from cmake.findgit()
+# 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_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)
-else()
- message(STATUS "Not in a git repo")
- endif()
- string(REGEX REPLACE "\#"
- "" GIT_COMMIT_SUBJECT
- ${GIT_COMMIT_SUBJECT})
+# 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)
+
+
-# 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}/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)
+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
+)
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
@@ -235,42 +265,41 @@ endif (OPENMP_FOUND)
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS}")
#boost
-if(MSVC)
- find_package(Boost COMPONENTS QUIET)
-else()
- find_package(Boost COMPONENTS REQUIRED)
+find_package(Boost COMPONENTS timer chrono system filesystem unit_test_framework REQUIRED)
-endif()
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)
- 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")
+
+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")
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
@@ -278,6 +307,10 @@ 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
@@ -334,11 +367,23 @@ routing/ff_router/FloorfieldViaFM.cpp
#routing/ff_router/FFKit.cpp
routing/ff_router/UnivFFviaFM.cpp
-#global_shortest
+#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
+
+ #global_shortest
routing/global_shortest/AccessPoint.cpp
routing/global_shortest/GlobalRouter.cpp
routing/global_shortest/DTriangulation.cpp
+#global_shortest
+routing/global_shortest_trips/AccessPoint.cpp
+routing/global_shortest_trips/GlobalRouterTrips.cpp
+routing/global_shortest_trips/DTriangulation.cpp
+
#general
routing/DirectionStrategy.cpp
routing/Router.cpp
@@ -372,13 +417,21 @@ 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
+geometry/Trips.cpp
+routing/trips_router/TripsRouter.cpp
+
forms/jpscore.rc
-math/KrauszModel.cpp IO/Trips.cpp IO/Trips.h)
+math/KrauszModel.cpp )
set(THIRD_PARTY_SRC
tinyxml/tinystr.cpp
@@ -418,6 +471,7 @@ JPSfire/B_walking_speed/WalkingSpeed.cpp
JPSfire/C_toxicity_analysis/ToxicityAnalysis.cpp
JPSfire/C_toxicity_analysis/ToxicityOutputhandler.cpp
)
+
set(header_files
#floorfield
@@ -429,6 +483,15 @@ routing/ff_router/FloorfieldViaFM.h
#routing/ff_router/FFKit.h
routing/ff_router/UnivFFviaFM.h
+#floorfield
+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
+
#general
routing/DirectionStrategy.h
routing/DummyRouter.h
@@ -443,6 +506,11 @@ routing/global_shortest/GlobalRouter.h
routing/global_shortest/AccessPoint.h
routing/global_shortest/DTriangulation.h
+#global_shortest trips
+routing/global_shortest_trips/AccessPoint.h
+routing/global_shortest_trips/GlobalRouterTrips.h
+routing/global_shortest_trips/DTriangulation.h
+
#quickest
routing/quickest/QuickestPathRouter.h
@@ -470,6 +538,7 @@ 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
@@ -487,6 +556,8 @@ voronoi-boost/VoronoiPositionGenerator.h
mpi/LCGrid.h
+tinyxml/tinyxml.h
+tinyxml/tinystr.h
general/ArgumentParser.h
general/Configuration.h
@@ -523,9 +594,22 @@ 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
-)
+
+ geometry/Trips.h
+ routing/trips_router/TripsRouter.h
+
+ )
set(JPSFIRE_HDR
JPSfire/generic/FDSMesh.h
@@ -535,8 +619,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
@@ -553,7 +637,24 @@ routing/ai_router/perception/visibleenvironment.cpp
routing/ai_router/perception/cgalgeometry.cpp
routing/ai_router/perception/sign.cpp
routing/ai_router/Cortex.cpp
+
+#AI router trips
+routing/ai_router_trips/AIRouterTrips.cpp
+routing/ai_router_trips/BrainStorage.cpp
+routing/ai_router_trips/cognitiveMap/associations.cpp
+routing/ai_router_trips/cognitiveMap/connection.cpp
+routing/ai_router_trips/cognitiveMap/cognitivemap.cpp
+routing/ai_router_trips/cognitiveMap/landmark.cpp
+routing/ai_router_trips/cognitiveMap/region.cpp
+routing/ai_router_trips/cognitiveMap/landmarknetwork.cpp
+routing/ai_router_trips/cognitiveMap/internnavigationnetwork.cpp
+routing/ai_router_trips/perception/visualsystem.cpp
+routing/ai_router_trips/perception/visibleenvironment.cpp
+routing/ai_router_trips/perception/cgalgeometry.cpp
+routing/ai_router_trips/perception/sign.cpp
+routing/ai_router_trips/Cortex.cpp
)
+
SET(AIROUTER_HDR
#AI router
routing/ai_router/AIRouter.h
@@ -570,6 +671,22 @@ routing/ai_router/perception/visibleenvironment.h
routing/ai_router/perception/cgalgeometry.h
routing/ai_router/perception/sign.h
routing/ai_router/Cortex.h
+
+#AI router trips
+routing/ai_router_trips/AIRouterTrips.h
+routing/ai_router_trips/BrainStorage.h
+routing/ai_router_trips/cognitiveMap/associations.h
+routing/ai_router_trips/cognitiveMap/connection.cpp
+routing/ai_router_trips/cognitiveMap/cognitivemap.h
+routing/ai_router_trips/cognitiveMap/landmark.h
+routing/ai_router_trips/cognitiveMap/region.h
+routing/ai_router_trips/cognitiveMap/landmarknetwork.h
+routing/ai_router_trips/cognitiveMap/internnavigationnetwork.h
+routing/ai_router_trips/perception/visualsystem.h
+routing/ai_router_trips/perception/visibleenvironment.h
+routing/ai_router_trips/perception/cgalgeometry.h
+routing/ai_router_trips/perception/sign.h
+routing/ai_router_trips/Cortex.h
)
message(STATUS "AIROUTER: ${AIROUTER}")
diff --git a/IO/GeoFileParser.cpp b/IO/GeoFileParser.cpp
index 4dce1fdbb619d4b1354e3b19cf2f3f3bf2f69754..42407da6c875f9ec731359771d6ffa1fc1f44581 100644
--- a/IO/GeoFileParser.cpp
+++ b/IO/GeoFileParser.cpp
@@ -482,34 +482,36 @@ bool GeoFileParser::LoadRoutingInfo(Building* building)
std::cout << waitingArea->toString() << std::endl;
}
+// building->AddTrip(trips);
+ _configuration->GetRoutingEngine()->AddTrip(trips);
+
- std::cout << trips;
}
//load routes
- TiXmlNode* xTripsNode = xRootNode->FirstChild("routing")->FirstChild("routes");
-
- if (xTripsNode)
- for (TiXmlElement* trip = xTripsNode->FirstChildElement("route"); trip;
- trip = trip->NextSiblingElement("route")) {
-
- double id = xmltof(trip->Attribute("id"), -1);
- if (id==-1) {
- Log->Write("ERROR:\t id missing for trip");
- return false;
- }
- std::string sTrip = trip->FirstChild()->ValueStr();
- std::vector<std::string> vTrip;
- vTrip.clear();
-
- char* str = (char*) sTrip.c_str();
- char* p = strtok(str, ":");
- while (p) {
- vTrip.push_back(xmltoa(p));
- p = strtok(NULL, ":");
- }
- _configuration->GetRoutingEngine()->AddTrip(vTrip);
- }
+// TiXmlNode* xTripsNode = xRootNode->FirstChild("routing")->FirstChild("routes");
+//
+// if (xTripsNode)
+// for (TiXmlElement* trip = xTripsNode->FirstChildElement("route"); trip;
+// trip = trip->NextSiblingElement("route")) {
+//
+// double id = xmltof(trip->Attribute("id"), -1);
+// if (id==-1) {
+// Log->Write("ERROR:\t id missing for trip");
+// return false;
+// }
+// std::string sTrip = trip->FirstChild()->ValueStr();
+// std::vector<std::string> vTrip;
+// vTrip.clear();
+//
+// char* str = (char*) sTrip.c_str();
+// char* p = strtok(str, ":");
+// while (p) {
+// vTrip.push_back(xmltoa(p));
+// p = strtok(NULL, ":");
+// }
+// _configuration->GetRoutingEngine()->AddTrip(vTrip);
+// }
Log->Write("INFO:\tdone with loading extra routing information");
return true;
}
diff --git a/IO/GeoFileParser.h b/IO/GeoFileParser.h
index 07de711703763d13de7b00acc1892df5c38ae547..ef4023e6bed20bf82a60935570d3d4ea0ffebaf9 100644
--- a/IO/GeoFileParser.h
+++ b/IO/GeoFileParser.h
@@ -25,7 +25,7 @@
#include "../general/Configuration.h"
#include "../geometry/Building.h"
#include "../geometry/GeometryReader.h"
-#include "Trips.h"
+#include "../geometry/Trips.h"
//TODO: the class name GeoFileParser is misleading as the ``geometry'' file contains among others also relations (transitions)
//TODO: between geometries/rooms. Probably, EnvironmentFileParser would be better, still parts of the environment are
diff --git a/IO/IniFileParser.cpp b/IO/IniFileParser.cpp
index 45523695040df9fdb526d0e3019e18a54f920192..ade857ea97c239f5acc7a37f596d404bdd0d7904 100644
--- a/IO/IniFileParser.cpp
+++ b/IO/IniFileParser.cpp
@@ -40,10 +40,13 @@
#include "../math/GradientModel.h"
#include "../math/VelocityModel.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/ai_router/AIRouter.h"
#include "../routing/ff_router/ffRouter.h"
+#include "../routing/ff_router_trips/ffRouterTrips.h"
+#include "../routing/trips_router/TripsRouter.h"
/* https://stackoverflow.com/questions/38530981/output-compiler-version-in-a-c-program#38531037 */
std::string ver_string(int a, int b, int c) {
@@ -1134,6 +1137,13 @@ bool IniFileParser::ParseRoutingStrategies(TiXmlNode* routingNode, TiXmlNode* ag
Router *r = new GlobalRouter(id, ROUTING_GLOBAL_SHORTEST);
_config->GetRoutingEngine()->AddRouter(r);
}
+ else if ((strategy == "global_shortest_trips") &&
+ (std::find(usedRouter.begin(), usedRouter.end(), id) != usedRouter.end()) ) {
+ //pRoutingStrategies.push_back(make_pair(id, ROUTING_GLOBAL_SHORTEST));
+ Router *r = new GlobalRouterTrips(id, ROUTING_GLOBAL_SHORTEST);
+ _config->GetRoutingEngine()->AddRouter(r);
+ }
+
else if ((strategy == "quickest") &&
(std::find(usedRouter.begin(), usedRouter.end(), id) != usedRouter.end()) ) {
//pRoutingStrategies.push_back(make_pair(id, ROUTING_QUICKEST));
@@ -1169,14 +1179,30 @@ bool IniFileParser::ParseRoutingStrategies(TiXmlNode* routingNode, TiXmlNode* ag
exit(EXIT_FAILURE);
#endif
}
- else if ((strategy == "ff_global_shortest") &&
+ 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()) ) {
//pRoutingStrategies.push_back(make_pair(id, ROUTING_FF_GLOBAL_SHORTEST));
- Router *r = new FFRouter(id, ROUTING_FF_GLOBAL_SHORTEST, hasSpecificGoals, _config);
+ Router *r = new FFRouterTrips(id, ROUTING_FF_GLOBAL_SHORTEST, hasSpecificGoals, _config);
_config->GetRoutingEngine()->AddRouter(r);
if ((_exit_strat_number == 8) || (_exit_strat_number == 9)){
- Log->Write("\nINFO: \tUsing FF Global Shortest Router");
+ Log->Write("\nINFO: \tUsing FF Global Shortest Router Trips");
}
else {
Log->Write("\nWARNING: \tExit Strategy Number is not 8 or 9!!!");
@@ -1188,6 +1214,26 @@ bool IniFileParser::ParseRoutingStrategies(TiXmlNode* routingNode, TiXmlNode* ag
return false;
}
}
+ else if ((strategy == "ff_global_shortest") &&
+ (std::find(usedRouter.begin(), usedRouter.end(), id) != usedRouter.end()) ) {
+ //pRoutingStrategies.push_back(make_pair(id, ROUTING_FF_GLOBAL_SHORTEST));
+ Router *r = new FFRouter(id, ROUTING_FF_GLOBAL_SHORTEST, hasSpecificGoals, _config);
+ _config->GetRoutingEngine()->AddRouter(r);
+
+ if ((_exit_strat_number == 8) || (_exit_strat_number == 9)){
+ Log->Write("\nINFO: \tUsing FF Global Shortest Router");
+ }
+ else {
+ Log->Write("\nWARNING: \tExit Strategy Number is not 8 or 9!!!");
+ // config object holds default values, so we omit any set operations
+ }
+
+ ///Parsing additional options
+ if (!ParseFfRouterOps(e, ROUTING_FF_GLOBAL_SHORTEST)) {
+ return false;
+ }
+ }
+
else if ((strategy == "ff_local_shortest") &&
(std::find(usedRouter.begin(), usedRouter.end(), id) != usedRouter.end()) ) {
//pRoutingStrategies.push_back(make_pair(id, ROUTING_FF_GLOBAL_SHORTEST));
@@ -1221,6 +1267,12 @@ bool IniFileParser::ParseRoutingStrategies(TiXmlNode* routingNode, TiXmlNode* ag
return false;
}
}
+ else if ((strategy == "trips") &&
+ (std::find(usedRouter.begin(), usedRouter.end(), id) != usedRouter.end()) ) {
+ Router *r = new TripsRouter(id, ROUTING_TRIPS, _config);
+ _config->GetRoutingEngine()->AddRouter(r);
+ }
+
else if (std::find(usedRouter.begin(), usedRouter.end(), id) != usedRouter.end()) {
Log->Write("ERROR: \twrong value for routing strategy [%s]!!!\n",
strategy.c_str());
diff --git a/Simulation.cpp b/Simulation.cpp
index eac581c262b956c3ba26c1c2bc3876b55786fa5d..15b9befd15ffc50db58553e7e19434687b6466a2 100644
--- a/Simulation.cpp
+++ b/Simulation.cpp
@@ -299,6 +299,12 @@ void Simulation::UpdateRoutesAndLocations()
const map<int, Goal*>& goals = _building->GetAllGoals();
auto allRooms = _building->GetAllRooms();
+// for (signed int p = 0; p < allPeds.size(); ++p) {
+// Pedestrian* ped = allPeds[p];
+//
+// std::cout << "FinalDestination of [" << ped->GetID() << "] in (" << ped->GetRoomID() << ", " << ped->GetSubRoomID() << "/" << ped->GetSubRoomUID() << "): " << ped->GetFinalDestination() << std::endl;
+// }
+
#pragma omp parallel for shared(pedsToRemove, allRooms)
for (signed int p = 0; p < allPeds.size(); ++p) {
auto ped = allPeds[p];
@@ -307,8 +313,7 @@ void Simulation::UpdateRoutesAndLocations()
//set the new room if needed
if ((ped->GetFinalDestination() == FINAL_DEST_OUT)
- && (room->GetCaption() == "outside")) {
-
+ && (room->GetCaption() == "outside")) { //TODO Hier aendern fuer inside goals?
#pragma omp critical(Simulation_Update_pedsToRemove)
pedsToRemove.insert(ped);
} else if ((ped->GetFinalDestination() != FINAL_DEST_OUT)
diff --git a/Simulation.h b/Simulation.h
index 518f5e264ed9a04f7643958c1ad660c6765ca737..9449b4972b0a3590c102baf791ed785becfe5294 100644
--- a/Simulation.h
+++ b/Simulation.h
@@ -41,6 +41,7 @@
#include "math/OperationalModel.h"
#include "math/ODESolver.h"
#include "routing/global_shortest/GlobalRouter.h"
+#include "routing/global_shortest_trips/GlobalRouterTrips.h"
#include "routing/quickest/QuickestPathRouter.h"
#include "routing/DirectionStrategy.h"
#include "routing/RoutingEngine.h"
diff --git a/general/ArgumentParser.cpp b/general/ArgumentParser.cpp
index e6838806cfe7d8c5f0dfb742587c348512823dd7..f8fb75d26d3f613f933fee40e09f66f742a83022 100644
--- a/general/ArgumentParser.cpp
+++ b/general/ArgumentParser.cpp
@@ -43,6 +43,7 @@
#include "ArgumentParser.h"
#include "../pedestrian/AgentsParameters.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 "../IO/IniFileParser.h"
diff --git a/general/Macros.h b/general/Macros.h
index 1efd970f527945ca5bdebbe0c92c4ff7dab966bc..11ca4b777e50ae4dc5ef2d7e82f3be5d640c4561 100644
--- a/general/Macros.h
+++ b/general/Macros.h
@@ -128,10 +128,12 @@ enum RoutingStrategy {
ROUTING_FROM_FILE,
ROUTING_SMOKE,
ROUTING_AI,
+ ROUTING_AI_TRIPS,
ROUTING_FLOORFIELD,
ROUTING_FF_GLOBAL_SHORTEST,
ROUTING_FF_LOCAL_SHORTEST,
ROUTING_FF_QUICKEST,
+ ROUTING_TRIPS,
ROUTING_UNDEFINED =-1
};
diff --git a/IO/Trips.cpp b/geometry/Trips.cpp
similarity index 91%
rename from IO/Trips.cpp
rename to geometry/Trips.cpp
index c7d02c7bcb87c8ef9bc19308fe8ddd8823cbd94d..5f16cf0dd554c235fca5e61b6d6251180f95116c 100644
--- a/IO/Trips.cpp
+++ b/geometry/Trips.cpp
@@ -3,7 +3,6 @@
//
#include "Trips.h"
-//#include <random>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/discrete_distribution.hpp>
@@ -39,7 +38,8 @@ VertexItr Trips::getGoal(int id)
for (boost::tie(vi, vi_end) = boost::vertices(trips); vi != vi_end; ++vi) {
if(trips[*vi].getID() == id) return vi;
}
- return vi_end;}
+ return vi_end;
+}
void Trips::addConnection(int sourceId, int destinationId, EdgeProperty& weight)
{
@@ -87,12 +87,6 @@ int Trips::getNextGoal(int id)
}
// Random number generator
-// std::random_device rd;
-// std::mt19937 gen(rd());
-//
-// std::default_random_engine generator;
-// std::discrete_distribution<double> distribution (weights.begin(), weights.end());
-
boost::mt19937 gen;
boost::random::discrete_distribution<> dist(weights.begin(), weights.end());
diff --git a/IO/Trips.h b/geometry/Trips.h
similarity index 100%
rename from IO/Trips.h
rename to geometry/Trips.h
diff --git a/routing/Router.cpp b/routing/Router.cpp
index 1c74795ca5bb241172e50912aa5616df01f016ea..a3b8224d3f47c49e9f70dfb9ac286100331a4f27 100644
--- a/routing/Router.cpp
+++ b/routing/Router.cpp
@@ -94,3 +94,7 @@ RoutingStrategy Router::GetStrategy() const
return _strategy;
}
+void Router::SetTrips(const Trips& trips){
+ _trips = trips;
+ std::cout << _trips << std::endl;
+}
diff --git a/routing/Router.h b/routing/Router.h
index 8bb99544a02eaa5b7b323a5f2b6d7bd555dc89d8..132b8781cac29c6244e74385cf753370d485efca 100644
--- a/routing/Router.h
+++ b/routing/Router.h
@@ -34,7 +34,7 @@
#include <vector>
#include "../general/Macros.h"
-#include "../IO/Trips.h"
+#include "../geometry/Trips.h"
class Building;
class Pedestrian;
@@ -140,6 +140,8 @@ public:
*/
virtual bool ParseAdditionalParameters(){return true;};
+ void SetTrips(const Trips& trips);
+
};
#endif /* _ROUTING_H */
diff --git a/routing/RoutingEngine.cpp b/routing/RoutingEngine.cpp
index 10001c9331adefd22879fd99139e5aecdad67395..287797055546c37bb5913b8fd4643e849fed7dec 100644
--- a/routing/RoutingEngine.cpp
+++ b/routing/RoutingEngine.cpp
@@ -28,6 +28,7 @@
#include "RoutingEngine.h"
#include "../pedestrian/Pedestrian.h"
+#include "../geometry/Trips.h"
using namespace std;
@@ -68,19 +69,19 @@ void RoutingEngine::AddRouter(Router* router)
}
_routersCollection.push_back(router);
}
-
-const vector<string>& RoutingEngine::GetTrip(int index) const
-{
- if ((index >= 0) && (index < (int) _tripsCollection.size()))
- return _tripsCollection[index];
- else {
- char tmp[CLENGTH];
- sprintf(tmp, "ERROR: \tWrong 'index' [%d] > [%d] in Routing::GetTrip()",
- index, int(_tripsCollection.size()));
- Log->Write(tmp);
- exit(EXIT_FAILURE);
- }
-}
+//
+//const vector<string>& RoutingEngine::GetTrip(int index) const
+//{
+// if ((index >= 0) && (index < (int) _tripsCollection.size()))
+// return _tripsCollection[index];
+// else {
+// char tmp[CLENGTH];
+// sprintf(tmp, "ERROR: \tWrong 'index' [%d] > [%d] in Routing::GetTrip()",
+// index, int(_tripsCollection.size()));
+// Log->Write(tmp);
+// exit(EXIT_FAILURE);
+// }
+//}
const std::vector<Router*> RoutingEngine::GetAvailableRouters() const
{
@@ -110,9 +111,14 @@ Router* RoutingEngine::GetRouter(int id) const
return /*(Router*)*/ nullptr;
}
-void RoutingEngine::AddTrip(vector<string> trip)
+void RoutingEngine::AddTrip(Trips trip)
{
- _tripsCollection.push_back(trip);
+// _tripsCollection.push_back(trip);
+// _tripsCollection = trip;
+ for(Router* router:_routersCollection)
+ {
+ router->SetTrips(trip);
+ }
}
bool RoutingEngine::Init(Building* building)
diff --git a/routing/RoutingEngine.h b/routing/RoutingEngine.h
index 3b39d723c5ffc8ec59463f286ea5f5e28bbbb7b3..94fac8e77b44a56b5b4734c656da47e546490659 100644
--- a/routing/RoutingEngine.h
+++ b/routing/RoutingEngine.h
@@ -36,6 +36,7 @@
class Pedestrian;
+class Trips;
class RoutingEngine {
public:
@@ -60,14 +61,14 @@ public:
* Add a new trip to the system. Individual pedestrian can be assigned a particular trip.
* @param trip
*/
- void AddTrip(std::vector<std::string> trip);
+ void AddTrip(Trips trip);
- /**
- * Return a trip/route with the particular id
- * @param id, the trip id
- * @return the corresponding trip
- */
- const std::vector<std::string>& GetTrip(int id) const;
+// /**
+// * Return a trip/route with the particular id
+// * @param id, the trip id
+// * @return the corresponding trip
+// */
+// const std::vector<std::string>& GetTrip(int id) const;
/**
* @return all available routers
@@ -111,7 +112,7 @@ private:
/// collections of all routers used
std::vector<Router*> _routersCollection;
/// collection of all trips/routes
- std::vector<std::vector<std::string> >_tripsCollection;
+ Trips _tripsCollection;
};
#endif /* ROUTINGENGINE_H_ */
diff --git a/routing/ai_router_trips/BrainStorage.cpp b/routing/ai_router_trips/BrainStorage.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..21fdaa52bc993b335b3a9ab7e184bec295231d56
--- /dev/null
+++ b/routing/ai_router_trips/BrainStorage.cpp
@@ -0,0 +1,383 @@
+/**
+ * \file CognitiveMapStorage.cpp
+ * \date Feb 1, 2014
+ * \version v0.7
+ * \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
+ * Cognitive Map Storage
+ *
+ *
+ **/
+
+
+#include "BrainStorage.h"
+#include "cognitiveMap/internnavigationnetwork.h"
+
+#include "../../tinyxml/tinyxml.h"
+#include <memory>
+
+#include "../../geometry/Building.h"
+#include "../../pedestrian/Pedestrian.h"
+//#include "NavigationGraph.h"
+
+
+AIBrainStorage::AIBrainStorage(const Building * const b, const std::string &cogMapFiles, const std::string &signFiles)
+ : _building(b),_visibleEnv(VisibleEnvironment(b)),_cogMapFiles(cogMapFiles),_signFiles(signFiles)
+{
+
+ //Set signs
+ ParseSigns();
+
+}
+
+Cortex* AIBrainStorage::operator[] (BStorageKeyType key)
+{
+ BStorageType::iterator it = _corteces.find(key);
+ if(it == _corteces.end()) {
+ CreateCortex(key);
+ }
+
+ return _corteces[key].get();
+}
+
+void AIBrainStorage::ParseCogMap(BStorageKeyType ped)
+{
+
+ _regions.clear();
+
+ //create filename
+
+ int groupId = ped->GetGroup();
+ if (_cogMapFiles=="")
+ return;
+ std::string cMFileName=_cogMapFiles;
+ cMFileName.replace(cMFileName.size()-4,4,std::to_string(groupId)+".xml");
+
+ std::string cogMapFilenameWithPath = _building->GetProjectRootDir() + cMFileName;
+
+ Log->Write(cogMapFilenameWithPath);
+ TiXmlDocument doccogMap(cogMapFilenameWithPath);
+ if (!doccogMap.LoadFile()) {
+ Log->Write("ERROR: \t%s", doccogMap.ErrorDesc());
+ Log->Write("\t could not parse the cognitive map file");
+ Log->Write("Cognitive map not specified");
+ return;
+ }
+
+ TiXmlElement* xRootNode = doccogMap.RootElement();
+ if( ! xRootNode ) {
+ Log->Write("ERROR:\tRoot element does not exist");
+ Log->Write("Cognitive map not specified");
+ return;
+ }
+
+ if( xRootNode->ValueStr () != "cognitiveMap" ) {
+ Log->Write("ERROR:\tRoot element value is not 'cognitiveMap'.");
+ Log->Write("Cognitive map not specified");
+ return;
+ }
+ if(xRootNode->Attribute("unit"))
+ if(std::string(xRootNode->Attribute("unit")) != "m") {
+ Log->Write("ERROR:\tOnly the unit m (meters) is supported. \n\tYou supplied [%s]",xRootNode->Attribute("unit"));
+ Log->Write("Cognitive map not specified");
+ return;
+ }
+
+ double version = xmltof(xRootNode->Attribute("version"), -1);
+
+ if (version < std::stod(JPS_OLD_VERSION) ) {
+ Log->Write(" \tWrong geometry version!");
+ Log->Write(" \tOnly version >= %s supported",JPS_VERSION);
+ Log->Write(" \tPlease update the version of your geometry file to %s",JPS_VERSION);
+ Log->Write("Cognitive map not specified");
+ return;
+ }
+
+ //processing the regions node
+ TiXmlNode* xRegionsNode = xRootNode->FirstChild("regions");
+ if (!xRegionsNode) {
+ Log->Write("ERROR: \tCognitive map file without region definition!");
+ Log->Write("Cognitive map not specified");
+ return;
+ }
+
+ for(TiXmlElement* xRegion = xRegionsNode->FirstChildElement("region"); xRegion;
+ xRegion = xRegion->NextSiblingElement("region"))
+ {
+
+ std::string idR = xmltoa(xRegion->Attribute("id"), "-1");
+ std::string captionR = xmltoa(xRegion->Attribute("caption"));
+ std::string pxinmapR = xmltoa(xRegion->Attribute("px"),"-1");
+ std::string pyinmapR = xmltoa(xRegion->Attribute("py"),"-1");
+ std::string aR = xmltoa(xRegion->Attribute("a"),"-1");
+ std::string bR = xmltoa(xRegion->Attribute("b"),"-1");
+
+ AIRegion region (Point(std::stod(pxinmapR),std::stod(pyinmapR)));
+ region.SetId(std::stoi(idR));
+
+ region.SetCaption(captionR);
+ region.SetPosInMap(Point(std::stod(pxinmapR),std::stod(pyinmapR)));
+ region.SetA(std::stod(aR));
+ region.SetB(std::stod(bR));
+
+
+
+ //processing the landmarks node
+ TiXmlNode* xLandmarksNode = xRegion->FirstChild("landmarks");
+ if (!xLandmarksNode) {
+ Log->Write("ERROR: \tCognitive map file without landmark definition!");
+ Log->Write("No landmarks specified");
+ return;
+ }
+
+
+
+ for(TiXmlElement* xLandmark = xLandmarksNode->FirstChildElement("landmark"); xLandmark;
+ xLandmark = xLandmark->NextSiblingElement("landmark"))
+ {
+
+ std::string id = xmltoa(xLandmark->Attribute("id"), "-1");
+ std::string caption = xmltoa(xLandmark->Attribute("caption"));
+ std::string type = xmltoa(xLandmark->Attribute("type"),"-1");
+ std::string roomId = xmltoa(xLandmark->Attribute("subroom1_id"),"-1");
+ std::string pxreal = xmltoa(xLandmark->Attribute("pxreal"),"-1");
+ std::string pyreal = xmltoa(xLandmark->Attribute("pyreal"),"-1");
+ std::string pxinmap = xmltoa(xLandmark->Attribute("px"),"-1");
+ std::string pyinmap = xmltoa(xLandmark->Attribute("py"),"-1");
+ std::string a = xmltoa(xLandmark->Attribute("a"),"-1");
+ std::string b = xmltoa(xLandmark->Attribute("b"),"-1");
+
+ AILandmark landmark(Point(std::stod(pxreal),std::stod(pyreal)));
+
+ if (roomId=="NaN")
+ {
+ Log->Write("ERROR:\t Subroom Id is NaN!");
+ return;
+ }
+ landmark.SetId(std::stoi(id));
+ landmark.SetCaption(caption);
+ landmark.SetType(type);
+ landmark.SetRealPos(Point(std::stod(pxreal),std::stod(pyreal)));
+ landmark.SetPosInMap(Point(std::stod(pxinmap),std::stod(pyinmap)));
+ landmark.SetA(std::stod(a));
+ landmark.SetB(std::stod(b));
+ //landmark->SetRoom(_building->GetSubRoomByUID(std::stoi(roomId)));
+
+ //processing the rooms node
+ TiXmlNode* xAssociationsNode = xLandmark->FirstChild("associations");
+ if (!xAssociationsNode) {
+ Log->Write("Landmark with no association!");
+ continue;
+ }
+
+ for(TiXmlElement* xAsso = xAssociationsNode->FirstChildElement("association"); xAsso;
+ xAsso = xAsso->NextSiblingElement("association"))
+ {
+ std::string asso_id = xmltoa(xAsso->Attribute("id"), "-1");
+ std::string asso_caption = xmltoa(xAsso->Attribute("caption"), "0");
+ //std::string asso_type = xmltoa(xAsso->Attribute("type"),"-1");
+ std::string asso_x = xmltoa(xAsso->Attribute("px"),"-1");
+ std::string asso_y = xmltoa(xAsso->Attribute("py"),"-1");
+ std::string asso_a = xmltoa(xAsso->Attribute("a"),"-1");
+ std::string asso_b = xmltoa(xAsso->Attribute("b"),"-1");
+ int connection = std::stoi(xmltoa(xAsso->Attribute("connectedwith"),"-1"));
+ //std::string priority = xmltoa(xAsso->Attribute("priority"),"-1");
+
+ AILandmark assolandmark(Point(std::stod(asso_x),std::stod(asso_y)),
+ std::stod(asso_a),std::stod(asso_b));
+ assolandmark.SetId(std::stod(asso_id));
+ //std::cout << assolandmark.GetId() << std::endl;
+ assolandmark.SetCaption(asso_caption);
+ //assolandmark->AddConnection(std::stoi(connection));
+ //assolandmark->SetPriority(std::stod(priority));
+ bool connected=false;
+ if (connection==landmark.GetId())
+ connected=true;
+ landmark.AddAssociation(AIAssociation(&landmark,&assolandmark,connected));
+
+ region.AddLandmarkSubCs(assolandmark);
+
+
+ }
+
+ region.AddLandmark(landmark);
+ Log->Write("INFO:\tLandmark added!");
+
+ }
+
+ //processing the connections node
+ TiXmlNode* xConnectionsNode = xRegion->FirstChild("connections");
+ if (!xConnectionsNode) {
+ //Log->Write("ERROR: \tGeometry file without landmark definition!");
+ Log->Write("No connections specified");
+ //return;
+ }
+
+ else
+ {
+ for(TiXmlElement* xConnection = xConnectionsNode->FirstChildElement("connection"); xConnection;
+ xConnection = xConnection->NextSiblingElement("connection"))
+ {
+ std::string idC = xmltoa(xConnection->Attribute("id"), "-1");
+ std::string captionC = xmltoa(xConnection->Attribute("caption"));
+ std::string typeC = xmltoa(xConnection->Attribute("type"),"-1");
+ std::string landmark1 = xmltoa(xConnection->Attribute("landmark1_id"),"-1");
+ std::string landmark2 = xmltoa(xConnection->Attribute("landmark2_id"),"-1");
+
+
+ AIConnection connection = AIConnection(std::stoi(idC),captionC,typeC,std::stoi(landmark1),std::stoi(landmark2));
+
+ region.AddConnection(connection);
+ Log->Write("INFO:\tConnection added!");
+ }
+ }
+
+ _regions.push_back(region);
+ Log->Write("INFO:\tRegion added!");
+ }
+}
+
+void AIBrainStorage::CreateCortex(BStorageKeyType ped)
+{
+
+ //todo: the possibility to have more then one creator.
+
+ _corteces.insert(std::make_pair(ped, std::unique_ptr<Cortex>(new Cortex(_building,
+ ped,
+ &_visibleEnv,
+ &_roominternalNetworks))));// creator->CreateCognitiveMap(ped)));
+
+
+
+
+ // load cogmap and do first cognitive step
+ ParseCogMap(ped);
+ _corteces[ped]->GetCognitiveMap().AddRegions(_regions);
+ _corteces[ped]->GetCognitiveMap().InitLandmarkNetworksInRegions();
+ _corteces[ped]->GetCognitiveMap().FindMainDestination();
+ //debug
+ //cognitive_maps[ped]->GetNavigationGraph()->WriteToDotFile(building->GetProjectRootDir());
+}
+
+void AIBrainStorage::InitInternalNetwork(const SubRoom* sub_room)
+{
+
+ _roominternalNetworks.emplace(sub_room,ptrIntNetwork(new InternNavigationNetwork(sub_room)));
+
+ for (Crossing* crossing:sub_room->GetAllCrossings())
+ {
+ _roominternalNetworks[sub_room]->AddVertex(crossing);
+ }
+
+ for (Transition* transition:sub_room->GetAllTransitions())
+ {
+ _roominternalNetworks[sub_room]->AddVertex(transition);
+ }
+
+ for (Hline* hline:sub_room->GetAllHlines())
+ {
+ _roominternalNetworks[sub_room]->AddVertex(hline);
+ }
+
+ _roominternalNetworks[sub_room]->EstablishConnections();
+
+}
+
+void AIBrainStorage::ParseSigns() {
+
+ if (_signFiles=="")
+ {
+ Log->Write("INFO: \tNo signs specified \n");
+ return;
+ }
+ std::string signFilenameWithPath = _building->GetProjectRootDir() + _signFiles;
+
+ Log->Write("INFO: \tRead Signs from " + signFilenameWithPath);
+ TiXmlDocument docSigns(signFilenameWithPath);
+ if (!docSigns.LoadFile()) {
+ Log->Write("WARNING: \t%s", docSigns.ErrorDesc());
+ Log->Write("\t could not parse the sign file");
+ Log->Write("No signs specified");
+ return;
+ }
+
+ TiXmlElement *xRootNode = docSigns.RootElement();
+ if (!xRootNode) {
+ Log->Write("WARNING:\tRoot element does not exist");
+ Log->Write("No signs specified");
+ return;
+ }
+
+ if (xRootNode->ValueStr() != "signage") {
+ Log->Write("WARNING:\tRoot element value is not 'signage'.");
+ Log->Write("No signs specified");
+ return;
+ }
+ if (xRootNode->Attribute("unit"))
+ if (std::string(xRootNode->Attribute("unit")) != "m") {
+ Log->Write("WARNING:\tOnly the unit m (meters) is supported. \n\tYou supplied [%s]",
+ xRootNode->Attribute("unit"));
+ Log->Write("No signs specified");
+ return;
+ }
+
+ double version = xmltof(xRootNode->Attribute("version"), -1);
+
+ if (version < std::stod(JPS_OLD_VERSION)) {
+ Log->Write("WARNING: \tWrong file version!");
+ Log->Write(" \tOnly version >= %s supported", JPS_VERSION);
+ Log->Write(" \tPlease update the version of your file to %s", JPS_VERSION);
+ Log->Write("No signs specified");
+ return;
+ }
+
+ //processing the regions node
+ TiXmlNode *xSignsNode = xRootNode->FirstChild("signs");
+ if (!xSignsNode) {
+ Log->Write("WARNING: \tSignage file without signs!");
+ Log->Write("No signs specified");
+ return;
+ }
+
+
+ for (TiXmlElement *xSign = xSignsNode->FirstChildElement("sign"); xSign;
+ xSign = xSign->NextSiblingElement("sign")) {
+
+ std::string id = xmltoa(xSign->Attribute("id"), "-1");
+ std::string room_id = xmltoa(xSign->Attribute("room_id"), "-1");
+ std::string px = xmltoa(xSign->Attribute("px"), "-1");
+ std::string py = xmltoa(xSign->Attribute("py"), "-1");
+ std::string alpha = xmltoa(xSign->Attribute("alpha"), "-1");
+ std::string alphaPointing = xmltoa(xSign->Attribute("alphaPointing"), "-1");
+
+ Log->Write("Sign read!");
+
+ _visibleEnv.AddSign(_building->GetAllRooms().at(std::stod(room_id)).get(),Sign(std::stod(id), std::stod(room_id), Point(std::stod(px), std::stod(py)),
+ std::stod(alpha), std::stod(alphaPointing)));
+
+ }
+
+}
+
+void AIBrainStorage::DeleteCortex(BStorageKeyType ped)
+{
+ _corteces.erase(ped);
+
+}
diff --git a/routing/ai_router_trips/BrainStorage.h b/routing/ai_router_trips/BrainStorage.h
new file mode 100644
index 0000000000000000000000000000000000000000..a01170eaf1ab31e38a716027319fc998d7682331
--- /dev/null
+++ b/routing/ai_router_trips/BrainStorage.h
@@ -0,0 +1,92 @@
+/**
+ * \file CognitiveMapStorage.h
+ * \date Feb 1, 2014
+ * \version v0.7
+ * \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
+ * Cognitive Map Storage
+ *
+ *
+ **/
+
+
+#ifndef BRAINSTORAGE_H_
+#define BRAINSTORAGE_H_
+
+#include <unordered_map>
+#include <vector>
+#include "./cognitiveMap/cognitivemap.h"
+#include "./perception/visibleenvironment.h"
+#include "Cortex.h"
+
+class Building;
+class Pedestrian;
+class InternNavigationNetwork;
+
+
+typedef const Pedestrian * BStorageKeyType;
+typedef std::unique_ptr<Cortex> BStorageValueType;
+typedef std::unordered_map<BStorageKeyType, BStorageValueType> BStorageType;
+
+
+
+/**
+ * @brief Brain Storage
+ *
+ * Cares about corteces, creation and delivery
+ *
+ */
+class AIBrainStorage {
+public:
+ AIBrainStorage(const Building * const b, const std::string& cogMapFiles="", const std::string& signFiles="");
+
+
+ Cortex* operator[] (BStorageKeyType key);
+
+ void DeleteCortex(BStorageKeyType ped);
+
+
+private:
+ const Building * const _building;
+ BStorageType _corteces;
+
+
+ //perception
+ //Complete environment
+ VisibleEnvironment _visibleEnv;
+
+ //cognitive map
+ std::vector<AIRegion> _regions;
+ std::string _cogMapFiles;
+ std::string _signFiles;
+
+ //Cortex
+ void CreateCortex(BStorageKeyType ped);
+ void ParseCogMap(BStorageKeyType ped);
+ void ParseSigns();
+
+
+ // internal graph network in every room (for locomotion purposes)
+ void InitInternalNetwork(const SubRoom *sub_room);
+ std::unordered_map<const SubRoom*,ptrIntNetwork> _roominternalNetworks;
+
+};
+
+#endif /* BRAINSTORAGE_H_ */
diff --git a/routing/ai_router_trips/Cortex.cpp b/routing/ai_router_trips/Cortex.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1f1fe31ece9a0a0f833cf36b75bdbb79e016532c
--- /dev/null
+++ b/routing/ai_router_trips/Cortex.cpp
@@ -0,0 +1,554 @@
+#include <cstdlib>
+#include <cmath>
+#include "Cortex.h"
+#include "../../pedestrian/Pedestrian.h"
+
+Cortex::Cortex()
+{
+
+}
+
+Cortex::Cortex(const Building *b, const Pedestrian *ped, const VisibleEnvironment *env, std::unordered_map<const SubRoom *, ptrIntNetwork> *roominternalNetworks)
+{
+ _b=b;
+ _ped=ped;
+ _wholeEnvironment=env;
+ _intNetworks=roominternalNetworks;
+
+ _perceptionAbilities=VisualSystem(_b,_ped,_wholeEnvironment);
+ _cMap=AICognitiveMap(_ped,&_perceptionAbilities);
+ _lastBestVisNavLine=NavLine(Line(Point(0,0),Point(0,0)));
+
+ if (_b->GetConfig()->GetRandomNumberGenerator()->GetRandomRealBetween0and1()<=0.11)
+ _statPreferToGoDown=true;
+ else
+ _statPreferToGoDown=false;
+
+
+}
+
+AICognitiveMap &Cortex::GetCognitiveMap()
+{
+ return _cMap;
+}
+
+VisualSystem &Cortex::GetPerceptionAbilities()
+{
+ return _perceptionAbilities;
+}
+
+const NavLine* Cortex::GetNextNavLine(const NavLine* nextTarget)
+{
+ const SubRoom* currentSubRoom = _b->GetSubRoomByUID(_ped->GetSubRoomUID());
+
+ _currentIntNetwork=*(_intNetworks->operator [](currentSubRoom));
+
+ return _currentIntNetwork.GetNextNavLineOnShortestPathToTarget(_ped->GetPos(),nextTarget);
+}
+
+bool Cortex::HlineReached() const
+{
+ const SubRoom* currentSubRoom = _b->GetSubRoomByUID(_ped->GetSubRoomUID());
+
+ for (Hline* hline:currentSubRoom->GetAllHlines())
+ {
+ // if distance hline to ped lower than 1 m
+ if (hline->DistTo(_ped->GetPos())<1)
+ {
+ return true;
+ }
+ }
+
+ return false;
+}
+
+
+std::vector<const NavLine*> Cortex::SortConGeneralKnow(const std::vector<const NavLine*> &navLines)
+{
+
+ //std::cout << dis(_randomEngine) << std::endl;
+
+ std::vector<const NavLine*> sortedNavLines;
+ std::vector<const NavLine*> sortedNavLinesOutside;
+ std::vector<const NavLine*> sortedNavLinesLobbies;
+ std::vector<const NavLine*> sortedNavLinesCirculs;
+ std::vector<const NavLine*> sortedNavLinesCommons;
+ std::vector<const NavLine*> sortedNavLinesStairsUp;
+ std::vector<const NavLine*> sortedNavLinesStairs;
+ std::vector<const NavLine*> sortedNavLinesStairsDown;
+ std::vector<const NavLine*> sortedNavLinesTrains;
+
+ for (const NavLine* navLine:navLines)
+ {
+
+ // navLine a transition?
+ const Transition* transition = _perceptionAbilities.IsTransition(navLine);
+
+ if (!transition)
+ {
+
+ if (_ped->GetRoomID()==11)
+ {
+ if (IsInsideRectangle(navLine->GetCentre(),Point(7.52,52.19),Point(11.31,46.0))
+ || IsInsideRectangle(navLine->GetCentre(),Point(20.25,68.38),Point(27.2,64.91))
+ || IsInsideRectangle(navLine->GetCentre(),Point(4.48,79.5),Point(13.78,73.14))
+ || IsInsideRectangle(navLine->GetCentre(),Point(-7.06,54.69),Point(-1.48,50.48)))
+ {
+ sortedNavLinesStairsDown.push_back(navLine);
+ continue;
+ }
+ }
+
+
+ // is line on same floor? If yes: subRoom!=nullptr
+ const SubRoom* subRoom=_perceptionAbilities.IsInSubRoom(navLine->GetCentre());
+ if (!subRoom)
+ sortedNavLinesStairsDown.push_back(navLine);
+ else if (subRoom->GetType()=="Lobby")
+ sortedNavLinesLobbies.push_back(navLine);
+ else if (subRoom->GetType()=="Corridor")
+ sortedNavLinesCirculs.push_back(navLine);
+ else if (subRoom->GetType()=="train")
+ sortedNavLinesTrains.push_back(navLine);
+ else
+ sortedNavLinesCommons.push_back(navLine);
+ }
+ else if (transition->IsExit())
+ {
+ sortedNavLinesOutside.push_back(transition);
+ }
+
+ else
+ {
+ if (transition->GetOtherRoom(_ped->GetRoomID())->GetZPos()>_b->GetRoom(_ped->GetRoomID())->GetZPos())
+ {
+ sortedNavLinesStairsUp.push_back(transition);
+ }
+
+ else
+ {
+ sortedNavLinesStairsDown.push_back(transition);
+
+ }
+
+ }
+ }
+
+
+
+ //settings for underground station!
+ sortedNavLines.insert(std::end(sortedNavLines), std::begin(sortedNavLinesOutside), std::end(sortedNavLinesOutside));
+ sortedNavLines.insert(std::end(sortedNavLines), std::begin(sortedNavLinesLobbies), std::end(sortedNavLinesLobbies));
+ sortedNavLines.insert(std::end(sortedNavLines), std::begin(sortedNavLinesStairsUp), std::end(sortedNavLinesStairsUp));
+ sortedNavLines.insert(std::end(sortedNavLines), std::begin(sortedNavLinesStairs), std::end(sortedNavLinesStairs));
+ sortedNavLines.insert(std::end(sortedNavLines), std::begin(sortedNavLinesCirculs), std::end(sortedNavLinesCirculs));
+ sortedNavLines.insert(std::end(sortedNavLines), std::begin(sortedNavLinesCommons), std::end(sortedNavLinesCommons));
+ sortedNavLines.insert(std::end(sortedNavLines), std::begin(sortedNavLinesStairsDown), std::end(sortedNavLinesStairsDown));
+ sortedNavLines.insert(std::end(sortedNavLines), std::begin(sortedNavLinesTrains), std::end(sortedNavLinesTrains));
+
+
+ return sortedNavLines;
+}
+
+
+
+const NavLine Cortex::FindApprCrossing(const std::vector<NavLine>& navLines)
+{
+
+ // suffle navLines randomly -> if no factor influences ranking of navLines a specific navLine is chosen randomly.
+ std::vector<NavLine> shuffledNavLines=navLines;
+
+ std::shuffle(shuffledNavLines.begin(),shuffledNavLines.end(),_b->GetConfig()->GetRandomNumberGenerator()->GetRandomEngine());
+
+ // knowledge generalized <- signage <- Cognitive Map <- Random shuffle
+ const NavLine* nextNvLine=SortConGeneralKnow(SortConSignage(_cMap.SortConLeastAngle(_cMap.GetNextTarget(),shuffledNavLines,_lastBestVisNavLine))).front();
+
+ _cMap.SetBestChoice(*nextNvLine);
+ if (!nextNvLine)
+ throw(std::invalid_argument("No NavLine found. Ped is trapped"));
+ else
+ return *nextNvLine;
+
+}
+
+const NavLine Cortex::FindApprVisibleCrossing(const NavLine& navLine, const std::vector<NavLine> &navLines)
+{
+ // if transition or outside is in sight
+ Point normal = Point(-(navLine.GetPoint2()-navLine.GetPoint1())._y,(navLine.GetPoint2()-navLine.GetPoint1())._x);
+ normal=normal/normal.Norm();
+ Point dirVec1 = navLine.GetCentre()+normal*0.5-_ped->GetPos();
+ Point dirVec2 = navLine.GetCentre()-normal*0.5-_ped->GetPos();
+
+ std::vector<int> transIDs= _b->GetRoom(_ped->GetRoomID())->GetAllTransitionsIDs();
+
+ for (int transID: transIDs)
+ {
+ if (*(_b->GetTransitionByUID(transID))==navLine)
+ {
+ Line targetToHead(Point(0.0,0.0),Point(0.0,0.0));
+ if (dirVec1.Norm() > dirVec2.Norm())
+ targetToHead=Line(navLine.GetPoint1()+normal*0.5,navLine.GetPoint2()+normal*0.5);
+ else
+ targetToHead=Line(navLine.GetPoint1()-normal*0.5,navLine.GetPoint2()-normal*0.5);
+
+ const NavLine visibleNavLine(targetToHead);
+ return visibleNavLine;
+ }
+ }
+
+ // not a transition
+ if (_lastBestVisNavLine.GetPoint1()==Point(0,0) && _lastBestVisNavLine.GetPoint2()==Point(0,0))
+ {
+ double minDistance=FLT_MAX;
+ for (const NavLine& candidate:navLines)
+ {
+ if (candidate.DistTo(navLine.GetCentre())<=minDistance)
+ {
+ minDistance=candidate.DistTo(navLine.GetCentre());
+ _lastBestVisNavLine=candidate;
+ }
+ }
+ return TargetToHead(&_lastBestVisNavLine);
+ }
+
+ bool statFoundBestChoice=false;
+
+ for (const NavLine& candidate:navLines)
+ {
+ Point vec1=candidate.GetPoint1()-_lastBestVisNavLine.GetPoint1();
+ Point vec2=candidate.GetPoint2()-_lastBestVisNavLine.GetPoint2();
+ Point vec3=candidate.GetPoint2()-_lastBestVisNavLine.GetPoint1();
+ Point vec4=candidate.GetPoint1()-_lastBestVisNavLine.GetPoint2();
+ if (vec1.Norm()<0.1 || vec2.Norm()<0.1 || vec3.Norm()<0.1 || vec4.Norm()<0.1)
+ {
+ _lastBestVisNavLine=candidate;
+ statFoundBestChoice=true;
+ }
+ }
+
+ double pedAlpha = std::atan2(_ped->GetV()._x,_ped->GetV()._y);
+ bool statFound=false;
+ if (statFoundBestChoice)
+ {
+
+
+ double minDistance=_lastBestVisNavLine.DistTo(navLine.GetCentre());
+ for (const NavLine& candidate:navLines)
+ {
+ Point vecPedNavLine = candidate.GetCentre()-_ped->GetPos();
+ double navAlpha=std::atan2(vecPedNavLine._x,vecPedNavLine._y);
+ double diffAlpha=std::fabs(navAlpha-pedAlpha);
+ if (diffAlpha>M_PI)
+ diffAlpha=2*M_PI-diffAlpha;
+ if (diffAlpha<M_PI/3.0 && candidate.DistTo(navLine.GetCentre())<=minDistance*0.8)
+ {
+ minDistance=candidate.DistTo(navLine.GetCentre());
+ _lastBestVisNavLine=candidate;
+ statFound=true;
+ }
+ }
+
+
+ if (!statFound)
+ {
+ for (const NavLine& candidate:navLines)
+ {
+ if (candidate.DistTo(navLine.GetCentre())<=minDistance*0.2)
+ {
+ minDistance=candidate.DistTo(navLine.GetCentre());
+ _lastBestVisNavLine=candidate;
+ }
+ }
+ }
+ }
+ else
+ {
+ double minDistance=FLT_MAX;
+ for (const NavLine& candidate:navLines)
+ {
+ Point vecPedNavLine = candidate.GetCentre()-_ped->GetPos();
+ double navAlpha=std::atan2(vecPedNavLine._x,vecPedNavLine._y);
+ //Log->Write(std::to_string(std::fabs(navAlpha-pedAlpha)*180/M_PI));
+ double diffAlpha=std::fabs(navAlpha-pedAlpha);
+ if (diffAlpha>M_PI)
+ diffAlpha=2*M_PI-diffAlpha;
+ if (diffAlpha<M_PI/3.0 && candidate.DistTo(navLine.GetCentre())<=minDistance)
+ {
+ minDistance=candidate.DistTo(navLine.GetCentre());
+ _lastBestVisNavLine=candidate;
+ statFound=true;
+ }
+ }
+
+ if (!statFound)
+ {
+ for (const NavLine& candidate:navLines)
+ {
+ if (candidate.DistTo(navLine.GetCentre())<=minDistance)
+ {
+ minDistance=candidate.DistTo(navLine.GetCentre());
+ _lastBestVisNavLine=candidate;
+ }
+ }
+ }
+ }
+
+
+ return TargetToHead(&_lastBestVisNavLine);
+}
+
+const NavLine Cortex::TargetToHead(const NavLine* visibleCrossing) const
+{
+ Point P1 = visibleCrossing->GetPoint1();
+ Point P2 = visibleCrossing->GetPoint2();
+ Point dirVector=P2-P1;
+ dirVector=dirVector/dirVector.Norm();
+ Point normalVector = Point(-dirVector._y,dirVector._x);
+
+ Point distVec1=P1-_ped->GetPos();
+ Point distVec2=P2-_ped->GetPos();
+
+ Point NavLineP1;
+ Point NavLineP2;
+ if (distVec1.Norm()<distVec2.Norm())
+ {
+ NavLineP1=P1;
+ NavLineP2=P1-normalVector*1.5;
+ }
+ else
+ {
+ NavLineP1=P2;
+ NavLineP2=P2-normalVector*1.5;
+ }
+
+ const NavLine navLine(Line(NavLineP1,NavLineP2));
+
+ return navLine;
+}
+
+
+std::vector<const NavLine *> Cortex::SortConSignage(const std::vector<const NavLine *> &navLines)
+{
+
+ std::vector<const Sign*> perceivedSigns = _perceptionAbilities.TryToDetectSigns();
+ std::vector<const Sign*> cSigns;
+
+ for (const Sign* sign:perceivedSigns)
+ {
+ if (std::find(_notUsedSigns.begin(),_notUsedSigns.end(),sign)==_notUsedSigns.end())
+ cSigns.push_back(sign);
+ }
+
+ if (cSigns.empty())
+ return navLines;
+
+
+ std::vector<const NavLine*> sortedNavLines;
+ //determine decisive sign (sign which has the recent instruction)
+
+ const Sign* decisiveSign=nullptr;
+ if (cSigns.size()>1)
+ decisiveSign=DetermineDecisiveSign(cSigns);
+ else
+ decisiveSign=cSigns.front();
+
+ if (!decisiveSign)
+ return navLines;
+
+ Point vecToNavLine;
+ double angleVecToNavLine;
+ double anglediff;
+
+ // find navLine which is most appropriately to follow sign instruction
+
+ for (const NavLine* navLine:navLines)
+ {
+
+ vecToNavLine = navLine->GetCentre()-decisiveSign->GetPos();
+ angleVecToNavLine = std::atan2(vecToNavLine._y,vecToNavLine._x)*180.0/M_PI;
+
+ anglediff=std::fabs(angleVecToNavLine-decisiveSign->GetAlphaPointing());
+ if (anglediff>180.0)
+ anglediff=360.0-anglediff;
+
+
+ if (anglediff<90.0)
+ {
+ sortedNavLines.push_back(navLine);
+ }
+ }
+
+ if (sortedNavLines.empty())
+ return navLines;
+
+ const NavLine* bestChoice=nullptr;
+
+ if (sortedNavLines.size()>1)
+ {
+ std::vector<double> orthogonalDistances;
+ for (const NavLine* navLine:sortedNavLines)
+ {
+ orthogonalDistances.push_back(OrthogonalDistanceFromTo(navLine,*decisiveSign));
+ }
+ size_t index=std::min_element(orthogonalDistances.begin(),orthogonalDistances.end())-orthogonalDistances.begin();
+
+ bestChoice = *(sortedNavLines.begin()+index);
+ sortedNavLines.clear();
+ sortedNavLines.push_back(bestChoice);
+ }
+ else
+ bestChoice=sortedNavLines.front();
+
+
+
+
+ for (const NavLine* navLine:navLines)
+ {
+ if (navLine!=bestChoice)
+ {
+ sortedNavLines.push_back(navLine);
+ }
+ }
+
+ return sortedNavLines;
+}
+
+const Sign* Cortex::DetermineDecisiveSign(const std::vector<const Sign*> &signs)
+{
+
+ std::vector<const Sign*> cSigns=signs;
+
+ if (cSigns.size()==1)
+ return cSigns.front();
+
+
+ std::vector<double> angles;
+ std::vector<const Sign*> signsNotPointing;
+
+ for (size_t i=0; i<cSigns.size(); ++i)
+ {
+ std::vector<double> cAngles;
+
+ for (size_t j=0; j<cSigns.size(); ++j)
+ {
+ if (i!=j)
+ {
+ Point vec_ij=cSigns[j]->GetPos()-cSigns[i]->GetPos();
+ cAngles.push_back(std::fabs(cSigns[i]->GetAlphaPointing()-std::atan2(vec_ij._y,vec_ij._x)*180.0/M_PI));
+ }
+ }
+ size_t index=std::min_element(cAngles.begin(),cAngles.end())-cAngles.begin();
+
+ if (cAngles.at(index)>45.0)
+ signsNotPointing.push_back(cSigns[i]);
+ angles.push_back(cAngles.at(index));
+ }
+
+ //how many angles are greater than 45 degrees or rather how many signs do not point to another?
+ if (signsNotPointing.size()>1)
+ {
+ std::shuffle(signsNotPointing.begin(),signsNotPointing.end(),_b->GetConfig()->GetRandomNumberGenerator()->GetRandomEngine());
+ for (auto it=signsNotPointing.begin();it!=signsNotPointing.end();++it)
+ {
+ if (it==signsNotPointing.begin())
+ continue;
+ _notUsedSigns.push_back(*it);
+ }
+ return signsNotPointing.front();
+ }
+ else
+ {
+
+ size_t index=std::max_element(angles.begin(),angles.end())-angles.begin();
+ return cSigns.at(index);
+ }
+
+}
+
+double Cortex::OrthogonalDistanceFromTo(const Sign &sign1, const Sign &sign2) const
+{
+ // line created with dir Vector of sign2
+
+ double a;
+ double b;
+
+ if (sign2.GetAlpha()==90.0 || sign2.GetAlpha()==270.0)
+ {
+ a=0.0;
+ b=1.0;
+ }
+ else
+ {
+ a=1.0;
+ b = std::tan(sign2.GetAlpha()*M_PI/180.);
+ }
+
+ Point pa = sign2.GetPos();
+ Point dirVec = Point(a,b);
+
+ Line signLine = Line(pa-dirVec*1000,pa+dirVec*1000);
+
+ return signLine.DistTo(sign1.GetPos());
+}
+
+double Cortex::OrthogonalDistanceFromTo(const NavLine* navLine, const Sign& sign) const
+{
+ // line created with dir Vector of sign
+
+ double a;
+ double b;
+ //sign1->GetAlpha();
+ if (sign.GetAlphaPointing()==90.0 || sign.GetAlphaPointing()==270.0)
+ {
+ a=0.0;
+ b=1.0;
+ }
+ else
+ {
+ a=1.0;
+ b = std::tan(sign.GetAlphaPointing()*M_PI/180.);
+ }
+
+ Point pa = sign.GetPos();
+ Point dirVec = Point(a,b);
+
+ Line signLine = Line(pa-dirVec*1000,pa+dirVec*1000);
+
+ return signLine.DistTo(navLine->GetCentre());
+}
+
+double Cortex::OrthogonalDistanceFromTo(const NavLine* navLine, const Point &pos, const double &angle) const
+{
+ // line created with dir Vector of sign
+
+ double a;
+ double b;
+
+ if (angle==90.0 || angle==270.0)
+ {
+ a=0.0;
+ b=1.0;
+ }
+ else
+ {
+ a=1.0;
+ b = std::tan(angle*M_PI/180.);
+ }
+
+ Point dirVec = Point(a,b);
+
+ Line line = Line(pos-dirVec*1000,pos+dirVec*1000);
+
+ return line.DistTo(navLine->GetCentre());
+}
+
+
+
+bool IsInsideRectangle(const Point &point, const Point &leftUp, const Point &rightDown)
+{
+ if (point._x> leftUp._x && point._x< rightDown._x
+ && point._y>rightDown._y && point._y<leftUp._y)
+ return true;
+ else
+ return false;
+}
diff --git a/routing/ai_router_trips/Cortex.h b/routing/ai_router_trips/Cortex.h
new file mode 100644
index 0000000000000000000000000000000000000000..ef814829ec6366673fd771129fdbaf7bdab64ea3
--- /dev/null
+++ b/routing/ai_router_trips/Cortex.h
@@ -0,0 +1,65 @@
+#ifndef CORTEX_H
+#define CORTEX_H
+
+#include "cognitiveMap/cognitivemap.h"
+#include "./perception/visibleenvironment.h"
+#include "./cognitiveMap/internnavigationnetwork.h"
+#include "./perception/visualsystem.h"
+
+
+using ptrIntNetwork = std::unique_ptr<InternNavigationNetwork>;
+
+
+
+class Cortex
+{
+
+public:
+ Cortex();
+ Cortex(const Building* b,const Pedestrian* ped, const VisibleEnvironment* env, std::unordered_map<const SubRoom*, ptrIntNetwork>* roominternalNetworks);
+
+ AICognitiveMap& GetCognitiveMap();
+ VisualSystem& GetPerceptionAbilities();
+
+ //to enable hline handling
+ const NavLine* GetNextNavLine(const NavLine *nextTarget);
+ bool HlineReached() const;
+
+ std::vector<const NavLine*> SortConGeneralKnow(const std::vector<const NavLine *> &navLines);
+
+ //select appropriate crossings
+ const NavLine FindApprCrossing(const std::vector<NavLine> &navLines);
+ const NavLine FindApprVisibleCrossing(const NavLine& navLine, const std::vector<NavLine> &navLines);
+ const NavLine TargetToHead(const NavLine *visibleCrossing) const;
+
+ //implement sign instruction
+ std::vector<const NavLine *> SortConSignage(const std::vector<const NavLine *>& navLines);
+ const Sign* DetermineDecisiveSign(const std::vector<const Sign *> &signs);
+ double OrthogonalDistanceFromTo(const Sign& sign1, const Sign& sign2) const;
+ double OrthogonalDistanceFromTo(const NavLine* navLine, const Sign &sign) const;
+ double OrthogonalDistanceFromTo(const NavLine* navLine, const Point& pos, const double& angle) const;
+
+
+
+private:
+ const Building* _b;
+ const Pedestrian* _ped;
+ AICognitiveMap _cMap;
+ //whole environment
+ const VisibleEnvironment* _wholeEnvironment;
+ // reference of roominternalNetwork
+ std::unordered_map<const SubRoom*, ptrIntNetwork>* _intNetworks;
+ InternNavigationNetwork _currentIntNetwork;
+ //perception abilities
+ VisualSystem _perceptionAbilities;
+ NavLine _lastBestVisNavLine;
+ bool _statPreferToGoDown;
+ std::vector<const Sign*> _notUsedSigns;
+
+
+
+};
+
+bool IsInsideRectangle(const Point& point, const Point& leftUp, const Point& rightDown);
+
+#endif // CORTEX_H
diff --git a/routing/ai_router_trips/cognitiveMap/associations.cpp b/routing/ai_router_trips/cognitiveMap/associations.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d6926f8c4a42044462ecec30f93526f0b98a1267
--- /dev/null
+++ b/routing/ai_router_trips/cognitiveMap/associations.cpp
@@ -0,0 +1,61 @@
+#include "associations.h"
+#include "connection.h"
+
+
+
+AIAssociation::AIAssociation()
+{
+ _landmark=nullptr;
+ _associatedLandmark=nullptr;
+
+}
+
+AIAssociation::AIAssociation(const AILandmark *landmark, const AILandmark *associated_landmark, bool connected)
+{
+ _landmark=landmark;
+ _associatedLandmark=associated_landmark;
+
+// if (connected)
+// _connection = std::make_shared<Connection>(_landmark, _associatedLandmark);
+
+// else
+ _connection=nullptr;
+
+
+}
+
+AIAssociation::AIAssociation(const AIConnection *connection)
+{
+ _connection=connection;
+ _landmark=nullptr;
+ _associatedLandmark=nullptr;
+}
+
+AIAssociation::~AIAssociation()
+{
+
+}
+
+const AILandmark *AIAssociation::GetLandmarkAssociation(const AILandmark *landmark) const
+{
+ if (landmark==nullptr)
+ return nullptr;
+ if (_landmark==landmark)
+ {
+ return _associatedLandmark;
+ }
+ else
+ return nullptr;
+
+}
+
+const AIConnection *AIAssociation::GetConnectionAssoziation() const
+{
+ return _connection;
+}
+
+bool AIAssociation::operator==(const AIAssociation &asso2) const
+{
+ return this==&asso2;
+}
+
diff --git a/routing/ai_router_trips/cognitiveMap/associations.h b/routing/ai_router_trips/cognitiveMap/associations.h
new file mode 100644
index 0000000000000000000000000000000000000000..deb958ebdd3d8343d53421abbd4a079e0c8156d6
--- /dev/null
+++ b/routing/ai_router_trips/cognitiveMap/associations.h
@@ -0,0 +1,26 @@
+#ifndef ASSOCIATIONS_H
+#define ASSOCIATIONS_H
+
+#include<memory>
+
+class AILandmark;
+class AIConnection;
+
+
+class AIAssociation
+{
+public:
+ AIAssociation();
+ AIAssociation(const AILandmark* landmark, const AILandmark* associated_landmark, bool connected=false);
+ AIAssociation(const AIConnection* connection);
+ ~AIAssociation();
+ const AILandmark* GetLandmarkAssociation(const AILandmark* landmark) const;
+ const AIConnection* GetConnectionAssoziation() const;
+ bool operator==(const AIAssociation& asso2) const;
+private:
+ const AILandmark* _landmark;
+ const AILandmark* _associatedLandmark;
+ const AIConnection* _connection;
+};
+
+#endif // ASSOCIATIONS_H
diff --git a/routing/ai_router_trips/cognitiveMap/cognitivemap.cpp b/routing/ai_router_trips/cognitiveMap/cognitivemap.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4981c3dbcbf7d622b89cba16e629795ce9fa2604
--- /dev/null
+++ b/routing/ai_router_trips/cognitiveMap/cognitivemap.cpp
@@ -0,0 +1,679 @@
+#include "cognitivemap.h"
+#include <chrono>
+#include <random>
+#include <algorithm>
+#include <cmath>
+#include <fstream>
+#include "../../../pedestrian/Pedestrian.h"
+#include "../perception/visualsystem.h"
+
+
+
+AICognitiveMap::AICognitiveMap()
+{
+
+}
+
+
+AICognitiveMap::AICognitiveMap(const Pedestrian *ped, const VisualSystem* perceptionalAbilities)
+{
+ _ped=ped;
+
+ _perceptAbis=perceptionalAbilities;
+ //Destination and regions
+ _currentRegion=nullptr;
+ //Find maindestination in cogmapstorage
+
+ _nextTarget=nullptr;
+ _lastBestChoice=NavLine(Line(Point(0,0),Point(0,0)));
+ _lastMinAngleDiff=FLT_MAX;
+
+
+// Region newRegion(Point(0.0));
+// newRegion.SetA(20.0);
+// newRegion.SetB(20.0);
+// newRegion.SetId(1);
+
+// Landmark finalDest(Point(5.0,5.0),0.1,0.1,1);
+// finalDest.SetPosInMap(Point(15.0,8.0));
+// finalDest.SetType("main");
+
+// newRegion.AddLandmark(finalDest);
+// AddRegion(newRegion);
+
+// InitLandmarkNetworksInRegions();
+// FindMainDestination();
+
+}
+
+
+void AICognitiveMap::UpdateMap()
+{
+ FindCurrentRegion();
+
+ CheckIfLandmarksReached();
+
+}
+
+
+
+void AICognitiveMap::AddRegions(const AIRegions ®ions)
+{
+ for (const AIRegion& region:regions)
+ {
+ _regions.push_back(region);
+ }
+
+}
+
+void AICognitiveMap::AddRegion(const AIRegion& region)
+{
+ _regions.push_back(region);
+}
+
+const AIRegion *AICognitiveMap::GetRegionByID(int regionID) const
+{
+ for (const AIRegion& region:_regions)
+ {
+ if (region.GetId()==regionID)
+ {
+ return ®ion;
+ }
+ }
+ return nullptr;
+}
+
+
+std::vector<const AILandmark *> AICognitiveMap::TriggerAssociations(const std::vector<const AILandmark *> &landmarks)
+{
+ std::vector<const AILandmark *> associatedlandmarks;
+ for (const AILandmark* landmark:landmarks)
+ {
+ AIAssociations associations = landmark->GetAssociations();
+ for (const AIAssociation& association:associations)
+ {
+ if (association.GetLandmarkAssociation(landmark)!=nullptr)
+ {
+ associatedlandmarks.push_back(association.GetLandmarkAssociation(landmark));
+
+ }
+
+ }
+ }
+ return associatedlandmarks;
+}
+
+
+std::vector<const NavLine*> AICognitiveMap::SortConLeastAngle(const AILandmark* landmark, const std::vector<NavLine> &navLines, const NavLine &focus)
+{
+ std::vector<const NavLine*> sortedNavLines;
+ std::vector<const NavLine*> cNavLines;
+ NavLine focusTo = focus;
+ for (const NavLine& navline:navLines)
+ {
+ cNavLines.push_back(&navline);
+ }
+
+
+
+ //check if lastChoice still exists
+ bool statusLastChoice=false;
+ if (_lastBestChoice.GetPoint1()==Point(0,0) && _lastBestChoice.GetPoint2()==Point(0,0))
+ {
+ _lastBestChoice=*(cNavLines.front());
+ statusLastChoice=true;
+ focusTo=_lastBestChoice;
+ //return cNavLines;
+ }
+ else
+ {
+
+ for (const NavLine* navline:cNavLines)
+ {
+
+ Point pCloser = GetCloserPoint(_ped->GetPos(),navline->GetPoint1(),navline->GetPoint2());
+ Point pCloserBestChoice = GetCloserPoint(_ped->GetPos(),_lastBestChoice.GetPoint1(),_lastBestChoice.GetPoint2());
+
+ if ((pCloserBestChoice-pCloser).Norm()<0.125)
+ {
+ statusLastChoice=true;
+ _lastBestChoice=*navline;
+ break;
+ }
+ }
+ }
+
+ // check if ped already in landmark (ellipse); if so, random choice *******************
+ if (landmark)
+ {
+ Point vecPedLCenter = _ped->GetPos()-landmark->GetPosInMap();
+ double angleVecPedLCenter = std::atan2(vecPedLCenter._y,vecPedLCenter._x);
+
+ Point onEllipse = Point(landmark->GetPosInMap()._x+landmark->GetA()*std::cos(angleVecPedLCenter),landmark->GetPosInMap()._y+landmark->GetB()*std::sin(angleVecPedLCenter));
+
+ if ((onEllipse-landmark->GetPosInMap()).Norm()>vecPedLCenter.Norm())
+ {
+ landmark=nullptr;
+ //std::cout << "Hier" << std::endl;
+ }
+ }
+ //****************************************
+
+ if (!landmark)
+ {
+ if (statusLastChoice)
+ {
+ sortedNavLines.push_back(&_lastBestChoice);
+ for (const NavLine* navLine:cNavLines)
+ {
+ if (navLine!=&_lastBestChoice)
+ sortedNavLines.push_back(navLine);
+ }
+ return sortedNavLines;
+ }
+ else
+ {
+
+
+ //prefer navLines that are in the agent's current heading direction
+ double cMinAngle=M_PI/4.0;
+ std::vector<const NavLine*> navLinesBehindMe;
+ //const NavLine* navLineInHeadDir=nullptr;
+ Point vec1 = focusTo.GetCentre()-_ped->GetPos();
+ double heading = std::atan2(vec1._y,vec1._x);
+ for (auto it=cNavLines.begin(); it!=cNavLines.end(); ++it)
+ {
+ Point vec2 = (*it)->GetCentre()-_ped->GetPos();
+ double angle = std::fabs(std::atan2(vec2._y,vec2._x)-heading);
+ if (angle>M_PI)
+ angle=2*M_PI-angle;
+ // if crossing is behind agent put it to the bottom of the list
+ if (angle > cMinAngle)
+ {
+ //cNavLines.erase(std::find(cNavLines.begin(), cNavLines.end(), navLine));
+ cNavLines.erase(it);
+ navLinesBehindMe.push_back(*it);
+ --it;
+ }
+ }
+
+ cNavLines.insert(cNavLines.end(),navLinesBehindMe.begin(),navLinesBehindMe.end());
+ _lastBestChoice=*(cNavLines.front());
+
+ return cNavLines;
+
+ }
+ }
+
+
+
+
+
+ Point beelineToLandmark = ShortestBeeLine(_ped->GetPos(),landmark);
+ double alpha1=std::atan2(beelineToLandmark._y,beelineToLandmark._x);
+
+ double anglediff;
+ std::vector<double> vecMinAngleDiff;
+ double currentMinAngleDiff;
+ const NavLine* bestChoice=nullptr;
+
+ while (cNavLines.size()>0)
+ {
+ bestChoice=nullptr;
+ currentMinAngleDiff=FLT_MAX;
+ for (auto it=cNavLines.begin(); it!=cNavLines.end(); ++it)
+ {
+
+ double alpha2=std::atan2(((*it)->GetCentre()-_ped->GetPos())._y,((*it)->GetCentre()-_ped->GetPos())._x);
+
+ anglediff=std::fabs(alpha1-alpha2);
+
+
+ if (anglediff>M_PI)
+ anglediff=2*M_PI-anglediff;
+
+
+ if (anglediff<currentMinAngleDiff)
+ {
+ currentMinAngleDiff=anglediff;
+ bestChoice=*it;
+
+ }
+
+ }
+
+ if (*bestChoice==_lastBestChoice)
+ {
+ _lastMinAngleDiff=currentMinAngleDiff;
+ }
+
+ vecMinAngleDiff.push_back(currentMinAngleDiff);
+ sortedNavLines.push_back(bestChoice);
+ cNavLines.erase(std::find(cNavLines.begin(), cNavLines.end(), bestChoice));
+ }
+
+
+// //prefer navLines that are in the agent's current heading direction******************
+
+// double cMinAngle=M_PI/3.0;
+// std::vector<const NavLine*> navLinesBehindMe;
+// //const NavLine* navLineInHeadDir=nullptr;
+// Point vec1 = focusTo.GetCentre()-_ped->GetPos();
+// double heading = std::atan2(vec1._y,vec1._x);
+// for (auto it=sortedNavLines.begin(); it!=sortedNavLines.end(); ++it)
+// {
+// Point vec2 = (*it)->GetCentre()-_ped->GetPos();
+// double angle = std::fabs(std::atan2(vec2._y,vec2._x)-heading);
+// if (angle>M_PI)
+// angle=2*M_PI-angle;
+// // if crossing is behind agent put it to the bottom of the list
+// if (angle > cMinAngle)
+// {
+// //cNavLines.erase(std::find(cNavLines.begin(), cNavLines.end(), navLine));
+// sortedNavLines.erase(it);
+// navLinesBehindMe.push_back(*it);
+// --it;
+// }
+// }
+
+// sortedNavLines.insert(sortedNavLines.end(),navLinesBehindMe.begin(),navLinesBehindMe.end());
+// _lastBestChoice=*(cNavLines.front());
+
+ //*********************************
+
+ if (_lastMinAngleDiff<vecMinAngleDiff.front()+M_PI/3.0 && statusLastChoice)
+ {
+ //erase the best choice navline from the vector and put it on the front
+ for (auto it=sortedNavLines.begin(); it!=sortedNavLines.end(); ++it)
+ {
+ if (*(*it)==_lastBestChoice)
+ {
+ sortedNavLines.erase(it);
+ break;
+ }
+ }
+ sortedNavLines.insert(sortedNavLines.begin(),&_lastBestChoice);
+ }
+ else
+ {
+
+ _lastBestChoice=*(sortedNavLines.front());
+ _lastMinAngleDiff=vecMinAngleDiff.front();
+
+ }
+
+
+ return sortedNavLines;
+
+}
+
+
+
+Point AICognitiveMap::ShortestBeeLine(const Point &pos, const AILandmark *landmark)
+{
+ Point pointOnShortestRoute = landmark->PointOnShortestRoute(pos);
+ Point beeLineVec = pointOnShortestRoute-pos;
+ return beeLineVec;
+}
+
+
+double AICognitiveMap::MakeItFuzzy(double mean, double std)
+{
+ using myClock = std::chrono::high_resolution_clock;
+ myClock::duration d = myClock::now().time_since_epoch();
+
+ auto seed = d.count();
+
+ std::default_random_engine generator(seed);
+ std::normal_distribution<double> distribution(mean,std);
+
+ double number = distribution(generator);
+
+ return number;
+}
+
+
+
+std::vector<const AILandmark*> AICognitiveMap::GetLandmarksConnectedWith(const AILandmark* landmark) const
+{
+ const AIRegion* cRegion = GetRegionContaining(landmark);
+
+ if (cRegion!=nullptr)
+ {
+ return cRegion->ConnectedWith(landmark);
+ }
+ else
+ {
+ return std::vector<const AILandmark*>();
+ }
+}
+
+const AIRegion* AICognitiveMap::GetRegionContaining(const AILandmark *landmark) const
+{
+ for (const AIRegion& region:_regions)
+ {
+ if (region.ContainsLandmark(landmark))
+ return ®ion;
+ }
+ return nullptr;
+}
+
+void AICognitiveMap::FindCurrentRegion()
+{
+
+ //for test purposes. has to be changed
+ if (_regions.empty())
+ return;
+
+ //needs to be fixed
+ _currentRegion=&(_regions.back());
+ return;
+
+ for (const AIRegion& region:_regions)
+ {
+ if (region.Contains(_ped->GetPos()))
+ {
+ _currentRegion=®ion;
+ return;
+ }
+ }
+ //path searching to region
+ _currentRegion=nullptr;
+}
+
+void AICognitiveMap::CheckIfLandmarksReached()
+{
+
+ BoostPolygon boostPolygon=_perceptAbis->GetCurrentEnvironment();
+
+ if (_currentRegion!=nullptr)
+ {
+ for (const AILandmark& landmark:_currentRegion->GetLandmarks())
+ {
+ if (boost::geometry::intersects(landmark.GetRealPos(),boostPolygon))
+ {
+ if (std::find(_landmarksRecentlyVisited.begin(), _landmarksRecentlyVisited.end(), &landmark) == _landmarksRecentlyVisited.end() )
+ {
+ _landmarksRecentlyVisited.push_back(&landmark);
+
+ _mainDestination=nullptr;
+ _nextTarget=nullptr;
+ }
+ }
+
+ }
+ }
+
+
+}
+
+const AILandmark *AICognitiveMap::FindConnectionPoint(const AIRegion *currentRegion, const AIRegion *targetRegion) const
+{
+ if (currentRegion!=nullptr && targetRegion!=nullptr)
+ {
+ for (const AILandmark& landmarka:currentRegion->GetLandmarks())
+ {
+ for (const AILandmark& landmarkb:targetRegion->GetLandmarks())
+ {
+ if (&landmarka==&landmarkb)
+ {
+ return &landmarka;
+ }
+ }
+ }
+ }
+
+ return nullptr;
+
+}
+
+void AICognitiveMap::FindMainDestination()
+{
+ std::vector<const AILandmark*> possibleMainTargets;
+ for (const AIRegion& region:_regions)
+ {
+ for (const AILandmark& landmark:region.GetLandmarks())
+ {
+ if (landmark.GetType()=="main" && std::find(_landmarksRecentlyVisited.begin(),_landmarksRecentlyVisited.end(),&landmark)==_landmarksRecentlyVisited.end())
+ {
+ possibleMainTargets.push_back(&landmark);
+ _targetRegion=®ion;
+
+ }
+ }
+ }
+ if (possibleMainTargets.empty())
+ {
+ _mainDestination=nullptr;
+ _targetRegion=nullptr;
+ return;
+ }
+ else
+ _mainDestination=GetNearestMainTarget(possibleMainTargets);
+
+}
+
+void AICognitiveMap::FindNextTarget()
+{
+
+ _nextTarget=nullptr;
+
+ // if not already in the region of the maindestination
+ if (_currentRegion==nullptr || _targetRegion==nullptr)
+ {
+ return;
+ }
+
+ if (_targetRegion!=_currentRegion)
+ {
+ _nextTarget=FindConnectionPoint(_currentRegion,_targetRegion);
+ // if connection point does not exist: Path searching to region
+ if (_nextTarget==nullptr)
+ {
+ //Region is target
+ _nextTarget=_targetRegion->GetRegionAsLandmark();
+ return;
+ }
+
+ }
+ else //destination is in current region
+ {
+ _nextTarget=_mainDestination;
+ }
+
+ // Function considers that nearLandmark can be the target itself if no nearer was found.
+ const AILandmark* nearLandmark = FindNearLandmarkConnectedToTarget(_nextTarget);
+
+ _nextTarget=nearLandmark;
+ //Direct way to target much shorter than via near Landmark?
+
+}
+
+void AICognitiveMap::FindShortCut()
+{
+
+}
+
+const AILandmark* AICognitiveMap::FindNearLandmarkConnectedToTarget(const AILandmark* target)
+{
+
+ std::vector<const AILandmark*> landmarksConnectedToTarget = FindLandmarksConnectedToTarget(target);
+
+ //if target has no connections return nullptr
+ if (landmarksConnectedToTarget.empty())
+ return target;
+
+ //look for nearest located landmark
+
+ //look for landmarks within a circle with the radius searchlimit
+ // if no landmarks were found radius will be enlarged
+ // if radius = distance(Pos->target) return target
+
+ Point vector=target->GetPosInMap()-_ped->GetPos();//_YAHPointer.GetPos();
+
+
+ double distanceToTarget=vector.Norm();
+ int divisor = 24;
+ double searchlimit=distanceToTarget/divisor;
+ std::vector<const AILandmark*> nearLandmarks;
+
+ while (searchlimit<distanceToTarget && nearLandmarks.empty())
+ {
+ for (const AILandmark* landmark:landmarksConnectedToTarget)
+ {
+
+ vector=landmark->GetPosInMap()-_ped->GetPos();
+
+ double distance = vector.Norm();
+
+
+ if (distance<=searchlimit)
+ {
+ nearLandmarks.push_back(landmark);
+ }
+ }
+ searchlimit+=searchlimit;
+
+ }
+
+ if (nearLandmarks.empty())
+ return target;
+
+ // select best route to target from one of the nearLandmarks
+
+ return FindBestRouteFromOneOf(nearLandmarks);
+
+
+}
+
+std::vector<const AILandmark*> AICognitiveMap::FindLandmarksConnectedToTarget(const AILandmark* target)
+{
+
+ std::vector<const AILandmark*> connectedLandmarks;
+
+ // landmarks directly connected to target
+ std::vector<const AILandmark*> firstCandidates = GetLandmarksConnectedWith(target);
+
+ for (const AILandmark* candidate:firstCandidates)
+ {
+ if(std::find(_landmarksRecentlyVisited.begin(), _landmarksRecentlyVisited.end(), candidate) == _landmarksRecentlyVisited.end())
+ {
+ connectedLandmarks.push_back(candidate);
+ }
+ }
+
+ //Landmarks connected to landmarks connected to target
+ std::vector<const AILandmark*> furtherCandidates;
+
+ for (size_t i=0; i<connectedLandmarks.size(); ++i)
+ {
+ furtherCandidates=GetLandmarksConnectedWith(connectedLandmarks[i]);
+
+ for (const AILandmark* candidate : furtherCandidates)
+ {
+ // if candidate not already taken into account, not visited before or target itself
+ if(std::find(connectedLandmarks.begin(), connectedLandmarks.end(), candidate) == connectedLandmarks.end()
+ && candidate!=target)
+ {
+ connectedLandmarks.push_back(candidate);
+
+ }
+ }
+ }
+ return connectedLandmarks;
+}
+
+const AILandmark* AICognitiveMap::FindBestRouteFromOneOf(const std::vector<const AILandmark*> &nearLandmarks)
+{
+ const AILandmark* bestChoice = nullptr;
+ double minDistance = FLT_MAX;
+ double cDistance;
+ for (const AILandmark* landmark:nearLandmarks)
+ {
+ cDistance=(_ped->GetPos()-landmark->GetRandomPoint()).Norm()+_currentRegion->PathLengthFromLandmarkToTarget(landmark, _nextTarget).second;
+
+ if (cDistance<minDistance)
+ {
+ minDistance=cDistance;
+
+ bestChoice=landmark;
+ }
+ }
+ std::vector<const AILandmark*> landmarksOnShortestPath = _currentRegion->PathLengthFromLandmarkToTarget(bestChoice, _nextTarget).first;
+
+ for (int i=landmarksOnShortestPath.size()-1; i>=0; --i)
+ {
+ if (std::find(_landmarksRecentlyVisited.begin(),_landmarksRecentlyVisited.end(),landmarksOnShortestPath[i])==_landmarksRecentlyVisited.end())
+ {
+ return landmarksOnShortestPath[i];
+ }
+
+ }
+ return nullptr;
+}
+
+const AILandmark* AICognitiveMap::GetNearestMainTarget(const std::vector<const AILandmark*> &mainTargets)
+{
+ const AILandmark* nearest = nullptr;
+ double dNearest = FLT_MAX;
+
+ if (mainTargets.size()==1)
+ return mainTargets[0];
+
+ for (const AILandmark* mainDest:mainTargets)
+ {
+ _nextTarget=mainDest;
+ const AILandmark* cLandmark = FindNearLandmarkConnectedToTarget(mainDest);
+ double cDistance=(_ped->GetPos()-cLandmark->GetRandomPoint()).Norm()+_currentRegion->PathLengthFromLandmarkToTarget(cLandmark,mainDest).second;
+
+ if (cDistance<dNearest)
+ {
+ dNearest=cDistance;
+ nearest=mainDest;
+ }
+ }
+
+ return nearest;
+}
+
+void AICognitiveMap::SetBestChoice(const NavLine &navLine)
+{
+ _lastBestChoice=navLine;
+}
+
+const NavLine* AICognitiveMap::GetBestChoice() const
+{
+ if (_lastBestChoice.GetPoint1()==Point(0,0) && _lastBestChoice.GetPoint2()==Point(0,0))
+ return nullptr;
+ else
+ return &_lastBestChoice;
+}
+
+void AICognitiveMap::InitLandmarkNetworksInRegions()
+{
+ for (AIRegion& region:_regions)
+ {
+ region.InitLandmarkNetwork();
+ }
+}
+
+const AILandmark* AICognitiveMap::GetNextTarget() const
+{
+ return _nextTarget;
+}
+
+
+Point GetCloserPoint(const Point &origin, const Point &target1, const Point& target2)
+{
+
+ Point vec1 = target1-origin;
+ Point vec2 = target2-origin;
+
+ if (vec1.Norm()<vec2.Norm())
+ return target1;
+ else
+ return target2;
+}
+
+
+
diff --git a/routing/ai_router_trips/cognitiveMap/cognitivemap.h b/routing/ai_router_trips/cognitiveMap/cognitivemap.h
new file mode 100644
index 0000000000000000000000000000000000000000..15e5b5b56aa0640fe60ace6f1d7cd9cfa3f538d0
--- /dev/null
+++ b/routing/ai_router_trips/cognitiveMap/cognitivemap.h
@@ -0,0 +1,87 @@
+#ifndef COGNITIVEMAP_H
+#define COGNITIVEMAP_H
+
+#include "associations.h"
+#include "region.h"
+#include <memory>
+#include <vector>
+#include "../../../geometry/NavLine.h"
+
+
+class Pedestrian;
+class VisualSystem;
+using AIRegions = std::vector<AIRegion>;
+
+
+class AICognitiveMap
+{
+public:
+ AICognitiveMap();
+ AICognitiveMap(const Pedestrian* ped, const VisualSystem* perceptionalAbilities);
+ //Map Updates
+ void UpdateMap();
+ //Regions
+ void AddRegions(const AIRegions ®ions);
+ void AddRegion(const AIRegion ®ion);
+ const AIRegion* GetRegionByID(int regionID) const;
+ // Landmarks
+ void AddLandmarkInRegion(const AILandmark& landmark, AIRegion& region);
+
+ // Associations
+ std::vector<const AILandmark *> TriggerAssociations(const std::vector<const AILandmark*> &landmarks);
+
+ std::vector<const NavLine *> SortConLeastAngle(const AILandmark* landmark, const std::vector<NavLine> &navLines, const NavLine& focus);
+
+ Point ShortestBeeLine(const Point& pos, const AILandmark* landmark);
+
+ //Tools
+ double MakeItFuzzy(double mean, double std);
+
+ //Find region/landmarks/connections
+ std::vector<const AILandmark *> GetLandmarksConnectedWith(const AILandmark *landmark) const;
+ const AIRegion* GetRegionContaining(const AILandmark* landmark) const;
+
+ //Locater
+ void FindCurrentRegion();
+ void CheckIfLandmarksReached();
+
+ //Find targets
+ const AILandmark* FindConnectionPoint(const AIRegion* currentRegion, const AIRegion* targetRegion) const;
+ void FindMainDestination();
+ void FindNextTarget();
+ const AILandmark *GetNextTarget() const;
+ void FindShortCut();
+ const AILandmark *FindNearLandmarkConnectedToTarget(const AILandmark *target);
+ std::vector<const AILandmark *> FindLandmarksConnectedToTarget(const AILandmark *target);
+ const AILandmark *FindBestRouteFromOneOf(const std::vector<const AILandmark *> &nearLandmarks);
+ const AILandmark *GetNearestMainTarget(const std::vector<const AILandmark *> &mainTargets);
+
+ void SetBestChoice(const NavLine &navLine);
+ const NavLine *GetBestChoice() const;
+
+ //Init LandmarkNetworks
+ void InitLandmarkNetworksInRegions();
+
+
+private:
+ const Pedestrian* _ped;
+ const VisualSystem* _perceptAbis;
+
+ std::vector<const AILandmark*> _landmarksRecentlyVisited;
+
+ const AILandmark* _mainDestination;
+ const AILandmark* _nextTarget;
+ AIRegions _regions;
+ const AIRegion* _currentRegion;
+ const AIRegion* _targetRegion;
+
+ NavLine _lastBestChoice;
+ double _lastMinAngleDiff;
+
+
+
+};
+
+Point GetCloserPoint(const Point &origin, const Point &target1, const Point& target2);
+
+#endif // COGNITIVEMAP_H
diff --git a/routing/ai_router_trips/cognitiveMap/connection.cpp b/routing/ai_router_trips/cognitiveMap/connection.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8941441f898741309e42cf9be107d248e8b46c06
--- /dev/null
+++ b/routing/ai_router_trips/cognitiveMap/connection.cpp
@@ -0,0 +1,53 @@
+#include "connection.h"
+#include <utility>
+
+AIConnection::AIConnection(int id, const std::string &caption, const std::string &type, int landmarkId1, int landmarkId2)
+{
+ _landmarkId1=landmarkId1;
+ _landmarkId2=landmarkId2;
+ _id=id;
+ _caption=caption;
+ _type=type;
+}
+
+AIConnection::~AIConnection()
+{
+
+}
+
+std::pair<int, int> AIConnection::GetLandmarkIds() const
+{
+ return std::make_pair(_landmarkId1, _landmarkId2);
+
+}
+
+int AIConnection::GetId() const
+{
+ return _id;
+}
+
+const std::string &AIConnection::GetCaption() const
+{
+ return _caption;
+}
+
+const std::string &AIConnection::GetType() const
+{
+ return _type;
+}
+
+void AIConnection::SetId(int id)
+{
+ _id=id;
+}
+
+void AIConnection::SetCaption(const std::string &caption)
+{
+ _caption=caption;
+}
+
+void AIConnection::SetType(const std::string &type)
+{
+ _type=type;
+}
+
diff --git a/routing/ai_router_trips/cognitiveMap/connection.h b/routing/ai_router_trips/cognitiveMap/connection.h
new file mode 100644
index 0000000000000000000000000000000000000000..afc3457291fb12e7c9ccdd9c3ecaf59f4c6c61a9
--- /dev/null
+++ b/routing/ai_router_trips/cognitiveMap/connection.h
@@ -0,0 +1,35 @@
+#ifndef CONNECTION_H
+#define CONNECTION_H
+
+#include "landmark.h"
+
+
+using AILandmarks = std::vector<AILandmark>;
+
+class AIConnection
+{
+public:
+ AIConnection(int id, const std::string& caption, const std::string& type, int landmarkId1, int landmarkId2);
+ ~AIConnection();
+
+ //Getter
+ int GetId() const;
+ const std::string& GetCaption() const;
+ const std::string& GetType() const;
+
+ std::pair<int,int> GetLandmarkIds() const;
+
+ //Setter
+ void SetId(int id);
+ void SetCaption(const std::string& caption);
+ void SetType(const std::string& type);
+
+private:
+ int _id;
+ std::string _caption;
+ std::string _type;
+ int _landmarkId1;
+ int _landmarkId2;
+};
+
+#endif // CONNECTION_H
diff --git a/routing/ai_router_trips/cognitiveMap/internnavigationnetwork.cpp b/routing/ai_router_trips/cognitiveMap/internnavigationnetwork.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a7ad20ff60730d372c74b8e80d6a95fdda8b2d46
--- /dev/null
+++ b/routing/ai_router_trips/cognitiveMap/internnavigationnetwork.cpp
@@ -0,0 +1,184 @@
+#include "internnavigationnetwork.h"
+#include "../../../geometry/Obstacle.h"
+
+#include <boost/graph/graph_traits.hpp>
+#include <boost/graph/adjacency_list.hpp>
+#include <boost/graph/dijkstra_shortest_paths.hpp>
+#include <boost/geometry.hpp>
+
+InternNavigationNetwork::InternNavigationNetwork()
+{
+ _graph=Graph();
+}
+
+InternNavigationNetwork::InternNavigationNetwork(const SubRoom *subRoom)
+{
+ _graph=Graph();
+
+ _subRoom=subRoom;
+
+ for (Wall wall:_subRoom->GetAllWalls())
+ {
+ Linestring lineS;
+ boost::geometry::append(lineS,wall.GetPoint1());
+ boost::geometry::append(lineS,wall.GetPoint2());
+ _currentRoom.push_back(lineS);
+
+ }
+
+ const std::vector<Obstacle* > obstacles = _subRoom->GetAllObstacles();
+
+ for (Obstacle* obstacle:obstacles)
+ {
+ for (Wall wall: obstacle->GetAllWalls())
+ {
+ Linestring lineS;
+ boost::geometry::append(lineS,wall.GetPoint1());
+ boost::geometry::append(lineS,wall.GetPoint2());
+ _currentRoom.push_back(lineS);
+ }
+ }
+}
+
+void InternNavigationNetwork::AddVertex(const NavLine *navLine)
+{
+ Vertex v = boost::add_vertex(_graph);
+ _navLines.push_back(std::pair<const NavLine*,Vertex> (navLine,v));
+}
+
+
+void InternNavigationNetwork::AddEdge(const NavLine *navLine1, const NavLine *navLine2)
+{
+ //find indeces of vertices(landmarks) in graph
+ Vertex A;
+ Vertex B;
+
+ for (auto it=_navLines.begin(); it!=_navLines.end(); ++it)
+ {
+ int counter=0;
+ if (it->first==navLine1)
+ {
+ A = it->second;
+ counter++;
+ if (counter==2)
+ break;
+ }
+ else if (it->first==navLine2)
+ {
+ B = it->second;
+ counter++;
+ if (counter==2)
+ break;
+ }
+ }
+
+ //vector between navLines
+ Point vector = navLine1->GetCentre()-navLine2->GetCentre();
+ //distance between navLines
+ double distance = vector.Norm();
+ _connections.push_back(std::pair<Edge,Weight>(Edge(A,B),distance));
+
+ boost::add_edge(A,B,distance,_graph);
+ boost::add_edge(B,A,distance,_graph);
+}
+
+const NavLine *InternNavigationNetwork::GetNextNavLineOnShortestPathToTarget(const Point &pos, const NavLine *target)
+{
+ // new vertex for the actual position of the pedestrian
+ Vertex v = boost::add_vertex(_graph);
+
+ // check whether actual position has intervisual contact with doors or hlines
+
+ //building lines from center points of the navlines and pos and check if those lines intersect the polygon representing the current room
+ for (auto &it:_navLines)
+ {
+ if (!LineIntersectsWalls(std::make_pair<const Point&,const Point&>(it.first->GetCentre(),pos),_currentRoom))
+ {
+ //vector between navLines
+ Point vector = pos-target->GetCentre();
+ //distance between navLines
+ double distance = vector.Norm();
+
+ boost::add_edge(v,it.second,distance,_graph);
+ boost::add_edge(it.second,v,distance,_graph);
+ }
+ }
+
+ //determine Vertex belonging to ptrNavline target
+ int targetVertex=-1;
+
+ for (auto it=_navLines.begin(); it!=_navLines.end(); ++it)
+ {
+ if (it->first==target)
+ {
+ targetVertex=it->second;
+ break;
+ }
+ }
+
+ if (targetVertex==-1)
+ return nullptr;
+
+ // determine shortest path to target
+ std::vector<Vertex> pMap(boost::num_vertices(_graph));
+ boost::dijkstra_shortest_paths(_graph, targetVertex, boost::predecessor_map(&pMap[0]));
+ // next vertex on shortest path:
+ Vertex predecessor = pMap[v];
+
+
+ //remove pos from graph network
+ boost::clear_vertex(v,_graph);
+ boost::remove_vertex(v,_graph);
+
+ //return navline belonging to predecessor vertex on shortest path
+ for (auto it=_navLines.begin(); it!=_navLines.end(); ++it)
+ {
+ if (it->second==predecessor)
+ {
+ return it->first;
+ }
+ }
+
+ return nullptr;
+
+}
+
+void InternNavigationNetwork::EstablishConnections()
+{
+
+ // Check which navlines have (vice-versa) visible connectivity
+
+ //building lines from center points of the navlines and check if those lines intersect the polygon representing the current room
+ for (auto it=_navLines.begin(); it!=_navLines.end();++it)
+ {
+ auto it2=it;
+ it2++;
+ for (; it2!=_navLines.end(); ++it2)
+ {
+ if (it->first!=it2->first)
+ {
+ if (!LineIntersectsWalls(std::make_pair<const Point&,const Point&>(it->first->GetCentre(),it2->first->GetCentre()),_currentRoom))
+ {
+ AddEdge(it->first,it2->first);
+ }
+ }
+ }
+ }
+}
+
+bool InternNavigationNetwork::LineIntersectsWalls(const std::pair<const Point&, const Point&> &line, const std::vector<Linestring> &walls)
+{
+
+ Linestring lineS;
+ boost::geometry::append(lineS,line.first);
+ boost::geometry::append(lineS,line.second);
+
+ for (Linestring wall:walls)
+ {
+ if (boost::geometry::intersects(wall,lineS))
+ return true;
+ }
+
+ return false;
+
+}
diff --git a/routing/ai_router_trips/cognitiveMap/internnavigationnetwork.h b/routing/ai_router_trips/cognitiveMap/internnavigationnetwork.h
new file mode 100644
index 0000000000000000000000000000000000000000..daef48896abf177e84b83a95b5220ca50fda7286
--- /dev/null
+++ b/routing/ai_router_trips/cognitiveMap/internnavigationnetwork.h
@@ -0,0 +1,62 @@
+#ifndef INTERNNAVIGATIONNETWORK_H
+#define INTERNNAVIGATIONNETWORK_H
+
+
+//based on boost
+
+#include <boost/graph/graph_traits.hpp>
+#include <boost/graph/adjacency_list.hpp>
+#include <boost/graph/dijkstra_shortest_paths.hpp>
+#include "../../../geometry/SubRoom.h"
+#include "../../../geometry/NavLine.h"
+
+typedef boost::adjacency_list<boost::listS, boost::vecS, boost::directedS,
+ boost::no_property, boost::property<boost::edge_weight_t, double> > Graph;
+typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
+typedef boost::geometry::model::polygon<Point> BoostPolygon;
+typedef boost::geometry::model::linestring<Point> Linestring;
+
+
+typedef std::pair<Vertex, Vertex> Edge;
+typedef double Weight;
+typedef std::vector<Edge> Edges;
+
+
+
+class InternNavigationNetwork
+{
+public:
+ InternNavigationNetwork();
+ InternNavigationNetwork(const SubRoom* subRoom);
+
+ void AddVertex(const NavLine* navLine); // add navline (crossing / transition or HLine)
+ //void RemoveVertex(ptrNavLine navLine);
+ void AddEdge(const NavLine* navLine1, const NavLine* navLine2); // add connection (edge) between two navlines. Function should only be
+ // used if each of the navline is visible from the position of the other.
+ //void RemoveEdge(ptrNavLine navLine1, ptrNavLine navLine2);
+
+ const NavLine* GetNextNavLineOnShortestPathToTarget(const Point& pos, const NavLine* target);
+
+ //Create edges (connection (edge) will be established if two vertices are visible from each other
+ void EstablishConnections();
+
+
+
+private:
+ Graph _graph;
+ std::list<std::pair<const NavLine*,Vertex>> _navLines; //vertices
+ std::list<std::pair<Edge,Weight>> _connections; //edges; Weight equals length
+ const SubRoom* _subRoom;
+ // current Subroom as boost polygon
+ std::vector<Linestring> _currentRoom;
+
+ bool LineIntersectsWalls(const std::pair<const Point&, const Point&> &line, const std::vector<Linestring> &walls);
+
+
+
+
+
+
+};
+
+#endif // INTERNNAVIGATIONNETWORK_H
diff --git a/routing/ai_router_trips/cognitiveMap/landmark.cpp b/routing/ai_router_trips/cognitiveMap/landmark.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..16d7a1eca50425aad5b16c3366847f9aebeffe53
--- /dev/null
+++ b/routing/ai_router_trips/cognitiveMap/landmark.cpp
@@ -0,0 +1,198 @@
+#include "landmark.h"
+#include "associations.h"
+#include <math.h>
+#include "../../../general/Macros.h"
+
+AILandmark::AILandmark(const Point &pos)
+{
+ _realPos=pos;
+ _visited=false;
+ _posInMap=Point(0.0,0.0);
+}
+
+AILandmark::AILandmark(const Point& pos, double a, double b, int id)
+{
+ _realPos=pos;
+ _a=a;
+ _b=b;
+ _visited=false;
+ _id=id;
+ _posInMap=Point(0.0,0.0);
+}
+
+AILandmark::~AILandmark()
+{
+
+}
+
+void AILandmark::SetId(int id)
+{
+ _id=id;
+}
+
+void AILandmark::SetA(double a)
+{
+ _a=a;
+}
+
+void AILandmark::SetB(double b)
+{
+ _b=b;
+}
+
+void AILandmark::SetRealPos(const Point &point)
+{
+ _realPos=point;
+}
+
+void AILandmark::SetPosInMap(const Point &point)
+{
+ _posInMap=point;
+}
+
+
+
+void AILandmark::SetCaption(const std::string &string)
+{
+ _caption=string;
+}
+
+void AILandmark::SetType(const std::string& type)
+{
+ _type=type;
+}
+
+int AILandmark::GetId() const
+{
+ return _id;
+}
+
+const Point &AILandmark::GetRealPos() const
+{
+ return _realPos;
+}
+
+const Point &AILandmark::GetPosInMap() const
+{
+ return _posInMap;
+}
+
+double AILandmark::GetA() const
+{
+ return _a;
+}
+
+double AILandmark::GetB() const
+{
+ return _b;
+}
+
+const std::string& AILandmark::GetType() const
+{
+ return _type;
+}
+
+
+const std::string &AILandmark::GetCaption() const
+{
+ return _caption;
+}
+
+
+Point AILandmark::GetRandomPoint() const
+{
+ const double pi = M_PI;//std::acos(-1);
+ int alpha1 = std::rand() % 360;
+ double factor_a = (std::rand() % 100)/100.0;
+ double x = _posInMap._x+std::cos(alpha1*pi/180.0)*factor_a*_a;
+ int alpha2 = std::rand() % 360;
+ double factor_b = (std::rand() % 100)/100.0;
+ double y = _posInMap._y+std::sin(alpha2*pi/180.0)*factor_b*_b;
+
+ return Point(x,y);
+}
+
+Point AILandmark::PointOnShortestRoute(const Point& point) const
+{
+ const double pi = std::acos(-1);
+ double distance;
+ int alpha_min=1;
+ double t_min=0;
+ double min=std::sqrt(std::pow((_posInMap._x+t_min*_a*std::cos(pi/180.0))-point._x,2)+std::pow((_posInMap._x+t_min*_b*std::sin(pi/180.0))-point._y,2));
+
+ for (double t=0.2; t<=1; t+=0.2)
+ {
+ for (int alpha=11; alpha<=360; alpha+=10)
+ {
+ distance=std::sqrt(std::pow((_posInMap._x+t*_a*std::cos(alpha*pi/180.0))-point._x,2)+std::pow((_posInMap._y+t*_b*std::sin(alpha*pi/180.0))-point._y,2));
+ if (distance<min)
+ {
+ min=distance;
+ alpha_min=alpha;
+ t_min=t;
+ }
+ }
+ }
+ //Log->Write(std::to_string(min));
+ return Point(_posInMap._x+_a*std::cos(alpha_min*pi/180.0),_posInMap._y+_b*std::sin(alpha_min*pi/180.0));
+}
+
+//bool Landmark::LandmarkReached(const Point& currentYAH)
+//{
+// if (std::abs(_realPos._x-currentYAH._x)<0.75*_a && std::abs(_realPos._y-currentYAH._y)<0.75*_b)
+// {
+// Log->Write("INFO:\t Landmark reached");
+// Log->Write(std::to_string(currentYAH._x)+" "+std::to_string(currentYAH._y));
+// _visited=true;
+// return true;
+// }
+// return false;
+//}
+
+bool AILandmark::Visited() const
+{
+ return _visited;
+}
+
+void AILandmark::SetVisited(bool stat)
+{
+ _visited=stat;
+}
+
+bool AILandmark::Contains(const Point &point) const
+{
+ double x = _a-(std::fabs(this->GetPosInMap()._x-point._x));
+ double y = _b-(std::fabs(this->GetPosInMap()._y-point._y));
+
+ if ((std::fabs(this->GetPosInMap()._x-point._x)<=_a && std::pow((1-std::pow(x,2)/std::pow(_a,2)),0.5)*_b<=_b)
+ || (std::fabs(this->GetPosInMap()._y-point._y)<=_b && std::pow((1-std::pow(y,2)/std::pow(_b,2)),0.5)*_a<=_a ))
+ return true;
+
+ return false;
+}
+
+const AIAssociations &AILandmark::GetAssociations() const
+{
+ return _assoContainer;
+}
+
+void AILandmark::AddAssociation(const AIAssociation &asso)
+{
+ if (std::find(_assoContainer.begin(), _assoContainer.end(), asso)!=_assoContainer.end())
+ return;
+ else
+ _assoContainer.push_back(asso);
+}
+
+const AIRegion *AILandmark::IsInRegion() const
+{
+ return _region;
+}
+
+void AILandmark::SetRegion(const AIRegion *region)
+{
+ _region=region;
+}
+
+
+
diff --git a/routing/ai_router_trips/cognitiveMap/landmark.h b/routing/ai_router_trips/cognitiveMap/landmark.h
new file mode 100644
index 0000000000000000000000000000000000000000..a6baccbcb62ae38626f613b32acd038d6f2adb2f
--- /dev/null
+++ b/routing/ai_router_trips/cognitiveMap/landmark.h
@@ -0,0 +1,82 @@
+#ifndef LANDMARK_H
+#define LANDMARK_H
+
+#include <vector>
+#include <list>
+#include "../../../geometry/Point.h"
+#include "associations.h"
+
+
+
+using AIAssociations = std::vector<AIAssociation>;
+
+class AIRegion;
+
+
+class AILandmark
+{
+
+public:
+ AILandmark(const Point& pos);
+ AILandmark(const Point &pos, double a, double b, int id=-1);
+ ~AILandmark();
+
+
+
+ //Setter
+ void SetId(int id);
+ void SetA(double a);
+ void SetB(double b);
+ void SetRealPos(const Point& point);
+ void SetPosInMap(const Point& point);
+ void SetCaption(const std::string& string);
+// void SetPriority(double priority);
+ void SetType(const std::string &type);
+ //Getter
+ int GetId() const;
+ const Point& GetRealPos() const;
+ const Point& GetPosInMap() const;
+ double GetA() const;
+ double GetB() const;
+ const std::string& GetType() const;
+ const std::string& GetCaption() const;
+ //double GetPriority() const;
+ //Random point somewhere within the waypoint
+ Point GetRandomPoint() const;
+ // Shortest Distance from waypoint egde (ellipse) to specific point
+ Point PointOnShortestRoute(const Point &point) const;
+ // Check if Waypoint reached (if YAH-Pointer is in Waypoint)
+ //bool LandmarkReached(const Point& currentYAH);
+
+ bool Visited() const;
+ void SetVisited(bool stat);
+
+ //check if landmark ellipse in cogmap contains certain point
+ bool Contains(const Point &point) const;
+
+ // Associations
+ const AIAssociations& GetAssociations() const;
+ void AddAssociation(const AIAssociation& asso);
+
+ //Region
+ const AIRegion* IsInRegion() const;
+ void SetRegion(const AIRegion* region);
+
+
+
+private:
+ int _id;
+ std::string _caption;
+ Point _realPos;
+ Point _posInMap;
+ double _a;
+ double _b;
+ //double _priority;
+ bool _visited;
+ std::string _type;
+ std::list<int> _connectedWith;
+ AIAssociations _assoContainer;
+ const AIRegion* _region;
+};
+
+#endif // LANDMARK_H
diff --git a/routing/ai_router_trips/cognitiveMap/landmarknetwork.cpp b/routing/ai_router_trips/cognitiveMap/landmarknetwork.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..32bc0b460325b4a4a6c1d6172832b33c3c8985a3
--- /dev/null
+++ b/routing/ai_router_trips/cognitiveMap/landmarknetwork.cpp
@@ -0,0 +1,258 @@
+#include "landmarknetwork.h"
+#include "region.h"
+
+// In the following example, we construct a graph and apply dijkstra_shortest_paths().
+// The complete source code for the example is in examples/dijkstra-example.cpp.
+// Dijkstra's algorithm computes the shortest distance from the starting vertex to every other vertex in the graph.
+
+// Dijkstra's algorithm requires that a weight property is associated with each edge and a distance property with each vertex.
+// Here we use an internal property for the weight and an external property for the distance.
+// For the weight property we use the property class and specify int as the type used to represent
+// weight values and edge_weight_t for the property tag (which is one of the BGL predefined property tags).
+// The weight property is then used as a template argument for adjacency_list.
+
+// The listS and vecS types are selectors that determine the data structure used inside the adjacency_list
+// (see Section Choosing the Edgelist and VertexList). The directedS type specifies that the graph
+// should be directed (versus undirected). The following code shows the specification of the graph
+// type and then the initialization of the graph. The edges and weights are passed to the graph
+// constructor in the form of iterators (a pointer qualifies as a RandomAccessIterator).
+
+
+// For the external distance property we will use a std::vector for storage.
+// BGL algorithms treat random access iterators as property maps,
+// so we can just pass the beginning iterator of the distance
+// vector to Dijkstra's algorithm. Continuing the above example,
+// the following code shows the creation of the distance vector,
+// the call to Dijkstra's algorithm (implicitly using the
+// internal edge weight property), and then the output of the results.
+
+// typedef adjacency_list<listS, vecS, directedS,
+// no_property, property<edge_weight_t, int> > Graph;
+// typedef graph_traits<Graph>::vertex_descriptor Vertex;
+// typedef std::pair<int,int> E;
+
+// const int num_nodes = 5;
+// E edges[] = { E(0,2),
+// E(1,1), E(1,3), E(1,4),
+// E(2,1), E(2,3),
+// E(3,4),
+// E(4,0), E(4,1) };
+// int weights[] = { 1, 2, 1, 2, 7, 3, 1, 1, 1};
+
+// Graph G(edges, edges + sizeof(edges) / sizeof(E), weights, num_nodes);
+
+
+// // vector for storing distance property
+// std::vector<int> d(num_vertices(G));
+
+// // get the first vertex
+// Vertex s = *(vertices(G).first);
+// // invoke variant 2 of Dijkstra's algorithm
+// dijkstra_shortest_paths(G, s, distance_map(&d[0]));
+
+// std::cout << "distances from start vertex:" << std::endl;
+// graph_traits<Graph>::vertex_iterator vi;
+// for(vi = vertices(G).first; vi != vertices(G).second; ++vi)
+// std::cout << "distance(" << index(*vi) << ") = "
+// << d[*vi] << std::endl;
+// std::cout << std::endl;
+
+
+AILandmarkNetwork::AILandmarkNetwork()
+{
+ _graph=Graph();
+}
+
+AILandmarkNetwork::AILandmarkNetwork(const AIRegion* region, const AILandmarks &landmarks, const std::vector<AIConnection> &connections)
+{
+ _region=region;
+ _graph=Graph();
+
+ for (const AILandmark& landmark:landmarks)
+ {
+ AddLandmark(&landmark);
+ }
+
+
+ for (const AIConnection& connection:connections)
+ {
+ AddConnection(&connection);
+
+ }
+}
+
+
+AILandmarkNetwork::~AILandmarkNetwork()
+{
+
+}
+
+void AILandmarkNetwork::AddLandmark(const AILandmark *landmark)
+{
+
+ Vertex v = boost::add_vertex(_graph);
+ //std::make_pair<int,int>(1,1);
+ _landmarks.emplace(landmark,v);
+
+}
+
+void AILandmarkNetwork::RemoveLandmark(const AILandmark *landmark)
+{
+
+
+ for (auto it=_landmarks.begin(); it!=_landmarks.end(); ++it)
+ {
+ if (it->first==landmark)
+ {
+ boost::clear_vertex(it->second,_graph);
+ RemoveAdjacentEdges(it->second);
+ boost::remove_vertex(it->second,_graph);
+ _landmarks.erase(it);
+ break;
+ }
+ }
+
+
+
+}
+
+void AILandmarkNetwork::AddConnection(const AIConnection *connection)
+{
+ //find indeces of vertices(landmarks) in graph
+ const AILandmark* landmarkA = _region->GetLandmarkByID(connection->GetLandmarkIds().first);
+ const AILandmark* landmarkB = _region->GetLandmarkByID(connection->GetLandmarkIds().second);
+ Vertex A;
+ Vertex B;
+
+// for (auto it=_landmarks.begin(); it!=_landmarks.end(); ++it)
+// {
+// int counter=0;
+// if (it->first==landmarkA)
+// {
+// A = it->second;
+// counter++;
+// if (counter==2)
+// break;
+// }
+// else if (it->first==landmarkB)
+// {
+// B = it->second;
+// counter++;
+// if (counter==2)
+// break;
+// }
+// }
+
+ A=_landmarks[landmarkA];
+ B=_landmarks[landmarkB];
+
+ Point vector = landmarkA->GetRandomPoint()-landmarkB->GetRandomPoint();//->GetRandomPoint()-landmarkB->GetRandomPoint();
+ double distance = vector.Norm();
+ _connections.push_back(std::pair<Edge,Weight>(Edge(A,B),distance));
+
+ boost::add_edge(A,B,distance,_graph);
+ boost::add_edge(B,A,distance,_graph);
+}
+
+std::pair<std::vector<const AILandmark*>,double> AILandmarkNetwork::LengthofShortestPathToTarget(const AILandmark *landmark, const AILandmark *target) const
+{
+
+ if (landmark==target)
+ return std::make_pair<std::vector<const AILandmark*>,double>(std::vector<const AILandmark*>{landmark},0.0);
+
+ int startVertex=-1;
+ int targetVertex=-1;
+
+ // get the start vertex
+
+
+// for (auto it=_landmarks.begin(); it!=_landmarks.end(); ++it)
+// {
+
+// int counter=0;
+// if (it->first==landmark)
+// {
+// startVertex=it->second;
+// counter++;
+// if (counter==2)
+// break;
+// }
+// else if (it->first==target)
+// {
+// targetVertex=it->second;
+// counter++;
+// if (counter==2)
+// break;
+// }
+
+// }
+
+ startVertex=_landmarks.at(landmark);
+ targetVertex=_landmarks.at(target);
+
+
+ if (targetVertex==-1 || startVertex==-1)
+ throw std::invalid_argument("Wrong target or landmark!");
+
+ //std::vector<double> edgeWeights;
+
+ // vector for storing distance property
+ std::vector<Vertex> p(boost::num_vertices(_graph));
+ std::vector<double> d(boost::num_vertices(_graph));
+
+// for (auto it =_connections.begin(); it!=_connections.end(); ++it)
+// {
+// edgeWeights.push_back(it->second);
+// }
+
+ // invoke variant 2 of Dijkstra's algorithm
+ //boost::dijkstra_shortest_paths(_graph, startVertex, boost::distance_map(&d[0]));
+ boost::dijkstra_shortest_paths(_graph, startVertex,
+ boost::predecessor_map(boost::make_iterator_property_map(p.begin(), get(boost::vertex_index, _graph))).
+ distance_map(boost::make_iterator_property_map(d.begin(), get(boost::vertex_index, _graph))));
+
+ //std::cout << "distances and parents:" << std::endl;
+ Vertex vi=targetVertex;
+
+
+ std::vector<const AILandmark*> landmarksOnShortestPath;
+
+ while (vi!=startVertex)
+ {
+ for (auto it=_landmarks.begin(); it!=_landmarks.end(); ++it)
+ {
+ if (it->second==vi)
+ {
+ landmarksOnShortestPath.push_back(it->first);
+
+ break;
+ }
+ //Log->Write(std::to_string(vi));
+
+ }
+ vi=p[vi];
+
+ //std::cout << d[targetVertex] << std::endl;
+ //std::cout << "parent(" << name[*vi] << ") = " << name[p[*vi]] << std::endl;
+ }
+ landmarksOnShortestPath.push_back(landmark);
+ //std::cout << "distance from start vertex to target:" << std::endl;
+// boost::graph_traits<Graph>::vertex_iterator vi;
+// for(vi = boost::vertices(_graph).first; vi != boost::vertices(_graph).second; ++vi)
+// std::cout << "distance = " << d[*vi] << std::endl;
+
+ //Log->Write(std::to_string(landmarksOnShortestPath.size()));
+ return std::make_pair(landmarksOnShortestPath,d[targetVertex]);
+}
+
+void AILandmarkNetwork::RemoveAdjacentEdges(const Vertex &vertex)
+{
+ for (auto it=_connections.begin(); it!=_connections.end(); ++it)
+ {
+ if (it->first.first == vertex || it->first.second == vertex)
+ {
+ _connections.remove(*it);
+ }
+ }
+}
+
diff --git a/routing/ai_router_trips/cognitiveMap/landmarknetwork.h b/routing/ai_router_trips/cognitiveMap/landmarknetwork.h
new file mode 100644
index 0000000000000000000000000000000000000000..1a8e0dc0a40e49c82f6616a18bc0aad82ea61859
--- /dev/null
+++ b/routing/ai_router_trips/cognitiveMap/landmarknetwork.h
@@ -0,0 +1,52 @@
+#ifndef LANDMARKNETWORK_H
+#define LANDMARKNETWORK_H
+
+
+
+//based on boost
+
+#include <boost/graph/graph_traits.hpp>
+#include <boost/graph/adjacency_list.hpp>
+#include <boost/graph/dijkstra_shortest_paths.hpp>
+#include "connection.h"
+#include "landmark.h"
+#include <unordered_map>
+
+
+
+typedef boost::adjacency_list<boost::listS, boost::vecS, boost::directedS,
+ boost::no_property, boost::property<boost::edge_weight_t, double> > Graph;
+typedef boost::graph_traits<Graph>::vertex_descriptor Vertex;
+typedef std::pair<Vertex, Vertex> Edge;
+typedef double Weight;
+typedef std::vector<Edge> Edges;
+
+
+class AILandmarkNetwork
+{
+public:
+ AILandmarkNetwork();
+ AILandmarkNetwork(const AIRegion *region, const AILandmarks& landmarks, const std::vector<AIConnection> &connections);
+ ~AILandmarkNetwork();
+ void AddLandmark(const AILandmark* landmark);
+ void RemoveLandmark(const AILandmark* landmark);
+ void AddConnection(const AIConnection* connection);
+
+ //Calculations
+ std::pair<std::vector<const AILandmark *>, double> LengthofShortestPathToTarget(const AILandmark *landmark, const AILandmark *target) const;
+
+
+private:
+ Graph _graph;
+ std::unordered_map<const AILandmark*,Vertex> _landmarks; //vertices
+ std::list<std::pair<Edge,Weight>> _connections; //edges; Weight equals length
+ //between two random point in the connected landmarks
+ const AIRegion* _region;
+
+ void RemoveAdjacentEdges(const Vertex& vertex);
+
+
+
+};
+
+#endif // LANDMARKNETWORK_H
diff --git a/routing/ai_router_trips/cognitiveMap/region.cpp b/routing/ai_router_trips/cognitiveMap/region.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ddf0b49f8106649876c96513cdb953841846e123
--- /dev/null
+++ b/routing/ai_router_trips/cognitiveMap/region.cpp
@@ -0,0 +1,112 @@
+#include "region.h"
+
+AIRegion::AIRegion(const Point &pos) : AILandmark(pos)
+{
+
+}
+
+AIRegion::~AIRegion()
+{
+
+}
+
+void AIRegion::AddLandmark(const AILandmark &landmark)
+{
+ _landmarks.push_back(landmark);
+
+}
+
+void AIRegion::AddLandmarkSubCs(const AILandmark &landmark)
+{
+ _landmarksSubConcious.push_back(landmark);
+}
+
+const AILandmark *AIRegion::GetRegionAsLandmark() const
+{
+ return static_cast<const AILandmark*>(this);
+}
+
+const AILandmarks &AIRegion::GetLandmarks() const
+{
+ return _landmarks;
+}
+
+const AILandmark *AIRegion::GetLandmarkByID(int ID) const
+{
+ for (const AILandmark& landmark:_landmarks)
+ {
+ if (landmark.GetId()==ID)
+ return &landmark;
+ }
+ return nullptr;
+}
+
+bool AIRegion::ContainsLandmark(const AILandmark* landmark) const
+{
+ for (const AILandmark& clandmark:_landmarks)
+ {
+ if (&clandmark==landmark)
+ return true;
+ }
+ return false;
+}
+
+void AIRegion::InitLandmarkNetwork()
+{
+ _landmarkNetwork = AILandmarkNetwork(this,GetLandmarks(),GetAllConnections());
+}
+
+std::pair<std::vector<const AILandmark*>,double> AIRegion::PathLengthFromLandmarkToTarget(const AILandmark* landmark, const AILandmark* target) const
+{
+ return _landmarkNetwork.LengthofShortestPathToTarget(landmark,target);
+}
+
+
+const std::vector<AIConnection>& AIRegion::GetAllConnections() const
+{
+ return _connections;
+}
+
+void AIRegion::AddConnection(const AIConnection& connection)
+{
+ _connections.push_back(connection);
+}
+
+void AIRegion::AddConnection(int id, const std::string& caption, const std::string& type, int landmarkId1, int landmarkId2)
+{
+ _connections.push_back(AIConnection(id,caption,type,landmarkId1,landmarkId2));
+}
+
+void AIRegion::RemoveConnections(const AILandmark* landmark)
+{
+ for (auto it=_connections.begin(); it!=_connections.end(); ++it)
+ {
+ if (it->GetLandmarkIds().first==landmark->GetId() || it->GetLandmarkIds().second==landmark->GetId())
+ {
+ it=_connections.erase(it);
+ --it;
+ }
+ }
+}
+
+std::vector<const AILandmark*> AIRegion::ConnectedWith(const AILandmark *landmark) const
+{
+ std::vector<const AILandmark*> cLandmarks;
+ for (const AIConnection& connection:_connections)
+ {
+ if (connection.GetLandmarkIds().first==landmark->GetId())
+ {
+ cLandmarks.push_back(this->GetLandmarkByID(connection.GetLandmarkIds().second));
+ }
+ else if (connection.GetLandmarkIds().second==landmark->GetId())
+ {
+ cLandmarks.push_back(this->GetLandmarkByID(connection.GetLandmarkIds().first));
+ }
+ }
+
+ return cLandmarks;
+
+}
+
+
+
diff --git a/routing/ai_router_trips/cognitiveMap/region.h b/routing/ai_router_trips/cognitiveMap/region.h
new file mode 100644
index 0000000000000000000000000000000000000000..cc89bac3c346af477d15c5e00211ae5d5b62c541
--- /dev/null
+++ b/routing/ai_router_trips/cognitiveMap/region.h
@@ -0,0 +1,46 @@
+#ifndef REGION_H
+#define REGION_H
+
+#include "landmark.h"
+#include "connection.h"
+#include "landmarknetwork.h"
+
+
+using AILandmarks = std::vector<AILandmark>;
+using AIConnections = std::vector<AIConnection>;
+
+class AIRegion : public AILandmark
+{
+public:
+ AIRegion(const Point& pos);
+ ~AIRegion();
+
+ void AddLandmark(const AILandmark& landmark);
+ void AddLandmarkSubCs(const AILandmark &landmark);
+ const AILandmark* GetRegionAsLandmark() const;
+
+ //Getter
+ const AILandmarks& GetLandmarks() const;
+ const AILandmark* GetLandmarkByID(int ID) const;
+ bool ContainsLandmark(const AILandmark* landmark) const;
+
+ //LandmarkNetwork
+ void InitLandmarkNetwork();
+ std::pair<std::vector<const AILandmark*>, double> PathLengthFromLandmarkToTarget(const AILandmark* landmark, const AILandmark* target) const;
+
+ //Connections
+ const std::vector<AIConnection> &GetAllConnections() const;
+ void AddConnection(const AIConnection &connection);
+ void AddConnection(int id, const std::string &caption, const std::string &type, int landmarkId1, int landmarkId2);
+ void RemoveConnections(const AILandmark *landmark);
+ std::vector<const AILandmark *> ConnectedWith(const AILandmark *landmark) const;
+
+private:
+ AILandmarks _landmarks;
+ AIConnections _connections;
+ AILandmarkNetwork _landmarkNetwork;
+ AIAssociations _assoContainer;
+ AILandmarks _landmarksSubConcious;
+};
+
+#endif // REGION_H
diff --git a/routing/ai_router_trips/perception/cgalgeometry.cpp b/routing/ai_router_trips/perception/cgalgeometry.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..821b3b0840a36066b0bb66ae13d1451e62477aad
--- /dev/null
+++ b/routing/ai_router_trips/perception/cgalgeometry.cpp
@@ -0,0 +1,268 @@
+#include "cgalgeometry.h"
+#include "../../../geometry/SubRoom.h"
+#include <iostream>
+
+CGALGeometry::CGALGeometry()
+{
+
+}
+
+CGALGeometry::CGALGeometry(const std::unordered_map<ptrFloor, std::vector<const Line*> > &allWalls)
+{
+
+ // for every room (floor) separately:
+
+ for (auto it=allWalls.begin(); it!=allWalls.end(); ++it)
+ {
+ //std::cout << it->first->GetCaption() << std::endl;
+ /// if geometry is not stored in advance (begin)
+ ///
+ const std::vector<const Line*>& walls=it->second;
+
+ Polygon_with_holes_2 unionR;
+
+ int n = 0;
+
+ std::vector<Polygon_2> separatedPolygons;
+
+
+ Point dirVector = walls[0]->GetPoint2()- walls[0]->GetPoint1();
+ dirVector = dirVector / dirVector.Norm();
+ Point normalVector = Point(-dirVector._y, dirVector._x);
+ Point P1 = walls[0]->GetPoint1();
+ Point P2 = walls[0]->GetPoint2();
+ Point Ps1 = P1 - normalVector * 0.12 - dirVector * 0.12;
+ Point Ps2 = P2 - normalVector * 0.12 + dirVector * 0.12;
+ Point Ps3 = P2 + normalVector * 0.12 + dirVector * 0.12;
+ Point Ps4 = P1 + normalVector * 0.12 - dirVector * 0.12;
+
+ unionR.outer_boundary().push_back(Point_2(Ps1._x, Ps1._y));
+ unionR.outer_boundary().push_back(Point_2(Ps2._x, Ps2._y));
+ unionR.outer_boundary().push_back(Point_2(Ps3._x, Ps3._y));
+ unionR.outer_boundary().push_back(Point_2(Ps4._x, Ps4._y));
+
+
+ Polygon_2 outer;
+
+
+ for (const Line* wall:walls)
+ {
+ if (n < 1)
+ {
+ n++;
+ continue;
+ }
+ dirVector = wall->GetPoint2()-wall->GetPoint1();//wall.at(1) - wall.at(0);
+ dirVector = dirVector / dirVector.Norm();
+ normalVector = Point(-dirVector._y, dirVector._x);
+ //0.12 equals the half of a wall's width
+ //BoostPolygon boostHole;
+ Polygon_2 cgalPolygon;
+
+ Point PP1 = wall->GetPoint1();
+ Point PP2 = wall->GetPoint2();
+ Point Pi1 = PP1 - normalVector * 0.12 - dirVector * 0.12;
+ Point Pi2 = PP2 - normalVector * 0.12 + dirVector * 0.12;
+ Point Pi3 = PP2 + normalVector * 0.12 + dirVector * 0.12;
+ Point Pi4 = PP1 + normalVector * 0.12 - dirVector * 0.12;
+
+ cgalPolygon.push_back(Point_2(Pi1._x, Pi1._y));
+ cgalPolygon.push_back(Point_2(Pi2._x, Pi2._y));
+ cgalPolygon.push_back(Point_2(Pi3._x, Pi3._y));
+ cgalPolygon.push_back(Point_2(Pi4._x, Pi4._y));
+
+ if (!CGAL::join(unionR, cgalPolygon, unionR))
+ {
+ separatedPolygons.push_back(cgalPolygon);
+ Log->Write(std::to_string(n)+" of " + std::to_string(walls.size()));
+ }
+
+ //outer = unionR.outer_boundary();
+
+ //try to join separated polygons
+ std::vector<Polygon_2>::iterator p = separatedPolygons.begin();
+ while (p != separatedPolygons.end()) {
+ if (CGAL::join(unionR, *p, unionR)) {
+ p = separatedPolygons.erase(p);
+ } else
+ ++p;
+ }
+ ++n;
+ }
+ std::vector<Polygon_with_holes_2> listPolygons;
+ //listPolygons.push_back(unionR);
+
+ size_t num_polygons;
+ while (!separatedPolygons.empty()) {
+ num_polygons = separatedPolygons.size();
+ std::vector<Polygon_2>::iterator p = separatedPolygons.begin();
+ while (p != separatedPolygons.end()) {
+ if (CGAL::join(unionR, *p, unionR)) {
+ p = separatedPolygons.erase(p);
+ } else
+ ++p;
+ }
+ if (num_polygons == separatedPolygons.size()) {
+
+ listPolygons.push_back(unionR);
+ unionR=Polygon_with_holes_2(separatedPolygons.front());
+ }
+ Log->Write(std::to_string(separatedPolygons.size())+" of " + std::to_string(walls.size()));
+ }
+
+ listPolygons.push_back(unionR);
+
+ std::vector<Segment_2> segments;
+
+ Point_2 pb1=Point_2(it->first->GetBoundaryVertices()[0]._x-1,it->first->GetBoundaryVertices()[0]._y-1);
+ Point_2 pb2=Point_2(it->first->GetBoundaryVertices()[3]._x+1,it->first->GetBoundaryVertices()[3]._y-1);
+ Point_2 pb3=Point_2(it->first->GetBoundaryVertices()[2]._x+1,it->first->GetBoundaryVertices()[2]._y+1);
+ Point_2 pb4=Point_2(it->first->GetBoundaryVertices()[1]._x-1,it->first->GetBoundaryVertices()[1]._y+1);
+// std::cout << pb1.x() << " " << pb1.y() << std::endl;
+// std::cout << pb2.x() << " " << pb2.y() << std::endl;
+// std::cout << pb3.x() << " " << pb3.y() << std::endl;
+// std::cout << pb4.x() << " " << pb4.y() << std::endl;
+ segments.push_back(Segment_2(pb1,pb2));
+ segments.push_back(Segment_2(pb2,pb3));
+ segments.push_back(Segment_2(pb3,pb4));
+ segments.push_back(Segment_2(pb4,pb1));
+
+ std::cout << "Room" << it->first->GetID() << std::endl;
+ for (Polygon_with_holes_2 poly:listPolygons)
+ {
+ outer = poly.outer_boundary();
+
+
+// Point veryFirstP=Point(roundfloat(CGAL::to_double(outer[0].x()),1),roundfloat(CGAL::to_double(outer[0].y()),1));
+// Point lastPoint = veryFirstP;
+
+// std::cout << "Polygon" << std::endl;
+// std::cout << lastPoint._x << " " << lastPoint._y << std::endl;
+
+ size_t i = 1;
+ for (; i < outer.size(); ++i)
+ {
+// Point newPoint = Point(roundfloat(CGAL::to_double(outer[i].x()),1),roundfloat(CGAL::to_double(outer[i].y()),1));
+// if (newPoint!=lastPoint && newPoint!=veryFirstP)
+// {
+// segments.push_back(Segment_2(Point_2(lastPoint._x,lastPoint._y), Point_2(newPoint._x,newPoint._y)));//-1].y()),Point_2(outer[i].x(),outer[i].y())));
+// lastPoint=newPoint;
+// std::cout << newPoint._x << " " << newPoint._y << std::endl;/
+ segments.push_back(Segment_2(outer[i-1],outer[i]));
+// }
+ }
+ segments.push_back(Segment_2(outer[i-1],outer[0]));
+// Point_2 CGALlastP = Point_2(lastPoint._x,lastPoint._y);
+// Point_2 CGALFirstP = Point_2(veryFirstP._x,veryFirstP._y);
+// segments.push_back(Segment_2(CGALlastP,CGALFirstP));
+
+
+ }
+
+
+ /// if geometry is not stored in advance (end)
+ ///
+ /// if geometry is stored in file in advance
+
+// std::vector<Segment_2> segments;
+
+
+// Point_2 pb1=Point_2(it->first->GetBoundaryVertices()[0]._x-1,it->first->GetBoundaryVertices()[0]._y-1);
+// Point_2 pb2=Point_2(it->first->GetBoundaryVertices()[3]._x+1,it->first->GetBoundaryVertices()[3]._y-1);
+// Point_2 pb3=Point_2(it->first->GetBoundaryVertices()[2]._x+1,it->first->GetBoundaryVertices()[2]._y+1);
+// Point_2 pb4=Point_2(it->first->GetBoundaryVertices()[1]._x-1,it->first->GetBoundaryVertices()[1]._y+1);
+// segments.push_back(Segment_2(pb1,pb2));
+// segments.push_back(Segment_2(pb2,pb3));
+// segments.push_back(Segment_2(pb3,pb4));
+// segments.push_back(Segment_2(pb4,pb1));
+
+
+// std::string line;
+// std::string filename = "D:/Dokumente/jpstests/osloerstr/polygons_id"+std::to_string(it->first->GetID())+".txt";
+// std::cout << filename << std::endl;
+// std::ifstream myfile (filename);
+// std::vector<std::vector<Point_2>> polyVertices;
+// Point oldPoint=Point(FLT_MAX,FLT_MAX);
+// if (myfile.is_open())
+// {
+// while ( std::getline (myfile,line) )
+// {
+// if (line=="Polygon")
+// {
+// polyVertices.push_back(std::vector<Point_2>{});
+// }
+// else
+// {
+// std::stringstream linestream(line);
+// //std::string data;
+// //std::getline(linestream, data, ';');
+// double x;
+// double y;
+// linestream >> x >> y;
+// Point point = Point(x,y);
+// Point vec = oldPoint-point;
+// if (vec.Norm()>=0.1)
+// {
+// polyVertices.back().push_back(Point_2(point._x,point._y));
+// //Log->Write(std::to_string(it->first->GetID()));
+// //Log->Write(std::to_string(point._x)+" "+std::to_string(point._y));
+// oldPoint=point;
+// }
+// }
+// }
+// myfile.close();
+// }
+
+// else
+// Log->Write("ERROR \t Unable to open file");
+
+
+// for (std::vector<Point_2> vector:polyVertices)
+// {
+// size_t i = 1;
+// for (; i < vector.size(); ++i)
+// {
+// segments.push_back(Segment_2(vector[i - 1], vector[i]));
+// }
+// segments.push_back(Segment_2(vector.back(), vector.front()));
+// }
+ /// end if
+
+
+
+ // insert geometry into the arrangement
+
+ Arrangement_2 env;
+ CGAL::insert_non_intersecting_curves(env, segments.begin(), segments.end());
+
+ //associate room with visible env (walls) of that room
+ _CGAL_env.emplace(it->first,env);
+
+ //std::shared_ptr<TEV> tev=std::make_shared<TEV>(env);
+
+ //associate room with TEV
+ _tev.emplace(it->first,std::unique_ptr<TEV>(new TEV(env)));
+
+
+ }
+
+}
+
+
+
+
+const TEV *CGALGeometry::GetTEV(ptrFloor floor) const
+{
+ return _tev.at(floor).get();
+}
+
+const Arrangement_2 &CGALGeometry::GetVisEnvCGAL(ptrFloor floor) const
+{
+ return _CGAL_env.at(floor);
+}
+
+
+float roundfloat(float num, int precision)
+{
+ return floorf(num * pow(10.0f,precision) + .5f)/pow(10.0f,precision);
+}
diff --git a/routing/ai_router_trips/perception/cgalgeometry.h b/routing/ai_router_trips/perception/cgalgeometry.h
new file mode 100644
index 0000000000000000000000000000000000000000..dd6dba67ca9c82bc784d4c66995073594fab45dc
--- /dev/null
+++ b/routing/ai_router_trips/perception/cgalgeometry.h
@@ -0,0 +1,53 @@
+#ifndef CGALGEOMETRY_H
+#define CGALGEOMETRY_H
+
+//#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
+#include <CGAL/Simple_cartesian.h>
+#include <CGAL/Boolean_set_operations_2.h>
+#include <CGAL/Triangular_expansion_visibility_2.h>
+#include <CGAL/Arr_segment_traits_2.h>
+#include <CGAL/Arr_naive_point_location.h>
+#include <CGAL/Arrangement_2.h>
+//#include <CGAL/Lazy_exact_nt.h>
+
+
+#include <unordered_map>
+
+#include "../../../geometry/Room.h"
+
+class Building;
+class Line;
+
+//typedef CGAL::Lazy_exact_nt<CGAL::Quotient<CGAL::MP_Float> > NT;
+//CGAL::Simple_cartesian::Point_2 CGAL::Lazy_exact_nt<
+//typedef CGAL::Exact_predicates_inexact_constructions_kernel Kernel;
+typedef CGAL::Simple_cartesian<CGAL::Lazy_exact_nt<CGAL::Gmpq>> Kernel;
+typedef Kernel::Point_2 Point_2;
+//Exact_predicates_inexact_constructions_kernel::Point_3
+typedef Kernel::Segment_2 Segment_2;
+typedef CGAL::Polygon_2<Kernel> Polygon_2;
+typedef CGAL::Polygon_with_holes_2<Kernel> Polygon_with_holes_2;
+typedef CGAL::Arr_segment_traits_2<Kernel> Traits_2;
+typedef CGAL::Arrangement_2<Traits_2> Arrangement_2;
+typedef CGAL::Triangular_expansion_visibility_2<Arrangement_2> TEV;
+
+using ptrFloor = const Room*;
+
+class CGALGeometry
+{
+public:
+ explicit CGALGeometry();
+ explicit CGALGeometry(const std::unordered_map<ptrFloor, std::vector<const Line *> > &allWalls);
+
+ const Arrangement_2 &GetVisEnvCGAL(ptrFloor floor) const;
+ const TEV* GetTEV(ptrFloor floor) const;
+
+private:
+
+ std::unordered_map<ptrFloor,const Arrangement_2> _CGAL_env;
+ std::unordered_map<ptrFloor,std::unique_ptr<TEV>> _tev;
+};
+
+float roundfloat(float num, int precision);
+
+#endif // CGALGEOMETRY_H
diff --git a/routing/ai_router_trips/perception/sign.cpp b/routing/ai_router_trips/perception/sign.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e0071e3b23a282aa0b234a467c0a9472af67c9b6
--- /dev/null
+++ b/routing/ai_router_trips/perception/sign.cpp
@@ -0,0 +1,83 @@
+//
+// Created by erik on 30.12.16.
+//
+
+#include "sign.h"
+
+Sign::Sign(int id, int room_id, const Point &pos, const double &alpha, const double &alphaPointing, const double &width, const double &height) : _id(id), _roomId(room_id), _pos(pos), _alpha(alpha),_alphaPointing(alphaPointing),_width(width), _height(height) {
+
+
+}
+
+Sign::~Sign() {
+
+}
+
+const Point &Sign::GetPos() const {
+ return _pos;
+}
+
+double Sign::GetAlpha() const {
+ return _alpha;
+}
+
+double Sign::GetAlphaPointing() const
+{
+ return _alphaPointing;
+}
+
+void Sign::SetPos(const Point &pos) {
+ _pos = pos;
+}
+
+void Sign::SetAlpha(const double& alpha) {
+ _alpha = alpha;
+}
+
+void Sign::SetAlphaPointing(const double &alpha)
+{
+ _alphaPointing=alpha;
+}
+
+int Sign::GetId() const {
+ return _id;
+}
+
+int Sign::GetRoomId() const
+{
+ return _roomId;
+}
+
+const double &Sign::GetWidth() const
+{
+ return _width;
+}
+
+const double &Sign::GetHeight() const
+{
+ return _height;
+}
+
+void Sign::SetId(int id) {
+
+ _id=id;
+}
+
+void Sign::SetRoomId(int room_id)
+{
+ _roomId=room_id;
+}
+
+void Sign::SetWidth(const double &width)
+{
+ _width=width;
+}
+
+void Sign::SetHeight(const double &height)
+{
+ _height=height;
+}
+
+
+
+
diff --git a/routing/ai_router_trips/perception/sign.h b/routing/ai_router_trips/perception/sign.h
new file mode 100644
index 0000000000000000000000000000000000000000..97c0e000f730d2715788639e1e6d6157bbca9e99
--- /dev/null
+++ b/routing/ai_router_trips/perception/sign.h
@@ -0,0 +1,50 @@
+//
+// Created by erik on 30.12.16.
+//
+
+#ifndef JPSCORE_SIGN_H
+#define JPSCORE_SIGN_H
+
+
+#include "../../../geometry/Point.h"
+
+class Sign {
+
+
+public:
+
+ Sign(int id, int room_id, const Point &pos, const double& alpha, const double& alphaPointing, const double& width=0.3, const double &height=0.1);
+
+ const Point &GetPos() const;
+ double GetAlpha() const;
+ double GetAlphaPointing() const;
+ int GetId() const;
+ int GetRoomId() const;
+ const double& GetWidth() const;
+ const double& GetHeight() const;
+
+ void SetPos(const Point &pos);
+ void SetAlpha(const double& alpha);
+ void SetAlphaPointing(const double& alpha);
+ void SetId(int id);
+ void SetRoomId(int room_id);
+ void SetWidth(const double& width);
+ void SetHeight(const double& height);
+
+ virtual ~Sign();
+
+private:
+
+ int _id;
+ int _roomId;
+ Point _pos;
+ double _alpha; // in deg
+ double _alphaPointing;
+ double _width;
+ double _height;
+
+
+};
+
+
+#endif //JPSCORE_SIGN_H
diff --git a/routing/ai_router_trips/perception/visibleenvironment.cpp b/routing/ai_router_trips/perception/visibleenvironment.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c88ba56eee943cbc7225a961884e952bbc8a3846
--- /dev/null
+++ b/routing/ai_router_trips/perception/visibleenvironment.cpp
@@ -0,0 +1,234 @@
+#include "visibleenvironment.h"
+#include "../../../geometry/Building.h"
+#include "../../../pedestrian/Pedestrian.h"
+#include "../../../geometry/SubRoom.h"
+
+#include <boost/foreach.hpp>
+
+
+
+VisibleEnvironment::VisibleEnvironment():_b(nullptr),_cgalGeometry(CGALGeometry())
+{
+
+}
+
+VisibleEnvironment::VisibleEnvironment(const Building *b):_b(b)
+{
+
+ Log->Write("INFO: PERCEPTION: Preparing visible environment ...");
+
+
+ //Creating complete spatial structure
+
+ std::vector<Linestring> lines;
+ std::unordered_map<ptrFloor,std::vector<const Line*>> cWalls;
+ std::vector<const Line*> wallsOfOneRoom;
+
+ //get walls of rooms and save them separately
+ for (auto it = _b->GetAllRooms().begin(); it != _b->GetAllRooms().end(); ++it)
+ {
+ lines.clear();
+ wallsOfOneRoom.clear();
+ for (auto it2 = it->second->GetAllSubRooms().begin(); it2 != it->second->GetAllSubRooms().end(); ++it2)
+ {
+ for (const Wall& wall:it2->second->GetAllWalls())
+ {
+
+ Linestring lineS;
+ boost::geometry::append(lineS, wall.GetPoint1());
+ boost::geometry::append(lineS, wall.GetPoint2());
+ if (std::find(lines.begin(), lines.end(), lineS) == lines.end())
+ {
+ lines.push_back(lineS);
+ wallsOfOneRoom.push_back(&wall);
+ }
+
+
+ }
+
+ for (const Obstacle *obstacle:it2->second->GetAllObstacles())
+ {
+ for (const Wall& wall: obstacle->GetAllWalls())
+ {
+ Linestring lineS;
+ boost::geometry::append(lineS, wall.GetPoint1());
+ boost::geometry::append(lineS, wall.GetPoint2());
+ if (std::find(lines.begin(), lines.end(), lineS) == lines.end())
+ {
+ lines.push_back(lineS);
+ wallsOfOneRoom.push_back(&wall);
+
+ }
+
+ }
+
+ }
+
+ }
+
+ if (it->second->GetID()==6 || it->second->GetID()==0 || it->second->GetID()==11)
+ {
+
+ for (int transID:it->second->GetAllTransitionsIDs())
+ {
+ //if ((it->second->GetID()==6 || it->second->GetID()==0))// && _b->GetTransitionByUID(transID)->GetOtherRoom(6)->GetID()==5) || (it->second->GetID()==11 && _b->GetTransitionByUID(transID)->GetOtherRoom(11)!=nullptr
+ //&& (_b->GetTransitionByUID(transID)->GetOtherRoom(11)->GetID()==9 ||
+ // _b->GetTransitionByUID(transID)->GetOtherRoom(11)->GetID()==10)))
+ //{
+ // shift transition a little bit to the outside
+ bool within=false;
+ Point normal = Point(-(_b->GetTransitionByUID(transID)->GetPoint2()-_b->GetTransitionByUID(transID)->GetPoint1())._y,
+ (_b->GetTransitionByUID(transID)->GetPoint2()-_b->GetTransitionByUID(transID)->GetPoint1())._x);
+ normal=normal/normal.Norm();
+ Point P1 = _b->GetTransitionByUID(transID)->GetCentre()+normal*0.5;
+
+
+ for (auto it2 = it->second->GetAllSubRooms().begin(); it2 != it->second->GetAllSubRooms().end(); ++it2)
+ {
+ BoostPolygon boostP;
+ for (Point vertex:it2->second->GetPolygon())
+ {
+ boost::geometry::append(boostP, vertex);
+ }
+ boost::geometry::correct(boostP);
+
+ if (boost::geometry::within(P1, boostP))
+ within=true;
+
+ }
+ // MEMORY LEAK!!! FIX ME!!!
+
+ if (within==true)
+ {
+ Linestring lineS;
+ boost::geometry::append(lineS,_b->GetTransitionByUID(transID)->GetPoint1()-normal*0.5);
+ boost::geometry::append(lineS,_b->GetTransitionByUID(transID)->GetPoint2()-normal*0.5);
+ lines.push_back(lineS);
+ Line* newLine = new Line(_b->GetTransitionByUID(transID)->GetPoint1()-normal*0.5,_b->GetTransitionByUID(transID)->GetPoint2()-normal*0.5);
+ wallsOfOneRoom.push_back(newLine);
+
+ Linestring lineS1;
+ boost::geometry::append(lineS1,_b->GetTransitionByUID(transID)->GetPoint1()-normal*0.5);
+ boost::geometry::append(lineS1,_b->GetTransitionByUID(transID)->GetPoint1());
+ lines.push_back(lineS1);
+ newLine = new Line(_b->GetTransitionByUID(transID)->GetPoint1()-normal*0.5,_b->GetTransitionByUID(transID)->GetPoint1());
+ wallsOfOneRoom.push_back(newLine);
+
+ Linestring lineS2;
+ boost::geometry::append(lineS2,_b->GetTransitionByUID(transID)->GetPoint2()-normal*0.5);
+ boost::geometry::append(lineS2,_b->GetTransitionByUID(transID)->GetPoint2());
+ lines.push_back(lineS2);
+ newLine = new Line(_b->GetTransitionByUID(transID)->GetPoint2()-normal*0.5,_b->GetTransitionByUID(transID)->GetPoint2());
+ wallsOfOneRoom.push_back(newLine);
+ }
+ else
+ {
+ Linestring lineS;
+ boost::geometry::append(lineS,_b->GetTransitionByUID(transID)->GetPoint1()+normal*0.5);
+ boost::geometry::append(lineS,_b->GetTransitionByUID(transID)->GetPoint2()+normal*0.5);
+ lines.push_back(lineS);
+ Line* newLine = new Line(_b->GetTransitionByUID(transID)->GetPoint1()+normal*0.5,_b->GetTransitionByUID(transID)->GetPoint2()+normal*0.5);
+ wallsOfOneRoom.push_back(newLine);
+
+ Linestring lineS1;
+ boost::geometry::append(lineS1,_b->GetTransitionByUID(transID)->GetPoint1()+normal*0.5);
+ boost::geometry::append(lineS1,_b->GetTransitionByUID(transID)->GetPoint1());
+ lines.push_back(lineS1);
+ newLine = new Line(_b->GetTransitionByUID(transID)->GetPoint1()+normal*0.5,_b->GetTransitionByUID(transID)->GetPoint1());
+ wallsOfOneRoom.push_back(newLine);
+
+ Linestring lineS2;
+ boost::geometry::append(lineS2,_b->GetTransitionByUID(transID)->GetPoint2()+normal*0.5);
+ boost::geometry::append(lineS2,_b->GetTransitionByUID(transID)->GetPoint2());
+ lines.push_back(lineS2);
+ newLine = new Line(_b->GetTransitionByUID(transID)->GetPoint2()+normal*0.5,_b->GetTransitionByUID(transID)->GetPoint2());
+ wallsOfOneRoom.push_back(newLine);
+ }
+ }
+ //lines.push_back(lineS);
+
+ //}
+
+ }
+
+
+
+ _allWalls.emplace(it->second.get(),lines);
+ cWalls.emplace(it->second.get(),wallsOfOneRoom);
+
+ }
+
+
+ _cgalGeometry=CGALGeometry(cWalls);
+
+
+ WriteOutWalls();
+
+ Log->Write("INFO: PERCEPTION: Visible environment prepared.");
+
+}
+
+//const VisiLibity::Environment* VisibleEnvironment::GetVisEnv() const
+//{
+// return &_env;
+//}
+
+
+
+const std::vector<Linestring> &VisibleEnvironment::GetAllWallsOfEnv(ptrFloor floor) const
+{
+ return _allWalls.at(floor);
+}
+
+void VisibleEnvironment::WriteOutWalls() const
+{
+
+ std::ofstream myfile;
+ std::string str = "./isovists/walls.txt";
+ myfile.open (str);
+
+ for (auto it=_allWalls.begin(); it!=_allWalls.end(); ++it)
+ {
+ const std::vector<Linestring>& walls=it->second;
+ for (Linestring wall:walls)
+ {
+ for (auto it2=boost::begin(wall);it2!=boost::end(wall);++it2)
+ myfile << std::to_string(it2->_x) << " " << std::to_string(it2->_y) << " ";
+
+ myfile << std::endl;
+
+ }
+
+ }
+
+ myfile.close();
+}
+
+void VisibleEnvironment::SetSigns(ptrFloor floor, const std::vector<Sign> &signs) {
+
+ _signs.clear();
+ _signs.emplace(std::make_pair(floor,signs));
+}
+
+const CGALGeometry &VisibleEnvironment::GetCGALGeometry() const
+{
+ return _cgalGeometry;
+}
+
+void VisibleEnvironment::AddSign(ptrFloor floor, const Sign &sign)
+{
+ _signs[floor].push_back(sign);
+}
+
+void VisibleEnvironment::AddSign(ptrFloor floor, Sign &&sign)
+{
+ _signs[floor].push_back(std::move(sign));
+}
+
+const std::vector<Sign> *VisibleEnvironment::GetSignsOfFloor(ptrFloor floor) const
+{
+ if (_signs.find(floor)==_signs.end())
+ return nullptr;
+ else
+ return &_signs.at(floor);
+}
diff --git a/routing/ai_router_trips/perception/visibleenvironment.h b/routing/ai_router_trips/perception/visibleenvironment.h
new file mode 100644
index 0000000000000000000000000000000000000000..b19aeb18f2a1997cd7adea7da3701c2d0eb25767
--- /dev/null
+++ b/routing/ai_router_trips/perception/visibleenvironment.h
@@ -0,0 +1,40 @@
+#ifndef VISIBLEENVIRONMENT_H
+#define VISIBLEENVIRONMENT_H
+
+#include "sign.h"
+#include "cgalgeometry.h"
+#include <boost/geometry/geometry.hpp>
+
+// Define the used kernel and arrangement
+
+typedef boost::geometry::model::linestring<Point> Linestring;
+typedef boost::geometry::model::polygon<Point> BoostPolygon;
+
+class VisibleEnvironment
+{
+public:
+ explicit VisibleEnvironment();
+ explicit VisibleEnvironment(const Building* b);
+ //const VisiLibity::Environment *GetVisEnv() const;
+ const std::vector<Linestring>& GetAllWallsOfEnv(ptrFloor floor) const;
+ void WriteOutWalls() const;
+ void SetSigns(ptrFloor floor, const std::vector<Sign> &signs);
+ const CGALGeometry& GetCGALGeometry() const;
+ void AddSign(ptrFloor floor, const Sign& sign);
+ void AddSign(ptrFloor floor, Sign&& sign);
+ const std::vector<Sign> *GetSignsOfFloor(ptrFloor floor) const;
+
+
+private:
+ const Building* _b;
+ //VisiLibity::Environment _env;
+ // int: room_id: LinestringVector: Walls in room with id=room_id
+ std::unordered_map<ptrFloor,std::vector<Linestring>> _allWalls;
+ //std::vector<Sign> _signs;
+ std::unordered_map<ptrFloor, std::vector<Sign>> _signs;
+ CGALGeometry _cgalGeometry;
+ std::vector<Line> _additionalLines;
+
+};
+
+#endif // VISIBLEENVIRONMENT_H
diff --git a/routing/ai_router_trips/perception/visualsystem.cpp b/routing/ai_router_trips/perception/visualsystem.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..add09ca56b07587fe570e719309beab1390dbccb
--- /dev/null
+++ b/routing/ai_router_trips/perception/visualsystem.cpp
@@ -0,0 +1,684 @@
+#include "visualsystem.h"
+#include "visibleenvironment.h"
+#include "../../../geometry/SubRoom.h"
+#include "../../../pedestrian/Pedestrian.h"
+#include <chrono>
+#include <iostream>
+
+
+
+VisualSystem::VisualSystem()
+{
+
+}
+
+VisualSystem::VisualSystem(const Building *b, const Pedestrian *ped, const VisibleEnvironment *env)
+{
+ _building=b;
+ _ped=ped;
+ _env=env;
+ _signPerceived=0;
+
+}
+
+const std::unordered_map<ptrFloor, BoostPolygon> &VisualSystem::UpdateSeenEnv()
+{
+
+
+ //SubRoom * sub_room = _building->GetRoom(_ped->GetRoomID())->GetSubRoom(_ped->GetSubRoomID());
+
+
+ //BoostPolygon roomP;
+
+// for (Point point:sub_room->GetPolygon())
+// boost::geometry::append(roomP,point);
+
+// std::vector<BoostPolygon> intersectPolygon;
+// boost::geometry::intersection(roomP,newPolygon,intersectPolygon);
+
+
+ //BoostPolygon currentEnv = GetCurrentEnvironment();
+
+
+
+ //limiting seen Environment
+ BoostPolygon rectangle;
+
+ boost::geometry::append(rectangle,Point(_ped->GetPos()._x-35,_ped->GetPos()._y-35));
+ boost::geometry::append(rectangle,Point(_ped->GetPos()._x-35,_ped->GetPos()._y+35));
+ boost::geometry::append(rectangle,Point(_ped->GetPos()._x+35,_ped->GetPos()._y+35));
+ boost::geometry::append(rectangle,Point(_ped->GetPos()._x+35,_ped->GetPos()._y-35));
+
+
+ try {
+ std::vector<BoostPolygon> output2;
+ boost::geometry::intersection(_seenEnvironment[_building->GetRoom(_ped->GetRoomID())],rectangle,output2);
+ if (!output2.empty())
+ _seenEnvironment.at(_building->GetRoom(_ped->GetRoomID()))=output2.front();
+
+
+ //union of current and seen Env
+
+ std::vector<BoostPolygon> output;
+ boost::geometry::union_(_seenEnvironment[_building->GetRoom(_ped->GetRoomID())],_currentEnv,output);
+ if (!output.empty())
+ _seenEnvironment.at(_building->GetRoom(_ped->GetRoomID()))=output.front();
+
+ boost::geometry::correct(_seenEnvironment.at(_building->GetRoom(_ped->GetRoomID())));
+
+ }
+ catch (...)
+ {
+ // Code that handles another exception type
+ // ...
+ //cerr << e.what();
+ _seenEnvironment.at(_building->GetRoom(_ped->GetRoomID())) = _currentEnv;
+ }
+
+// std::ofstream myfile2;
+// std::string str2 = "./isovists/seen_env_"+std::to_string(_ped->GetID())+"_"+std::to_string((int)(std::round(_ped->GetGlobalTime()*100)))+".txt";
+// myfile2.open (str2);
+// for(auto it1 = boost::begin(boost::geometry::exterior_ring(_seenEnvironment.at(_building->GetRoom(_ped->GetRoomID()))));
+// it1 != boost::end(boost::geometry::exterior_ring(_seenEnvironment.at(_building->GetRoom(_ped->GetRoomID()))));
+// ++it1)
+// {
+// myfile2 << it1->_x << " " << it1->_y << std::endl;
+// //seenEnvPolygon.push_back(*it1);
+
+// }
+// myfile2.close();
+
+// std::ofstream myfile3;
+// std::string str3 = "./isovists/real_pos"+std::to_string(_ped->GetID())+"_"+std::to_string((int)(std::round(_ped->GetGlobalTime()*100)))+".txt";
+// myfile3.open (str3);
+// myfile3 << std::to_string(_ped->GetPos()._x) << " " << std::to_string(_ped->GetPos()._y) << std::endl;
+// myfile3.close();
+
+
+ return _seenEnvironment;
+
+}
+
+const BoostPolygon &VisualSystem::UpdateCurrentEnvironment()
+{
+// VisiLibity::Visibility_Polygon
+// my_visibility_polygon(VisiLibity::Point(_ped->GetPos()._x, _ped->GetPos()._y), (*_env->GetVisEnv()),0.01);
+// //std::cout << "The visibility polygon is \n" << my_visibility_polygon << std::endl;
+
+
+
+ // find the face of the query point
+ // (usually you may know that by other means)
+ Point_2 q(_ped->GetPos()._x,_ped->GetPos()._y);
+ Arrangement_2::Face_const_handle *face;
+ //only visEnv of actual room
+ const Arrangement_2& arr = _env->GetCGALGeometry().GetVisEnvCGAL(_building->GetRoom(_ped->GetRoomID()));
+ CGAL::Arr_naive_point_location<Arrangement_2> pl(arr);
+ CGAL::Arr_point_location_result<Arrangement_2>::Type obj = pl.locate(q);
+ // The query point locates in the interior of a face
+ face = boost::get<Arrangement_2::Face_const_handle> (&obj);
+
+
+ //visibility query
+ Arrangement_2 output_arr;
+ Arrangement_2::Face_handle fh = _env->GetCGALGeometry().GetTEV(_building->GetRoom(_ped->GetRoomID()))->compute_visibility(q,*face,output_arr);//query_point, he, output_arr);
+
+ //print out the visibility region.
+// std::cout << "Regularized visibility region of q has "
+// << output_arr.number_of_edges()
+// << " edges." << std::endl;
+
+ //std::cout << "Boundary edges of the visibility region:" << std::endl;
+ Arrangement_2::Ccb_halfedge_circulator curr = fh->outer_ccb();
+
+ BoostPolygon currentEnv;
+
+ //std::cout << "[" << curr->source()->point() << " -> " << curr->target()->point() << "]" << std::endl;
+ boost::geometry::append(currentEnv,Point(CGAL::to_double(curr->source()->point().x()) ,CGAL::to_double(curr->source()->point().y())));
+ while (++curr != fh->outer_ccb())
+ {
+ boost::geometry::append(currentEnv,Point(CGAL::to_double(curr->source()->point().x()) ,CGAL::to_double(curr->source()->point().y())));
+ //std::cout << "[" << curr->source()->point() << " -> " << curr->target()->point() << "]" << std::endl;
+ }
+
+ //delete face;
+// //std::vector<Point> visPolygon;
+
+
+// for (size_t i=0; i<my_visibility_polygon.n(); ++i)
+// {
+// //check for small holes
+// for (size_t j=i+1; j<my_visibility_polygon.n(); ++j)
+// {
+// Point vec = Point(my_visibility_polygon[i].x(),my_visibility_polygon[i].y())-Point(my_visibility_polygon[j].x(),my_visibility_polygon[j].y());
+// if (vec.Norm()<=0.1)
+// {
+// i=j+1;
+// }
+// }
+// if (i==my_visibility_polygon.n())
+// break;
+
+// else
+// {
+// visPolygon.push_back(Point(my_visibility_polygon[i].x(),my_visibility_polygon[i].y()));
+// boost::geometry::append(currentEnv,visPolygon[i]);
+// }
+
+// }
+
+// std::ofstream myfile;
+// std::string str = "./isovists/current_env_"+std::to_string(_ped->GetID())+"_"+std::to_string((int)(std::round(_ped->GetGlobalTime()*100)))+".txt";
+// myfile.open (str);
+// for(auto it1 = boost::begin(boost::geometry::exterior_ring(currentEnv));
+// it1 != boost::end(boost::geometry::exterior_ring(currentEnv));
+// ++it1)
+// {
+// myfile << it1->_x << " " << it1->_y << std::endl;
+// //seenEnvPolygon.push_back(*it1);
+
+// }
+// myfile.close();
+
+ boost::geometry::correct(currentEnv);
+
+ _currentEnv=currentEnv;
+
+ //PerceiveSigns();
+
+ return _currentEnv;
+
+}
+
+std::vector<NavLine> VisualSystem::GetPossibleNavLines(const std::vector<Point>& envPolygon) const
+{
+ //check if transitions are in the visible surroundings
+ std::vector<NavLine> navLines=FindTransitions();
+ //std::vector<ptrNavLine> navLines;
+// if (!navLines.empty())
+// return navLines;
+
+
+ std::vector<Point> visPolygon = envPolygon;// UpdateSeenEnv();
+ //visPolygon.push_back(visPolygon.front());
+
+
+ for (size_t i=1; i<visPolygon.size(); ++i)
+ {
+
+// for(auto it1 = boost::begin(boost::geometry::exterior_ring(_seenEnvironment)++);
+// it1 != boost::end(boost::geometry::exterior_ring(_seenEnvironment));
+// ++it1)
+ //{
+ bool intersect=false;
+// bool intersectsSeenEnv=false;
+ Point center = (visPolygon[i-1]+visPolygon[i])/2.0;//
+
+// std::cout << boost::geometry::distance(center,_seenEnvironment) << std::endl;
+ //if (boost::geometry::within(center,_seenEnvironment))
+ //{
+// bool status=false;
+// for (ptrNavLine navLine:_oldNavLines)
+// {
+// Linestring old_line;
+
+// boost::geometry::append(old_line, navLine->GetPoint1());
+// boost::geometry::append(old_line, navLine->GetPoint2());
+// if (boost::geometry::distance(center,navLine->GetCentre())<1.0)
+// {
+// status=true;
+// }
+// }
+// if (!status)
+// continue;
+ // intersectsSeenEnv=true;
+ //}
+ for (Linestring lineS: _env->GetAllWallsOfEnv(_building->GetRoom(_ped->GetRoomID())))
+ {
+ //Point vecWall = lineS.at(1)-lineS.at(0);
+ //Point vecVis = visPolygon[i]+visPolygon[i-1];
+ if (boost::geometry::distance(center,lineS)<=0.15)// && boost::geometry::distance(visPolygon[i],lineS)<=0.15)
+ {
+ //std::cout << "Distance" << boost::geometry::distance(visPolygon[i-1],lineS) << " " << boost::geometry::distance(visPolygon[i],lineS) << std::endl;
+ intersect=true;
+ break;
+ }
+ }
+ // if not wall and in current room (no transition)
+ if (intersect==false) //&& (IsInSubRoom(visPolygon[i-1])!=nullptr || IsInSubRoom(visPolygon[i])!=nullptr))
+ {
+ if (Line(visPolygon[i-1],visPolygon[i]).GetLength()>0.4)
+ {
+
+// Point dirVector=visPolygon[i-1]-visPolygon[i];
+// dirVector=dirVector/dirVector.Norm();
+// Point normalVector = Point(-dirVector._y,dirVector._x);
+
+ //if (!intersectsSeenEnv)
+ const NavLine navLine(Line(visPolygon[i-1],visPolygon[i]));
+ navLines.push_back(navLine);//Line(center+normalVector*3.0,center)));
+ //else
+ // navLinesSecondChoice.push_back(std::make_shared<const NavLine>(Line(center+normalVector*3.0,center)));
+// Log->Write("Center:");
+// Log->Write(std::to_string((center-normalVector*3.0)._x));
+// Log->Write(std::to_string((center-normalVector*3.0)._y));
+// Log->Write(" ");
+ }
+ }
+
+ }
+
+// std::cout << navLines.size() << std::endl;
+ if (navLines.empty())
+ {
+
+ navLines.push_back(NavLine(Line(_ped->GetPos(),_ped->GetPos()+_ped->GetV())));
+
+
+//// if (navLinesSecondChoice.empty())
+//// {
+// Log->Write("ERROR:\t Pedestrian seems to be trapped. No way out^^\n");
+// Log->Write(std::to_string(_ped->GetID()));
+// Log->Write("\n");
+// std::cout << _ped->GetPos()._x << " " << _ped->GetPos()._y << std::endl;
+// for(auto it1 = boost::begin(boost::geometry::exterior_ring(_currentEnv));
+// it1 != boost::end(boost::geometry::exterior_ring(_currentEnv));
+// ++it1)
+// {
+// std::cout << it1->_x << " " << it1->_y << std::endl;
+// //seenEnvPolygon.push_back(*it1);
+
+// }
+ //exit(EXIT_FAILURE);
+// //return navLines;
+//// }
+//// return navLinesSecondChoice;
+ }
+ // std::cout << navLines.size() << std::endl;
+
+ //_oldNavLines=navLines;
+ return navLines;
+
+
+}
+
+const std::unordered_map<ptrFloor, BoostPolygon> &VisualSystem::GetSeenEnvironment() const
+{
+ return _seenEnvironment;
+}
+
+const BoostPolygon &VisualSystem::GetCurrentEnvironment() const
+{
+ return _currentEnv;
+}
+
+std::vector<Point> VisualSystem::GetSeenEnvironmentAsPointVec(const ptrFloor& floor) const
+{
+ std::vector<Point> seenEnvPolygon;
+ for(auto it = boost::begin(boost::geometry::exterior_ring(_seenEnvironment.at(floor)));
+ it != boost::end(boost::geometry::exterior_ring(_seenEnvironment.at(floor)));
+ ++it)
+ {
+ seenEnvPolygon.push_back(*it);
+ }
+
+
+
+ for(size_t j=0; j<_seenEnvironment.at(floor).inners().size(); ++j)
+ {
+ auto innerPolygon = _seenEnvironment.at(floor).inners()[j];
+ for (size_t i=0; i<innerPolygon.size(); ++i)
+ {
+ seenEnvPolygon.push_back(innerPolygon[i]);
+ }
+ }
+
+
+ return seenEnvPolygon;
+}
+
+std::vector<Point> VisualSystem::GetCurrentEnvironmentAsPointVec() const
+{
+ std::vector<Point> currentEnvPolygon;
+ for(auto it = boost::begin(boost::geometry::exterior_ring(_currentEnv));
+ it != boost::end(boost::geometry::exterior_ring(_currentEnv));
+ ++it)
+ {
+ currentEnvPolygon.push_back(*it);
+
+ }
+
+ for(size_t j=0; j<_currentEnv.inners().size(); ++j)
+ {
+ auto innerPolygon = _currentEnv.inners()[j];
+ for (size_t i=0; i<innerPolygon.size(); ++i)
+ {
+ currentEnvPolygon.push_back(innerPolygon[i]);
+ }
+ }
+
+ return currentEnvPolygon;
+}
+
+std::vector<NavLine> VisualSystem::FindTransitions() const
+{
+
+ std::vector<Transition*> transitions;
+ for (int transID:_building->GetRoom(_ped->GetRoomID())->GetAllTransitionsIDs())
+ {
+ Point normal = Point(-(_building->GetTransitionByUID(transID)->GetPoint2()-_building->GetTransitionByUID(transID)->GetPoint1())._y,
+ (_building->GetTransitionByUID(transID)->GetPoint2()-_building->GetTransitionByUID(transID)->GetPoint1())._x);
+ normal=normal/normal.Norm();
+
+ if (boost::geometry::within(_building->GetTransitionByUID(transID)->GetCentre()+normal*0.5,_currentEnv) ||
+ boost::geometry::within(_building->GetTransitionByUID(transID)->GetCentre()-normal*0.5,_currentEnv))
+ {
+ transitions.push_back(_building->GetTransitionByUID(transID));
+ }
+ }
+ std::vector<NavLine> transNavLines;
+ for (const Transition* transition:transitions)
+ {
+ //Point dirVec = transition->GetCentre()-_ped->GetPos();
+ //dirVec=dirVec/dirVec.Norm();
+ NavLine navLine(Line(transition->GetPoint1(),transition->GetPoint2()));
+
+ transNavLines.push_back(navLine);
+
+ }
+ return transNavLines;
+
+}
+
+const SubRoom* VisualSystem::IsInSubRoom(const Point &point) const
+{
+
+ for (auto it2 = _building->GetRoom(_ped->GetRoomID())->GetAllSubRooms().begin(); it2 != _building->GetRoom(_ped->GetRoomID())->GetAllSubRooms().end(); ++it2)
+ {
+
+ BoostPolygon boostP;
+ for (Point vertex:it2->second->GetPolygon())
+ boost::geometry::append(boostP, vertex);
+
+ boost::geometry::correct(boostP);
+
+ if (boost::geometry::within(point, boostP))
+ return it2->second.get();
+
+ }
+ return nullptr;
+
+
+}
+
+const Room* VisualSystem::IsInRoom(const Point &point) const
+{
+
+ std::vector<int> transIDs= _building->GetRoom(_ped->GetRoomID())->GetAllTransitionsIDs();
+ std::vector<const Room*> adjacentRooms;
+
+ for (int transID:transIDs)
+ {
+ if (_building->GetTransitionByUID(transID)->GetOtherRoom(_ped->GetRoomID()))
+ adjacentRooms.push_back(_building->GetTransitionByUID(transID)->GetOtherRoom(_ped->GetRoomID()));
+ }
+
+ for (const Room* room:adjacentRooms)
+ {
+ for (auto it2 = room->GetAllSubRooms().begin(); it2 != room->GetAllSubRooms().end(); ++it2)
+ {
+
+ BoostPolygon boostP;
+ for (Point vertex:it2->second->GetPolygon())
+ {
+ boost::geometry::append(boostP, vertex);
+ }
+ boost::geometry::correct(boostP);
+
+ if (boost::geometry::within(point, boostP))
+ return _building->GetRoom(room->GetID());
+
+ }
+ }
+
+
+ return nullptr;
+
+}
+
+const Transition* VisualSystem::IsTransition(const NavLine *navLine) const
+{
+ std::vector<int> transIDs= _building->GetRoom(_ped->GetRoomID())->GetAllTransitionsIDs();
+
+ for (int transID:transIDs)
+ {
+ if (navLine->GetCentre()==_building->GetTransitionByUID(transID)->GetCentre())
+ return _building->GetTransitionByUID(transID);
+ }
+
+ return nullptr;
+}
+
+void VisualSystem::GetVisibleSigns()
+{
+
+ _newVisibleSigns.clear();
+
+ const std::vector<Sign>* signsInCurrentFloor = _env->GetSignsOfFloor(_building->GetAllRooms().at(_ped->GetRoomID()).get());
+
+ if (!signsInCurrentFloor)
+ {
+ _visibleSigns.clear();
+ _detectedSigns.clear();
+ return;
+ }
+
+ for (const Sign& sign:*signsInCurrentFloor)
+ {
+ // if sign is in visible range
+ if (SignLegible(sign))
+ {
+ // sign in visibility range for the first time
+ if (std::find(_visibleSigns.begin(),_visibleSigns.end(),&sign)==_visibleSigns.end())
+ {
+ _newVisibleSigns.push_back(&sign);
+ _visibleSigns.push_back(&sign);
+ }
+ }
+ // if sign not anymore in field of view
+ else if (std::find(_visibleSigns.begin(),_visibleSigns.end(),&sign)!=_visibleSigns.end())
+ {
+ _visibleSigns.erase(std::find(_visibleSigns.begin(),_visibleSigns.end(),&sign));
+
+ if (std::find(_detectedSigns.begin(),_detectedSigns.end(),&sign)!=_detectedSigns.end())
+ _detectedSigns.erase(std::find(_detectedSigns.begin(),_detectedSigns.end(),&sign));
+
+ }
+ }
+
+}
+
+bool VisualSystem::SignLegible(const Sign &sign) const
+{
+ //following Signage Legibility Distances as a Function of Observation Angle by Xie et al. 2007
+
+
+ Point vec1 = _ped->GetPos()-sign.GetPos();
+ Point vec2 = sign.GetAlpha()*M_PI/180.0;
+
+ double phi1=std::atan2(vec1._y,vec1._x);
+ double phi2=std::atan2(vec2._y,vec2._x);
+ double phi = std::fabs(phi2-phi1);
+
+ if(vec1.Norm()>LegibleDistance(phi))
+ return false;
+
+ //checking if sign is in not hided by walls or obstacles
+ if (boost::geometry::within(sign.GetPos(),_currentEnv))
+ return true;
+
+
+ return false;
+
+}
+
+double VisualSystem::HeadDirCompToSign(const Sign &sign) const
+{
+ Point pedVec=_ped->GetV();
+ double pedAlpha=std::atan2(pedVec._y,pedVec._x);
+
+ Point pedToSignVec=sign.GetPos()-_ped->GetPos();
+ double pedToSignAlpha=std::atan2(pedToSignVec._y,pedToSignVec._x);
+
+ return std::fabs(pedAlpha-pedToSignAlpha);
+
+}
+
+
+double VisualSystem::GetProbabilityToDetectSign(double headDir, bool hardlyVisSign)
+{
+
+ double P=1.0;
+ double kappa_alpha=1.0;
+
+ double zeta=0.0;//0.16;
+ double eta=0.0;//0.375;
+
+ if (_signPerceived>=1)
+ P+=_signPerceived*zeta;
+ else if (_signPerceived<0)
+ P+=_signPerceived*eta;
+
+ if (hardlyVisSign)
+ {
+ kappa_alpha=0.5;
+ P=(1-kappa_alpha)/(1-P*kappa_alpha)*P;
+ }
+
+// if (headDir>M_PI/4.0)
+// {
+// kappa_alpha=0.5;
+// }
+// else if (headDir>M_PI/2.0)
+// {
+// kappa_alpha=0.0;
+// }
+// else
+// {
+// kappa_alpha=1.0;
+// }
+
+ P*=kappa_alpha;
+
+
+ if (P<0)
+ P=0;
+ else if (P>1)
+ P=1;
+
+ return P;
+
+
+
+
+
+}
+
+std::vector<const Sign*> VisualSystem::TryToDetectSigns()
+{
+ //Visible:= sign in visible range
+ //Detect:= Instruction of sign will be followed
+ GetVisibleSigns();
+
+ for (const Sign* sign:_newVisibleSigns)
+ {
+ double headDir=HeadDirCompToSign(*sign);
+
+ TryToDetectSign(*sign, headDir,false);
+ //if hard to detect signs shall be considered:
+// if (!TryToDetectSign(*sign, headDir,false))// && headDir>M_PI/4.0)
+// {
+// if(headDir>M_PI/4.0)
+// {
+// _hardlyVisibleSigns.push_back(sign);
+// }
+// }
+ }
+
+// for (const Sign* sign:_hardlyVisibleSigns)
+// {
+// double headDir=HeadDirCompToSign(*sign);
+
+// if (headDir<M_PI/4.0)
+// {
+// _hardlyVisibleSigns.erase(std::find(_hardlyVisibleSigns.begin(),_hardlyVisibleSigns.end(),sign));
+// TryToDetectSign(*sign,headDir,true);
+// }
+// }
+
+ return _detectedSigns;
+
+}
+
+bool VisualSystem::TryToDetectSign(const Sign &sign, double headDir, bool hardlyVisSign)
+{
+
+ double P=GetProbabilityToDetectSign(headDir,hardlyVisSign);
+ std::discrete_distribution<int> distribution {1.0-P,P};
+
+ int number = distribution(_building->GetConfig()->GetRandomNumberGenerator()->GetRandomEngine());
+
+
+ if (number)
+ {
+ _detectedSigns.push_back(&sign);
+ if (_signPerceived<0)
+ _signPerceived=1;
+ else
+ ++_signPerceived;
+
+ return true;
+ }
+ else
+ {
+ //if (headDir<M_PI/4.0)
+ //{
+ if (_signPerceived<=0)
+ --_signPerceived;
+ //}
+ //Log->Write("ROUTEINFO: Sign not detected\n");
+ return false;
+ }
+}
+
+std::vector<std::pair<const Point&,double>> VisualSystem::PerceiveDirsOfOtherPedestrians() const
+{
+ std::vector<std::pair<const Point&,double>> dirVectors;
+
+ if (_ped->GetV()._x==0.0 && _ped->GetV()._y==0.0)
+ return dirVectors;
+
+
+ const std::vector<Pedestrian*> pedestrians= _building->GetAllPedestrians();
+
+ for (const Pedestrian* ped:pedestrians)
+ {
+ if (ped!=_ped)
+ {
+ if (boost::geometry::within(ped->GetPos(),_currentEnv))
+ {
+ double angle = std::atan2(ped->GetV()._y,ped->GetV()._x)*180/M_PI;
+ dirVectors.push_back(std::make_pair<const Point&,double&>(ped->GetPos(),angle));
+ }
+ }
+ }
+
+ return dirVectors;
+}
+
+
+double LegibleDistance(double phi) // based on Xie et al. 2007
+{
+ return -0.003335*phi*phi+0.07863*phi+19.79;
+
+}
diff --git a/routing/ai_router_trips/perception/visualsystem.h b/routing/ai_router_trips/perception/visualsystem.h
new file mode 100644
index 0000000000000000000000000000000000000000..f063f2f10c2301ed11f8ab990e56f58d279ad772
--- /dev/null
+++ b/routing/ai_router_trips/perception/visualsystem.h
@@ -0,0 +1,87 @@
+#ifndef VISUALSYSTEM_H
+#define VISUALSYSTEM_H
+
+#include <boost/geometry/geometry.hpp>
+#include <memory>
+#include <vector>
+#include <unordered_map>
+#include "../../../geometry/Point.h"
+#include "../../../general/Macros.h"
+
+
+class NavLine;
+class Pedestrian;
+class Building;
+class VisibleEnvironment;
+class Transition;
+class Room;
+class SubRoom;
+class Sign;
+
+using ptrNavLine = std::unique_ptr<const NavLine>;
+using ptrFloor = const Room*;
+typedef boost::geometry::model::linestring<Point> Linestring;
+typedef boost::geometry::model::polygon<Point> BoostPolygon;
+
+
+class VisualSystem
+{
+public:
+ VisualSystem();
+ VisualSystem(const Building* b, const Pedestrian* ped, const VisibleEnvironment* env);
+
+ const std::unordered_map<ptrFloor,BoostPolygon>& UpdateSeenEnv();
+ const BoostPolygon& UpdateCurrentEnvironment();
+ std::vector<NavLine> GetPossibleNavLines(const std::vector<Point> &envPolygon) const;
+
+ const std::unordered_map<ptrFloor,BoostPolygon>& GetSeenEnvironment() const;
+ const BoostPolygon& GetCurrentEnvironment() const;
+
+ std::vector<Point> GetSeenEnvironmentAsPointVec(const ptrFloor& room) const;
+ std::vector<Point> GetCurrentEnvironmentAsPointVec() const;
+
+ //Transitions in currentEnv?
+ std::vector<NavLine> FindTransitions() const;
+ const SubRoom* IsInSubRoom(const Point& point) const;
+ const Room *IsInRoom(const Point& point) const;
+ const Transition* IsTransition(const NavLine* navLine) const;
+
+ //Signage
+ void GetVisibleSigns();
+ // sign not to far away in consideration of observation angle (see Xie et. al 2007)
+ bool SignLegible(const Sign& sign) const;
+ //Heading direction of ped compared to direction of beeline to sign
+ double HeadDirCompToSign(const Sign& sign) const;
+ //double GetAngleFactor(const double& angle) const;
+ //calculate probability to detect sign in current time step
+ double GetProbabilityToDetectSign(double headDir, bool hardlyVisSign);
+ std::vector<const Sign*> TryToDetectSigns();
+ bool TryToDetectSign(const Sign& sign, double headDir, bool hardlyVisSign);
+
+ //Herding
+ // walking direction of other pedestrians are returned / only an idea! not working!
+ std::vector<std::pair<const Point &, double> > PerceiveDirsOfOtherPedestrians() const;
+
+private:
+
+ const Building* _building;
+ const Pedestrian* _ped;
+ const VisibleEnvironment* _env;
+ std::unordered_map<ptrFloor,BoostPolygon> _seenEnvironment;
+ BoostPolygon _currentEnv;
+ // in vector: sign,vector:time in visibility to sign, spent probability to detect sign,
+ //last angle sector to sign, spent probability to detect sign in current angle sector
+ std::vector<const Sign*> _visibleSigns;
+ std::vector<const Sign*> _newVisibleSigns;
+ std::vector<const Sign*> _hardlyVisibleSigns;
+ std::vector<const Sign*> _detectedSigns;
+ int _signPerceived;
+
+
+
+
+};
+
+double LegibleDistance(double phi);
+
+#endif // VISUALSYSTEM_H
diff --git a/routing/ff_router_trips/FFKit.cpp b/routing/ff_router_trips/FFKit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2a22f377a21127acd4701792a02413d46375f52e
--- /dev/null
+++ b/routing/ff_router_trips/FFKit.cpp
@@ -0,0 +1,43 @@
+//
+// 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
new file mode 100644
index 0000000000000000000000000000000000000000..557dcfe84e9ad3be5eca94545684ea45bc68f460
--- /dev/null
+++ b/routing/ff_router_trips/FFKit.h
@@ -0,0 +1,103 @@
+//
+// 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
new file mode 100644
index 0000000000000000000000000000000000000000..b25c1a52e8cfdb4d99068995a347eda794e0b13e
--- /dev/null
+++ b/routing/ff_router_trips/FloorfieldViaFM.cpp
@@ -0,0 +1,1785 @@
+/**
+ * \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
new file mode 100644
index 0000000000000000000000000000000000000000..c61eea13696aad6e947d9075120224172fcf2516
--- /dev/null
+++ b/routing/ff_router_trips/FloorfieldViaFM.h
@@ -0,0 +1,226 @@
+/**
+ * \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
new file mode 100644
index 0000000000000000000000000000000000000000..145c764e5a5a618068d644d5c8bcf5c6c547a2b2
--- /dev/null
+++ b/routing/ff_router_trips/LocalFloorfieldViaFM.cpp
@@ -0,0 +1,519 @@
+//
+// 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
new file mode 100644
index 0000000000000000000000000000000000000000..02a3d6ccd0691e2dbc55679f25d8729bfd8f2c90
--- /dev/null
+++ b/routing/ff_router_trips/LocalFloorfieldViaFM.h
@@ -0,0 +1,86 @@
+/**
+ * \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
new file mode 100644
index 0000000000000000000000000000000000000000..6a6e23b0fe32d9f71036754a6f1b33d98d530853
--- /dev/null
+++ b/routing/ff_router_trips/UnivFFviaFM.cpp
@@ -0,0 +1,1881 @@
+//
+// 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
new file mode 100644
index 0000000000000000000000000000000000000000..53080ec1993fba5e32e6ae937c39868505dd41a3
--- /dev/null
+++ b/routing/ff_router_trips/UnivFFviaFM.h
@@ -0,0 +1,179 @@
+//
+// 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
new file mode 100644
index 0000000000000000000000000000000000000000..dcecf4067b1b9a0736fe015f742124463c6fa5ed
--- /dev/null
+++ b/routing/ff_router_trips/ffRouterTrips.cpp
@@ -0,0 +1,664 @@
+/**
+ * \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 "ffRouterTrips.h"
+//#include "FloorfieldViaFM.h"
+//#include "../../geometry/Building.h"
+
+int FFRouterTrips::_cnt = 0;
+
+FFRouterTrips::FFRouterTrips()
+{
+
+}
+
+FFRouterTrips::FFRouterTrips(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)
+
+FFRouterTrips::~FFRouterTrips()
+{
+ 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 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);
+ 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();
+
+ for (auto dist : _distMatrix){
+ std::cout << dist.first.first << "->" << dist.first.second << ": " << dist.second << std::endl;
+ }
+
+ Log->Write("INFO: \tFF Router Init done.");
+ return true;
+}
+
+bool FFRouterTrips::ReInit()
+{
+ std::cout << "bool FFRouterTrips::ReInit()" << std::endl;
+
+ //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 FFRouterTrips::FindExit(Pedestrian* ped)
+{
+ std::cout << "Ped[" << ped->GetID() << "] in (" << ped->GetRoomID() << ", " << ped->GetSubRoomID()
+ << "/" << ped->GetSubRoomUID() << "): " << std::endl;
+ std::cout << "FinalDestination: " << ped->GetFinalDestination() << std::endl;
+// std::cout << "ExitLine: " << ped->GetExitLine() << std::endl;
+ std::cout << "ExitIndex: " << ped->GetExitIndex() << std::endl << std::endl;
+
+
+ // Check if current position is already waiting area
+ // yes: set next goal and return findExit(p)
+ int ret = FindExit1(ped);
+
+ std::cout << "Ped[" << ped->GetID() << "] in (" << ped->GetRoomID() << ", " << ped->GetSubRoomID()
+ << "/" << ped->GetSubRoomUID() << "): " << std::endl;
+ std::cout << "FinalDestination: " << ped->GetFinalDestination() << std::endl;
+// std::cout << "ExitLine: " << ped->GetExitLine() << std::endl;
+ std::cout << "ExitIndex: " << ped->GetExitIndex() << std::endl;
+ std::cout << "====================================================" << std::endl;
+
+ return ret;
+}
+
+int FFRouterTrips::FindExit1(Pedestrian* p)
+{
+
+ std::cout << "------ FindExit1 ------" << std::endl;
+ 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));
+ }
+
+ std::cout << "-----------------------" << std::endl << std::endl;
+
+ return bestDoor; //-1 if no way was found, doorUID of best, if path found
+}
+
+void FFRouterTrips::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 FFRouterTrips::SetMode(std::string s)
+{
+ if (s == "global_shortest"){
+ _mode = global_shortest;
+ return;
+ }
+
+ if (s == "quickest") {
+ _mode = quickest;
+ return;
+ }
+
+ _mode = global_shortest;
+ return;
+}
+
+bool FFRouterTrips::MustReInit() {
+ return _plzReInit;
+}
+
+void FFRouterTrips::SetRecalc(double t) {
+ _timeToRecalc = t + _recalc_interval;
+}
\ No newline at end of file
diff --git a/routing/ff_router_trips/ffRouterTrips.h b/routing/ff_router_trips/ffRouterTrips.h
new file mode 100644
index 0000000000000000000000000000000000000000..5b4396bb8d07b5507c953913d88f8dd6e54d40df
--- /dev/null
+++ b/routing/ff_router_trips/ffRouterTrips.h
@@ -0,0 +1,232 @@
+/**
+ * \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.
+ *
+ * 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 FFROUTERTRIPS_H_
+#define FFROUTERTRIPS_H_
+
+
+#include "../Router.h"
+#include "../../general/Macros.h"
+#include "../../geometry/Building.h"
+#include "FloorfieldViaFM.h"
+#include "UnivFFviaFM.h"
+
+class Building;
+class Pedestrian;
+class OutputHandler;
+
+//log output
+extern OutputHandler* Log;
+
+/*!
+ * \class FFRouter
+ *
+ * \brief router using floor fields to measure distances of doors, no hlines
+ *
+ *\ingroup Router
+ * 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.
+ *
+ * \author Arne Graf
+ * \date Feb, 2016
+ */
+
+class FFRouterTrips : public Router
+{
+public:
+ /**
+ * A constructor.
+ *
+ */
+ FFRouterTrips();
+ FFRouterTrips(int id, RoutingStrategy s, bool hasSpecificGoals, Configuration* config);
+ //FFRouter(const Building* const);
+
+ /**
+ * Destructor
+ */
+ virtual ~FFRouterTrips();
+
+ /*!
+ * \brief Init the router (must be called before use)
+ *
+ * Init() will construct the graph (nodes = doors, edges = costs) and
+ * find shortest paths via Floyd-Warshall. It needs the floor fields
+ *
+ *
+ * \param[in] [name of input parameter] [its description]
+ * \param[out] [name of output parameter] [its description]
+ * \return [information about return value]
+ * \sa [see also section]
+ * \note [any note about the function you might have]
+ * \warning [any warning if necessary]
+ */
+ virtual bool Init(Building* building);
+
+ /*!
+ * \brief ReInit the router if quickest router is used. Current position of agents is considered.
+ *
+ * ReInit() will reconstruct the graph (nodes = doors, edges = costs) and
+ * find shortest paths via Floyd-Warshall. It will reconstruct the floorfield to
+ * evaluate the best doors to certain goals as they could change.
+ *
+ *
+ * \param[in] [name of input parameter] [its description]
+ * \param[out] [name of output parameter] [its description]
+ * \return [information about return value]
+ * \sa [see also section]
+ * \note [any note about the function you might have]
+ * \warning [any warning if necessary]
+ */
+ virtual bool ReInit();
+
+ /*!
+ * \brief interface used by __Pedestrian__, sets (*p).exitline/.exitindex
+ *
+ * additional info: not available
+ *
+ */
+ virtual int FindExit(Pedestrian* p);
+
+ /*!
+ * \brief interface used by __Pedestrian__, sets (*p).exitline/.exitindex
+ *
+ * additional info: not available
+ *
+ */
+ int FindExit1(Pedestrian* p);
+
+
+ /*!
+ * \brief Perform the FloydWarshall algorithm
+ */
+ void FloydWarshall();
+
+// /*!
+// * \brief Sets the door that leaves the subroom in _pathsMatrix
+// *
+// * Due to the way we calculate door distances (entries in _pathsMatrix), pedestrians in a corridor
+// * tend to jump from door to door, i.e. they walk to the next door in the correct direction, but they
+// * do not traverse it. This algorithm searches for the door on the way that really leaves the subroom,
+// * and sets this door in _pathsMatrix, which in turn is needed by GetPresumableExitRoute().
+// */
+// void AvoidDoorHopping();
+
+ /*!
+ * \brief set mode (shortest, quickest, ...)
+ */
+ void SetMode(std::string s);
+ bool MustReInit();
+ void SetRecalc(double t);
+
+private:
+
+protected:
+ Configuration* _config;
+ std::map< std::pair<int, int> , double > _distMatrix;
+ std::map< std::pair<int, int> , int > _pathsMatrix;
+ //std::map< std::pair<int, int> , SubRoom* > _subroomMatrix;
+ std::vector<int> _allDoorUIDs;
+ std::vector<int> _localShortestSafedPeds;
+ 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, Transition*> _TransByUID;
+ std::map<int, Transition*> _ExitsByUID;
+ std::map<int, Crossing*> _CroTrByUID;
+
+ 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, int> _finalDoors; // _finalDoors[i] the UID of the last door the pedestrian with ID i wants to walk through
+
+ int _mode;
+ double _timeToRecalc = 0.;
+ double _recalc_interval;
+ bool _plzReInit = false;
+ bool _hasSpecificGoals;
+ bool _targetWithinSubroom;
+ // If we use CentrePointDistance (i.e. CentrePointLocalFFViaFM), some algorithms can maybe be simplified
+ // (AvoidDoorHopping and _subroomMatrix might be unnecessary, and some code in FindExit could go). --f.mack
+ bool _useCentrePointDistance = true;
+ //output filename counter: cnt
+ static int _cnt;
+};
+
+#endif /* FFROUTER_H_ */
diff --git a/routing/ff_router_trips/mesh/RectGrid.h b/routing/ff_router_trips/mesh/RectGrid.h
new file mode 100644
index 0000000000000000000000000000000000000000..d12273a6ddcbde61f16b603e8e3e27eb2987004b
--- /dev/null
+++ b/routing/ff_router_trips/mesh/RectGrid.h
@@ -0,0 +1,257 @@
+/**
+ * \file RectGrid.h
+ * \date Mar 05, 2014
+ * \version v0.5
+ * \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
+ * Header of class for rectangular grids.
+ *
+ *
+ **/
+
+#ifndef RECTGRID_H
+#define RECTGRID_H
+
+#include "../../../geometry/Point.h"
+
+// geometric interpretation of index in "directNeighbor"
+// x (1) ^ y
+// | |
+// (2) x----0----x (0) o---> x
+// |
+// x (3)
+
+typedef struct directNeighbor_t {
+ long int key[4];
+} directNeighbor;
+
+
+class RectGrid
+{
+ public:
+ RectGrid() {
+ this->isInitialized = false;
+ }
+
+ virtual ~RectGrid() {
+ }
+
+ RectGrid(const RectGrid& other) {
+ nPoints = other.nPoints;
+ xMin = other.xMin;
+ yMin = other.yMin;
+ xMax = other.xMax;
+ yMax = other.yMax;
+ hx = other.hx;
+ hy = other.hy;
+ iMax = other.iMax;
+ jMax = other.jMax;
+ isInitialized = other.isInitialized;
+ }
+
+ RectGrid( long int nPointsArg,
+ double xMinArg,
+ double yMinArg,
+ double xMaxArg,
+ double yMaxArg,
+ double hxArg,
+ double hyArg,
+ long int iMaxArg, // indices must be smaller than iMax
+ long int jMaxArg, // indices must be smaller than jMax
+ bool isInitializedArg) {
+ nPoints = nPointsArg; // must not be corrupted..
+ xMin = xMinArg;
+ yMin = yMinArg;
+ xMax = xMaxArg;
+ yMax = yMaxArg;
+ hx = hxArg;
+ hy = hyArg;
+ iMax = iMaxArg;
+ jMax = jMaxArg;
+ isInitialized = isInitializedArg;
+ }
+
+ long int GetnPoints() const { return nPoints; }
+ //void SetnPoints(long int val) { nPoints = val; }
+ double GetxMin() const { return xMin; }
+ void SetxMin(double val) { if (!isInitialized) xMin = val; }
+ double GetyMin() const { return yMin; }
+ void SetyMin(double val) { if (!isInitialized) yMin = val; }
+ double GetxMax() const { return xMax; }
+ void SetxMax(double val) { if (!isInitialized) xMax = val; }
+ double GetyMax() const { return yMax; }
+ void SetyMax(double val) { if (!isInitialized) yMax = val; }
+ long int GetiMax() const { return iMax; }
+ //void SetiMax(long int val) { iMax = val; }
+ long int GetjMax() const { return jMax; }
+ //void SetjMax(long int val) { jMax = val; }
+ double Gethx() const { return hx; }
+ //void Sethx(double val) { hx = val; }
+ double Gethy() const { return hy; }
+ //void Sethy(double val) { hy = val; }
+
+ double get_x_fromKey (long int key) const { return (key%iMax)*hx+xMin; }
+ double get_y_fromKey (long int key) const { return (key/iMax)*hy+yMin; }
+ double get_i_fromKey (long int key) const { return (key%iMax); }
+ double get_j_fromKey (long int key) const { return (key/iMax); }
+
+ long int getKeyAtXY(const double x, const double y) const {//key = index in (extern managed) array
+ //Point nearest = getNearestGridPoint(Point(x,y));
+ long int i = (long int)(((x-xMin)/hx)+.5);
+ long int j = (long int)(((y-yMin)/hy)+.5);
+ if ((i >= 0) && (i <= iMax) && (j >= 0) && (j <= jMax)) //@todo: ar.graf: check in #ifdef block
+ return (j*iMax+i); // 0-based; index of (closest gridpoint)
+ std::cerr << "ERROR 1 in RectGrid::getKeyAtPoint with:" << std::endl;
+ std::cerr << " Point: " << x << ", " << y << std::endl;
+ std::cerr << " xMin, yMin: " << xMin << ", " << yMin << std::endl;
+ std::cerr << " xMax, yMax: " << xMax << ", " << yMax << std::endl;
+ std::cerr << " Point is out of Grid-Scope, Tip: check if correct Floorfield is called" << std::endl;
+ return -1; // invalid indices
+ }
+
+ long int getKeyAtPoint(const Point p) const {
+ long int i = (long int) (((p._x-xMin)/hx)+.5);
+ long int j = (long int) (((p._y-yMin)/hy)+.5);
+ //if ((i >= 0) && (i <= iMax) && (j >= 0) && (j <= jMax)) //@todo: ar.graf: check in #ifdef block
+ if (includesPoint(p)) //@todo: ar.graf: this if must be made work
+ return (j*iMax+i); // 0-based; index of (closest gridpoint)
+ else {
+ if (p._x < xMin) { std::cerr << "Out of left bound by: " << (xMin - p._x) << std::endl; i = 0; }
+ if (p._x > xMax) { std::cerr << "Out of right bound by: " << (p._x - xMax) << std::endl; i = iMax; }
+ if (p._y < yMin) { std::cerr << "Out of lower bound by: " << (yMin - p._y) << std::endl; j = 0; }
+ if (p._y > yMax) { std::cerr << "Out of upper bound by: " << (p._y - yMax) << std::endl; j = jMax; }
+ return (j * iMax + i);
+ }
+ std::cerr << "ERROR 2 in RectGrid::getKeyAtPoint with:" << std::endl;
+ std::cerr << " Point: " << p._x << ", " << p._y << std::endl;
+ std::cerr << " xMin, yMin: " << xMin << ", " << yMin << std::endl;
+ std::cerr << " xMax, yMax: " << xMax << ", " << yMax << std::endl;
+ std::cerr << " Point is out of Grid-Scope, Tip: check if correct Floorfield is called" << std::endl;
+ return -1; // invalid indices
+ }
+
+ void setBoundaries(const double xMinA, const double yMinA,
+ const double xMaxA, const double yMaxA) {
+ if (!isInitialized) {
+ xMin = xMinA;
+ xMax = xMaxA;
+ yMin = yMinA;
+ yMax = yMaxA;
+ }
+ }
+
+ void setBoundaries(const Point xy_min, const Point xy_max) {
+ if (!isInitialized) {
+ xMin = xy_min._x;
+ xMax = xy_max._x;
+ yMin = xy_min._y;
+ yMax = xy_max._y;
+ }
+ }
+
+ void setSpacing(const double h_x, const double h_y) {
+ if (!isInitialized) {
+ hy = h_y;
+ hx = h_x;
+ }
+ }
+
+ void createGrid(){ //what if cast chops off float, if any changes: get_x_fromKey still correct?
+ if (!isInitialized) {
+ iMax = (long int)((xMax-xMin)/hx) + 2; //check plus 2 (one for ceil, one for starting point)
+ jMax = (long int)((yMax-yMin)/hy) + 2;
+ nPoints = iMax * jMax;
+ //@todo: see if necessary to align xMax/yMax
+ xMax = xMin + iMax*hx;
+ yMax = yMin + jMax*hy;
+ isInitialized = true;
+ }
+ }
+
+ Point getNearestGridPoint(const Point& currPoint) const {
+ //if ((currPoint._x > xMax) || (currPoint._y > yMax) ||
+ // (currPoint._x < xMin) || (currPoint._y < yMin)) {
+ if (!includesPoint(currPoint)) {
+ std::cerr << "ERROR 3 in RectGrid::getKeyAtPoint with:"
+ << std::endl;
+ std::cerr << " Point: " << currPoint._x << ", " << currPoint._y << std::endl;
+ std::cerr << " xMin, yMin: " << xMin << ", " << yMin
+ << std::endl;
+ std::cerr << " xMax, yMax: " << xMax << ", " << yMax
+ << std::endl;
+ std::cerr
+ << " Point is out of Grid-Scope, Tip: check if correct Floorfield is called"
+ << std::endl;
+ return Point(-7, -7);
+ }
+ long int i = (long int)(((currPoint._x-xMin)/hx)+.5);
+ long int j = (long int)(((currPoint._y-yMin)/hy)+.5);
+ return Point(i*hx+xMin, j*hy+yMin);
+ }
+
+ Point getPointFromKey(const long int key) const {
+ long int i = key%iMax;
+ long int j = key/iMax; //integer division
+
+ return Point(i*hx+xMin, j*hy+yMin);
+ }
+
+ directNeighbor getNeighbors(const long int key) const {
+ directNeighbor neighbors = {{-1, -1, -1, -1}}; //curleybrackets for struct, then for int[4]
+ long int i = get_i_fromKey(key);
+ long int j = get_j_fromKey(key);
+
+ //right //-2 marks invalid neighbor
+ neighbors.key[0] = (i == (iMax-1)) ? -2 : (j*iMax+i+1);
+ //upper
+ neighbors.key[1] = (j == (jMax-1)) ? -2 : ((j+1)*iMax+i);
+ //left
+ neighbors.key[2] = (i == 0) ? -2 : (j*iMax+i-1);
+ //lower
+ neighbors.key[3] = (j == 0) ? -2 : ((j-1)*iMax+i);
+
+ return neighbors;
+ }
+
+ bool includesPoint(const Point& point) const {
+ if ((point._x < (xMin-hx/2)) ||
+ (point._x > (xMax+hx/2)) ||
+ (point._y < (yMin-hy/2)) ||
+ (point._y > (yMax+hy/2))) {
+ return false;
+ }
+ return true;
+ }
+
+ protected:
+ private:
+ long int nPoints;
+ double xMin;
+ double yMin;
+ double xMax;
+ double yMax;
+ double hx;
+ double hy;
+ long int iMax; // indices must be smaller than iMax
+ long int jMax; // indices must be smaller than jMax
+ bool isInitialized;
+};
+
+#endif // RECTGRID_H
diff --git a/routing/ff_router_trips/mesh/Trial.h b/routing/ff_router_trips/mesh/Trial.h
new file mode 100644
index 0000000000000000000000000000000000000000..a934fe1b2941a7ae272a61bd39cdece0ebe64091
--- /dev/null
+++ b/routing/ff_router_trips/mesh/Trial.h
@@ -0,0 +1,172 @@
+/**
+ * \file
+ * \date Mar 26, 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 a double linked list with smallest at start
+ * and biggest element at the end (biggest) and father < child relation.
+ * As updates can break relations, the function lift assures smallest elem at
+ * top of linked list (smallest). A true sort could be extended - keep this for
+ * later.
+ *
+ *
+ **/
+
+#ifndef TRIAL_H
+#define TRIAL_H
+
+#include "../../../geometry/Point.h"
+
+class Trial // father := smaller; child := bigger (in terms of cost); cost/speed := ptr to its cost/speed
+{
+ public:
+ long int key;
+ int* flag;
+ Trial* child;
+ Trial* father;
+ double* cost; //pointer to its cost (cost = &costarray[key])
+ double* speed;//pointer to its speed (speed = &speedarray[key])
+ Point* neggrad;
+
+ Trial() {
+ key = 0;
+ flag = nullptr;
+ child = nullptr;
+ father = nullptr;
+ cost = nullptr;
+ speed = nullptr;
+ neggrad = nullptr;
+ }
+
+ Trial(long int keyArg, Trial* fatherArg, Trial* childArg, double* t, double* f, int* flagArg, Point* neggradArg) {
+ key = keyArg;
+ father = fatherArg;
+ child = childArg;
+ cost = t;
+ speed = f;
+ flag = flagArg;
+ neggrad = neggradArg;
+ }
+
+ virtual ~Trial() {}
+
+ // insertion (in order)
+ void insert(Trial* &smallest, Trial* &biggest, Trial* add) {
+ if(smallest != nullptr) {
+ //long int key_curr = smallest->key;
+ if (smallest->cost[0] > add->cost[0]) {
+ add->child = smallest;
+ add->father = smallest->father;
+ smallest->father = add;
+ smallest = add;
+ } else {
+ insert(smallest->child, biggest, add);
+ }
+ } else { //wenn add der groesste wert ist, wird er durchgereich und muss korr am ende eingebunden werden.
+ smallest = add; //wird anstelle des nullptr eingefuegt (smallest ist nicht immer der global smallest sondern auch das rekursionsargument)
+ if (biggest == nullptr) { //indicator, dass keine liste existierte, also setze smallest und biggest auf add
+ add->father = nullptr;
+ } else { //es gab eine liste und das vorher letzte (biggest) wird nun vorletzter
+ add->father = biggest;
+ }
+
+ add->child = nullptr; //add ist letzter, dahinter nichts
+ biggest = add; //add wird neuer letzter (bzw. einziger im sonderfall "alles leer")
+ }
+ }
+
+// void sink(Trial* &smallest, Trial* &biggest) { //only asserts that biggest is at end
+// if (smallest != nullptr) {
+// Trial* aux;
+// aux = smallest->child;
+// if (aux != nullptr) {
+// if (smallest->cost[0] > aux->cost[0]) {
+// if (aux->child != nullptr) {
+// (aux->child)->father = smallest;
+// } else {
+// biggest = smallest;
+// }
+// if (smallest->father != nullptr) {
+// (smallest->father)->child = aux;
+// }
+// aux->father = smallest->father;
+// smallest->child = aux->child;
+// smallest->father = aux;
+// aux->child = smallest;
+// }
+// sink(aux, biggest);
+// }
+// }
+// }
+
+ void lift(Trial* &smallest, Trial* &biggest) { //only asserts that smallest is at start
+ if (biggest != nullptr) {
+ Trial* aux;
+ aux = biggest->father;
+ if (aux != nullptr) {
+ if (biggest->cost[0] < aux->cost[0]) {
+ if (aux->father != nullptr) {
+ (aux->father)->child = biggest;
+ } else {
+ smallest = biggest;
+ }
+ if (biggest->child != nullptr) {
+ (biggest->child)->father = aux;
+ }
+ aux->child = biggest->child;
+ biggest->father = aux->father;
+ biggest->child = aux;
+ aux->father = biggest;
+ }
+ lift(smallest, aux);
+ }
+ }
+ }
+
+ void removecurr(Trial* &smallest, Trial* &biggest, Trial* curr) {
+ if (smallest == curr && biggest == curr) {
+ smallest = nullptr;
+ biggest = nullptr;
+ } else {
+ if (smallest == curr) {
+ curr->child->father = nullptr;
+ smallest = curr->child;
+ } else {
+ if (biggest == curr) {
+ curr->father->child = nullptr;
+ biggest = curr->father;
+ } else {
+ curr->father->child = curr->child;
+ curr->child->father = curr->father;
+ }
+ }
+ }
+ curr->father = nullptr;
+ curr->child = nullptr;
+ }
+
+
+
+ protected:
+ private:
+};
+
+#endif // TRIAL_H
diff --git a/routing/global_shortest_trips/AccessPoint.cpp b/routing/global_shortest_trips/AccessPoint.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b7d0acd1173b5f7027ecb986b5e2dcd251a93218
--- /dev/null
+++ b/routing/global_shortest_trips/AccessPoint.cpp
@@ -0,0 +1,315 @@
+/**
+ * \file AccessPoint.cpp
+ * \date Aug 24, 2010
+ * \version v0.7
+ * \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
+ *
+ *
+ **/
+
+
+#include "AccessPoint.h"
+
+using namespace std;
+
+
+AccessPoint::AccessPoint(int id, double center[2],double radius)
+{
+ _id=id;
+ _center[0]=center[0];
+ _center[1]=center[1];
+ _radius=radius;
+ _finaExitToOutside=false;
+ _finalGoalOutside=false;
+ _room1ID=-1;
+ _room2ID=-1;
+ _connectingAPs.clear();
+ _mapDestToDist.clear();
+ pCentre=Point(center[0],center[1]);
+ _transitPedestrians = vector<Pedestrian*>();
+ _connectingAPs = vector<AccessPoint*>();
+ _isClosed=0;
+ _navLine=nullptr;
+}
+
+AccessPoint::~AccessPoint()
+{
+ if(_navLine) delete _navLine;
+}
+
+int AccessPoint::GetID()
+{
+ return _id;
+}
+
+int AccessPoint::IsClosed()
+{
+ return _isClosed;
+}
+
+void AccessPoint::SetClosed(int isClosed)
+{
+ _isClosed=isClosed;
+}
+void AccessPoint::SetFinalExitToOutside(bool isFinal)
+{
+ _finaExitToOutside=isFinal;
+}
+
+bool AccessPoint::GetFinalExitToOutside()
+{
+ return _finaExitToOutside;
+}
+
+const Point& AccessPoint::GetCentre() const
+{
+ return pCentre;
+}
+
+void AccessPoint::SetFinalGoalOutside(bool isFinal)
+{
+ _finalGoalOutside=isFinal;
+}
+
+bool AccessPoint::GetFinalGoalOutside()
+{
+ return _finalGoalOutside;
+}
+
+//TODO: possibly remove
+void AccessPoint::AddIntermediateDest(int final, int inter)
+{
+ _mapDestToAp[final]=inter;
+}
+
+void AccessPoint::AddFinalDestination(int UID, double distance)
+{
+ _mapDestToDist[UID]=distance;
+}
+
+double AccessPoint::GetDistanceTo(int UID)
+{
+ //this is probably a final destination
+ if(_mapDestToDist.count(UID)==0) {
+ Log->Write("ERROR:\tNo route to destination [ %d ]",UID);
+ Log->Write("ERROR:\tCheck your configuration file");
+ Dump();
+ //return 0;
+ exit(EXIT_FAILURE);
+ }
+ return _mapDestToDist[UID];
+}
+
+double AccessPoint::GetDistanceTo(AccessPoint* ap)
+{
+ return (pCentre-ap->GetCentre()).Norm();
+}
+
+void AccessPoint::AddConnectingAP(AccessPoint* ap)
+{
+ //only add of not already inside
+ for(unsigned int p=0; p<_connectingAPs.size(); p++) {
+ if(_connectingAPs[p]->GetID()==ap->GetID()) return;
+ }
+ _connectingAPs.push_back(ap);
+}
+
+//TODO: remove this one
+int AccessPoint::GetNextApTo(int UID)
+{
+ //this is probably a final destination
+ if(_mapDestToAp.count(UID)==0) {
+ Log->Write("ERROR:\tNo route to destination [ %d ]",UID);
+ Log->Write("ERROR:\t Did you forget to define the goal in the configuration file?");
+ Dump();
+ exit(EXIT_FAILURE);
+ }
+ return _mapDestToAp[UID];
+}
+
+int AccessPoint::GetNearestTransitAPTO(int UID)
+{
+ const vector <AccessPoint*>& possibleDest=_navigationGraphTo[UID];
+
+ if(possibleDest.size()==0) {
+ return -1;
+ } else if (possibleDest.size()==1) {
+ return possibleDest[0]->GetID();
+ } else {
+ AccessPoint* best_ap=possibleDest[0];
+ double min_dist=GetDistanceTo(best_ap) + best_ap->GetDistanceTo(UID); // FIXME: add the shortest distance to outside
+
+ for (unsigned int i=0; i<possibleDest.size(); i++) {
+ double tmp= GetDistanceTo(possibleDest[i]);
+ if(tmp<min_dist) {
+ min_dist=tmp;
+ best_ap=possibleDest[i];
+ }
+ }
+ return best_ap->GetID();
+ }
+}
+
+void AccessPoint::setConnectingRooms(int r1, int r2)
+{
+ _room1ID=r1;
+ _room2ID=r2;
+}
+
+double AccessPoint::DistanceTo(double x, double y)
+{
+
+ return sqrt((x-_center[0])*(x-_center[0]) + (y-_center[1])*(y-_center[1]));
+ //return _navLine->DistTo(Point(x,y));
+}
+
+
+bool AccessPoint::isInRange(int roomID)
+{
+ if((roomID!=_room1ID) && (roomID!=_room2ID)) {
+ return false;
+ }
+ return true;
+}
+
+bool AccessPoint::IsInRange(double xPed, double yPed, int roomID)
+{
+
+ if((roomID!=_room1ID)&& (roomID!=_room2ID)) {
+ return false;
+ }
+ if (((xPed - _center[0]) * (xPed - _center[0]) + (yPed - _center[1]) * (yPed
+ - _center[1])) <= _radius * _radius)
+ return true;
+
+ return false;
+}
+
+void AccessPoint::SetNavLine(NavLine* line)
+{
+ //todo: check this
+ //_navLine= line;
+ //_navLine->SetPoint1(line->GetPoint1());
+ //_navLine->SetPoint2(line->GetPoint2());
+ _navLine= new NavLine(*line);
+}
+
+NavLine* AccessPoint::GetNavLine() const
+{
+ return _navLine;
+}
+
+const vector <AccessPoint*>& AccessPoint::GetConnectingAPs()
+{
+ return _connectingAPs;
+}
+
+void AccessPoint::RemoveConnectingAP(AccessPoint* ap)
+{
+ vector<AccessPoint*>::iterator it;
+ it = find (_connectingAPs.begin(), _connectingAPs.end(), ap);
+ if(it==_connectingAPs.end()) {
+ cout<<" there is no connection to AP: "<< ap->GetID()<<endl;
+ } else {
+ _connectingAPs.erase(it);
+ }
+}
+
+const vector <AccessPoint*>& AccessPoint::GetTransitAPsTo(int UID)
+{
+ return _navigationGraphTo[UID];
+}
+
+void AccessPoint::AddTransitAPsTo(int UID,AccessPoint* ap)
+{
+ _navigationGraphTo[UID].push_back(ap);
+}
+
+void AccessPoint::Reset(int UID)
+{
+ _navigationGraphTo[UID].clear();
+}
+
+
+void AccessPoint::SetFriendlyName(const std::string& name)
+{
+ _friendlyName=name;
+}
+
+
+const std::string AccessPoint::GetFriendlyName()
+{
+ return _friendlyName;
+}
+
+
+void AccessPoint::Dump()
+{
+
+ cout<<endl<<"--------> Dumping AP <-----------"<<endl<<endl;
+ //cout<<" ID: " <<_id<<" centre = [ "<< _center[0] <<", " <<_center[1] <<" ]"<<endl;
+ cout<<" Friendly ID: " <<_friendlyName<<" centre = [ "<< _center[0] <<", " <<_center[1] <<" ]"<<endl;
+ cout<<" Real ID: " <<_id<<endl;
+ cout<<" Length: "<<_navLine->LengthSquare()<<endl;
+
+ cout <<" Is final exit to outside :"<<GetFinalExitToOutside()<<endl;
+ cout <<" Distance to final goals"<<endl;
+
+ for(std::map<int, double>::iterator p = _mapDestToDist.begin(); p != _mapDestToDist.end(); ++p) {
+ cout<<"\t [ "<<p->first<<", " << p->second<<" m ]";
+ }
+ cout<<endl<<endl;
+
+ cout<<" transit to final goals:"<<endl;
+ for(std::map<int, std::vector<AccessPoint*> >::iterator p = _navigationGraphTo.begin(); p != _navigationGraphTo.end(); ++p) {
+ cout<<endl<<"\t to UID ---> [ "<<p->first <<" ]";
+
+ if(p->second.size()==0) {
+ cout<<"\t ---> [ Nothing ]";
+ } else {
+
+ for(unsigned int i=0; i<p->second.size(); i++) {
+ cout<<"\t distance ---> [ "<<GetDistanceTo(p->second[i])+p->second[i]->GetDistanceTo(p->first) <<" m via "<<p->second[i]->GetID() <<" ]";
+ //cout<<"\t distance ---> [ "<<p->second[i]->GetID()<<" @ " << GetDistanceTo(p->first)<<" ]";
+ }
+ }
+ }
+
+ cout<<endl<<endl;
+
+ cout<<" connected to aps : " ;
+ for(unsigned int p=0; p<_connectingAPs.size(); p++) {
+ //cout<<" [ "<<_connectingAPs[p]->GetID()<<" , "<<_connectingAPs[p]->GetDistanceTo(this)<<" m ]";
+ cout<<endl<<"\t [ "<<_connectingAPs[p]->GetID()<<"_"<<_connectingAPs[p]->GetFriendlyName()<<" , "<<_connectingAPs[p]->GetDistanceTo(this)<<" m ]";
+ }
+
+ cout<<endl<<endl;
+ cout <<" queue [ ";
+ for(unsigned int p=0; p<_transitPedestrians.size(); p++) {
+ cout<<" "<<_transitPedestrians[p]->GetID();
+ }
+ cout<<" ]"<<endl;
+
+ //cout<<endl<<" connected to rooms: "<<_room1ID<<" and "<<_room2ID<<endl;
+ cout<<endl;
+ cout<<endl<<"------------------------------"<<endl<<endl;
+
+}
diff --git a/routing/global_shortest_trips/AccessPoint.h b/routing/global_shortest_trips/AccessPoint.h
new file mode 100644
index 0000000000000000000000000000000000000000..a214729ca44e30c909e1df46ec3db107eca948d6
--- /dev/null
+++ b/routing/global_shortest_trips/AccessPoint.h
@@ -0,0 +1,228 @@
+/**
+ * \file AccessPoint.h
+ * \date Aug 24, 2010
+ * \version v0.7
+ * \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
+ *
+ *
+ **/
+
+
+#ifndef ACCESSPOINT_H_
+#define ACCESSPOINT_H_
+
+#include <map>
+#include <vector>
+#include <cmath>
+#include <iostream>
+#include <string>
+#include <algorithm>
+#include "../../pedestrian/Pedestrian.h"
+#include "../../geometry/Point.h"
+#include "../../geometry/Line.h"
+
+class AccessPoint {
+public:
+
+ /**
+ * create a new access point with the provided data
+ *
+ * @param id
+ * @param center
+ * @param radius
+ */
+
+ AccessPoint(int id, double center[2],double radius=0.30);
+
+ /**
+ *
+ * destroy the access point
+ */
+ virtual ~AccessPoint();
+
+ /**
+ * @return the Id of the access point
+ */
+ int GetID();
+
+ /**
+ * determines if the given coordinates (x ,y) are in the range of that Aps.
+ *
+ * @param x
+ * @param y
+ * @return
+ */
+ bool IsInRange(double xPed, double yPed, int roomID);
+
+ /**
+ * given the actual room of the pedestrian
+ * determine if that AP is 'visible'
+ */
+
+ bool isInRange(int roomID);
+
+ /**
+ * each AP is connected to at most 2 rooms
+ * @param r1
+ * @param r1
+ */
+ void setConnectingRooms(int r1, int r2);
+
+ int GetConnectingRoom1()
+ {
+ return _room1ID;
+ }
+
+ int GetConnectingRoom2()
+ {
+ return _room2ID;
+ }
+
+ /**
+ * return the distance to the point x ,y
+ * @param x
+ * @param y
+ * @return
+ */
+ double DistanceTo(double x, double y);
+
+ /**
+ * set/get a human readable name for this points.
+ * t23 means Transition with id 23
+ * c23 means Crossing with id 23
+ * h23 means Hlines with id 23
+ */
+ void SetFriendlyName(const std::string& name);
+
+ /**
+ * set/get a human readable name for this points.
+ * t23 means Transition with id 23
+ * c23 means Crossing with id 23
+ * h23 means Hlines with id 23
+ */
+ const std::string GetFriendlyName();
+
+ /**
+ * dump the class
+ */
+ void Dump();
+
+ /**
+ * Set/Get the centre of the navigation line defining this access point
+ */
+ const Point& GetCentre() const;
+
+ /**
+ * True if this is the last exit that leads to the outside
+ */
+ void SetFinalExitToOutside(bool isFinal);
+
+ /**
+ * True if this is the last exit that leads to the outside
+ */
+ bool GetFinalExitToOutside();
+
+ /**
+ * @return true if the door is closed
+ */
+ int IsClosed();
+
+ /**
+ * Close the door
+ * @param isClosed
+ */
+ void SetClosed(int isClosed);
+
+ /**
+ * Set/Get the navigation line.
+ * The direction taken by the pedestrian strongly depends on this line.
+ */
+ void SetNavLine(NavLine* line);
+
+ /**
+ * Set/Get the navigation line.
+ * The direction taken by the pedestrian strongly depends on this line.
+ */
+ NavLine* GetNavLine() const;
+
+ /**
+ * True if this is a goal outside the building
+ */
+ void SetFinalGoalOutside(bool isFinal);
+
+ /**
+ * True if this is a goal outside the building
+ */
+ bool GetFinalGoalOutside();
+
+ void AddIntermediateDest(int final, int inter);
+ void AddFinalDestination(int UID, double distance);
+ double GetDistanceTo(int UID);
+ double GetDistanceTo(AccessPoint* ap);
+ void RemoveConnectingAP(AccessPoint* ap);
+
+ // reset all setting relative to the destination
+ void Reset(int UID=FINAL_DEST_OUT);
+
+ //FIXME: remove those functions
+ void AddConnectingAP(AccessPoint* ap);
+ int GetNextApTo(int UID=FINAL_DEST_OUT); //default is the shortest path to the outside ( -1 )
+ const std::vector <AccessPoint*>& GetConnectingAPs();
+
+
+ const std::vector <AccessPoint*>& GetTransitAPsTo(int UID=FINAL_DEST_OUT);
+ int GetNearestTransitAPTO(int UID=FINAL_DEST_OUT);
+ void AddTransitAPsTo(int UID,AccessPoint* ap);
+
+private:
+ int _id;
+ double _center[2];
+ double _radius;
+ /// true if this exit leads to outside
+ bool _finaExitToOutside;
+ /// true if this a goal outside the building
+ bool _finalGoalOutside;
+ int _room1ID;
+ int _room2ID;
+ Point pCentre;
+ NavLine* _navLine;
+ std::vector<Pedestrian*> _transitPedestrians;
+ int _isClosed;
+ std::string _friendlyName;
+
+ // stores the connecting APs
+ std::vector<AccessPoint*>_connectingAPs;
+
+ // store part of a graph
+ // map a final destination to the next ap to reach it
+ // store the nearest AP to reach the destination
+ std::map<int, int> _mapDestToAp;
+
+ // store part of the weight matrix
+ // store the total distance to the destination int
+ std::map <int,double> _mapDestToDist;
+
+ //store the navigation graph
+ std::map<int,std::vector<AccessPoint*> > _navigationGraphTo;
+
+};
+
+#endif /* ACCESSPOINT_H_ */
diff --git a/routing/global_shortest_trips/ConvexDecomp.h b/routing/global_shortest_trips/ConvexDecomp.h
new file mode 100644
index 0000000000000000000000000000000000000000..a88841552231f80f6fcc241f7d6728d31e6fa09e
--- /dev/null
+++ b/routing/global_shortest_trips/ConvexDecomp.h
@@ -0,0 +1,327 @@
+/**
+ * \file ConvexDecomp.h
+ * \date Jul 4, 2014
+ * \version v0.7
+ * \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
+ *
+ *
+ **/
+
+
+#ifndef _GEOMETRY_FUNCTIONS_
+#define _GEOMETRY_FUNCTIONS_
+
+#ifdef _CGAL
+#include <CGAL/basic.h>
+#include <CGAL/Exact_predicates_inexact_constructions_kernel.h>
+#include <CGAL/Partition_traits_2.h>
+#include <CGAL/Partition_is_valid_traits_2.h>
+#include <CGAL/polygon_function_objects.h>
+#include <CGAL/partition_2.h>
+#include <CGAL/point_generators_2.h>
+#include <CGAL/random_polygon_2.h>
+#include <CGAL/connect_holes.h>
+#include <cassert>
+#include <list>
+//
+//#include <CGAL/Partition_traits_2.h>
+//#include <CGAL/Partition_is_valid_traits_2.h>
+//#include <CGAL/polygon_function_objects.h>
+//#include <CGAL/partition_2.h>
+//#include <CGAL/point_generators_2.h>
+//#include <CGAL/random_polygon_2.h>
+//
+
+#include <CGAL/Constrained_Delaunay_triangulation_2.h>
+#include <CGAL/Triangulation_face_base_with_info_2.h>
+#include <CGAL/Polygon_2.h>
+
+#include <CGAL/IO/Geomview_stream.h>
+#include <CGAL/Delaunay_triangulation_2.h>
+#include <CGAL/Alpha_shape_euclidean_traits_2.h>
+#include <CGAL/Alpha_shape_vertex_base_2.h>
+#include <CGAL/Triangulation_face_base_2.h>
+#include <CGAL/Alpha_shape_face_base_2.h>
+#include <CGAL/Alpha_shape_2.h>
+
+#include <iostream>
+#include <cstdio>
+#include <cstdlib>
+
+
+#include <CGAL/Cartesian.h>
+#include <CGAL/centroid.h>
+
+
+struct FaceInfo2 {
+ FaceInfo2() {}
+ int nesting_level;
+
+ bool in_domain() {
+ return nesting_level%2 == 1;
+ }
+};
+
+
+
+typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
+typedef CGAL::Partition_traits_2<K> Traits;
+typedef CGAL::Is_convex_2<Traits> Is_convex_2;
+typedef Traits::Polygon_2 Polygon_2;
+//typedef CGAL::Polygon_2<K> Polygon_2;
+typedef Traits::Point_2 Point_2;
+typedef Polygon_2::Vertex_const_iterator Vertex_iterator;
+typedef std::list<Polygon_2> Polygon_list;
+typedef Polygon_list::const_iterator Polygon_iterator;
+typedef CGAL::Partition_is_valid_traits_2<Traits, Is_convex_2> Validity_traits;
+typedef Polygon_2::Edge_const_iterator Edge_iterator;
+
+
+typedef CGAL::Triangulation_vertex_base_2<K> Vb;
+typedef CGAL::Triangulation_face_base_with_info_2<FaceInfo2,K> Fbb;
+typedef CGAL::Constrained_triangulation_face_base_2<K,Fbb> Fb;
+typedef CGAL::Triangulation_data_structure_2<Vb,Fb> TDS;
+typedef CGAL::Exact_predicates_tag Itag;
+typedef CGAL::Constrained_Delaunay_triangulation_2<K, TDS, Itag> CDT;
+
+
+
+typedef CGAL::Alpha_shape_vertex_base_2<K> Av;
+typedef CGAL::Triangulation_face_base_2<K> Tf;
+typedef CGAL::Alpha_shape_face_base_2<K,Tf> Af;
+typedef CGAL::Triangulation_default_data_structure_2<K,Av,Af> Tds;
+typedef CGAL::Delaunay_triangulation_2<K,Tds> DT;
+
+typedef CGAL::Polygon_with_holes_2<K> Polygon_with_holes_2;
+
+typedef CGAL::Segment_2<K> Segment_2;
+
+//fast delauney
+//typedef CGAL::Alpha_shape_vertex_base_3<K> Vb;
+//typedef CGAL::Alpha_shape_cell_base_3<K> Fb;
+//typedef CGAL::Triangulation_data_structure_3<Vb,Fb> Tds;
+//typedef CGAL::Delaunay_triangulation_3<K,Tds,CGAL::Fast_location> Delaunay;
+//typedef CGAL::Alpha_shape_3<Delaunay> Alpha_shape_3;
+//typedef Alpha_shape_3::Alpha_iterator Alpha_iterator;
+//typedef Alpha_shape_3::NT NT;
+
+
+typedef CGAL::Alpha_shape_2<DT> Alpha_shape_2;
+typedef Alpha_shape_2::Alpha_iterator Alpha_iterator;
+typedef Alpha_shape_2::Vertex_iterator Alpha_vertex_iterator;
+
+
+
+
+inline
+void
+mark_domains(CDT& ct,
+ CDT::Face_handle start,
+ int index,
+ std::list<CDT::Edge>& border )
+{
+ if(start->info().nesting_level != -1) {
+ return;
+ }
+ std::list<CDT::Face_handle> queue;
+ queue.push_back(start);
+
+ while(! queue.empty()) {
+ CDT::Face_handle fh = queue.front();
+ queue.pop_front();
+ if(fh->info().nesting_level == -1) {
+ fh->info().nesting_level = index;
+ for(int i = 0; i < 3; i++) {
+ CDT::Edge e(fh,i);
+ CDT::Face_handle n = fh->neighbor(i);
+ if(n->info().nesting_level == -1) {
+ if(ct.is_constrained(e)) border.push_back(e);
+ else queue.push_back(n);
+ }
+ }
+ }
+ }
+}
+
+//explore set of facets connected with non constrained edges,
+//and attribute to each such set a nesting level.
+//We start from facets incident to the infinite vertex, with a nesting
+//level of 0. Then we recursively consider the non-explored facets incident
+//to constrained edges bounding the former set and increase the nesting level by 1.
+//Facets in the domain are those with an odd nesting level.
+inline
+void
+mark_domains(CDT& cdt)
+{
+ for(CDT::All_faces_iterator it = cdt.all_faces_begin(); it != cdt.all_faces_end(); ++it) {
+ it->info().nesting_level = -1;
+ }
+
+ int index = 0;
+ std::list<CDT::Edge> border;
+ mark_domains(cdt, cdt.infinite_face(), index++, border);
+ while(! border.empty()) {
+ CDT::Edge e = border.front();
+ border.pop_front();
+ CDT::Face_handle n = e.first->neighbor(e.second);
+ if(n->info().nesting_level == -1) {
+ mark_domains(cdt, n, e.first->info().nesting_level+1, border);
+ }
+ }
+}
+
+inline
+void insert_polygon(CDT& cdt,const Polygon_2& polygon)
+{
+ if ( polygon.is_empty() ) return;
+ CDT::Vertex_handle v_prev=cdt.insert(*CGAL::cpp0x::prev(polygon.vertices_end()));
+ for (Polygon_2::Vertex_iterator vit=polygon.vertices_begin();
+ vit!=polygon.vertices_end(); ++vit) {
+ CDT::Vertex_handle vh=cdt.insert(*vit);
+ cdt.insert_constraint(vh,v_prev);
+ v_prev=vh;
+ }
+}
+
+inline
+int test_triangulation( )
+{
+ //construct two non-intersecting nested polygons
+ Polygon_2 polygon1;
+ polygon1.push_back(Point_2(0,0));
+ polygon1.push_back(Point_2(2,0));
+ polygon1.push_back(Point_2(2,2));
+ polygon1.push_back(Point_2(0,2));
+ Polygon_2 polygon2;
+ polygon2.push_back(Point_2(0.5,0.5));
+ polygon2.push_back(Point_2(1.5,0.5));
+ polygon2.push_back(Point_2(1.5,1.5));
+ polygon2.push_back(Point_2(0.5,1.5));
+
+ //Insert the polyons into a constrained triangulation
+ CDT cdt;
+ insert_polygon(cdt,polygon1);
+ insert_polygon(cdt,polygon2);
+
+ //Mark facets that are inside the domain bounded by the polygon
+ mark_domains(cdt);
+
+ int count=0;
+ for (CDT::Finite_faces_iterator fit=cdt.finite_faces_begin();
+ fit!=cdt.finite_faces_end(); ++fit) {
+ if ( fit->info().in_domain() ) ++count;
+ }
+
+ //cdt.draw_triangulation(std::cout);
+ std::cout << "There are " << count << " facets in the domain." << std::endl;
+
+
+ CGAL::Geomview_stream gv(CGAL::Bbox_3(-100, -100, -100, 100, 100, 100));
+ gv.set_line_width(4);
+ gv.set_trace(true);
+ gv.set_bg_color(CGAL::Color(0, 200, 200));
+ // gv.clear();
+
+ // use different colors, and put a few sleeps/clear.
+ //gv << CGAL::BLUE;
+ //gv.set_wired(true);
+
+
+ CDT::Finite_faces_iterator it;
+ for (it = cdt.finite_faces_begin(); it != cdt.finite_faces_end(); it++) {
+ std::cout << cdt.triangle(it) << std::endl;
+ gv << cdt.triangle(it) ;
+ }
+
+ return 0;
+}
+
+inline
+int test_alpha_shape()
+{
+ std::list<Point_2> lp;
+
+
+ lp.push_back(Point_2(0.5,0.5));
+ lp.push_back(Point_2(1.5,0.5));
+ lp.push_back(Point_2(1.5,1.5));
+ lp.push_back(Point_2(0.5,1.5));
+
+ // compute alpha shape
+ Alpha_shape_2 as(lp.begin(),lp.end());
+ std::cout << "Alpha shape computed in REGULARIZED mode by default"
+ << std::endl;
+
+
+ std::list<Point_2> env=as.Output();
+ std::cout <<"size: " <<env.size()<<std::endl;
+ getc(stdin);
+
+ for (std::list<Point_2>::const_iterator iterator = env.begin(), end = env.end(); iterator != end; ++iterator) {
+ std::cout << *iterator;
+ }
+
+ // for (Alpha_vertex_iterator vit = as.Alpha_shape_vertices_begin();
+ // vit != as.alpha_shape_vertices_end(); ++vit) {
+ //
+ // }
+
+ // find optimal alpha value
+ Alpha_iterator opt = as.find_optimal_alpha(1);
+ std::cout << "Optimal alpha value to get one connected component is "
+ << *opt << std::endl;
+ as.set_alpha(*opt);
+ assert(as.number_of_solid_components() == 1);
+ return 0;
+}
+
+template<class Kernel, class Container>
+void print_polygon (const CGAL::Polygon_2<Kernel, Container>& P)
+{
+ typename CGAL::Polygon_2<Kernel, Container>::Vertex_const_iterator vit;
+
+ std::cout << "[ " << P.size() << " vertices:";
+ for (vit = P.vertices_begin(); vit != P.vertices_end(); ++vit)
+ std::cout << " (" << *vit << ')';
+ std::cout << " ]" << std::endl;
+}
+
+template<class Kernel, class Container>
+void print_polygon_with_holes(const CGAL::Polygon_with_holes_2<Kernel, Container> & pwh)
+{
+ if (! pwh.is_unbounded()) {
+ std::cout << "{ Outer boundary = ";
+ print_polygon (pwh.outer_boundary());
+ } else
+ std::cout << "{ Unbounded polygon." << std::endl;
+
+ typename CGAL::Polygon_with_holes_2<Kernel,Container>::Hole_const_iterator hit;
+ unsigned int k = 1;
+
+ std::cout << " " << pwh.number_of_holes() << " holes:" << std::endl;
+ for (hit = pwh.holes_begin(); hit != pwh.holes_end(); ++hit, ++k) {
+ std::cout << " Hole #" << k << " = ";
+ print_polygon (*hit);
+ }
+ std::cout << " }" << std::endl;
+}
+#endif /* _CGAL */
+#endif /* _GEOMETRY_FUNCTIONS_ */
diff --git a/routing/global_shortest_trips/DTriangulation.cpp b/routing/global_shortest_trips/DTriangulation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..546bafebc682cef894ef0d2267014e8252955bf1
--- /dev/null
+++ b/routing/global_shortest_trips/DTriangulation.cpp
@@ -0,0 +1,78 @@
+/**
+ * \file DTriangulation.cpp
+ * \date Nov 30, 2012
+ * \version v0.7
+ * \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
+ *
+ *
+ **/
+
+
+#include "DTriangulation.h"
+
+using namespace std;
+
+DTriangulation::DTriangulation()
+{
+ _cdt=nullptr;
+}
+
+DTriangulation::~DTriangulation()
+{
+
+ for(unsigned int i = 0; i < _holesPolylines.size(); i++) {
+ vector<p2t::Point*> poly = _holesPolylines[i];
+ FreeClear(poly);
+ }
+
+ FreeClear(_outerConstraintsPolyline);
+ delete _cdt;
+}
+
+void DTriangulation::Triangulate()
+{
+
+ _cdt= new p2t::CDT(_outerConstraintsPolyline);
+
+ for(unsigned int h=0; h<_holesPolylines.size(); h++) {
+ _cdt->AddHole(_holesPolylines[h]);
+ }
+ _cdt->Triangulate();
+}
+
+void DTriangulation::SetOuterPolygone(const std::vector<Point>& outPoly)
+{
+
+ for(unsigned int i=0; i<outPoly.size(); i++) {
+ _outerConstraintsPolyline.push_back(new p2t::Point(outPoly[i]._x,outPoly[i]._y));
+ }
+}
+
+void DTriangulation::AddHole(const std::vector<Point>& hole)
+{
+
+ std::vector<p2t::Point*> newHole;
+
+ for(unsigned int i=0; i<hole.size(); i++) {
+ newHole.push_back(new p2t::Point(hole[i]._x,hole[i]._y));
+ }
+ _holesPolylines.push_back(newHole);
+}
diff --git a/routing/global_shortest_trips/DTriangulation.h b/routing/global_shortest_trips/DTriangulation.h
new file mode 100644
index 0000000000000000000000000000000000000000..69cbd10e5496a5c3702734957906bc7c5b7eb51f
--- /dev/null
+++ b/routing/global_shortest_trips/DTriangulation.h
@@ -0,0 +1,96 @@
+/**
+ * \file DTriangulation.h
+ * \date Nov 30, 2012
+ * \version v0.7
+ * \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
+ * Perform the Delauney triangulation of a polygon with holes.
+ *
+ *
+ **/
+
+
+#ifndef DTRIANGULATION_H_
+#define DTRIANGULATION_H_
+
+#include <cstdlib>
+#include <time.h>
+#include <fstream>
+#include <string>
+#include <sstream>
+#include <algorithm>
+#include <iterator>
+#include <iostream>
+#include <vector>
+
+#include "../../geometry/Point.h"
+#include "../../poly2tri/poly2tri.h"
+
+
+class DTriangulation {
+public:
+ DTriangulation();
+
+ virtual ~DTriangulation();
+
+ /**
+ * Triangulate the specified domain
+ * \see SetOuterPolygone
+ * \see AddHole
+ */
+ void Triangulate();
+
+ /**
+ * @return the triangles resulting from the triangulation
+ */
+ std::vector<p2t::Triangle*> GetTriangles() {
+ return _cdt->GetTriangles();
+ }
+
+ /**
+ * Set the boundaries of the domain
+ * @param outerConstraints
+ */
+ void SetOuterPolygone(const std::vector<Point>& outerConstraints);
+
+ /**
+ * Add a new hole
+ * A domain can contains holes.
+ * They should fully be inside the domain.
+ */
+ void AddHole(const std::vector<Point>& hole);
+
+ //templates for freeing and clearing a vector of pointers
+ template <class C> void FreeClear( C & cntr ) {
+ for ( typename C::iterator it = cntr.begin();
+ it != cntr.end(); ++it ) {
+ delete * it;
+ }
+ cntr.clear();
+ }
+
+private:
+ std::vector< std::vector<p2t::Point*> > _holesPolylines;
+ std::vector<p2t::Point*> _outerConstraintsPolyline;
+ p2t::CDT* _cdt;
+
+};
+
+#endif /* DTRIANGULATION_H_ */
diff --git a/routing/global_shortest_trips/GlobalRouterTrips.cpp b/routing/global_shortest_trips/GlobalRouterTrips.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aef2dc98bd650322589c2b3839ebcfd420c6b792
--- /dev/null
+++ b/routing/global_shortest_trips/GlobalRouterTrips.cpp
@@ -0,0 +1,1612 @@
+/**
+ * \file GlobalRouter.cpp
+ * \date Dec 15, 2010
+ * \version v0.7
+ * \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
+ *
+ *
+ **/
+
+
+#include "GlobalRouterTrips.h"
+
+#include "AccessPoint.h"
+//#include "Router.h"
+//#include "NavMesh.h"
+#include "DTriangulation.h"
+
+//#include "../geometry/Building.h"
+//#include "../pedestrian/Pedestrian.h"
+#include "../../tinyxml/tinyxml.h"
+#include "../../geometry/SubRoom.h"
+//#include "../geometry/Wall.h"
+//#include "../IO/OutputHandler.h"
+
+//#include <sstream>
+//#include <cfloat>
+//#include <fstream>
+//#include <iomanip>
+
+
+using namespace std;
+
+GlobalRouterTrips::GlobalRouterTrips() :
+ Router()
+{
+ _accessPoints = map<int, AccessPoint*>();
+ _map_id_to_index = std::map<int, int>();
+ _map_index_to_id = std::map<int, int>();
+ _distMatrix = nullptr;
+ _pathsMatrix = nullptr;
+ _building = nullptr;
+ _edgeCost=100;
+ // _rdDistribution = uniform_real_distribution<double> (0,1);
+ // _rdGenerator = default_random_engine(56);
+
+}
+
+GlobalRouterTrips::GlobalRouterTrips(int id, RoutingStrategy s) : Router(id, s)
+{
+ _accessPoints = map<int, AccessPoint*>();
+ _map_id_to_index = std::map<int, int>();
+ _map_index_to_id = std::map<int, int>();
+ _distMatrix = nullptr;
+ _pathsMatrix = nullptr;
+ _building = nullptr;
+ _edgeCost=100;
+
+ // _rdDistribution = uniform_real_distribution<double> (0,1);
+ // _rdGenerator = default_random_engine(56);
+
+}
+
+GlobalRouterTrips::~GlobalRouterTrips()
+{
+ if (_distMatrix && _pathsMatrix) {
+ const int exitsCnt = (const int) (_building->GetNumberOfGoals() + _building->GetAllGoals().size());
+ for (int p = 0; p < exitsCnt; ++p) {
+ delete[] _distMatrix[p];
+ delete[] _pathsMatrix[p];
+ }
+
+ delete[] _distMatrix;
+ delete[] _pathsMatrix;
+ }
+
+ map<int, AccessPoint*>::const_iterator itr;
+ for (itr = _accessPoints.begin(); itr != _accessPoints.end(); ++itr) {
+ delete itr->second;
+ }
+ _accessPoints.clear();
+}
+
+bool GlobalRouterTrips::Init(Building* building)
+{
+ //necessary if the init is called several times during the simulation
+ Reset();
+ Log->Write("INFO:\tInit the Global Router Trips Engine");
+ _building = building;
+ //only load the information if not previously loaded
+ //if(_building->GetNumberOfGoals()==0)
+
+ //TODO: implement the ParseAdditionalParameter Interface
+ LoadRoutingInfos(GetRoutingInfoFile());
+
+ if(_generateNavigationMesh)
+ {
+ //GenerateNavigationMesh();
+ TriangulateGeometry();
+ //return true;
+ }
+
+ // initialize the distances matrix for the floydwahrshall
+ const int exitsCnt = (const int) (_building->GetNumberOfGoals() + _building->GetAllGoals().size());
+
+ _distMatrix = new double*[exitsCnt];
+ _pathsMatrix = new int*[exitsCnt];
+
+ for (int i = 0; i < exitsCnt; ++i) {
+ _distMatrix[i] = new double[exitsCnt];
+ _pathsMatrix[i] = new int[exitsCnt];
+ }
+
+ // Initializing the values
+ // all nodes are disconnected
+ for (int p = 0; p < exitsCnt; ++p) {
+ for (int r = 0; r < exitsCnt; ++r) {
+ _distMatrix[p][r] = (r == p) ? 0.0 : FLT_MAX;/*0.0*/
+ _pathsMatrix[p][r] = p;/*0.0*/
+ }
+ }
+
+ // init the access points
+ int index = 0;
+ for(const auto & itr:_building->GetAllHlines())
+ {
+ //int door=itr->first;
+ int door = itr.second->GetUniqueID();
+ Hline* cross = itr.second;
+ Point centre = cross->GetCentre();
+ double center[2] = { centre._x, centre._y };
+
+ AccessPoint* ap = new AccessPoint(door, center);
+ ap->SetNavLine(cross);
+ char friendlyName[CLENGTH];
+ sprintf(friendlyName, "hline_%d_room_%d_subroom_%d", cross->GetID(),
+ cross->GetRoom1()->GetID(),
+ cross->GetSubRoom1()->GetSubRoomID());
+ ap->SetFriendlyName(friendlyName);
+
+ // save the connecting sub/rooms IDs
+ int id1 = -1;
+ if (cross->GetSubRoom1()) {
+ id1 = cross->GetSubRoom1()->GetUID();
+ }
+
+ ap->setConnectingRooms(id1, id1);
+ _accessPoints[door] = ap;
+
+ //very nasty
+ _map_id_to_index[door] = index;
+ _map_index_to_id[index] = door;
+ index++;
+ }
+
+ for(const auto & itr:_building->GetAllCrossings())
+ {
+ int door = itr.second->GetUniqueID();
+ Crossing* cross = itr.second;
+ const Point& centre = cross->GetCentre();
+ double center[2] = { centre._x, centre._y };
+
+ AccessPoint* ap = new AccessPoint(door, center);
+ ap->SetNavLine(cross);
+ char friendlyName[CLENGTH];
+ sprintf(friendlyName, "cross_%d_room_%d_subroom_%d", cross->GetID(),
+ cross->GetRoom1()->GetID(),
+ cross->GetSubRoom1()->GetSubRoomID());
+ ap->SetFriendlyName(friendlyName);
+
+ ap->SetClosed(!cross->IsOpen());
+ // save the connecting sub/rooms IDs
+ int id1 = -1;
+ if (cross->GetSubRoom1()) {
+ id1 = cross->GetSubRoom1()->GetUID();
+ }
+
+ int id2 = -1;
+ if (cross->GetSubRoom2()) {
+ id2 = cross->GetSubRoom2()->GetUID();
+ }
+
+ ap->setConnectingRooms(id1, id2);
+ _accessPoints[door] = ap;
+
+ //very nasty
+ _map_id_to_index[door] = index;
+ _map_index_to_id[index] = door;
+ index++;
+ }
+
+
+ for(const auto & itr:_building->GetAllTransitions())
+ {
+ int door = itr.second->GetUniqueID();
+ Transition* cross = itr.second;
+ const Point& centre = cross->GetCentre();
+ double center[2] = { centre._x, centre._y };
+
+ AccessPoint* ap = new AccessPoint(door, center);
+ ap->SetNavLine(cross);
+ char friendlyName[CLENGTH];
+ sprintf(friendlyName, "trans_%d_room_%d_subroom_%d", cross->GetID(),
+ cross->GetRoom1()->GetID(),
+ cross->GetSubRoom1()->GetSubRoomID());
+ ap->SetFriendlyName(friendlyName);
+
+ ap->SetClosed(!cross->IsOpen());
+ // save the connecting sub/rooms IDs
+ int id1 = -1;
+ if (cross->GetSubRoom1()) {
+ id1 = cross->GetSubRoom1()->GetUID();
+ }
+
+ int id2 = -1;
+ if (cross->GetSubRoom2()) {
+ id2 = cross->GetSubRoom2()->GetUID();
+ }
+
+ ap->setConnectingRooms(id1, id2);
+ _accessPoints[door] = ap;
+
+ //set the final destination
+ if (cross->IsExit() && cross->IsOpen()) {
+ ap->SetFinalExitToOutside(true);
+ Log->Write("INFO: \tExit to outside found: %d [%s]",ap->GetID(),ap->GetFriendlyName().c_str());
+ } else if ((id1 == -1) && (id2 == -1)) {
+ Log->Write("INFO:\t a final destination outside the geometry was found");
+ ap->SetFinalExitToOutside(true);
+ } else if (cross->GetRoom1()->GetCaption() == "outside") {
+ ap->SetFinalExitToOutside(true);
+ }
+
+ //very nasty
+ _map_id_to_index[door] = index;
+ _map_index_to_id[index] = door;
+ index++;
+ }
+
+ // populate the subrooms at the elevation
+ for(auto && itroom:_building->GetAllRooms())
+ {
+ auto&& room= (shared_ptr<Room>&&) itroom.second;
+ for(const auto & it_sub:room->GetAllSubRooms())
+ {
+ auto&& sub= (shared_ptr<SubRoom>&&) it_sub.second;
+ //maybe truncate the elevation.
+ // because using a double as key to map is not exact
+ //double elevation = ceilf(sub->GetMaxElevation() * 100) / 100;
+ //_subroomsAtElevation[elevation].push_back(sub.get());
+ _subroomsAtElevation[sub->GetElevation(sub->GetCentroid())].push_back(sub.get());
+ }
+ }
+
+ // loop over the rooms
+ // loop over the subrooms
+ // get the transitions in the subrooms
+ // and compute the distances
+
+ for(auto && itroom:_building->GetAllRooms())
+ {
+ auto&& room= (shared_ptr<Room>&&) itroom.second;
+ for(const auto & it_sub:room->GetAllSubRooms())
+ {
+ // The penalty factor should discourage pedestrians to evacuation through rooms
+ auto&& sub= (shared_ptr<SubRoom>&&) it_sub.second;
+ double penalty=1.0;
+ if((sub->GetType()!="floor") && (sub->GetType()!="dA") ) {
+ penalty=_edgeCost;
+ }
+
+ //collect all navigation objects
+ vector<Hline*> allGoals;
+ const auto & crossings = sub->GetAllCrossings();
+ allGoals.insert(allGoals.end(), crossings.begin(), crossings.end());
+ const auto & transitions = sub->GetAllTransitions();
+ allGoals.insert(allGoals.end(), transitions.begin(),
+ transitions.end());
+ const auto & hlines = sub->GetAllHlines();
+ allGoals.insert(allGoals.end(), hlines.begin(), hlines.end());
+
+
+ //process the hlines
+ //process the crossings
+ //process the transitions
+ for (unsigned int n1 = 0; n1 < allGoals.size(); n1++)
+ {
+ Hline* nav1 = allGoals[n1];
+ AccessPoint* from_AP = _accessPoints[nav1->GetUniqueID()];
+ int from_door = _map_id_to_index[nav1->GetUniqueID()];
+ if(from_AP->IsClosed()) continue;
+
+ for (unsigned int n2 = 0; n2 < allGoals.size(); n2++) {
+ Hline* nav2 = allGoals[n2];
+ AccessPoint* to_AP = _accessPoints[nav2->GetUniqueID()];
+ if(to_AP->IsClosed()) continue;
+
+ if (n1 == n2)
+ continue;
+ if (nav1->operator ==(*nav2))
+ continue;
+
+ //vector<SubRoom*> emptyVector;
+ //emptyVector.push_back(sub.get());
+ //add all subrooms at the same elevation
+ //double elevation = sub->GetMaxElevation();
+
+ double elevation = sub->GetElevation(sub->GetCentroid());
+ // special case for stairs and for convex rooms
+ //if()
+
+ if (building->IsVisible(nav1->GetCentre(), nav2->GetCentre(), _subroomsAtElevation[elevation],true))
+ {
+ int to_door = _map_id_to_index[nav2->GetUniqueID()];
+ _distMatrix[from_door][to_door] = penalty*(nav1->GetCentre()
+ - nav2->GetCentre()).Norm();
+ from_AP->AddConnectingAP(
+ _accessPoints[nav2->GetUniqueID()]);
+ }
+ }
+ }
+ }
+ }
+
+ //complete the matrix with the final distances between the exits to the outside and the
+ //final marked goals
+
+ for (int final_dest:_finalDestinations)
+ {
+ Goal* goal =_building->GetFinalGoal(final_dest);
+ const Wall& line=_building->GetFinalGoal(final_dest)->GetAllWalls()[0];
+ double center[2] = { goal->GetCentroid()._x, goal->GetCentroid()._y };
+
+ AccessPoint* to_AP = new AccessPoint(line.GetUniqueID(), center);
+ to_AP->SetFinalGoalOutside(true);
+ //NavLine* tmpline=new NavLine(line);
+ NavLine tmpline(line);
+ to_AP->SetNavLine(&tmpline);
+ //delete tmpline;
+
+ char friendlyName[CLENGTH];
+ sprintf(friendlyName, "finalGoal_%d_located_outside", goal->GetId());
+ to_AP->SetFriendlyName(friendlyName);
+ to_AP->AddFinalDestination(FINAL_DEST_OUT,0.0);
+ to_AP->AddFinalDestination(goal->GetId(),0.0);
+ _accessPoints[to_AP->GetID()] = to_AP;
+
+ //very nasty
+ _map_id_to_index[to_AP->GetID()] = index;
+ _map_index_to_id[index] = to_AP->GetID();
+ index++;
+
+ //only make a connection to final exit to outside
+ for(const auto & itr1: _accessPoints)
+ {
+ AccessPoint* from_AP = itr1.second;
+ if(!from_AP->GetFinalExitToOutside()) continue;
+ if(from_AP->GetID()==to_AP->GetID()) continue;
+ from_AP->AddConnectingAP(to_AP);
+ int from_door= _map_id_to_index[from_AP->GetID()];
+ int to_door= _map_id_to_index[to_AP->GetID()];
+ // I assume a direct line connection between every exit connected to the outside and
+ // any final goal also located outside
+ _distMatrix[from_door][to_door] = _edgeCost*from_AP->GetNavLine()->DistTo(goal->GetCentroid());
+
+ // add a penalty for goals outside due to the direct line assumption while computing the distances
+ if (_distMatrix[from_door][to_door] > 10.0)
+ _distMatrix[from_door][to_door]*=100;
+ }
+ }
+
+ //run the floyd warshall algorithm
+ FloydWarshall();
+
+ // set the configuration for reaching the outside
+ // set the distances to all final APs
+
+ for(const auto & itr: _accessPoints)
+ {
+ AccessPoint* from_AP = itr.second;
+ int from_door = _map_id_to_index[itr.first];
+ if(from_AP->GetFinalGoalOutside()) continue;
+
+ //maybe put the distance to FLT_MAX
+ if(from_AP->IsClosed()) continue;
+
+ double tmpMinDist = FLT_MAX;
+ int tmpFinalGlobalNearestID = from_door;
+
+ for(const auto & itr1: _accessPoints)
+ {
+ AccessPoint* to_AP = itr1.second;
+ if(from_AP->GetID()==to_AP->GetID()) continue;
+ if(from_AP->GetFinalExitToOutside()) continue;
+ //if(from_AP->GetFinalGoalOutside()) continue;
+
+ if (to_AP->GetFinalExitToOutside())
+ {
+ int to_door = _map_id_to_index[itr1.first];
+ if (from_door == to_door)
+ continue;
+
+ //cout <<" checking final destination: "<< pAccessPoints[j]->GetID()<<endl;
+ double dist = _distMatrix[from_door][to_door];
+ if (dist < tmpMinDist) {
+ tmpFinalGlobalNearestID = to_door;
+ tmpMinDist = dist;
+ }
+ }
+ }
+
+ // in the case it is the final APs
+ if (tmpFinalGlobalNearestID == from_door)
+ tmpMinDist = 0.0;
+
+ if (tmpMinDist == FLT_MAX) {
+ Log->Write(
+ "ERROR: \tGlobalRouter: There is no visibility path from [%s] to the outside 1\n"
+ " You can solve this by enabling triangulation.",
+ from_AP->GetFriendlyName().c_str());
+ from_AP->Dump();
+ return false;
+ }
+
+ // set the distance to the final destination ( OUT )
+ from_AP->AddFinalDestination(FINAL_DEST_OUT, tmpMinDist);
+
+ // set the intermediate path to global final destination
+ GetPath(from_door, tmpFinalGlobalNearestID);
+
+ if (_tmpPedPath.size() >= 2) {
+ from_AP->AddTransitAPsTo(FINAL_DEST_OUT,
+ _accessPoints[_map_index_to_id[_tmpPedPath[1]]]);
+ } else {
+ if ((!from_AP->GetFinalExitToOutside())
+ && (!from_AP->IsClosed()))
+ {
+ Log->Write(
+ "ERROR: \tGlobalRouter: There is no visibility path from [%s] to the outside 2\n"
+ " \tYou can solve this by enabling triangulation.",
+ from_AP->GetFriendlyName().c_str());
+ from_AP->Dump();
+ return false;
+ }
+ }
+ _tmpPedPath.clear();
+ }
+
+ // set the configuration to reach the goals specified in the ini file
+ // set the distances to alternative destinations
+
+ for (unsigned int p = 0; p < _finalDestinations.size(); p++) {
+ int to_door_uid =
+ _building->GetFinalGoal(_finalDestinations[p])->GetAllWalls()[0].GetUniqueID();
+ int to_door_matrix_index=_map_id_to_index[to_door_uid];
+
+ // thats probably a goal located outside the geometry or not an exit from the geometry
+ if(to_door_uid==-1) {
+ Log->Write(
+ "ERROR: \tGlobalRouter: there is something wrong with the final destination [ %d ]\n",
+ _finalDestinations[p]);
+ return false;
+ }
+
+ for(const auto & itr:_accessPoints)
+ {
+ AccessPoint* from_AP = itr.second;
+ if(from_AP->GetFinalGoalOutside()) continue;
+ if(from_AP->IsClosed()) continue;
+ int from_door_matrix_index = _map_id_to_index[itr.first];
+
+ //comment this if you want infinite as distance to unreachable destinations
+ double dist = _distMatrix[from_door_matrix_index][to_door_matrix_index];
+ from_AP->AddFinalDestination(_finalDestinations[p], dist);
+
+ // set the intermediate path
+ // set the intermediate path to global final destination
+ GetPath(from_door_matrix_index, to_door_matrix_index);
+ if (_tmpPedPath.size() >= 2) {
+ from_AP->AddTransitAPsTo(_finalDestinations[p],
+ _accessPoints[_map_index_to_id[_tmpPedPath[1]]]);
+ } else {
+ if (((!from_AP->IsClosed()))) {
+ Log->Write(
+ "ERROR: \tGlobalRouter: There is no visibility path from [%s] to goal [%d]\n"
+ " You can solve this by enabling triangulation.",
+ from_AP->GetFriendlyName().c_str(), _finalDestinations[p]);
+ from_AP->Dump();
+ return false;
+ }
+ }
+ _tmpPedPath.clear();
+ }
+ }
+
+ //dumping the complete system
+ //DumpAccessPoints(15); exit(0);
+ //DumpAccessPoints(1259);
+ //DumpAccessPoints(4912); //exit(0);
+ //DumpAccessPoints(-1); exit(0);
+ //vector<string> rooms;
+ //rooms.push_back("Verteilerebene");
+ //WriteGraphGV("routing_graph.gv",FINAL_DEST_OUT,rooms); exit(0);
+ //WriteGraphGV("routing_graph.gv",4,rooms);exit(0);
+ Log->Write("INFO:\tDone with the Global Router Engine!");
+ return true;
+}
+
+void GlobalRouterTrips::Reset(){
+ //clean all allocated spaces
+ if (_distMatrix && _pathsMatrix) {
+ const int exitsCnt = _building->GetNumberOfGoals();
+ for (int p = 0; p < exitsCnt; ++p) {
+ delete[] _distMatrix[p];
+ delete[] _pathsMatrix[p];
+ }
+
+ delete[] _distMatrix;
+ delete[] _pathsMatrix;
+ }
+
+ for (auto itr = _accessPoints.begin(); itr != _accessPoints.end(); ++itr) {
+ delete itr->second;
+ }
+
+ _accessPoints.clear();
+ _tmpPedPath.clear();
+ _map_id_to_index.clear();
+ _map_index_to_id.clear();
+ _mapIdToFinalDestination.clear();
+}
+
+
+void GlobalRouterTrips::SetEdgeCost(double cost)
+{
+ _edgeCost=cost;
+}
+
+double GlobalRouterTrips::GetEdgeCost() const
+{
+ return _edgeCost;
+}
+
+void GlobalRouterTrips::GetPath(int i, int j)
+{
+ if (_distMatrix[i][j] == FLT_MAX)
+ return;
+ if (i != j)
+ GetPath(i, _pathsMatrix[i][j]);
+ _tmpPedPath.push_back(j);
+}
+
+bool GlobalRouterTrips::GetPath(Pedestrian* ped, std::vector<NavLine*>& path)
+{
+ std::vector<AccessPoint*> aps_path;
+
+ bool done=false;
+ int currentNavLine = ped->GetNextDestination();
+ if (currentNavLine == -1)
+ {
+ currentNavLine= GetBestDefaultRandomExit(ped);
+ }
+ aps_path.push_back(_accessPoints[currentNavLine]);
+
+ int loop_count=1;
+ do
+ {
+ const auto & ap=aps_path.back();
+ int next_dest = ap->GetNearestTransitAPTO(ped->GetFinalDestination());
+
+ if(next_dest==-1) break; //we are done
+
+ auto & next_ap= _accessPoints[next_dest];
+
+ if(next_ap->GetFinalExitToOutside())
+ {
+ done =true;
+ }
+
+ if (! IsElementInVector(aps_path,next_ap))
+ {
+ aps_path.push_back(next_ap);
+ }
+ else
+ {
+ Log->Write("WARNING:\t the line [%d] is already included in the path.");
+ }
+
+ //work arround to detect a potential infinte loop.
+ if(loop_count++>1000)
+ {
+ Log->Write("ERROR:\t A path could not be found for pedestrian [%d] going to destination [%d]",ped->GetID(),ped->GetFinalDestination());
+ Log->Write(" \t Stuck in an infinite loop [%d].",loop_count);
+ return false;
+ }
+
+
+ } while (!done);
+
+
+ for(const auto & aps: aps_path)
+ path.push_back(aps->GetNavLine());
+
+ return true;
+
+}
+
+bool GlobalRouterTrips::GetPath(Pedestrian*ped, int goalID, std::vector<SubRoom*>& path)
+{
+
+ //clear the global variable holding the paths
+ _tmpPedPath.clear();
+
+ int tmpFinalDest=ped->GetFinalDestination();
+ ped->SetFinalDestination(goalID);
+
+ //find the nearest APs and start from there
+ int next = GetBestDefaultRandomExit(ped);
+ if(next==-1) {
+ Log->Write("ERROR:\t there is an error in getting the path for ped %d to the goal %d", ped->GetID(),goalID);
+ return false;
+ }
+
+ // get the transformed goal_id
+ int to_door_uid =
+ _building->GetFinalGoal(goalID)->GetAllWalls()[0].GetUniqueID();
+ int to_door_matrix_index=_map_id_to_index[to_door_uid];
+ int from_door_matrix_index=_map_id_to_index[next];
+
+ // thats probably a goal located outside the geometry or not an exit from the geometry
+ if(to_door_uid==-1) {
+ Log->Write("ERROR: \tGlobalRouter: there is something wrong with final destination [ %d ]\n",goalID);
+ return false;
+ }
+
+ //populate the line unique id to cross
+ GetPath(from_door_matrix_index,to_door_matrix_index);
+
+ for(unsigned int i=0; i<_tmpPedPath.size(); i++) {
+ int ap_id= _map_index_to_id[_tmpPedPath[i]];
+ int subroom_uid=_accessPoints[ap_id]->GetConnectingRoom1();
+ if(subroom_uid==-1) continue;
+ SubRoom* sub = _building->GetSubRoomByUID(subroom_uid);
+ if (sub && !IsElementInVector(path, sub)) path.push_back(sub);
+ }
+
+ for(unsigned int i=0; i<_tmpPedPath.size(); i++) {
+ int ap_id= _map_index_to_id[_tmpPedPath[i]];
+ int subroom_uid=_accessPoints[ap_id]->GetConnectingRoom2();
+ if(subroom_uid==-1) continue;
+ SubRoom* sub = _building->GetSubRoomByUID(subroom_uid);
+ if (sub && !IsElementInVector(path, sub)) path.push_back(sub);
+ }
+
+ //clear the global variable holding the paths
+ _tmpPedPath.clear();
+
+ ped->SetFinalDestination(tmpFinalDest);
+ return true;
+ //double distance = _accessPoints[next]->GetDistanceTo(0)+ped->GetDistanceToNextTarget();
+ //cout<<"shortest distance to outside: " <<distance<<endl;
+}
+
+/*
+ floyd_warshall()
+ after calling this function dist[i][j] will the the minimum distance
+ between i and j if it exists (i.e. if there's a path between i and j)
+ or 0, otherwise
+ */
+void GlobalRouterTrips::FloydWarshall()
+{
+ const int n = (const int) (_building->GetNumberOfGoals() + _building->GetAllGoals().size());
+ for (int k = 0; k < n; k++)
+ for (int i = 0; i < n; i++)
+ for (int j = 0; j < n; j++)
+ if (_distMatrix[i][k] + _distMatrix[k][j] < _distMatrix[i][j]) {
+ _distMatrix[i][j] = _distMatrix[i][k] + _distMatrix[k][j];
+ _pathsMatrix[i][j] = _pathsMatrix[k][j];
+ }
+}
+
+//void GlobalRouter::DumpAccessPoints(int p)
+//{
+// if (p != -1) {
+// _accessPoints.at(p)->Dump();
+// } else {
+// for (const auto & itr: _accessPoints)
+// {
+// itr.second->Dump();
+// }
+// }
+//}
+
+int GlobalRouterTrips::FindExit(Pedestrian* ped)
+{
+ if(!_useMeshForLocalNavigation)
+ {
+ std::vector<NavLine*> path;
+ GetPath(ped,path);
+ SubRoom* sub = _building->GetRoom(ped->GetRoomID())->GetSubRoom(
+ ped->GetSubRoomID());
+
+ //return the next path which is an exit
+ for(const auto & navLine: path)
+ {
+ //TODO: only set if the pedestrian is already in the subroom.
+ // cuz all lines are returned
+ if(IsCrossing(*navLine,{sub}) || IsTransition(*navLine,{sub}))
+ {
+ int nav_id= navLine->GetUniqueID();
+ ped->SetExitIndex(nav_id);
+ ped->SetExitLine(navLine);
+ return nav_id;
+ }
+ }
+
+ //something bad happens
+ Log->Write(
+ "ERROR:\t Cannot find a valid destination for ped [%d] located in room [%d] subroom [%d] going to destination [%d]",
+ ped->GetID(), ped->GetRoomID(), ped->GetSubRoomID(),
+ ped->GetFinalDestination());
+ return -1;
+
+ }
+ // else proceed as usual and return the closest navigation line
+ int nextDestination = ped->GetNextDestination();
+
+ if (nextDestination == -1)
+ {
+ return GetBestDefaultRandomExit(ped);
+
+ } else
+ {
+ SubRoom* sub = _building->GetRoom(ped->GetRoomID())->GetSubRoom(
+ ped->GetSubRoomID());
+
+ for(const auto & apID:sub->GetAllGoalIDs())
+ {
+ AccessPoint* ap = _accessPoints[apID];
+ const Point& pt3 = ped->GetPos();
+ double distToExit = ap->GetNavLine()->DistTo(pt3);
+
+ if (distToExit > J_EPS_DIST)
+ continue;
+
+ //continue until my target is reached
+ if(apID!=ped->GetExitIndex())
+ continue;
+
+ //one AP is near actualize destination:
+ nextDestination = ap->GetNearestTransitAPTO(
+ ped->GetFinalDestination());
+
+ //if(ped->GetID()==6) {ap->Dump();getc(stdin);}
+ if (nextDestination == -1)
+ {
+ // we are almost at the exit
+ // so keep the last destination
+ return ped->GetNextDestination();
+ } else {
+ //check that the next destination is in the actual room of the pedestrian
+ if (!_accessPoints[nextDestination]->isInRange(
+ sub->GetUID())) {
+ //return the last destination if defined
+ int previousDestination = ped->GetNextDestination();
+
+ //we are still somewhere in the initialization phase
+ if (previousDestination == -1) {
+ ped->SetExitIndex(apID);
+ ped->SetExitLine(_accessPoints[apID]->GetNavLine());
+ return apID;
+ } else { // we are still having a valid destination, don't change
+ return previousDestination;
+ }
+ } else { // we have reached the new room
+ ped->SetExitIndex(nextDestination);
+ ped->SetExitLine(
+ _accessPoints[nextDestination]->GetNavLine());
+ return nextDestination;
+ }
+ }
+ }
+
+ // still have a valid destination, so return it
+ return nextDestination;
+ }
+}
+
+int GlobalRouterTrips::GetBestDefaultRandomExit(Pedestrian* ped)
+{
+ // get the relevant opened exits
+ vector <AccessPoint*> relevantAPs;
+ GetRelevantRoutesTofinalDestination(ped,relevantAPs);
+ //in the case there is only one alternative
+ //save some computation
+ if(relevantAPs.size()==1)
+ {
+ auto&& ap= (AccessPoint*&&) relevantAPs[0];
+ ped->SetExitIndex(ap->GetID());
+ ped->SetExitLine(ap->GetNavLine());
+ return ap->GetID();
+ }
+
+ int bestAPsID = -1;
+ double minDistGlobal = FLT_MAX;
+ //double minDistLocal = FLT_MAX;
+
+ // get the opened exits
+ SubRoom* sub = _building->GetRoom(ped->GetRoomID())->GetSubRoom(
+ ped->GetSubRoomID());
+
+ for(unsigned int g=0; g<relevantAPs.size(); g++)
+ {
+ AccessPoint* ap=relevantAPs[g];
+
+ if (!ap->isInRange(sub->GetUID()))
+ continue;
+ //check if that exit is open.
+ if (ap->IsClosed())
+ continue;
+
+ //the line from the current position to the centre of the nav line.
+ // at least the line in that direction minus EPS
+ const Point& posA = ped->GetPos();
+ const Point& posB = ap->GetNavLine()->GetCentre();
+ const Point& posC = (posB - posA).Normalized() * ((posA - posB).Norm() - J_EPS) + posA;
+
+ //check if visible
+ //only if the room is convex
+ //otherwise check all rooms at that level
+ if(!_building->IsVisible(posA, posC, _subroomsAtElevation[sub->GetElevation(sub->GetCentroid())],true))
+ {
+ ped->RerouteIn(10);
+ continue;
+ }
+ double dist1 = ap->GetDistanceTo(ped->GetFinalDestination());
+ double dist2 = ap->DistanceTo(posA._x, posA._y);
+ double dist=dist1+dist2;
+
+ if (dist < minDistGlobal)
+ {
+ bestAPsID = ap->GetID();
+ minDistGlobal = dist;
+ //minDistLocal=dist2;
+ }
+ }
+
+ if (bestAPsID != -1)
+ {
+ ped->SetExitIndex(bestAPsID);
+ ped->SetExitLine(_accessPoints[bestAPsID]->GetNavLine());
+ return bestAPsID;
+ }
+ else
+ {
+ if (_building->GetRoom(ped->GetRoomID())->GetCaption() != "outside")
+ {
+ //Log->Write(
+ //
+ // "ERROR:\t GetBestDefaultRandomExit() \nCannot find valid destination for ped [%d] "
+ // "located in room [%d] subroom [%d] going to destination [%d]",
+ // ped->GetID(), ped->GetRoomID(), ped->GetSubRoomID(),
+ // ped->GetFinalDestination());
+
+
+ //FIXME: assign the nearest and not only a random one
+ //{
+
+ relevantAPs[0]->GetID();
+ ped->SetExitIndex(relevantAPs[0]->GetID());
+ ped->SetExitLine(relevantAPs[0]->GetNavLine());
+ ped->RerouteIn(5);
+ return relevantAPs[0]->GetID();
+ }
+ return -1;
+ }
+}
+
+
+void GlobalRouterTrips::GetRelevantRoutesTofinalDestination(Pedestrian *ped, vector<AccessPoint*>& relevantAPS)
+{
+
+ Room* room=_building->GetRoom(ped->GetRoomID());
+ SubRoom* sub=room->GetSubRoom(ped->GetSubRoomID());
+
+ // This is best implemented by closing one door and checking if there is still a path to outside
+ // and itereating over the others.
+ // It might be time consuming, you many pre compute and cache the results.
+ if(sub->GetAllHlines().size()==0)
+ {
+ const vector<int>& goals=sub->GetAllGoalIDs();
+ //filter to keep only the emergencies exits.
+
+ for(unsigned int g1=0; g1<goals.size(); g1++) {
+ AccessPoint* ap=_accessPoints[goals[g1]];
+ bool relevant=true;
+ for(unsigned int g2=0; g2<goals.size(); g2++) {
+ if(goals[g2]==goals[g1]) continue; // always skip myself
+ if(ap->GetNearestTransitAPTO(ped->GetFinalDestination())==goals[g2]) {
+ // crossings only
+ relevant=false;
+ break;
+ }
+ }
+ if(relevant) {
+ //only if not closed
+ if(ap->IsClosed()==0)
+ relevantAPS.push_back(ap);
+ }
+ }
+ }
+ //quick fix for extra hlines
+ // it should be safe now to delete the first preceding if block
+ else
+ {
+ const vector<int>& goals=sub->GetAllGoalIDs();
+ for(unsigned int g1=0; g1<goals.size(); g1++)
+ {
+ AccessPoint* ap=_accessPoints[goals[g1]];
+
+ //check for visibility
+ //the line from the current position to the centre of the nav line.
+ // at least the line in that direction minus EPS
+ const Point& posA = ped->GetPos();
+ const Point& posB = ap->GetNavLine()->GetCentre();
+ const Point& posC = (posB - posA).Normalized() * ((posA - posB).Norm() - J_EPS) + posA;
+
+ //check if visible
+ if (!_building->IsVisible(posA, posC, _subroomsAtElevation[sub->GetElevation(sub->GetCentroid())],true))
+ //if (sub->IsVisible(posA, posC, true) == false)
+ {
+ continue;
+ }
+
+ bool relevant=true;
+ for(unsigned int g2=0; g2<goals.size(); g2++)
+ {
+ if(goals[g2]==goals[g1]) continue; // always skip myself
+ if(ap->GetNearestTransitAPTO(ped->GetFinalDestination())==goals[g2])
+ {
+
+ //pointing only to the one i dont see
+ //the line from the current position to the centre of the nav line.
+ // at least the line in that direction minus EPS
+ AccessPoint* ap2=_accessPoints[goals[g2]];
+ const Point& posA_ = ped->GetPos();
+ const Point& posB_ = ap2->GetNavLine()->GetCentre();
+ const Point& posC_ = (posB_ - posA_).Normalized()* ((posA_ - posB_).Norm() - J_EPS) + posA_;
+
+ //it points to a destination that I can see anyway
+ if (_building->IsVisible(posA_, posC_, _subroomsAtElevation[sub->GetElevation(sub->GetCentroid())],true))
+ //if (sub->IsVisible(posA_, posC_, true) == true)
+ {
+ relevant=false;
+ }
+
+ break;
+ }
+ }
+ if(relevant)
+ {
+ if(ap->IsClosed()==0)
+ relevantAPS.push_back(ap);
+ }
+ }
+ }
+
+ //fallback
+ if(relevantAPS.size()==0)
+ {
+ //fixme: this should also never happened. But happen due to previous bugs..
+ const vector<int>& goals=sub->GetAllGoalIDs();
+ for(unsigned int g1=0; g1<goals.size(); g1++)
+ {
+ relevantAPS.push_back(_accessPoints[goals[g1]]);
+ }
+
+ }
+
+}
+
+//void GlobalRouter::WriteGraphGV(string filename, int finalDestination,
+// const vector<string> rooms_captions)
+//{
+// ofstream graph_file(filename.c_str());
+// if (graph_file.is_open() == false) {
+// Log->Write("Unable to open file" + filename);
+// return;
+// }
+//
+// //header
+// graph_file << "## Produced by JuPedSim" << endl;
+// //graph_file << "##comand: \" sfdp -Goverlap=prism -Gcharset=latin1"<<filename <<"| gvmap -e | neato -Ecolor=\"#55555522\" -n2 -Tpng > "<< filename<<".png \""<<endl;
+// graph_file << "##Command to produce the output: \"neato -n -s -Tpng "
+// << filename << " > " << filename << ".png\"" << endl;
+// graph_file << "digraph JUPEDSIM_ROUTING {" << endl;
+// graph_file << "overlap=scale;" << endl;
+// graph_file << "splines=false;" << endl;
+// graph_file << "fontsize=20;" << endl;
+// graph_file
+// << "label=\"Graph generated by the routing engine for destination: "
+// << finalDestination << "\"" << endl;
+//
+// vector<int> rooms_ids = vector<int>();
+//
+// if (rooms_captions.empty())
+// {
+// // then all rooms should be printed
+// for(auto && itroom:_building->GetAllRooms())
+// {
+// for(const auto & it_sub:itroom.second->GetAllSubRooms())
+// {
+// rooms_ids.push_back(it_sub.second->GetUID());
+// }
+// }
+//
+//
+// } else {
+// for (auto && caption: rooms_captions)
+// {
+// for(const auto & it_sub:_building->GetRoom(caption)->GetAllSubRooms())
+// {
+// rooms_ids.push_back(it_sub.second->GetUID());
+// }
+// }
+// }
+//
+// for (map<int, AccessPoint*>::const_iterator itr = _accessPoints.begin();
+// itr != _accessPoints.end(); ++itr) {
+//
+// AccessPoint* from_AP = itr->second;
+//
+// int from_door = from_AP->GetID();
+//
+// // check for valid room
+// int room_id = from_AP->GetConnectingRoom1();
+// int room_id1=from_AP->GetConnectingRoom2();
+//
+// if ( (IsElementInVector(rooms_ids, room_id) == false) && (IsElementInVector(rooms_ids, room_id1) == false) )
+// continue;
+// double px = from_AP->GetCentre()._x;
+// double py = from_AP->GetCentre()._y;
+// //graph_file << from_door <<" [shape=ellipse, pos=\""<<px<<", "<<py<<" \"] ;"<<endl;
+// //graph_file << from_door <<" [shape=ellipse, pos=\""<<px<<","<<py<<"\" ];"<<endl;
+//
+// //const vector<AccessPoint*>& from_aps = from_AP->GetConnectingAPs();
+// const vector<AccessPoint*>& from_aps = from_AP->GetTransitAPsTo(
+// finalDestination);
+//
+// if (from_aps.size() == 0) {
+//
+// if (from_AP->GetFinalExitToOutside()) {
+// graph_file << from_door << " [pos=\"" << px << ", " << py
+// << " \", style=filled, color=green,fontsize=5] ;"
+// << endl;
+// // graph_file << from_door <<" [width=\"0.41\", height=\"0.31\",fixedsize=false,pos=\""<<px<<", "<<py<<" \", style=filled, color=green,fontsize=4] ;"<<endl;
+// } else {
+// graph_file << from_door << " [pos=\"" << px << ", " << py
+// << " \", style=filled, color=red,fontsize=5] ;" << endl;
+// // graph_file << from_door <<" [width=\"0.41\", height=\"0.31\",fixedsize=false,pos=\""<<px<<", "<<py<<" \", style=filled, color=red,fontsize=4] ;"<<endl;
+// }
+// } else {
+// // check that all connecting aps are contained in the room_ids list
+// // if not marked as sink.
+// bool isSink = true;
+// for (unsigned int j = 0; j < from_aps.size(); j++)
+// {
+// int room_id_ = from_aps[j]->GetConnectingRoom1();
+// if (IsElementInVector(rooms_ids, room_id_))
+// {
+// isSink = false;
+// break;
+// }
+// }
+//
+// if (isSink) {
+// //graph_file << from_door <<" [width=\"0.3\", height=\"0.21\",fixedsize=false,pos=\""<<px<<", "<<py<<" \" ,style=filled, color=green, fontsize=4] ;"<<endl;
+// graph_file << from_door << " [pos=\"" << px << ", " << py
+// << " \" ,style=filled, color=blue, fontsize=5] ;"
+// << endl;
+// } else {
+// //graph_file << from_door <<" [width=\"0.3\", height=\"0.231\",fixedsize=false, pos=\""<<px<<", "<<py<<" \", fontsize=4] ;"<<endl;
+// graph_file << from_door << " [pos=\"" << px << ", " << py
+// << " \", style=filled, color=yellow, fontsize=5] ;"
+// << endl;
+// }
+// }
+//
+// }
+//
+// //connections
+// for (const auto & itr: _accessPoints)
+// {
+// AccessPoint* from_AP = itr.second;
+// int from_door = from_AP->GetID();
+//
+// int room_id = from_AP->GetConnectingRoom1();
+//
+// if (IsElementInVector(rooms_ids, room_id) == false)
+// continue;
+//
+// //const vector<AccessPoint*>& aps = from_AP->GetConnectingAPs();
+// const vector<AccessPoint*>& aps = from_AP->GetTransitAPsTo(
+// finalDestination);
+//
+// for (const auto & to_AP:aps)
+// {
+// int to_door = to_AP->GetID();
+//
+// int lroom_id = to_AP->GetConnectingRoom1();
+//
+// if (IsElementInVector(rooms_ids, lroom_id) == false)
+// continue;
+//
+// graph_file << from_door << " -> " << to_door << " [ label="
+// << from_AP->GetDistanceTo(to_AP)
+// + to_AP->GetDistanceTo(finalDestination)
+// << ", fontsize=10]; " << endl;
+// }
+// }
+//
+// //graph_file << "node [shape=box]; gy2; yr2; rg2; gy1; yr1; rg1;"<<endl;
+// //graph_file << "node [shape=circle,fixedsize=true,width=0.9]; green2; yellow2; red2; safe2; safe1; green1; yellow1; red1;"<<endl;
+//
+// //graph_file << "0 -> 1 ;"<<endl;
+//
+// graph_file << "}" << endl;
+//
+// //done
+// graph_file.close();
+//}
+
+void GlobalRouterTrips::TriangulateGeometry()
+{
+ Log->Write("INFO:\tUsing the triangulation in the global router");
+ for(auto&& itr_room: _building->GetAllRooms())
+ {
+ for(auto&& itr_subroom: itr_room.second->GetAllSubRooms())
+ {
+ auto&& subroom= (shared_ptr<SubRoom>&&) itr_subroom.second;
+ auto&& room= (shared_ptr<Room>&&) itr_room.second;
+ auto&& obstacles= (const vector<Obstacle*>&&) subroom->GetAllObstacles();
+ if(!subroom->IsAccessible()) continue;
+
+ //Triangulate if obstacle or concave and no hlines ?
+ //if(subroom->GetAllHlines().size()==0)
+ if((obstacles.size()>0 ) || !subroom->IsConvex())
+ {
+
+ // DTriangulation* tri= new DTriangulation();
+ // auto outerhull=subroom->GetPolygon();
+ // if(subroom->IsClockwise())
+ // std::reverse(outerhull.begin(), outerhull.end());
+ //
+ // tri->SetOuterPolygone(outerhull);
+ //
+ // for (const auto & obst: obstacles)
+ // {
+ // auto outerhullObst=obst->GetPolygon();
+ // if(obst->IsClockwise())
+ // std::reverse(outerhullObst.begin(), outerhullObst.end());
+ // tri->AddHole(outerhullObst);
+ // }
+ // tri->Triangulate();
+ // vector<p2t::Triangle*> triangles=tri->GetTriangles();
+
+ vector<p2t::Triangle*> triangles=subroom->GetTriangles();
+
+ for (const auto & tr: triangles)
+ {
+ Point P0 = Point (tr->GetPoint(0)->x,tr->GetPoint(0)->y);
+ Point P1 = Point (tr->GetPoint(1)->x,tr->GetPoint(1)->y);
+ Point P2 = Point (tr->GetPoint(2)->x,tr->GetPoint(2)->y);
+ vector<Line> edges;
+ edges.push_back(Line(P0,P1));
+ edges.push_back(Line(P1,P2));
+ edges.push_back(Line(P2,P0));
+
+ for (const auto & line: edges)
+ {
+ //reduce edge that are too close 50 cm is assumed
+ if (MinDistanceToHlines(line.GetCentre(),
+ *subroom)
+ < _minDistanceBetweenTriangleEdges)
+ continue;
+
+ if (MinAngle(P0, P1, P2) < _minAngleInTriangles)
+ continue;
+
+ if((IsWall(line,{subroom.get()})==false) && (IsCrossing(line,{subroom.get()})==false)
+ && (IsTransition(line,{subroom.get()})==false) && (IsHline(line,{subroom.get()})==false))
+ {
+ //add as a Hline
+ int id=_building->GetAllHlines().size();
+ Hline* h = new Hline();
+ h->SetID(id);
+ h->SetPoint1(line.GetPoint1());
+ h->SetPoint2(line.GetPoint2());
+ h->SetRoom1(room.get());
+ h->SetSubRoom1(subroom.get());
+ subroom->AddHline(h);
+ Log->Write(std::to_string(h->GetPoint1()._x)+""+std::to_string(h->GetPoint1()._y));
+ Log->Write(std::to_string(h->GetPoint2()._x)+""+std::to_string(h->GetPoint2()._y));
+ _building->AddHline(h);
+ }
+ }
+ }
+ }
+ }
+ }
+ Log->Write("INFO:\tDone...");
+}
+
+bool GlobalRouterTrips::GenerateNavigationMesh()
+{
+ // //Navigation mesh implementation
+ // NavMesh* nv= new NavMesh(_building);
+ // nv->BuildNavMesh();
+ // _building->SaveGeometry("test_geometry.xml");
+ // exit(0);
+ // //nv->WriteToFileTraVisTo()
+ //
+ // const std::vector<NavMesh::JEdge*>& edges = nv->GetEdges();
+ //
+ // for(const auto & edge: edges)
+ // {
+ // //construct and add a new navigation line if non existing
+ // Line line(edge->pStart.pPos,edge->pEnd.pPos);
+ // bool isEdge=false;
+ //
+ // //check if it is already a crossing
+ // const map<int, Crossing*>& crossings = _building->GetAllCrossings();
+ // for (const auto & crossing: crossings)
+ // {
+ // Crossing* cross=crossing.second;
+ // if(line.operator ==(*cross))
+ // {
+ // isEdge=true;
+ // break;
+ // }
+ // }
+ // if(isEdge) continue;
+ //
+ //
+ // //check if it is already a transition
+ // const map<int, Transition*>& transitions = _building->GetAllTransitions();
+ // for (const auto & transition: transitions)
+ // {
+ // Transition* trans=transition.second;
+ // if(line.operator ==(*trans))
+ // {
+ // isEdge=true;
+ // break;
+ // }
+ // }
+ // if(isEdge) continue;
+ //
+ // //check if it is already a
+ // const map<int, Hline*>& hlines = _building->GetAllHlines();
+ // for (const auto & hline: hlines)
+ // {
+ // Hline* navLine=hline.second;
+ // if(line.operator ==(*navLine))
+ // {
+ // isEdge=true;
+ // break;
+ // }
+ // }
+ // if(isEdge) continue;
+ //
+ //
+ // Hline* h = new Hline();
+ // h->SetID(hlines.size());
+ // int assigned=0;
+ //
+ // //look for the room/subroom containing the new edge
+ // const vector<Room*>& rooms=_building->GetAllRooms();
+ // for(const auto & room: rooms)
+ // {
+ // const vector<SubRoom*>& subrooms= room->GetAllSubRooms();
+ //
+ // for(const auto & subroom: subrooms)
+ // {
+ // if(subroom->IsInSubRoom(line.GetCentre()))
+ // {
+ // h->SetRoom1(room);
+ // h->SetSubRoom1(subroom);
+ // assigned++;
+ // }
+ // }
+ // }
+ //
+ // if(assigned!=1)
+ // {
+ // Log->Write("WARNING:\t a navigation line from the mesh was not correctly assigned");
+ // return false;
+ // }
+ // //add the new edge as navigation line
+ //
+ // h->SetPoint1(edge->pStart.pPos);
+ // h->SetPoint2(edge->pEnd.pPos);
+ // h->GetSubRoom1()->AddHline(h); //double linked ??
+ // _building->AddHline(h);
+ //
+ // }
+ //
+ // //string geometry;
+ // //nv->WriteToString(geometry);
+ // //Write("<geometry>");
+ // //Write(geometry);
+ // //Write("</geometry>");
+ // //nv->WriteToFile(building->GetProjectFilename()+".full.nav");
+ //
+ // //cout<<"bye"<<endl;
+ // delete nv;
+ return true;
+}
+
+string GlobalRouterTrips::GetRoutingInfoFile()
+{
+
+ TiXmlDocument doc(_building->GetProjectFilename());
+ if (!doc.LoadFile()) {
+ Log->Write("ERROR: \t%s", doc.ErrorDesc());
+ Log->Write("ERROR: \t GlobalRouter: could not parse the project file");
+ return "";
+ }
+
+ // everything is fine. proceed with parsing
+ TiXmlElement* xMainNode = doc.RootElement();
+ TiXmlNode* xRouters=xMainNode->FirstChild("route_choice_models");
+ string nav_line_file="";
+
+ for(TiXmlElement* e = xRouters->FirstChildElement("router"); e;
+ e = e->NextSiblingElement("router"))
+ {
+
+ string strategy=e->Attribute("description");
+ vector<string> routers={"local_shortest", "global_shortest", "global_safest","dynamic","quickest"};
+
+ if(std::find(routers.begin(), routers.end(), strategy) != routers.end())
+ {
+
+ if(e->FirstChild("parameters"))
+ {
+
+ if (e->FirstChild("parameters")->FirstChildElement("navigation_lines")) //fixme:
+ //this
+ //reads
+ //the
+ //wronf
+ //router section
+ nav_line_file=e->FirstChild("parameters")->FirstChildElement("navigation_lines")->Attribute("file");
+
+ TiXmlElement* para =e->FirstChild("parameters")->FirstChildElement("navigation_mesh");
+ if (para)
+ {
+ //triangulate the geometry
+ if(!_building->Triangulate())
+ {
+ Log->Write("ERROR:\t could not triangulate the geometry!");
+ exit (EXIT_FAILURE);
+ }
+
+ string local_planing=xmltoa(para->Attribute("use_for_local_planning"),"false");
+ if(local_planing=="true") {
+ _useMeshForLocalNavigation = 1;
+ }
+ else {
+ _useMeshForLocalNavigation = 0;
+ }
+
+ string method = xmltoa(para->Attribute("method"),"");
+ if(method=="triangulation")
+ {
+ _generateNavigationMesh=true;
+ }
+ else
+ {
+ Log->Write("WARNING:\t only triangulation is supported for the mesh. You supplied [%s]",method.c_str());
+ }
+ _minDistanceBetweenTriangleEdges=xmltof(para->Attribute("minimum_distance_between_edges"),-FLT_MAX);
+ _minAngleInTriangles=xmltof(para->Attribute("minimum_angle_in_triangles"),-FLT_MAX);
+ }
+ }
+ }
+ }
+ if (nav_line_file == "")
+ return nav_line_file;
+ else
+ return _building->GetProjectRootDir()+nav_line_file;
+}
+
+
+bool GlobalRouterTrips::LoadRoutingInfos(const std::string &filename)
+{
+ if(filename=="") return true;
+
+ Log->Write("INFO:\tLoading extra routing information for the global/quickest path router");
+ Log->Write("INFO:\t from the file "+filename);
+
+ TiXmlDocument docRouting(filename);
+ if (!docRouting.LoadFile()) {
+ Log->Write("ERROR: \t%s", docRouting.ErrorDesc());
+ Log->Write("ERROR: \t could not parse the routing file [%s]",filename.c_str());
+ return false;
+ }
+
+ TiXmlElement* xRootNode = docRouting.RootElement();
+ if( ! xRootNode ) {
+ Log->Write("ERROR:\tRoot element does not exist");
+ return false;
+ }
+
+ if( xRootNode->ValueStr () != "routing" ) {
+ Log->Write("ERROR:\tRoot element value is not 'routing'.");
+ return false;
+ }
+
+ string version = xRootNode->Attribute("version");
+ if (version < JPS_OLD_VERSION) {
+ Log->Write("ERROR: \tOnly version greater than %d supported",JPS_OLD_VERSION);
+ Log->Write("ERROR: \tparsing routing file failed!");
+ return false;
+ }
+ int HlineCount = 0;
+ for(TiXmlElement* xHlinesNode = xRootNode->FirstChildElement("Hlines"); xHlinesNode;
+ xHlinesNode = xHlinesNode->NextSiblingElement("Hlines")) {
+
+
+ for(TiXmlElement* hline = xHlinesNode->FirstChildElement("Hline"); hline;
+ hline = hline->NextSiblingElement("Hline")) {
+
+ double id = xmltof(hline->Attribute("id"), -1);
+ int room_id = xmltoi(hline->Attribute("room_id"), -1);
+ int subroom_id = xmltoi(hline->Attribute("subroom_id"), -1);
+
+ double x1 = xmltof( hline->FirstChildElement("vertex")->Attribute("px"));
+ double y1 = xmltof( hline->FirstChildElement("vertex")->Attribute("py"));
+ double x2 = xmltof( hline->LastChild("vertex")->ToElement()->Attribute("px"));
+ double y2 = xmltof( hline->LastChild("vertex")->ToElement()->Attribute("py"));
+
+ Room* room = _building->GetRoom(room_id);
+ SubRoom* subroom = room->GetSubRoom(subroom_id);
+
+ //new implementation
+ Hline* h = new Hline();
+ h->SetID(id);
+ h->SetPoint1(Point(x1, y1));
+ h->SetPoint2(Point(x2, y2));
+ h->SetRoom1(room);
+ h->SetSubRoom1(subroom);
+
+ if(_building->AddHline(h))
+ {
+ subroom->AddHline(h);
+ HlineCount++;
+ //h is freed in building
+ }
+ else
+ {
+ delete h;
+ }
+ }
+ }
+ Log->Write("INFO:\tDone with loading extra routing information. Loaded <%d> Hlines", HlineCount);
+ return true;
+}
+
+bool GlobalRouterTrips::IsWall(const Line& line, const std::vector<SubRoom*>& subrooms) const
+{
+
+ for(auto&& subroom: subrooms)
+ {
+ for (auto&& obst: subroom->GetAllObstacles())
+ {
+ for (auto&& wall:obst->GetAllWalls())
+ {
+ if(line.operator ==(wall))
+ return true;
+ }
+ }
+ for (auto&& wall:subroom->GetAllWalls())
+ {
+ if(line.operator ==(wall))
+ return true;
+ }
+ }
+
+ return false;
+}
+
+bool GlobalRouterTrips::IsCrossing(const Line& line, const std::vector<SubRoom*>& subrooms) const
+{
+ for(auto&& subroom: subrooms)
+ {
+ for (const auto & crossing : subroom->GetAllCrossings())
+ {
+ if (crossing->operator ==(line))
+ return true;
+ }
+ }
+ return false;
+}
+
+bool GlobalRouterTrips::IsTransition(const Line& line, const std::vector<SubRoom*>& subrooms) const
+{
+ for(auto&& subroom: subrooms)
+ {
+ for(const auto & transition: subroom->GetAllTransitions())
+ {
+ if (transition->operator ==(line))
+ return true;
+ }
+ }
+ return false;
+}
+
+bool GlobalRouterTrips::IsHline(const Line& line, const std::vector<SubRoom*>& subrooms) const
+{
+ for(auto&& subroom: subrooms)
+ {
+ for(const auto & hline: subroom->GetAllHlines())
+ {
+ if (hline->operator ==(line))
+ return true;
+ }
+ }
+ return false;
+}
+
+double GlobalRouterTrips::MinDistanceToHlines(const Point& point, const SubRoom& sub)
+{
+ double minDist=FLT_MAX;
+ for(const auto & hline: sub.GetAllHlines())
+ {
+ double dist=hline->DistTo(point);
+ if (dist<minDist)
+ minDist=dist;
+ }
+ for(const auto & cross: sub.GetAllCrossings())
+ {
+ double dist=cross->DistTo(point);
+ if (dist<minDist)
+ minDist=dist;
+ }
+ for(const auto & trans: sub.GetAllTransitions())
+ {
+ double dist=trans->DistTo(point);
+ if (dist<minDist)
+ minDist=dist;
+ }
+ for(const auto & wall: sub.GetAllWalls())
+ {
+ double dist=wall.DistTo(point);
+ if (dist<minDist)
+ minDist=dist;
+ }
+ //also to the obstacles
+ for (const auto& obst: sub.GetAllObstacles())
+ {
+ for(const auto & wall: obst->GetAllWalls())
+ {
+ double dist=wall.DistTo(point);
+ if (dist<minDist)
+ minDist=dist;
+ }
+ }
+
+ return minDist;
+}
+
+double GlobalRouterTrips::MinAngle(const Point& p1, const Point& p2, const Point& p3)
+{
+ double a = (p1 - p2).NormSquare();
+ double b = (p1 - p3).NormSquare();
+ double c = (p3 - p2).NormSquare();
+
+ double alpha=acos((a+b-c)/(2*sqrt(a)*sqrt(b)));
+ double beta=acos((a+c-b)/(2*sqrt(a)*sqrt(c)));
+ double gamma=acos((c+b-a)/(2*sqrt(c)*sqrt(b)));
+
+ if(fabs(alpha+beta+gamma-M_PI)<J_EPS)
+ {
+ std::vector<double> vec = { alpha, beta, gamma };
+ return *std::min_element(vec.begin(), vec.end()) * (180.0 / M_PI);
+ }
+ else
+ {
+ Log->Write("ERROR:\t Error in angle calculation");
+ exit(EXIT_FAILURE);
+ }
+
+ return 0;
+}
diff --git a/routing/global_shortest_trips/GlobalRouterTrips.h b/routing/global_shortest_trips/GlobalRouterTrips.h
new file mode 100644
index 0000000000000000000000000000000000000000..4bc3abdfc5bf622353bb792664e3656904696201
--- /dev/null
+++ b/routing/global_shortest_trips/GlobalRouterTrips.h
@@ -0,0 +1,244 @@
+/**
+ * \file GlobalRouter.h
+ * \date Dec 15, 2010
+ * \version v0.7
+ * \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
+ * implement the global shortest path using the dijkstra algorithm
+ *
+ *
+ **/
+
+
+#ifndef GLOBALROUTERTRIPS_H_
+#define GLOBALROUTERTRIPS_H_
+
+#include <string>
+#include <sstream>
+#include <cfloat>
+#include <fstream>
+#include <vector>
+#include "../Router.h"
+#include "../../geometry/Building.h"
+
+// forwarded classes
+class Pedestrian;
+class AccessPoint;
+class Building;
+class OutputHandler;
+
+//log output
+extern OutputHandler* Log;
+
+#include <random>
+
+/*!
+ * \class GlobalRouter
+ *
+ * \brief global router
+ *
+ *\ingroup Router
+ *
+ * We will derive from the <Router> class to fit the interface.
+ *
+ * \author Ulrich Kemloh
+ */
+class GlobalRouterTrips: public Router
+{
+
+public:
+ /**
+ * Constructor
+ */
+ GlobalRouterTrips();
+ GlobalRouterTrips(int id, RoutingStrategy s);
+
+ /**
+ * Destructor
+ */
+ virtual ~GlobalRouterTrips();
+
+ virtual bool Init(Building* building);
+
+ virtual int FindExit(Pedestrian* p);
+
+ /**
+ * Performs a check of the geometry and fixes if possible.
+ * NOT IMPLEMENTED
+ */
+ void CheckInconsistencies();
+
+ /**
+ * write the graph as GV format to be used with graphviz
+ * @param filename
+ */
+ void WriteGraphGV(std::string filename, int finalDestination,
+ const std::vector<std::string> rooms= std::vector<std::string>());
+
+ /**
+ * Reset the routing engine and clear all pre-computed paths
+ */
+ void Reset();
+
+ /**
+ * Set/Get the edge cost for certain paths.
+ * prefer the use of paths through floors instead of rooms
+ */
+ void SetEdgeCost(double cost);
+
+ /**
+ * Set/Get the edge cost for certain paths.
+ * prefer the use of paths through floors instead of rooms
+ */
+ double GetEdgeCost() const;
+
+
+protected:
+
+ void DumpAccessPoints(int p=-1);
+
+ /**
+ * @return true if the two points are in the visibility range of each other
+ * @note based on http://alienryderflex.com/intersect/
+ */
+ bool CanSeeEachother(const Point&pt1, const Point&pt2);
+
+ /**
+ * @obsolete
+ * return a random exit
+ */
+ virtual int GetBestDefaultRandomExit(Pedestrian* p);
+
+ /**
+ * Generate a navigation mesh based on delauney triangulation
+ */
+ bool GenerateNavigationMesh();
+
+
+ /**
+ * Triangulate the geometry and generate the navigation lines
+ */
+ void TriangulateGeometry();
+
+
+ /**
+ *
+ * @param ped the pedestrian
+ * @param goalID, the goal ID.
+ * @param path vector to store the intermediate destination
+ */
+ bool GetPath(Pedestrian* ped, int goalID, std::vector<SubRoom*>& path);
+
+ /**
+ * Populates the navigations line to cross in the vector path
+ * @param ped the pedestrian
+ * @param path, the vector to store
+ */
+ bool GetPath(Pedestrian* ped, std::vector<NavLine*>& path);
+
+ /**
+ * return the relevant aps that lead to the pedestrian final destination
+ * @param ped
+ */
+ void GetRelevantRoutesTofinalDestination(Pedestrian *ped, std::vector<AccessPoint*>& relevantAPS);
+
+private:
+ /**
+ * Compute the intermediate paths between the two given transitions IDs
+ */
+ void GetPath(int transID1, int transID2);
+
+ /**
+ * Perform the FloydWahrshal algorithm
+ */
+ void FloydWarshall();
+
+ /**
+ * Load extra routing information e.g navigation lines
+ */
+ bool LoadRoutingInfos(const std::string &filename);
+
+ /**
+ * Each router is responsible of getting the correct filename
+ * and doing other initializations
+ */
+ virtual std::string GetRoutingInfoFile();
+
+ /**
+ * @return true if the supplied line is a wall.
+ */
+ bool IsWall(const Line& line, const std::vector<SubRoom*>& subrooms) const;
+
+ /**
+ * @return true if the supplied line is a Crossing.
+ */
+ bool IsCrossing(const Line& line, const std::vector<SubRoom*>& subrooms) const;
+
+ /**
+ * @return true if the supplied line is a Transition.
+ */
+ bool IsTransition(const Line& line, const std::vector<SubRoom*>& subrooms) const;
+
+ /**
+ * @return true if the supplied line is a navigation line.
+ */
+ bool IsHline(const Line& line, const std::vector<SubRoom*>& subrooms) const;
+
+ /**
+ * @return the minimum distance between the point and any line in the subroom.
+ * This include walls,hline,crossings,transitions,obstacles
+ */
+ double MinDistanceToHlines(const Point& point, const SubRoom& sub);
+
+ /**
+ * @return the minimal angle in the the triangle formed by the three points
+ */
+ double MinAngle(const Point& p1, const Point& p2, const Point& p3);
+
+private:
+ int **_pathsMatrix;
+ double **_distMatrix;
+ double _edgeCost;
+ //if false, the router will only return the exits and not the navigations line created through the mesh or inserted
+ //via the routing file. The mesh will only be used for computing the distance.
+ bool _useMeshForLocalNavigation=true;
+ bool _generateNavigationMesh=false;
+ //used to filter skinny edges in triangulation
+ double _minDistanceBetweenTriangleEdges=-FLT_MAX;
+ double _minAngleInTriangles=-FLT_MAX;
+ std::vector<int> _tmpPedPath;
+ std::map<int,int> _map_id_to_index;
+ std::map<int,int> _map_index_to_id;
+ ///map the internal crossings/transition id to
+ ///the global ID (description) for that final destination
+ std::map<int, int> _mapIdToFinalDestination;
+ // normalize the probs
+ std::default_random_engine _rdGenerator;
+ std::uniform_real_distribution<double> _rdDistribution;
+
+protected:
+ // store all subrooms at the same elevation
+ std::map<double, std::vector<SubRoom*> > _subroomsAtElevation;
+ std::map <int, AccessPoint*> _accessPoints;
+ Building *_building;
+
+};
+
+#endif /* GLOBALROUTER_H_ */
diff --git a/routing/global_shortest_trips/Triangulation.h b/routing/global_shortest_trips/Triangulation.h
new file mode 100644
index 0000000000000000000000000000000000000000..4e2f95126279891d52a377662df15c938d5fe2c0
--- /dev/null
+++ b/routing/global_shortest_trips/Triangulation.h
@@ -0,0 +1,130 @@
+/**
+ * \file Triangulation.h
+ * \date Nov 30, 2012
+ * \version v0.7
+ * \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
+ *
+ *
+ **/
+
+
+#ifndef TRIANGULATION_H_
+#define TRIANGULATION_H_
+
+#include <cstdlib>
+#include <time.h>
+#include <fstream>
+#include <string>
+#include <sstream>
+#include <algorithm>
+#include <iterator>
+#include <iostream>
+#include <vector>
+
+#include "../poly2tri/poly2tri.h"
+using std::vector;
+using std::endl;
+using std::cout;
+
+
+template <class C> void FreeClear( C & cntr )
+{
+ for ( typename C::iterator it = cntr.begin();
+ it != cntr.end(); ++it ) {
+ delete * it;
+ }
+ cntr.clear();
+}
+
+inline
+std::vector<p2t::Triangle*> triangles ()
+{
+ int num_points = 0;
+ double max, min;
+
+
+ vector< vector<p2t::Point*> > polylines;
+ vector<p2t::Point*> polyline;
+
+ // Create a simple bounding box
+ polyline.push_back(new p2t::Point(min,min));
+ polyline.push_back(new p2t::Point(min,max));
+ polyline.push_back(new p2t::Point(max,max));
+ polyline.push_back(new p2t::Point(max,min));
+
+
+ cout << "Number of constrained edges = " << polyline.size() << endl;
+ polylines.push_back(polyline);
+
+
+ /*
+ * Perform triangulation!
+ */
+
+
+ /*
+ * STEP 1: Create CDT and add primary polyline
+ * NOTE: polyline must be a simple polygon. The polyline's points
+ * constitute constrained edges. No repeat points!!!
+ */
+ p2t::CDT* cdt = new p2t::CDT(polyline);
+
+ /*
+ * STEP 2: Add holes or Steiner points if necessary
+ */
+
+ vector<p2t::Point*> head_hole ;
+ cdt->AddHole(head_hole);
+ // Add chest hole
+ vector<p2t::Point*> chest_hole;
+ cdt->AddHole(chest_hole);
+ polylines.push_back(head_hole);
+ polylines.push_back(chest_hole);
+
+ /*
+ * STEP 3: Triangulate!
+ */
+ cdt->Triangulate();
+
+ /// Constrained triangles
+ std::vector<p2t::Triangle*> triangles;
+ /// Triangle map
+ std::list<p2t::Triangle*> map;
+
+ triangles = cdt->GetTriangles();
+ map = cdt->GetMap();
+
+ cout << "Number of points = " << num_points << endl;
+ cout << "Number of triangles = " << triangles.size() << endl;
+
+
+
+ // Free points
+ for(unsigned int i = 0; i < polylines.size(); i++) {
+ vector<p2t::Point*> poly = polylines[i];
+ FreeClear(poly);
+ }
+
+ // delete cdt;
+ return triangles;
+}
+
+#endif /* TRIANGULATION_H_ */
diff --git a/routing/quickest/QuickestPathRouter.h b/routing/quickest/QuickestPathRouter.h
index af8b081012c32e378571ec3441eb38a580e984b8..97e28964f7ed4e8c3dc125dd465aba1d9e756508 100644
--- a/routing/quickest/QuickestPathRouter.h
+++ b/routing/quickest/QuickestPathRouter.h
@@ -32,6 +32,9 @@
#include "../global_shortest/GlobalRouter.h"
#include "../global_shortest/AccessPoint.h"
+//#include "../global_shortest_trips/GlobalRouterTrips.h"
+//#include "../global_shortest_trips/AccessPoint.h"
+
#include "../Router.h"
#include "../../geometry/Building.h"
#include "../../pedestrian/Pedestrian.h"
@@ -70,8 +73,8 @@ extern OutputHandler* Log;
*
* \author Ulrich Kemloh
*/
-class QuickestPathRouter: public GlobalRouter
-{
+class QuickestPathRouter: public GlobalRouter{
+
public:
QuickestPathRouter();
diff --git a/routing/trips_router/TripsRouter.cpp b/routing/trips_router/TripsRouter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..776f3da0ba85880f1607f1249416186a51a2c8ec
--- /dev/null
+++ b/routing/trips_router/TripsRouter.cpp
@@ -0,0 +1,43 @@
+//
+// Created by Tobias Schrödter on 2018-12-04.
+//
+
+#include "TripsRouter.h"
+#include <cfloat>
+#include <algorithm>
+
+TripsRouter::TripsRouter()
+{
+ Log->Write("TripsRouter!");
+}
+
+TripsRouter::TripsRouter(int id, RoutingStrategy s, Configuration* config) : Router(id, s)
+{
+
+}
+
+TripsRouter::~TripsRouter()
+{
+
+}
+
+bool TripsRouter::Init(Building* building)
+{
+ return true;
+}
+
+bool TripsRouter::ReInit()
+{
+ return true;
+}
+
+int TripsRouter::FindExit(Pedestrian* p)
+{
+ // Check if current position is already waiting area
+ // yes: set next goal and return findExit(p)
+
+ p->SetExitIndex(17);
+ p->SetExitLine(0);
+
+ return 17;
+}
diff --git a/routing/trips_router/TripsRouter.h b/routing/trips_router/TripsRouter.h
new file mode 100644
index 0000000000000000000000000000000000000000..1b035052a93af5632232e29c08379660eebf02f3
--- /dev/null
+++ b/routing/trips_router/TripsRouter.h
@@ -0,0 +1,37 @@
+//
+// Created by Tobias Schrödter on 2018-12-04.
+//
+
+#ifndef JPSCORE_TRIPSROUTER_H
+#define JPSCORE_TRIPSROUTER_H
+
+#include "../Router.h"
+#include "../../general/Macros.h"
+#include "../../geometry/Building.h"
+
+#include "../../pedestrian/Pedestrian.h"
+
+class Building;
+class Pedestrian;
+class OutputHandler;
+
+//log output
+extern OutputHandler* Log;
+
+class TripsRouter : public Router {
+
+public:
+ TripsRouter();
+ TripsRouter(int id, RoutingStrategy s, Configuration* config);
+
+ virtual ~TripsRouter();
+
+ virtual bool Init(Building* building);
+
+ virtual bool ReInit();
+
+ virtual int FindExit(Pedestrian* p);
+
+};
+
+#endif //JPSCORE_TRIPSROUTER_H