diff --git a/include/gz/math/VolumetricGridLookupField.hh b/include/gz/math/VolumetricGridLookupField.hh new file mode 100644 index 000000000..46c406031 --- /dev/null +++ b/include/gz/math/VolumetricGridLookupField.hh @@ -0,0 +1,223 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef IGNITION_MATH_VOLUMETRIC_GRID_LOOKUP_FIELD_HH_ +#define IGNITION_MATH_VOLUMETRIC_GRID_LOOKUP_FIELD_HH_ + +#include +#include + +#include +#include + +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + template + /// \brief Lookup table for a volumetric dataset. This class is used to + /// lookup indices for a large dataset that's organized in a grid. This + /// class is not meant to be used with non-grid like data sets. The grid + /// may be sparse or non uniform and missing data points. + class VolumetricGridLookupField + { + /// Get the index along the given axis + private: AxisIndex z_indices_by_depth; + + private: AxisIndex x_indices_by_lat; + + private: AxisIndex y_indices_by_lon; + + private: std::vector< + std::vector>>> index_table; + + /// \brief Constructor + /// \param[in] _cloud The cloud of points to use to construct the grid. + public: VolumetricGridLookupField( + const std::vector> &_cloud) + { + // NOTE: This part of the code assumes an exact grid of points. + // The grid may be distorted or the stride between different points + // may not be the same, but fundamentally the data is structured still + // forms a grid-like structure. It keeps track of the axis indices for + // each point in the grid. + for(auto pt : _cloud) + { + x_indices_by_lat.AddIndexIfNotFound(pt.X()); + y_indices_by_lon.AddIndexIfNotFound(pt.Y()); + z_indices_by_depth.AddIndexIfNotFound(pt.Z()); + } + + int num_x = x_indices_by_lat.GetNumUniqueIndices(); + int num_y = y_indices_by_lon.GetNumUniqueIndices(); + int num_z = z_indices_by_depth.GetNumUniqueIndices(); + + index_table.resize(num_z); + for(int i = 0; i < num_z; ++i) + { + index_table[i].resize(num_y); + for(int j = 0; j < num_y; ++j) + { + index_table[i][j].resize(num_x); + } + } + + for(std::size_t i = 0; i < _cloud.size(); ++i) + { + const auto &pt = _cloud[i]; + const std::size_t x_index = + x_indices_by_lat.GetIndex(pt.X()).value(); + const std::size_t y_index = + y_indices_by_lon.GetIndex(pt.Y()).value(); + const std::size_t z_index = + z_indices_by_depth.GetIndex(pt.Z()).value(); + index_table[z_index][y_index][x_index] = i; + } + } + + /// \brief Retrieves the indices of the points that are to be used for + /// interpolation. Separate tolerance values are used for each axis as + /// data may have different magnitudes on each axis. + /// \param[in] _pt The point to get the interpolators for. + /// \param[in] _xTol The tolerance for the x axis. + /// \param[in] _yTol The tolerance for the y axis. + /// \param[in] _zTol The tolerance for the z axis. + /// \returns A list of points which are to be used for interpolation. If + /// the point is in the middle of a grid cell then it returns the four + /// corners of the grid cell. If its along a plane then it returns the + /// four corners, if along an edge two points and if coincident with a + /// corner one point. If the data is sparse and missing indices then a + /// nullopt will be returned. + public: std::vector> + GetInterpolators( + const Vector3 &_pt, + const double _xTol = 1e-6, + const double _yTol = 1e-6, + const double _zTol = 1e-6) const + { + std::vector> interpolators; + + auto x_indices = x_indices_by_lat.GetInterpolators(_pt.X(), _xTol); + auto y_indices = y_indices_by_lon.GetInterpolators(_pt.Y(), _yTol); + auto z_indices = z_indices_by_depth.GetInterpolators(_pt.Z(), _zTol); + + for(const auto &x_index : x_indices) + { + for(const auto &y_index : y_indices) + { + for(const auto &z_index : z_indices) + { + auto index = + index_table[z_index.index][y_index.index][x_index.index]; + interpolators.push_back( + InterpolationPoint3D{ + Vector3( + x_index.position, + y_index.position, + z_index.position + ), + std::optional{index} + }); + } + } + } + + return interpolators; + } + + /// \brief Estimates the values for a grid given a list of values to + /// interpolate. This method uses Trilinear interpolation. + /// \param[in] _pt The point to estimate for. + /// \param[in] _values The values to interpolate. + /// \param[in] _default If a value is not found at a specific point then + /// this value will be used. + /// \returns The estimated value for the point. + public: template + std::optional EstimateValueUsingTrilinear( + const Vector3 &_pt, + const std::vector &_values, + const V &_default = V(0)) const + { + auto interpolators = GetInterpolators(_pt); + return EstimateValueUsingTrilinear( + interpolators, + _pt, + _values, + _default); + } + + /// \brief Estimates the values for a grid given a list of values to + /// interpolate. This method uses Trilinear interpolation. + /// \param[in] _interpolators The list of interpolators to use. + /// Retrieved by calling GetInterpolators(). + /// \param[in] _pt The point to estimate for. + /// \param[in] _values The values to interpolate. + /// \param[in] _default If a value is not found at a specific point then + /// this value will be used. + /// \returns The estimated value for the point. Nullopt if we are + /// outside the field. Default value if in the field but no value is + /// in the index. + public: template + std::optional EstimateValueUsingTrilinear( + const std::vector> _interpolators, + const Vector3 &_pt, + const std::vector &_values, + const V &_default = V(0)) const + { + if (_interpolators.size() == 0) + { + return std::nullopt; + } + else if (_interpolators.size() == 1) + { + if (!_interpolators[0].index.has_value()) + { + return _default; + } + return _values[_interpolators[0].index.value()]; + } + else if (_interpolators.size() == 2) + { + return LinearInterpolate(_interpolators[0], _interpolators[1], + _values, _pt, _default); + } + else if (_interpolators.size() == 4) + { + return BiLinearInterpolate( + _interpolators, 0, _values, _pt, _default); + } + else if (_interpolators.size() == 8) + { + return TrilinearInterpolate(_interpolators, _values, _pt, _default); + } + else + { + // should never get here + return std::nullopt; + } + } + + }; + } + } +} + +#endif diff --git a/include/gz/math/detail/AxisIndex.hh b/include/gz/math/detail/AxisIndex.hh new file mode 100644 index 000000000..53db280e9 --- /dev/null +++ b/include/gz/math/detail/AxisIndex.hh @@ -0,0 +1,126 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef IGNITION_MATH_DETAIL_AXIS_INDEX_LOOKUP_FIELD_HH_ +#define IGNITION_MATH_DETAIL_AXIS_INDEX_LOOKUP_FIELD_HH_ + +#include +#include +#include +#include + +#include + +#include + +namespace ignition +{ + namespace math + { + // Inline bracket to help doxygen filtering. + inline namespace IGNITION_MATH_VERSION_NAMESPACE { + + /// \brief Represents a sparse number line which can be searched via + /// search for indices. + template + class AxisIndex + { + /// \brief Binary tree with indices + private: std::map axisIndex; + + /// \brief Number of indices + private: int numIndices{0}; + + /// \brief Register the existance of a measurement at _value. + /// \param[in] _value The position of the measurement. + public: void AddIndexIfNotFound(T _value) + { + if (axisIndex.find(_value) == axisIndex.end()) + { + axisIndex[_value] = numIndices; + numIndices++; + } + } + + /// \brief Get the number of unique indices. + /// \return The number of unique indices. + public: std::size_t GetNumUniqueIndices() const + { + return axisIndex.size(); + } + + /// \brief Get the index of a measurement + /// \param[in] _value The position of the measurement. + /// \return The index of the measurement if found else return nullopt. + public: std::optional GetIndex(T _value) const + { + auto res = axisIndex.find(_value); + if (res == axisIndex.end()) + { + return std::nullopt; + } + else + { + return res->second; + } + } + + /// \brief Get interpolators for a measurement. + /// \param[in] _value The position of the measurement. + /// \param[in] _tol The tolerance for the search. Cannot be zero. + /// \return The indices of the measurements that should be used for + /// interpolation. If the value is out of range, an empty vector is + /// returned. If the value is exact, a vector with a single index is + /// returned otherwise return the two indices which should be used for + /// interpolation. + public: std::vector> GetInterpolators( + const T &_value, + double _tol = 1e-6) const + { + assert(_tol > 0); + // Performs a BST to find the first element that is greater than or + // equal to the value. + auto it = axisIndex.lower_bound(_value); + if (it == axisIndex.end()) + { + // Out of range + return {}; + } + else if (fabs(it->first - _value) < _tol) + { + // Exact match + return {InterpolationPoint1D{it->first, it->second}}; + } + else if (it == axisIndex.begin()) + { + // Below range + return {}; + } + else + { + // Interpolate + auto itPrev = it; + itPrev--; + return {InterpolationPoint1D{itPrev->first, itPrev->second}, + InterpolationPoint1D{it->first, it->second}}; + } + } + }; + } + } +} +#endif diff --git a/include/gz/math/detail/InterpolationPoint.hh b/include/gz/math/detail/InterpolationPoint.hh new file mode 100644 index 000000000..38296bfda --- /dev/null +++ b/include/gz/math/detail/InterpolationPoint.hh @@ -0,0 +1,251 @@ +/* + * Copyright (C) 2022 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#ifndef IGNITION_MATH_INTERPOLATION_POINT_HH_ +#define IGNITION_MATH_INTERPOLATION_POINT_HH_ + +#include +#include + +#include +#include + +#include +namespace ignition +{ + namespace math + { + /// \brief Describes an interpolation point. + template + struct InterpolationPoint3D + { + /// \brief The position of the point + Vector3 position; + + /// \brief The index from which this point was retrieved. + /// Can be used by the application calling it to index. The reason + /// this is optional is that data may be missing from a sparse grid. + std::optional index; + }; + + /// \brief Describes an interpolation point in1d. + template + struct InterpolationPoint1D + { + /// \brief The position of the point + T position; + + /// \brief The index from which this point was retrieved. + /// Can be used by the application calling it to index. + std::size_t index; + }; + + /// \brief Linear Interpolation of two points along a 1D line. + /// \param[in] _a The first point. + /// \param[in] _b The second point. + /// \param[in] _lst An array of values that are to be used by the + /// interpolator. _lst[a.index] and _lst[b.index] are the values + /// to be interpolated. + /// \param[in] _pos The position to interpolate. + /// Warning: This function assumes that the indices of _a and _b correspond + /// to values in _lst. It performs no bounds checking whatsoever and if you + /// pass it invalid data, it will crash. It also assumes that _a and _b + /// are not in the same position. + template + V LinearInterpolate( + const InterpolationPoint1D &_a, + const InterpolationPoint1D &_b, + const std::vector &_lst, + const T &_pos + ) + { + assert(abs(_a.position - _b.position) > 0); + assert(_a.index < _lst.size()); + assert(_b.index < _lst.size()); + + auto t = (_pos - _b.position)/ (_a.position - _b.position); + return (1 - t) * _lst[_b.index] + t * _lst[_a.index]; + } + + /// \brief Linear Interpolation of two points in 3D space + /// \param[in] _a The first point. + /// \param[in] _b The second point. + /// \param[in] _lst An array of values that are to be used by the + /// interpolator. _lst[a.index] and _lst[b.index] are the values + /// to be interpolated. If a.index or b.index is std::nullopt then use the + /// default value. + /// \param[in] _pos The position to interpolate. + /// \param[in] _default The default value to use if a.index or b.index is + /// std::nullopt. + /// Warning: This function assumes that the indices of _a and _b correspond + /// to values in _lst. It performs no bounds checking whatsoever and if you + /// pass it invalid data, it will crash. + template + V LinearInterpolate( + const InterpolationPoint3D &_a, + const InterpolationPoint3D &_b, + const std::vector &_lst, + const Vector3 &_pos, + const V &_default = V(0) + ) + { + assert((_a.position - _b.position).Length() > 0); + assert((_a.index.has_value()) ? _a.index.value() < _lst.size(): true); + assert((_b.index.has_value()) ? _b.index.value() < _lst.size(): true); + + auto t = + (_pos - _b.position).Length() / (_a.position - _b.position).Length(); + + auto b_val = (_b.index.has_value()) ? _lst[_b.index.value()]: _default; + auto a_val = (_a.index.has_value()) ? _lst[_a.index.value()]: _default; + return (1 - t) * b_val + t * a_val; + } + + /// \brief Bilinear interpolation of four points in 3D space. It assumes + /// these 4 points form a plane. + /// \param[in] _a The list of points to interpolate. The list must have at + /// least 4 entries. Furthermore the order of the indices should be such + /// that every consecutive pair of indices lie on the same edge. Furthermore + /// the 4 points must be coplanar and corners of a rectangular patch. + /// \param[in] _start_index The starting index of points to interpolate. + /// \param[in] _lst An array of values that are to be used by the + /// interpolator. _lst[a.index] and _lst[b.index] are the values + /// to be interpolated. If a.index or b.index is std::nullopt then use the + /// default value. + /// \param[in] _pos The position to interpolate. + /// \param[in] _default The default value to use if a.index or b.index is + /// std::nullopt. + /// Warning: This function assumes that the indices of _a and _b correspond + /// to values in _lst. It performs no bounds checking whatsoever and if you + /// pass it invalid data, it will crash. + template + V BiLinearInterpolate( + const std::vector> &_a, + const std::size_t &_start_index, + const std::vector &_lst, + const Vector3 &_pos, + const V &_default = V(0)) + { + /// Assertions + #ifndef NDEBUG + assert(_a.size() >= _start_index + 4); + /// Check if all points co planar. + auto planeNormal = (_a[_start_index + 1].position - + _a[_start_index].position).Cross(_a[_start_index + 2].position - + _a[_start_index].position); + auto planeScalar = planeNormal.Dot(_a[_start_index].position); + assert( + !(abs(planeNormal.Dot(_a[_start_index + 3].position) - planeScalar) > 0) + ); + #endif + + // Project point onto line + std::vector linres; + auto n0 = _a[_start_index]; + auto n1 = _a[_start_index + 1]; + auto proj1 = n1.position-n0.position; + auto unitProj1 = proj1.Normalized(); + auto pos1 = + (_pos - n0.position).Dot(unitProj1) * unitProj1 + n0.position; + // Apply normal linear interpolation + linres.push_back(LinearInterpolate(n0, n1, _lst, pos1, _default)); + + // Project point onto second line + auto n2 = _a[_start_index + 2]; + auto n3 = _a[_start_index + 3]; + auto proj2 = n3.position-n2.position; + auto unitProj2 = proj2.Normalized(); + auto pos2 = + (_pos - n2.position).Dot(unitProj1) * unitProj1 + n2.position; + linres.push_back(LinearInterpolate(n2, n3, _lst, pos2, _default)); + + // Perform final linear interpolation + InterpolationPoint3D p1 { + pos1, + 0 + }; + + InterpolationPoint3D p2 { + pos2, + 1 + }; + return LinearInterpolate(p1, p2, linres, _pos, _default); + } + + /// \brief Project Point onto a plane. + /// \param[in] _points a vector of min length _start_index + 3. The points + /// at _start_index, _start_index + 1 and _start_index + 2 are used to + /// define the plane. + /// \param[in] _start_index defines the slice to use. + /// \param[in] _pos The position to project onto the plane + template + ignition::math::Vector3 ProjectPointToPlane( + const std::vector> _points, + const std::size_t &_start_index, + const Vector3 &_pos) + { + auto n = + (_points[_start_index + 1].position - _points[_start_index].position) + .Cross( + _points[_start_index + 2].position - _points[_start_index].position); + return + _pos - n.Dot(_pos - _points[_start_index].position) * n.Normalized(); + } + + /// \brief Trilinear interpolation of eight points in 3D space. It assumes + /// these eight points form a rectangular prism. + /// \param[in] _a The list of points to interpolate. The list must have 8 + /// points. The first 4 points must form a plane as must the last 4. + /// The order of the points within a plane should be such that consecutive + /// pairs of indices lie on the same edge. + /// \param[in] _lst An array of values that are to be used for interpolation + /// \param[in] _pos The position to interpolate. + /// \param[in] _default The default value to use if a.index or b.index is + /// std::nullopt. + template + V TrilinearInterpolate( + const std::vector> &_a, + const std::vector &_lst, + const Vector3 &_pos, + const V &_default = V(0)) + { + assert(_a.size() == 8); + + std::vector linres; + // First plane + auto pos1 = ProjectPointToPlane(_a, 0, _pos); + linres.push_back(BiLinearInterpolate(_a, 0, _lst, pos1, _default)); + + // Second plane + auto pos2 = ProjectPointToPlane(_a, 4, _pos); + linres.push_back(BiLinearInterpolate(_a, 4, _lst, pos2, _default)); + + // Perform final linear interpolation + InterpolationPoint3D p1 { + pos1, + 0 + }; + + InterpolationPoint3D p2 { + pos2, + 1 + }; + return LinearInterpolate(p1, p2, linres, _pos, _default); + } + } +} +#endif diff --git a/include/ignition/math/VolumetricGridLookupField.hh b/include/ignition/math/VolumetricGridLookupField.hh index 46c406031..3d3d40484 100644 --- a/include/ignition/math/VolumetricGridLookupField.hh +++ b/include/ignition/math/VolumetricGridLookupField.hh @@ -15,209 +15,4 @@ * */ -#ifndef IGNITION_MATH_VOLUMETRIC_GRID_LOOKUP_FIELD_HH_ -#define IGNITION_MATH_VOLUMETRIC_GRID_LOOKUP_FIELD_HH_ - -#include -#include - -#include -#include - -#include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - template - /// \brief Lookup table for a volumetric dataset. This class is used to - /// lookup indices for a large dataset that's organized in a grid. This - /// class is not meant to be used with non-grid like data sets. The grid - /// may be sparse or non uniform and missing data points. - class VolumetricGridLookupField - { - /// Get the index along the given axis - private: AxisIndex z_indices_by_depth; - - private: AxisIndex x_indices_by_lat; - - private: AxisIndex y_indices_by_lon; - - private: std::vector< - std::vector>>> index_table; - - /// \brief Constructor - /// \param[in] _cloud The cloud of points to use to construct the grid. - public: VolumetricGridLookupField( - const std::vector> &_cloud) - { - // NOTE: This part of the code assumes an exact grid of points. - // The grid may be distorted or the stride between different points - // may not be the same, but fundamentally the data is structured still - // forms a grid-like structure. It keeps track of the axis indices for - // each point in the grid. - for(auto pt : _cloud) - { - x_indices_by_lat.AddIndexIfNotFound(pt.X()); - y_indices_by_lon.AddIndexIfNotFound(pt.Y()); - z_indices_by_depth.AddIndexIfNotFound(pt.Z()); - } - - int num_x = x_indices_by_lat.GetNumUniqueIndices(); - int num_y = y_indices_by_lon.GetNumUniqueIndices(); - int num_z = z_indices_by_depth.GetNumUniqueIndices(); - - index_table.resize(num_z); - for(int i = 0; i < num_z; ++i) - { - index_table[i].resize(num_y); - for(int j = 0; j < num_y; ++j) - { - index_table[i][j].resize(num_x); - } - } - - for(std::size_t i = 0; i < _cloud.size(); ++i) - { - const auto &pt = _cloud[i]; - const std::size_t x_index = - x_indices_by_lat.GetIndex(pt.X()).value(); - const std::size_t y_index = - y_indices_by_lon.GetIndex(pt.Y()).value(); - const std::size_t z_index = - z_indices_by_depth.GetIndex(pt.Z()).value(); - index_table[z_index][y_index][x_index] = i; - } - } - - /// \brief Retrieves the indices of the points that are to be used for - /// interpolation. Separate tolerance values are used for each axis as - /// data may have different magnitudes on each axis. - /// \param[in] _pt The point to get the interpolators for. - /// \param[in] _xTol The tolerance for the x axis. - /// \param[in] _yTol The tolerance for the y axis. - /// \param[in] _zTol The tolerance for the z axis. - /// \returns A list of points which are to be used for interpolation. If - /// the point is in the middle of a grid cell then it returns the four - /// corners of the grid cell. If its along a plane then it returns the - /// four corners, if along an edge two points and if coincident with a - /// corner one point. If the data is sparse and missing indices then a - /// nullopt will be returned. - public: std::vector> - GetInterpolators( - const Vector3 &_pt, - const double _xTol = 1e-6, - const double _yTol = 1e-6, - const double _zTol = 1e-6) const - { - std::vector> interpolators; - - auto x_indices = x_indices_by_lat.GetInterpolators(_pt.X(), _xTol); - auto y_indices = y_indices_by_lon.GetInterpolators(_pt.Y(), _yTol); - auto z_indices = z_indices_by_depth.GetInterpolators(_pt.Z(), _zTol); - - for(const auto &x_index : x_indices) - { - for(const auto &y_index : y_indices) - { - for(const auto &z_index : z_indices) - { - auto index = - index_table[z_index.index][y_index.index][x_index.index]; - interpolators.push_back( - InterpolationPoint3D{ - Vector3( - x_index.position, - y_index.position, - z_index.position - ), - std::optional{index} - }); - } - } - } - - return interpolators; - } - - /// \brief Estimates the values for a grid given a list of values to - /// interpolate. This method uses Trilinear interpolation. - /// \param[in] _pt The point to estimate for. - /// \param[in] _values The values to interpolate. - /// \param[in] _default If a value is not found at a specific point then - /// this value will be used. - /// \returns The estimated value for the point. - public: template - std::optional EstimateValueUsingTrilinear( - const Vector3 &_pt, - const std::vector &_values, - const V &_default = V(0)) const - { - auto interpolators = GetInterpolators(_pt); - return EstimateValueUsingTrilinear( - interpolators, - _pt, - _values, - _default); - } - - /// \brief Estimates the values for a grid given a list of values to - /// interpolate. This method uses Trilinear interpolation. - /// \param[in] _interpolators The list of interpolators to use. - /// Retrieved by calling GetInterpolators(). - /// \param[in] _pt The point to estimate for. - /// \param[in] _values The values to interpolate. - /// \param[in] _default If a value is not found at a specific point then - /// this value will be used. - /// \returns The estimated value for the point. Nullopt if we are - /// outside the field. Default value if in the field but no value is - /// in the index. - public: template - std::optional EstimateValueUsingTrilinear( - const std::vector> _interpolators, - const Vector3 &_pt, - const std::vector &_values, - const V &_default = V(0)) const - { - if (_interpolators.size() == 0) - { - return std::nullopt; - } - else if (_interpolators.size() == 1) - { - if (!_interpolators[0].index.has_value()) - { - return _default; - } - return _values[_interpolators[0].index.value()]; - } - else if (_interpolators.size() == 2) - { - return LinearInterpolate(_interpolators[0], _interpolators[1], - _values, _pt, _default); - } - else if (_interpolators.size() == 4) - { - return BiLinearInterpolate( - _interpolators, 0, _values, _pt, _default); - } - else if (_interpolators.size() == 8) - { - return TrilinearInterpolate(_interpolators, _values, _pt, _default); - } - else - { - // should never get here - return std::nullopt; - } - } - - }; - } - } -} - -#endif +#include diff --git a/include/ignition/math/detail/AxisIndex.hh b/include/ignition/math/detail/AxisIndex.hh index 53db280e9..7ffbddb9d 100644 --- a/include/ignition/math/detail/AxisIndex.hh +++ b/include/ignition/math/detail/AxisIndex.hh @@ -15,112 +15,4 @@ * */ -#ifndef IGNITION_MATH_DETAIL_AXIS_INDEX_LOOKUP_FIELD_HH_ -#define IGNITION_MATH_DETAIL_AXIS_INDEX_LOOKUP_FIELD_HH_ - -#include -#include -#include -#include - -#include - -#include - -namespace ignition -{ - namespace math - { - // Inline bracket to help doxygen filtering. - inline namespace IGNITION_MATH_VERSION_NAMESPACE { - - /// \brief Represents a sparse number line which can be searched via - /// search for indices. - template - class AxisIndex - { - /// \brief Binary tree with indices - private: std::map axisIndex; - - /// \brief Number of indices - private: int numIndices{0}; - - /// \brief Register the existance of a measurement at _value. - /// \param[in] _value The position of the measurement. - public: void AddIndexIfNotFound(T _value) - { - if (axisIndex.find(_value) == axisIndex.end()) - { - axisIndex[_value] = numIndices; - numIndices++; - } - } - - /// \brief Get the number of unique indices. - /// \return The number of unique indices. - public: std::size_t GetNumUniqueIndices() const - { - return axisIndex.size(); - } - - /// \brief Get the index of a measurement - /// \param[in] _value The position of the measurement. - /// \return The index of the measurement if found else return nullopt. - public: std::optional GetIndex(T _value) const - { - auto res = axisIndex.find(_value); - if (res == axisIndex.end()) - { - return std::nullopt; - } - else - { - return res->second; - } - } - - /// \brief Get interpolators for a measurement. - /// \param[in] _value The position of the measurement. - /// \param[in] _tol The tolerance for the search. Cannot be zero. - /// \return The indices of the measurements that should be used for - /// interpolation. If the value is out of range, an empty vector is - /// returned. If the value is exact, a vector with a single index is - /// returned otherwise return the two indices which should be used for - /// interpolation. - public: std::vector> GetInterpolators( - const T &_value, - double _tol = 1e-6) const - { - assert(_tol > 0); - // Performs a BST to find the first element that is greater than or - // equal to the value. - auto it = axisIndex.lower_bound(_value); - if (it == axisIndex.end()) - { - // Out of range - return {}; - } - else if (fabs(it->first - _value) < _tol) - { - // Exact match - return {InterpolationPoint1D{it->first, it->second}}; - } - else if (it == axisIndex.begin()) - { - // Below range - return {}; - } - else - { - // Interpolate - auto itPrev = it; - itPrev--; - return {InterpolationPoint1D{itPrev->first, itPrev->second}, - InterpolationPoint1D{it->first, it->second}}; - } - } - }; - } - } -} -#endif +#include diff --git a/include/ignition/math/detail/InterpolationPoint.hh b/include/ignition/math/detail/InterpolationPoint.hh index 38296bfda..1656262c0 100644 --- a/include/ignition/math/detail/InterpolationPoint.hh +++ b/include/ignition/math/detail/InterpolationPoint.hh @@ -15,237 +15,4 @@ * */ -#ifndef IGNITION_MATH_INTERPOLATION_POINT_HH_ -#define IGNITION_MATH_INTERPOLATION_POINT_HH_ - -#include -#include - -#include -#include - -#include -namespace ignition -{ - namespace math - { - /// \brief Describes an interpolation point. - template - struct InterpolationPoint3D - { - /// \brief The position of the point - Vector3 position; - - /// \brief The index from which this point was retrieved. - /// Can be used by the application calling it to index. The reason - /// this is optional is that data may be missing from a sparse grid. - std::optional index; - }; - - /// \brief Describes an interpolation point in1d. - template - struct InterpolationPoint1D - { - /// \brief The position of the point - T position; - - /// \brief The index from which this point was retrieved. - /// Can be used by the application calling it to index. - std::size_t index; - }; - - /// \brief Linear Interpolation of two points along a 1D line. - /// \param[in] _a The first point. - /// \param[in] _b The second point. - /// \param[in] _lst An array of values that are to be used by the - /// interpolator. _lst[a.index] and _lst[b.index] are the values - /// to be interpolated. - /// \param[in] _pos The position to interpolate. - /// Warning: This function assumes that the indices of _a and _b correspond - /// to values in _lst. It performs no bounds checking whatsoever and if you - /// pass it invalid data, it will crash. It also assumes that _a and _b - /// are not in the same position. - template - V LinearInterpolate( - const InterpolationPoint1D &_a, - const InterpolationPoint1D &_b, - const std::vector &_lst, - const T &_pos - ) - { - assert(abs(_a.position - _b.position) > 0); - assert(_a.index < _lst.size()); - assert(_b.index < _lst.size()); - - auto t = (_pos - _b.position)/ (_a.position - _b.position); - return (1 - t) * _lst[_b.index] + t * _lst[_a.index]; - } - - /// \brief Linear Interpolation of two points in 3D space - /// \param[in] _a The first point. - /// \param[in] _b The second point. - /// \param[in] _lst An array of values that are to be used by the - /// interpolator. _lst[a.index] and _lst[b.index] are the values - /// to be interpolated. If a.index or b.index is std::nullopt then use the - /// default value. - /// \param[in] _pos The position to interpolate. - /// \param[in] _default The default value to use if a.index or b.index is - /// std::nullopt. - /// Warning: This function assumes that the indices of _a and _b correspond - /// to values in _lst. It performs no bounds checking whatsoever and if you - /// pass it invalid data, it will crash. - template - V LinearInterpolate( - const InterpolationPoint3D &_a, - const InterpolationPoint3D &_b, - const std::vector &_lst, - const Vector3 &_pos, - const V &_default = V(0) - ) - { - assert((_a.position - _b.position).Length() > 0); - assert((_a.index.has_value()) ? _a.index.value() < _lst.size(): true); - assert((_b.index.has_value()) ? _b.index.value() < _lst.size(): true); - - auto t = - (_pos - _b.position).Length() / (_a.position - _b.position).Length(); - - auto b_val = (_b.index.has_value()) ? _lst[_b.index.value()]: _default; - auto a_val = (_a.index.has_value()) ? _lst[_a.index.value()]: _default; - return (1 - t) * b_val + t * a_val; - } - - /// \brief Bilinear interpolation of four points in 3D space. It assumes - /// these 4 points form a plane. - /// \param[in] _a The list of points to interpolate. The list must have at - /// least 4 entries. Furthermore the order of the indices should be such - /// that every consecutive pair of indices lie on the same edge. Furthermore - /// the 4 points must be coplanar and corners of a rectangular patch. - /// \param[in] _start_index The starting index of points to interpolate. - /// \param[in] _lst An array of values that are to be used by the - /// interpolator. _lst[a.index] and _lst[b.index] are the values - /// to be interpolated. If a.index or b.index is std::nullopt then use the - /// default value. - /// \param[in] _pos The position to interpolate. - /// \param[in] _default The default value to use if a.index or b.index is - /// std::nullopt. - /// Warning: This function assumes that the indices of _a and _b correspond - /// to values in _lst. It performs no bounds checking whatsoever and if you - /// pass it invalid data, it will crash. - template - V BiLinearInterpolate( - const std::vector> &_a, - const std::size_t &_start_index, - const std::vector &_lst, - const Vector3 &_pos, - const V &_default = V(0)) - { - /// Assertions - #ifndef NDEBUG - assert(_a.size() >= _start_index + 4); - /// Check if all points co planar. - auto planeNormal = (_a[_start_index + 1].position - - _a[_start_index].position).Cross(_a[_start_index + 2].position - - _a[_start_index].position); - auto planeScalar = planeNormal.Dot(_a[_start_index].position); - assert( - !(abs(planeNormal.Dot(_a[_start_index + 3].position) - planeScalar) > 0) - ); - #endif - - // Project point onto line - std::vector linres; - auto n0 = _a[_start_index]; - auto n1 = _a[_start_index + 1]; - auto proj1 = n1.position-n0.position; - auto unitProj1 = proj1.Normalized(); - auto pos1 = - (_pos - n0.position).Dot(unitProj1) * unitProj1 + n0.position; - // Apply normal linear interpolation - linres.push_back(LinearInterpolate(n0, n1, _lst, pos1, _default)); - - // Project point onto second line - auto n2 = _a[_start_index + 2]; - auto n3 = _a[_start_index + 3]; - auto proj2 = n3.position-n2.position; - auto unitProj2 = proj2.Normalized(); - auto pos2 = - (_pos - n2.position).Dot(unitProj1) * unitProj1 + n2.position; - linres.push_back(LinearInterpolate(n2, n3, _lst, pos2, _default)); - - // Perform final linear interpolation - InterpolationPoint3D p1 { - pos1, - 0 - }; - - InterpolationPoint3D p2 { - pos2, - 1 - }; - return LinearInterpolate(p1, p2, linres, _pos, _default); - } - - /// \brief Project Point onto a plane. - /// \param[in] _points a vector of min length _start_index + 3. The points - /// at _start_index, _start_index + 1 and _start_index + 2 are used to - /// define the plane. - /// \param[in] _start_index defines the slice to use. - /// \param[in] _pos The position to project onto the plane - template - ignition::math::Vector3 ProjectPointToPlane( - const std::vector> _points, - const std::size_t &_start_index, - const Vector3 &_pos) - { - auto n = - (_points[_start_index + 1].position - _points[_start_index].position) - .Cross( - _points[_start_index + 2].position - _points[_start_index].position); - return - _pos - n.Dot(_pos - _points[_start_index].position) * n.Normalized(); - } - - /// \brief Trilinear interpolation of eight points in 3D space. It assumes - /// these eight points form a rectangular prism. - /// \param[in] _a The list of points to interpolate. The list must have 8 - /// points. The first 4 points must form a plane as must the last 4. - /// The order of the points within a plane should be such that consecutive - /// pairs of indices lie on the same edge. - /// \param[in] _lst An array of values that are to be used for interpolation - /// \param[in] _pos The position to interpolate. - /// \param[in] _default The default value to use if a.index or b.index is - /// std::nullopt. - template - V TrilinearInterpolate( - const std::vector> &_a, - const std::vector &_lst, - const Vector3 &_pos, - const V &_default = V(0)) - { - assert(_a.size() == 8); - - std::vector linres; - // First plane - auto pos1 = ProjectPointToPlane(_a, 0, _pos); - linres.push_back(BiLinearInterpolate(_a, 0, _lst, pos1, _default)); - - // Second plane - auto pos2 = ProjectPointToPlane(_a, 4, _pos); - linres.push_back(BiLinearInterpolate(_a, 4, _lst, pos2, _default)); - - // Perform final linear interpolation - InterpolationPoint3D p1 { - pos1, - 0 - }; - - InterpolationPoint3D p2 { - pos2, - 1 - }; - return LinearInterpolate(p1, p2, linres, _pos, _default); - } - } -} -#endif +#include diff --git a/src/InterpolationPoint_TEST.cc b/src/InterpolationPoint_TEST.cc index 01b27a370..e4ac0d999 100644 --- a/src/InterpolationPoint_TEST.cc +++ b/src/InterpolationPoint_TEST.cc @@ -14,7 +14,7 @@ * limitations under the License. * */ -#include +#include #include diff --git a/src/VolumetricGridLookupField_TEST.cc b/src/VolumetricGridLookupField_TEST.cc index 12cd6ce97..cf7a6555b 100644 --- a/src/VolumetricGridLookupField_TEST.cc +++ b/src/VolumetricGridLookupField_TEST.cc @@ -16,7 +16,7 @@ */ -#include +#include #include #include using namespace ignition;