failed attempt to use 2 aabb trees

Former-commit-id: 5e79472162ff95dee6092e5daf086ca8489529be

include/igl/AABB.h

@@ -23,210 +23,252 @@ namespace igl
   template <typename DerivedV, int DIM>
     class AABB 
     {
-      public:
-        typedef typename DerivedV::Scalar Scalar;
-        typedef Eigen::Matrix<Scalar,1,DIM> RowVectorDIMS;
-        typedef Eigen::Matrix<Scalar,DIM,1> VectorDIMS;
-        typedef Eigen::Matrix<Scalar,Eigen::Dynamic,DIM> MatrixXDIMS;
-        // Shared pointers are slower...
-        AABB * m_left;
-        AABB * m_right;
-        Eigen::AlignedBox<Scalar,DIM> m_box;
-        // -1 non-leaf
-        int m_primitive;
-        AABB():
-          m_left(NULL), m_right(NULL),
-          m_box(), m_primitive(-1)
-      {}
-        // http://stackoverflow.com/a/3279550/148668
-        AABB(const AABB& other):
-          m_left(other.m_left ? new AABB(*other.m_left) : NULL),
-          m_right(other.m_right ? new AABB(*other.m_right) : NULL),
-          m_box(other.m_box),
-          m_primitive(other.m_primitive)
-          {
-          }
-        // copy-swap idiom
-        friend void swap(AABB& first, AABB& second)
-        {
-          // Enable ADL
-          using std::swap;
-          swap(first.m_left,second.m_left);
-          swap(first.m_right,second.m_right);
-          swap(first.m_box,second.m_box);
-          swap(first.m_primitive,second.m_primitive);
-        }
-        // Pass-by-value (aka copy)
-        AABB& operator=(AABB other)
+public:
+      typedef typename DerivedV::Scalar Scalar;
+      typedef Eigen::Matrix<Scalar,1,DIM> RowVectorDIMS;
+      typedef Eigen::Matrix<Scalar,DIM,1> VectorDIMS;
+      typedef Eigen::Matrix<Scalar,Eigen::Dynamic,DIM> MatrixXDIMS;
+      // Shared pointers are slower...
+      AABB * m_left;
+      AABB * m_right;
+      Eigen::AlignedBox<Scalar,DIM> m_box;
+      // -1 non-leaf
+      int m_primitive;
+      //Scalar m_max_sqr_d;
+      //int m_depth;
+      AABB():
+        m_left(NULL), m_right(NULL),
+        m_box(), m_primitive(-1)
+        //m_max_sqr_d(std::numeric_limits<double>::infinity()),
+        //m_depth(0)
+    {}
+      // http://stackoverflow.com/a/3279550/148668
+      AABB(const AABB& other):
+        m_left(other.m_left ? new AABB(*other.m_left) : NULL),
+        m_right(other.m_right ? new AABB(*other.m_right) : NULL),
+        m_box(other.m_box),
+        m_primitive(other.m_primitive)
+        //m_max_sqr_d(other.m_max_sqr_d),
+        //m_depth(std::max(
+        //   m_left ? m_left->m_depth + 1 : 0,
+        //   m_right ? m_right->m_depth + 1 : 0))
         {
-          swap(*this,other);
-          return *this;
         }
-        AABB(AABB&& other):
-          // initialize via default constructor
-          AABB() 
+      // copy-swap idiom
+      friend void swap(AABB& first, AABB& second)
+      {
+        // Enable ADL
+        using std::swap;
+        swap(first.m_left,second.m_left);
+        swap(first.m_right,second.m_right);
+        swap(first.m_box,second.m_box);
+        swap(first.m_primitive,second.m_primitive);
+        //swap(first.m_max_sqr_d,second.m_max_sqr_d);
+        //swap(first.m_depth,second.m_depth);
+      }
+      // Pass-by-value (aka copy)
+      AABB& operator=(AABB other)
       {
         swap(*this,other);
+        return *this;
       }
-        // Seems like there should have been an elegant solution to this using
-        // the copy-swap idiom above:
-        inline void deinit()
-        {
-          m_primitive = -1;
-          m_box = Eigen::AlignedBox<Scalar,DIM>();
-          delete m_left;
-          m_left = NULL;
-          delete m_right;
-          m_right = NULL;
-        }
-        ~AABB()
-        {
-          deinit();
-        }
-        // Build an Axis-Aligned Bounding Box tree for a given mesh and given
-        // serialization of a previous AABB tree.
-        //
-        // Inputs:
-        //   V  #V by dim list of mesh vertex positions. 
-        //   Ele  #Ele by dim+1 list of mesh indices into #V. 
-        //   bb_mins  max_tree by dim list of bounding box min corner positions
-        //   bb_maxs  max_tree by dim list of bounding box max corner positions
-        //   elements  max_tree list of element or (not leaf id) indices into Ele
-        //   i  recursive call index {0}
-        template <typename Derivedbb_mins, typename Derivedbb_maxs>
-          inline void init(
-              const Eigen::PlainObjectBase<DerivedV> & V,
-              const Eigen::MatrixXi & Ele, 
-              const Eigen::PlainObjectBase<Derivedbb_mins> & bb_mins,
-              const Eigen::PlainObjectBase<Derivedbb_maxs> & bb_maxs,
-              const Eigen::VectorXi & elements,
-              const int i = 0);
-        // Wrapper for root with empty serialization
-        inline void init(
-            const Eigen::PlainObjectBase<DerivedV> & V,
-            const Eigen::MatrixXi & Ele);
-        // Build an Axis-Aligned Bounding Box tree for a given mesh.
-        //
-        // Inputs:
-        //   V  #V by dim list of mesh vertex positions. 
-        //   Ele  #Ele by dim+1 list of mesh indices into #V. 
-        //   SI  #Ele by dim list revealing for each coordinate where Ele's
-        //     barycenters would be sorted: SI(e,d) = i --> the dth coordinate of
-        //     the barycenter of the eth element would be placed at position i in a
-        //     sorted list.
-        //   I  #I list of indices into Ele of elements to include (for recursive
-        //     calls)
-        // 
+      AABB(AABB&& other):
+        // initialize via default constructor
+        AABB() 
+    {
+      swap(*this,other);
+    }
+      // Seems like there should have been an elegant solution to this using
+      // the copy-swap idiom above:
+      inline void deinit()
+      {
+        m_primitive = -1;
+        m_box = Eigen::AlignedBox<Scalar,DIM>();
+        delete m_left;
+        m_left = NULL;
+        delete m_right;
+        m_right = NULL;
+      }
+      ~AABB()
+      {
+        deinit();
+      }
+      // Build an Axis-Aligned Bounding Box tree for a given mesh and given
+      // serialization of a previous AABB tree.
+      //
+      // Inputs:
+      //   V  #V by dim list of mesh vertex positions. 
+      //   Ele  #Ele by dim+1 list of mesh indices into #V. 
+      //   bb_mins  max_tree by dim list of bounding box min corner positions
+      //   bb_maxs  max_tree by dim list of bounding box max corner positions
+      //   elements  max_tree list of element or (not leaf id) indices into Ele
+      //   i  recursive call index {0}
+      template <typename Derivedbb_mins, typename Derivedbb_maxs>
         inline void init(
             const Eigen::PlainObjectBase<DerivedV> & V,
             const Eigen::MatrixXi & Ele, 
-            const Eigen::MatrixXi & SI,
-            const Eigen::VectorXi & I);
-        // Return whether at leaf node
-        inline bool is_leaf() const;
-        // Find the indices of elements containing given point.
-        //
-        // Inputs:
-        //   V  #V by dim list of mesh vertex positions. **Should be same as used to
-        //     construct mesh.**
-        //   Ele  #Ele by dim+1 list of mesh indices into #V. **Should be same as used to
-        //     construct mesh.**
-        //   q  dim row-vector query position
-        //   first  whether to only return first element containing q
-        // Returns:
-        //   list of indices of elements containing q
-        template <typename Derivedq>
-        inline std::vector<int> find(
-            const Eigen::PlainObjectBase<DerivedV> & V,
-            const Eigen::MatrixXi & Ele, 
-            const Eigen::PlainObjectBase<Derivedq> & q,
-            const bool first=false) const;
-
-        // If number of elements m then total tree size should be 2*h where h is
-        // the deepest depth 2^ceil(log(#Ele*2-1))
-        inline int subtree_size() const;
-
-        // Serialize this class into 3 arrays (so we can pass it pack to matlab)
-        //
-        // Outputs:
-        //   bb_mins  max_tree by dim list of bounding box min corner positions
-        //   bb_maxs  max_tree by dim list of bounding box max corner positions
-        //   elements  max_tree list of element or (not leaf id) indices into Ele
-        //   i  recursive call index into these arrays {0}
-        template <typename Derivedbb_mins, typename Derivedbb_maxs>
-          inline void serialize(
-              Eigen::PlainObjectBase<Derivedbb_mins> & bb_mins,
-              Eigen::PlainObjectBase<Derivedbb_maxs> & bb_maxs,
-              Eigen::VectorXi & elements,
-              const int i = 0) const;
-        // Compute squared distance to a query point
-        //
-        // Inputs:
-        //   V  #V by dim list of vertex positions
-        //   Ele  #Ele by dim list of simplex indices
-        //   P  3 list of query point coordinates
-        //   min_sqr_d  current minimum squared distance (only find distances
-        //   less than this)
-        // Outputs:
-        //   I  #P list of facet indices corresponding to smallest distances
-        //   C  #P by 3 list of closest points
-        // Returns squared distance
-        //
-        // Known bugs: currently assumes Elements are triangles regardless of
-        // dimension.
-        inline Scalar squared_distance(
-            const Eigen::PlainObjectBase<DerivedV> & V,
-            const Eigen::MatrixXi & Ele, 
-            const RowVectorDIMS & p,
-            int & i,
-            RowVectorDIMS & c) const;
-      private:
-        inline Scalar squared_distance(
-            const Eigen::PlainObjectBase<DerivedV> & V,
-            const Eigen::MatrixXi & Ele, 
-            const RowVectorDIMS & p,
-            const Scalar min_sqr_d,
-            int & i,
-            RowVectorDIMS & c) const;
-      public:
-        template <
-          typename DerivedP, 
-                   typename DerivedsqrD, 
-                   typename DerivedI, 
-                   typename DerivedC>
-                     inline void squared_distance(
-                         const Eigen::PlainObjectBase<DerivedV> & V,
-                         const Eigen::MatrixXi & Ele, 
-                         const Eigen::PlainObjectBase<DerivedP> & P,
-                         Eigen::PlainObjectBase<DerivedsqrD> & sqrD,
-                         Eigen::PlainObjectBase<DerivedI> & I,
-                         Eigen::PlainObjectBase<DerivedC> & C) const;
-      private:
-        // Helper function for leaves: works in-place on sqr_d
-        inline void leaf_squared_distance(
-            const Eigen::PlainObjectBase<DerivedV> & V,
-            const Eigen::MatrixXi & Ele, 
-            const RowVectorDIMS & p,
-            Scalar & sqr_d,
-            int & i,
-            RowVectorDIMS & c) const;
-        inline void set_min(
-            const RowVectorDIMS & p,
-            const Scalar sqr_d_candidate,
-            const int i_candidate,
-            const RowVectorDIMS & c_candidate,
-            Scalar & sqr_d,
-            int & i,
-            RowVectorDIMS & c) const;
-      public:
-        template <int SS>
-          static
-          void barycentric_coordinates(
-              const RowVectorDIMS & p, 
-              const RowVectorDIMS & a, 
-              const RowVectorDIMS & b, 
-              const RowVectorDIMS & c,
-              Eigen::Matrix<Scalar,1,SS> & bary);
+            const Eigen::PlainObjectBase<Derivedbb_mins> & bb_mins,
+            const Eigen::PlainObjectBase<Derivedbb_maxs> & bb_maxs,
+            const Eigen::VectorXi & elements,
+            const int i = 0);
+      // Wrapper for root with empty serialization
+      inline void init(
+          const Eigen::PlainObjectBase<DerivedV> & V,
+          const Eigen::MatrixXi & Ele);
+      // Build an Axis-Aligned Bounding Box tree for a given mesh.
+      //
+      // Inputs:
+      //   V  #V by dim list of mesh vertex positions. 
+      //   Ele  #Ele by dim+1 list of mesh indices into #V. 
+      //   SI  #Ele by dim list revealing for each coordinate where Ele's
+      //     barycenters would be sorted: SI(e,d) = i --> the dth coordinate of
+      //     the barycenter of the eth element would be placed at position i in a
+      //     sorted list.
+      //   I  #I list of indices into Ele of elements to include (for recursive
+      //     calls)
+      // 
+      inline void init(
+          const Eigen::PlainObjectBase<DerivedV> & V,
+          const Eigen::MatrixXi & Ele, 
+          const Eigen::MatrixXi & SI,
+          const Eigen::VectorXi & I);
+      // Return whether at leaf node
+      inline bool is_leaf() const;
+      // Find the indices of elements containing given point.
+      //
+      // Inputs:
+      //   V  #V by dim list of mesh vertex positions. **Should be same as used to
+      //     construct mesh.**
+      //   Ele  #Ele by dim+1 list of mesh indices into #V. **Should be same as used to
+      //     construct mesh.**
+      //   q  dim row-vector query position
+      //   first  whether to only return first element containing q
+      // Returns:
+      //   list of indices of elements containing q
+      template <typename Derivedq>
+      inline std::vector<int> find(
+          const Eigen::PlainObjectBase<DerivedV> & V,
+          const Eigen::MatrixXi & Ele, 
+          const Eigen::PlainObjectBase<Derivedq> & q,
+          const bool first=false) const;
+
+      // If number of elements m then total tree size should be 2*h where h is
+      // the deepest depth 2^ceil(log(#Ele*2-1))
+      inline int subtree_size() const;
+
+      // Serialize this class into 3 arrays (so we can pass it pack to matlab)
+      //
+      // Outputs:
+      //   bb_mins  max_tree by dim list of bounding box min corner positions
+      //   bb_maxs  max_tree by dim list of bounding box max corner positions
+      //   elements  max_tree list of element or (not leaf id) indices into Ele
+      //   i  recursive call index into these arrays {0}
+      template <typename Derivedbb_mins, typename Derivedbb_maxs>
+        inline void serialize(
+            Eigen::PlainObjectBase<Derivedbb_mins> & bb_mins,
+            Eigen::PlainObjectBase<Derivedbb_maxs> & bb_maxs,
+            Eigen::VectorXi & elements,
+            const int i = 0) const;
+      // Compute squared distance to a query point
+      //
+      // Inputs:
+      //   V  #V by dim list of vertex positions
+      //   Ele  #Ele by dim list of simplex indices
+      //   P  3 list of query point coordinates
+      //   min_sqr_d  current minimum squared distance (only find distances
+      //   less than this)
+      // Outputs:
+      //   I  #P list of facet indices corresponding to smallest distances
+      //   C  #P by 3 list of closest points
+      // Returns squared distance
+      //
+      // Known bugs: currently assumes Elements are triangles regardless of
+      // dimension.
+      inline Scalar squared_distance(
+          const Eigen::PlainObjectBase<DerivedV> & V,
+          const Eigen::MatrixXi & Ele, 
+          const RowVectorDIMS & p,
+          int & i,
+          RowVectorDIMS & c) const;
+private:
+      inline Scalar squared_distance(
+          const Eigen::PlainObjectBase<DerivedV> & V,
+          const Eigen::MatrixXi & Ele, 
+          const RowVectorDIMS & p,
+          const Scalar min_sqr_d,
+          int & i,
+          RowVectorDIMS & c) const;
+public:
+      template <
+        typename DerivedP, 
+        typename DerivedsqrD, 
+        typename DerivedI, 
+        typename DerivedC>
+      inline void squared_distance(
+        const Eigen::PlainObjectBase<DerivedV> & V,
+        const Eigen::MatrixXi & Ele, 
+        const Eigen::PlainObjectBase<DerivedP> & P,
+        Eigen::PlainObjectBase<DerivedsqrD> & sqrD,
+        Eigen::PlainObjectBase<DerivedI> & I,
+        Eigen::PlainObjectBase<DerivedC> & C) const;
+
+      template < 
+        typename Derivedother_V,
+        typename DerivedsqrD, 
+        typename DerivedI, 
+        typename DerivedC>
+      inline void squared_distance(
+        const Eigen::PlainObjectBase<DerivedV> & V,
+        const Eigen::MatrixXi & Ele, 
+        const AABB<Derivedother_V,DIM> & other,
+        const Eigen::PlainObjectBase<Derivedother_V> & other_V,
+        const Eigen::MatrixXi & other_Ele, 
+        Eigen::PlainObjectBase<DerivedsqrD> & sqrD,
+        Eigen::PlainObjectBase<DerivedI> & I,
+        Eigen::PlainObjectBase<DerivedC> & C) const;
+private:
+      template < 
+        typename Derivedother_V,
+        typename DerivedsqrD, 
+        typename DerivedI, 
+        typename DerivedC>
+      inline Scalar squared_distance_helper(
+        const Eigen::PlainObjectBase<DerivedV> & V,
+        const Eigen::MatrixXi & Ele, 
+        const AABB<Derivedother_V,DIM> * other,
+        const Eigen::PlainObjectBase<Derivedother_V> & other_V,
+        const Eigen::MatrixXi & other_Ele, 
+        const Scalar min_sqr_d,
+        Eigen::PlainObjectBase<DerivedsqrD> & sqrD,
+        Eigen::PlainObjectBase<DerivedI> & I,
+        Eigen::PlainObjectBase<DerivedC> & C) const;
+      // Helper function for leaves: works in-place on sqr_d
+      inline void leaf_squared_distance(
+        const Eigen::PlainObjectBase<DerivedV> & V,
+        const Eigen::MatrixXi & Ele, 
+        const RowVectorDIMS & p,
+        Scalar & sqr_d,
+        int & i,
+        RowVectorDIMS & c) const;
+      inline void set_min(
+        const RowVectorDIMS & p,
+        const Scalar sqr_d_candidate,
+        const int i_candidate,
+        const RowVectorDIMS & c_candidate,
+        Scalar & sqr_d,
+        int & i,
+        RowVectorDIMS & c) const;
+public:
+      template <int SS>
+      static
+      void barycentric_coordinates(
+        const RowVectorDIMS & p, 
+        const RowVectorDIMS & a, 
+        const RowVectorDIMS & b, 
+        const RowVectorDIMS & c,
+        Eigen::Matrix<Scalar,1,SS> & bary);
+public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
     };
 }
 
@@ -241,7 +283,9 @@ namespace igl
 #include "sort.h"
 #include "volume.h"
 #include <iostream>
+#include <iomanip>
 #include <limits>
+#include <list>
 
 template <typename DerivedV, int DIM>
   template <typename Derivedbb_mins, typename Derivedbb_maxs>
@@ -273,12 +317,21 @@ inline void igl::AABB<DerivedV,DIM>::init(
       m_left->init( V,Ele,bb_mins,bb_maxs,elements,2*i+1);
       m_right = new AABB();
       m_right->init( V,Ele,bb_mins,bb_maxs,elements,2*i+2);
+      //m_depth = std::max( m_left->m_depth, m_right->m_depth)+1;
     }
   }else
   {
     VectorXi allI = colon<int>(0,Ele.rows()-1);
     MatrixXDIMS BC;
-    barycenter(V,Ele,BC);
+    if(Ele.cols() == 1)
+    {
+      // points
+      BC = V;
+    }else
+    {
+      // Simplices
+      barycenter(V,Ele,BC);
+    }
     MatrixXi SI(BC.rows(),BC.cols());
     {
       MatrixXDIMS _;
@@ -384,15 +437,18 @@ inline void igl::AABB<DerivedV,DIM>::init(
             }
           }
         }
+        //m_depth = 0;
         if(LI.rows()>0)
         {
           m_left = new AABB();
           m_left->init(V,Ele,SI,LI);
+          //m_depth = std::max(m_depth, m_left->m_depth+1);
         }
         if(RI.rows()>0)
         {
           m_right = new AABB();
           m_right->init(V,Ele,SI,RI);
+          //m_depth = std::max(m_depth, m_right->m_depth+1);
         }
       }
   }
@@ -584,7 +640,7 @@ igl::AABB<DerivedV,DIM>::squared_distance(
     const auto & look_left = [&]()
     {
       int i_left;
-      RowVectorDIMS c_left;
+      RowVectorDIMS c_left = c;
       Scalar sqr_d_left = m_left->squared_distance(V,Ele,p,sqr_d,i_left,c_left);
       set_min(p,sqr_d_left,i_left,c_left,sqr_d,i,c);
       looked_left = true;
@@ -592,7 +648,7 @@ igl::AABB<DerivedV,DIM>::squared_distance(
     const auto & look_right = [&]()
     {
       int i_right;
-      RowVectorDIMS c_right;
+      RowVectorDIMS c_right = c;
       Scalar sqr_d_right = m_right->squared_distance(V,Ele,p,sqr_d,i_right,c_right);
       set_min(p,sqr_d_right,i_right,c_right,sqr_d,i,c);
       looked_right = true;
@@ -663,6 +719,269 @@ inline void igl::AABB<DerivedV,DIM>::squared_distance(
   }
 }
 
+template <typename DerivedV, int DIM>
+template < 
+  typename Derivedother_V,
+  typename DerivedsqrD, 
+  typename DerivedI, 
+  typename DerivedC>
+inline void igl::AABB<DerivedV,DIM>::squared_distance(
+  const Eigen::PlainObjectBase<DerivedV> & V,
+  const Eigen::MatrixXi & Ele, 
+  const AABB<Derivedother_V,DIM> & other,
+  const Eigen::PlainObjectBase<Derivedother_V> & other_V,
+  const Eigen::MatrixXi & other_Ele, 
+  Eigen::PlainObjectBase<DerivedsqrD> & sqrD,
+  Eigen::PlainObjectBase<DerivedI> & I,
+  Eigen::PlainObjectBase<DerivedC> & C) const
+{
+  assert(other_Ele.cols() == 1 && 
+    "Only implemented for other as list of points");
+  assert(other_V.cols() == V.cols() && "other must match this dimension");
+  sqrD.setConstant(other_Ele.rows(),1,std::numeric_limits<double>::infinity());
+  I.resize(other_Ele.rows(),1);
+  C.resize(other_Ele.rows(),other_V.cols());
+  // All points in other_V currently think they need to check against root of
+  // this. The point of using another AABB is to quickly prune chunks of
+  // other_V so that most points just check some subtree of this.
+
+  // This holds a conservative estimate of max(sqr_D) where sqr_D is the
+  // current best minimum squared distance for all points in this subtree
+  double min_sqr_d = std::numeric_limits<double>::infinity();
+  squared_distance_helper(
+    V,Ele,&other,other_V,other_Ele,min_sqr_d,sqrD,I,C);
+}
+
+template <typename DerivedV, int DIM>
+template < 
+  typename Derivedother_V,
+  typename DerivedsqrD, 
+  typename DerivedI, 
+  typename DerivedC>
+inline typename igl::AABB<DerivedV,DIM>::Scalar igl::AABB<DerivedV,DIM>::squared_distance_helper(
+  const Eigen::PlainObjectBase<DerivedV> & V,
+  const Eigen::MatrixXi & Ele, 
+  const AABB<Derivedother_V,DIM> * other,
+  const Eigen::PlainObjectBase<Derivedother_V> & other_V,
+  const Eigen::MatrixXi & other_Ele, 
+  const Scalar min_sqr_d,
+  Eigen::PlainObjectBase<DerivedsqrD> & sqrD,
+  Eigen::PlainObjectBase<DerivedI> & I,
+  Eigen::PlainObjectBase<DerivedC> & C) const
+{
+  using namespace std;
+  using namespace Eigen;
+
+  // This implementation is a bit disappointing. There's no major speed up. Any
+  // performance gains seem to come from accidental cache coherency and
+  // diminish for larger "other" (the opposite of what was intended).
+
+  // Base case
+  if(other->is_leaf() && this->is_leaf())
+  {
+    Scalar sqr_d = sqrD(other->m_primitive);
+    int i = I(other->m_primitive);
+    RowVectorDIMS c = C.row(      other->m_primitive);
+    RowVectorDIMS p = other_V.row(other->m_primitive);
+    leaf_squared_distance(V,Ele,p,sqr_d,i,c);
+    sqrD( other->m_primitive) = sqr_d;
+    I(    other->m_primitive) = i;
+    C.row(other->m_primitive) = c;
+    //cout<<"leaf: "<<sqr_d<<endl;
+    //other->m_max_sqr_d = sqr_d;
+    return sqr_d;
+  }
+
+  if(other->is_leaf())
+  {
+    Scalar sqr_d = sqrD(other->m_primitive);
+    int i = I(other->m_primitive);
+    RowVectorDIMS c = C.row(      other->m_primitive);
+    RowVectorDIMS p = other_V.row(other->m_primitive);
+    sqr_d = squared_distance(V,Ele,p,sqr_d,i,c);
+    sqrD( other->m_primitive) = sqr_d;
+    I(    other->m_primitive) = i;
+    C.row(other->m_primitive) = c;
+    //other->m_max_sqr_d = sqr_d;
+    return sqr_d;
+  }
+
+  //// Exact minimum squared distance between arbitary primitives inside this and
+  //// othre's bounding boxes
+  //const auto & min_squared_distance = [&](
+  //  const AABB<DerivedV,DIM> * A,
+  //  const AABB<Derivedother_V,DIM> * B)->Scalar
+  //{
+  //  return A->m_box.squaredExteriorDistance(B->m_box);
+  //};
+
+  if(this->is_leaf())
+  {
+    //if(min_squared_distance(this,other) < other->m_max_sqr_d)
+    if(true)
+    {
+      this->squared_distance_helper(
+        V,Ele,other->m_left,other_V,other_Ele,0,sqrD,I,C);
+      this->squared_distance_helper(
+        V,Ele,other->m_right,other_V,other_Ele,0,sqrD,I,C);
+    }else
+    {
+      // This is never reached...
+    }
+    //// we know other is not a leaf
+    //other->m_max_sqr_d = std::max(other->m_left->m_max_sqr_d,other->m_right->m_max_sqr_d);
+    return 0;
+  }
+
+  // FORCE DOWN TO OTHER LEAF EVAL
+  //if(min_squared_distance(this,other) < other->m_max_sqr_d)
+  if(true)
+  {
+    if(true)
+    {
+      this->squared_distance_helper(
+        V,Ele,other->m_left,other_V,other_Ele,0,sqrD,I,C);
+      this->squared_distance_helper(
+        V,Ele,other->m_right,other_V,other_Ele,0,sqrD,I,C);
+    }else // this direction never seems to be faster
+    {
+      this->m_left->squared_distance_helper(
+        V,Ele,other,other_V,other_Ele,0,sqrD,I,C);
+      this->m_right->squared_distance_helper(
+        V,Ele,other,other_V,other_Ele,0,sqrD,I,C);
+    }
+  }else
+  {
+    // this is never reached ... :-(
+  }
+  //// we know other is not a leaf
+  //other->m_max_sqr_d = std::max(other->m_left->m_max_sqr_d,other->m_right->m_max_sqr_d);
+
+  return 0;
+#if false
+
+  // _Very_ conservative approximation of maximum squared distance between
+  // primitives inside this and other's bounding boxes
+  const auto & max_squared_distance = [](
+    const AABB<DerivedV,DIM> * A,
+    const AABB<Derivedother_V,DIM> * B)->Scalar
+  {
+    AlignedBox<Scalar,DIM> combo = A->m_box;
+    combo.extend(B->m_box);
+    return combo.diagonal().squaredNorm();
+  };
+
+  //// other base-case
+  //if(other->is_leaf())
+  //{
+  //  double sqr_d = sqrD(other->m_primitive);
+  //  int i = I(other->m_primitive);
+  //  RowVectorDIMS c = C.row(m_primitive);
+  //  RowVectorDIMS p = other_V.row(m_primitive);
+  //  leaf_squared_distance(V,Ele,p,sqr_d,i,c);
+  //  sqrD(other->m_primitive) = sqr_d;
+  //  I(other->m_primitive) = i;
+  //  C.row(m_primitive) = c;
+  //  return;
+  //}
+  std::vector<const AABB<DerivedV,DIM> * > this_list;
+  if(this->is_leaf())
+  {
+    this_list.push_back(this);
+  }else
+  {
+    assert(this->m_left);
+    this_list.push_back(this->m_left);
+    assert(this->m_right);
+    this_list.push_back(this->m_right);
+  }
+  std::vector<AABB<Derivedother_V,DIM> *> other_list;
+  if(other->is_leaf())
+  {
+    other_list.push_back(other);
+  }else
+  {
+    assert(other->m_left);
+    other_list.push_back(other->m_left);
+    assert(other->m_right);
+    other_list.push_back(other->m_right);
+  }
+
+  //const std::function<Scalar(
+  //  const AABB<Derivedother_V,DIM> * other)
+  //    > max_sqr_d = [&sqrD,&max_sqr_d](const AABB<Derivedother_V,DIM> * other)->Scalar
+  //  {
+  //    if(other->is_leaf())
+  //    {
+  //      return sqrD(other->m_primitive);
+  //    }else
+  //    {
+  //      return std::max(max_sqr_d(other->m_left),max_sqr_d(other->m_right));
+  //    }
+  //  };
+
+  //// Potentially recurse on all pairs, if minimum distance is less than running
+  //// bound
+  //Eigen::Matrix<Scalar,Eigen::Dynamic,1> other_max_sqr_d =
+  //  Eigen::Matrix<Scalar,Eigen::Dynamic,1>::Constant(other_list.size(),1,min_sqr_d);
+  for(size_t child = 0;child<other_list.size();child++)
+  {
+    auto other_tree = other_list[child];
+
+    Eigen::Matrix<Scalar,Eigen::Dynamic,1> this_max_sqr_d(this_list.size(),1);
+    for(size_t t = 0;t<this_list.size();t++)
+    {
+      const auto this_tree = this_list[t];
+      this_max_sqr_d(t) = max_squared_distance(this_tree,other_tree);
+    }
+    if(this_list.size() ==2 &&
+      ( this_max_sqr_d(0) > this_max_sqr_d(1))
+      )
+    {
+      std::swap(this_list[0],this_list[1]);
+      //std::swap(this_max_sqr_d(0),this_max_sqr_d(1));
+    }
+    const Scalar sqr_d = this_max_sqr_d.minCoeff();
+
+
+    for(size_t t = 0;t<this_list.size();t++)
+    {
+      const auto this_tree = this_list[t];
+
+      //const auto mm = max_sqr_d(other_tree);
+      //const Scalar mc = other_max_sqr_d(child);
+      //assert(mc == mm);
+      // Only look left/right in this_list if can possible decrease somebody's
+      // distance in this_tree.
+      const Scalar min_this_other = min_squared_distance(this_tree,other_tree); 
+      if(
+          min_this_other < sqr_d && 
+          min_this_other < other_tree->m_max_sqr_d)
+      {
+        //cout<<"before: "<<other_max_sqr_d(child)<<endl;
+        //other_max_sqr_d(child) = std::min(
+        //  other_max_sqr_d(child),
+        //  this_tree->squared_distance_helper(
+        //    V,Ele,other_tree,other_V,other_Ele,other_max_sqr_d(child),sqrD,I,C));
+        //cout<<"after: "<<other_max_sqr_d(child)<<endl;
+          this_tree->squared_distance_helper(
+            V,Ele,other_tree,other_V,other_Ele,0,sqrD,I,C);
+      }
+    }
+  }
+  //const Scalar ret = other_max_sqr_d.maxCoeff();
+  //const auto mm = max_sqr_d(other);
+  //assert(mm == ret);
+  //cout<<"non-leaf: "<<ret<<endl;
+  //return ret;
+  if(!other->is_leaf())
+  {
+    other->m_max_sqr_d = std::max(other->m_left->m_max_sqr_d,other->m_right->m_max_sqr_d);
+  }
+  return 0;
+#endif
+}
+
 template <typename DerivedV, int DIM>
 inline void igl::AABB<DerivedV,DIM>::leaf_squared_distance(
   const Eigen::PlainObjectBase<DerivedV> & V,
@@ -719,6 +1038,7 @@ inline void igl::AABB<DerivedV,DIM>::leaf_squared_distance(
   }
   const auto & point_point_squared_distance = [&](const RowVectorDIMS & s)
   {
+    cout<<"pp"<<endl;
     const Scalar sqr_d_s = (p-s).squaredNorm();
     set_min(p,sqr_d_s,m_primitive,s,sqr_d,i,c);
   };
@@ -779,7 +1099,9 @@ inline void igl::AABB<DerivedV,DIM>::set_min(
   RowVectorDIMS & c) const
 {
 #ifndef NDEBUG
-  const Scalar diff = fabs(sqr_d_candidate - (p-c_candidate).squaredNorm());
+  //std::cout<<matlab_format(c_candidate,"c_candidate")<<std::endl;
+  const Scalar pc_norm = (p-c_candidate).squaredNorm();
+  const Scalar diff = fabs(sqr_d_candidate - pc_norm);
   assert(diff<=1e-10 && "distance should match norm of difference");
 #endif
   if(sqr_d_candidate < sqr_d)