Implement constant time lookup for wall segemtns

Walls are now stored in grid cells where all segmetns are part of that
are in approximate distance to this cell. This gives a constant time /
no allocation lookup for relevant line segments.

libsimulator/CMakeLists.txt

@@ -76,7 +76,7 @@ target_link_libraries(simulator PUBLIC
     CGAL::CGAL
     build_info
 )
-target_link_options(simulator PUBLIC 
+target_link_options(simulator PUBLIC
     $<$<AND:$<OR:$<CXX_COMPILER_ID:Clang>,$<CXX_COMPILER_ID:AppleClang>>,$<BOOL:${BUILD_WITH_ASAN}>>:-fsanitize=address>
 )
 target_include_directories(simulator PUBLIC
@@ -121,6 +121,8 @@ if (BUILD_BENCHMARKS)
     add_executable(libsimulator-benchmarks
         benchmark/BenchmarkMain.cpp
         benchmark/benchmarkLineSegment.hpp
+        benchmark/benchmarkCollisionGeometry.hpp
+        benchmark/buildGeometries.hpp
     )
 
     target_link_libraries(libsimulator-benchmarks PRIVATE

libsimulator/benchmark/BenchmarkMain.cpp

@@ -2,6 +2,7 @@
 /// SPDX-License-Identifier: LGPL-3.0-or-later
 #include <benchmark/benchmark.h>
 
+#include "benchmarkCollisionGeometry.hpp"
 #include "benchmarkLineSegment.hpp"
 
 BENCHMARK_MAIN();

libsimulator/benchmark/benchmarkCollisionGeometry.hpp

@@ -0,0 +1,45 @@
+// Copyright © 2012-2023 Forschungszentrum Jülich GmbH
+// SPDX-License-Identifier: LGPL-3.0-or-later
+
+#include <benchmark/benchmark.h>
+
+#include "CollisionGeometry.hpp"
+#include "GeometryBuilder.hpp"
+#include "LineSegment.hpp"
+#include "buildGeometries.hpp"
+
+template <class... Args>
+void bmLineSegmentsInDistanceTo(benchmark::State& state, Args&&... args)
+{
+    auto args_tuple = std::make_tuple(std::move(args)...);
+    auto geometry = std::move(std::get<Geometry>(args_tuple));
+
+    for(auto _ : state) {
+        benchmark::DoNotOptimize(geometry.collisionGeometry->LineSegmentsInDistanceTo(5., {0, 0}));
+        benchmark::ClobberMemory();
+    }
+}
+
+template <class... Args>
+void bmLineSegmentsInApproxDistanceTo(benchmark::State& state, Args&&... args)
+{
+    auto args_tuple = std::make_tuple(std::move(args)...);
+    auto geometry = std::move(std::get<Geometry>(args_tuple));
+
+    for(auto _ : state) {
+        benchmark::DoNotOptimize(
+            geometry.collisionGeometry->LineSegmentsInApproxDistanceTo({0, 0}));
+        benchmark::ClobberMemory();
+    }
+}
+
+BENCHMARK_CAPTURE(bmLineSegmentsInDistanceTo, large_street_network, buildLargeStreetNetwork());
+
+BENCHMARK_CAPTURE(bmLineSegmentsInDistanceTo, grosser_stern, buildGrosserStern());
+
+BENCHMARK_CAPTURE(
+    bmLineSegmentsInApproxDistanceTo,
+    large_street_network,
+    buildLargeStreetNetwork());
+
+BENCHMARK_CAPTURE(bmLineSegmentsInApproxDistanceTo, grosser_stern, buildGrosserStern());

libsimulator/src/AABB.cpp

@@ -1,3 +1,33 @@
 /// Copyright © 2012-2022 Forschungszentrum Jülich GmbH
 /// SPDX-License-Identifier: LGPL-3.0-or-later
 #include "AABB.hpp"
+
+static bool intersectsLine(const AABB& boundingBox, const LineSegment& lineSegment)
+{
+    const Point base = lineSegment.p1;
+    const Point dir = lineSegment.p2 - lineSegment.p1;
+    const Point n = Point{dir.y, -dir.x};
+
+    const Point c1 = boundingBox.BottomLeft() - base;
+    const Point c2 = boundingBox.TopRight() - base;
+    const Point c3 = boundingBox.BottomRight() - base;
+    const Point c4 = boundingBox.TopLeft() - base;
+
+    const double dp1 = n.ScalarProduct(c1);
+    const double dp2 = n.ScalarProduct(c2);
+    const double dp3 = n.ScalarProduct(c3);
+    const double dp4 = n.ScalarProduct(c4);
+
+    return (dp1 * dp2 <= 0.) || (dp2 * dp3 <= 0.) || (dp3 * dp4 <= 0.);
+}
+
+bool AABB::Intersects(const LineSegment& lineSegment) const
+{
+    if(!intersectsLine(*this, lineSegment)) {
+        return false;
+    }
+
+    const AABB bbLineSegment({lineSegment.p1, lineSegment.p2});
+
+    return this->Overlap(bbLineSegment);
+}

libsimulator/src/AABB.hpp

@@ -3,6 +3,7 @@
 #pragma once
 
 #include "IteratorPair.hpp"
+#include "LineSegment.hpp"
 #include "Point.hpp"
 #include "SimulationError.hpp"
 
@@ -44,4 +45,11 @@ struct AABB {
     {
         return !(xmax < other.xmin || xmin > other.xmax || ymax < other.ymin || ymin > other.ymax);
     };
+
+    bool Intersects(const LineSegment& lineSegment) const;
+
+    Point TopLeft() const { return Point{xmin, ymax}; };
+    Point TopRight() const { return Point{xmax, ymax}; };
+    Point BottomLeft() const { return Point{xmin, ymin}; };
+    Point BottomRight() const { return Point{xmax, ymin}; };
 };

libsimulator/src/CollisionGeometry.cpp

@@ -110,6 +110,50 @@ CollisionGeometry::CollisionGeometry(PolyWithHoles accessibleArea) : _accessible
         for(const auto& cell : cells) {
             _grid[cell].insert(ls);
         }
+
+        insertIntoApproximateGrid(ls);
+    }
+
+    for(auto& [_, vec] : _approximateGrid) {
+        vec.shrink_to_fit();
+    }
+}
+
+const std::vector<LineSegment>& CollisionGeometry::LineSegmentsInApproxDistanceTo(Point p) const
+{
+    const auto cell = makeCell(p);
+    if(const auto it = _approximateGrid.find(cell); it != _approximateGrid.end()) {
+        return it->second;
+    }
+    static const std::vector<LineSegment> empty{};
+    return empty;
+}
+
+void CollisionGeometry::insertIntoApproximateGrid(const LineSegment& ls)
+{
+    constexpr double searchRadius = 4.;
+
+    const auto searchExtend = Point(searchRadius, searchRadius);
+    const AABB lineSegmentBounds({ls.p1, ls.p2});
+    const AABB searchBounds(
+        lineSegmentBounds.BottomLeft() - searchExtend, lineSegmentBounds.TopRight() + searchExtend);
+
+    auto cellBottomLeft = makeCell(searchBounds.BottomLeft());
+    auto cellTopRight = makeCell(searchBounds.TopRight());
+
+    for(double x = cellBottomLeft.x; x <= cellTopRight.x; x += CELL_EXTEND) {
+        for(double y = cellBottomLeft.y; y <= cellTopRight.y; y += CELL_EXTEND) {
+            const auto cell = makeCell({x, y});
+
+            const AABB bbWithSearchRadius(
+                {cell.x - searchRadius, cell.y - searchRadius},
+                {cell.x + searchRadius + CELL_EXTEND, cell.y + searchRadius + CELL_EXTEND});
+
+            if(bbWithSearchRadius.Intersects(ls)) {
+                auto& vec = _approximateGrid[cell];
+                vec.push_back(ls);
+            }
+        }
     }
 }
 
@@ -121,7 +165,7 @@ CollisionGeometry::LineSegmentsInDistanceTo(double distance, Point p) const
         DistanceQueryIterator<LineSegment>{distance, p, _segments.cend(), _segments.cend()}};
 }
 
-bool CollisionGeometry::IntersectsAny(LineSegment linesegment) const
+bool CollisionGeometry::IntersectsAny(const LineSegment& linesegment) const
 {
     const auto cellsToQuery = cellsFromLineSegment(linesegment);
     for(const auto& cell : cellsToQuery) {

libsimulator/src/CollisionGeometry.hpp

@@ -6,6 +6,9 @@
 #include "IteratorPair.hpp"
 #include "LineSegment.hpp"
 
+#include <map>
+#include <set>
+#include <unordered_map>
 #include <vector>
 
 #include <CGAL/Exact_predicates_exact_constructions_kernel.h>
@@ -95,6 +98,7 @@ class CollisionGeometry
     PolyWithHoles _accessibleArea;
     std::vector<LineSegment> _segments;
     std::unordered_map<Cell, std::set<LineSegment>> _grid{};
+    std::unordered_map<Cell, std::vector<LineSegment>> _approximateGrid{};
 
 public:
     using LineSegmentRange = IteratorPair<DistanceQueryIterator<LineSegment>>;
@@ -116,11 +120,17 @@ class CollisionGeometry
     /// @param p reference point
     /// @return iterator_pair to all linesegments in range
     LineSegmentRange LineSegmentsInDistanceTo(double distance, Point p) const;
+
+    const std::vector<LineSegment>& LineSegmentsInApproxDistanceTo(Point p) const;
+
     /// Will perfrom a linesegment intersection versus the whole geometry, i.e. walls and closed
     /// doors.
     /// @param linesegment to test for intersection with geometry
     /// @return if any linesegment of the geometry was intersected.
-    bool IntersectsAny(LineSegment linesegment) const;
+    bool IntersectsAny(const LineSegment& linesegment) const;
 
     bool InsideGeometry(Point p) const;
+
+private:
+    void insertIntoApproximateGrid(const LineSegment& ls);
 };

libsimulator/src/GCFMModel.cpp

@@ -47,6 +47,7 @@ PedestrianUpdate GCFMModel::ComputeNewPosition(
     const auto p1 = agent.pos;
     Point F_rep;
     for(const auto neighbor : neighborhood) {
+        // TODO: Only use neighbors who have an unobstructed line of sight to the current agent
         if(neighbor->id == agent.id) {
             continue;
         }
@@ -241,11 +242,11 @@ Point GCFMModel::ForceRepPed(const Data& ped1, const Data& ped2) const
 
 inline Point GCFMModel::ForceRepRoom(const Data& ped, const CollisionGeometry& geometry) const
 {
-    auto walls = geometry.LineSegmentsInDistanceTo(5.0, ped.pos);
+    const auto& walls = geometry.LineSegmentsInApproxDistanceTo(ped.pos);
 
     auto f = std::accumulate(
-        walls.begin(),
-        walls.end(),
+        walls.cbegin(),
+        walls.cend(),
         Point(0, 0),
         [this, &ped](const auto& acc, const auto& element) {
             return acc + ForceRepWall(ped, element);

libsimulator/src/GeometricFunctions.hpp

@@ -83,7 +83,7 @@ inline bool intersectsWithCGAL(const LineSegment l1, const LineSegment l2)
     return CGAL::do_intersect(this_segment, other_segment);
 }
 
-inline bool intersects(const LineSegment l1, const LineSegment l2)
+inline bool intersects(const LineSegment& l1, const LineSegment& l2)
 {
     return intersects(l1.p1, l1.p2, l2.p1, l2.p2);
 }

libsimulator/src/VelocityModel.cpp

@@ -3,6 +3,7 @@
 #include "VelocityModel.hpp"
 
 #include "GenericAgent.hpp"
+#include "GeometricFunctions.hpp"
 #include "Macros.hpp"
 #include "Mathematics.hpp"
 #include "NeighborhoodSearch.hpp"
@@ -37,18 +38,27 @@ PedestrianUpdate VelocityModel::ComputeNewPosition(
 {
     const double radius = 4.0;
     auto neighborhood = neighborhoodSearch.GetNeighboringAgents(ped.pos, radius);
-    // Remove any agent from the neighborhood that is obstructed by geometry and the current agent
+    const auto& walls = geometry.LineSegmentsInApproxDistanceTo(ped.pos);
+
+    // Remove any agent from the neighborhood that is obstructed by geometry and the current
+    // agent
     neighborhood.erase(
         std::remove_if(
             std::begin(neighborhood),
             std::end(neighborhood),
-            [&ped, &geometry](const auto& n) {
+            [&ped, &walls](const auto& n) {
                 if(ped.id == n->id) {
                     return true;
                 }
-                if(geometry.IntersectsAny(LineSegment(ped.pos, n->pos))) {
+                const auto lineSegment = LineSegment(ped.pos, n->pos);
+
+                if(std::find_if(
+                       walls.cbegin(), walls.cend(), [&lineSegment](const auto& candidate) {
+                           return intersects(lineSegment, candidate);
+                       }) != walls.end()) {
                     return true;
                 }
+
                 return false;
             }),
         std::end(neighborhood));
@@ -186,11 +196,11 @@ Point VelocityModel::ForceRepPed(const Data& ped1, const Data& ped2) const
 
 Point VelocityModel::ForceRepRoom(const Data& ped, const CollisionGeometry& geometry) const
 {
-    auto walls = geometry.LineSegmentsInDistanceTo(5.0, ped.pos);
+    const auto& walls = geometry.LineSegmentsInApproxDistanceTo(ped.pos);
 
     auto f = std::accumulate(
-        walls.begin(),
-        walls.end(),
+        walls.cbegin(),
+        walls.cend(),
         Point(0, 0),
         [this, &ped](const auto& acc, const auto& element) {
             return acc + ForceRepWall(ped, element);