Add PiecewiseScalarField3 class (#398)

Signed-off-by: Michel Hidalgo <[email protected]>

examples/CMakeLists.txt

@@ -36,6 +36,9 @@ target_link_libraries(kmeans ignition-math${IGN_MATH_VER}::ignition-math${IGN_MA
 add_executable(matrix3_example matrix3_example.cc)
 target_link_libraries(matrix3_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER})
 
+add_executable(piecewise_scalar_field3_example piecewise_scalar_field3_example.cc)
+target_link_libraries(piecewise_scalar_field3_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER})
+
 add_executable(polynomial3_example polynomial3_example.cc)
 target_link_libraries(polynomial3_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER})
 

examples/piecewise_scalar_field3_example.cc

@@ -0,0 +1,73 @@
+/*
+ * 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.
+ *
+*/
+//! [complete]
+#include <iostream>
+
+#include <ignition/math/AdditivelySeparableScalarField3.hh>
+#include <ignition/math/PiecewiseScalarField3.hh>
+#include <ignition/math/Polynomial3.hh>
+
+int main(int argc, char **argv)
+{
+  const double kConstant = 1.;
+  const ignition::math::Polynomial3d xPoly(
+      ignition::math::Vector4d(0., 1., 0., 1.));
+  const ignition::math::Polynomial3d yPoly(
+      ignition::math::Vector4d(1., 0., 1., 0.));
+  const ignition::math::Polynomial3d zPoly(
+      ignition::math::Vector4d(1., 0., 0., -1.));
+  using AdditivelySeparableScalarField3dT =
+      ignition::math::AdditivelySeparableScalarField3d<
+        ignition::math::Polynomial3d>;
+  using PiecewiseScalarField3dT =
+      ignition::math::PiecewiseScalarField3d<
+        AdditivelySeparableScalarField3dT>;
+  const PiecewiseScalarField3dT scalarField({
+      {ignition::math::Region3d(  // x < 0 halfspace
+          ignition::math::Intervald::Open(
+              -ignition::math::INF_D, 0.),
+          ignition::math::Intervald::Unbounded,
+          ignition::math::Intervald::Unbounded),
+       AdditivelySeparableScalarField3dT(
+           kConstant, xPoly, yPoly, zPoly)},
+      {ignition::math::Region3d(  // x >= 0 halfspace
+          ignition::math::Intervald::LeftClosed(
+              0., ignition::math::INF_D),
+          ignition::math::Intervald::Unbounded,
+          ignition::math::Intervald::Unbounded),
+       AdditivelySeparableScalarField3dT(
+           -kConstant, xPoly, yPoly, zPoly)}});
+
+  // A printable piecewise scalar field.
+  std::cout << "A piecewise scalar field in R^3 is made up of "
+            << "several pieces e.g. P(x, y, z) = "
+            << scalarField << std::endl;
+
+  // A piecewise scalar field can be evaluated.
+  std::cout << "Evaluating P(x, y, z) at (1, 0, 0) yields "
+            << scalarField(ignition::math::Vector3d::UnitX)
+            << std::endl;
+  std::cout << "Evaluating P(x, y, z) at (-1, 0, 0) yields "
+            << scalarField(-ignition::math::Vector3d::UnitX)
+            << std::endl;
+
+  // A piecewise scalar field can be queried for its minimum
+  // (provided the underlying scalar function allows it).
+  std::cout << "The global minimum of P(x, y, z) is "
+            << scalarField.Minimum() << std::endl;
+}
+//! [complete]

include/ignition/math/PiecewiseScalarField3.hh

@@ -0,0 +1,219 @@
+/*
+ * 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_PIECEWISE_SCALAR_FIELD3_HH_
+#define IGNITION_MATH_PIECEWISE_SCALAR_FIELD3_HH_
+
+#include <algorithm>
+#include <iostream>
+#include <limits>
+#include <utility>
+#include <vector>
+
+#include <ignition/math/Region3.hh>
+#include <ignition/math/Vector3.hh>
+#include <ignition/math/config.hh>
+
+namespace ignition
+{
+  namespace math
+  {
+    // Inline bracket to help doxygen filtering.
+    inline namespace IGNITION_MATH_VERSION_NAMESPACE {
+    //
+    /** \class PiecewiseScalarField3 PiecewiseScalarField3.hh\
+     * ignition/math/PiecewiseScalarField3.hh
+     */
+    /// \brief The PiecewiseScalarField3 class constructs a scalar field F
+    /// in R^3 as a union of scalar fields Pn, defined over regions Rn i.e.
+    /// piecewise.
+    ///
+    /// \tparam ScalarField3T a callable type taking a single Vector3<ScalarT>
+    ///   value as argument and returning a ScalarT value. Additionally:
+    ///   - for PiecewiseScalarField3 to have a stream operator overload,
+    ///     ScalarField3T must support stream operator overload;
+    ///   - for PiecewiseScalarField3::Minimum to be callable, ScalarField3T
+    ///     must implement a
+    ///     ScalarT Minimum(const Region3<ScalarT> &, Vector3<ScalarT> &)
+    ///     method that computes its minimum in the given region and returns
+    ///     an argument value that yields said minimum.
+    /// \tparam ScalarT a numeric type for which std::numeric_limits<> traits
+    ///   have been specialized.
+    ///
+    /// ## Example
+    ///
+    /// \snippet examples/piecewise_scalar_field3_example.cc complete
+    template<typename ScalarField3T, typename ScalarT>
+    class PiecewiseScalarField3
+    {
+      /// \brief A scalar field P in R^3 and
+      /// the region R in which it is defined
+      public: struct Piece {
+        Region3<ScalarT> region;
+        ScalarField3T field;
+      };
+
+      /// \brief Constructor
+      public: PiecewiseScalarField3() = default;
+
+      /// \brief Constructor
+      /// \param[in] _pieces scalar fields Pn and the regions Rn
+      ///   in which these are defined. Regions should not overlap.
+      public: explicit PiecewiseScalarField3(const std::vector<Piece> &_pieces)
+      : pieces(_pieces)
+      {
+        for (size_t i = 0; i < pieces.size(); ++i)
+        {
+          if (pieces[i].region.Empty())
+          {
+            std::cerr << "Region #" << i << " (" << pieces[i].region
+                      << ") in piecewise scalar field definition is empty."
+                      << std::endl;
+          }
+          for (size_t j = i + 1; j < pieces.size(); ++j)
+          {
+            if (pieces[i].region.Intersects(pieces[j].region))
+            {
+              std::cerr << "Detected overlap between regions in "
+                        << "piecewise scalar field definition: "
+                        << "region #" << i << " (" << pieces[i].region
+                        << ") overlaps with region #" << j << " ("
+                        << pieces[j].region << "). Region #" << i
+                        << " will take precedence when overlapping."
+                        << std::endl;
+            }
+          }
+        }
+      }
+
+      /// \brief Define piecewise scalar field as `_field` throughout R^3 space
+      /// \param[in] _field a scalar field in R^3
+      /// \return `_field` as a piecewise scalar field
+      public: static PiecewiseScalarField3 Throughout(ScalarField3T _field)
+      {
+        return PiecewiseScalarField3<ScalarField3T, ScalarT>({
+            {Region3<ScalarT>::Unbounded, std::move(_field)}});
+      }
+
+      /// \brief Evaluate the piecewise scalar field at `_p`
+      /// \param[in] _p piecewise scalar field argument
+      /// \return the result of evaluating `F(_p)`, or NaN
+      ///   if the scalar field is not defined at `_p`
+      public: ScalarT Evaluate(const Vector3<ScalarT> &_p) const
+      {
+        auto it = std::find_if(
+            this->pieces.begin(), this->pieces.end(),
+            [&](const Piece &piece)
+            {
+              return piece.region.Contains(_p);
+            });
+        if (it == this->pieces.end())
+        {
+          return std::numeric_limits<ScalarT>::quiet_NaN();
+        }
+        return it->field(_p);
+      }
+
+      /// \brief Call operator overload
+      /// \see PiecewiseScalarField3::Evaluate()
+      /// \param[in] _p piecewise scalar field argument
+      /// \return the result of evaluating `F(_p)`, or NaN
+      ///   if the scalar field is not defined at `_p`
+      public: ScalarT operator()(const Vector3<ScalarT> &_p) const
+      {
+        return this->Evaluate(_p);
+      }
+
+      /// \brief Compute the piecewise scalar field minimum
+      /// Note that, since this method computes the minimum
+      /// for each region independently, it implicitly assumes
+      /// continuity in the boundaries between regions, if any.
+      /// \param[out] _pMin scalar field argument that yields
+      ///   the minimum, or NaN if the scalar field is not
+      ///   defined anywhere (i.e. default constructed)
+      /// \return the scalar field minimum, or NaN if the
+      ///   scalar field is not defined anywhere (i.e. default
+      ///   constructed)
+      public: ScalarT Minimum(Vector3<ScalarT> &_pMin) const
+      {
+        if (this->pieces.empty())
+        {
+          _pMin = Vector3<ScalarT>::NaN;
+          return std::numeric_limits<ScalarT>::quiet_NaN();
+        }
+        ScalarT yMin = std::numeric_limits<ScalarT>::infinity();
+        for (const Piece &piece : this->pieces)
+        {
+          if (!piece.region.Empty())
+          {
+            Vector3<ScalarT> p;
+            const ScalarT y = piece.field.Minimum(piece.region, p);
+            if (y < yMin)
+            {
+              _pMin = p;
+              yMin = y;
+            }
+          }
+        }
+        return yMin;
+      }
+
+      /// \brief Compute the piecewise scalar field minimum
+      /// \return the scalar field minimum, or NaN if the
+      ///   scalar field is not defined anywhere (i.e. default
+      ///   constructed)
+      public: ScalarT Minimum() const
+      {
+        Vector3<ScalarT> pMin;
+        return this->Minimum(pMin);
+      }
+
+      /// \brief Stream insertion operator
+      /// \param _out output stream
+      /// \param _field SeparableScalarField3 to output
+      /// \return the stream
+      public: friend std::ostream &operator<<(
+          std::ostream &_out,
+          const ignition::math::PiecewiseScalarField3<
+          ScalarField3T, ScalarT> &_field)
+      {
+        if (_field.pieces.empty())
+        {
+          return _out << "undefined";
+        }
+        for (size_t i = 0; i < _field.pieces.size() - 1; ++i)
+        {
+          _out << _field.pieces[i].field << " if (x, y, z) in "
+               << _field.pieces[i].region << "; ";
+        }
+        return _out << _field.pieces.back().field
+                    << " if (x, y, z) in "
+                    << _field.pieces.back().region;
+      }
+
+      /// \brief Scalar fields Pn and the regions Rn in which these are defined
+      private: std::vector<Piece> pieces;
+    };
+
+    template<typename ScalarField3T>
+    using PiecewiseScalarField3f = PiecewiseScalarField3<ScalarField3T, float>;
+    template<typename ScalarField3T>
+    using PiecewiseScalarField3d = PiecewiseScalarField3<ScalarField3T, double>;
+    }
+  }
+}
+
+#endif