decomp_base.h

Go to the documentation of this file.

#ifndef DECOMP_BASE_H
#define DECOMP_BASE_H

#include <decomp_geometry/ellipsoid.h>
#include <decomp_geometry/polyhedron.h>
//#include <decomp_geometry/geometry_utils.h>

template <int Dim>
class DecompBase {
  public:
    DecompBase() {}
    void set_local_bbox(const Vecf<Dim>& bbox) {
      local_bbox_ = bbox;
    }

    void set_obs(const vec_Vecf<Dim> &obs) {
      // only consider points inside local bbox
      Polyhedron<Dim> vs;
      add_local_bbox(vs);
      obs_ = vs.points_inside(obs);
    }

    vec_Vecf<Dim> get_obs() const { return obs_; }

    Ellipsoid<Dim> get_ellipsoid() const { return ellipsoid_; }

    Polyhedron<Dim> get_polyhedron() const { return polyhedron_; }

    virtual void dilate(decimal_t radius = 0) = 0;

    virtual void shrink(double shrink_distance) {}
 protected:
    virtual void add_local_bbox(Polyhedron<Dim> &Vs) = 0;

    void find_polyhedron() {
      //**** find half-space
      Polyhedron<Dim> Vs;
      vec_Vecf<Dim> obs_remain = obs_;
      while (!obs_remain.empty()) {
        const auto v = ellipsoid_.closest_hyperplane(obs_remain);
        Vs.add(v);
        vec_Vecf<Dim> obs_tmp;
        for (const auto &it : obs_remain) {
          if (v.signed_dist(it) < 0)
            obs_tmp.push_back(it);
        }
        obs_remain = obs_tmp;
        /*
           std::cout << "a: " << a.transpose() << std::endl;
           std::cout << "b: " << b << std::endl;
           */
      }

      polyhedron_ = Vs;
    }

    vec_Vecf<Dim> obs_;

    Ellipsoid<Dim> ellipsoid_;
    Polyhedron<Dim> polyhedron_;

    Vecf<Dim> local_bbox_{Vecf<Dim>::Zero()};
};
#endif