Merge branch 'main' into drop_mwf_pimpl

Migration.md

@@ -29,6 +29,11 @@ release will remove the deprecated code.
     + ***Deprecation:*** public: void Col(unsigned int, const Vector3<T> &)
     + ***Replacement:*** public: void SetCol(unsigned int, const Vector3<T> &)
 
+1. **Pose3.hh**
+    + The addition operators `+` and `+=` are deprecated in favor of multiplication
+      operators `*` and `*=`, though the order of operands is reversed
+      (A + B = B * A).
+
 1. **Quaternion.hh**
     + ***Deprecation:*** public: void Axis(T, T, T, T)
     + ***Replacement:*** public: void SetFromAxisAngle(T, T, T, T)

examples/CMakeLists.txt

@@ -6,6 +6,9 @@ project(ignition-math-examples)
 set(IGN_MATH_VER 7)
 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})
 
@@ -33,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/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]

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/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