diff --git a/navmap_core/bench/navmap_bench.cpp b/navmap_core/bench/navmap_bench.cpp index 706565e..236bdbd 100644 --- a/navmap_core/bench/navmap_bench.cpp +++ b/navmap_core/bench/navmap_bench.cpp @@ -190,44 +190,58 @@ int main(int argc, char ** argv) const double us = median_us(R, n, body); std::snprintf(col[ci++], sizeof(col[0]), "%.1f (%zu)", us, cells / (R * n)); }; - run(M, [&](int k) { - navmap::for_each_in_disc(nm, seeds[k], pts[k], 0.5f, count, - navmap::Metric::EUCLIDEAN, &sc); - }); - run(M / 10, [&](int k) { - navmap::for_each_in_disc(nm, seeds[k], pts[k], 2.0f, count, - navmap::Metric::EUCLIDEAN, &sc); - }); - run(M, [&](int k) { - navmap::for_each_in_disc(nm, seeds[k], pts[k], 0.5f, count, - navmap::Metric::GEODESIC, &sc); - }); - run(M / 10, [&](int k) { - navmap::for_each_in_disc(nm, seeds[k], pts[k], 2.0f, count, - navmap::Metric::GEODESIC, &sc); - }); - run(M, [&](int k) { + run( + M, [&](int k) { + navmap::for_each_in_disc( + nm, seeds[k], pts[k], 0.5f, count, + navmap::Metric::EUCLIDEAN, &sc); + }); + run( + M / 10, [&](int k) { + navmap::for_each_in_disc( + nm, seeds[k], pts[k], 2.0f, count, + navmap::Metric::EUCLIDEAN, &sc); + }); + run( + M, [&](int k) { + navmap::for_each_in_disc( + nm, seeds[k], pts[k], 0.5f, count, + navmap::Metric::GEODESIC, &sc); + }); + run( + M / 10, [&](int k) { + navmap::for_each_in_disc( + nm, seeds[k], pts[k], 2.0f, count, + navmap::Metric::GEODESIC, &sc); + }); + run( + M, [&](int k) { navmap::AABB b; b.expand(pts[k] - Eigen::Vector3f(1, 1, 1)); b.expand(pts[k] + Eigen::Vector3f(1, 1, 1)); navmap::for_each_in_aabb(nm, b, count, &sc); }); - run(M, [&](int k) { + run( + M, [&](int k) { const float x = pts[k].x(), y = pts[k].y(); navmap::for_each_in_polygon( nm, seeds[k], {{x - 0.5f, y - 0.5f}, {x + 0.5f, y - 0.5f}, {x + 0.5f, y + 0.5f}, {x - 0.5f, y + 0.5f}}, count, &sc); }); - run(M, [&](int k) { - navmap::for_each_on_segment(nm, seeds[k], pts[k], - pts[(k + 1) % M].head<2>().isZero() ? pts[k] : - Eigen::Vector3f(pts[k] + 5.0f * (pts[(k + 1) % M] - pts[k]).normalized()), - [&](NavCelId, float, float) {++cells;}); + run( + M, [&](int k) { + navmap::for_each_on_segment( + nm, seeds[k], pts[k], + pts[(k + 1) % M].head<2>().isZero() ? pts[k] : + Eigen::Vector3f(pts[k] + 5.0f * (pts[(k + 1) % M] - pts[k]).normalized()), + [&](NavCelId, float, float) {++cells;}); + }); + run( + M / 10, [&](int k) { + navmap::for_each_by_distance( + nm, seeds[k], pts[k], 2.0f, + [&](NavCelId, float) {++cells;}, &sc); }); - run(M / 10, [&](int k) { - navmap::for_each_by_distance(nm, seeds[k], pts[k], 2.0f, - [&](NavCelId, float) {++cells;}, &sc); - }); std::printf("%9zu", nm.navcels.size()); for (auto & c : col) { std::printf(" %16s", c); diff --git a/navmap_core/include/navmap_core/Iterators.hpp b/navmap_core/include/navmap_core/Iterators.hpp index 62aafbf..3dc1b25 100644 --- a/navmap_core/include/navmap_core/Iterators.hpp +++ b/navmap_core/include/navmap_core/Iterators.hpp @@ -265,8 +265,8 @@ std::size_t grow( sc.heap.push_back(e); std::push_heap(sc.heap.begin(), sc.heap.end(), cmp); }; - auto visible = [&](NavCelId from, const Eigen::Vector3f & p, const Eigen::Vector3f & q, - NavCelId target) { + using V3 = Eigen::Vector3f; + auto visible = [&](NavCelId from, const V3 & p, const V3 & q, NavCelId target) { NavCelId last = kNoCel; const bool reached = for_each_on_segment_impl( nm, from, p, q, [&](NavCelId x, float, float) {last = x;}); diff --git a/navmap_core/tests/test_adjacency.cpp b/navmap_core/tests/test_adjacency.cpp index de454c0..3d62072 100644 --- a/navmap_core/tests/test_adjacency.cpp +++ b/navmap_core/tests/test_adjacency.cpp @@ -94,7 +94,7 @@ TEST(NavMap_Adjacency, OnlyTheNonManifoldEdgeIsAffected) const NavCelId t3 = nm.add_navcel(b, f, c); // regular neighbor of t0 on b-c for (auto t : {t0, t1, t2, t3}) { nm.add_navcel_to_surface(s, t); - } + } nm.rebuild_geometry_accels(); EXPECT_EQ(nm.nonmanifold_edges(), 1u); @@ -159,7 +159,7 @@ TEST(NavMap_Adjacency, LargeGridIsManifoldAndSymmetric) std::size_t links = 0; for (NavCelId cid = 0; cid < nm.navcels.size(); ++cid) { links += nm.navcel_neighbors(cid).size(); - } + } // Interior edges: 3 n^2 - 2 n, each linked from both sides. EXPECT_EQ(links, 2u * (3u * n * n - 2u * n)); } diff --git a/navmap_core/tests/test_combine.cpp b/navmap_core/tests/test_combine.cpp index bf04616..3a19dd1 100644 --- a/navmap_core/tests/test_combine.cpp +++ b/navmap_core/tests/test_combine.cpp @@ -48,8 +48,9 @@ NavMap strip(std::size_t n) } for (std::size_t k = 0; k < n; ++k) { const uint32_t i = static_cast(k / 2) * 2; - const NavCelId c = (k % 2 == 0) ? nm.add_navcel(i, i + 2, i + 3) : nm.add_navcel(i, i + 3, - i + 1); + const NavCelId c = (k % 2 == 0) ? nm.add_navcel(i, i + 2, i + 3) : nm.add_navcel( + i, i + 3, + i + 1); nm.add_navcel_to_surface(s, c); } nm.rebuild_geometry_accels(); @@ -148,7 +149,8 @@ TEST(Combine, OnlyTheGivenCells) { CombineOptions o; o.cells = {1, 3, 99}; // out of range ids are ignored - EXPECT_EQ(cost({0, 0, 0, 0}, {9, 9, 9, 9}, CombineOp::OVERWRITE, o), + EXPECT_EQ( + cost({0, 0, 0, 0}, {9, 9, 9, 9}, CombineOp::OVERWRITE, o), (std::vector{0, 9, 0, 9})); } diff --git a/navmap_core/tests/test_distance_field.cpp b/navmap_core/tests/test_distance_field.cpp index 497a9c0..91b7679 100644 --- a/navmap_core/tests/test_distance_field.cpp +++ b/navmap_core/tests/test_distance_field.cpp @@ -125,7 +125,7 @@ TEST(NavMap_DistanceField, SeedCellsGiveStraightLineDistance) } for (NavCelId s : seeds) { EXPECT_FLOAT_EQ(d[s], 0.0f); - } + } // Seeds and boundary together: the nearest of both. const auto both = nm.distance_field(seeds, true); @@ -162,10 +162,10 @@ TEST(NavMap_DistanceField, StackedFloorsDoNotSeeEachOther) const auto d = nm.distance_field({low}, false); for (auto c : nm.surfaces[1].navcels) { EXPECT_EQ(d[c], kInf); - } + } for (auto c : nm.surfaces[0].navcels) { EXPECT_LT(d[c], kInf); - } + } // Boundary of the upper floor only reaches the upper floor's own cells. const NavCelId high = cell_near(nm, 2.6f, 2.6f, 2.5f); @@ -199,7 +199,7 @@ TEST(NavMap_DistanceField, InteriorHoleEdgesAreBoundary) const auto ref = brute(nm, {}, true); for (NavCelId c = 0; c < nm.navcels.size(); ++c) { EXPECT_NEAR(d[c], ref[c], 1e-4f) << c; - } + } // Next to the 1x1 m hole (x, y in [4.5, 5.5]). EXPECT_LT(d[cell_near(nm, 4.2f, 5.0f, 0.0f)], 0.31f); } @@ -244,11 +244,11 @@ TEST(NavMap_DistanceField, DegenerateInputs) nm.rebuild_geometry_accels(); for (float v : nm.distance_field({}, false)) { EXPECT_EQ(v, kInf); - } + } const auto d = nm.distance_field({9999}, false); // out of range: ignored for (float v : d) { EXPECT_EQ(v, kInf); - } + } const auto z = nm.distance_field({0}, false, 0.0f); // only the seed itself EXPECT_FLOAT_EQ(z[0], 0.0f); EXPECT_EQ(std::count_if(z.begin(), z.end(), [](float v) {return v < kInf;}), 1); diff --git a/navmap_core/tests/test_geometry.cpp b/navmap_core/tests/test_geometry.cpp index 9504d8b..aefeb17 100644 --- a/navmap_core/tests/test_geometry.cpp +++ b/navmap_core/tests/test_geometry.cpp @@ -156,15 +156,18 @@ TEST(Geometry_RayTriangle, RaysThroughASharedEdgeAlwaysHit) { for (const float off : {0.0f, 5.0f, 100.0f, 1000.0f}) { for (const float size : {0.1f, 3.0f, 30.0f}) { const Vec3 a(off, off, 4.7f), b(off + size, off - 0.3f * size, 4.7f); - const Vec3 c(off + 0.4f * size, off + 0.9f * size, 4.701f), d(off + 1.3f * size, off + size, + const Vec3 c(off + 0.4f * size, off + 0.9f * size, 4.701f), d( + off + 1.3f * size, off + size, 4.702f); for (int i = 1; i < 500; ++i) { const float s = i / 500.0f; const Vec3 p = b + s * (c - b); // on the shared edge b-c float t, u, v; - const bool h1 = navmap::ray_triangle_intersect(p + Vec3(0, 0, 1), Vec3(0, 0, -1), a, b, c, + const bool h1 = navmap::ray_triangle_intersect( + p + Vec3(0, 0, 1), Vec3(0, 0, -1), a, b, c, t, u, v); - const bool h2 = navmap::ray_triangle_intersect(p + Vec3(0, 0, 1), Vec3(0, 0, -1), b, d, c, + const bool h2 = navmap::ray_triangle_intersect( + p + Vec3(0, 0, 1), Vec3(0, 0, -1), b, d, c, t, u, v); ASSERT_TRUE(h1 || h2) << "offset " << off << " size " << size << " s " << s; } diff --git a/navmap_core/tests/test_incremental.cpp b/navmap_core/tests/test_incremental.cpp index 67ccaca..c6e01d0 100644 --- a/navmap_core/tests/test_incremental.cpp +++ b/navmap_core/tests/test_incremental.cpp @@ -61,10 +61,11 @@ void add_grid( std::vector maps() { std::vector out(3); - add_grid(out[0], 20, 20, 0.5f, [](float, float) {return 0.0f;}, + add_grid( + out[0], 20, 20, 0.5f, [](float, float) {return 0.0f;}, [](int i, int j) { return (i == 8 && j > 3 && j < 15) || (i > 12 && i < 15 && j > 12 && j < 15); - }); + }); add_grid(out[1], 16, 16, 0.5f, [](float, float) {return 0.0f;}); add_grid(out[1], 16, 16, 0.5f, [](float, float) {return 3.0f;}); add_grid(out[2], 30, 6, 0.4f, [](float x, float) {return 0.25f * x;}); @@ -106,7 +107,8 @@ std::vector brute( for (NavCelId c = 0; c < nm.navcels.size(); ++c) { for (int e = 0; e < 3; ++e) { if (nm.navcels[c].neighbor[e] != std::numeric_limits::max()) {continue;} - all.push_back({nm.positions.at(nm.navcels[c].v[e]), + all.push_back( + {nm.positions.at(nm.navcels[c].v[e]), nm.positions.at(nm.navcels[c].v[(e + 1) % 3]), c}); } } @@ -240,7 +242,7 @@ TEST(DistanceFieldIncremental, AddingAndRemovingTheSameCellAndEdgeIds) df.update({}, {5}); for (float d : df.distances()) { EXPECT_TRUE(std::isinf(d)); - } + } // A seed on the upper floor never reaches the lower one NavCelId upper = 0; for (NavCelId c = 0; c < nm.navcels.size(); ++c) { diff --git a/navmap_core/tests/test_iterators.cpp b/navmap_core/tests/test_iterators.cpp index b58b8f6..b68a42d 100644 --- a/navmap_core/tests/test_iterators.cpp +++ b/navmap_core/tests/test_iterators.cpp @@ -233,21 +233,25 @@ TEST(Iterators, DiscEdgeValues) const NavCelId seed = seed_at(nm, c); for (const Metric m : {Metric::EUCLIDEAN, Metric::GEODESIC}) { // Radius 0, or below the seed's centroid: just the seed - EXPECT_EQ(collect([&](auto f) {return navmap::for_each_in_disc(nm, seed, c, 0.0f, f, m);}), + EXPECT_EQ( + collect([&](auto f) {return navmap::for_each_in_disc(nm, seed, c, 0.0f, f, m);}), std::set{seed}); // Negative or NaN radius, bad seed: nothing EXPECT_EQ(navmap::for_each_in_disc(nm, seed, c, -1.0f, [](NavCelId) {}, m), 0u); EXPECT_EQ(navmap::for_each_in_disc(nm, seed, c, NAN, [](NavCelId) {}, m), 0u); EXPECT_EQ(navmap::for_each_in_disc(nm, NavCelId{100000}, c, 1.0f, [](NavCelId) {}, m), 0u); // Off the map: nothing - EXPECT_EQ(navmap::for_each_in_disc(nm, Eigen::Vector3f(50, 50, 0), 3.0f, [](NavCelId) {}, m), + EXPECT_EQ( + navmap::for_each_in_disc(nm, Eigen::Vector3f(50, 50, 0), 3.0f, [](NavCelId) {}, m), 0u); // Early stop int calls = 0; - EXPECT_EQ(navmap::for_each_in_disc(nm, seed, c, 3.0f, [&](NavCelId) {return ++calls < 5;}, m), + EXPECT_EQ( + navmap::for_each_in_disc(nm, seed, c, 3.0f, [&](NavCelId) {return ++calls < 5;}, m), 5u); } - EXPECT_EQ(navmap::for_each_in_disc(NavMap{}, Eigen::Vector3f(0, 0, 0), 1.0f, [](NavCelId) {}), + EXPECT_EQ( + navmap::for_each_in_disc(NavMap{}, Eigen::Vector3f(0, 0, 0), 1.0f, [](NavCelId) {}), 0u); } @@ -276,7 +280,7 @@ TEST(Iterators, GeodesicDiscDoesNotCrossAWallThatEuclideanSees) EXPECT_FALSE(g_far.count(b)); for (NavCelId x : g_far) { EXPECT_TRUE(e_far.count(x)) << x; - } // geodesic >= Euclidean + } // geodesic >= Euclidean } TEST(Iterators, GeodesicDistanceAroundAWallIsTheShortestDetour) @@ -289,7 +293,8 @@ TEST(Iterators, GeodesicDistanceAroundAWallIsTheShortestDetour) const Eigen::Vector3f c(4.75f, 2.0f, 0.0f); const NavCelId seed = seed_at(nm, c), b = seed_at(nm, {5.75f, 2.0f, 0.0f}); float db = -1.0f; - navmap::for_each_by_distance(nm, seed, c, 30.0f, [&](NavCelId x, float d) { + navmap::for_each_by_distance( + nm, seed, c, 30.0f, [&](NavCelId x, float d) { if (x == b) {db = d;} }); const Eigen::Vector3f pb = nm.navcel_centroid(b); @@ -308,7 +313,8 @@ TEST(Iterators, RingIsTheDiscMinusItsInnerPartAndCrossesIt) const auto ring = collect( [&](auto f) {return navmap::for_each_in_ring(nm, seed, c, 2.0f, 3.0f, f, m);}); std::set want; - navmap::for_each_by_distance(nm, seed, c, 3.0f, [&](NavCelId x, float d) { + navmap::for_each_by_distance( + nm, seed, c, 3.0f, [&](NavCelId x, float d) { if (m == Metric::GEODESIC ? d >= 2.0f : (nm.navcel_centroid(x) - c).norm() >= 2.0f) { want.insert(x); } @@ -336,9 +342,10 @@ TEST(Iterators, ByDistanceIsOrderedCompleteAndNeverShorterThanAStraightLine) const Eigen::Vector3f c(3.3f, 6.1f, 0.0f); const NavCelId seed = seed_at(nm, c); std::vector> seq; - const std::size_t n = navmap::for_each_by_distance(nm, seed, c, 4.0f, [&](NavCelId x, float d) { - seq.emplace_back(x, d); - }); + const std::size_t n = navmap::for_each_by_distance( + nm, seed, c, 4.0f, [&](NavCelId x, float d) { + seq.emplace_back(x, d); + }); ASSERT_EQ(n, seq.size()); ASSERT_FALSE(seq.empty()); EXPECT_EQ(seq.front().first, seed); @@ -362,9 +369,11 @@ TEST(Iterators, ByDistanceIsOrderedCompleteAndNeverShorterThanAStraightLine) if ((nm.navcel_centroid(x) - c).norm() <= 3.0f) {EXPECT_TRUE(ids.count(x)) << x;} } int calls = 0; - EXPECT_EQ(navmap::for_each_by_distance(nm, seed, c, 4.0f, [&](NavCelId, float) { - return ++calls < 3; - }), 3u); + EXPECT_EQ( + navmap::for_each_by_distance( + nm, seed, c, 4.0f, [&](NavCelId, float) { + return ++calls < 3; + }), 3u); } TEST(Iterators, ByDistanceOnASlopeMeasuresAlongTheSurface) @@ -377,7 +386,8 @@ TEST(Iterators, ByDistanceOnASlopeMeasuresAlongTheSurface) const Eigen::Vector3f c(1.0f, 0.5f, k * 1.0f), far(5.0f, 0.5f, k * 5.0f); const NavCelId seed = seed_at(nm, c), target = seed_at(nm, far); float d_target = -1.0f; - navmap::for_each_by_distance(nm, seed, c, 10.0f, [&](NavCelId x, float d) { + navmap::for_each_by_distance( + nm, seed, c, 10.0f, [&](NavCelId x, float d) { if (x == target) {d_target = d;} }); const float straight = (nm.navcel_centroid(target) - c).norm(); @@ -414,9 +424,10 @@ TEST(Iterators, PolygonStaysOnTheSeedLevelAndHandlesDegenerateInput) const NavMap nm = two_floors(); const std::vector sq{{2, 2}, {6, 2}, {6, 6}, {2, 6}}; for (const float z : {0.0f, 3.0f}) { - const auto got = collect([&](auto f) { - return navmap::for_each_in_polygon(nm, Eigen::Vector3f(4, 4, z), sq, f); - }); + const auto got = collect( + [&](auto f) { + return navmap::for_each_in_polygon(nm, Eigen::Vector3f(4, 4, z), sq, f); + }); EXPECT_EQ(got.size(), 2u * 8u * 8u); for (NavCelId x : got) { EXPECT_NEAR(nm.navcel_centroid(x).z(), z, 1e-5f); @@ -426,9 +437,11 @@ TEST(Iterators, PolygonStaysOnTheSeedLevelAndHandlesDegenerateInput) EXPECT_EQ(navmap::for_each_in_polygon(nm, seed, {{0, 0}, {1, 1}}, [](NavCelId) {}), 0u); EXPECT_EQ(navmap::for_each_in_polygon(nm, seed, {}, [](NavCelId) {}), 0u); // A polygon away from the seed is not reached (regions grow from the seed) - EXPECT_EQ(navmap::for_each_in_polygon( + EXPECT_EQ( + navmap::for_each_in_polygon( nm, seed, {{8, 8}, {9, 8}, {9, 9}, {8, 9}}, [](NavCelId) {}), 0u); - EXPECT_EQ(navmap::for_each_in_polygon( + EXPECT_EQ( + navmap::for_each_in_polygon( nm, Eigen::Vector3f(50, 50, 0), sq, [](NavCelId) {}), 0u); int calls = 0; EXPECT_EQ(navmap::for_each_in_polygon(nm, seed, sq, [&](NavCelId) {return ++calls < 4;}), 4u); @@ -448,9 +461,10 @@ struct Walk Walk walk(const NavMap & nm, const Eigen::Vector3f & a, const Eigen::Vector3f & b) { Walk w; - w.reached = navmap::for_each_on_segment(nm, a, b, [&](NavCelId c, float t0, float t1) { - w.cells.emplace_back(c, t0, t1); - }); + w.reached = navmap::for_each_on_segment( + nm, a, b, [&](NavCelId c, float t0, float t1) { + w.cells.emplace_back(c, t0, t1); + }); return w; } @@ -537,8 +551,10 @@ TEST(Iterators, SegmentWalkStopsAtBordersAndHoles) EXPECT_FALSE(walk(nm, {-3, 1, 0}, {2, 1, 0}).reached); // Early stop int calls = 0; - EXPECT_FALSE(navmap::for_each_on_segment(nm, Eigen::Vector3f(1, 1, 0), Eigen::Vector3f(9, 1, 0), - [&](NavCelId, float, float) {return ++calls < 3;})); + EXPECT_FALSE( + navmap::for_each_on_segment( + nm, Eigen::Vector3f(1, 1, 0), Eigen::Vector3f(9, 1, 0), + [&](NavCelId, float, float) {return ++calls < 3;})); EXPECT_EQ(calls, 3); } diff --git a/navmap_core/tests/test_layers_unknown_occupancy.cpp b/navmap_core/tests/test_layers_unknown_occupancy.cpp index 8e2333d..acb2a1a 100644 --- a/navmap_core/tests/test_layers_unknown_occupancy.cpp +++ b/navmap_core/tests/test_layers_unknown_occupancy.cpp @@ -187,7 +187,7 @@ TEST(Occupancy, SequencesClampAndClassify) // Saturation both ways, and recovery from saturation takes a bounded number of updates for (int k = 0; k < 100; ++k) { occ.hit(2); - } + } EXPECT_EQ(nm.layer_get(occ.name(), 2), 3476); EXPECT_NEAR(occ.probability(2), 0.97f, 1e-3f); int misses = 0; @@ -195,7 +195,7 @@ TEST(Occupancy, SequencesClampAndClassify) EXPECT_EQ(misses, (3476 + 405 + 404) / 405); // from the top to <= -405 for (int k = 0; k < 100; ++k) { occ.miss(4); - } + } EXPECT_EQ(nm.layer_get(occ.name(), 4), -1992); // Reset and out-of-range cells occ.reset(4); diff --git a/navmap_core/tests/test_links.cpp b/navmap_core/tests/test_links.cpp index a793163..62ce580 100644 --- a/navmap_core/tests/test_links.cpp +++ b/navmap_core/tests/test_links.cpp @@ -79,7 +79,7 @@ TEST(Links, AddQueryAndDefaultLength) // Links do not change the mesh adjacency for (NavCelId n : nm.navcel_neighbors(0)) { EXPECT_LT(n, 8u); - } + } } TEST(Links, InvalidLinksAreRejected) diff --git a/navmap_core/tests/test_navmap_properties.cpp b/navmap_core/tests/test_navmap_properties.cpp index 77905c5..4cab18d 100644 --- a/navmap_core/tests/test_navmap_properties.cpp +++ b/navmap_core/tests/test_navmap_properties.cpp @@ -106,8 +106,9 @@ std::vector heights_at(const NavMap & nm, float x, float y) const Eigen::Vector3f o(x, y, kTopZ), d(0.0f, 0.0f, -1.0f); for (NavCelId cid = 0; cid < nm.navcels.size(); ++cid) { float t, u, v; - if (navmap::ray_triangle_intersect(o, d, vtx(nm, cid, 0), vtx(nm, cid, 1), vtx(nm, cid, 2), - t, u, v)) + if (navmap::ray_triangle_intersect( + o, d, vtx(nm, cid, 0), vtx(nm, cid, 1), vtx(nm, cid, 2), + t, u, v)) { out.push_back(o.z() - t); } @@ -122,8 +123,9 @@ std::optional brute_raycast( std::optional best; for (NavCelId cid = 0; cid < nm.navcels.size(); ++cid) { float t, u, v; - if (navmap::ray_triangle_intersect(o, d, vtx(nm, cid, 0), vtx(nm, cid, 1), vtx(nm, cid, 2), - t, u, v) && (!best || t < *best)) + if (navmap::ray_triangle_intersect( + o, d, vtx(nm, cid, 0), vtx(nm, cid, 1), vtx(nm, cid, 2), + t, u, v) && (!best || t < *best)) { best = t; } diff --git a/navmap_core/tests/test_simplify.cpp b/navmap_core/tests/test_simplify.cpp index f4ee8bb..8ae44ce 100644 --- a/navmap_core/tests/test_simplify.cpp +++ b/navmap_core/tests/test_simplify.cpp @@ -35,8 +35,8 @@ using navmap::SimplifyParams; namespace { -using Height = std::function; -using Keep = std::function; +using Height = std::function; +using Keep = std::function; // nx x ny cells of side res (two NavCels each) at offset z, cells where keep(i, j); one surface void add_grid( @@ -120,8 +120,8 @@ void expect_same_values( for (const auto & p : probes) { ASSERT_TRUE(a.locate_navcel(p, s, ca, bary, nullptr)) << p.transpose(); ASSERT_TRUE(b.locate_navcel(p, s, cb, bary, nullptr)) << p.transpose(); - ASSERT_EQ(a.layer_get(layer, ca), b.layer_get(layer, cb)) << layer << " at " - << p.transpose(); + ASSERT_EQ(a.layer_get(layer, ca), b.layer_get(layer, cb)) << layer << " at " + << p.transpose(); } } @@ -196,7 +196,8 @@ TEST(Simplify, WithoutLayersAFlatSquareBecomesAFewNavCels) for (std::size_t i = 0; i < nm.positions.size(); ++i) { const float x = nm.positions.x[i], y = nm.positions.y[i]; // The square's outline stays: every vertex on its border (no interior vertex needed) - EXPECT_TRUE(std::abs(x) < 1e-4f || std::abs(x - 3.0f) < 1e-4f || + EXPECT_TRUE( + std::abs(x) < 1e-4f || std::abs(x - 3.0f) < 1e-4f || std::abs(y) < 1e-4f || std::abs(y - 3.0f) < 1e-4f) << x << " " << y; } } @@ -259,7 +260,8 @@ TEST(Simplify, CurvedTerrainStaysWithinTheErrorAndZeroErrorKeepsIt) worst = std::max(worst, std::abs(hit.z() - q.z())); } EXPECT_LE(worst, 1.5f * e) << e; - std::printf("max_error %.3f: worst %.4f m, %zu -> %zu NavCels\n", e, worst, + std::printf( + "max_error %.3f: worst %.4f m, %zu -> %zu NavCels\n", e, worst, orig.navcels.size(), nm.navcels.size()); } NavMap nm = orig; @@ -273,10 +275,11 @@ TEST(Simplify, HolesAndStackedFloorsAreKept) { NavMap orig; // Ground floor with a 1 x 1 m hole (a stairwell), and a full floor 3 m above - add_grid(orig, 40, 40, 0.1f, flat, [](int i, int j) { - return !(i >= 15 && i < 25 && j >= 15 && - j < 25); - }); + add_grid( + orig, 40, 40, 0.1f, flat, [](int i, int j) { + const bool hole = i >= 15 && i < 25 && j >= 15 && j < 25; + return !hole; + }); add_grid(orig, 40, 40, 0.1f, [](float, float) {return 3.0f;}); orig.rebuild_geometry_accels(); const int64_t chi = euler(orig); @@ -384,8 +387,9 @@ TEST(Simplify, DegenerateInputsAreSafe) NavMap one; const auto s = one.create_surface("map"); one.add_navcel_to_surface( - s, one.add_navcel(one.add_vertex({0, 0, 0}), one.add_vertex({1, 0, 0}), - one.add_vertex({0, 1, 0}))); + s, one.add_navcel( + one.add_vertex({0, 0, 0}), one.add_vertex({1, 0, 0}), + one.add_vertex({0, 1, 0}))); one.rebuild_geometry_accels(); navmap::simplify(one); EXPECT_EQ(one.navcels.size(), 1u); diff --git a/navmap_core/tests/test_surface_tree.cpp b/navmap_core/tests/test_surface_tree.cpp index 76b4ede..85c46ce 100644 --- a/navmap_core/tests/test_surface_tree.cpp +++ b/navmap_core/tests/test_surface_tree.cpp @@ -114,8 +114,9 @@ float brute_closest_sq(const NavMap & nm, const Eigen::Vector3f & p) { float best = std::numeric_limits::infinity(); for (const auto & cel : nm.navcels) { - const Eigen::Vector3f q = closest_on_tri(p, nm.positions.at(cel.v[0]), - nm.positions.at(cel.v[1]), nm.positions.at(cel.v[2])); + const Eigen::Vector3f q = closest_on_tri( + p, nm.positions.at(cel.v[0]), + nm.positions.at(cel.v[1]), nm.positions.at(cel.v[2])); best = std::min(best, (p - q).squaredNorm()); } return best;