gz/math/PiecewiseScalarField3.hh

Go to the documentation of this file.

/*
 * 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 GZ_MATH_PIECEWISE_SCALAR_FIELD3_HH_
#define GZ_MATH_PIECEWISE_SCALAR_FIELD3_HH_
 
#include <algorithm>
#include <iostream>
#include <limits>
#include <utility>
#include <vector>
 
#include <gz/math/Region3.hh>
#include <gz/math/Vector3.hh>
#include <gz/math/config.hh>
 
namespace ignition
{
  namespace math
  {
    // Inline bracket to help doxygen filtering.
    inline namespace IGNITION_MATH_VERSION_NAMESPACE {
    //
    template<typename ScalarField3T, typename ScalarT>
    class PiecewiseScalarField3
    {
      public: struct Piece {
        Region3<ScalarT> region;
        ScalarField3T field;
      };
 
      public: PiecewiseScalarField3() = default;
 
      public: explicit PiecewiseScalarField3(const std::vector<Piece> &_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;
            }
          }
        }
      }
 
      public: static PiecewiseScalarField3 Throughout(ScalarField3T _field)
      {
        return PiecewiseScalarField3<ScalarField3T, ScalarT>({
            {Region3<ScalarT>::Unbounded, std::move(_field)}});
      }
 
      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);
      }
 
      public: ScalarT operator()(const Vector3<ScalarT> &_p) const
      {
        return this->Evaluate(_p);
      }
 
      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;
      }
 
      public: ScalarT Minimum() const
      {
        Vector3<ScalarT> pMin;
        return this->Minimum(pMin);
      }
 
      public: friend std::ostream &operator<<(
          std::ostream &_out,
          const gz::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;
      }
 
      private: std::vector<Piece> pieces;
    };
 
    template<typename ScalarField3T>
    using PiecewiseScalarField3f = PiecewiseScalarField3<ScalarField3T, float>;
    template<typename ScalarField3T>
    using PiecewiseScalarField3d = PiecewiseScalarField3<ScalarField3T, double>;
    }
  }
}
 
#endif