kdtree_nanoflann_dof.h 8.26 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
/***********************************************************************
 * Software License Agreement (BSD License)
 *
 * Copyright 2011 Jose Luis Blanco (joseluisblancoc@gmail.com).
 *   All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
 * IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *************************************************************************/
/** \file kdtree_nanoflann_dof.h */

#ifndef KDTREE_NANOFLANN_DOF_H
#define KDTREE_NANOFLANN_DOF_H

#include <nanoflann.hpp>

#include <cstdlib>
#include <iostream>

#include "AMDiS.h"

using namespace nanoflann;
using namespace AMDiS;

namespace experimental {

// =====================================================================================
typedef WorldVector<double> PointType;
Praetorius, Simon's avatar
Praetorius, Simon committed
47
typedef DOFVector<PointType> VectorOfVectors_Dof;
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
// =====================================================================================


  /** A simple vector-of-vectors adaptor for nanoflann, without duplicating the storage.
    *  The i'th vector represents a point in the state space.
    *
    *  \tparam DIM If set to >0, it specifies a compile-time fixed dimensionality for the points in the data set, allowing more compiler optimizations.
    *  \tparam value_type The type of the point coordinates (typically, double or float).
    *  \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc.
    *  \tparam index_type The type for indices in the KD-tree index (typically, size_t of int)
    */
  template < class VectorOfVectorsType, 
	    typename value_type = double, 
	    int DIM = -1, 
	    class Distance = nanoflann::metric_L2_Simple, 
	    typename index_type = DegreeOfFreedom >
Praetorius, Simon's avatar
Praetorius, Simon committed
64
  struct KDTreeDOFVectorOfWorldVectorsAdaptor
65
  {
Praetorius, Simon's avatar
Praetorius, Simon committed
66
    typedef KDTreeDOFVectorOfWorldVectorsAdaptor< VectorOfVectorsType,
67
68
						value_type, 
						DIM, 
Praetorius, Simon's avatar
Praetorius, Simon committed
69
70
						Distance,
						index_type > self_t;
71
72
73
74
75
76
77
78
79
    typedef typename Distance::template traits<value_type, self_t>::distance_t metric_t;
    typedef KDTreeSingleIndexAdaptor< metric_t, 
				      self_t, 
				      DIM, 
				      index_type > index_t;

    index_t* index; //! The kd-tree index for the user to call its methods as usual with any other FLANN index.

    /// Constructor: takes a const ref to the vector of vectors object with the data points
Praetorius, Simon's avatar
Praetorius, Simon committed
80
    KDTreeDOFVectorOfWorldVectorsAdaptor(const int dimensionality,
81
82
83
84
85
86
87
88
89
90
91
				      VectorOfVectorsType &mat, 
				      const int leaf_max_size = 10) : m_data(mat)
    {
      const size_t dims = (*mat.begin()).getSize();
      assert(mat.getUsedSize() != 0 && dims != 0);
      if (DIM > 0 && static_cast<int>(dims) != DIM)
	throw std::runtime_error("Data set dimensionality does not match the 'DIM' template argument");
      index = new index_t( dims, *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size, dims ) );
      index->buildIndex();
    }

Praetorius, Simon's avatar
Praetorius, Simon committed
92
    ~KDTreeDOFVectorOfWorldVectorsAdaptor() {
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
      delete index;
    }
    
    void reinit(const int leaf_max_size = 10)
    {
      delete index;
      const size_t dims = (*m_data.begin()).getSize();
      index = new index_t( dims, *this /* adaptor */, nanoflann::KDTreeSingleIndexAdaptorParams(leaf_max_size, dims ) );
      index->buildIndex();
    }

    VectorOfVectorsType &m_data;

    /** Query for the \a num_closest closest points to a given point (entered as query_point[0:dim-1]).
      *  Note that this is a short-cut method for index->findNeighbors().
      *  The user can also call index->... methods as desired.
      * \note nChecks_IGNORED is ignored but kept for compatibility with the original FLANN interface.
      */
    inline void query(const value_type *query_point, 
		      const size_t num_closest, 
		      index_type *out_indices, 
		      value_type *out_distances_sq, 
		      const int nChecks_IGNORED = 10) const
    {
      nanoflann::KNNResultSet<value_type, index_type> resultSet(num_closest);
      resultSet.init(out_indices, out_distances_sq);
      index->findNeighbors(resultSet, query_point, nanoflann::SearchParams());
    }

    /** @name Interface expected by KDTreeSingleIndexAdaptor
      * @{ */

    const self_t & derived() const {
      return *this;
    }
    self_t & derived() {
      return *this;
    }

    // Must return the number of data points
    inline size_t kdtree_get_point_count() const {
      return m_data.getUsedSize();
    }

    // Returns the distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
    inline value_type kdtree_distance(const value_type *p1, 
				      const index_type idx_p2, 
				      size_t size) const
    {
      value_type s = 0.0;
      for (size_t i = 0; i < size; i++) {
	const value_type d = p1[i] - m_data[idx_p2][i];
	s += d*d;
      }
      return s;
    }

    // Returns the dim'th component of the idx'th point in the class:
    inline value_type kdtree_get_pt(const index_type idx, size_t dim) const {
      return m_data[idx][dim];
    }

    // Optional bounding-box computation: return false to default to a standard bbox computation loop.
    //   Return true if the BBOX was already computed by the class and returned in "bb" so it can be avoided to redo it again.
    //   Look at bb.size() to find out the expected dimensionality (e.g. 2 or 3 for point clouds)
    template <class BBOX>
    bool kdtree_get_bbox(BBOX &bb) const {
      return false;
    }

    /** @} */
    
    
    /**
    * strategy and nrOfPoints are ignored
    *  == evalAtPoint_simple
    **/
    template<typename T>
Praetorius, Simon's avatar
Praetorius, Simon committed
171
    T evalAtPoint(const DOFVector<T>& vec, const PointType& x, int strategy = 0, int nrOfPoints = -1)
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
    {
      const size_t nnp = 1;
      std::vector<DegreeOfFreedom> ret_indexes(nnp);
      std::vector<double> out_dists_sqr(nnp);
      nanoflann::KNNResultSet<double, DegreeOfFreedom> resultSet(nnp);
      
      double eps = 0.0;
      Parameters::get("KD-Tree->eps",eps);
      resultSet.init(&ret_indexes[0], &out_dists_sqr[0]);
      index->findNeighbors(resultSet, x.begin(), nanoflann::SearchParams(10, eps));
      
      T value; nullify(value);
      value = vec[ret_indexes[0]];
      return value;
    }

Praetorius, Simon's avatar
Praetorius, Simon committed
188
  }; // end of KDTreeDOFVectorOfWorldVectorsAdaptor
189

Praetorius, Simon's avatar
Praetorius, Simon committed
190
191
  typedef KDTreeDOFVectorOfWorldVectorsAdaptor< VectorOfVectors_Dof, double >  KD_Tree_Dof;
  static std::map<const FiniteElemSpace*, std::pair<int, KD_Tree_Dof*> > kdtreeMap_Dof;
192

Praetorius, Simon's avatar
Praetorius, Simon committed
193
  inline KD_Tree_Dof* provideKDTree(const FiniteElemSpace* feSpace)
194
  {
Praetorius, Simon's avatar
Praetorius, Simon committed
195
196
197
198
199
    if (kdtreeMap_Dof.find(feSpace) != kdtreeMap_Dof.end()) {
      if (kdtreeMap_Dof[feSpace].first != feSpace->getMesh()->getChangeIndex()) {
	feSpace->getMesh()->getDofIndexCoords(feSpace, kdtreeMap_Dof[feSpace].second->m_data);
	kdtreeMap_Dof[feSpace].second->reinit();
	kdtreeMap_Dof[feSpace].first = feSpace->getMesh()->getChangeIndex();
200
201
202
203
204
      }
    } else {
      DOFVector<WorldVector<double> >* coords = new DOFVector<WorldVector<double> >(feSpace, "coords");
      feSpace->getMesh()->getDofIndexCoords(feSpace, *coords);
      
Praetorius, Simon's avatar
Praetorius, Simon committed
205
206
      KD_Tree_Dof* kdtree = new KD_Tree_Dof(Global::getGeo(WORLD), *coords);
      kdtreeMap_Dof[feSpace] = std::make_pair(feSpace->getMesh()->getChangeIndex(), kdtree);
207
208
    }

Praetorius, Simon's avatar
Praetorius, Simon committed
209
    return kdtreeMap_Dof[feSpace].second;
210
211
212
213
214
  }
  
} // end namespace

#endif