Adding distance map to Routing

libdistance/CMakeLists.txt

@@ -12,13 +12,13 @@ target_link_libraries(distance INTERFACE
 set_property(TARGET distance PROPERTY INTERPROCEDURAL_OPTIMIZATION ${USE_IPO})
 set_property(TARGET distance PROPERTY INTERPROCEDURAL_OPTIMIZATION_DEBUG OFF)
 
-add_executable(distance_map_test
-    src/distance_test.cpp
-)
-target_link_libraries(distance_map_test
-    PRIVATE
-        distance
-)
+# add_executable(distance_map_test
+#     src/distance_test.cpp
+# )
+# target_link_libraries(distance_map_test
+#     PRIVATE
+#         distance
+# )
 
 #if(BUILD_TESTS)
 #    add_executable(libdistance-tests

libdistance/src/DistanceMap.hpp

@@ -17,6 +17,8 @@
 #include <type_traits>
 #include <vector>
 
+namespace distance
+{
 template <typename T>
 concept SignedIntegral = std::is_integral_v<T> && std::is_signed_v<T>;
 
@@ -565,7 +567,7 @@ void PrintDistanceMap(const Map<UI>& distance)
     std::cout << output << "\n";
 }
 
-void PrintBoolDistanceMap(const std::vector<bool>& visited, size_t xDim, size_t yDim)
+static void PrintBoolDistanceMap(const std::vector<bool>& visited, size_t xDim, size_t yDim)
 {
     using I = typename std::make_signed_t<size_t>; // Get the corresponding signed type
 
@@ -978,7 +980,7 @@ class DistanceMapBuilder
     void AddArc(const Arc<U>& arc) { arcs.emplace_back(arc); }
     void AddCircle(const Circle<U>& circle) { circles.emplace_back(circle); }
     void AddExitLine(const Line<U>& exitLine) { exitLines.emplace_back(exitLine); }
-    void AddExitPolygon(const Polygon<U> exitPolygon);
+    void AddExitPolygon(const Polygon<U> exitPolygon){};
     //    TODO(TS): needed for Simulex?
     //    void AddBoundingBox(const )
 
@@ -1006,13 +1008,13 @@ class DistanceMapBuilder
         //        PrintDistanceMap<T, U>(distance, xDim, yDim);
 
         PrepareDistanceMap(distance, xMin, yMin);
-//        std::cout << "Prepared: \n";
-//        PrintDistanceMap<T, U>(distance);
+        //        std::cout << "Prepared: \n";
+        //        PrintDistanceMap<T, U>(distance);
 
         ComputeDistanceMap(distance, localDistance);
 
-//        std::cout << "Final: \n";
-//        PrintDistanceMap<T, U>(distance);
+        //        std::cout << "Final: \n";
+        //        PrintDistanceMap<T, U>(distance);
 
         //        auto bytes = DumpDistanceMap<T, U>(distance);
         //        std::fstream out("dump.data", std::ios::trunc | std::ios::binary |
@@ -1042,3 +1044,4 @@ class DistanceMapBuilder
         //        CELL_SIZE);
     }
 };
+} // namespace distance

libjupedsim/src/agent.cpp

@@ -10,6 +10,7 @@
 #include <Unreachable.hpp>
 
 #include <cassert>
+#include <variant>
 
 using jupedsim::detail::intoJPS_Point;
 using jupedsim::detail::intoPoint;
@@ -49,7 +50,10 @@ JPS_Point JPS_Agent_GetTarget(JPS_Agent handle)
 {
     assert(handle);
     const auto agent = reinterpret_cast<const GenericAgent*>(handle);
-    return intoJPS_Point(agent->target);
+    if(auto val = std::get_if<Point>(&agent->target); val != nullptr) {
+        return intoJPS_Point(*val);
+    }
+    return {};
 }
 
 bool JPS_Agent_SetTarget(JPS_Agent handle, JPS_Point waypoint, JPS_ErrorMessage* errorMessage)

libsimulator/CMakeLists.txt

@@ -54,6 +54,8 @@ add_library(simulator STATIC
     src/Point.hpp
     src/Polygon.cpp
     src/Polygon.hpp
+    src/Routing.cpp
+    src/Routing.hpp
     src/RoutingEngine.cpp
     src/RoutingEngine.hpp
     src/Simulation.cpp
@@ -90,6 +92,7 @@ target_compile_definitions(simulator PUBLIC
 )
 target_link_libraries(simulator PUBLIC
     common
+    distance
     fmt::fmt
     CGAL::CGAL
     build_info

libsimulator/src/GenericAgent.hpp

@@ -23,7 +23,8 @@ struct GenericAgent {
 
     // This is evaluated by the "operational level"
     Point destination{};
-    Point target{};
+    // TODO(kkratz): Type needs to be BaseStage::Location
+    std::variant<Point, jps::UniqueID<BaseStage>> target{};
 
     // Agent fields common for all models
     Point pos{};

libsimulator/src/Journey.hpp

@@ -179,7 +179,7 @@ class Journey
 
     ID Id() const { return id; }
 
-    std::tuple<Point, BaseStage::ID> Target(const GenericAgent& agent) const
+    std::tuple<BaseStage::Location, BaseStage::ID> Target(const GenericAgent& agent) const
     {
         auto& node = stages.at(agent.stageId);
         auto stage = node.stage;

libsimulator/src/Routing.cpp

@@ -0,0 +1,86 @@
+// Copyright © 2012-2024 Forschungszentrum Jülich GmbH
+// SPDX-License-Identifier: LGPL-3.0-or-later
+#include "Routing.hpp"
+
+#include "CfgCgal.hpp"
+#include "Unreachable.hpp"
+
+#include <memory.h>
+
+distance::Point<double> cvt_pt(const Poly::Point_2& p)
+{
+    return distance::Point<double>{CGAL::to_double(p.x()), CGAL::to_double(p.y())};
+};
+
+distance::Polygon<double> cvt_poly(const Poly& cgalContainer)
+{
+    distance::Polygon<double> poly;
+    poly.points.reserve(cgalContainer.size());
+    std::transform(
+        std::begin(cgalContainer),
+        std::end(cgalContainer),
+        std::back_inserter(poly.points),
+        cvt_pt);
+    return poly;
+};
+
+Routing::Routing(std::unique_ptr<CollisionGeometry> geometry)
+    : _geometry(std::move(geometry))
+    , _routingEngine(std::make_unique<RoutingEngine>(_geometry->Polygon()))
+{
+}
+
+void Routing::AddDistanceMapForStage(
+    const BaseStage::ID id,
+    const StageDescription stageDescription)
+{
+    std::visit(
+        overloaded{
+            [this, id](const WaypointDescription& d) -> void {
+                auto builder = PrepareDistanceMapBuilder();
+                builder->AddExitLine({{d.position.x, d.position.y}, {d.position.x, d.position.y}});
+                _distanceMaps.emplace(id, builder->Build());
+                // HACK(kkratz):
+                // distance::DumpDistanceMapMatplotlibCSV(*_distanceMaps[id]);
+            },
+            [this, id](const ExitDescription& d) -> void {
+                auto builder = PrepareDistanceMapBuilder();
+                const Poly& p = d.polygon;
+                builder->AddExitPolygon(cvt_poly(p));
+                _distanceMaps.emplace(id, builder->Build());
+                // HACK(kkratz):
+                // distance::DumpDistanceMapMatplotlibCSV(*_distanceMaps[id]);
+            },
+            [](const NotifiableWaitingSetDescription&) -> void {},
+            [](const NotifiableQueueDescription&) -> void {},
+            [](const DirectSteeringDescription&) -> void {}},
+        stageDescription);
+}
+
+std::unique_ptr<Routing::DMapBuilder> Routing::PrepareDistanceMapBuilder() const
+{
+    auto builder = std::make_unique<DMapBuilder>();
+
+    const auto poly = _geometry->Polygon();
+    builder->AddPolygon(cvt_poly(poly.outer_boundary()));
+    for(const auto& hole : poly.holes()) {
+        builder->AddPolygon(cvt_poly(hole));
+    }
+
+    return builder;
+}
+
+Point Routing::ComputeWaypoint(Point currentPosition, BaseStage::Location destination) const
+{
+    return std::visit(
+        overloaded{
+            [this, currentPosition](const Point& destination) {
+                return _routingEngine->ComputeWaypoint(currentPosition, destination);
+            },
+            [this, currentPosition](const BaseStage::ID id) {
+                const auto val =
+                    _distanceMaps.at(id)->GetNextTarget({currentPosition.x, currentPosition.y});
+                return Point{val.x, val.y};
+            }},
+        destination);
+}

libsimulator/src/Routing.hpp

@@ -0,0 +1,35 @@
+// Copyright © 2012-2024 Forschungszentrum Jülich GmbH
+// SPDX-License-Identifier: LGPL-3.0-or-later
+#pragma once
+
+#include "DistanceMap.hpp"
+#include "RoutingEngine.hpp"
+#include "Stage.hpp"
+#include "StageDescription.hpp"
+
+class Routing
+{
+    using DMap = distance::DistanceMap<int, double>;
+    using DMapBuilder = distance::DistanceMapBuilder<int, double>;
+    using DPoly = distance::Polygon<double>;
+    using DPoint = decltype(DPoly::points)::value_type;
+    std::unique_ptr<CollisionGeometry> _geometry;
+    std::unique_ptr<RoutingEngine> _routingEngine;
+    std::unordered_map<BaseStage::ID, std::unique_ptr<const DMap>> _distanceMaps{};
+
+public:
+    explicit Routing(std::unique_ptr<CollisionGeometry> geometry);
+    ~Routing() = default;
+    Routing(const Routing& other) = delete;
+    Routing& operator=(const Routing& other) = delete;
+    Routing(Routing&& other) = default;
+    Routing& operator=(Routing&& other) = default;
+    void AddDistanceMapForStage(const BaseStage::ID id, const StageDescription stageDescription);
+
+    const CollisionGeometry& Geometry() const { return *_geometry; }
+    bool InsideGeometry(const Point& p) const { return _geometry->InsideGeometry(p); }
+    Point ComputeWaypoint(Point currentPosition, BaseStage::Location destination) const;
+
+private:
+    std::unique_ptr<DMapBuilder> PrepareDistanceMapBuilder() const;
+};

libsimulator/src/RoutingEngine.hpp

@@ -14,9 +14,6 @@
 #include <memory>
 #include <vector>
 
-using LocationID = size_t;
-using Location = std::variant<Point, LocationID>;
-
 class RoutingEngine : public Clonable<RoutingEngine>
 {
     CDT cdt{};

libsimulator/src/Simulation.cpp

@@ -18,16 +18,12 @@ Simulation::Simulation(
     double dT)
     : _clock(dT), _operationalDecisionSystem(std::move(operationalModel))
 {
-    const auto p = geometry->Polygon();
-    const auto& [tup, res] = geometries.emplace(
-        std::piecewise_construct,
-        std::forward_as_tuple(geometry->Id()),
-        std::forward_as_tuple(std::move(geometry), std::make_unique<RoutingEngine>(p)));
+    const auto& [val, res] =
+        geometries.emplace(geometry->Id(), std::make_unique<Routing>(std::move(geometry)));
     if(!res) {
         throw SimulationError("Internal error");
     }
-    _geometry = std::get<0>(tup->second).get();
-    _routingEngine = std::get<1>(tup->second).get();
+    _routing = val->second.get();
 }
 const SimulationClock& Simulation::Clock() const
 {
@@ -51,13 +47,13 @@ void Simulation::Iterate()
     _agentRemovalSystem.Run(_agents, _removedAgentsInLastIteration, _stageManager);
     _neighborhoodSearch.Update(_agents);
 
-    _stageSystem.Run(_stageManager, _neighborhoodSearch, *_geometry);
+    _stageSystem.Run(_stageManager, _neighborhoodSearch, _routing->Geometry());
     _stategicalDecisionSystem.Run(_journeys, _agents, _stageManager);
-    _tacticalDecisionSystem.Run(*_routingEngine, _agents);
+    _tacticalDecisionSystem.Run(*_routing, _agents);
     {
         auto t2 = _perfStats.TraceOperationalDecisionSystemRun();
         _operationalDecisionSystem.Run(
-            _clock.dT(), _clock.ElapsedTime(), _neighborhoodSearch, *_geometry, _agents);
+            _clock.dT(), _clock.ElapsedTime(), _neighborhoodSearch, _routing->Geometry(), _agents);
     }
     _clock.Advance();
 }
@@ -142,26 +138,26 @@ BaseStage::ID Simulation::AddStage(const StageDescription stageDescription)
     std::visit(
         overloaded{
             [this](const WaypointDescription& d) -> void {
-                if(!this->_geometry->InsideGeometry(d.position)) {
+                if(!this->_routing->InsideGeometry(d.position)) {
                     throw SimulationError("WayPoint {} not inside walkable area", d.position);
                 }
             },
             [this](const ExitDescription& d) -> void {
-                if(!this->_geometry->InsideGeometry(d.polygon.Centroid())) {
+                if(!this->_routing->InsideGeometry(d.polygon.Centroid())) {
                     throw SimulationError("Exit {} not inside walkable area", d.polygon.Centroid());
                 }
             },
             [this](const NotifiableWaitingSetDescription& d) -> void {
                 for(const auto& point : d.slots) {
-                    if(!this->_geometry->InsideGeometry(point)) {
+                    if(!this->_routing->InsideGeometry(point)) {
                         throw SimulationError(
                             "NotifiableWaitingSet point {} not inside walkable area", point);
                     }
                 }
             },
             [this](const NotifiableQueueDescription& d) -> void {
                 for(const auto& point : d.slots) {
-                    if(!this->_geometry->InsideGeometry(point)) {
+                    if(!this->_routing->InsideGeometry(point)) {
                         throw SimulationError(
                             "NotifiableQueue point {} not inside walkable area", point);
                     }
@@ -172,17 +168,19 @@ BaseStage::ID Simulation::AddStage(const StageDescription stageDescription)
             }},
         stageDescription);
 
-    return _stageManager.AddStage(stageDescription, _removedAgentsInLastIteration);
+    const auto id = _stageManager.AddStage(stageDescription, _removedAgentsInLastIteration);
+
+    return id;
 }
 
 GenericAgent::ID Simulation::AddAgent(GenericAgent&& agent)
 {
-    if(!_geometry->InsideGeometry(agent.pos)) {
+    if(!_routing->InsideGeometry(agent.pos)) {
         throw SimulationError("Agent {} not inside walkable area", agent.pos);
     }
 
     agent.orientation = agent.orientation.Normalized();
-    _operationalDecisionSystem.ValidateAgent(agent, _neighborhoodSearch, *_geometry);
+    _operationalDecisionSystem.ValidateAgent(agent, _neighborhoodSearch, _routing->Geometry());
 
     if(_journeys.count(agent.journeyId) == 0) {
         throw SimulationError("Unknown journey id: {}", agent.journeyId);
@@ -197,7 +195,7 @@ GenericAgent::ID Simulation::AddAgent(GenericAgent&& agent)
 
     auto v = IteratorPair(std::prev(std::end(_agents)), std::end(_agents));
     _stategicalDecisionSystem.Run(_journeys, v, _stageManager);
-    _tacticalDecisionSystem.Run(*_routingEngine, v);
+    _tacticalDecisionSystem.Run(*_routing, v);
     return _agents.back().id.getID();
 }
 
@@ -326,26 +324,22 @@ StageProxy Simulation::Stage(BaseStage::ID stageId)
 }
 CollisionGeometry Simulation::Geo() const
 {
-    return *_geometry;
+    return _routing->Geometry();
 }
 
 void Simulation::SwitchGeometry(std::unique_ptr<CollisionGeometry>&& geometry)
 {
     ValidateGeometry(geometry);
     if(const auto& iter = geometries.find(geometry->Id()); iter != std::end(geometries)) {
-        _geometry = std::get<0>(iter->second).get();
-        _routingEngine = std::get<1>(iter->second).get();
+        _routing = iter->second.get();
     } else {
         const auto p = geometry->Polygon();
-        const auto& [tup, res] = geometries.emplace(
-            std::piecewise_construct,
-            std::forward_as_tuple(geometry->Id()),
-            std::forward_as_tuple(std::move(geometry), std::make_unique<RoutingEngine>(p)));
+        const auto& [val, res] =
+            geometries.emplace(geometry->Id(), std::make_unique<Routing>(std::move(geometry)));
         if(!res) {
             throw SimulationError("Internal error");
         }
-        _geometry = std::get<0>(tup->second).get();
-        _routingEngine = std::get<1>(tup->second).get();
+        _routing = val->second.get();
     }
 }