diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index bc440dbfa..edbb56e8b 100644 --- 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 new file mode 100644 index 000000000..c46ffd5ee --- /dev/null +++ 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 +#include +#include + +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 new file mode 100644 index 000000000..e05128a35 --- /dev/null +++ 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 +#include + +#include +#include +#include + +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 &) 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 + 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 &_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 &_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 &_region, + Vector3 &_pMin) const + { + if (_region.Empty()) + { + _pMin = Vector3::NaN; + return std::numeric_limits::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 &_region) const + { + Vector3 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 &_pMin) const + { + return this->Minimum(Region3::Unbounded, _pMin); + } + + /// \brief Compute scalar field minimum + /// \return the scalar field minimum + public: ScalarT Minimum() const + { + Vector3 pMin; + return this->Minimum(Region3::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::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 + using AdditivelySeparableScalarField3f = + AdditivelySeparableScalarField3; + + template + using AdditivelySeparableScalarField3d = + AdditivelySeparableScalarField3; + } + } +} +#endif diff --git a/src/AdditivelySeparableScalarField3_TEST.cc b/src/AdditivelySeparableScalarField3_TEST.cc new file mode 100644 index 000000000..bbd52d323 --- /dev/null +++ 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 +#include +#include + +#include "ignition/math/AdditivelySeparableScalarField3.hh" +#include "ignition/math/Polynomial3.hh" + +using namespace ignition; + +///////////////////////////////////////////////// +TEST(AdditivelySeparableScalarField3Test, Evaluate) +{ + using ScalarFunctionT = std::function; + using AdditivelySeparableScalarField3dT = + math::AdditivelySeparableScalarField3d; + 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; + 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; + { + 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)]"); + } +}