diff options
Diffstat (limited to 'src/particles/contactDetection')
| -rw-r--r-- | src/particles/contactDetection/contactDetection.h | 66 | ||||
| -rw-r--r-- | src/particles/contactDetection/nanoflann.hpp | 1634 | ||||
| -rw-r--r-- | src/particles/contactDetection/nanoflann_adaptor.hpp | 127 | ||||
| -rw-r--r-- | src/particles/contactDetection/pLattice.h | 205 | ||||
| -rw-r--r-- | src/particles/contactDetection/sortAlgorithms.h | 31 |
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 |