Fix navigation not returning shortest path

container/perf-measurement/container_perf_test.py

@@ -157,7 +157,7 @@ def run_test(test, args, build_dir, result_dir):
     perf_stats = pd.read_sql_query("SELECT * FROM perf_statistics", db)
 
     geometry_as_wkt = (
-        db.cursor().execute("SELECT * from geometry LIMIT 1").fetchone()[0]
+        db.cursor().execute("SELECT wkt from geometry LIMIT 1").fetchone()[0]
     )
     geometry = shapely.from_wkt(geometry_as_wkt)
 

libjupedsim/include/jupedsim/routing.h

@@ -12,34 +12,75 @@
 extern "C" {
 #endif
 
-struct JPS_Path {
+/**
+ * A path inside the walkable area.
+ */
+typedef struct JPS_Path {
+    /**
+     * Number of points in this path
+     */
     size_t len;
+    /**
+     * JPS_Points in this path.
+     */
     const JPS_Point* points;
-};
+} JPS_Path;
 
-JUPEDSIM_API void JPS_Path_Free(JPS_Path* path);
-
-struct JPS_Triangle {
-    JPS_Point points[3];
-};
+/**
+ * Frees memory held by JPS_Path
+ *
+ * @param path to free its memory.
+ */
+JUPEDSIM_API void JPS_Path_Free(JPS_Path path);
 
-struct JPS_TriangleMesh {
+/**
+ * Describes a polygon in terms of indices of vertices belonging to a JPS_Mesh.
+ */
+typedef struct JPS_Polygon_Desc {
+    /**
+     * Offset point to the beginning of this polygons indices in the JPS_Mesh indices array.
+     */
+    size_t offset;
+    /**
+     * Number of vertices in this polygon
+     */
     size_t len;
-    JPS_Triangle* triangles;
-};
-
-JUPEDSIM_API void JPS_TriangleMesh_Free(JPS_TriangleMesh* mesh);
+} JPS_Polygon_Desc;
 
-struct JPS_Line {
-    JPS_Point points[2];
-};
-
-struct JPS_Lines {
-    size_t len;
-    JPS_Line* lines;
-};
+/**
+ * Describes a polygonal mesh.
+ * Each polygon in the mesh is defined in CCW order.
+ * Each vertex is stored once and referred by index in each polygon.
+ */
+typedef struct JPS_Mesh {
+    /**
+     * Number of vertices in this mesh
+     */
+    size_t vertices_len;
+    /**
+     * The vertex data
+     */
+    JPS_Point* vertices;
+    /**
+     * Number of polygons in this mesh
+     */
+    size_t polygons_len;
+    /**
+     * Definitions of all polygons in this mesh
+     */
+    JPS_Polygon_Desc* polygons;
+    /**
+     * Array of all vertex indices used by the polygons.
+     * Length of this array is sum of all polygon indices.
+     * (Sum of all JPS_Polygon_Desc.len)
+     */
+    uint16_t* indices;
+} JPS_Mesh;
 
-JUPEDSIM_API void JPS_Lines_Free(JPS_Lines* lines);
+/**
+ * Frees the memory held by JPS_Mesh
+ */
+JUPEDSIM_API void JPS_Mesh_Free(JPS_Mesh mesh);
 
 /**
  * WARNING this is currently a NavMeshRoutingEngine! This does not account possible other types of
@@ -54,10 +95,7 @@ JPS_RoutingEngine_ComputeWaypoint(JPS_RoutingEngine handle, JPS_Point from, JPS_
 
 JUPEDSIM_API bool JPS_RoutingEngine_IsRoutable(JPS_RoutingEngine handle, JPS_Point p);
 
-JUPEDSIM_API JPS_TriangleMesh JPS_RoutingEngine_Mesh(JPS_RoutingEngine handle);
-
-/// Note: Currently the ID is the index of the vertex from 'JPS_RoutingEngine_Mesh'
-JUPEDSIM_API JPS_Lines JPS_RoutingEngine_EdgesFor(JPS_RoutingEngine handle, uint32_t id);
+JUPEDSIM_API JPS_Mesh JPS_RoutingEngine_Mesh(JPS_RoutingEngine handle);
 
 JUPEDSIM_API void JPS_RoutingEngine_Free(JPS_RoutingEngine handle);
 

libjupedsim/src/routing.cpp

@@ -16,31 +16,21 @@ using jupedsim::detail::intoTuple;
 ////////////////////////////////////////////////////////////////////////////////
 /// JPS_Path
 ////////////////////////////////////////////////////////////////////////////////
-JUPEDSIM_API void JPS_Path_Free(JPS_Path* path)
+JUPEDSIM_API void JPS_Path_Free(JPS_Path path)
 {
-    delete[] path->points;
-    path->points = nullptr;
-    path->len = 0;
+    delete[] path.points;
+    path.points = nullptr;
+    path.len = 0;
 }
 
 ////////////////////////////////////////////////////////////////////////////////
-/// JPS_TriangleMesh
+/// JPS_Mesh
 ////////////////////////////////////////////////////////////////////////////////
-JUPEDSIM_API void JPS_TriangleMesh_Free(JPS_TriangleMesh* mesh)
+JUPEDSIM_API void JPS_Mesh_Free(JPS_Mesh mesh)
 {
-    delete[] mesh->triangles;
-    mesh->triangles = nullptr;
-    mesh->len = 0;
-}
-
-////////////////////////////////////////////////////////////////////////////////
-/// JPS_Lines
-////////////////////////////////////////////////////////////////////////////////
-JUPEDSIM_API void JPS_Lines_Free(JPS_Lines* lines)
-{
-    delete[] lines->lines;
-    lines->lines = nullptr;
-    lines->len = 0;
+    delete[] mesh.indices;
+    delete[] mesh.polygons;
+    delete[] mesh.vertices;
 }
 
 ////////////////////////////////////////////////////////////////////////////////
@@ -70,36 +60,38 @@ JUPEDSIM_API bool JPS_RoutingEngine_IsRoutable(JPS_RoutingEngine handle, JPS_Poi
     return engine->IsRoutable(intoPoint(p));
 }
 
-JUPEDSIM_API JPS_TriangleMesh JPS_RoutingEngine_Mesh(JPS_RoutingEngine handle)
+JUPEDSIM_API JPS_Mesh JPS_RoutingEngine_Mesh(JPS_RoutingEngine handle)
 {
     const auto* engine = reinterpret_cast<NavMeshRoutingEngine*>(handle);
-    const auto res = engine->Mesh();
-    JPS_TriangleMesh mesh{res.size(), new JPS_Triangle[res.size()]};
-    std::transform(std::begin(res), std::end(res), mesh.triangles, [](const auto& t) {
-        auto tri = JPS_Triangle{};
-        tri.points[0] = intoJPS_Point(t.points[0]);
-        tri.points[1] = intoJPS_Point(t.points[1]);
-        tri.points[2] = intoJPS_Point(t.points[2]);
-        return tri;
-    });
-    return mesh;
-}
+    const auto mesh = engine->MeshData();
 
-JUPEDSIM_API JPS_Lines JPS_RoutingEngine_EdgesFor(JPS_RoutingEngine handle, uint32_t id)
-{
-    const auto* engine = reinterpret_cast<NavMeshRoutingEngine*>(handle);
-    const auto res = engine->EdgesFor(id);
+    JPS_Mesh result{};
+    result.vertices_len = mesh->CountVertices();
+    result.vertices = new JPS_Point[result.vertices_len];
+    for(size_t index = 0; index < result.vertices_len; ++index) {
+        const auto pt = mesh->Vertex(index);
+        result.vertices[index] = JPS_Point{pt.x, pt.y};
+    }
+    result.polygons_len = mesh->CountPolygons();
+    result.polygons = new JPS_Polygon_Desc[result.polygons_len];
+    size_t offset = 0;
+    for(size_t index = 0; index < result.polygons_len; ++index) {
+        const auto& polygon = mesh->Polygons(index);
+        const auto num_vertices = polygon.vertices.size();
+        result.polygons[index] = JPS_Polygon_Desc{offset, num_vertices};
+        offset += num_vertices;
+    }
+    result.indices = new uint16_t[offset];
+    auto index_ptr = result.indices;
+    for(size_t index = 0; index < result.polygons_len; ++index) {
+        const auto& polygon = mesh->Polygons(index);
+        for(const auto& v : polygon.vertices) {
+            *index_ptr = v;
+            ++index_ptr;
+        }
+    };
 
-    JPS_Lines lines{};
-    lines.lines = new JPS_Line[res.size()];
-    lines.len = res.size();
-    std::transform(std::begin(res), std::end(res), lines.lines, [](const auto& edge) {
-        JPS_Line line{};
-        line.points[0] = intoJPS_Point(edge.edge.p1);
-        line.points[1] = intoJPS_Point(edge.edge.p2);
-        return line;
-    });
-    return lines;
+    return result;
 }
 
 JUPEDSIM_API void JPS_RoutingEngine_Free(JPS_RoutingEngine handle)

libsimulator/CMakeLists.txt

@@ -6,10 +6,20 @@ add_library(simulator STATIC
     src/AABB.hpp
     src/AgentRemovalSystem.hpp
     src/Clonable.hpp
+    src/CollisionFreeSpeedModel.cpp
+    src/CollisionFreeSpeedModel.hpp
+    src/CollisionFreeSpeedModelBuilder.cpp
+    src/CollisionFreeSpeedModelBuilder.hpp
+    src/CollisionFreeSpeedModelData.hpp
+    src/CollisionFreeSpeedModelUpdate.hpp
+    src/CollisionFreeSpeedModelV2.cpp
+    src/CollisionFreeSpeedModelV2.hpp
+    src/CollisionFreeSpeedModelV2Builder.cpp
+    src/CollisionFreeSpeedModelV2Builder.hpp
+    src/CollisionFreeSpeedModelV2Data.hpp
+    src/CollisionFreeSpeedModelV2Update.hpp
     src/CollisionGeometry.cpp
     src/CollisionGeometry.hpp
-    src/DTriangulation.cpp
-    src/DTriangulation.hpp
     src/Ellipse.cpp
     src/Ellipse.hpp
     src/Enum.hpp
@@ -36,6 +46,8 @@ add_library(simulator STATIC
     src/Macros.hpp
     src/Mathematics.cpp
     src/Mathematics.hpp
+    src/Mesh.cpp
+    src/Mesh.hpp
     src/NeighborhoodSearch.hpp
     src/OperationalDecisionSystem.hpp
     src/OperationalModel.hpp
@@ -53,8 +65,8 @@ add_library(simulator STATIC
     src/SimulationError.hpp
     src/SocialForceModel.cpp
     src/SocialForceModel.hpp
-    src/SocialForceModelBuilder.hpp
     src/SocialForceModelBuilder.cpp
+    src/SocialForceModelBuilder.hpp
     src/SocialForceModelData.hpp
     src/SocialForceModelUpdate.hpp
     src/Stage.cpp
@@ -69,23 +81,8 @@ add_library(simulator STATIC
     src/TemplateHelper.hpp
     src/Tracing.cpp
     src/Tracing.hpp
-    src/Triangle.cpp
-    src/Triangle.hpp
     src/UniqueID.hpp
     src/Util.hpp
-    src/CollisionFreeSpeedModel.cpp
-    src/CollisionFreeSpeedModel.hpp
-    src/CollisionFreeSpeedModelBuilder.cpp
-    src/CollisionFreeSpeedModelBuilder.hpp
-    src/CollisionFreeSpeedModelData.hpp
-    src/CollisionFreeSpeedModelUpdate.hpp
-    src/CollisionFreeSpeedModelV2.cpp
-    src/CollisionFreeSpeedModelV2.hpp
-    src/CollisionFreeSpeedModelV2Builder.cpp
-    src/CollisionFreeSpeedModelV2Builder.hpp
-    src/CollisionFreeSpeedModelV2Data.hpp
-    src/CollisionFreeSpeedModelV2Update.hpp
-
 )
 target_compile_options(simulator PRIVATE
     ${COMMON_COMPILE_OPTIONS}
@@ -95,11 +92,10 @@ target_compile_definitions(simulator PUBLIC
 )
 target_link_libraries(simulator PUBLIC
     common
-    Boost::boost
-    poly2tri
     fmt::fmt
     CGAL::CGAL
     build_info
+    glm::glm
 )
 target_link_options(simulator PUBLIC
     $<$<AND:$<OR:$<CXX_COMPILER_ID:Clang>,$<CXX_COMPILER_ID:AppleClang>>,$<BOOL:${BUILD_WITH_ASAN}>>:-fsanitize=address>
@@ -121,6 +117,7 @@ if (BUILD_TESTS)
         test/TestGraph.cpp
         test/TestJourney.cpp
         test/TestLineSegment.cpp
+        test/TestMesh.cpp
         test/TestNeighborhoodSearch.cpp
         test/TestPoint.cpp
         test/TestSimulationClock.cpp

libsimulator/src/CfgCgal.hpp

@@ -0,0 +1,44 @@
+// Copyright © 2012-2024 Forschungszentrum Jülich GmbH
+// SPDX-License-Identifier: LGPL-3.0-or-later
+#pragma once
+
+#include <CGAL/Boolean_set_operations_2.h>
+#include <CGAL/Constrained_Delaunay_triangulation_2.h>
+#include <CGAL/Exact_predicates_exact_constructions_kernel.h>
+#include <CGAL/Polygon_with_holes_2.h>
+#include <CGAL/Surface_mesh.h>
+#include <CGAL/Triangulation_vertex_base_with_info_2.h>
+#include <CGAL/draw_triangulation_2.h>
+#include <CGAL/mark_domain_in_triangulation.h>
+
+#include <list>
+
+// using K = CGAL::Exact_predicates_exact_constructions_kernel;
+using K = CGAL::Simple_cartesian<double>;
+
+using Poly = CGAL::Polygon_2<K>;
+using PolyWithHoles = CGAL::Polygon_with_holes_2<K>;
+using PolyWithHolesList = std::list<PolyWithHoles>;
+using PolyList = std::list<Poly>;
+using Vb = CGAL::Triangulation_vertex_base_2<K>;
+
+template <class Gt, class Fb = CGAL::Constrained_triangulation_face_base_2<Gt>>
+class MyFace : public Fb
+{
+    bool in{false};
+    typedef Fb Base;
+    typedef typename Fb::Triangulation_data_structure TDS;
+
+public:
+    using Fb::Fb;
+    template <typename TDS2>
+    struct Rebind_TDS {
+        typedef typename Fb::template Rebind_TDS<TDS2>::Other Fb2;
+        typedef MyFace<Gt, Fb2> Other;
+    };
+    void set_in_domain(bool v) { in = v; }
+    bool get_in_domain() const { return in; }
+};
+using TDS = CGAL::Triangulation_data_structure_2<Vb, MyFace<K>>;
+using Itag = CGAL::Exact_predicates_tag;
+using CDT = CGAL::Constrained_Delaunay_triangulation_2<K, TDS, Itag>;

libsimulator/src/Clonable.hpp

@@ -9,4 +9,7 @@ class Clonable
 {
 public:
     virtual std::unique_ptr<T> Clone() const = 0;
+
+protected:
+    virtual ~Clonable() = default;
 };

libsimulator/src/CollisionGeometry.cpp

@@ -84,7 +84,7 @@ size_t CountLineSegments(const PolyWithHoles& poly)
     return count;
 }
 
-Point fromPoint_2(const Kernel::Point_2& p)
+Point fromPoint_2(const K::Point_2& p)
 {
     return {CGAL::to_double(p.x()), CGAL::to_double(p.y())};
 }
@@ -205,7 +205,7 @@ bool CollisionGeometry::IntersectsAny(const LineSegment& linesegment) const
 
 bool CollisionGeometry::InsideGeometry(Point p) const
 {
-    return CGAL::oriented_side(Kernel::Point_2(p.x, p.y), _accessibleAreaPolygon) !=
+    return CGAL::oriented_side(K::Point_2(p.x, p.y), _accessibleAreaPolygon) !=
            CGAL::ON_NEGATIVE_SIDE;
 }