diff --git a/include/gz/math/TimeVaryingVolumetricGrid.hh b/include/gz/math/TimeVaryingVolumetricGrid.hh index d0fb1c98c..4b98a234a 100644 --- a/include/gz/math/TimeVaryingVolumetricGrid.hh +++ b/include/gz/math/TimeVaryingVolumetricGrid.hh @@ -53,6 +53,10 @@ class TimeVaryingVolumetricGrid /// \param[in] _pos - The position of the point we want to query public: std::optional LookUp(const S &_session, const Vector3

&_pos) const; + + /// \brief Get the bounds of this grid field at given time. + /// \return A pair of vectors. All zeros if session is invalid. + public: std::pair, Vector3> Bounds(const S &_session) const; }; /// \brief Specialization of TimeVaryingVolumetricGrid which loads the whole of @@ -93,6 +97,14 @@ class TimeVaryingVolumetricGrid, P> return result; } + /// \brief Get the bounds of this grid field at given time. + /// \return A pair of vectors. All zeros if session is invalid. + public: std::pair, Vector3> Bounds( + const InMemorySession &_session) const + { + return indices.Bounds(_session); + } + /// Buffer for values being stored private: std::vector values; diff --git a/include/gz/math/TimeVaryingVolumetricGridLookupField.hh b/include/gz/math/TimeVaryingVolumetricGridLookupField.hh index 7e0bbec2f..b505b6acd 100644 --- a/include/gz/math/TimeVaryingVolumetricGridLookupField.hh +++ b/include/gz/math/TimeVaryingVolumetricGridLookupField.hh @@ -87,6 +87,10 @@ namespace gz const std::vector &_values2, const X _default = X(0) ) const; + + /// \brief Get the bounds of this grid field. + /// \return A pair of vectors. + public: std::pair, Vector3> Bounds(const S &_session); }; /// \brief An in-memory session. Loads the whole dataset in memory and @@ -285,6 +289,22 @@ namespace gz // Return nullopt if we are out of range return std::nullopt; } + + /// \brief Get the bounds of this grid field. + /// \return A pair of vectors. All zeros if session is invalid. + public: std::pair, Vector3> Bounds( + const InMemorySession &_session) const + { + if (_session.iter == this->gridFields.end()) + { + // Out of bounds + return std::pair, Vector3>( + Vector3{0, 0, 0}, Vector3{0, 0, 0} + ); + } + return _session.iter->second.Bounds(); + } + private: std::map> gridFields; }; } diff --git a/include/gz/math/VolumetricGridLookupField.hh b/include/gz/math/VolumetricGridLookupField.hh index 0449497f7..7da02503f 100644 --- a/include/gz/math/VolumetricGridLookupField.hh +++ b/include/gz/math/VolumetricGridLookupField.hh @@ -18,8 +18,9 @@ #ifndef GZ_MATH_VOLUMETRIC_GRID_LOOKUP_FIELD_HH_ #define GZ_MATH_VOLUMETRIC_GRID_LOOKUP_FIELD_HH_ -#include #include +#include +#include #include #include @@ -262,6 +263,23 @@ namespace gz } } + /// \brief Get the bounds of this grid field. + /// \return A pair of vectors. + public: std::pair, Vector3> Bounds() const + { + return std::pair, Vector3>( + Vector3{ + x_indices_by_lat.MinKey(), + y_indices_by_lon.MinKey(), + z_indices_by_depth.MinKey() + }, + Vector3{ + x_indices_by_lat.MaxKey(), + y_indices_by_lon.MaxKey(), + z_indices_by_depth.MaxKey() + }); + } + }; } } diff --git a/include/gz/math/detail/AxisIndex.hh b/include/gz/math/detail/AxisIndex.hh index 1c837e341..3d4dabb1a 100644 --- a/include/gz/math/detail/AxisIndex.hh +++ b/include/gz/math/detail/AxisIndex.hh @@ -45,6 +45,30 @@ namespace gz /// \brief Number of indices private: int numIndices{0}; + /// \brief Minimum key + /// \return the minimum key or zero if the axis is empty. + public: T MinKey() const + { + auto key = axisIndex.begin(); + if (key == axisIndex.end()) + { + return T(0); + } + return key->first; + } + + /// \brief Maximum key + /// \return the maximum key or zero if the axis is empty. + public: T MaxKey() const + { + auto key = axisIndex.rbegin(); + if (key == axisIndex.rend()) + { + return T(0); + } + return key->first; + } + /// \brief Register the existance of a measurement at _value. /// \param[in] _value The position of the measurement. public: void AddIndexIfNotFound(T _value) diff --git a/src/TimeVaryingVolumetricGrid_TEST.cc b/src/TimeVaryingVolumetricGrid_TEST.cc index d940fbcc4..685c75253 100644 --- a/src/TimeVaryingVolumetricGrid_TEST.cc +++ b/src/TimeVaryingVolumetricGrid_TEST.cc @@ -63,3 +63,55 @@ TEST(TimeVaryingVolumetricGridTest, TestConstruction) val = grid.LookUp(new_sess.value(), Vector3d{2.5, 2.5, 2.5}); ASSERT_FALSE(val.has_value()); } + +///////////////////////////////////////////////// +TEST(TimeVaryingVolumetricGridTest, TestBounds) +{ + InMemoryTimeVaryingVolumetricGridFactory gridFactory; + + for (double t = 0; t <= 1; t+=0.2) + { + for (double x = -1; x <= 1; x+=0.5) + { + for (double y = -1; y <= 1; y+=0.5) + { + for (double z = -1; z <= 1; z+=0.5) + { + gridFactory.AddPoint(t, Vector3d{x, y, z}, t); + } + } + } + } + + auto grid = gridFactory.Build(); + auto session = grid.CreateSession(); + + auto [min, max] = grid.Bounds(session); + + ASSERT_NEAR(min.X(), -1.0, 1e-6); + ASSERT_NEAR(min.Y(), -1.0, 1e-6); + ASSERT_NEAR(min.Z(), -1.0, 1e-6); + + ASSERT_NEAR(max.X(), 1.0, 1e-6); + ASSERT_NEAR(max.Y(), 1.0, 1e-6); + ASSERT_NEAR(max.Z(), 1.0, 1e-6); +} + +///////////////////////////////////////////////// +TEST(TimeVaryingVolumetricGridTest, TestEmptyGrid) +{ + InMemoryTimeVaryingVolumetricGridFactory gridFactory; + + auto grid = gridFactory.Build(); + auto session = grid.CreateSession(); + + auto [min, max] = grid.Bounds(session); + + ASSERT_NEAR(min.X(), 0, 1e-6); + ASSERT_NEAR(min.Y(), 0, 1e-6); + ASSERT_NEAR(min.Z(), 0, 1e-6); + + ASSERT_NEAR(max.X(), 0, 1e-6); + ASSERT_NEAR(max.Y(), 0, 1e-6); + ASSERT_NEAR(max.Z(), 0, 1e-6); +} diff --git a/src/VolumetricGridLookupField_TEST.cc b/src/VolumetricGridLookupField_TEST.cc index 7f6798784..75589bcb0 100644 --- a/src/VolumetricGridLookupField_TEST.cc +++ b/src/VolumetricGridLookupField_TEST.cc @@ -182,12 +182,15 @@ TEST(VolumetricGridLookupField, CheckTrilinearInterpolationBoxEightPoints) } } - TEST(VolumetricGridLookupField, AxisIndexTest) { AxisIndex axis; + EXPECT_EQ(axis.MinKey(), 0.0); + EXPECT_EQ(axis.MaxKey(), 0.0); axis.AddIndexIfNotFound(300); axis.AddIndexIfNotFound(300); + EXPECT_EQ(axis.MinKey(), 300.0); + EXPECT_EQ(axis.MaxKey(), 300.0); EXPECT_EQ(axis.GetNumUniqueIndices(), 1UL); EXPECT_EQ(axis.GetIndex(300).value(), 0UL); EXPECT_EQ(axis.GetIndex(200).has_value(), false);