polyhedron.h

#ifndef DECOMP_POLYGON_H
#define DECOMP_POLYGON_H

#include <decomp_basis/data_type.h>

template <int Dim>
struct Hyperplane {
  Hyperplane() {}
  Hyperplane(const Vecf<Dim>& p, const Vecf<Dim>& n) : p_(p), n_(n) {}

  decimal_t signed_dist(const Vecf<Dim>& pt) const {
    return n_.dot(pt - p_);
  }

  decimal_t dist(const Vecf<Dim>& pt) const {
    return std::abs(signed_dist(pt));
  }

  Vecf<Dim> p_;
  Vecf<Dim> n_;
};

typedef Hyperplane<2> Hyperplane2D;
typedef Hyperplane<3> Hyperplane3D;


template <int Dim>
struct Polyhedron {
  Polyhedron() {}
  Polyhedron(const vec_E<Hyperplane<Dim>>& vs) : vs_(vs) {}


  void add(const Hyperplane<Dim>& v) {
    vs_.push_back(v);
  }

  bool inside(const Vecf<Dim>& pt) const {
    for (const auto& v : vs_) {
      if (v.signed_dist(pt) > epsilon_) {
        //printf("rejected pt: (%f, %f), d: %f\n",pt(0), pt(1), v.signed_dist(pt));
        return false;
      }
    }
    return true;
  }

  vec_Vecf<Dim> points_inside(const vec_Vecf<Dim> &O) const {
    vec_Vecf<Dim> new_O;
    for (const auto &it : O) {
      if (inside(it))
        new_O.push_back(it);
    }
    return new_O;
  }

  vec_E<std::pair<Vecf<Dim>, Vecf<Dim>>> cal_normals() const {
    vec_E<std::pair<Vecf<Dim>, Vecf<Dim>>> ns(vs_.size());
    for (size_t i = 0; i < vs_.size(); i++)
      ns[i] = std::make_pair(vs_[i].p_, vs_[i].n_); // fist is point, second is normal
    return ns;
  }

  vec_E<Hyperplane<Dim>> hyperplanes() const {
    return vs_;
  }

  vec_E<Hyperplane<Dim>> vs_; // normal must go outside

};

typedef Polyhedron<2> Polyhedron2D;
typedef Polyhedron<3> Polyhedron3D;

template <int Dim>
struct LinearConstraint {
  LinearConstraint() {}
  LinearConstraint(const MatDNf<Dim>& A, const VecDf& b) : A_(A), b_(b) {}
    LinearConstraint(const Vecf<Dim> p0, const vec_E<Hyperplane<Dim>>& vs) {
        const unsigned int size = vs.size();
        MatDNf<Dim> A(size, Dim);
        VecDf b(size);

        for (unsigned int i = 0; i < size; i++) {
            const auto n = vs[i].n_;
            decimal_t c = vs[i].p_.dot(n);
            if (n.dot(p0) - c > 0) {
                n = -n;
                c = -c;
            }
            A.row(i) = n;
            b(i) = c;
        }

        A_ = A;
        b_ = b;
    }

  bool inside(const Vecf<Dim> &pt) {
    VecDf d = A_ * pt - b_;
    for (unsigned int i = 0; i < d.rows(); i++) {
      if (d(i) > 0)
        return false;
    }
    return true;
  }

  MatDNf<Dim> A_;
  VecDf b_;
};

typedef LinearConstraint<2> LinearConstraint2D;
typedef LinearConstraint<3> LinearConstraint3D;

#endif