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