Implicit_manifold_intersection_oracle.h

/*    This file is part of the Gudhi Library - https://gudhi.inria.fr/ - which is released under MIT.
 *    See file LICENSE or go to https://gudhi.inria.fr/licensing/ for full license details.
 *    Author(s):       Siargey Kachanovich
 *
 *    Copyright (C) 2019 Inria
 *
 *    Modification(s):
 *      - YYYY/MM Author: Description of the modification
 */
 
#ifndef IMPLICIT_MANIFOLD_INTERSECTION_ORACLE_H_
#define IMPLICIT_MANIFOLD_INTERSECTION_ORACLE_H_
 
#include <Eigen/Dense>
 
#include <gudhi/Permutahedral_representation/face_from_indices.h>
#include <gudhi/Functions/Constant_function.h>
#include <gudhi/Functions/PL_approximation.h>
#include <gudhi/Coxeter_triangulation/Query_result.h>
#include <gudhi/Debug_utils.h>  // for GUDHI_CHECK
 
#include <vector>
#include <limits>  // for std::numeric_limits<>
#include <cmath>   // for std::fabs
 
namespace Gudhi {
 
namespace coxeter_triangulation {
 
template <class Function_, class Domain_function_ = Constant_function>
class Implicit_manifold_intersection_oracle {
  /* Computes the affine coordinates of the intersection point of the implicit manifold
   * and the affine hull of the simplex. */
  template <class Simplex_handle, class Triangulation>
  Eigen::VectorXd compute_lambda(const Simplex_handle& simplex, const Triangulation& triangulation) const {
    std::size_t cod_d = this->cod_d();
    Eigen::MatrixXd matrix(cod_d + 1, cod_d + 1);
    for (std::size_t i = 0; i < cod_d + 1; ++i) matrix(0, i) = 1;
    std::size_t j = 0;
    for (auto v : simplex.vertex_range()) {
      Eigen::VectorXd v_coords = fun_(triangulation.cartesian_coordinates(v));
      for (std::size_t i = 1; i < cod_d + 1; ++i) matrix(i, j) = v_coords(i - 1);
      j++;
    }
    Eigen::VectorXd z(cod_d + 1);
    z(0) = 1;
    for (std::size_t i = 1; i < cod_d + 1; ++i) z(i) = 0;
    Eigen::VectorXd lambda = matrix.colPivHouseholderQr().solve(z);
    if (!z.isApprox(matrix*lambda)) {
      // NaN non valid results
      for (std::size_t i = 0; i < (std::size_t)lambda.size(); ++i) lambda(i) =
        std::numeric_limits<double>::quiet_NaN();
    }
    return lambda;
  }
 
  /* Computes the affine coordinates of the intersection point of the boundary
   * of the implicit manifold and the affine hull of the simplex. */
  template <class Simplex_handle, class Triangulation>
  Eigen::VectorXd compute_boundary_lambda(const Simplex_handle& simplex, const Triangulation& triangulation) const {
    std::size_t cod_d = this->cod_d();
    Eigen::MatrixXd matrix(cod_d + 2, cod_d + 2);
    for (std::size_t i = 0; i < cod_d + 2; ++i) matrix(0, i) = 1;
    std::size_t j = 0;
    for (auto v : simplex.vertex_range()) {
      Eigen::VectorXd v_coords = fun_(triangulation.cartesian_coordinates(v));
      for (std::size_t i = 1; i < cod_d + 1; ++i) matrix(i, j) = v_coords(i - 1);
      Eigen::VectorXd bv_coords = domain_fun_(triangulation.cartesian_coordinates(v));
      matrix(cod_d + 1, j) = bv_coords(0);
      j++;
    }
    Eigen::VectorXd z(cod_d + 2);
    z(0) = 1;
    for (std::size_t i = 1; i < cod_d + 2; ++i) z(i) = 0;
    Eigen::VectorXd lambda = matrix.colPivHouseholderQr().solve(z);
    if (!z.isApprox(matrix*lambda)) {
      // NaN non valid results
      for (std::size_t i = 0; i < (std::size_t)lambda.size(); ++i) lambda(i) =
        std::numeric_limits<double>::quiet_NaN();
    }
    return lambda;
  }
 
  /* Computes the intersection result for a given simplex in a triangulation. */
  template <class Simplex_handle, class Triangulation>
  Query_result<Simplex_handle> intersection_result(const Eigen::VectorXd& lambda, const Simplex_handle& simplex,
                                                   const Triangulation& triangulation) const {
    using QR = Query_result<Simplex_handle>;
    std::size_t amb_d = triangulation.dimension();
    std::size_t cod_d = simplex.dimension();
    for (std::size_t i = 0; i < (std::size_t)lambda.size(); ++i) {
      if (std::isnan(lambda(i))) return QR({Eigen::VectorXd(), false});
      GUDHI_CHECK((std::fabs(lambda(i) - 1.) > std::numeric_limits<double>::epsilon() &&
                   std::fabs(lambda(i) - 0.) > std::numeric_limits<double>::epsilon()),
                  std::invalid_argument("A vertex of the triangulation lies exactly on the manifold"));
      if (lambda(i) < 0. || lambda(i) > 1.) return QR({Eigen::VectorXd(), false});
    }
    Eigen::MatrixXd vertex_matrix(cod_d + 1, amb_d);
    auto v_range = simplex.vertex_range();
    auto v_it = v_range.begin();
    for (std::size_t i = 0; i < cod_d + 1 && v_it != v_range.end(); ++v_it, ++i) {
      Eigen::VectorXd v_coords = triangulation.cartesian_coordinates(*v_it);
      for (std::size_t j = 0; j < amb_d; ++j) vertex_matrix(i, j) = v_coords(j);
    }
    Eigen::VectorXd intersection = lambda.transpose() * vertex_matrix;
    return QR({intersection, true});
  }
 
 public:
  std::size_t amb_d() const { return fun_.amb_d(); }
 
  std::size_t cod_d() const { return fun_.cod_d(); }
 
  Eigen::VectorXd seed() const { return fun_.seed(); }
 
  template <class Simplex_handle, class Triangulation>
  Query_result<Simplex_handle> intersects(const Simplex_handle& simplex, const Triangulation& triangulation) const {
    Eigen::VectorXd lambda = compute_lambda(simplex, triangulation);
    return intersection_result(lambda, simplex, triangulation);
  }
 
  template <class Simplex_handle, class Triangulation>
  Query_result<Simplex_handle> intersects_boundary(const Simplex_handle& simplex,
                                                   const Triangulation& triangulation) const {
    //std::cout << "intersects_boundary" << std::endl;
    Eigen::VectorXd lambda = compute_boundary_lambda(simplex, triangulation);
    return intersection_result(lambda, simplex, triangulation);
  }
 
  template <class Triangulation>
  bool lies_in_domain(const Eigen::VectorXd& p, const Triangulation& triangulation) const {
    Eigen::VectorXd pl_p = make_pl_approximation(domain_fun_, triangulation)(p);
    return pl_p(0) < 0;
  }
 
  const Function_& function() const { return fun_; }
 
  Implicit_manifold_intersection_oracle(const Function_& function, const Domain_function_& domain_function)
      : fun_(function), domain_fun_(domain_function) {}
 
  Implicit_manifold_intersection_oracle(const Function_& function)
      : fun_(function), domain_fun_(function.amb_d(), 1, Eigen::VectorXd::Constant(1, -1)) {}
 
 private:
  Function_ fun_;
  Domain_function_ domain_fun_;
};
 
template <class Function_, class Domain_function_>
Implicit_manifold_intersection_oracle<Function_, Domain_function_> make_oracle(
    const Function_& function, const Domain_function_& domain_function) {
  return Implicit_manifold_intersection_oracle<Function_, Domain_function_>(function, domain_function);
}
 
template <class Function_>
Implicit_manifold_intersection_oracle<Function_> make_oracle(const Function_& function) {
  return Implicit_manifold_intersection_oracle<Function_>(function);
}
 
}  // namespace coxeter_triangulation
 
}  // namespace Gudhi
 
#endif