diff --git a/Migration.md b/Migration.md index 7cb3297b3..44e742a80 100644 --- a/Migration.md +++ b/Migration.md @@ -29,6 +29,11 @@ release will remove the deprecated code. + ***Deprecation:*** public: void Col(unsigned int, const Vector3 &) + ***Replacement:*** public: void SetCol(unsigned int, const Vector3 &) +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) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 27d306cbe..a2bd49e71 100644 --- 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}) @@ -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}) 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/examples/piecewise_scalar_field3_example.cc b/examples/piecewise_scalar_field3_example.cc new file mode 100644 index 000000000..d584ff09c --- /dev/null +++ b/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 + +#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>; + 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] 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/include/ignition/math/Interval.hh b/include/ignition/math/Interval.hh index fb0229b10..115db95dc 100644 --- a/include/ignition/math/Interval.hh +++ b/include/ignition/math/Interval.hh @@ -51,11 +51,14 @@ namespace ignition /// \brief Constructor /// \param[in] _leftValue leftmost interval value - /// \param[in] _leftClosed whether the interval is left-closed or not + /// \param[in] _leftClosed whether the interval is + /// left-closed or not /// \param[in] _rightValue rightmost interval value - /// \param[in] _rightClosed whether the interval is right-closed or not - public: Interval(T _leftValue, bool _leftClosed, - T _rightValue, bool _rightClosed) + /// \param[in] _rightClosed whether the interval + /// is right-closed or not + public: constexpr Interval( + T _leftValue, bool _leftClosed, + T _rightValue, bool _rightClosed) : leftValue(std::move(_leftValue)), rightValue(std::move(_rightValue)), leftClosed(_leftClosed), @@ -67,7 +70,8 @@ namespace ignition /// \param[in] _leftValue leftmost interval value /// \param[in] _rightValue rightmost interval value /// \return the open interval - public: static Interval Open(T _leftValue, T _rightValue) + public: static constexpr Interval + Open(T _leftValue, T _rightValue) { return Interval( std::move(_leftValue), false, @@ -78,7 +82,8 @@ namespace ignition /// \param[in] _leftValue leftmost interval value /// \param[in] _rightValue rightmost interval value /// \return the left-closed interval - public: static Interval LeftClosed(T _leftValue, T _rightValue) + public: static constexpr Interval + LeftClosed(T _leftValue, T _rightValue) { return Interval( std::move(_leftValue), true, @@ -89,7 +94,8 @@ namespace ignition /// \param[in] _leftValue leftmost interval value /// \param[in] _rightValue rightmost interval value /// \return the left-closed interval - public: static Interval RightClosed(T _leftValue, T _rightValue) + public: static constexpr Interval + RightClosed(T _leftValue, T _rightValue) { return Interval( std::move(_leftValue), false, @@ -100,7 +106,8 @@ namespace ignition /// \param[in] _leftValue leftmost interval value /// \param[in] _rightValue rightmost interval value /// \return the closed interval - public: static Interval Closed(T _leftValue, T _rightValue) + public: static constexpr Interval + Closed(T _leftValue, T _rightValue) { return Interval{ std::move(_leftValue), true, @@ -276,7 +283,7 @@ namespace ignition namespace detail { template - const Interval gUnboundedInterval = + constexpr Interval gUnboundedInterval = Interval::Open(-std::numeric_limits::infinity(), std::numeric_limits::infinity()); } // namespace detail diff --git a/include/ignition/math/PiecewiseScalarField3.hh b/include/ignition/math/PiecewiseScalarField3.hh new file mode 100644 index 000000000..ce938d1bf --- /dev/null +++ b/include/ignition/math/PiecewiseScalarField3.hh @@ -0,0 +1,219 @@ +/* + * 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. + /// + /// \tparam ScalarField3T a callable type taking a single Vector3 + /// value as argument and returning a ScalarT value. Additionally: + /// - for PiecewiseScalarField3 to have a stream operator overload, + /// ScalarField3T must support stream operator overload; + /// - for PiecewiseScalarField3::Minimum to be callable, ScalarField3T + /// must implement a + /// ScalarT Minimum(const Region3 &, Vector3 &) + /// method that computes its minimum in the given region 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/piecewise_scalar_field3_example.cc complete + template + class 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 should not overlap. + public: explicit PiecewiseScalarField3(const std::vector &_pieces) + : pieces(_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 + /// \param[in] _field a scalar field in R^3 + /// \return `_field` as a piecewise scalar field + 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() + /// \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 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/include/ignition/math/Pose3.hh b/include/ignition/math/Pose3.hh index 247bb7054..8e6eb14fd 100644 --- a/include/ignition/math/Pose3.hh +++ b/include/ignition/math/Pose3.hh @@ -183,7 +183,7 @@ namespace ignition /// then, B + A is the transform from O to Q specified in frame O /// \param[in] _pose Pose3 to add to this pose. /// \return The resulting pose. - public: Pose3 operator+(const Pose3 &_pose) const + public: IGN_DEPRECATED(7) Pose3 operator+(const Pose3 &_pose) const { Pose3 result; @@ -197,7 +197,8 @@ namespace ignition /// \param[in] _pose Pose3 to add to this pose. /// \sa operator+(const Pose3 &_pose) const. /// \return The resulting pose. - public: const Pose3 &operator+=(const Pose3 &_pose) + public: IGN_DEPRECATED(7) const Pose3 & + operator+=(const Pose3 &_pose) { this->p = this->CoordPositionAdd(_pose); this->q = this->CoordRotationAdd(_pose.q); diff --git a/include/ignition/math/Region3.hh b/include/ignition/math/Region3.hh index 5d5bb4f8d..339a9d91b 100644 --- a/include/ignition/math/Region3.hh +++ b/include/ignition/math/Region3.hh @@ -60,7 +60,8 @@ namespace ignition /// \param[in] _ix x-axis interval /// \param[in] _iy y-axis interval /// \param[in] _iz z-axis interval - public: Region3(Interval _ix, Interval _iy, Interval _iz) + public: constexpr Region3( + Interval _ix, Interval _iy, Interval _iz) : ix(std::move(_ix)), iy(std::move(_iy)), iz(std::move(_iz)) { } @@ -74,7 +75,7 @@ namespace ignition /// \param[in] _zRight righmost z-axis interval value /// \return the (`_xLeft`, `_xRight`) ✕ (`_yLeft`, `_yRight`) /// ✕ (`_zLeft`, `_zRight`) open region - public: static Region3 Open( + public: static constexpr Region3 Open( T _xLeft, T _yLeft, T _zLeft, T _xRight, T _yRight, T _zRight) { @@ -92,7 +93,7 @@ namespace ignition /// \param[in] _zRight righmost z-axis interval value /// \return the [`_xLeft`, `_xRight`] ✕ [`_yLeft`, `_yRight`] /// ✕ [`_zLeft`, `_zRight`] closed region - public: static Region3 Closed( + public: static constexpr Region3 Closed( T _xLeft, T _yLeft, T _zLeft, T _xRight, T _yRight, T _zRight) { @@ -188,7 +189,7 @@ namespace ignition namespace detail { template - const Region3 gUnboundedRegion3( + constexpr Region3 gUnboundedRegion3( Interval::Open(-std::numeric_limits::infinity(), std::numeric_limits::infinity()), Interval::Open(-std::numeric_limits::infinity(), 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)]"); + } +} diff --git a/src/PiecewiseScalarField3_TEST.cc b/src/PiecewiseScalarField3_TEST.cc new file mode 100644 index 000000000..ac7c10c1f --- /dev/null +++ b/src/PiecewiseScalarField3_TEST.cc @@ -0,0 +1,185 @@ +/* + * 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/PiecewiseScalarField3.hh" +#include "ignition/math/Polynomial3.hh" +#include "ignition/math/Vector3.hh" + +using namespace ignition; + +///////////////////////////////////////////////// +TEST(PiecewiseScalarField3Test, Evaluate) +{ + using ScalarField3dT = std::function; + using PiecewiseScalarField3dT = math::PiecewiseScalarField3d; + { + const PiecewiseScalarField3dT scalarField; + EXPECT_TRUE(std::isnan(scalarField(math::Vector3d::Zero))); + } + { + const PiecewiseScalarField3dT scalarField = + PiecewiseScalarField3dT::Throughout( + [](const math::Vector3d& v) { return v.X(); }); + EXPECT_DOUBLE_EQ(scalarField(math::Vector3d::Zero), 0.); + EXPECT_DOUBLE_EQ(scalarField(math::Vector3d(0.5, 0.5, 0.5)), 0.5); + EXPECT_DOUBLE_EQ(scalarField(math::Vector3d::One), 1.); + EXPECT_DOUBLE_EQ(scalarField(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 scalarField({ + {region0, field0}, {region1, field1}}); + EXPECT_DOUBLE_EQ(scalarField(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 scalarField({ + {region0, field0}, {region1, field1}}); + EXPECT_DOUBLE_EQ(scalarField(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 scalarField({ + {region0, field0}, {region1, field1}}); + EXPECT_DOUBLE_EQ(scalarField(math::Vector3d(0.5, 0.25, 0.5)), 0.5); + EXPECT_DOUBLE_EQ(scalarField(math::Vector3d(-0.5, -0.25, -0.5)), -0.25); + EXPECT_TRUE(std::isnan(scalarField(math::Vector3d(0.5, -0.25, 0.5)))); + EXPECT_TRUE(std::isnan(scalarField(math::Vector3d(-0.5, 0.25, -0.5)))); + } +} + +///////////////////////////////////////////////// +TEST(PiecewiseScalarField3Test, Minimum) +{ + using AdditivelySeparableScalarField3dT = + math::AdditivelySeparableScalarField3d; + using PiecewiseScalarField3dT = + math::PiecewiseScalarField3d; + { + const PiecewiseScalarField3dT scalarField; + math::Vector3d pMin = math::Vector3d::Zero; + EXPECT_TRUE(std::isnan(scalarField.Minimum())); + EXPECT_TRUE(std::isnan(scalarField.Minimum(pMin))); + EXPECT_TRUE(std::isnan(pMin.X())); + EXPECT_TRUE(std::isnan(pMin.Y())); + EXPECT_TRUE(std::isnan(pMin.Z())); + } + { + const PiecewiseScalarField3dT scalarField = + 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(scalarField.Minimum(), 1.); + EXPECT_DOUBLE_EQ(scalarField.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 scalarField({ + {region0, field0}, {region1, field1}}); + math::Vector3d pMin = math::Vector3d::NaN; + EXPECT_DOUBLE_EQ(scalarField.Minimum(), 0.); + EXPECT_DOUBLE_EQ(scalarField.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 scalarField({ + {region0, field0}, {region1, field1}}); + output << scalarField; + EXPECT_EQ(output.str(), expected.str()); + } +} diff --git a/src/Pose_TEST.cc b/src/Pose_TEST.cc index 978c85dfb..f51de223f 100644 --- a/src/Pose_TEST.cc +++ b/src/Pose_TEST.cc @@ -17,8 +17,9 @@ #include -#include "ignition/math/Helpers.hh" -#include "ignition/math/Pose3.hh" +#include +#include +#include using namespace ignition; @@ -61,17 +62,28 @@ TEST(PoseTest, Pose) // test hypothesis that if // A is the transform from O to P specified in frame O // B is the transform from P to Q specified in frame P - // then, B + A is the transform from O to Q specified in frame O + // then, A * B is the transform from O to Q specified in frame O math::Pose3d A(math::Vector3d(1, 0, 0), math::Quaterniond(0, 0, IGN_PI/4.0)); math::Pose3d B(math::Vector3d(1, 0, 0), math::Quaterniond(0, 0, IGN_PI/2.0)); - EXPECT_TRUE(math::equal((B + A).Pos().X(), 1.0 + 1.0/sqrt(2))); - EXPECT_TRUE(math::equal((B + A).Pos().Y(), 1.0/sqrt(2))); - EXPECT_TRUE(math::equal((B + A).Pos().Z(), 0.0)); - EXPECT_TRUE(math::equal((B + A).Rot().Euler().X(), 0.0)); - EXPECT_TRUE(math::equal((B + A).Rot().Euler().Y(), 0.0)); - EXPECT_TRUE(math::equal((B + A).Rot().Euler().Z(), 3.0*IGN_PI/4.0)); + EXPECT_TRUE(math::equal((A * B).Pos().X(), 1.0 + 1.0/sqrt(2))); + EXPECT_TRUE(math::equal((A * B).Pos().Y(), 1.0/sqrt(2))); + EXPECT_TRUE(math::equal((A * B).Pos().Z(), 0.0)); + EXPECT_TRUE(math::equal((A * B).Rot().Euler().X(), 0.0)); + EXPECT_TRUE(math::equal((A * B).Rot().Euler().Y(), 0.0)); + EXPECT_TRUE(math::equal((A * B).Rot().Euler().Z(), 3.0*IGN_PI/4.0)); + + IGN_UTILS_WARN_IGNORE__DEPRECATED_DECLARATION + // Coverage for + operator + EXPECT_EQ(A * B, B + A); + EXPECT_NE(A * B, A + B); + + // Coverage for += operator + math::Pose3d C(B); + C += A; + EXPECT_EQ(C, A * B); + IGN_UTILS_WARN_RESUME__DEPRECATED_DECLARATION } { // If: @@ -135,11 +147,11 @@ TEST(PoseTest, Pose) EXPECT_TRUE(pose1.Rot() == math::Quaterniond(0.946281, -0.0933066, -0.226566, -0.210984)); - pose = math::Pose3d(1, 2, 3, .1, .2, .3) + math::Pose3d(4, 5, 6, .4, .5, .6); + pose = math::Pose3d(4, 5, 6, .4, .5, .6) * math::Pose3d(1, 2, 3, .1, .2, .3); EXPECT_TRUE(pose == math::Pose3d(5.74534, 7.01053, 8.62899, 0.675732, 0.535753, 1.01174)); - pose += pose; + pose *= pose; EXPECT_TRUE(pose == math::Pose3d(11.314, 16.0487, 15.2559, 1.49463, 0.184295, 2.13932));