AdditivelySeparableScalarField3.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_SEPARABLE_SCALAR_FIELD3_HH_
#define GZ_MATH_SEPARABLE_SCALAR_FIELD3_HH_
 
#include <limits>
#include <utility>
 
#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 ScalarFunctionT, typename ScalarT>
  class AdditivelySeparableScalarField3
  {
    public: AdditivelySeparableScalarField3(
        ScalarT _k, ScalarFunctionT _p,
        ScalarFunctionT _q, ScalarFunctionT _r)
    : k(_k), p(std::move(_p)), q(std::move(_q)), r(std::move(_r))
    {
    }
 
    public: ScalarT Evaluate(const Vector3<ScalarT> &_point) const
    {
      return this->k * (
          this->p(_point.X()) +
          this->q(_point.Y()) +
          this->r(_point.Z()));
    }
 
    public: ScalarT operator()(const Vector3<ScalarT> &_point) const
    {
      return this->Evaluate(_point);
    }
 
    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()));
    }
 
    public: ScalarT Minimum(const Region3<ScalarT> &_region) const
    {
      Vector3<ScalarT> pMin;
      return this->Minimum(_region, pMin);
    }
 
    public: ScalarT Minimum(Vector3<ScalarT> &_pMin) const
    {
      return this->Minimum(Region3<ScalarT>::Unbounded, _pMin);
    }
 
    public: ScalarT Minimum() const
    {
      Vector3<ScalarT> pMin;
      return this->Minimum(Region3<ScalarT>::Unbounded, pMin);
    }
 
    public: friend std::ostream &operator<<(
        std::ostream &_out,
        const gz::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 << ")]";
    }
 
    private: ScalarT k;
 
    private: ScalarFunctionT p;
 
    private: ScalarFunctionT q;
 
    private: ScalarFunctionT r;
  };
 
  template<typename ScalarFunctionT>
  using AdditivelySeparableScalarField3f =
      AdditivelySeparableScalarField3<ScalarFunctionT, float>;
 
  template<typename ScalarFunctionT>
  using AdditivelySeparableScalarField3d =
      AdditivelySeparableScalarField3<ScalarFunctionT, double>;
  }  // namespace GZ_MATH_VERSION_NAMESPACE
}  // namespace gz::math
#endif  // GZ_MATH_SEPARABLE_SCALAR_FIELD3_HH_