Initialize at openlb-1-3

1 files changed, 1634 insertions, 0 deletions

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(inputData) {
+    m_size = 0;
+    if (DIM > 0)
+      dim = DIM;
+    else {
+      if (params.dim > 0)
+        dim = params.dim;
+    }
+    m_leaf_max_size = params.leaf_max_size;
+  }
+
+  void init() {
+    m_size = dataset.kdtree_get_point_count();
+
+    // Create a permutable array of indices to the input vectors.
+    init_vind();
+  }
+
+  /**
+   * Standard destructor
+   */
+  ~KDTreeSingleIndexAdaptor() {
+  }
+
+  /** Frees the previously-built index. Automatically called within buildIndex(). */
+  void freeIndex() {
+    pool.free_all();
+    root_node = NULL;
+  }
+
+  /**
+   * Builds the index
+   */
+  void buildIndex() {
+    init_vind();
+    freeIndex();
+    if (m_size == 0)
+      return;
+    computeBoundingBox(root_bbox);
+    root_node = divideTree(0, m_size, root_bbox);   // construct the tree
+  }
+
+  /**
+   *  Returns size of index.
+   */
+  size_t size() const {
+    return m_size;
+  }
+
+  /**
+   * Returns the length of an index feature.
+   */
+  size_t veclen() const {
+    return static_cast<size_t>(DIM > 0 ? DIM : dim);
+  }
+
+  /**
+   * Computes the inde memory usage
+   * Returns: memory used by the index
+   */
+  size_t usedMemory() const {
+    return pool.usedMemory + pool.wastedMemory
+        + dataset.kdtree_get_point_count() * sizeof(IndexType);  // pool memory and vind array memory
+  }
+
+  /** \name Query methods
+   * @{ */
+
+  /**
+   * Find set of nearest neighbors to vec[0:dim-1]. Their indices are stored inside
+   * the result object.
+   *
+