Add AdditivelySeparableScalarField3 class (#397)

Signed-off-by: Michel Hidalgo <[email protected]> Signed-off-by: Louise Poubel <[email protected]> Co-authored-by: Louise Poubel <[email protected]>
gazebosim · Jul 4, 2022 · 6a6cbd4 · 6a6cbd4
1 parent 519e794
commit 6a6cbd4
Show file tree

Hide file tree

Showing 4 changed files with 364 additions and 0 deletions.
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
@@ -6,6 +6,9 @@ project(ignition-math-examples)
 set(IGN_MATH_VER 6)
 find_package(ignition-math${IGN_MATH_VER} REQUIRED)
 
+add_executable(additively_separable_scalar_field3_example additively_separable_scalar_field3_example.cc)
+target_link_libraries(additively_separable_scalar_field3_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER})
+
 add_executable(angle_example angle_example.cc)
 target_link_libraries(angle_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER})
 

diff --git a/examples/additively_separable_scalar_field3_example.cc b/examples/additively_separable_scalar_field3_example.cc
@@ -0,0 +1,52 @@
+/*
+ * 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/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>;
+  const AdditivelySeparableScalarField3dT scalarField(
+      kConstant, xPoly, yPoly, zPoly);
+
+  // A printable, additively separable scalar field.
+  std::cout << "An additively separable scalar field in R^3 "
+            << "can be expressed as a sum of scalar functions "
+            << "e.g. F(x, y, z) = " << scalarField << std::endl;
+
+  // An additively separable scalar field can be evaluated.
+  std::cout << "Evaluating F(x, y, z) at (0, 1, 0) yields "
+            << scalarField(ignition::math::Vector3d::UnitY)
+            << std::endl;
+
+  // An additively separable scalar field can be queried for its
+  // minimum (provided the underlying scalar function allows it).
+  std::cout << "The global minimum of F(x, y, z) is "
+            << scalarField.Minimum() << std::endl;
+}
+//! [complete]
diff --git a/include/ignition/math/AdditivelySeparableScalarField3.hh b/include/ignition/math/AdditivelySeparableScalarField3.hh
@@ -0,0 +1,197 @@
+/*
+ * 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_SEPARABLE_SCALAR_FIELD3_HH_
+#define IGNITION_MATH_SEPARABLE_SCALAR_FIELD3_HH_
+
+#include <limits>
+#include <utility>
+
+#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 AdditivelySeparableScalarField3\
+     * AdditivelySeparableScalarField3.hh\
+     * ignition/math/AdditivelySeparableScalarField3.hh
+     */
+    /// \brief The AdditivelySeparableScalarField3 class constructs
+    /// a scalar field F in R^3 as a sum of scalar functions i.e.
+    /// F(x, y, z) = k (p(x) + q(y) + r(z)).
+    ///
+    /// \tparam ScalarFunctionT a callable type that taking a single ScalarT
+    ///   value as argument and returning another ScalarT value. Additionally:
+    ///   - for AdditivelySeparableScalarField3T to have a stream operator
+    ///     overload, ScalarFunctionT must implement a
+    ///     void Print(std::ostream &, const std::string &) method that streams
+    ///     a representation of it using the given string as argument variable
+    ///     name;
+    ///   - for AdditivelySeparableScalarField3T::Minimum to be callable,
+    ///     ScalarFunctionT must implement a
+    ///     ScalarT Minimum(const Interval<ScalarT> &, ScalarT &) method that
+    ///     computes its minimum in the given interval 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/additively_separable_scalar_field3_example.cc complete
+    template<typename ScalarFunctionT, typename ScalarT>
+    class AdditivelySeparableScalarField3
+    {
+      /// \brief Constructor
+      /// \param[in] _k scalar constant
+      /// \param[in] _p scalar function of x
+      /// \param[in] _q scalar function of y
+      /// \param[in] _r scalar function of z
+      public: AdditivelySeparableScalarField3(
+          ScalarT _k, ScalarFunctionT _p,
+          ScalarFunctionT _q, ScalarFunctionT _r)
+      : k(_k), p(std::move(_p)), q(std::move(_q)), r(std::move(_r))
+      {
+      }
+
+      /// \brief Evaluate the scalar field at `_point`
+      /// \param[in] _point scalar field argument
+      /// \return the result of evaluating `F(_point)`
+      public: ScalarT Evaluate(const Vector3<ScalarT> &_point) const
+      {
+        return this->k * (
+            this->p(_point.X()) +
+            this->q(_point.Y()) +
+            this->r(_point.Z()));
+      }
+
+      /// \brief Call operator overload
+      /// \see SeparableScalarField3::Evaluate()
+      /// \param[in] _point scalar field argument
+      /// \return the result of evaluating `F(_point)`
+      public: ScalarT operator()(const Vector3<ScalarT> &_point) const
+      {
+        return this->Evaluate(_point);
+      }
+
+      /// \brief Compute scalar field minimum in a `_region`
+      /// \param[in] _region scalar field argument set to check
+      /// \param[out] _pMin scalar field argument that yields
+      ///   the minimum, or NaN if `_region` is empty
+      /// \return the scalar field minimum in the given `_region`,
+      ///   or NaN if `_region` is empty
+      public: ScalarT Minimum(const Region3<ScalarT> &_region,
+                              Vector3<ScalarT> &_pMin) const
+      {
+        if (_region.Empty())
+        {
+          _pMin = Vector3<ScalarT>::NaN;
+          return std::numeric_limits<ScalarT>::quiet_NaN();
+        }
+        return this->k * (
+            this->p.Minimum(_region.Ix(), _pMin.X()) +
+            this->q.Minimum(_region.Iy(), _pMin.Y()) +
+            this->r.Minimum(_region.Iz(), _pMin.Z()));
+      }
+
+      /// \brief Compute scalar field minimum in a `_region`
+      /// \param[in] _region scalar field argument set to check
+      /// \return the scalar field minimum in the given `_region`,
+      ///   or NaN if `_region` is empty
+      public: ScalarT Minimum(const Region3<ScalarT> &_region) const
+      {
+        Vector3<ScalarT> pMin;
+        return this->Minimum(_region, pMin);
+      }
+
+      /// \brief Compute scalar field minimum
+      /// \param[out] _pMin scalar field argument that yields
+      ///   the minimum, or NaN if `_region` is empty
+      /// \return the scalar field minimum
+      public: ScalarT Minimum(Vector3<ScalarT> &_pMin) const
+      {
+        return this->Minimum(Region3<ScalarT>::Unbounded, _pMin);
+      }
+
+      /// \brief Compute scalar field minimum
+      /// \return the scalar field minimum
+      public: ScalarT Minimum() const
+      {
+        Vector3<ScalarT> pMin;
+        return this->Minimum(Region3<ScalarT>::Unbounded, 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::AdditivelySeparableScalarField3<
+          ScalarFunctionT, ScalarT> &_field)
+      {
+        using std::abs;  // enable ADL
+        constexpr ScalarT epsilon =
+            std::numeric_limits<ScalarT>::epsilon();
+        if ((abs(_field.k) - ScalarT(1)) < epsilon)
+        {
+          if (_field.k < ScalarT(0))
+          {
+            _out << "-";
+          }
+        }
+        else
+        {
+          _out << _field.k << " ";
+        }
+        _out << "[(";
+        _field.p.Print(_out, "x");
+        _out << ") + (";
+        _field.q.Print(_out, "y");
+        _out << ") + (";
+        _field.r.Print(_out, "z");
+        return _out << ")]";
+      }
+
+      /// \brief Scalar constant
+      private: ScalarT k;
+
+      /// \brief Scalar function of x
+      private: ScalarFunctionT p;
+
+      /// \brief Scalar function of y
+      private: ScalarFunctionT q;
+
+      /// \brief Scalar function of z
+      private: ScalarFunctionT r;
+    };
+
+    template<typename ScalarFunctionT>
+    using AdditivelySeparableScalarField3f =
+        AdditivelySeparableScalarField3<ScalarFunctionT, float>;
+
+    template<typename ScalarFunctionT>
+    using AdditivelySeparableScalarField3d =
+        AdditivelySeparableScalarField3<ScalarFunctionT, double>;
+    }
+  }
+}
+#endif
diff --git a/src/AdditivelySeparableScalarField3_TEST.cc b/src/AdditivelySeparableScalarField3_TEST.cc
@@ -0,0 +1,112 @@
+/*
+ * 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.
+ *
+*/
+#include <gtest/gtest.h>
+#include <functional>
+#include <ostream>
+
+#include "ignition/math/AdditivelySeparableScalarField3.hh"
+#include "ignition/math/Polynomial3.hh"
+
+using namespace ignition;
+
+/////////////////////////////////////////////////
+TEST(AdditivelySeparableScalarField3Test, Evaluate)
+{
+  using ScalarFunctionT = std::function<double(double)>;
+  using AdditivelySeparableScalarField3dT =
+      math::AdditivelySeparableScalarField3d<ScalarFunctionT>;
+  const double kConstant = 1.;
+  auto xyzFunc = [](double x) { return x; };
+  const AdditivelySeparableScalarField3dT scalarField(
+      kConstant, xyzFunc, xyzFunc, xyzFunc);
+  EXPECT_DOUBLE_EQ(scalarField(math::Vector3d::Zero), 0.);
+  EXPECT_DOUBLE_EQ(scalarField(math::Vector3d::One), 3.);
+  EXPECT_DOUBLE_EQ(scalarField(math::Vector3d::UnitX), 1.);
+  EXPECT_DOUBLE_EQ(scalarField(math::Vector3d::UnitY), 1.);
+  EXPECT_DOUBLE_EQ(scalarField(math::Vector3d::UnitZ), 1.);
+  const math::Vector3d INF_V(
+      math::INF_D, math::INF_D, math::INF_D);
+  EXPECT_DOUBLE_EQ(scalarField(INF_V), math::INF_D);
+}
+
+/////////////////////////////////////////////////
+TEST(AdditivelySeparableScalarField3Test, Minimum)
+{
+  using AdditivelySeparableScalarField3dT =
+      math::AdditivelySeparableScalarField3d<math::Polynomial3d>;
+  constexpr double kConstant = 1. / 3.;
+  const math::Polynomial3d xyzPoly(math::Vector4d(0., 1., 1., 1));
+  const AdditivelySeparableScalarField3dT scalarField(
+      kConstant, xyzPoly, xyzPoly, xyzPoly);
+  {
+    const math::Region3d region =
+        math::Region3d::Open(0., 0., 0., 0., 0., 0.);
+    math::Vector3d pMin = math::Vector3d::Zero;
+    EXPECT_TRUE(std::isnan(scalarField.Minimum(region)));
+    EXPECT_TRUE(std::isnan(scalarField.Minimum(region, pMin)));
+    EXPECT_TRUE(std::isnan(pMin.X()));
+    EXPECT_TRUE(std::isnan(pMin.Y()));
+    EXPECT_TRUE(std::isnan(pMin.Z()));
+  }
+  {
+    math::Vector3d pMin = math::Vector3d::NaN;
+    EXPECT_DOUBLE_EQ(scalarField.Minimum(), 0.75);
+    EXPECT_DOUBLE_EQ(scalarField.Minimum(pMin), 0.75);
+    EXPECT_EQ(pMin, -0.5 * math::Vector3d::One);
+  }
+  {
+    const math::Region3d region =
+        math::Region3d::Open(1., 1., 1., 2., 2., 2.);
+    math::Vector3d pMin = math::Vector3d::NaN;
+    EXPECT_DOUBLE_EQ(scalarField.Minimum(region), 3.);
+    EXPECT_DOUBLE_EQ(scalarField.Minimum(region, pMin), 3.);
+    EXPECT_EQ(pMin, math::Vector3d::One);
+  }
+}
+
+/////////////////////////////////////////////////
+TEST(AdditivelySeparableScalarField3Test, Stream)
+{
+  using AdditivelySeparableScalarField3dT =
+      math::AdditivelySeparableScalarField3d<math::Polynomial3d>;
+  {
+    constexpr double kConstant = 1.;
+    const math::Polynomial3d xyzPoly(math::Vector4d(0., 1., 0., 1));
+    std::ostringstream os;
+    os << AdditivelySeparableScalarField3dT(
+        kConstant, xyzPoly, xyzPoly, xyzPoly);
+    EXPECT_EQ(os.str(), "[(x^2 + 1) + (y^2 + 1) + (z^2 + 1)]");
+  }
+  {
+    constexpr double kConstant = -1.;
+    const math::Polynomial3d xyzPoly(math::Vector4d(1., 0., 1., 0));
+    std::ostringstream os;
+    os << AdditivelySeparableScalarField3dT(
+        kConstant, xyzPoly, xyzPoly, xyzPoly);
+    EXPECT_EQ(os.str(), "-[(x^3 + x) + (y^3 + y) + (z^3 + z)]");
+  }
+  {
+    constexpr double kConstant = 3.;
+    const math::Polynomial3d xPoly(math::Vector4d(0., 1., 0., 0));
+    const math::Polynomial3d yPoly(math::Vector4d(-1., 0., 0., 0));
+    const math::Polynomial3d zPoly(math::Vector4d(0., 0., 0., 1));
+    std::ostringstream os;
+    os << AdditivelySeparableScalarField3dT(
+        kConstant, xPoly, yPoly, zPoly);
+    EXPECT_EQ(os.str(), "3 [(x^2) + (-y^3) + (1)]");
+  }
+}