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 · Mar 29, 2022 · dcde2c0 · dcde2c0
1 parent 71b6db2
commit dcde2c0
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 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})
 

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)]");
+ }
+}