From d17cd9b72612ae4908f953061a1aa4085de93e88 Mon Sep 17 00:00:00 2001 From: Michel Hidalgo Date: Mon, 14 Mar 2022 21:30:02 -0300 Subject: [PATCH] Add PiecewiseScalarField3 class Signed-off-by: Michel Hidalgo --- examples/CMakeLists.txt | 3 + examples/piecewise_scalar_field3_example.cc | 70 ++++++ .../ignition/math/PiecewiseScalarField3.hh | 200 ++++++++++++++++++ src/PiecewiseScalarField3_TEST.cc | 181 ++++++++++++++++ 4 files changed, 454 insertions(+) create mode 100644 examples/piecewise_scalar_field3_example.cc create mode 100644 include/ignition/math/PiecewiseScalarField3.hh create mode 100644 src/PiecewiseScalarField3_TEST.cc diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 720ed66ac..a2bd49e71 100644 --- a/examples/CMakeLists.txt +++ b/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}) diff --git a/examples/piecewise_scalar_field3_example.cc b/examples/piecewise_scalar_field3_example.cc new file mode 100644 index 000000000..e7d90d12b --- /dev/null +++ b/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 + +#include +#include +#include + +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] diff --git a/include/ignition/math/PiecewiseScalarField3.hh b/include/ignition/math/PiecewiseScalarField3.hh new file mode 100644 index 000000000..9b42904c9 --- /dev/null +++ b/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 +#include +#include +#include +#include + +#include +#include +#include + +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 + 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 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 _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({ + {Region3::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 &_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::quiet_NaN(); + } + return it->field(_p); + } + + /// \brief Call operator overload + /// \see PiecewiseScalarField3::Evaluate() + public: ScalarT operator()(const Vector3 &_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 &_pMin) const + { + if (this->pieces.empty()) { + _pMin = Vector3::NaN; + return std::numeric_limits::quiet_NaN(); + } + ScalarT yMin = std::numeric_limits::infinity(); + for (const Piece &piece : this->pieces) + { + if (!piece.region.Empty()) + { + Vector3 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 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 pieces; + }; + + template + using PiecewiseScalarField3f = PiecewiseScalarField3; + template + using PiecewiseScalarField3d = PiecewiseScalarField3; + } + } +} + +#endif diff --git a/src/PiecewiseScalarField3_TEST.cc b/src/PiecewiseScalarField3_TEST.cc new file mode 100644 index 000000000..8a78d9045 --- /dev/null +++ b/src/PiecewiseScalarField3_TEST.cc @@ -0,0 +1,181 @@ +/* + * 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 + +#include "ignition/math/AdditivelySeparableScalarField3.hh" +#include "ignition/math/PiecewiseScalarField3.hh" +#include "ignition/math/Polynomial3.hh" + +using namespace ignition; + +///////////////////////////////////////////////// +TEST(PiecewiseScalarField3Test, Evaluate) +{ + using ScalarField3dT = std::function; + using PiecewiseScalarField3dT = math::PiecewiseScalarField3d; + { + const PiecewiseScalarField3dT F; + EXPECT_TRUE(std::isnan(F(math::Vector3d::Zero))); + } + { + const PiecewiseScalarField3dT F = + PiecewiseScalarField3dT::Throughout( + [](const math::Vector3d& v) { return v.X(); }); + EXPECT_DOUBLE_EQ(F(math::Vector3d::Zero), 0.); + EXPECT_DOUBLE_EQ(F(math::Vector3d(0.5, 0.5, 0.5)), 0.5); + EXPECT_DOUBLE_EQ(F(math::Vector3d::One), 1.); + EXPECT_DOUBLE_EQ(F(math::Vector3d::UnitX), 1.); + } + { + const math::Region3d region0 = + math::Region3d::Open(0., 0., 0., 0., 0., 0.); + auto field0 = [](const math::Vector3d&) { return 1.; }; + const math::Region3d region1 = + math::Region3d::Closed(0., 0., 0., 1., 1., 1.); + auto field1 = [](const math::Vector3d& v) { return v.X(); }; + const PiecewiseScalarField3dT F({ + {region0, field0}, {region1, field1}}); + EXPECT_DOUBLE_EQ(F(math::Vector3d::Zero), 0.); + } + { + const math::Region3d region0 = + math::Region3d::Closed(0., 0., 0., 0., 0., 0.); + auto field0 = [](const math::Vector3d&) { return 1.; }; + const math::Region3d region1 = + math::Region3d::Closed(0., 0., 0., 1., 1., 1.); + auto field1 = [](const math::Vector3d& v) { return v.X(); }; + const PiecewiseScalarField3dT F({ + {region0, field0}, {region1, field1}}); + EXPECT_DOUBLE_EQ(F(math::Vector3d::Zero), 1.); + } + { + const math::Region3d region0 = + math::Region3d::Open(0., 0., 0., 1., 1., 1.); + auto field0 = [](const math::Vector3d& v) { return v.X(); }; + const math::Region3d region1 = + math::Region3d::Open(-1., -1., -1., 0., 0., 0.); + auto field1 = [](const math::Vector3d& v) { return v.Y(); }; + const PiecewiseScalarField3dT F({ + {region0, field0}, {region1, field1}}); + EXPECT_DOUBLE_EQ(F(math::Vector3d(0.5, 0.25, 0.5)), 0.5); + EXPECT_DOUBLE_EQ(F(math::Vector3d(-0.5, -0.25, -0.5)), -0.25); + EXPECT_TRUE(std::isnan(F(math::Vector3d(0.5, -0.25, 0.5)))); + EXPECT_TRUE(std::isnan(F(math::Vector3d(-0.5, 0.25, -0.5)))); + } +} + +///////////////////////////////////////////////// +TEST(PiecewiseScalarField3Test, Minimum) +{ + using AdditivelySeparableScalarField3dT = + math::AdditivelySeparableScalarField3d; + using PiecewiseScalarField3dT = + math::PiecewiseScalarField3d; + { + const PiecewiseScalarField3dT F; + math::Vector3d pMin = math::Vector3d::Zero; + EXPECT_TRUE(std::isnan(F.Minimum())); + EXPECT_TRUE(std::isnan(F.Minimum(pMin))); + EXPECT_TRUE(std::isnan(pMin.X())); + EXPECT_TRUE(std::isnan(pMin.Y())); + EXPECT_TRUE(std::isnan(pMin.Z())); + } + { + const PiecewiseScalarField3dT F = + PiecewiseScalarField3dT::Throughout( + AdditivelySeparableScalarField3dT( + 1., + math::Polynomial3d::Constant(0.), + math::Polynomial3d::Constant(1.), + math::Polynomial3d::Constant(0.))); + math::Vector3d pMin = math::Vector3d::NaN; + EXPECT_DOUBLE_EQ(F.Minimum(), 1.); + EXPECT_DOUBLE_EQ(F.Minimum(pMin), 1.); + EXPECT_FALSE(std::isnan(pMin.X())); + EXPECT_FALSE(std::isnan(pMin.Y())); + EXPECT_FALSE(std::isnan(pMin.Z())); + } + { + const math::Region3d region0 = + math::Region3d::Open(0., 0., 0., 1., 1., 1.); + const AdditivelySeparableScalarField3dT field0( + 1., + math::Polynomial3d( + math::Vector4d(0., 1., 0., 0.)), + math::Polynomial3d::Constant(0.), + math::Polynomial3d::Constant(0.)); + const math::Region3d region1 = + math::Region3d::Open(-1., -1., -1., 0., 0., 0.); + const AdditivelySeparableScalarField3dT field1( + 1., + math::Polynomial3d::Constant(0.), + math::Polynomial3d( + math::Vector4d(0., 1., 0., 1.)), + math::Polynomial3d::Constant(0.)); + const PiecewiseScalarField3dT F({ + {region0, field0}, {region1, field1}}); + math::Vector3d pMin = math::Vector3d::NaN; + EXPECT_DOUBLE_EQ(F.Minimum(), 0.); + EXPECT_DOUBLE_EQ(F.Minimum(pMin), 0.); + EXPECT_EQ(pMin, math::Vector3d::Zero); + } +} + + +///////////////////////////////////////////////// +TEST(PiecewiseScalarField3Test, Stream) +{ + using AdditivelySeparableScalarField3dT = + math::AdditivelySeparableScalarField3d; + using PiecewiseScalarField3dT = + math::PiecewiseScalarField3d; + { + std::ostringstream output; + output << PiecewiseScalarField3dT(); + EXPECT_EQ(output.str(), "undefined"); + } + { + std::ostringstream output; + std::ostringstream expected; + const math::Region3d region0( + math::Intervald::Unbounded, + math::Intervald::Unbounded, + math::Intervald::Open(-math::INF_D, 0.)); + const AdditivelySeparableScalarField3dT field0( + 1., + math::Polynomial3d(math::Vector4d(0., 1., 0., 0.)), + math::Polynomial3d(math::Vector4d(0., 1., 0., 0.)), + math::Polynomial3d(math::Vector4d(0., 1., 0., 0.))); + expected << "[(x^2) + (y^2) + (z^2)] if (x, y, z) " + << "in (-inf, inf) x (-inf, inf) x (-inf, 0); "; + const math::Region3d region1( + math::Intervald::Unbounded, + math::Intervald::Unbounded, + math::Intervald::LeftClosed(0., math::INF_D)); + const AdditivelySeparableScalarField3dT field1( + -1., + math::Polynomial3d(math::Vector4d(0., 0., 1., 0.)), + math::Polynomial3d(math::Vector4d(0., 0., 1., 0.)), + math::Polynomial3d(math::Vector4d(0., 0., 1., 0.))); + expected << "-[(x) + (y) + (z)] if (x, y, z) " + << "in (-inf, inf) x (-inf, inf) x [0, inf)"; + const PiecewiseScalarField3dT F({ + {region0, field0}, {region1, field1}}); + output << F; + EXPECT_EQ(output.str(), expected.str()); + } +}