summaryrefslogtreecommitdiff
path: root/src/particles/contactDetection
diff options
context:
space:
mode:
Diffstat (limited to 'src/particles/contactDetection')
-rw-r--r--src/particles/contactDetection/contactDetection.h66
-rw-r--r--src/particles/contactDetection/nanoflann.hpp1634
-rw-r--r--src/particles/contactDetection/nanoflann_adaptor.hpp127
-rw-r--r--src/particles/contactDetection/pLattice.h205
-rw-r--r--src/particles/contactDetection/sortAlgorithms.h31
5 files changed, 2063 insertions, 0 deletions
diff --git a/src/particles/contactDetection/contactDetection.h b/src/particles/contactDetection/contactDetection.h
new file mode 100644
index 0000000..c5a6218
--- /dev/null
+++ b/src/particles/contactDetection/contactDetection.h
@@ -0,0 +1,66 @@
+/* This file is part of the OpenLB library
+ *
+ * Copyright (C) 2015 Thomas Henn
+ * E-mail contact: info@openlb.net
+ * The most recent release of OpenLB can be downloaded at
+ * <http://www.openlb.net/>
+ *
+ * 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 2
+ * 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, write to the Free
+ * Software Foundation, Inc., 51 Franklin Street, Fifth Floor,
+ * Boston, MA 02110-1301, USA.
+ */
+
+#ifndef CONTACTDETECTION_H
+#define CONTACTDETECTION_H
+
+namespace olb {
+
+template<typename T, template<typename U> class PARTICLETYPE>
+class ParticleSystem3D;
+
+/*
+ * Prototype for future contact detection algorithms
+ **/
+
+template<typename T, template<typename U> class PARTICLETYPE>
+class ContactDetection {
+public:
+ ContactDetection(ParticleSystem3D<T, PARTICLETYPE>& pSys) : _pSys(pSys), _name("ContactDetection") {};
+ ContactDetection(ParticleSystem3D<T, PARTICLETYPE>& pSys, std::string name) : _pSys(pSys), _name(name) {};
+
+ virtual void sort() {};
+ virtual int getMatches(int pInt, std::vector<std::pair<size_t, T> >& matches)
+ {
+ return 0;
+ };
+
+ virtual ~ContactDetection() {};
+ virtual ContactDetection<T, PARTICLETYPE>* generate(ParticleSystem3D<T, PARTICLETYPE>& pSys)
+ {
+ return this;
+ };
+
+ inline std::string getName()
+ {
+ return _name;
+ }
+
+protected:
+ ParticleSystem3D<T, PARTICLETYPE>& _pSys;
+ std::string _name;
+};
+
+} // namespace olb
+
+#endif /*CONTACTDETECTION_H*/
diff --git a/src/particles/contactDetection/nanoflann.hpp b/src/particles/contactDetection/nanoflann.hpp
new file mode 100644
index 0000000..2e33377
--- /dev/null
+++ b/src/particles/contactDetection/nanoflann.hpp
@@ -0,0 +1,1634 @@
+/***********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ * Copyright 2008-2009 Marius Muja (mariusm@cs.ubc.ca). All rights reserved.
+ * Copyright 2008-2009 David G. Lowe (lowe@cs.ubc.ca). All rights reserved.
+ * Copyright 2011-2014 Jose Luis Blanco (joseluisblancoc@gmail.com).
+ * All rights reserved.
+ *
+ * THE BSD LICENSE
+ *
+ * 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.
+ *************************************************************************/
+
+#ifndef NANOFLANN_HPP_
+#define NANOFLANN_HPP_
+
+#include <vector>
+#include <cassert>
+#include <algorithm>
+#include <stdexcept>
+#include <cstdio> // for fwrite()
+#include <cmath> // for fabs(),...
+#include <limits>
+
+// Avoid conflicting declaration of min/max macros in windows headers
+#if !defined(NOMINMAX) && (defined(_WIN32) || defined(_WIN32_) || defined(WIN32) || defined(_WIN64))
+# define NOMINMAX
+# ifdef max
+# undef max
+# undef min
+# endif
+#endif
+
+namespace nanoflann {
+/** @addtogroup nanoflann_grp nanoflann C++ library for ANN
+ * @{ */
+
+/** Library version: 0xMmP (M=Major,m=minor,P=patch) */
+#define NANOFLANN_VERSION 0x119
+
+/** @addtogroup result_sets_grp Result set classes
+ * @{ */
+template<typename DistanceType, typename IndexType = size_t,
+ typename CountType = size_t>
+class KNNResultSet {
+ IndexType * indices;
+ DistanceType* dists;
+ CountType capacity;
+ CountType count;
+
+ public:
+ inline KNNResultSet(CountType capacity_)
+ : indices(0),
+ dists(0),
+ capacity(capacity_),
+ count(0) {
+ }
+
+ inline void init(IndexType* indices_, DistanceType* dists_) {
+ indices = indices_;
+ dists = dists_;
+ count = 0;
+ dists[capacity - 1] = (std::numeric_limits<DistanceType>::max)();
+ }
+
+ inline CountType size() const {
+ return count;
+ }
+
+ inline bool full() const {
+ return count == capacity;
+ }
+
+ inline void addPoint(DistanceType dist, IndexType index) {
+ CountType i;
+ for (i = count; i > 0; --i) {
+#ifdef NANOFLANN_FIRST_MATCH // If defined and two poins have the same distance, the one with the lowest-index will be returned first.
+ if ( (dists[i-1]>dist) || ((dist==dists[i-1])&&(indices[i-1]>index)) ) {
+#else
+ if (dists[i - 1] > dist) {
+#endif
+ if (i < capacity) {
+ dists[i] = dists[i - 1];
+ indices[i] = indices[i - 1];
+ }
+ } else
+ break;
+ }
+ if (i < capacity) {
+ dists[i] = dist;
+ indices[i] = index;
+ }
+ if (count < capacity)
+ count++;
+ }
+
+ inline DistanceType worstDist() const {
+ return dists[capacity - 1];
+ }
+};
+
+/**
+ * A result-set class used when performing a radius based search.
+ */
+template<typename DistanceType, typename IndexType = size_t>
+class RadiusResultSet {
+ public:
+ const DistanceType radius;
+
+ std::vector<std::pair<IndexType, DistanceType> >& m_indices_dists;
+
+ inline RadiusResultSet(
+ DistanceType radius_,
+ std::vector<std::pair<IndexType, DistanceType> >& indices_dists)
+ : radius(radius_),
+ m_indices_dists(indices_dists) {
+ init();
+ }
+
+ inline ~RadiusResultSet() {
+ }
+
+ inline void init() {
+ clear();
+ }
+ inline void clear() {
+ m_indices_dists.clear();
+ }
+
+ inline size_t size() const {
+ return m_indices_dists.size();
+ }
+
+ inline bool full() const {
+ return true;
+ }
+
+ inline void addPoint(DistanceType dist, IndexType index) {
+ if (dist < radius)
+ m_indices_dists.push_back(std::make_pair(index, dist));
+ }
+
+ inline DistanceType worstDist() const {
+ return radius;
+ }
+
+ /** Clears the result set and adjusts the search radius. */
+ inline void set_radius_and_clear(const DistanceType r) {
+ radius = r;
+ clear();
+ }
+
+ /**
+ * Find the worst result (furtherest neighbor) without copying or sorting
+ * Pre-conditions: size() > 0
+ */
+ std::pair<IndexType, DistanceType> worst_item() const {
+ if (m_indices_dists.empty())
+ throw std::runtime_error(
+ "Cannot invoke RadiusResultSet::worst_item() on an empty list of results.");
+ typedef typename std::vector<std::pair<IndexType, DistanceType> >::const_iterator DistIt;
+ DistIt it = std::max_element(m_indices_dists.begin(),
+ m_indices_dists.end());
+ return *it;
+ }
+};
+
+template<typename DistanceType, typename IndexType = size_t>
+class RadiusResultList {
+ public:
+ const DistanceType radius;
+
+// std::vector<std::pair<IndexType,DistanceType> >& m_indices_dists;
+ std::list<IndexType>& m_indices_list;
+
+ inline RadiusResultList(DistanceType radius_,
+ std::list<IndexType>& indices_list)
+ : radius(radius_),
+ m_indices_list(indices_list) {
+ init();
+ }
+
+ inline ~RadiusResultList() {
+ }
+
+ inline void init() {
+ clear();
+ }
+ inline void clear() {
+ m_indices_list.clear();
+ }
+
+ inline size_t size() const {
+ return m_indices_list.size();
+ }
+
+ inline bool full() const {
+ return true;
+ }
+
+ inline void addPoint(DistanceType dist, IndexType index) {
+ if (dist < radius)
+ m_indices_list.push_back(index);
+ }
+
+ inline DistanceType worstDist() const {
+ return radius;
+ }
+
+ /** Clears the result set and adjusts the search radius. */
+ inline void set_radius_and_clear(const DistanceType r) {
+ radius = r;
+ clear();
+ }
+
+ /**
+ * Find the worst result (furtherest neighbor) without copying or sorting
+ * Pre-conditions: size() > 0
+ */
+ std::pair<IndexType, DistanceType> worst_item() const {
+ if (m_indices_list.empty())
+ throw std::runtime_error(
+ "Cannot invoke RadiusResultSet::worst_item() on an empty list of results.");
+ typedef typename std::vector<std::pair<IndexType, DistanceType> >::const_iterator DistIt;
+ DistIt it = std::max_element(m_indices_list.begin(), m_indices_list.end());
+ return *it;
+ }
+};
+
+/** operator "<" for std::sort() */
+struct IndexDist_Sorter {
+ /** PairType will be typically: std::pair<IndexType,DistanceType> */
+ template<typename PairType>
+ inline bool operator()(const PairType &p1, const PairType &p2) const {
+ return p1.second < p2.second;
+ }
+};
+
+/** @} */
+
+/** @addtogroup loadsave_grp Load/save auxiliary functions
+ * @{ */
+template<typename T>
+void save_value(FILE* stream, const T& value, size_t count = 1) {
+ fwrite(&value, sizeof(value), count, stream);
+}
+
+template<typename T>
+void save_value(FILE* stream, const std::vector<T>& value) {
+ size_t size = value.size();
+ fwrite(&size, sizeof(size_t), 1, stream);
+ fwrite(&value[0], sizeof(T), size, stream);
+}
+
+template<typename T>
+void load_value(FILE* stream, T& value, size_t count = 1) {
+ size_t read_cnt = fread(&value, sizeof(value), count, stream);
+ if (read_cnt != count) {
+ throw std::runtime_error("Cannot read from file");
+ }
+}
+
+template<typename T>
+void load_value(FILE* stream, std::vector<T>& value) {
+ size_t size;
+ size_t read_cnt = fread(&size, sizeof(size_t), 1, stream);
+ if (read_cnt != 1) {
+ throw std::runtime_error("Cannot read from file");
+ }
+ value.resize(size);
+ read_cnt = fread(&value[0], sizeof(T), size, stream);
+ if (read_cnt != size) {
+ throw std::runtime_error("Cannot read from file");
+ }
+}
+/** @} */
+
+/** @addtogroup metric_grp Metric (distance) classes
+ * @{ */
+
+template<typename T> inline T abs(T x) {
+ return (x < 0) ? -x : x;
+}
+template<> inline int abs<int>(int x) {
+ return ::abs(x);
+}
+template<> inline float abs<float>(float x) {
+ return fabsf(x);
+}
+template<> inline double abs<double>(double x) {
+ return fabs(x);
+}
+template<> inline long double abs<long double>(long double x) {
+ return fabsl(x);
+}
+
+/** Manhattan distance functor (generic version, optimized for high-dimensionality data sets).
+ * Corresponding distance traits: nanoflann::metric_L1
+ * \tparam T Type of the elements (e.g. double, float, uint8_t)
+ * \tparam DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t)
+ */
+template<class T, class DataSource, typename _DistanceType = T>
+struct L1_Adaptor {
+ typedef T ElementType;
+ typedef _DistanceType DistanceType;
+
+ const DataSource &data_source;
+
+ L1_Adaptor(const DataSource &_data_source)
+ : data_source(_data_source) {
+ }
+
+ inline DistanceType operator()(const T* a, const size_t b_idx, size_t size,
+ DistanceType worst_dist = -1) const {
+ DistanceType result = DistanceType();
+ const T* last = a + size;
+ const T* lastgroup = last - 3;
+ size_t d = 0;
+
+ /* Process 4 items with each loop for efficiency. */
+ while (a < lastgroup) {
+ const DistanceType diff0 = nanoflann::abs(
+ a[0] - data_source.kdtree_get_pt(b_idx, d++));
+ const DistanceType diff1 = nanoflann::abs(
+ a[1] - data_source.kdtree_get_pt(b_idx, d++));
+ const DistanceType diff2 = nanoflann::abs(
+ a[2] - data_source.kdtree_get_pt(b_idx, d++));
+ const DistanceType diff3 = nanoflann::abs(
+ a[3] - data_source.kdtree_get_pt(b_idx, d++));
+ result += diff0 + diff1 + diff2 + diff3;
+ a += 4;
+ if ((worst_dist > 0) && (result > worst_dist)) {
+ return result;
+ }
+ }
+ /* Process last 0-3 components. Not needed for standard vector lengths. */
+ while (a < last) {
+ result += nanoflann::abs(*a++ - data_source.kdtree_get_pt(b_idx, d++));
+ }
+ return result;
+ }
+
+ template<typename U, typename V>
+ inline DistanceType accum_dist(const U a, const V b, int) const {
+ return nanoflann::abs(a - b);
+ }
+};
+
+/** Squared Euclidean distance functor (generic version, optimized for high-dimensionality data sets).
+ * Corresponding distance traits: nanoflann::metric_L2
+ * \tparam T Type of the elements (e.g. double, float, uint8_t)
+ * \tparam DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t)
+ */
+template<class T, class DataSource, typename _DistanceType = T>
+struct L2_Adaptor {
+ typedef T ElementType;
+ typedef _DistanceType DistanceType;
+
+ const DataSource &data_source;
+
+ L2_Adaptor(const DataSource &_data_source)
+ : data_source(_data_source) {
+ }
+
+ inline DistanceType operator()(const T* a, const size_t b_idx, size_t size,
+ DistanceType worst_dist = -1) const {
+ DistanceType result = DistanceType();
+ const T* last = a + size;
+ const T* lastgroup = last - 3;
+ size_t d = 0;
+
+ /* Process 4 items with each loop for efficiency. */
+ while (a < lastgroup) {
+ const DistanceType diff0 = a[0] - data_source.kdtree_get_pt(b_idx, d++);
+ const DistanceType diff1 = a[1] - data_source.kdtree_get_pt(b_idx, d++);
+ const DistanceType diff2 = a[2] - data_source.kdtree_get_pt(b_idx, d++);
+ const DistanceType diff3 = a[3] - data_source.kdtree_get_pt(b_idx, d++);
+ result += diff0 * diff0 + diff1 * diff1 + diff2 * diff2 + diff3 * diff3;
+ a += 4;
+ if ((worst_dist > 0) && (result > worst_dist)) {
+ return result;
+ }
+ }
+ /* Process last 0-3 components. Not needed for standard vector lengths. */
+ while (a < last) {
+ const DistanceType diff0 = *a++ - data_source.kdtree_get_pt(b_idx, d++);
+ result += diff0 * diff0;
+ }
+ return result;
+ }
+
+ template<typename U, typename V>
+ inline DistanceType accum_dist(const U a, const V b, int) const {
+ return (a - b) * (a - b);
+ }
+};
+
+/** Squared Euclidean (L2) distance functor (suitable for low-dimensionality datasets, like 2D or 3D point clouds)
+ * Corresponding distance traits: nanoflann::metric_L2_Simple
+ * \tparam T Type of the elements (e.g. double, float, uint8_t)
+ * \tparam DistanceType Type of distance variables (must be signed) (e.g. float, double, int64_t)
+ */
+template<class T, class DataSource, typename _DistanceType = T>
+struct L2_Simple_Adaptor {
+ typedef T ElementType;
+ typedef _DistanceType DistanceType;
+
+ const DataSource &data_source;
+
+ L2_Simple_Adaptor(const DataSource &_data_source)
+ : data_source(_data_source) {
+ }
+
+ inline DistanceType operator()(const T* a, const size_t b_idx,
+ size_t size) const {
+ return data_source.kdtree_distance(a, b_idx, size);
+ }
+
+ template<typename U, typename V>
+ inline DistanceType accum_dist(const U a, const V b, int) const {
+ return (a - b) * (a - b);
+ }
+};
+
+/** Metaprogramming helper traits class for the L1 (Manhattan) metric */
+struct metric_L1 {
+ template<class T, class DataSource>
+ struct traits {
+ typedef L1_Adaptor<T, DataSource> distance_t;
+ };
+};
+/** Metaprogramming helper traits class for the L2 (Euclidean) metric */
+struct metric_L2 {
+ template<class T, class DataSource>
+ struct traits {
+ typedef L2_Adaptor<T, DataSource> distance_t;
+ };
+};
+/** Metaprogramming helper traits class for the L2_simple (Euclidean) metric */
+struct metric_L2_Simple {
+ template<class T, class DataSource>
+ struct traits {
+ typedef L2_Simple_Adaptor<T, DataSource> distance_t;
+ };
+};
+
+/** @} */
+
+/** @addtogroup param_grp Parameter structs
+ * @{ */
+
+/** Parameters (see http://code.google.com/p/nanoflann/ for help choosing the parameters)
+ */
+struct KDTreeSingleIndexAdaptorParams {
+ KDTreeSingleIndexAdaptorParams(size_t _leaf_max_size = 10, int dim_ = -1)
+ : leaf_max_size(_leaf_max_size),
+ dim(dim_) {
+ }
+
+ size_t leaf_max_size;
+ int dim;
+};
+
+/** Search options for KDTreeSingleIndexAdaptor::findNeighbors() */
+struct SearchParams {
+ /** Note: The first argument (checks_IGNORED_) is ignored, but kept for compatibility with the FLANN interface */
+ SearchParams(int checks_IGNORED_ = 32, float eps_ = 0, bool sorted_ = true)
+ : checks(checks_IGNORED_),
+ eps(eps_),
+ sorted(sorted_) {
+ }
+
+ int checks; //!< Ignored parameter (Kept for compatibility with the FLANN interface).
+ float eps; //!< search for eps-approximate neighbours (default: 0)
+ bool sorted; //!< only for radius search, require neighbours sorted by distance (default: true)
+};
+/** @} */
+
+/** @addtogroup memalloc_grp Memory allocation
+ * @{ */
+
+/**
+ * Allocates (using C's malloc) a generic type T.
+ *
+ * Params:
+ * count = number of instances to allocate.
+ * Returns: pointer (of type T*) to memory buffer
+ */
+template<typename T>
+inline T* allocate(size_t count = 1) {
+ T* mem = (T*) ::malloc(sizeof(T) * count);
+ return mem;
+}
+
+/**
+ * Pooled storage allocator
+ *
+ * The following routines allow for the efficient allocation of storage in
+ * small chunks from a specified pool. Rather than allowing each structure
+ * to be freed individually, an entire pool of storage is freed at once.
+ * This method has two advantages over just using malloc() and free(). First,
+ * it is far more efficient for allocating small objects, as there is
+ * no overhead for remembering all the information needed to free each
+ * object or consolidating fragmented memory. Second, the decision about
+ * how long to keep an object is made at the time of allocation, and there
+ * is no need to track down all the objects to free them.
+ *
+ */
+
+const size_t WORDSIZE = 16;
+const size_t BLOCKSIZE = 8192;
+
+class PooledAllocator {
+ /* We maintain memory alignment to word boundaries by requiring that all
+ allocations be in multiples of the machine wordsize. */
+ /* Size of machine word in bytes. Must be power of 2. */
+ /* Minimum number of bytes requested at a time from the system. Must be multiple of WORDSIZE. */
+
+ size_t remaining; /* Number of bytes left in current block of storage. */
+ void* base; /* Pointer to base of current block of storage. */
+ void* loc; /* Current location in block to next allocate memory. */
+ size_t blocksize;
+
+ void internal_init() {
+ remaining = 0;
+ base = NULL;
+ usedMemory = 0;
+ wastedMemory = 0;
+ }
+
+ public:
+ size_t usedMemory;
+ size_t wastedMemory;
+
+ /**
+ Default constructor. Initializes a new pool.
+ */
+ PooledAllocator(const size_t blocksize_ = BLOCKSIZE)
+ : blocksize(blocksize_) {
+ internal_init();
+ }
+
+ /**
+ * Destructor. Frees all the memory allocated in this pool.
+ */
+ ~PooledAllocator() {
+ free_all();
+ }
+
+ /** Frees all allocated memory chunks */
+ void free_all() {
+ while (base != NULL) {
+ void *prev = *((void**) base); /* Get pointer to prev block. */
+ ::free(base);
+ base = prev;
+ }
+ internal_init();
+ }
+
+ /**
+ * Returns a pointer to a piece of new memory of the given size in bytes
+ * allocated from the pool.
+ */
+ void* malloc(const size_t req_size) {
+ /* Round size up to a multiple of wordsize. The following expression
+ only works for WORDSIZE that is a power of 2, by masking last bits of
+ incremented size to zero.
+ */
+ const size_t size = (req_size + (WORDSIZE - 1)) & ~(WORDSIZE - 1);
+
+ /* Check whether a new block must be allocated. Note that the first word
+ of a block is reserved for a pointer to the previous block.
+ */
+ if (size > remaining) {
+
+ wastedMemory += remaining;
+
+ /* Allocate new storage. */
+ const size_t blocksize =
+ (size + sizeof(void*) + (WORDSIZE - 1) > BLOCKSIZE) ?
+ size + sizeof(void*) + (WORDSIZE - 1) : BLOCKSIZE;
+
+ // use the standard C malloc to allocate memory
+ void* m = ::malloc(blocksize);
+ if (!m) {
+ fprintf(stderr, "Failed to allocate memory.\n");
+ return NULL;
+ }
+
+ /* Fill first word of new block with pointer to previous block. */
+ ((void**) m)[0] = base;
+ base = m;
+
+ size_t shift = 0;
+ //int size_t = (WORDSIZE - ( (((size_t)m) + sizeof(void*)) & (WORDSIZE-1))) & (WORDSIZE-1);
+
+ remaining = blocksize - sizeof(void*) - shift;
+ loc = ((char*) m + sizeof(void*) + shift);
+ }
+ void* rloc = loc;
+ loc = (char*) loc + size;
+ remaining -= size;
+
+ usedMemory += size;
+
+ return rloc;
+ }
+
+ /**
+ * Allocates (using this pool) a generic type T.
+ *
+ * Params:
+ * count = number of instances to allocate.
+ * Returns: pointer (of type T*) to memory buffer
+ */
+ template<typename T>
+ T* allocate(const size_t count = 1) {
+ T* mem = (T*) this->malloc(sizeof(T) * count);
+ return mem;
+ }
+
+};
+/** @} */
+
+/** @addtogroup nanoflann_metaprog_grp Auxiliary metaprogramming stuff
+ * @{ */
+
+// ---------------- CArray -------------------------
+/** A STL container (as wrapper) for arrays of constant size defined at compile time (class imported from the MRPT project)
+ * This code is an adapted version from Boost, modifed for its integration
+ * within MRPT (JLBC, Dec/2009) (Renamed array -> CArray to avoid possible potential conflicts).
+ * See
+ * http://www.josuttis.com/cppcode
+ * for details and the latest version.
+ * See
+ * http://www.boost.org/libs/array for Documentation.
+ * for documentation.
+ *
+ * (C) Copyright Nicolai M. Josuttis 2001.
+ * Permission to copy, use, modify, sell and distribute this software
+ * is granted provided this copyright notice appears in all copies.
+ * This software is provided "as is" without express or implied
+ * warranty, and with no claim as to its suitability for any purpose.
+ *
+ * 29 Jan 2004 - minor fixes (Nico Josuttis)
+ * 04 Dec 2003 - update to synch with library TR1 (Alisdair Meredith)
+ * 23 Aug 2002 - fix for Non-MSVC compilers combined with MSVC libraries.
+ * 05 Aug 2001 - minor update (Nico Josuttis)
+ * 20 Jan 2001 - STLport fix (Beman Dawes)
+ * 29 Sep 2000 - Initial Revision (Nico Josuttis)
+ *
+ * Jan 30, 2004
+ */
+template<typename T, std::size_t N>
+class CArray {
+ public:
+ T elems[N]; // fixed-size array of elements of type T
+
+ public:
+ // type definitions
+ typedef T value_type;
+ typedef T* iterator;
+ typedef const T* const_iterator;
+ typedef T& reference;
+ typedef const T& const_reference;
+ typedef std::size_t size_type;
+ typedef std::ptrdiff_t difference_type;
+
+ // iterator support
+ inline iterator begin() {
+ return elems;
+ }
+ inline const_iterator begin() const {
+ return elems;
+ }
+ inline iterator end() {
+ return elems + N;
+ }
+ inline const_iterator end() const {
+ return elems + N;
+ }
+
+ // reverse iterator support
+#if !defined(BOOST_NO_TEMPLATE_PARTIAL_SPECIALIZATION) && !defined(BOOST_MSVC_STD_ITERATOR) && !defined(BOOST_NO_STD_ITERATOR_TRAITS)
+ typedef std::reverse_iterator<iterator> reverse_iterator;
+ typedef std::reverse_iterator<const_iterator> const_reverse_iterator;
+#elif defined(_MSC_VER) && (_MSC_VER == 1300) && defined(BOOST_DINKUMWARE_STDLIB) && (BOOST_DINKUMWARE_STDLIB == 310)
+ // workaround for broken reverse_iterator in VC7
+ typedef std::reverse_iterator<std::_Ptrit<value_type, difference_type, iterator,
+ reference, iterator, reference> > reverse_iterator;
+ typedef std::reverse_iterator<std::_Ptrit<value_type, difference_type, const_iterator,
+ const_reference, iterator, reference> > const_reverse_iterator;
+#else
+ // workaround for broken reverse_iterator implementations
+ typedef std::reverse_iterator<iterator,T> reverse_iterator;
+ typedef std::reverse_iterator<const_iterator,T> const_reverse_iterator;
+#endif
+
+ reverse_iterator rbegin() {
+ return reverse_iterator(end());
+ }
+ const_reverse_iterator rbegin() const {
+ return const_reverse_iterator(end());
+ }
+ reverse_iterator rend() {
+ return reverse_iterator(begin());
+ }
+ const_reverse_iterator rend() const {
+ return const_reverse_iterator(begin());
+ }
+ // operator[]
+ inline reference operator[](size_type i) {
+ return elems[i];
+ }
+ inline const_reference operator[](size_type i) const {
+ return elems[i];
+ }
+ // at() with range check
+ reference at(size_type i) {
+ rangecheck(i);
+ return elems[i];
+ }
+ const_reference at(size_type i) const {
+ rangecheck(i);
+ return elems[i];
+ }
+ // front() and back()
+ reference front() {
+ return elems[0];
+ }
+ const_reference front() const {
+ return elems[0];
+ }
+ reference back() {
+ return elems[N - 1];
+ }
+ const_reference back() const {
+ return elems[N - 1];
+ }
+ // size is constant
+ static inline size_type size() {
+ return N;
+ }
+ static bool empty() {
+ return false;
+ }
+ static size_type max_size() {
+ return N;
+ }
+ enum {
+ static_size = N
+ };
+ /** This method has no effects in this class, but raises an exception if the expected size does not match */
+ inline void resize(const size_t nElements) {
+ if (nElements != N)
+ throw std::logic_error("Try to change the size of a CArray.");
+ }
+ // swap (note: linear complexity in N, constant for given instantiation)
+ void swap(CArray<T, N>& y) {
+ std::swap_ranges(begin(), end(), y.begin());
+ }
+ // direct access to data (read-only)
+ const T* data() const {
+ return elems;
+ }
+ // use array as C array (direct read/write access to data)
+ T* data() {
+ return elems;
+ }
+ // assignment with type conversion
+ template<typename T2> CArray<T, N>& operator=(const CArray<T2, N>& rhs) {
+ std::copy(rhs.begin(), rhs.end(), begin());
+ return *this;
+ }
+ // assign one value to all elements
+ inline void assign(const T& value) {
+ for (size_t i = 0; i < N; i++)
+ elems[i] = value;
+ }
+ // assign (compatible with std::vector's one) (by JLBC for MRPT)
+ void assign(const size_t n, const T& value) {
+ assert(N == n);
+ for (size_t i = 0; i < N; i++)
+ elems[i] = value;
+ }
+ private:
+ // check range (may be private because it is static)
+ static void rangecheck(size_type i) {
+ if (i >= size()) {
+ throw std::out_of_range("CArray<>: index out of range");
+ }
+ }
+};
+// end of CArray
+
+/** Used to declare fixed-size arrays when DIM>0, dynamically-allocated vectors when DIM=-1.
+ * Fixed size version for a generic DIM:
+ */
+template<int DIM, typename T>
+struct array_or_vector_selector {
+ typedef CArray<T, DIM> container_t;
+};
+/** Dynamic size version */
+template<typename T>
+struct array_or_vector_selector<-1, T> {
+ typedef std::vector<T> container_t;
+};
+/** @} */
+
+/** @addtogroup kdtrees_grp KD-tree classes and adaptors
+ * @{ */
+
+/** kd-tree index
+ *
+ * Contains the k-d trees and other information for indexing a set of points
+ * for nearest-neighbor matching.
+ *
+ * The class "DatasetAdaptor" must provide the following interface (can be non-virtual, inlined methods):
+ *
+ * \code
+ * // Must return the number of data points
+ * inline size_t kdtree_get_point_count() const { ... }
+ *
+ * // [Only if using the metric_L2_Simple type] Must return the Euclidean (L2) distance between the vector "p1[0:size-1]" and the data point with index "idx_p2" stored in the class:
+ * inline DistanceType kdtree_distance(const T *p1, const size_t idx_p2,size_t size) const { ... }
+ *
+ * // Must return the dim'th component of the idx'th point in the class:
+ * inline T kdtree_get_pt(const size_t idx, int dim) const { ... }
+ *
+ * // 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
+ * {
+ * bb[0].low = ...; bb[0].high = ...; // 0th dimension limits
+ * bb[1].low = ...; bb[1].high = ...; // 1st dimension limits
+ * ...
+ * return true;
+ * }
+ *
+ * \endcode
+ *
+ * \tparam DatasetAdaptor The user-provided adaptor (see comments above).
+ * \tparam Distance The distance metric to use: nanoflann::metric_L1, nanoflann::metric_L2, nanoflann::metric_L2_Simple, etc.
+ * \tparam IndexType Will be typically size_t or int
+ */
+template<typename Distance, class DatasetAdaptor, int DIM = -1,
+ typename IndexType = size_t>
+class KDTreeSingleIndexAdaptor {
+ private:
+ /** Hidden copy constructor, to disallow copying indices (Not implemented) */
+ KDTreeSingleIndexAdaptor(
+ const KDTreeSingleIndexAdaptor<Distance, DatasetAdaptor, DIM, IndexType>&);
+ public:
+ typedef typename Distance::ElementType ElementType;
+ typedef typename Distance::DistanceType DistanceType;
+ protected:
+
+ /**
+ * Array of indices to vectors in the dataset.
+ */
+ std::vector<IndexType> vind;
+
+ size_t m_leaf_max_size;
+
+ /**
+ * The dataset used by this index
+ */
+ const DatasetAdaptor &dataset; //!< The source of our data
+
+ const KDTreeSingleIndexAdaptorParams index_params;
+
+ size_t m_size;
+ int dim; //!< Dimensionality of each data point
+
+ /*--------------------- Internal Data Structures --------------------------*/
+ struct Node {
+ union {
+ struct {
+ /**
+ * Indices of points in leaf node
+ */
+ IndexType left, right;
+ } lr;
+ struct {
+ /**
+ * Dimension used for subdivision.
+ */
+ int divfeat;
+ /**
+ * The values used for subdivision.
+ */
+ DistanceType divlow, divhigh;
+ } sub;
+ };
+ /**
+ * The child nodes.
+ */
+ Node* child1, *child2;
+ };
+ typedef Node* NodePtr;
+
+ struct Interval {
+ ElementType low, high;
+ };
+
+ /** Define "BoundingBox" as a fixed-size or variable-size container depending on "DIM" */
+ typedef typename array_or_vector_selector<DIM, Interval>::container_t BoundingBox;
+
+ /** Define "distance_vector_t" as a fixed-size or variable-size container depending on "DIM" */
+ typedef typename array_or_vector_selector<DIM, DistanceType>::container_t distance_vector_t;
+
+ /** This record represents a branch point when finding neighbors in
+ the tree. It contains a record of the minimum distance to the query
+ point, as well as the node at which the search resumes.
+ */
+ template<typename T, typename DistanceType>
+ struct BranchStruct {
+ T node; /* Tree node at which search resumes */
+ DistanceType mindist; /* Minimum distance to query for all nodes below. */
+
+ BranchStruct() {
+ }
+ BranchStruct(const T& aNode, DistanceType dist)
+ : node(aNode),
+ mindist(dist) {
+ }
+
+ inline bool operator<(const BranchStruct<T, DistanceType>& rhs) const {
+ return mindist < rhs.mindist;
+ }
+ };
+
+ /**
+ * Array of k-d trees used to find neighbours.
+ */
+ NodePtr root_node;
+ typedef BranchStruct<NodePtr, DistanceType> BranchSt;
+ typedef BranchSt* Branch;
+
+ BoundingBox root_bbox;
+
+ /**
+ * Pooled memory allocator.
+ *
+ * Using a pooled memory allocator is more efficient
+ * than allocating memory directly when there is a large
+ * number small of memory allocations.
+ */
+ PooledAllocator pool;
+
+ public:
+
+ Distance distance;
+
+ /**
+ * KDTree constructor
+ *
+ * Params:
+ * inputData = dataset with the input features
+ * params = parameters passed to the kdtree algorithm (see http://code.google.com/p/nanoflann/ for help choosing the parameters)
+ */
+ KDTreeSingleIndexAdaptor(const int dimensionality,
+ const DatasetAdaptor& inputData,
+ const KDTreeSingleIndexAdaptorParams& params =
+ KDTreeSingleIndexAdaptorParams())
+ : m_leaf_max_size(0),
+ dataset(inputData),
+ index_params(params),
+ m_size(0),
+ dim(dimensionality),
+ root_node(NULL),
+ distance(in