TimeVaryingVolumetricGridLookupField.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_TIME_VARYING_VOLUMETRIC_GRID_LOOKUP_FIELD_HH_
#define GZ_MATH_TIME_VARYING_VOLUMETRIC_GRID_LOOKUP_FIELD_HH_
#include <gz/math/VolumetricGridLookupField.hh>
#include <gz/math/detail/InterpolationPoint.hh>
 
#include <map>
#include <optional>
#include <utility>
#include <vector>
 
namespace gz::math
{
  template<typename T, typename V, typename S>
  class TimeVaryingVolumetricGridLookupField
  {
    public: TimeVaryingVolumetricGridLookupField();
 
    public: void AddVolumetricGridField(
      const T& time, const VolumetricGridLookupField<V> &_field);
 
    public: S CreateSession() const;
 
    public: S CreateSession(const T &_time) const;
 
    public: std::optional<S> StepTo(const S &_session, const T &_time);
 
    public: std::vector<InterpolationPoint4D<T, V>>
    LookUp(const S &_session,
      const Vector3<V> &_point,
      const Vector3<V> &_tol) const;
 
    public: template<typename X>
    std::optional<X> EstimateQuadrilinear(
      const S &_session,
      const std::vector<InterpolationPoint4D<T, V>> &_points,
      const std::vector<X> &_values1,
      const std::vector<X> &_values2,
      const X _default = X(0)
    ) const;
 
    public: std::pair<Vector3<V>, Vector3<V>> Bounds(const S &_session);
  };
 
  template<typename T, typename V>
  class InMemorySession
  {
    private:
      typename std::map<T, VolumetricGridLookupField<V>>::const_iterator iter;
 
    public: T time;
 
    friend class
      TimeVaryingVolumetricGridLookupField<T, V, InMemorySession<T, V>>;
  };
 
  template<typename T, typename V>
  class TimeVaryingVolumetricGridLookupField<T, V, InMemorySession<T, V>>
  {
    public: TimeVaryingVolumetricGridLookupField()
    {}
 
    public: void AddVolumetricGridField(
      const T &_time, const VolumetricGridLookupField<V> &_field) {
      this->gridFields.emplace(_time, _field);
    }
 
    public: InMemorySession<T, V> CreateSession() const {
      InMemorySession<T, V> sess;
      sess.iter = this->gridFields.begin();
      sess.time = T(0);
      return sess;
    }
 
    public: InMemorySession<T, V> CreateSession(const T &_time) const {
      InMemorySession<T, V> sess;
      sess.iter = this->gridFields.lower_bound(_time);
      sess.time = _time;
      return sess;
    }
 
    public: bool IsValid(const InMemorySession<T, V> &_session) const {
      return this->gridFields.end() != _session.iter;
    }
 
    public: std::optional<InMemorySession<T, V>> StepTo(
      const InMemorySession<T, V> &_session, const T &_time) const {
      if (_session.iter == gridFields.end())
      {
        return std::nullopt;
      }
 
      InMemorySession<T, V> newSess(_session);
 
      auto nextTime = std::next(_session.iter);
      if (nextTime == this->gridFields.end() || _time < _session.iter->first)
      {
        return std::nullopt;
      }
 
      while (nextTime != this->gridFields.end()
        && nextTime->first <= _time)
      {
        newSess.iter = nextTime;
        nextTime = std::next(nextTime);
      }
      newSess.time = _time;
      return newSess;
    }
 
    public: std::vector<InterpolationPoint4D<T, V>>
      LookUp(const InMemorySession<T, V> &_session,
        const Vector3<V> &_point,
        const Vector3<V> &_tol = Vector3<V>{1e-6, 1e-6, 1e-6}) const {
 
      std::vector<InterpolationPoint4D<T, V>> res;
 
      if (_session.iter == this->gridFields.end())
      {
        // Out of bounds
        return res;
      }
 
      InterpolationPoint4D<T, V> slice1;
      slice1.timeSlice = _session.iter->second.GetInterpolators(
          _point, _tol.X(), _tol.Y(), _tol.Z());
      slice1.time = _session.iter->first;
      res.push_back(slice1);
 
      auto nextTime = std::next(_session.iter);
      if (nextTime != this->gridFields.end())
      {
        // Only add next item if field exists
        InterpolationPoint4D<T, V> slice2;
        slice2.timeSlice = nextTime->second.GetInterpolators(
          _point, _tol.X(), _tol.Y(), _tol.Z());
        slice2.time = nextTime->first;
        res.push_back(slice2);
      }
      return res;
    }
 
    public: template<typename X>
    std::optional<X> EstimateQuadrilinear(
      const InMemorySession<T, V> &_session,
      const std::vector<InterpolationPoint4D<T, V>> &_interpolators,
      const std::vector<X> &_values1,
      const std::vector<X> &_values2,
      const Vector3<X> &_position,
      const X _default = X(0)
    ) const
    {
      if (_session.iter == this->gridFields.end())
      {
        // Out of bounds
        return std::nullopt;
      }
 
      auto time = _session.time;
      if (_interpolators.size() == 0) return std::nullopt;
      if (_interpolators.size() == 1)
      {
        // This happens we reach the end of time
        return _session.iter->second.EstimateValueUsingTrilinear(
          _position,
          _values2,
          _default);
      }
 
      if (_interpolators[0].timeSlice.size() == 0
       && _interpolators[1].timeSlice.size() == 0)
        return std::nullopt;
 
      auto next = std::next(_session.iter);
      if (_interpolators[1].timeSlice.size() == 0)
      {
        return _session.iter->second.EstimateValueUsingTrilinear(
          _position,
          _values1,
          _default
        );
      }
      if (_interpolators[0].timeSlice.size() == 0)
      {
        return next->second.EstimateValueUsingTrilinear(
          _position,
          _values2,
          _default
        );
      }
 
      auto res1 = _session.iter->second.EstimateValueUsingTrilinear(
        _position,
        _values1,
        _default
      );
 
      auto res2 = next->second.EstimateValueUsingTrilinear(
        _position,
        _values2,
        _default
      );
 
      if (res1.has_value() || res2.has_value())
      {
        InterpolationPoint1D<T>
          pt1{_session.iter->first, 0}, pt2{next->first, 1};
        // If either has value interpolate using default value.
        std::vector<X> times{
          res1.value_or(_default), res2.value_or(_default)};
        return LinearInterpolate(pt1, pt2, times, time);
      }
      // Return nullopt if we are out of range
      return std::nullopt;
    }
 
    public: std::pair<Vector3<V>, Vector3<V>> Bounds(
      const InMemorySession<T, V> &_session) const
    {
      if (_session.iter == this->gridFields.end())
      {
        // Out of bounds
        return std::pair<Vector3<T>, Vector3<T>>(
          Vector3<T>{0, 0, 0}, Vector3<T>{0, 0, 0}
        );
      }
      return _session.iter->second.Bounds();
    }
 
    private: std::map<T, VolumetricGridLookupField<V>> gridFields;
  };
}  // namespace gz::math
#endif  // GZ_MATH_TIME_VARYING_VOLUMETRIC_GRID_LOOKUP_FIELD_HH_