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 gz::math
{
  // Inline bracket to help doxygen filtering.
  inline namespace GZ_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>;
  }  // namespace GZ_MATH_VERSION_NAMESPACE
}  // namespace gz::math
#endif  // GZ_MATH_PIECEWISE_SCALAR_FIELD3_HH_