Add prototype for direct steering agents

PedestrianDynamics · Nov 16, 2023 · 93a75a3 · 93a75a3
1 parent b3fbd52
commit 93a75a3
Show file tree

Hide file tree

Showing 11 changed files with 199 additions and 9 deletions.
diff --git a/libjupedsim/include/jupedsim/jupedsim.h b/libjupedsim/include/jupedsim/jupedsim.h
@@ -737,7 +737,13 @@ JPS_CollisionFreeSpeedModelState_SetRadius(JPS_CollisionFreeSpeedModelState hand
 /**
  * Identifies the type of stage
  */
-enum JPS_StageType { JPS_NotifiableQueueType, JPS_WaitingSetType, JPS_WaypointType, JPS_ExitType };
+enum JPS_StageType {
+    JPS_NotifiableQueueType,
+    JPS_WaitingSetType,
+    JPS_WaypointType,
+    JPS_ExitType,
+    JPS_DirectSteeringType
+};
 
 /**
  * Opaque type of an NotifiableQueueProxy
@@ -784,6 +790,12 @@ JUPEDSIM_API size_t JPS_ExitProxy_GetCountTargeting(JPS_ExitProxy handle);
 
 JUPEDSIM_API void JPS_ExitProxy_Free(JPS_ExitProxy handle);
 
+typedef struct JPS_DirectSteeringProxy_t* JPS_DirectSteeringProxy;
+
+JUPEDSIM_API size_t JPS_DirectSteeringProxy_GetCountTargeting(JPS_DirectSteeringProxy handle);
+
+JUPEDSIM_API void JPS_DirectSteeringProxy_Free(JPS_DirectSteeringProxy handle);
+
 /**
  * Opaque type of an agent
  */
@@ -817,6 +829,16 @@ JUPEDSIM_API JPS_StageId JPS_Agent_GetStageId(JPS_Agent handle);
  */
 JUPEDSIM_API JPS_Point JPS_Agent_GetPosition(JPS_Agent handle);
 
+/**
+ * Access the agents waypoint.
+ * @param handle of the agent to access.
+ * @return waypoint
+ */
+JUPEDSIM_API JPS_Point JPS_Agent_GetWayPoint(JPS_Agent handle);
+
+JUPEDSIM_API bool
+JPS_Agent_SetWayPoint(JPS_Agent handle, JPS_Point waypoint, JPS_ErrorMessage* errorMessage);
+
 /**
  * Access the agents orientation.
  * The orientation is unit length.
@@ -1091,6 +1113,12 @@ JUPEDSIM_API JPS_StageId JPS_Simulation_AddStageWaitingSet(
     size_t len_waiting_positions,
     JPS_ErrorMessage* errorMessage);
 
+/**
+ * @todo
+ */
+JUPEDSIM_API JPS_StageId
+JPS_Simulation_AddStageDirectSteering(JPS_Simulation handle, JPS_ErrorMessage* errorMessage);
+
 /**
  * Adds a new agent to the simulation.
  * This can be called at any time, i.e. agents can be added at any iteration.
@@ -1264,6 +1292,11 @@ JUPEDSIM_API JPS_ExitProxy JPS_Simulation_GetExitProxy(
     JPS_StageId stageId,
     JPS_ErrorMessage* errorMessage);
 
+JUPEDSIM_API JPS_DirectSteeringProxy JPS_Simulation_GetDirectSteeringProxy(
+    JPS_Simulation handle,
+    JPS_StageId stageId,
+    JPS_ErrorMessage* errorMessage);
+
 /**
  * Enable / disable  collection of performance data.
  * @param handle of the Simulation to operate on

diff --git a/libjupedsim/src/jupedsim.cpp b/libjupedsim/src/jupedsim.cpp
@@ -659,7 +659,22 @@ void JPS_WaitingSetProxy_Free(JPS_WaitingSetProxy handle)
 }
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////
-/// WaypointProxy
+/// DirectSteeringProxy
+////////////////////////////////////////////////////////////////////////////////////////////////////
+size_t JPS_DirectSteeringProxy_GetCountTargeting(JPS_DirectSteeringProxy handle)
+{
+    assert(handle);
+    auto proxy = reinterpret_cast<DirectSteeringProxy*>(handle);
+    return proxy->CountTargeting();
+}
+
+void JPS_DirectSteeringProxy_Free(JPS_DirectSteeringProxy handle)
+{
+    delete reinterpret_cast<DirectSteeringProxy*>(handle);
+}
+
+////////////////////////////////////////////////////////////////////////////////////////////////////
+/// WaitPointProxy
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 size_t JPS_WaypointProxy_GetCountTargeting(JPS_WaypointProxy handle)
 {
@@ -674,7 +689,7 @@ void JPS_WaypointProxy_Free(JPS_WaypointProxy handle)
 }
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////
-/// WaitingSetProxy
+/// ExitProxy
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 size_t JPS_ExitProxy_GetCountTargeting(JPS_ExitProxy handle)
 {
@@ -719,6 +734,33 @@ JPS_Point JPS_Agent_GetPosition(JPS_Agent handle)
     return intoJPS_Point(agent->pos);
 }
 
+JPS_Point JPS_Agent_GetWayPoint(JPS_Agent handle)
+{
+    assert(handle);
+    const auto agent = reinterpret_cast<const GenericAgent*>(handle);
+    return intoJPS_Point(agent->waypoint);
+}
+
+bool JPS_Agent_SetWayPoint(JPS_Agent handle, JPS_Point waypoint, JPS_ErrorMessage* errorMessage)
+{
+    assert(handle);
+    try {
+        auto agent = reinterpret_cast<GenericAgent*>(handle);
+        agent->waypoint = intoPoint(waypoint);
+        return true;
+    } catch(const std::exception& ex) {
+        if(errorMessage) {
+            *errorMessage = reinterpret_cast<JPS_ErrorMessage>(new JPS_ErrorMessage_t{ex.what()});
+        }
+    } catch(...) {
+        if(errorMessage) {
+            *errorMessage = reinterpret_cast<JPS_ErrorMessage>(
+                new JPS_ErrorMessage_t{"Unknown internal error."});
+        }
+    }
+    return false;
+}
+
 JPS_Point JPS_Agent_GetOrientation(JPS_Agent handle)
 {
     assert(handle);
@@ -1089,6 +1131,12 @@ JPS_StageId JPS_Simulation_AddStageWaitingSet(
     return add_stage(handle, NotifiableWaitingSetDescription{positions}, errorMessage);
 }
 
+JPS_StageId
+JPS_Simulation_AddStageDirectSteering(JPS_Simulation handle, JPS_ErrorMessage* errorMessage)
+{
+    return add_stage(handle, DirectSteeringDescription{}, errorMessage);
+}
+
 JPS_AgentId JPS_Simulation_AddGeneralizedCentrifugalForceModelAgent(
     JPS_Simulation handle,
     JPS_GeneralizedCentrifugalForceModelAgentParameters parameters,
@@ -1357,6 +1405,8 @@ JPS_StageType JPS_Simulation_GetStageType(JPS_Simulation handle, JPS_StageId id)
                 return JPS_NotifiableQueueType;
             case 3:
                 return JPS_ExitType;
+            case 4:
+                return JPS_DirectSteeringType;
         }
         UNREACHABLE();
     };
@@ -1434,6 +1484,24 @@ JUPEDSIM_API JPS_ExitProxy JPS_Simulation_GetExitProxy(
     }
 }
 
+JPS_DirectSteeringProxy JPS_Simulation_GetDirectSteeringProxy(
+    JPS_Simulation handle,
+    JPS_StageId stageId,
+    JPS_ErrorMessage* errorMessage)
+{
+    assert(handle);
+    auto simulation = reinterpret_cast<Simulation*>(handle);
+    try {
+        return reinterpret_cast<JPS_DirectSteeringProxy>(
+            new DirectSteeringProxy(std::get<DirectSteeringProxy>(simulation->Stage(stageId))));
+    } catch(const std::exception& ex) {
+        if(errorMessage) {
+            *errorMessage = reinterpret_cast<JPS_ErrorMessage>(new JPS_ErrorMessage_t{ex.what()});
+        }
+        return nullptr;
+    }
+}
+
 void JPS_Simulation_SetTracing(JPS_Simulation handle, bool status)
 {
     assert(handle);

diff --git a/libsimulator/src/GenericAgent.hpp b/libsimulator/src/GenericAgent.hpp
@@ -40,6 +40,7 @@ struct GenericAgent {
         : id(id_ != ID::Invalid ? id_ : ID{})
         , journeyId(journeyId_)
         , stageId(stageId_)
+        , waypoint(pos_)
         , pos(pos_)
         , orientation(orientation_)
         , model(std::move(model_))

diff --git a/libsimulator/src/Simulation.cpp b/libsimulator/src/Simulation.cpp
@@ -151,6 +151,9 @@ BaseStage::ID Simulation::AddStage(const StageDescription stageDescription)
                             "NotifiableQueue point {} not inside walkable area", point);
                     }
                 }
+            },
+            [](const DirectSteeringDescription&) -> void {
+
             }},
         stageDescription);
 

diff --git a/libsimulator/src/Simulation.hpp b/libsimulator/src/Simulation.hpp
@@ -77,6 +77,7 @@ class Simulation
     double DT() const;
     void
     SwitchAgentJourney(GenericAgent::ID agent_id, Journey::ID journey_id, BaseStage::ID stage_id);
+    void ChangeAgentWaypoint(GenericAgent::ID agent_id, Point waypoint);
     uint64_t Iteration() const;
     std::vector<GenericAgent::ID> AgentsInRange(Point p, double distance);
     /// Returns IDs of all agents inside the defined polygon

diff --git a/libsimulator/src/Stage.hpp b/libsimulator/src/Stage.hpp
@@ -78,8 +78,20 @@ class ExitProxy : public BaseProxy
     ExitProxy(Simulation* simulation_, BaseStage* stage_) : BaseProxy(simulation_, stage_) {}
 };
 
-using StageProxy =
-    std::variant<WaypointProxy, NotifiableWaitingSetProxy, NotifiableQueueProxy, ExitProxy>;
+class DirectSteeringProxy : public BaseProxy
+{
+public:
+    DirectSteeringProxy(Simulation* simulation_, BaseStage* stage_) : BaseProxy(simulation_, stage_)
+    {
+    }
+};
+
+using StageProxy = std::variant<
+    WaypointProxy,
+    NotifiableWaitingSetProxy,
+    NotifiableQueueProxy,
+    ExitProxy,
+    DirectSteeringProxy>;
 
 class BaseStage
 {
@@ -96,7 +108,7 @@ class BaseStage
     virtual Point Target(const GenericAgent& agent) = 0;
     virtual StageProxy Proxy(Simulation* simulation_) = 0;
     ID Id() const { return id; }
-    size_t CountTargeting() const { return targeting; }
+    virtual size_t CountTargeting() const { return targeting; }
     void IncreaseTargeting() { targeting = targeting + 1; }
     void DecreaseTargeting()
     {
@@ -248,3 +260,18 @@ void NotifiableQueue::Update(const NeighborhoodSearch<T>& neighborhoodSearch)
         }
     }
 }
+
+class DirectSteering : public BaseStage
+{
+public:
+    DirectSteering() = default;
+    ~DirectSteering() override = default;
+    bool IsCompleted(const GenericAgent&) override { return false; };
+    Point Target(const GenericAgent& agent) override { return agent.waypoint; };
+    StageProxy Proxy(Simulation* simulation) override
+    {
+        return DirectSteeringProxy(simulation, this);
+    };
+
+    size_t CountTargeting() const override { return 1; };
+};
diff --git a/libsimulator/src/StageDescription.hpp b/libsimulator/src/StageDescription.hpp
@@ -8,6 +8,9 @@
 #include <variant>
 #include <vector>
 
+struct DirectSteeringDescription {
+};
+
 struct WaypointDescription {
     Point position;
     double distance;
@@ -26,6 +29,7 @@ struct NotifiableQueueDescription {
 };
 
 using StageDescription = std::variant<
+    DirectSteeringDescription,
     WaypointDescription,
     ExitDescription,
     NotifiableWaitingSetDescription,

diff --git a/libsimulator/src/StageManager.hpp b/libsimulator/src/StageManager.hpp
@@ -38,13 +38,17 @@ class StageManager
                 },
                 [](const NotifiableQueueDescription& d) -> std::unique_ptr<BaseStage> {
                     return std::make_unique<NotifiableQueue>(d.slots);
+                },
+                [](const DirectSteeringDescription&) -> std::unique_ptr<BaseStage> {
+                    return std::make_unique<DirectSteering>();
                 }},
             stageDescription);
         if(stages.find(stage->Id()) != stages.end()) {
             throw SimulationError("Internal error, stage id already in use.");
         }
         const auto id = stage->Id();
         stages.emplace(id, std::move(stage));
+
         return id;
     }
 

diff --git a/python_bindings_jupedsim/bindings_jupedsim.cpp b/python_bindings_jupedsim/bindings_jupedsim.cpp
@@ -71,6 +71,7 @@ OWNED_WRAPPER(JPS_NotifiableQueueProxy);
 OWNED_WRAPPER(JPS_WaitingSetProxy);
 OWNED_WRAPPER(JPS_WaypointProxy);
 OWNED_WRAPPER(JPS_ExitProxy);
+OWNED_WRAPPER(JPS_DirectSteeringProxy);
 WRAPPER(JPS_Agent);
 WRAPPER(JPS_GeneralizedCentrifugalForceModelState);
 WRAPPER(JPS_CollisionFreeSpeedModelState);
@@ -384,8 +385,7 @@ PYBIND11_MODULE(py_jupedsim, m)
             py::arg("max_geometry_repulsion_force"))
         .def("build", [](JPS_GeneralizedCentrifugalForceModelBuilder_Wrapper& w) {
             JPS_ErrorMessage errorMsg{};
-            auto result =
-                JPS_GeneralizedCentrifugalForceModelBuilder_Build(w.handle, &errorMsg);
+            auto result = JPS_GeneralizedCentrifugalForceModelBuilder_Build(w.handle, &errorMsg);
             if(result) {
                 return std::make_unique<JPS_OperationalModel_Wrapper>(result);
             }
@@ -662,6 +662,11 @@ PYBIND11_MODULE(py_jupedsim, m)
         .def("count_targeting", [](const JPS_ExitProxy_Wrapper& w) {
             return JPS_ExitProxy_GetCountTargeting(w.handle);
         });
+    py::class_<JPS_DirectSteeringProxy_Wrapper>(m, "DirectSteeringProxy")
+        .def("count_targeting", [](const JPS_DirectSteeringProxy_Wrapper& w) {
+            return JPS_DirectSteeringProxy_GetCountTargeting(w.handle);
+        });
+
     py::class_<JPS_Agent_Wrapper>(m, "Agent")
         .def_property_readonly(
             "id", [](const JPS_Agent_Wrapper& w) { return JPS_Agent_GetId(w.handle); })
@@ -678,6 +683,18 @@ PYBIND11_MODULE(py_jupedsim, m)
             [](const JPS_Agent_Wrapper& w) {
                 return intoTuple(JPS_Agent_GetOrientation(w.handle));
             })
+        .def_property(
+            "waypoint",
+            [](const JPS_Agent_Wrapper& w) { return intoTuple(JPS_Agent_GetWayPoint(w.handle)); },
+            [](JPS_Agent_Wrapper& w, std::tuple<double, double> waypoint) {
+                JPS_ErrorMessage errorMsg{};
+                auto success = JPS_Agent_SetWayPoint(w.handle, intoJPS_Point(waypoint), &errorMsg);
+                if(!success) {
+                    auto msg = std::string(JPS_ErrorMessage_GetMessage(errorMsg));
+                    JPS_ErrorMessage_Free(errorMsg);
+                    throw std::runtime_error{msg};
+                }
+            })
         .def_property_readonly(
             "model",
             [](const JPS_Agent_Wrapper& w)
@@ -769,6 +786,18 @@ PYBIND11_MODULE(py_jupedsim, m)
                 JPS_ErrorMessage_Free(errorMsg);
                 throw std::runtime_error{msg};
             })
+        .def(
+            "add_direct_steering_stage",
+            [](JPS_Simulation_Wrapper& w) {
+                JPS_ErrorMessage errorMsg{};
+                const auto result = JPS_Simulation_AddStageDirectSteering(w.handle, &errorMsg);
+                if(result != 0) {
+                    return result;
+                }
+                auto msg = std::string(JPS_ErrorMessage_GetMessage(errorMsg));
+                JPS_ErrorMessage_Free(errorMsg);
+                throw std::runtime_error{msg};
+            })
         .def(
             "add_journey",
             [](JPS_Simulation_Wrapper& simulation, JPS_JourneyDescription_Wrapper& journey) {
@@ -920,7 +949,8 @@ PYBIND11_MODULE(py_jupedsim, m)
                     std::unique_ptr<JPS_WaypointProxy_Wrapper>,
                     std::unique_ptr<JPS_NotifiableQueueProxy_Wrapper>,
                     std::unique_ptr<JPS_WaitingSetProxy_Wrapper>,
-                    std::unique_ptr<JPS_ExitProxy_Wrapper>> {
+                    std::unique_ptr<JPS_ExitProxy_Wrapper>,
+                    std::unique_ptr<JPS_DirectSteeringProxy_Wrapper>> {
                 const auto type = JPS_Simulation_GetStageType(w.handle, id);
                 JPS_ErrorMessage errorMessage{};
                 const auto raise = [](JPS_ErrorMessage err) {
@@ -962,6 +992,14 @@ PYBIND11_MODULE(py_jupedsim, m)
                         }
                         return ptr;
                     }
+                    case JPS_DirectSteeringType: {
+                        auto ptr = std::make_unique<JPS_DirectSteeringProxy_Wrapper>(
+                            JPS_Simulation_GetDirectSteeringProxy(w.handle, id, &errorMessage));
+                        if(!ptr) {
+                            raise(errorMessage);
+                        }
+                        return ptr;
+                    }
                 }
                 UNREACHABLE();
             })
-Original file line number
+Diff line change
@@ Expand Up @@
                                 "NotifiableQueue point {} not inside walkable area", point);
                         }
                     }
+                },
+                [](const DirectSteeringDescription&) -> void {
                 }},
             stageDescription);
@@ Expand Down @@