BinaryMatrix::which(), BinaryMatrix all_visitor bug fix, triangulatio…

…n node_patch, tree_search can return all the elements containing a point, simplex nearest point (best approximation of free point in the simplex, aka projection), exact and not-exact projection routine

fdaPDE/geometry/kd_tree.h

@@ -33,11 +33,11 @@ template <int K> class KDTree {
     using Container    = BinaryTree<int>;
     using node_type    = Container::node_type;
     using node_pointer = Container::node_pointer;
-    using iterator     = Container::dfs_iterator;
     using data_type = Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>;
     Container kdtree_;   // the actual BinaryTree container
     data_type data_;     // set of data indexed by this tree
    public:
+    using iterator = Container::dfs_iterator;
     // computes the kd-tree structure for a set of points
     KDTree() = default;
     template <typename DataType_> explicit KDTree(DataType_&& data) : data_(std::forward<DataType_>(data)) {

fdaPDE/geometry/project.h

@@ -0,0 +1,72 @@
+// This file is part of fdaPDE, a C++ library for physics-informed
+// spatial and functional data analysis.
+//
+// This program is free software: you can redistribute it and/or modify
+// it under the terms of the GNU General Public License as published by
+// the Free Software Foundation, either version 3 of the License, or
+// (at your option) any later version.
+//
+// This program is distributed in the hope that it will be useful,
+// but WITHOUT ANY WARRANTY; without even the implied warranty of
+// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+// GNU General Public License for more details.
+//
+// You should have received a copy of the GNU General Public License
+// along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+#ifndef __PROJECT_H__
+#define __PROJECT_H__
+
+#include "kd_tree.h"
+#include "../utils/symbols.h"
+
+namespace fdapde {
+namespace core {
+
+template <typename Policy> struct Project;
+
+template <> struct Project<Exact> {
+    template <typename MeshType> static DMatrix<double> compute(const DMatrix<double>& points, const MeshType& mesh) {
+        DVector<double> best = DVector<double>::Constant(points.rows(), std::numeric_limits<double>::max());
+        DMatrix<double> proj(points.rows(), MeshType::embed_dim);
+        for (typename MeshType::cell_iterator it = mesh.cells_begin(); it != mesh.cells_end(); ++it) {
+            for (int i = 0; i < points.rows(); ++i) {
+                SVector<MeshType::embed_dim> proj_point = it->nearest(points.row(i));
+                double dist = (proj_point - points.row(i).transpose()).norm();
+                if (dist < best[i]) {
+                    best[i] = dist;
+                    proj.row(i) = proj_point;
+                }
+            }
+        }
+        return proj;
+    }
+};
+
+template <> struct Project<NotExact> {
+    template <typename MeshType> static DMatrix<double> compute(const DMatrix<double>& points, const MeshType& mesh) {
+        DMatrix<double> proj(points.rows(), MeshType::embed_dim);
+        // build kdtree of mesh nodes for fast nearest neighborhood searches
+        KDTree<MeshType::embed_dim> tree_(mesh.nodes());
+        for (int i = 0; i < points.rows(); ++i) {
+            // find nearest mesh node (in euclidean sense, approximation)
+            typename KDTree<MeshType::embed_dim>::iterator it = tree_.nn_search(points.row(i));
+            // search nearest element in the node patch
+            double best = std::numeric_limits<double>::max();
+            for (int j : mesh.node_patch(*it)) {
+                SVector<MeshType::embed_dim> proj_point = mesh.cell(j).nearest(points.row(i));
+                double dist = (proj_point - points.row(i).transpose()).norm();
+                if (dist < best) {
+                    best = dist;
+                    proj.row(i) = proj_point;
+                }
+            }
+        }
+        return proj;
+    }
+};
+
+}   // namespace core
+}   // namespace fdapde
+
+#endif // __PROJECT_H__

fdaPDE/geometry/simplex.h

@@ -18,7 +18,9 @@
 #define __SIMPLEX_H__
 
 #include <optional>
+#include <numeric>
 
+#include "../linear_algebra/binary_matrix.h"
 #include "../utils/symbols.h"
 #include "hyperplane.h"
 #include "utils.h"
@@ -49,7 +51,6 @@ template <int Order_, int EmbedDim_> class Simplex {
         for (int i = 0; i < embed_dim; ++i) coords(i, i + 1) = 1;
         return Simplex(coords);
     }
-
     // getters
     NodeType node(int v) const { return coords_.col(v); }
     NodeType operator[](int v) const { return coords_.col(v); }
@@ -151,6 +152,34 @@ template <int Order_, int EmbedDim_> class Simplex {
     };
     boundary_iterator boundary_begin() const requires(Order_ >= 1) { return boundary_iterator(0, this); }
     boundary_iterator boundary_end() const requires(Order_ >= 1) { return boundary_iterator(Order_ + 1, this); }
+
+    // finds the best approximation of p in the simplex (q \in simplex : q = \argmin_{t \in simplex}{\norm{t - p}})
+    SVector<embed_dim> nearest(const SVector<embed_dim>& p) const {
+        SVector<local_dim + 1> q = barycentric_coords(p);
+	// check if point inside simplex
+        if constexpr (local_dim != embed_dim) {
+            if (
+              (q.array() > -fdapde::machine_epsilon).all() && supporting_plane().distance(p) < fdapde::machine_epsilon)
+                return p;
+        } else {
+            if ((q.array() > -fdapde::machine_epsilon).all()) return p;
+        }
+        if constexpr (Order_ == 1) {   // end of recursion
+            if (q[0] < 0) return coords_.col(1);
+            return coords_.col(0);
+        } else {
+            // find nearest face
+            std::array<double, n_nodes> dst;
+            for (int i = 0; i < n_nodes; ++i) { dst[i] = (coords_.col(i) - p).norm(); }
+            std::array<int, n_nodes> idx;
+            std::iota(idx.begin(), idx.end(), 0);
+            std::sort(idx.begin(), idx.end(), [&](int a, int b) { return dst[a] < dst[b]; });
+	    // recurse on Order_ - 1 subsimplex
+            Simplex<Order_ - 1, embed_dim> s(coords_(Eigen::all, std::vector<int>(idx.begin(), idx.end() - 1)));
+            return s.nearest(p);
+        }
+    }
+
    protected:
     void initialize() {
       for (int j = 0; j < n_nodes - 1; ++j) { J_.col(j) = coords_.col(j + 1) - coords_.col(0); }

fdaPDE/geometry/tree_search.h

@@ -32,6 +32,14 @@ template <typename MeshType> class TreeSearch {
     KDTree<2 * embed_dim> tree_;
     const MeshType* mesh_;
     SVector<embed_dim> c_;   // normalization constants
+    // build search query for point p
+    KDTree<2 * embed_dim>::RangeType query(const SVector<embed_dim>& p) const {
+        SVector<embed_dim> scaled_p = (p - mesh_->range().row(0).transpose()).array() * c_.array();
+        SVector<2 * embed_dim> ll, ur;
+        ll << SVector<embed_dim>::Zero(), scaled_p;
+        ur << scaled_p, SVector<embed_dim>::Ones();
+        return {ll, ur};
+    }
    public:
     TreeSearch() = default;
     TreeSearch(const MeshType* mesh) : mesh_(mesh) {
@@ -50,16 +58,19 @@ template <typename MeshType> class TreeSearch {
         }
         tree_ = KDTree<2 * embed_dim>(std::move(data));   // organize elements in a KD-tree structure
     }
-    // finds element containing p, returns nullptr if element not found
+    // finds all the elements containing p
+    std::unordered_set<int> all_locate(const SVector<embed_dim>& p) const {
+        std::unordered_set<int> result;
+        for (int id : tree_.range_search(query(p))) {
+            typename MeshType::CellType c = mesh_->cell(id);
+            if (c.contains(p)) { result.insert(c.id()); }
+        }
+        return result;
+    }
+    // finds element containing p, returns -1 if element not found
     int locate(const SVector<embed_dim>& p) const {
-        // build search query
-        SVector<embed_dim> scaled_p = (p - mesh_->range().row(0).transpose()).array() * c_.array();
-        SVector<2 * embed_dim> ll, ur;
-        ll << SVector<embed_dim>::Zero(), scaled_p;
-        ur << scaled_p, SVector<embed_dim>::Ones();
-        auto found = tree_.range_search({ll, ur});
         // exhaustively scan the query results to get the searched mesh element
-        for (int id : found) {
+        for (int id : tree_.range_search(query(p))) {
             typename MeshType::CellType c = mesh_->cell(id);
             if (c.contains(p)) { return c.id(); }
         }

fdaPDE/geometry/triangulation.h

@@ -68,7 +68,7 @@ template <int M, int N, typename Derived> class TriangulationBase {
     int n_nodes() const { return n_nodes_; }
     int n_boundary_nodes() const { return nodes_markers_.count(); }
     SMatrix<2, N> range() const { return range_; }
-  
+
     // iterators over cells
     class cell_iterator : public index_based_iterator<cell_iterator, CellType> {
         using Base = index_based_iterator<cell_iterator, CellType>;
@@ -253,6 +253,11 @@ template <int N> class Triangulation<2, N> : public TriangulationBase<2, N, Tria
         if (!location_policy_.has_value()) location_policy_ = LocationPolicy(this);
         return location_policy_->locate(points);
     }
+    // computes the set of elements which have node id as vertex
+    std::unordered_set<int> node_patch(int id) const {
+        if (!location_policy_.has_value()) location_policy_ = LocationPolicy(this);
+        return location_policy_->all_locate(Base::node(id));
+    }
    protected:
     std::vector<int> edges_ {};                        // nodes (as row indexes in nodes_ matrix) composing each edge
     std::vector<int> edge_to_cells_ {};                // for each edge, the ids of adjacent cells
@@ -460,9 +465,10 @@ template <> class Triangulation<3, 3> : public TriangulationBase<3, 3, Triangula
         if (!location_policy_.has_value()) location_policy_ = LocationPolicy(this);
         return location_policy_->locate(points);
     }
-    int locate(const SVector<embed_dim>& p) const {
+    // computes the set of elements which have node id as vertex
+    std::unordered_set<int> node_patch(int id) const {
         if (!location_policy_.has_value()) location_policy_ = LocationPolicy(this);
-        return location_policy_->locate(p);
+        return location_policy_->all_locate(Base::node(id));
     }
    protected:
     std::vector<int> faces_, edges_;   // nodes (as row indexes in nodes_ matrix) composing each face and edge

fdaPDE/linear_algebra/binary_matrix.h

@@ -90,29 +90,30 @@ template <int Rows, int Cols = Rows> class BinaryMatrix : public BinMtxBase<Rows
             }
         }
     }
-    // construct from STL iterator
-    template <typename Iter> BinaryMatrix(Iter begin, Iter end, int n_rows, int n_cols) : Base(n_rows, n_cols) {
+    // construct from iterator pair
+    template <typename Iterator>
+    BinaryMatrix(Iterator begin, Iterator end, int n_rows, int n_cols) : Base(n_rows, n_cols) {
         fdapde_static_assert(
           Rows == fdapde::Dynamic && Cols == fdapde::Dynamic, THIS_METHOD_IS_ONLY_FOR_DYNAMIC_SIZED_MATRICES);
         resize(n_rows, n_cols);   // reserve space
         int i = 0, size = n_rows * n_cols;
-        for (Iter it = begin; it != end || i < size; ++it, ++i) {
+        for (Iterator it = begin; it != end || i < size; ++it, ++i) {
             if (*it) set(i / n_rows, i % n_cols);
         }
     }
-    template <typename Iter> BinaryMatrix(Iter begin, Iter end, int n_rows) : BinaryMatrix(n_rows, 1) {
+    template <typename Iterator> BinaryMatrix(Iterator begin, Iterator end, int n_rows) : BinaryMatrix(n_rows, 1) {
         fdapde_static_assert(Rows == Dynamic && Cols == 1, THIS_METHOD_IS_ONLY_FOR_VECTORS);
         resize(n_rows);   // reserve space
         int i = 0;
-        for (Iter it = begin; it != end || i < n_rows; ++it, ++i) {
+        for (Iterator it = begin; it != end || i < n_rows; ++it, ++i) {
             if (*it) set(i);
         }
     }
 
     // constructs a matrix of ones
     static BinaryMatrix<Rows, Cols> Ones(int i, int j) {
         BinaryMatrix<Rows, Cols> result;
-        result.resize(i, j);
+        if constexpr(Rows == Dynamic || Cols == Dynamic) result.resize(i, j);
         for (int k = 0; k < result.bitpacks(); ++k) { result.bitpack(k) = -1; }
         return result;
     }
@@ -365,7 +366,7 @@ template <typename XprType, typename Visitor> struct linear_bitpack_visit {
         int size = xpr.size();
         if (size == 0) return;
         if (size < PackSize) {
-            visitor.apply(xpr.bitpack(0), size);
+            visitor.apply(xpr.bitpack(0), PackSize - size);
             return;
         }
         int k = 0, i = 0;   // k: current bitpack, i: maximum coefficient index processed
@@ -413,7 +414,9 @@ template <typename XprType> struct all_visitor {
     static constexpr int PackSize = XprType::PackSize;   // number of bits in a packet
     bool res = true;
     inline void apply(BitPackType b) { res &= (~b == 0); }
-    inline void apply(BitPackType b, int size) { res &= (~((((BitPackType)1 << (size - 1)) - 1) | b) == 0); }
+    inline void apply(BitPackType b, int size) {
+        res &= ((((BitPackType)1 << (PackSize - size)) - 1) & b) == (((BitPackType)1 << (PackSize - size)) - 1);
+    }
     operator bool() const { return res == false; }   // stop if already false
 };
 // evaluates true if at least one coefficient in the binary expression is true
@@ -517,6 +520,16 @@ template <int Rows, int Cols, typename XprType> class BinMtxBase {
         fdapde_assert(i < n_rows_ && j < n_cols_);
         return get().operator()(i, j);
     }
+    // returns all the indices (in row-major order) having coefficients equal to b
+    std::vector<int> which(bool b) const {
+        std::vector<int> result;
+        for (int i = 0; i < n_rows_; ++i) {
+            for (int j = 0; j < n_cols_; ++j) {
+                if (get()(i, j) == b) result.push_back(i * n_cols_ + j);
+            }
+        }
+        return result;
+    }
     // access to i-th bitpack of the expression
     BitPackType bitpack(int i) const { return get().bitpack(i); }
     // send matrix to out stream
@@ -643,6 +656,11 @@ bool operator!=(const BinMtxBase<Rows1, Cols1, XprType1>& op1, const BinMtxBase<
 // alias export for binary vectors
 template <int Rows> using BinaryVector = BinaryMatrix<Rows, 1>;
 
+// out-of-class which function
+template <int Rows, int Cols, typename XprType> std::vector<int> which(const BinMtxBase<Rows, Cols, XprType>& mtx) {
+    return mtx.which(true);
+}
+
 }   // namespace core
 }   // namespace fdapde
 

fdaPDE/utils/symbols.h

@@ -40,6 +40,10 @@ namespace fdapde {
 constexpr int Dynamic = -1;       // used when the size of a vector or matrix is not known at compile time
 constexpr int random_seed = -1;   // signals that a random seed is used somewhere
 
+// algorithm computation policies
+struct Exact { };
+struct NotExact { };
+
 template <int N, typename T = double> struct static_dynamic_vector_selector {
     using type = typename std::conditional<N == Dynamic, DVector<T>, SVector<N, T>>::type;
 };

test/src/binary_matrix_test.cpp

@@ -239,6 +239,21 @@ TEST(binary_matrix_test, visitors) {
     v2.clear(300);
     v2.set(499);
     EXPECT_TRUE(v2.any());
+
+    // static sized
+    BinaryVector<3> v3;
+    for (int i = 0; i < 3; ++i) v3.set(i);
+    EXPECT_TRUE(v3.all());
+    v3.clear(1);
+    EXPECT_FALSE(v3.all());    
+    for (int i = 0; i < 3; ++i) v3.clear(i);
+    EXPECT_FALSE(v3.any());
+    // dynamic sized (one bitpack only)
+    BinaryVector<Dynamic> v4(3);
+    for (int i = 0; i < 3; ++i) v4.set(i);
+    EXPECT_TRUE(v4.all());
+    for (int i = 0; i < 3; ++i) v4.clear(i);
+    EXPECT_FALSE(v4.any());
 }
 
 TEST(binary_matrix_test, block_repeat) {