Add PiecewiseScalarField3 class

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,70 @@
+/*
+ * 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 k = 1.;
+ const ignition::math::Polynomial3d px(
+ ignition::math::Vector4d(0., 1., 0., 1.));
+ const ignition::math::Polynomial3d qy(
+ ignition::math::Vector4d(1., 0., 1., 0.));
+ const ignition::math::Polynomial3d rz(
+ ignition::math::Vector4d(1., 0., 0., -1.));
+ using AdditivelySeparableScalarField3dT =
+ ignition::math::AdditivelySeparableScalarField3d<
+ ignition::math::Polynomial3d>;
+ using PiecewiseScalarField3dT =
+ ignition::math::PiecewiseScalarField3d<
+ AdditivelySeparableScalarField3dT>;
+ const PiecewiseScalarField3dT P({
+ {ignition::math::Region3d( // x < 0 halfspace
+ ignition::math::Intervald::Open(
+ -ignition::math::INF_D, 0.),
+ ignition::math::Intervald::Unbounded,
+ ignition::math::Intervald::Unbounded),
+ AdditivelySeparableScalarField3dT(k, px, qy, rz)},
+ {ignition::math::Region3d( // x >= 0 halfspace
+ ignition::math::Intervald::LeftClosed(
+ 0., ignition::math::INF_D),
+ ignition::math::Intervald::Unbounded,
+ ignition::math::Intervald::Unbounded),
+ AdditivelySeparableScalarField3dT(-k, px, qy, rz)}});
+
+ // 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) = " << P << std::endl;
+
+ // A piecewise scalar field can be evaluated.
+ std::cout << "Evaluating P(x, y, z) at (1, 0, 0) yields "
+ << P(ignition::math::Vector3d::UnitX)
+ << std::endl;
+ std::cout << "Evaluating P(x, y, z) at (-1, 0, 0) yields "
+ << P(-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 "
+ << P.Minimum() << std::endl;
+}
+//! [complete]

include/ignition/math/PiecewiseScalarField3.hh

@@ -0,0 +1,200 @@
+/*
+ * 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.
+ ///
+ /// ## Example
+ ///
+ /// \snippet examples/piecewise_scalar_field3_example.cc complete
+ template<typename ScalarField3T, typename ScalarT>
+ class IGNITION_MATH_VISIBLE 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 may not overlap.
+ public: explicit PiecewiseScalarField3(std::vector<Piece> _pieces)
+ : pieces(std::move(_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
+ 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()
+ 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