Print Page - Kd Tree

Title: Kd Tree
Post by: gile on April 01, 2013, 05:08:56 PM

Hi,

I'm trying to implement a .NET Kd Tree for AutoCAD Point3d with a NearestNeighbours method.
I tried my class with this (old but successfull) challenge (http://www.theswamp.org/index.php?topic=32874).
Using my Point3dTree is not very faster than the quite naive algorithm I posted in reply #31 (http://www.theswamp.org/index.php?topic=32874.msg383652#msg383652) with 10000 points (
notwithstanding 3 times faster with 100000 points).
Is this due to a bad implementation of the Kd Tree or to the fact this kind of data structure is not so good to solve this type of challenge ?

Code - C#: [Select]

using System;
using System.Collections.Generic;
using System.Linq;
using Autodesk.AutoCAD.Geometry;
 
namespace PointKdTree
{
    public class KdTreeNode<T>
    {
        public T Value { get; internal set; }
        public KdTreeNode<T> LeftChild { get; internal set; }
        public KdTreeNode<T> RightChild { get; internal set; }
        public int Depth { get; internal set; }
        public bool Processed { get; set; }
 
        public KdTreeNode(T value) { this.Value = value; }
    }
 
    public class Point3dTree
    {
        private int dimension;
        public KdTreeNode<Point3d> Root { get; private set; }
 
        public Point3dTree(IEnumerable<Point3d> points) : this(points, false) { }
 
        public Point3dTree(IEnumerable<Point3d> points, bool ignoreZ)
        {
            if (points == null)
                throw new ArgumentNullException("points");
            this.dimension = ignoreZ ? 2 : 3;
            Point3d[] pts = points.Distinct().ToArray();
            this.Root = Construct(pts, 0);
        }
 
        public List<Point3d> NearestNeighbours(KdTreeNode<Point3d> node, double radius)
        {
            if (node == null)
                throw new ArgumentNullException("node");
            List<Point3d> result = new List<Point3d>();
            GetNeighbours(node.Value, radius, this.Root.LeftChild, ref result);
            GetNeighbours(node.Value, radius, this.Root.RightChild, ref result);
            return result;
        }
 
        private void GetNeighbours(Point3d center, double radius, KdTreeNode<Point3d> node, ref List<Point3d> result)
        {
            if (node == null) return;
            Point3d pt = node.Value;
            if (!node.Processed && center.DistanceTo(pt) <= radius) 
            {
                result.Add(pt);
            }
            int d = node.Depth % this.dimension;
            double coordCen = center[d];
            double coordPt = pt[d];
            if (Math.Abs(coordCen - coordPt) > radius)
            {
                if (coordCen < coordPt)
                {
                    GetNeighbours(center, radius, node.LeftChild, ref result);
                }
                else
                {
                    GetNeighbours(center, radius, node.RightChild, ref result);
                }
            }
            else
            {
                GetNeighbours(center, radius, node.LeftChild, ref result);
                GetNeighbours(center, radius, node.RightChild, ref result);
            }
        }
 
        private KdTreeNode<Point3d> Construct(Point3d[] points, int depth)
        {
            int length = points.Length;
            if (length == 0) return null;
            int d = depth % this.dimension;
            Array.Sort(points, (p1, p2) => p1[d].CompareTo(p2[d]));
            int med = length / 2;
            KdTreeNode<Point3d> node = new KdTreeNode<Point3d>(points[med]);
            node.Depth = depth;
            node.LeftChild = Construct(points.Take(med).ToArray(), depth + 1);
            node.RightChild = Construct(points.Skip(med + 1).ToArray(), depth + 1);
            return node;
        }
    }
}

I also improve the speed for drawing lines by using the ObjectId.Open() method instead of using a transaction.

Code - C#: [Select]

using System.Collections.Generic;
using System.Linq;
using Autodesk.AutoCAD.ApplicationServices;
using Autodesk.AutoCAD.DatabaseServices;
using Autodesk.AutoCAD.EditorInput;
using Autodesk.AutoCAD.Geometry;
using Autodesk.AutoCAD.Runtime;
using AcAp = Autodesk.AutoCAD.ApplicationServices.Application;
 
[assembly: CommandClass(typeof(PointKdTree.CommandMethods))]
 
namespace PointKdTree
{
    public class CommandMethods
    {
        struct Point3dPair
        {
            public readonly Point3d Start;
            public readonly Point3d End;
 
            public Point3dPair(Point3d start, Point3d end)
            {
                this.Start = start;
                this.End = end;
            }
        }
 
        [CommandMethod("Connect")]
        public void Connect()
        {
            System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
            sw.Start();
            Document doc = AcAp.DocumentManager.MdiActiveDocument;
            Database db = doc.Database;
            Editor ed = doc.Editor;
            RXClass rxc = RXClass.GetClass(typeof(DBPoint));
            using (BlockTableRecord btr = (BlockTableRecord)db.CurrentSpaceId.Open(OpenMode.ForWrite))
            {
                var pts = btr.Cast<ObjectId>()
                    .Where(id => id.ObjectClass == rxc)
                    .Select(id =>
                    {
                        using (DBPoint pt = (DBPoint)id.Open(OpenMode.ForRead))
                        { return pt.Position; }
                    });
                Point3dTree tree = new Point3dTree(pts, true);
                List<Point3dPair> pairs = new List<Point3dPair>();
                ConnectPoints(tree, tree.Root, 70.0, ref pairs);
                foreach (Point3dPair pair in pairs)
                {
                    using (Line line = new Line(pair.Start, pair.End))
                    {
                        btr.AppendEntity(line);
                    }
                }
            }
            sw.Stop();
            ed.WriteMessage("\nElapsed milliseconds: {0}", sw.ElapsedMilliseconds);
        }
 
        private void ConnectPoints(Point3dTree tree, KdTreeNode<Point3d> node, double dist, ref List<Point3dPair> pointPairs)
        {
            if (node == null) return;
            node.Processed = true;
            Point3d center = node.Value;
            foreach (Point3d pt in tree.NearestNeighbours(node, dist))
            {
                pointPairs.Add(new Point3dPair(center, pt));
            }
            ConnectPoints(tree, node.LeftChild, dist, ref pointPairs);
            ConnectPoints(tree, node.RightChild, dist, ref pointPairs);
        }
    }
}

Title: Re: Kd Tree
Post by: TheMaster on April 01, 2013, 05:21:03 PM

Hi Gile. You could try using some execution profiling tools to identify bottlenecks, and I can't tell you much about the overall performance characteristics without some analysis, but just skimming your code, this sticks out:

Code - C#: [Select]

 
    node.LeftChild = Construct(points.Take(med).ToArray(), depth + 1); 
    node.RightChild = Construct(points.Skip(med + 1).ToArray(), depth + 1);

In this case, I would probably not use Linq, because Take/Skip(...).ToArray() can't produce a result in a single allocation, and will have to do it incrementally (starting with 4 elements, and doubling it each time more capacity is needed), which depending on how many elements there are, can be a major performance hit.

I would use Array.Copy() or Array.Constrained.Copy() instead.

And one more possible optimization:

Code - C#: [Select]

 
    Array.Sort(points, (p1, p2) => p1[d].CompareTo(p2[d]));

The above (from your Construct() method), appears to be redundant, since Construct() is recursive, and each call is sorting its argument, which for all but the outer-most call, should not be necessary.

Title: Re: Kd Tree
Post by: gile on April 01, 2013, 06:07:29 PM

Thanks Tony,

Quote

I would use Array.Copy() or Array.Constrained.Copy() instead.

That makes sense.

Quote

The above (from your Construct() method), appears to be redundant, since Construct() is recursive, and each call is sorting its argument, which for all but the outer-most call, should not be necessary.

I do not agree. At each call 'd' is changing according to the depth in tree and the tree dimension (2d or 3d):
first call: d = 0, the points are sorted by X,
second call: d = 1 => the points are sorted b Y
third call: d = 0 if dimension = 2 (ignore Z) => the points are sorted by X or d = 2 if dimension = 3 => the points are sorted by Z

Title: Re: Kd Tree
Post by: TheMaster on April 01, 2013, 08:03:21 PM

Quote from: gile on April 01, 2013, 06:07:29 PM

Thanks Tony,

Quote
I would use Array.Copy() or Array.Constrained.Copy() instead.
That makes sense.

Quote
The above (from your Construct() method), appears to be redundant, since Construct() is recursive, and each call is sorting its argument, which for all but the outer-most call, should not be necessary.
I do not agree. At each call 'd' is changing according to the depth in tree and the tree dimension (2d or 3d):
first call: d = 0, the points are sorted by X,
second call: d = 1 => the points are sorted b Y
third call: d = 0 if dimension = 2 (ignore Z) => the points are sorted by X or d = 2 if dimension = 3 => the points are sorted by Z

Hi Gile. Yes, I didn't look close enough, and was thinking 'b-tree', not 'kd-tree' :laugh:

Title: Re: Kd Tree
Post by: pkohut on April 02, 2013, 01:05:34 AM

Hi Gile,

Profiling will probably reveal the bottleneck to be in the Construct method, and the call to sort being a big contributor.

Code: [Select]

Array.Sort(points, (p1, p2) => p1[d].CompareTo(p2[d]));Change it to a lighter weight partial sort, like C++'s std::nth_element (http://stackoverflow.com/questions/2540602/does-c-sharp-have-a-stdnth-element-equivalent)

Here is a code snippet from http://www.theswamp.org/index.php?topic=32874.msg401826#msg401826, which is similar to your Construct method.

Code: [Select]

    void OptimizeKdTree(typename std::vector<NODE_POINT<T, DIM>>::iterator & i1,
        typename std::vector<NODE_POINT<T, DIM>>::iterator & i2,
        const size_t dir)
    {
        if(i1 == i2)
            return;

        // Create instance of the compare function and set with the KDTree direction
        NodeCompare<NODE_POINT<T, DIM>, DIM> compare(dir % DIM);

        // Find the halfway mark between the 2 iterators
        std::vector<NODE_POINT<T, DIM>>::iterator iMid = i1 + (i2 - i1) / 2;

        /*
        * rearrange so elements left of iMid are less than iMid.pos
        * and elements right of iMid are greater than iMid.pos
        */
        std::nth_element(i1, iMid, i2, compare);

        // Insert the iMid into the KDTree
        Insert(iMid->Data(), 0);

        // Continue until no more elements to insert
        if(iMid != i1)
            OptimizeKdTree(i1, iMid, dir + 1);
        if(++iMid != i2)
            OptimizeKdTree(iMid, i2, dir + 1);
    }

Title: Re: Kd Tree
Post by: gile on April 02, 2013, 01:32:40 AM

Thanks Paul.

That makes sense, the issue now is: am I able to build a quick select routine faster than Array.Sort() ?

Title: Re: Kd Tree
Post by: TheMaster on April 02, 2013, 06:35:00 AM

Quote from: gile on April 02, 2013, 01:32:40 AM

Thanks Paul.

That makes sense, the issue now is: am I able to build a quick select routine faster than Array.Sort() ?

Purely out of curiosity, I tried to parallelize your Construct() method, but only saw a minor improvement of around 20-30%.

The search can also be parallelized, but it requires major surgery (it needs to return results rather than add them to a single list, or you have to use a collection from System.Collections.Concurrent).

Also, you don't need to use 'ref' parameters to pass and use reference types, like List<T>. Use 'ref' only when you're going to actually change the value of the variable in the caller, rather than operate on an instance of a passed reference type. See my note in the code.

Code - C#: [Select]

 
using System;
using System.Collections.Generic;
using System.Linq;
using Autodesk.AutoCAD.ApplicationServices;
using Autodesk.AutoCAD.DatabaseServices;
using Autodesk.AutoCAD.EditorInput;
using Autodesk.AutoCAD.Geometry;
using Autodesk.AutoCAD.Runtime;
using AcAp = Autodesk.AutoCAD.ApplicationServices.Application;
using System.Diagnostics;
 
namespace PointKdTree
{
   public class KdTreeNode<T>
   {
      public T Value
      {
         get;
         internal set;
      }
      public KdTreeNode<T> LeftChild
      {
         get;
         internal set;
      }
      public KdTreeNode<T> RightChild
      {
         get;
         internal set;
      }
      public int Depth
      {
         get;
         internal set;
      }
      public bool Processed
      {
         get;
         set;
      }
 
      public KdTreeNode( T value )
      {
         this.Value = value;
      }
   }
 
   public class Point3dNode : KdTreeNode<Point3d>
   {
      public Point3dNode( Point3d value ) : base(value)
      {
      }
 
      public double this[int dim]
      {
         get
         {
            return this.Value[this.Depth % dim];
         }
      }
   }
 
   public class Point3dTree
   {
      private int dimension;
      public KdTreeNode<Point3d> Root
      {
         get;
         private set;
      }
 
      public Point3dTree( IEnumerable<Point3d> points ) : this( points, false )
      {
      }
 
      public Point3dTree( IEnumerable<Point3d> points, bool ignoreZ )
      {
         if( points == null )
            throw new ArgumentNullException( "points" );
         this.dimension = ignoreZ ? 2 : 3;
         Point3d[] pts = points.Distinct().ToArray();
         this.Root = Construct( pts, 0 );
      }
 
      public List<Point3d> NearestNeighbours( KdTreeNode<Point3d> node, double radius )
      {
         if( node == null )
            throw new ArgumentNullException( "node" );
         List<Point3d> result = new List<Point3d>();
         GetNeighbours( node.Value, radius, this.Root.LeftChild, result );
         GetNeighbours( node.Value, radius, this.Root.RightChild, result );
         return result;
      }
 
      private void GetNeighbours( Point3d center, double radius, KdTreeNode<Point3d> node, List<Point3d> result )
      {
         if( node == null ) return;
         Point3d pt = node.Value;
         if( !node.Processed && center.DistanceTo( pt ) <= radius )
         {
            result.Add( pt );
         }
         int d = node.Depth % this.dimension;
         double coordCen = center[d];
         double coordPt = pt[d];
         if( Math.Abs( coordCen - coordPt ) > radius )
         {
            if( coordCen < coordPt )
            {
               GetNeighbours( center, radius, node.LeftChild, result );
            }
            else
            {
               GetNeighbours( center, radius, node.RightChild, result );
            }
         }
         else
         {
            GetNeighbours( center, radius, node.LeftChild, result );
            GetNeighbours( center, radius, node.RightChild, result );
         }
      }
 
      internal static bool useParallel = false;
 
      private KdTreeNode<Point3d> Construct( Point3d[] points, int depth )
      {
         int length = points.Length;
         if( length == 0 ) return null;
         int d = depth % this.dimension;
         Array.Sort( points, ( p1, p2 ) => p1[d].CompareTo( p2[d] ) );
         int med = length / 2;
         KdTreeNode<Point3d> node = new KdTreeNode<Point3d>( points[med] );
         node.Depth = depth;
         var left = points.Take( med ).ToArray();
         var right = points.Skip( med + 1 ).ToArray();\
         
         /////////////////////////////////////////////////////////////
         /// Do construction in parallel up to 8 levels (it 
         /// makes little sense to parallelize any more than 
         /// that, unless you got dozens of CPUs):
         
         if( useParallel && depth < 9 ) 
         {
            System.Threading.Tasks.Parallel.Invoke(
               () => node.LeftChild = Construct( left, depth + 1 ),
               () => node.RightChild = Construct( right, depth + 1 ) 
            );
         }
         else
         {
            node.LeftChild = Construct( left, depth + 1 );
            node.RightChild = Construct( right, depth + 1 );
         }
         return node;
      }
 
   }
 
   public class CommandMethods
   {
      struct Point3dPair
      {
         public readonly Point3d Start;
         public readonly Point3d End;
 
         public Point3dPair( Point3d start, Point3d end )
         {
            this.Start = start;
            this.End = end;
         }
      }
 
      /// Toggle parallel execution on/off:
      
      [CommandMethod( "CONNECT_PARALLEL" )]
      public static void ConnectParallelSwitch()
      {
         Point3dTree.useParallel ^= true;
         Application.DocumentManager.MdiActiveDocument.Editor.WriteMessage(
            "\nConnect using Parallel = {0}", Point3dTree.useParallel );
      }
 
      [CommandMethod( "Connect" )]
      public static void Connect()
      {
         Stopwatch sw = new System.Diagnostics.Stopwatch();
         Stopwatch sw2 = null;
         sw.Start();
         Document doc = AcAp.DocumentManager.MdiActiveDocument;
         Database db = doc.Database;
         Editor ed = doc.Editor;
         if( Point3dTree.useParallel )
            ed.WriteMessage( "\n(Using Parallel Execution)\n" );
         else
            ed.WriteMessage( "\n(Using Sequential Execution)\n" );
         RXClass rxc = RXClass.GetClass( typeof( DBPoint ) );
         using( BlockTableRecord btr = (BlockTableRecord) db.CurrentSpaceId.Open( OpenMode.ForWrite ) )
         {
            var pts = btr.Cast<ObjectId>()
                .Where( id => id.ObjectClass == rxc )
                .Select( id =>
                {
                   using( DBPoint pt = (DBPoint) id.Open( OpenMode.ForRead ) )
                   {
                      return pt.Position;
                   }
                } );
 
            sw2 = Stopwatch.StartNew();
            Point3dTree tree = new Point3dTree( pts, true );
            List<Point3dPair> pairs = new List<Point3dPair>();
            ConnectPoints( tree, tree.Root, 70.0, pairs );
            sw2.Stop();
            foreach( Point3dPair pair in pairs )
            {
               using( Line line = new Line( pair.Start, pair.End ) )
               {
                  btr.AppendEntity( line );
               }
            }
         }
         sw.Stop();
         ed.WriteMessage( "\nElapsed milliseconds: {0}", sw.ElapsedMilliseconds );
         ed.WriteMessage( "\nNearestNeighbor time: {0}", sw2.ElapsedMilliseconds );
      }
 
 
      /// Don't use 'ref' parameters for reference types, unless
      /// your intention is to modify the value of the variable
      /// in the caller. If you are only accessing the reference
      /// type (e.g., adding items to a List<T>), you don't need
      /// 'ref', which has some performance overhead.
      
      private static void ConnectPoints( Point3dTree tree, KdTreeNode<Point3d> node, double dist, List<Point3dPair> pointPairs )
      {
         if( node == null ) return;
         node.Processed = true;
         Point3d center = node.Value;
         foreach( Point3d pt in tree.NearestNeighbours( node, dist ) )
         {
            pointPairs.Add( new Point3dPair( center, pt ) );
         }
         ConnectPoints( tree, node.LeftChild, dist, pointPairs );
         ConnectPoints( tree, node.RightChild, dist, pointPairs );
      }
   }
}
 
 

Quick test with 50K points:

Code - Text: [Select]

 
Command: CONNECT
(Using Sequential Execution)
Elapsed milliseconds: 875
NearestNeighbor time: 858
 
Command: CONNECT_PARALLEL
Connect using Parallel = True
 
Command: CONNECT
(Using Parallel Execution)
Elapsed milliseconds: 604
NearestNeighbor time: 602
 
 

Title: Re: Kd Tree
Post by: pkohut on April 02, 2013, 10:00:17 AM

Nice speed bump and coding Tony.

FWIW, Array.Sort is O(n log n), while nth_element is O(n).

Title: Re: Kd Tree
Post by: TheMaster on April 02, 2013, 06:29:50 PM

Quote from: pkohut on April 02, 2013, 10:00:17 AM

Nice speed bump and coding Tony.

FWIW, Array.Sort is O(n log n), while nth_element is O(n).

Hi Paul, and thanks.

I posted what I thought was a notable improvement to Gile's original code, but as it turned out, it was Linq's deferred execution that was corrupting the timings, so I retracted it.

I tried using the QuickSelect algorithm in place of Array.Sort(), but that didn't seem to help much, and I think that's mainly because the majority of the execution time of Gile's code is consumed by getting the coordinate data from the DBPoints, drawing the Lines, and most-notably, by a call to Enumerable.Distinct() to remove all duplicates. Only a small fraction of it is consumed by building the tree, and I don't know if that's as important as searching it.

Below is Gile's code with a few ideas on how he might improve overall performance implemented, which has various tweaks, such as avoiding the use of ToArray() when the quantity is known, avoiding use of Enumerable.Distinct() (which could be dealt with in the process of building the tree), and storing the entire dataset of points in array, and operating on int[] arrays of indices into the array, rather than directly on the Point3ds themselves. So, the Construct() method was refactored to pass arrays of int[], that contain indices of elements in the Point3d[] array, which is never duplicated.

Code - C#: [Select]

 
using System;
using System.Collections.Generic;
using System.Linq;
using Autodesk.AutoCAD.ApplicationServices;
using Autodesk.AutoCAD.DatabaseServices;
using Autodesk.AutoCAD.EditorInput;
using Autodesk.AutoCAD.Geometry;
using Autodesk.AutoCAD.Runtime;
using AcAp = Autodesk.AutoCAD.ApplicationServices.Application;
using System.Diagnostics;
 
namespace PointKdTree
{
   public class KdTreeNode<T>
   {
      public T Value
      {
         get;
         internal set;
      }
      public KdTreeNode<T> LeftChild
      {
         get;
         internal set;
      }
      public KdTreeNode<T> RightChild
      {
         get;
         internal set;
      }
      public int Depth
      {
         get;
         internal set;
      }
      public bool Processed
      {
         get;
         set;
      }
 
      public KdTreeNode( T value )
      {
         this.Value = value;
      }
   }
 
   public class Point3dNode : KdTreeNode<Point3d>
   {
      public Point3dNode( Point3d value ) : base(value)
      {
      }
 
      public double this[int dim]
      {
         get
         {
            return this.Value[this.Depth % dim];
         }
      }
   }
 
   public class Point3dTree
   {
      private int dimension;
 
      public KdTreeNode<Point3d> Root
      {
         get;
         private set;
      }
 
      public Point3dTree( IEnumerable<Point3d> points )
         : this( points, false )
      {
      }
 
      /// <summary>
      /// Eliminated call to Enumerable.Distinct() which 
      /// was consuming most of the time required here:
      /// </summary>
      public Point3dTree( IEnumerable<Point3d> points, bool ignoreZ )
      {
         if( points == null )
            throw new ArgumentNullException( "points" );
         this.dimension = ignoreZ ? 2 : 3;
         var data = points as Point3d[] ?? points.ToArray(); // 
         int[] indices = new int[data.Length];
         for( int i = 0; i < data.Length; i++ )
            indices[i] = i;
         this.Root = Construct( data, indices, 0 );
      }
 
      public List<Point3d> NearestNeighbours( KdTreeNode<Point3d> node, double radius )
      {
         if( node == null )
            throw new ArgumentNullException( "node" );
         List<Point3d> result = new List<Point3d>();
         GetNeighbours( node.Value, radius, this.Root.LeftChild, result );
         GetNeighbours( node.Value, radius, this.Root.RightChild, result );
         return result;
      }
 
      private void GetNeighbours( Point3d center, double radius, KdTreeNode<Point3d> node, List<Point3d> result )
      {
         if( node == null ) return;
         Point3d pt = node.Value;
         if( !node.Processed && center.DistanceTo( pt ) <= radius )
         {
            result.Add( pt );
         }
         int d = node.Depth % this.dimension;
         double coordCen = center[d];
         double coordPt = pt[d];
         if( Math.Abs( coordCen - coordPt ) > radius )
         {
            if( coordCen < coordPt )
            {
               GetNeighbours( center, radius, node.LeftChild, result );
            }
            else
            {
               GetNeighbours( center, radius, node.RightChild, result );
            }
         }
         else
         {
            GetNeighbours( center, radius, node.LeftChild, result );
            GetNeighbours( center, radius, node.RightChild, result );
         }
      }
 
      internal static bool useParallel = false;
 
      /// <summary>
      /// Refactored to pass entire dataset of Point3d in data, 
      /// and to manipulate int[] arrays of indices.
      /// </summary>
      
      private KdTreeNode<Point3d> Construct( Point3d[] data, int[] points, int depth )
      {
         int length = points.Length;
         if( length == 0 ) return null;
         int d = depth % this.dimension;
         int l1 = length / 2;
         int l2 = length - l1 - 1;
         var items = new KeyValuePair<double, int>[length];
         for( int i = 0; i < length; i++ )
            items[i] = new KeyValuePair<double, int>( data[points[i]][d], i );
         int pivot = OrderedSelect( items, d );
         var left = new int[l1];
         for( int i = 0; i < l1; i++ )
            left[i] = items[i].Value;
         var right = new int[l2];
         ++l1;
         for( int i = 0; i < l2; i++ )
            right[i] = items[i+l1].Value;
         KdTreeNode<Point3d> node = new KdTreeNode<Point3d>( data[pivot] );
         KdTreeNode<Point3d> leftchild = null;
         KdTreeNode<Point3d> rightchild = null;
         node.Depth = depth;
         if( useParallel && depth < 4 ) // parallelize up to 4 levels deep
         {
            System.Threading.Tasks.Parallel.Invoke(
               () => leftchild = Construct( data, left, depth + 1 ),
               () => rightchild = Construct( data, right, depth + 1 ) );
         }
         else
         {
            leftchild = Construct( data, left, depth + 1 );
            rightchild = Construct( data, right, depth + 1 );
         }
         node.LeftChild = leftchild;
         node.RightChild = rightchild;
         return node;
      }
 
      // Quick select algorithm operating on KeyValuePair<double,int>
      // where the key is the ordinate/dimension and the value is the 
      // index of the associated Point3d in the dataset:
 
      public static int OrderedSelect( KeyValuePair<double, int>[] points, int dim )
      {
         int k = points.Length / 2;
         if( points == null || points.Length <= k )
            throw new ArgumentException( "array" );
 
         int from = 0, to = points.Length - 1;
         while( from < to )
         {
            int r = from, w = to;
            double pivot = points[( r + w ) / 2].Key;
            while( r < w )
            {
               if( points[r].Key >= pivot )
               {
                  KeyValuePair<double, int> tmp = points[w];
                  points[w] = points[r];
                  points[r] = tmp;
                  w--;
               }
               else
               {
                  r++;
               }
            }
            if( points[r].Key > pivot )
            {
               r--;
            }
            if( k <= r )
            {
               to = r;
            }
            else
            {
               from = r + 1;
            }
         }
         return points[k].Value;
      }
 
   }
 
   public class CommandMethods
   {
      struct Point3dPair
      {
         public readonly Point3d Start;
         public readonly Point3d End;
 
         public Point3dPair( Point3d start, Point3d end )
         {
            this.Start = start;
            this.End = end;
         }
      }
 
      [CommandMethod( "CONNECT_PARALLEL" )]
      public static void ConnectParallelSwitch()
      {
         Point3dTree.useParallel ^= true;
         Application.DocumentManager.MdiActiveDocument.Editor.WriteMessage(
            "\nConnect using Parallel = {0}", Point3dTree.useParallel );
      }
 
      [CommandMethod( "Connect" )]
      public static void Connect()
      {
         Stopwatch sw = new System.Diagnostics.Stopwatch();
         Stopwatch sw2 = null;
         Stopwatch sw3 = null;
         long distinctTime = 0L;
         sw.Start();
         Document doc = AcAp.DocumentManager.MdiActiveDocument;
         Database db = doc.Database;
         Editor ed = doc.Editor;
         if( Point3dTree.useParallel )
            ed.WriteMessage( "\n(Using Parallel Execution)\n" );
         else
            ed.WriteMessage( "\n(Using Sequential Execution)\n" );
         RXClass rxc = RXClass.GetClass( typeof( DBPoint ) );
         int cnt = 0;
 
         using( BlockTableRecord btr = (BlockTableRecord) db.CurrentSpaceId.Open( OpenMode.ForWrite ) )
         {
            var dbpoints = btr.Cast<ObjectId>().Where( id => id.ObjectClass == rxc );
            ObjectId[] ids = dbpoints.ToArray();
            int len = ids.Length;
            Point3d[] array = new Point3d[len];
            for( int i = 0; i < len; i++ )
            {
               using( DBPoint pt = (DBPoint) ids[i].Open( OpenMode.ForRead ) )
               {
                  array[i] = pt.Position;
               }
            }
            sw3 = Stopwatch.StartNew();
            Point3dTree tree = new Point3dTree( array, true );
            sw3.Stop();
            sw2 = Stopwatch.StartNew();
            List<Point3dPair> pairs = new List<Point3dPair>( cnt / 2 );
            ConnectPoints( tree, tree.Root, 70.0, pairs );
            sw2.Stop();
            foreach( Point3dPair pair in pairs )
            {
               using( Line line = new Line( pair.Start, pair.End ) )
               {
                  btr.AppendEntity( line );
               }
            }
         }
         sw.Stop();
         ed.WriteMessage( "\nTotal time:                {0,5}", sw.ElapsedMilliseconds );
         ed.WriteMessage( "\nConstruct time:            {0,5}", sw3.ElapsedMilliseconds );
         ed.WriteMessage( "\nNearestNeighbor time:      {0,5}", sw2.ElapsedMilliseconds );
      }
 
      private static void ConnectPoints( Point3dTree tree, KdTreeNode<Point3d> node, double dist, List<Point3dPair> pointPairs )
      {
         if( node == null ) return;
         node.Processed = true;
         Point3d center = node.Value;
         foreach( Point3d pt in tree.NearestNeighbours( node, dist ) )
         {
            pointPairs.Add( new Point3dPair( center, pt ) );
         }
         ConnectPoints( tree, node.LeftChild, dist, pointPairs );
         ConnectPoints( tree, node.RightChild, dist, pointPairs );
      }
   }
}
 
 

Title: Re: Kd Tree
Post by: gile on April 03, 2013, 07:00:58 AM

Hi,

First of all, thanks to Tony and Paul for their interest and advice.

Parallelization
It was part of my goals. I tried to develop (in parallel) the same thing in F# in order to use these features but I still encountering some problems.
I tried to parallelize the search part using a ConcurrentBag, it seems to have much overhead so that it slows down significantly the process.

Unnecessary use of ref
Thank you for pointing that. The back and forth between C# and F#, imperative and functional programming, mutable and immutable data have probably helped to make me miss it.

Array.Copy vs Take / Skip
I changed this as Tony's advice.

Array.Sort vs QuickSelect
I tried to implement some other QuickSelect and nth-smallest algorithms. Each time Array.Sort () is (much) faster.
Tony, your OrderedSelect() function is not working here.

Distinct ()
I did not notice any significant speed difference with or without.

Here're somme results with my current but not last version (changes made for point 1, 2 and 3, keeping using Array.Sort() and Distinct()).

100K points

Code: [Select]

Connect using Parallel = False
Get points:      388
Build tree:      718
Connect points:  659
Draw lines:      869
Total:          2634

Connect using Parallel = True
Get points:      374
Build tree:      287
Connect points:  669
Draw lines:      883
Total:          2213

Title: Re: Kd Tree
Post by: TheMaster on April 03, 2013, 10:09:32 AM

Quote from: gile on April 03, 2013, 07:00:58 AM

Tony, your OrderedSelect() function is not working here.

Hi Gile, yes, there's a bug in the code I posted.

Code - C#: [Select]

 
     for( int i = 0; i < length; i++ )
          items[i] = new KeyValuePair<double, int>( data[points[i]][d], i );
 
// Should be:
 
     for( int i = 0; i < length; i++ )
     {   
          int index = points[i];
          items[i] = new KeyValuePair<double, int>( data[index][d], index );
     }
 
 

I believe, with that it should work, but I haven't seen much of a difference in the result.

FWIW, the C++ kdtree code I've used in the past uses a bubble-sort, which is probably also not optimal.

[/quote]

Title: Re: Kd Tree
Post by: TheMaster on April 03, 2013, 10:22:47 AM

Quote from: gile on April 03, 2013, 07:00:58 AM

Here're somme results with my current but not last version (changes made for point 1, 2 and 3, keeping using Array.Sort() and Distinct()).

100K points
Code: [Select]
Connect using Parallel = False Get points: 388 Build tree: 718 Connect points: 659 Draw lines: 869 Total: 2634 Connect using Parallel = True Get points: 374 Build tree: 287 Connect points: 669 Draw lines: 883 Total: 2213

Hi Gile. In your original code, it wasn't possible to break out the time required to build the tree, because the Linq code that opens each DBPoint and gets its position wasn't executing until the constructor of your tree class called Distinct().ToArray() on the IEnumerable<Point3d> argument.

You might notice that I rewrote that part, so that I could measure the tree construction time, without also including the time required to iterate the BlockTableRecord, open each DBPoint and get its Position.

So, I'm not entirely sure what to make of your numbers because I don't know if you're using the same code from the above post, or code based on your original post, but because of the Linq deferred execution, the times will be very different.

Title: Re: Kd Tree
Post by: gile on April 03, 2013, 11:02:02 AM

Quote

So, I'm not entirely sure what to make of your numbers because I don't know if you're using the same code from the above post, or code based on your original post, but because of the Linq deferred execution, the times will be very different.

I changed the code the same way you did: build a Point3d array (Get points) and then corstruvt the tree (Build tree).

Title: Re: Kd Tree
Post by: TheMaster on April 03, 2013, 12:39:09 PM

Quote from: gile on April 03, 2013, 07:00:58 AM

Array.Sort vs QuickSelect
I tried to implement some other QuickSelect and nth-smallest algorithms. Each time Array.Sort () is (much) faster. Tony, your OrderedSelect() function is not working here.

Gile, after applying the bug fix I showed in my other reply, it seems to be working, and I wanted to be sure, so I wrote this quick test, and it also confirms what Paul has suggested (much faster than Array.Sort), but of course, since the time to build the tree is a very small fraction of your overall time, it isn't significant to that, but I would still use it rather than Array.Sort anyways.

Code - C#: [Select]

 
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Autodesk.AutoCAD.Runtime;
using Autodesk.AutoCAD.EditorInput;
using System.Diagnostics;
using Autodesk.AutoCAD.ApplicationServices;
 
namespace System
{
 
   public static class QuickSelectExtensions
   {
      /// <summary>
      /// Quick Select algorithm generic implementation
      ///
      /// Rearranges the given array up to the k'th element 
      /// (the median element) so that all values to the left
      /// are < the median element. Returns the median element.
      /// 
      /// </summary>
      
      public static T QuickSelectMedian<T>( this T[] items, Comparison<T> compare )
      {
         if( items == null )
            throw new ArgumentException( "items" );
         int k = items.Length / 2;
         int from = 0;
         int to = items.Length - 1;
         while( from < to )
         {
            int r = from;
            int w = to;
            T current = items[( r + w ) / 2];
            while( r < w )
            {
               if( compare( items[r], current ) > -1 )
               {
                  var tmp = items[w];
                  items[w] = items[r];
                  items[r] = tmp;
                  w--;
               }
               else
               {
                  r++;
               }
            }
            if( compare( items[r], current ) > 0 )
            {
               r--;
            }
            if( k <= r )
            {
               to = r;
            }
            else
            {
               from = r + 1;
            }
         }
         return items[k];
      }
   }
 
   public static class QSelectVsQuickSortCommands
   {
      /// <summary>
      /// 
      /// Compare QuickSort (Array.Sort), with QuickSelectMedian()
      /// on a random array of double[]:
      /// 
      /// </summary>
      
      [CommandMethod( "QSELVSQSORT" )]
      public static void QuickSelectVersesQuickSort()
      {
         Editor ed = Application.DocumentManager.MdiActiveDocument.Editor;
 
#if( DEBUG )
         ed.WriteMessage( "\nTesting should NOT be done in DEBUG builds 8)");
         return;
#endif
         int count = 1000000;
         Random rnd = new Random();
         double[] array = new double[count];
         for( int i = 0; i < count; i++ )
            array[i] = rnd.NextDouble();
         double[] array2 = new double[count];
         Array.Copy( array, array2, count );
         Stopwatch sw = Stopwatch.StartNew();
         Array.Sort( array, ( a, b ) => a.CompareTo( b ) );
         sw.Stop();
         Stopwatch sw2 = Stopwatch.StartNew();
         double med = array2.QuickSelectMedian(
            Comparer<double>.Default.Compare );
         sw2.Stop();
         ed.WriteMessage( "\nArray.Sort():        {0,6}",
            sw.ElapsedMilliseconds );
         ed.WriteMessage( "\nQuickSelectMedian(): {0,6}",
            sw2.ElapsedMilliseconds );
      }
 
      /// <summary>
      ///  Verify if QuickSelectMedian() is working:
      ///  
      ///  After returning, all values in the array
      ///  whose indices are below the median value
      ///  must be less than the median value. The
      ///  median value is the value at [length/2]
      ///  
      /// </summary>
      [CommandMethod( "QSELTEST" )]
      public static void Qselect()
      {
         int count = 25;
         Random rnd = new Random();
         double[] array = new double[count];
         for( int i = 0; i < count; i++ )
            array[i] = 100 * rnd.NextDouble();
         WriteLine( "\narray Before QuickSelectMedian():\n" );
         Dump( array );
         double med = array.QuickSelectMedian(
            Comparer<double>.Default.Compare );
         WriteLine( "\nQuickSelectMedian(): [{0}] = {1}\n",
            array.Length / 2, med );
         WriteLine( "\narray After QuickSelectMedian():\n" );
         Dump( array );
      }
 
      public static void WriteLine( string msg, params object[] args )
      {
         Application.DocumentManager.MdiActiveDocument.Editor.WriteMessage(
            "\n" + msg, args );
      }
 
      static void Dump( double[] array )
      {
         int cnt = array.Length;
         for( int i = 0; i < cnt; i++ )
         {
            WriteLine( "array[{0}] = {1:f4}", i, array[i] );
         }
      }
   }
}
 
 

Code - Text: [Select]

 
Command: QSELTEST
 
array Before QuickSelectMedian():
 
array[0] = 99.9556
array[1] = 87.1161
array[2] = 65.3429
array[3] = 95.0263
array[4] = 5.5207
array[5] = 38.1086
array[6] = 82.9302
array[7] = 83.4299
array[8] = 34.1567
array[9] = 46.9312
array[10] = 24.4187
array[11] = 55.2761
array[12] = 9.0754
array[13] = 92.8103
array[14] = 35.5625
array[15] = 88.6875
array[16] = 7.8821
array[17] = 10.9855
array[18] = 69.7092
array[19] = 17.6414
array[20] = 82.9813
array[21] = 12.4893
array[22] = 2.8624
array[23] = 21.3571
array[24] = 74.9873
 
QuickSelectMedian(): [12] = 46.9311974229902
 
array After QuickSelectMedian():
 
array[0] = 2.8624
array[1] = 7.8821
array[2] = 5.5207
array[3] = 21.3571
array[4] = 12.4893
array[5] = 17.6414
array[6] = 10.9855
array[7] = 34.1567
array[8] = 9.0754
array[9] = 24.4187
array[10] = 35.5625
array[11] = 38.1086
array[12] = 46.9312
array[13] = 55.2761
array[14] = 65.3429
array[15] = 69.7092
array[16] = 82.9302
array[17] = 82.9813
array[18] = 74.9873
array[19] = 87.1161
array[20] = 83.4299
array[21] = 88.6875
array[22] = 99.9556
array[23] = 95.0263
array[24] = 92.8103
 
 
 
 
Command: QSELVSQSORT
 
Array.Sort():           379
QuickSelectMedian():     47
 
Command:
 
 

Title: Re: Kd Tree
Post by: gile on April 03, 2013, 02:16:00 PM

Thanks again, Tony.
I'll give a look and try to understand why my QuickSelect was so slow.

Title: Re: Kd Tree
Post by: gile on April 03, 2013, 06:05:14 PM

thanks again Tony, I get some improvement using your QuickSelectMedian<T>() method.

Here were I am:

Benchmarck (Release mode for 100K points)

Code: [Select]

Connect using Parallel = False
Get points:      236
Build tree:      203
Connect points:  265
Draw lines:      722
Total:          1426

Connect using Parallel = True
Get points:      244
Build tree:      105
Connect points:  282
Draw lines:      721
Total:          1352

The Point3dTree class.
I added a NearestNeighbour() method to find the nearest point of a given point.

Code - C#: [Select]

using System;
using System.Collections.Concurrent;
using System.Collections.Generic;
using System.Linq;
using Autodesk.AutoCAD.Geometry;
 
namespace PointKdTree
{
    public class KdTreeNode<T>
    {
        public KdTreeNode(T value) { this.Value = value; }
 
        public T Value { get; internal set; }
        public KdTreeNode<T> LeftChild { get; internal set; }
        public KdTreeNode<T> RightChild { get; internal set; }
        public int Depth { get; internal set; }
        public bool Processed { get; set; }
    }
 
    public class Point3dTree
    {
        private int dimension;
        internal static bool useParallel = false;
        public KdTreeNode<Point3d> Root { get; private set; }
 
        public Point3dTree(IEnumerable<Point3d> points) : this(points, false) { }
 
        public Point3dTree(IEnumerable<Point3d> points, bool ignoreZ)
        {
            if (points == null)
                throw new ArgumentNullException("points");
            this.dimension = ignoreZ ? 2 : 3; 
            Point3d[] pts = points.Distinct().ToArray();
            this.Root = Construct(pts, 0);
        }
 
        public List<Point3d> NearestNeighbours(KdTreeNode<Point3d> node, double radius)
        {
            if (node == null)
                throw new ArgumentNullException("node");
            List<Point3d> result = new List<Point3d>();
            NearestNeighbours(node.Value, radius, this.Root.LeftChild, result);
            NearestNeighbours(node.Value, radius, this.Root.RightChild, result);
            return result;
        }
 
        private void NearestNeighbours(Point3d center, double radius, KdTreeNode<Point3d> node, List<Point3d> result)
        {
            if (node == null) return;
            Point3d pt = node.Value;
            if (!node.Processed && center.DistanceTo(pt) <= radius)
            {
                result.Add(pt);
            }
            int d = node.Depth % this.dimension;
            double coordCen = center[d];
            double coordPt = pt[d];
            if (Math.Abs(coordCen - coordPt) > radius)
            {
                if (coordCen < coordPt)
                {
                    NearestNeighbours(center, radius, node.LeftChild, result);
                }
                else
                {
                    NearestNeighbours(center, radius, node.RightChild, result);
                }
            }
            else
            {
                NearestNeighbours(center, radius, node.LeftChild, result);
                NearestNeighbours(center, radius, node.RightChild, result);
            }
        }
 
        public Point3d NearestNeighbour(Point3d location)
        {
            return NearestNeighbour(location, this.Root, this.Root.Value, double.MaxValue);
        }
 
        private Point3d NearestNeighbour(Point3d location, KdTreeNode<Point3d> node, Point3d currentBest, double bestDistance)
        {
            if (node == null)
                return currentBest;
            int dim = node.Depth % this.dimension;
            Point3d nodeLocation = node.Value;
            double distance = location.DistanceTo(nodeLocation);
            if (distance >= 0.0 && distance < bestDistance)
            {
                currentBest = nodeLocation;
                bestDistance = distance;
            }
            bool isLeftNearer = location[dim] < nodeLocation[dim];
            KdTreeNode<Point3d> nearestChild = isLeftNearer ? node.LeftChild : node.RightChild;
            if (nearestChild != null)
            {
                currentBest = NearestNeighbour(location, nearestChild, currentBest, bestDistance);
                bestDistance = currentBest.DistanceTo(location);
            }
            if (bestDistance > Math.Abs(location[dim] - nodeLocation[dim]))
            {
                KdTreeNode<Point3d> farestChild = isLeftNearer ? node.RightChild : node.LeftChild;
                if (farestChild != null)
                {
                    currentBest = NearestNeighbour(location, farestChild, currentBest, bestDistance);
                    bestDistance = currentBest.DistanceTo(location);
                }
            }
            return currentBest;
        }
 
        private KdTreeNode<Point3d> Construct(Point3d[] points, int depth)
        {
            int length = points.Length;
            if (length == 0) return null;
            int d = depth % this.dimension;
            Point3d median = QuickSelectMedian(points, (p1, p2) => p1[d].CompareTo(p2[d]));
            KdTreeNode<Point3d> node = new KdTreeNode<Point3d>(median);
            node.Depth = depth;
            int mid = length / 2;
            int rlen = length - mid - 1;
            Point3d[] left = new Point3d[mid];
            Point3d[] right = new Point3d[rlen];
            Array.Copy(points, 0, left, 0, mid);
            Array.Copy(points, mid + 1, right, 0, rlen);
            if (useParallel && depth < 4)
            {
                System.Threading.Tasks.Parallel.Invoke(
                   () => node.LeftChild = Construct(left, depth + 1),
                   () => node.RightChild = Construct(right, depth + 1)
                );
            }
            else
            {
                node.LeftChild = Construct(left, depth + 1);
                node.RightChild = Construct(right, depth + 1);
            }
            return node;
        }
 
        // From Tony Tanzillo 
        // http://www.theswamp.org/index.php?topic=44312.msg495808#msg495808
        private T QuickSelectMedian<T>(T[] items, Comparison<T> compare)
        {
            int l = items.Length;
            int k = l / 2;
            if (items == null || items.Length == 0)
                throw new ArgumentException("array");
            int from = 0;
            int to = l - 1;
            while (from < to)
            {
                int r = from;
                int w = to;
                T current = items[(r + w) / 2];
                while (r < w)
                {
                    if (compare(items[r], current) > -1)
                    {
                        var tmp = items[w];
                        items[w] = items[r];
                        items[r] = tmp;
                        w--;
                    }
                    else
                    {
                        r++;
                    }
                }
                if (compare(items[r], current) > 0)
                {
                    r--;
                }
                if (k <= r)
                {
                    to = r;
                }
                else
                {
                    from = r + 1;
                }
            }
            return items[k];
        }
    }
}
 

The command used for the tests

Code - C#: [Select]

// (C) Copyright 2012 by Gilles Chanteau 
//
using System.Collections.Generic;
using System.Collections.Concurrent;
using System.Linq;
using Autodesk.AutoCAD.ApplicationServices;
using Autodesk.AutoCAD.DatabaseServices;
using Autodesk.AutoCAD.EditorInput;
using Autodesk.AutoCAD.Geometry;
using Autodesk.AutoCAD.Runtime;
using AcAp = Autodesk.AutoCAD.ApplicationServices.Application;
using System;
 
[assembly: CommandClass(typeof(PointKdTree.CommandMethods))]
 
namespace PointKdTree
{
    public class CommandMethods
    {
        struct Point3dPair
        {
            public readonly Point3d Start;
            public readonly Point3d End;
 
            public Point3dPair(Point3d start, Point3d end)
            {
                this.Start = start;
                this.End = end;
            }
        }
 
        // From Tony Tanzillo
        [CommandMethod("CONNECT_PARALLEL")]
        public static void ConnectParallelSwitch()
        {
            Point3dTree.useParallel ^= true;
            Application.DocumentManager.MdiActiveDocument.Editor.WriteMessage(
               "\nConnect using Parallel = {0}", Point3dTree.useParallel);
        }
 
        [CommandMethod("CONNECT")]
        public void Connect()
        {
            System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
            sw.Start();
            Document doc = AcAp.DocumentManager.MdiActiveDocument;
            Database db = doc.Database;
            Editor ed = doc.Editor;
            RXClass rxc = RXClass.GetClass(typeof(DBPoint));
            using (BlockTableRecord btr = (BlockTableRecord)db.CurrentSpaceId.Open(OpenMode.ForWrite))
            {
                long t0 = sw.ElapsedMilliseconds;
                ObjectId[] ids = btr.Cast<ObjectId>().Where(id => id.ObjectClass == rxc).ToArray();
                int len = ids.Length;
                Point3d[] pts = new Point3d[len];
                for (int i = 0; i < len; i++)
                {
                    using (DBPoint pt = (DBPoint)ids[i].Open(OpenMode.ForRead))
                    {
                        pts[i] = pt.Position;
                    }
                }
                long t1 = sw.ElapsedMilliseconds;
                Point3dTree tree = new Point3dTree(pts, true);
                long t2 = sw.ElapsedMilliseconds;
                List<Point3dPair> pairs = new List<Point3dPair>();
                ConnectPoints(tree, tree.Root, 70.0, pairs);
                long t3 = sw.ElapsedMilliseconds;
                foreach (Point3dPair pair in pairs)
                {
                    using (Line line = new Line(pair.Start, pair.End))
                    {
                        btr.AppendEntity(line);
                    }
                }
                sw.Stop();
                long t4 = sw.ElapsedMilliseconds;
                ed.WriteMessage("\nConnect using Parallel = {0}", Point3dTree.useParallel);
                ed.WriteMessage(
                    "\nGet points:{0,9}\nBuild tree:{1,9}\nConnect points:{2,5}\nDraw lines:{3,9}\nTotal:{4,14}",
                    t1 - t0, t2 - t1, t3 - t2, t4 - t3, t4);
            }
        }
 
        private void ConnectPoints(Point3dTree tree, KdTreeNode<Point3d> node, double dist, List<Point3dPair> pointPairs)
        {
            if (node == null) return;
            node.Processed = true;
            Point3d center = node.Value;
            foreach (Point3d pt in tree.NearestNeighbours(node, dist))
            {
                pointPairs.Add(new Point3dPair(center, pt));
            }
            ConnectPoints(tree, node.LeftChild, dist, pointPairs);
            ConnectPoints(tree, node.RightChild, dist, pointPairs);
        }
    }
}
 

Title: Re: Kd Tree
Post by: TheMaster on April 03, 2013, 07:42:37 PM

Hi Gile - Great job.

Unless you're running on a box with 8 physical CPU cores, change the test to run in parallel only if depth < 3 for a quad-core system. You could also check the number of physical CPU cores at runtime and adjust that (for example, on a 2-core system, use Parallel.Invoke() only if depth < 2).

Title: Re: Kd Tree
Post by: TheMaster on April 07, 2013, 11:25:11 AM

Quote from: TT on April 03, 2013, 07:42:37 PM

Hi Gile - Great job.

Unless you're running on a box with 8 physical CPU cores, change the test to run in parallel only if depth < 3 for a quad-core system. You could also check the number of physical CPU cores at runtime and adjust that (for example, on a 2-core system, use Parallel.Invoke() only if depth < 2).

As it turns out, getting the number of physical cores isn't trivial.

Code - Text: [Select]

 
/// <summary>
/// Queries the system for the number of physical
/// CPU cores and logical processors. Requires a
/// reference to System.Management.dll
/// 
/// Credit: http://www.stev.org/post/2011/10/27/C-Number-Of-Cores-and-Processors.aspx
/// 
/// </summary>
 
public static class ThreadSystemInfo
{
    static ThreadSystemInfo()
    {
        try
        {
            ObjectQuery wql = new ObjectQuery( "SELECT * FROM Win32_Processor" );
            using( var searcher = new ManagementObjectSearcher( wql ) )
            using( var results = searcher.Get() )
            {
                if( results != null )
                {
                    var items = results.Cast<ManagementObject>();
                    if( items.Any() )
                    {
                        var item = items.First();
                        cores = int.Parse( item["NumberOfCores"].ToString() );
                        logicalProcessors = int.Parse( item["NumberOfLogicalProcessors"].ToString() );
                    }
                }
            }
        }
        catch // above can fail on some systems, so kludge it.
        {
            logicalProcessors = System.Environment.ProcessorCount;
            if( logicalProcessors > 1 )
                cores = logicalProcessors / 2;
        }
    }
 
    private static int cores = 1;
    public static int PhysicalCoreCount
    {
        get
        {
            return cores;
        }
    }
 
    private static int logicalProcessors = 1;
    public static int LogicalProcessorCount
    {
        get
        {
            return logicalProcessors;
        }
    }
}
 
 

Title: Re: Kd Tree
Post by: gile on April 08, 2013, 04:19:01 AM

Quote

Hi Gile - Great job.

"With a Little Help from My Friends", thanks.

For now I care less which is specific to "connect points challenge" and I focus more on the implementation of a class with methods more useful.

Title: Re: Kd Tree
Post by: TheMaster on April 08, 2013, 07:15:20 AM

Quote from: gile on April 08, 2013, 04:19:01 AM

Quote
Hi Gile - Great job.
"With a Little Help from My Friends", thanks.

For now I care less which is specific to "connect points challenge" and I focus more on the implementation of a class with methods more useful.

In case anyone wants to know more about the concepts of parallelism and its use in recursive solutions to various programming problems, like that which was used in this thread, this is highly-recommended reading:

http://blogs.msdn.com/b/pfxteam/archive/2008/01/31/7357135.aspx

Title: Re: Kd Tree
Post by: Jeff H on April 09, 2013, 05:55:51 PM

Hi Gile,
I am coming down with something and hopefully will not get too sick, but if you like I could send you more info tomorrow or the next day when feeling better if you wanted analyze it.

Title: Re: Kd Tree
Post by: gile on April 12, 2013, 12:09:18 PM

Thanks Jeff, I hope you feel better.

I added some public methods to the Point3dTree class.

This class may be used to improve performances in case of many queries in a quite large amount of points.
It targets the Framwork 4.0 to use some parallelization features.

According to the value of the 'ignoreZ' constructor argument, a 2d tree (ignoreZ = true) or a 3d tree (ignoreZ = false, default).
Use ignoreZ = true if all points in the input collection lie on a plane parallel to XY or if the points have to be considered as projected on the XY plane.

Public methods:
NearestNeighbour(Point3d) Gets the nearest neighbour.
NearestNeighbours(Point3d, int) Gets the n nearest neighbours.
NearestNeighbours(Point3d, double) Gets the nearest neighbours within the distance.
BoxedRange(Point3d, Point3d) Gets the points in a range.
ConnectAll(double) Gets all the pairs of points which distance is less or equal than the specified distance.
The last one was was almost used for performance tests (and to reply to this challenge (http://www.theswamp.org/index.php?topic=32874.msg383557#msg383557)).

Code - C#: [Select]

using System;
using System.Collections.Generic;
using System.Linq;
using Autodesk.AutoCAD.Geometry;
 
namespace PointKdTree
{
    /// <summary>
    /// Node of a Point3dTree
    /// </summary>
    public class TreeNode
    {
        /// <summary>
        /// Creates a new instance of TreeNode
        /// </summary>
        /// <param name="value">The 3d point value of the node.</param>
        public TreeNode(Point3d value) { this.Value = value; }
 
        /// <summary>
        /// Gets the value (Point3d) of the node.
        /// </summary>
        public Point3d Value { get; internal set; }
 
        /// <summary>
        /// Gets the parent node.
        /// </summary>
        public TreeNode Parent { get; internal set; }
 
        /// <summary>
        /// Gets the left child node.
        /// </summary>
        public TreeNode LeftChild { get; internal set; }
 
        /// <summary>
        /// Gets the right child node.
        /// </summary>
        public TreeNode RightChild { get; internal set; }
 
        /// <summary>
        /// Gets the depth of the node in tree.
        /// </summary>
        public int Depth { get; internal set; }
    }
 
    /// <summary>
    /// Provides methods to organize 3d points in a Kd tree structure to speed up the search of neighbours.
    /// A boolean constructor parameter (ignoreZ) indicates if the resulting Kd tree is a 3d tree or a 2d tree.
    /// Use ignoreZ = true if all points in the input collection lie on a plane parallel to XY 
    /// or if the points have to be considered as projected on the XY plane. 
    /// </summary>
    public class Point3dTree
    {
        #region Private fields
 
        private int dimension;
        private int parallelDepth;
        private bool ignoreZ;
        private Point2d center2d;
        private Point3d center;
        private Point3d current;
        private Func<Point3d, double> distanceTo;
 
        #endregion
 
        #region Constructor
 
        /// <summary>
        /// Creates an new instance of Point3dTree.
        /// </summary>
        /// <param name="points">The Point3d collection to fill the tree.</param>
        /// <param name="ignoreZ">A value indicating if the Z coordinate of points is ignored 
        /// (as if all points were projected to the XY plane).</param>
        public Point3dTree(IEnumerable<Point3d> points, bool ignoreZ = false)
        {
            if (points == null)
                throw new ArgumentNullException("points");
            this.ignoreZ = ignoreZ;
            this.dimension = ignoreZ ? 2 : 3;
            int numProc = System.Environment.ProcessorCount;
            this.parallelDepth = -1;
            while (numProc >> ++this.parallelDepth > 1) ;
            Point3d[] pts = points.Distinct().ToArray();
            this.Root = Construct(pts, 0, null);
        }
 
        #endregion
 
        #region Public properties
 
        /// <summary>
        /// Gets the root node of the tree.
        /// </summary>
        public TreeNode Root { get; private set; }
 
        #endregion
 
        #region Public methods
 
        /// <summary>
        /// Gets the nearest neighbour.
        /// </summary>
        /// <param name="point">The point from which search the nearest neighbour.</param>
        /// <returns>The nearest point in the collection from the specified one.</returns>
        public Point3d NearestNeighbour(Point3d point)
        {
            this.center = point;
            this.current = this.Root.Value;
            if (ignoreZ)
            {
                center2d = point.Flatten();
                this.distanceTo = Distance2dTo;
            }
            else
            {
                this.distanceTo = Distance3dTo;
            }
            return GetNeighbour(this.Root, this.Root.Value, double.MaxValue);
        }
 
        /// <summary>
        /// Gets the neighbours within the specified distance.
        /// </summary>
        /// <param name="point">The point from which search the nearest neighbours.</param>
        /// <param name="radius">The distance in which collect the neighbours.</param>
        /// <returns>The points which distance from the specified point is less or equal to the specified distance.</returns>
        public Point3dCollection NearestNeighbours(Point3d point, double radius)
        {
            Point3dCollection points = new Point3dCollection();
            this.center = point;
            if (this.ignoreZ)
            {
                this.center2d = point.Flatten();
                this.distanceTo = Distance2dTo;
            }
            else
            {
                this.distanceTo = Distance3dTo;
            }
            GetNeighboursAtDistance(radius, this.Root.LeftChild, points);
            GetNeighboursAtDistance(radius, this.Root.RightChild, points);
            return points;
        }
 
        /// <summary>
        /// Gets the n nearest neighbours.
        /// </summary>
        /// <param name="point">The point from which search the nearest neighbours.</param>
        /// <param name="number">The number of points to collect.</param>
        /// <returns>The n nearest neighbours of the specified point.</returns>
        public Point3dCollection NearestNeighbours(Point3d point, int number)
        {
            List<Tuple<double, Point3d>> pairs = new List<Tuple<double, Point3d>>(number);
            this.center = point;
            if (ignoreZ)
            {
                this.center2d = point.Flatten();
                this.distanceTo = Distance2dTo;
            }
            else
            {
                this.distanceTo = Distance3dTo;
            }
            GetNNeighbours(number, double.MaxValue, this.Root, pairs);
            Point3dCollection points = new Point3dCollection();
            for (int i = 0; i < pairs.Count; i++)
            {
                points.Add(pairs[i].Item2);
            }
            return points;
        }
 
        /// <summary>
        /// Gets the points in a range.
        /// </summary>
        /// <param name="pt1">The first corner of range.</param>
        /// <param name="pt2">The opposite corner of the range.</param>
        /// <returns>All points within the box.</returns>
        public Point3dCollection BoxedRange(Point3d pt1, Point3d pt2)
        {
            Point3d lowerLeft = new Point3d(
                Math.Min(pt1.X, pt2.X), Math.Min(pt1.Y, pt2.Y), Math.Min(pt1.Z, pt2.Z));
            Point3d upperRight = new Point3d(
                Math.Max(pt1.X, pt2.X), Math.Max(pt1.Y, pt2.Y), Math.Max(pt1.Z, pt2.Z));
            Point3dCollection points = new Point3dCollection();
            FindRange(lowerLeft, upperRight, this.Root, points);
            return points;
        }
 
        /// <summary>
        /// Gets all the pairs of points which distance is less or equal than the specified distance.
        /// </summary>
        /// <param name="radius">The maximum distance between two points. </param>
        /// <returns>The pairs of points which distance is less or equal than the specified distance.</returns>
        public List<Tuple<Point3d, Point3d>> ConnectAll(double radius)
        {
            List<Tuple<Point3d, Point3d>> connexions = new List<Tuple<Point3d, Point3d>>();
            Stack<TreeNode> nodes = new Stack<TreeNode>();
            GetConnexions(this.Root, radius, connexions, nodes);
            return connexions;
        }
 
        #endregion
 
        #region Private methods
 
        private TreeNode Construct(Point3d[] points, int depth, TreeNode parent)
        {
            int length = points.Length;
            if (length == 0) return null;
            int d = depth % this.dimension;
            Point3d median = points.QuickSelectMedian((p1, p2) => p1[d].CompareTo(p2[d]));
            TreeNode node = new TreeNode(median);
            node.Depth = depth;
            node.Parent = parent;
            int mid = length / 2;
            int rlen = length - mid - 1;
            Point3d[] left = new Point3d[mid];
            Point3d[] right = new Point3d[rlen];
            Array.Copy(points, 0, left, 0, mid);
            Array.Copy(points, mid + 1, right, 0, rlen);
            if (depth < this.parallelDepth)
            {
                System.Threading.Tasks.Parallel.Invoke(
                   () => node.LeftChild = Construct(left, depth + 1, node),
                   () => node.RightChild = Construct(right, depth + 1, node)
                );
            }
            else
            {
                node.LeftChild = Construct(left, depth + 1, node);
                node.RightChild = Construct(right, depth + 1, node);
            }
            return node;
        }
 
        private Point3d GetNeighbour(TreeNode node, Point3d currentBest, double bestDist)
        {
            if (node == null)
                return currentBest;
            this.current = node.Value;
            int d = node.Depth % this.dimension;
            double coordCen = center[d];
            double coordCur = current[d];
            double dist = this.distanceTo(this.current);
            if (dist >= 0.0 && dist < bestDist)
            {
                currentBest = this.current;
                bestDist = dist;
            }
            if (bestDist >= Math.Abs(coordCen - coordCur))
            {
                currentBest = GetNeighbour(
                    coordCen < coordCur ? node.LeftChild : node.RightChild, currentBest, bestDist);
                bestDist = this.distanceTo(currentBest);
            }
            else
            {
                currentBest = GetNeighbour(node.LeftChild, currentBest, bestDist);
                bestDist = this.distanceTo(currentBest);
                currentBest = GetNeighbour(node.RightChild, currentBest, bestDist);
                bestDist = this.distanceTo(currentBest);
            }
            return currentBest;
        }
 
        private void GetNeighboursAtDistance(double radius, TreeNode node, Point3dCollection points)
        {
            if (node == null) return;
            this.current = node.Value;
            double dist = this.distanceTo(this.current);
            if (dist <= radius)
            {
                points.Add(this.current);
            }
            int d = node.Depth % this.dimension;
            double coordCen = this.center[d];
            double coordCur = this.current[d];
            if (Math.Abs(coordCen - coordCur) > radius)
            {
                if (coordCen < coordCur)
                {
                    GetNeighboursAtDistance(radius, node.LeftChild, points);
                }
                else
                {
                    GetNeighboursAtDistance(radius, node.RightChild, points);
                }
            }
            else
            {
                GetNeighboursAtDistance(radius, node.LeftChild, points);
                GetNeighboursAtDistance(radius, node.RightChild, points);
            }
        }
 
        private void GetNNeighbours(int number, double worstDist, TreeNode node, List<Tuple<double, Point3d>> pairs)
        {
            if (node == null) return;
            this.current = node.Value;
            double dist = this.distanceTo(this.current);
            int cnt = pairs.Count;
            if (cnt < number)
            {
                pairs.Add(new Tuple<double, Point3d>(dist, this.current));
                pairs.Sort((p1, p2) => p1.Item1.CompareTo(p2.Item1));
                worstDist = pairs[cnt].Item1;
            }
            else if (dist < worstDist)
            {
                pairs.RemoveAt(number - 1);
                pairs.Add(new Tuple<double, Point3d>(dist, this.current));
                pairs.Sort((p1, p2) => p1.Item1.CompareTo(p2.Item1));
                worstDist = pairs[number - 1].Item1;
            }
            int d = node.Depth % this.dimension;
            double coordCen = center[d];
            double coordCur = current[d];
            if (Math.Abs(coordCen - coordCur) > worstDist)
            {
                if (coordCen < coordCur)
                {
                    GetNNeighbours(number, worstDist, node.LeftChild, pairs);
                }
                else
                {
                    GetNNeighbours(number, worstDist, node.RightChild, pairs);
                }
            }
            else
            {
                GetNNeighbours(number, worstDist, node.LeftChild, pairs);
                GetNNeighbours(number, pairs[pairs.Count - 1].Item1, node.RightChild, pairs);
            }
        }
 
        private void FindRange(Point3d lowerLeft, Point3d upperRight, TreeNode node, Point3dCollection points)
        {
            if (node == null)
                return;
            this.current = node.Value;
            if (ignoreZ)
            {
                if (this.current.X >= lowerLeft.X && this.current.X <= upperRight.X &&
                    this.current.Y >= lowerLeft.Y && this.current.Y <= upperRight.Y)
                    points.Add(this.current);
            }
            else
            {
                if (this.current.X >= lowerLeft.X && this.current.X <= upperRight.X &&
                    this.current.Y >= lowerLeft.Y && this.current.Y <= upperRight.Y &&
                    this.current.Z >= lowerLeft.Z && this.current.Z <= upperRight.Z)
                    points.Add(this.current);
            }
            int d = node.Depth % this.dimension;
            if (upperRight[d] < this.current[d])
                FindRange(lowerLeft, upperRight, node.LeftChild, points);
            else if (lowerLeft[d] > this.current[d])
                FindRange(lowerLeft, upperRight, node.RightChild, points);
            else
            {
                FindRange(lowerLeft, upperRight, node.LeftChild, points);
                FindRange(lowerLeft, upperRight, node.RightChild, points);
            }
        }
 
        private void GetConnexions(TreeNode node, double radius, List<Tuple<Point3d, Point3d>> connexions, Stack<TreeNode> nodes)
        {
            if (node == null) return;
            Point3dCollection points = new Point3dCollection();
            this.center = node.Value;
            if (ignoreZ)
            {
                this.center2d = this.center.Flatten();
                this.distanceTo = Distance2dTo;
            }
            else
            {
                this.distanceTo = Distance3dTo;
            }
            foreach (TreeNode tn in nodes)
            {
                TreeNode parent = tn.Parent;
                int d = parent.Depth % this.dimension;
                if (Math.Abs(this.center[d] - parent.Value[d]) <= radius)
                {
                    GetNeighboursAtDistance(radius, tn, points);
                }
            }
            GetNeighboursAtDistance(radius, node.LeftChild, points);
            GetNeighboursAtDistance(radius, node.RightChild, points);
            for (int i = 0; i < points.Count; i++)
            {
                connexions.Add(new Tuple<Point3d, Point3d>(this.center, points[i]));
            }
 
            if (node.RightChild != null)
            {
                nodes.Push(node.RightChild);
            }
            else if (node.LeftChild == null && nodes.Count > 0)
            {
                nodes.Pop();
            }
            GetConnexions(node.LeftChild, radius, connexions, nodes);
            GetConnexions(node.RightChild, radius, connexions, nodes);
        }
 
        private double Distance2dTo(Point3d pt)
        {
            return this.center2d.GetDistanceTo(pt.Flatten());
        }
 
        private double Distance3dTo(Point3d pt)
        {
            return this.center.DistanceTo(pt);
        }
 
        #endregion
    }
 
    static class Extensions
    {
        public static Point2d Flatten(this Point3d pt)
        {
            return new Point2d(pt.X, pt.Y);
        }
 
        // Credit: Tony Tanzillo
        // http://www.theswamp.org/index.php?topic=44312.msg495808#msg495808
        public static T QuickSelectMedian<T>(this T[] items, Comparison<T> compare)
        {
            int l = items.Length;
            int k = l / 2;
            if (items == null || l == 0)
                throw new ArgumentException("array");
            int from = 0;
            int to = l - 1;
            while (from < to)
            {
                int r = from;
                int w = to;
                T current = items[(r + w) / 2];
                while (r < w)
                {
                    if (compare(items[r], current) > -1)
                    {
                        var tmp = items[w];
                        items[w] = items[r];
                        items[r] = tmp;
                        w--;
                    }
                    else
                    {
                        r++;
                    }
                }
                if (compare(items[r], current) > 0)
                {
                    r--;
                }
                if (k <= r)
                {
                    to = r;
                }
                else
                {
                    from = r + 1;
                }
            }
            return items[k];
        }
    }
}
 

Title: Re: Kd Tree
Post by: TheMaster on April 12, 2013, 03:09:57 PM

Quote from: gile on April 12, 2013, 12:09:18 PM

The last one was was almost used for performance tests (and to reply to this challenge (http://www.theswamp.org/index.php?topic=32874.msg383557#msg383557)).

I read that thread, but has anyone compared your implementation to the C/C++ based ones from that thread?

I also noticed in one post, that someone is using (getvar "DATE") to measure times - which is not very accurate (for sub-second times).

Title: Re: Kd Tree
Post by: gile on April 12, 2013, 04:12:46 PM

Quote from: TT on April 12, 2013, 03:09:57 PM

I read that thread, but has anyone compared your implementation to the C/C++ based ones from that thread?

I don't think so, this thread is quite old and all the C/C++ part was/is other my head. I remember there was impressive LISP results from Evgeniy.

Title: Re: Kd Tree
Post by: gile on April 13, 2013, 05:32:02 PM

I changed the way to avoid duplicated lines in the ConnectAll. Rather than a boolean 'Processed' property in the TreeNode, I now use a Stack<TreeNode> containing the right children of the parents of the current node where to search (or not if far enough) for neighbours. This improves a little the 'connect points" part speed.
The tree is now double linked.

Some results:

Code - Text: [Select]

100K points (128 026 lines)
Get points:      220
Build tree:      114
Connect points:  199
Draw lines:      764
Total:          1297
 
1 Million points (1 280 741 lines)
Get points:     2118
Build tree:     1388
Connect points: 2107
Draw lines:    12046
Total:         17659

The testing command:

Code - C#: [Select]

        [CommandMethod("CONNECT")]
        public void Connect()
        {
            Document doc = AcAp.DocumentManager.MdiActiveDocument;
            Database db = doc.Database;
            Editor ed = doc.Editor;
            try
            {
                System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
                sw.Start();
                RXClass rxc = RXClass.GetClass(typeof(DBPoint));
                using (BlockTableRecord btr = (BlockTableRecord)db.CurrentSpaceId.Open(OpenMode.ForWrite))
                {
                    long t0 = sw.ElapsedMilliseconds;
                    ObjectId[] ids = btr.Cast<ObjectId>().Where(id => id.ObjectClass == rxc).ToArray();
                    int len = ids.Length;
                    Point3d[] pts = new Point3d[len];
                    for (int i = 0; i < len; i++)
                    {
                        using (DBPoint pt = (DBPoint)ids[i].Open(OpenMode.ForRead))
                        {
                            pts[i] = pt.Position;
                        }
                    }
                    long t1 = sw.ElapsedMilliseconds;
                    Point3dTree tree = new Point3dTree(pts, true); // <- true = 2d tree, false = 3d tree
                    long t2 = sw.ElapsedMilliseconds;
                    List<Tuple<Point3d, Point3d>> pairs = tree.ConnectAll(70.0);
                    long t3 = sw.ElapsedMilliseconds;
                    foreach (var pair in pairs)
                    {
                        using (Line line = new Line(pair.Item1, pair.Item2))
                        {
                            btr.AppendEntity(line);
                        }
                    }
                    db.TransactionManager.QueueForGraphicsFlush();
                    sw.Stop();
                    long t4 = sw.ElapsedMilliseconds;
                    ed.WriteMessage(
                        "\nGet points:{0,9}\nBuild tree:{1,9}\nConnect points:{2,5}\nDraw lines:{3,9}\nTotal:{4,14}",
                        t1 - t0, t2 - t1, t3 - t2, t4 - t3, t4);
                }
            }
            catch (System.Exception ex)
            {
                ed.WriteMessage("\n{0}\n{1}", ex.Message, ex.StackTrace);
            }
        }

Title: Re: Kd Tree
Post by: TheMaster on April 14, 2013, 01:25:23 PM

Quote from: gile on April 12, 2013, 04:12:46 PM

Quote from: TT on April 12, 2013, 03:09:57 PM
I read that thread, but has anyone compared your implementation to the C/C++ based ones from that thread?
I don't think so, this thread is quite old and all the C/C++ part was/is other my head. I remember there was impressive LISP results from Evgeniy.

I would expect that once a dataset is indexed, locating points within a given distance should not be too difficult in any language, since the whole point to indexing the data is to minimize the amount of work needed to find nearest coordinates.

LISP is well-suited to tree-based, recursive algorithms, so it wouldn't surprise me that can find points in an index dataset quickly. But, I would be surprised if he was able to construct the tree as fast as it can be done in .NET or native code. I very much doubt that, if for no other reason, because of the inherent overhead of dealing with immutable lists.

Title: Re: Kd Tree
Post by: gile on April 14, 2013, 01:52:17 PM

Hi Tony,

It seems to me he didn't use a tree structure but a 'divide and conquer' algorithm as shown by this picture (http://divide and conqueer).
By my side, I tried to implement a more reusable (and may be usefull) structure.

I found another way to avoid duplicated connexions with the ConnectAll() method. Rather than storing nodes in a stack, I can get the 'right parents nodes' of the current node using a new property in the TreeNode which indicates if the the node is a left child node or not. This seems to improve a little ConnectAll() speed.

Here's the new implementation.

New version:
- Replaced the using global fields by function parameters so that the public methods can be used in parallel execution.
- Using of square distances

Code - C#: [Select]

using System;
using System.Collections.Generic;
using System.Linq;
using Autodesk.AutoCAD.Geometry;
 
namespace PointKdTree
{
    /// <summary>
    /// Node of a Point3dTree
    /// </summary>
    public class TreeNode
    {
        /// <summary>
        /// Creates a new instance of TreeNode
        /// </summary>
        /// <param name="value">The 3d point value of the node.</param>
        public TreeNode(Point3d value) { this.Value = value; }
 
        /// <summary>
        /// Gets the value (Point3d) of the node.
        /// </summary>
        public Point3d Value { get; internal set; }
 
        /// <summary>
        /// Gets the parent node.
        /// </summary>
        public TreeNode Parent { get; internal set; }
 
        /// <summary>
        /// Gets the left child node.
        /// </summary>
        public TreeNode LeftChild { get; internal set; }
 
        /// <summary>
        /// Gets the right child node.
        /// </summary>
        public TreeNode RightChild { get; internal set; }
 
        /// <summary>
        /// Gets the depth of the node in tree.
        /// </summary>
        public int Depth { get; internal set; }
 
        /// <summary>
        /// Gets a value indicating if the current node is a LeftChild node.
        /// </summary>
        public bool IsLeft { get; internal set; }
    }
 
    /// <summary>
    /// Provides methods to organize 3d points in a Kd tree structure to speed up the search of neighbours.
    /// A boolean constructor parameter (ignoreZ) indicates if the resulting Kd tree is a 3d tree or a 2d tree.
    /// Use ignoreZ = true if all points in the input collection lie on a plane parallel to XY 
    /// or if the points have to be considered as projected on the XY plane. 
    /// </summary>
    public class Point3dTree
    {
        #region Private fields
 
        private int dimension;
        private int parallelDepth;
        private bool ignoreZ;
        private Func<Point3d, Point3d, double> sqrDist;
 
        #endregion
 
        #region Constructor
 
        /// <summary>
        /// Creates an new instance of Point3dTree.
        /// </summary>
        /// <param name="points">The Point3d collection to fill the tree.</param>
        /// <param name="ignoreZ">A value indicating if the Z coordinate of points is ignored 
        /// (as if all points were projected to the XY plane).</param>
        public Point3dTree(IEnumerable<Point3d> points, bool ignoreZ = false)
        {
            if (points == null)
                throw new ArgumentNullException("points");
            this.ignoreZ = ignoreZ;
            this.dimension = ignoreZ ? 2 : 3;
            if (ignoreZ)
                this.sqrDist = SqrDistance2d;
            else
                this.sqrDist = SqrDistance3d;
            int numProc = System.Environment.ProcessorCount;
            this.parallelDepth = -1;
            while (numProc >> ++this.parallelDepth > 1) ;
            Point3d[] pts = points.Distinct().ToArray();
            this.Root = Create(pts, 0, null, false);
        }
 
        #endregion
 
        #region Public properties
 
        /// <summary>
        /// Gets the root node of the tree.
        /// </summary>
        public TreeNode Root { get; private set; }
 
        #endregion
 
        #region Public methods
 
        /// <summary>
        /// Gets the nearest neighbour.
        /// </summary>
        /// <param name="point">The point from which search the nearest neighbour.</param>
        /// <returns>The nearest point in the collection from the specified one.</returns>
        public Point3d NearestNeighbour(Point3d point)
        {
            return GetNeighbour(point, this.Root, this.Root.Value, double.MaxValue);
        }
 
        /// <summary>
        /// Gets the neighbours within the specified distance.
        /// </summary>
        /// <param name="point">The point from which search the nearest neighbours.</param>
        /// <param name="radius">The distance in which collect the neighbours.</param>
        /// <returns>The points which distance from the specified point is less or equal to the specified distance.</returns>
        public Point3dCollection NearestNeighbours(Point3d point, double radius)
        {
            Point3dCollection points = new Point3dCollection();
            GetNeighboursAtDistance(point, radius * radius, this.Root, points);
            return points;
        }
 
        /// <summary>
        /// Gets the given number of nearest neighbours.
        /// </summary>
        /// <param name="point">The point from which search the nearest neighbours.</param>
        /// <param name="number">The number of points to collect.</param>
        /// <returns>The n nearest neighbours of the specified point.</returns>
        public Point3dCollection NearestNeighbours(Point3d point, int number)
        {
            List<Tuple<double, Point3d>> pairs = new List<Tuple<double, Point3d>>(number);
            GetKNeighbours(point, number, this.Root, pairs);
            Point3dCollection points = new Point3dCollection();
            for (int i = 0; i < pairs.Count; i++)
            {
                points.Add(pairs[i].Item2);
            }
            return points;
        }
 
        /// <summary>
        /// Gets the points in a range.
        /// </summary>
        /// <param name="pt1">The first corner of range.</param>
        /// <param name="pt2">The opposite corner of the range.</param>
        /// <returns>All points within the box.</returns>
        public Point3dCollection BoxedRange(Point3d pt1, Point3d pt2)
        {
            Point3d lowerLeft = new Point3d(
                Math.Min(pt1.X, pt2.X), Math.Min(pt1.Y, pt2.Y), Math.Min(pt1.Z, pt2.Z));
            Point3d upperRight = new Point3d(
                Math.Max(pt1.X, pt2.X), Math.Max(pt1.Y, pt2.Y), Math.Max(pt1.Z, pt2.Z));
            Point3dCollection points = new Point3dCollection();
            FindRange(lowerLeft, upperRight, this.Root, points);
            return points;
        }
 
        /// <summary>
        /// Gets all the pairs of points which distance is less or equal than the specified distance.
        /// </summary>
        /// <param name="radius">The maximum distance between two points. </param>
        /// <returns>The pairs of points which distance is less or equal than the specified distance.</returns>
        public List<Tuple<Point3d, Point3d>> ConnectAll(double radius)
        {
            List<Tuple<Point3d, Point3d>> connexions = new List<Tuple<Point3d, Point3d>>();
            GetConnexions(this.Root, radius * radius, connexions);
            return connexions;
        }
 
        #endregion
 
        #region Private methods
 
        private TreeNode Create(Point3d[] points, int depth, TreeNode parent, bool isLeft)
        {
            int length = points.Length;
            if (length == 0) return null;
            int d = depth % this.dimension;
            Point3d median = points.QuickSelectMedian((p1, p2) => p1[d].CompareTo(p2[d]));
            TreeNode node = new TreeNode(median);
            node.Depth = depth;
            node.Parent = parent;
            node.IsLeft = isLeft;
            int mid = length / 2;
            int rlen = length - mid - 1;
            Point3d[] left = new Point3d[mid];
            Point3d[] right = new Point3d[rlen];
            Array.Copy(points, 0, left, 0, mid);
            Array.Copy(points, mid + 1, right, 0, rlen);
            if (depth < this.parallelDepth)
            {
                System.Threading.Tasks.Parallel.Invoke(
                   () => node.LeftChild = Create(left, depth + 1, node, true),
                   () => node.RightChild = Create(right, depth + 1, node, false)
                );
            }
            else
            {
                node.LeftChild = Create(left, depth + 1, node, true);
                node.RightChild = Create(right, depth + 1, node, false);
            }
            return node;
        }
 
        private Point3d GetNeighbour(Point3d center, TreeNode node, Point3d currentBest, double bestDist)
        {
            if (node == null)
                return currentBest;
            Point3d current = node.Value;
            int d = node.Depth % this.dimension;
            double coordCen = center[d];
            double coordCur = current[d];
            double dist = this.sqrDist(center, current);
            if (dist >= 0.0 && dist < bestDist)
            {
                currentBest = current;
                bestDist = dist;
            }
            dist = coordCen - coordCur;
            if (bestDist < dist * dist)
            {
                currentBest = GetNeighbour(
                    center, coordCen < coordCur ? node.LeftChild : node.RightChild, currentBest, bestDist);
                bestDist = this.sqrDist(center, currentBest);
            }
            else
            {
                currentBest = GetNeighbour(center, node.LeftChild, currentBest, bestDist);
                bestDist = this.sqrDist(center, currentBest);
                currentBest = GetNeighbour(center, node.RightChild, currentBest, bestDist);
                bestDist = this.sqrDist(center, currentBest);
            }
            return currentBest;
        }
 
        private void GetNeighboursAtDistance(Point3d center, double radius, TreeNode node, Point3dCollection points)
        {
            if (node == null) return;
            Point3d current = node.Value;
            double dist = this.sqrDist(center, current);
            if (dist <= radius)
            {
                points.Add(current);
            }
            int d = node.Depth % this.dimension;
            double coordCen = center[d];
            double coordCur = current[d];
            dist = coordCen - coordCur;
            if (dist * dist > radius)
            {
                if (coordCen < coordCur)
                {
                    GetNeighboursAtDistance(center, radius, node.LeftChild, points);
                }
                else
                {
                    GetNeighboursAtDistance(center, radius, node.RightChild, points);
                }
            }
            else
            {
                GetNeighboursAtDistance(center, radius, node.LeftChild, points);
                GetNeighboursAtDistance(center, radius, node.RightChild, points);
            }
        }
 
        private void GetKNeighbours(Point3d center, int number, TreeNode node, List<Tuple<double, Point3d>> pairs)
        {
            if (node == null) return;
            Point3d current = node.Value;
            double dist = this.sqrDist(center, current);
            int cnt = pairs.Count;
            if (cnt == 0)
            {
                pairs.Add(new Tuple<double, Point3d>(dist, current));
            }
            else if (cnt < number)
            {
                if (dist > pairs[0].Item1)
                {
                    pairs.Insert(0, new Tuple<double, Point3d>(dist, current));
                }
                else
                {
                    pairs.Add(new Tuple<double, Point3d>(dist, current));
                }
            }
            else if (dist < pairs[0].Item1)
            {
                pairs[0] = new Tuple<double, Point3d>(dist, current);
                pairs.Sort((p1, p2) => p2.Item1.CompareTo(p1.Item1));
            }
            int d = node.Depth % this.dimension;
            double coordCen = center[d];
            double coordCur = current[d];
            dist = coordCen - coordCur;
            if (dist * dist > pairs[0].Item1)
            {
                if (coordCen < coordCur)
                {
                    GetKNeighbours(center, number, node.LeftChild, pairs);
                }
                else
                {
                    GetKNeighbours(center, number, node.RightChild, pairs);
                }
            }
            else
            {
                GetKNeighbours(center, number, node.LeftChild, pairs);
                GetKNeighbours(center, number, node.RightChild, pairs);
            }
        }
 
        private void FindRange(Point3d lowerLeft, Point3d upperRight, TreeNode node, Point3dCollection points)
        {
            if (node == null)
                return;
            Point3d current = node.Value;
            if (ignoreZ)
            {
                if (current.X >= lowerLeft.X && current.X <= upperRight.X &&
                    current.Y >= lowerLeft.Y && current.Y <= upperRight.Y)
                    points.Add(current);
            }
            else
            {
                if (current.X >= lowerLeft.X && current.X <= upperRight.X &&
                    current.Y >= lowerLeft.Y && current.Y <= upperRight.Y &&
                    current.Z >= lowerLeft.Z && current.Z <= upperRight.Z)
                    points.Add(current);
            }
            int d = node.Depth % this.dimension;
            if (upperRight[d] < current[d])
                FindRange(lowerLeft, upperRight, node.LeftChild, points);
            else if (lowerLeft[d] > current[d])
                FindRange(lowerLeft, upperRight, node.RightChild, points);
            else
            {
                FindRange(lowerLeft, upperRight, node.LeftChild, points);
                FindRange(lowerLeft, upperRight, node.RightChild, points);
            }
        }
 
        private void GetConnexions(TreeNode node, double radius, List<Tuple<Point3d, Point3d>> connexions)
        {
            if (node == null) return;
            Point3dCollection points = new Point3dCollection();
            Point3d center = node.Value;
            if (ignoreZ)
            GetRightParentsNeighbours(center, node, radius, points);
            GetNeighboursAtDistance(center, radius, node.LeftChild, points);
            GetNeighboursAtDistance(center, radius, node.RightChild, points);
            for (int i = 0; i < points.Count; i++)
            {
                connexions.Add(new Tuple<Point3d, Point3d>(center, points[i]));
            }
            GetConnexions(node.LeftChild, radius, connexions);
            GetConnexions(node.RightChild, radius, connexions);
        }
 
        private void GetRightParentsNeighbours(Point3d center, TreeNode node, double radius, Point3dCollection points)
        {
            TreeNode parent = GetRightParent(node);
            if (parent == null) return;
            int d = parent.Depth % this.dimension;
            double dist = center[d] - parent.Value[d];
            if (dist * dist <= radius)
            {
                GetNeighboursAtDistance(center, radius, parent.RightChild, points);
            }
            GetRightParentsNeighbours(center, parent, radius, points);
        }
 
        private TreeNode GetRightParent(TreeNode node)
        {
            TreeNode parent = node.Parent;
            if (parent == null) return null;
            if (node.IsLeft) return parent;
            return GetRightParent(parent);
        }
 
        private double SqrDistance2d(Point3d p1, Point3d p2)
        {
            return (p1.X - p2.X) * (p1.X - p2.X) +
                (p1.Y - p2.Y) * (p1.Y - p2.Y);
        }
 
        private double SqrDistance3d(Point3d p1, Point3d p2)
        {
            return (p1.X - p2.X) * (p1.X - p2.X) + 
                (p1.Y - p2.Y) * (p1.Y - p2.Y) + 
                (p1.Z - p2.Z) * (p1.Z - p2.Z);
        }
 
        #endregion
    }
 
    static class Extensions
    {
        // Credit: Tony Tanzillo
        // http://www.theswamp.org/index.php?topic=44312.msg495808#msg495808
        public static T QuickSelectMedian<T>(this T[] items, Comparison<T> compare)
        {
            int l = items.Length;
            int k = l / 2;
            if (items == null || l == 0)
                throw new ArgumentException("array");
            int from = 0;
            int to = l - 1;
            while (from < to)
            {
                int r = from;
                int w = to;
                T current = items[(r + w) / 2];
                while (r < w)
                {
                    if (compare(items[r], current) > -1)
                    {
                        var tmp = items[w];
                        items[w] = items[r];
                        items[r] = tmp;
                        w--;
                    }
                    else
                    {
                        r++;
                    }
                }
                if (compare(items[r], current) > 0)
                {
                    r--;
                }
                if (k <= r)
                {
                    to = r;
                }
                else
                {
                    from = r + 1;
                }
            }
            return items[k];
        }
    }
}
 

Title: Re: Kd Tree
Post by: TheMaster on April 14, 2013, 02:59:15 PM

Quote from: gile on April 14, 2013, 01:52:17 PM

Hi Tony,

It seems to me he didn't use a tree structure but a 'divide and conquer' algorithm as shown by this picture (http://divide and conqueer).
By my side, I tried to implement a more reusable (and may be usefull) structure.

I found another way to avoid duplicated connexions with the ConnectAll() method. Rather than storing nodes in a stack, I can get the 'right parents nodes' of the current node using a new property in the TreeNode which indicates if the the node is a left child node or not. This seems to improve a little ConnectAll() speed.

Hi Gile. Very good.

Here's one more suggestion: Depending on a number of factors like point distribution, there can be a very large number of leaf nodes in the tree (leaf nodes are nodes with no child nodes). You can save some memory by not having fields for child nodes on leaf nodes, by using two different classes for leaf and branch nodes, using something like this:

Code - C#: [Select]

 
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
 
namespace Graph.Nodes
{
 
   /// Use for leaf nodes with no children:
   /// (contains no fields for child nodes)
   
   public class Node<T>
   {
      protected Node( T value )
      {
         this.Value = value;
      }
 
      public T Value
      {
         get;
         set;
      }
 
      public virtual Node<T> Left
      {
         get
         {
            return null;
         }
         protected set
         {
         }
      }
 
      public virtual Node<T> Right
      {
         get
         {
            return null;
         }
         protected set
         {
         }
      }
 
      // Create leaf or branch node depending on
      // if child nodes are provided:
      
      public static Node<T> Create( T value, Node<T> left = null, Node<T> right = null )
      {
         if( left == null && right == null )
            return new Node<T>( value );
         else
            return new BranchNode<T>( value, left, right );
      }
   }
   
   /// Used for branch nodes with 1 or 2 children
 
   public class BranchNode<T> : Node<T>
   {
      Node<T> left = null;
      Node<T> right = null;
 
      protected internal BranchNode( T value, Node<T> left, Node<T> right ) 
         : base( value )
      {
         this.left = left;
         this.right = right;
      }
 
      public override Node<T> Left
      {
         get
         {
            return left;
         }
         protected set
         {
            left = value;
         }
      }
 
      public override Node<T> Right
      {
         get
         {
            return right;
         }
         protected set
         {
            right = value;
         }
      }
   }
 
}
 
 

Title: Re: Kd Tree
Post by: gile on April 14, 2013, 04:46:21 PM

Thanks Tony, I'll give a try.
As the tree is balanced, there would never be more than half number of points plus one leaf nodes.

Title: Re: Kd Tree
Post by: Jerry J on April 16, 2013, 04:07:34 AM

Thanks for stretching my peanut of a brain.

Title: Re: Kd Tree
Post by: gile on April 19, 2013, 11:32:50 AM

I made some changes in the upper code.
I replaced the using of fields (global) by method parameters so that the public method can run in parallel execution.
I replaced the DistanceTo/GetDistanceTo methods by square distances.

Title: Re: Kd Tree
Post by: WILL HATCH on April 19, 2013, 12:16:44 PM

Great work gile!

Title: Re: Kd Tree
Post by: gile on April 26, 2013, 02:47:20 PM

Here's the F# version.
The main interest is that it allows parallel execution for building the tree while targeting the Framwork 2.0 so that it can be used with AutoCAD version form 2007 to 2011.

The F# specific constructs and types are only used internally.
The public methods (the same as in the C# version) are writen to be used with other .NET languages (C# or VB) they return Point3d arrays rather than Point3dCollections. A 'Pair' type (class) is used to replace the missing Tuple class in .NET Frameworks prior to 4.0.

Attached the FsPoint3dTree.dll which can be referenced in C# or VB projects (it may be needed to install the F# runtime (http://msdn.microsoft.com/en-us/library/ee829875%28v=vs.100%29.aspx)).

Note: I can't understand why, but the building of the tree runs about ten times slower in A 2012 and later than in prior versions, so for using with A2012 or later, use the C# version instead.

Code - F#: [Select]

namespace Gile.Point3dTree
 
open System
open Autodesk.AutoCAD.Geometry
 
module private Array =
    let median (compare: 'a -> 'a -> int) (items: 'a[])=
        if items = null || items.Length = 0 then 
            failwith "items"
        let l = items.Length
        let k = l / 2
 
        let rec loop f t =
            if f < t then swap f t items.[(f+t) / 2] f t
            else items.[.. k-1], items.[k], items.[k+1 ..]
        and swap a b c f t =
            if a < b then
                if compare items.[a] c > -1 then
                    let tmp = items.[b]
                    items.[b] <- items.[a]
                    items.[a] <- tmp
                    swap a (b-1) c f t
                else
                    swap (a+1) b c f t
            else
                let n = if compare items.[a] c > 0 then a - 1 else a
                if k <= n 
                then loop f n
                else loop (n+1) t
 
        loop 0 (l-1)
 
type private TreeNode =
    | Empty
    | Node of int * Point3d option * Point3d * TreeNode * TreeNode  
 
/// <summary>
/// Defines a tuple (double) to be used with versions of .NET Framework prior to 4.0
/// </summary>
/// <typeparam name="T1">Type of the first item.</typeparam>
/// <typeparam name="T2">Type of the second item.</typeparam>
/// <param name="item1">First item.</param>
/// <param name="item2">Second item.</param>
type Pair<'T1, 'T2>(item1, item2) =
 
    /// <summary>
    /// Gets the first item of the pair.
    /// </summary>
    member this.Item1 with get() = item1
 
    /// <summary>
    /// Gets the second item of the pair.
    /// </summary>
    member this.Item2 with get() = item2
 
    /// <summary>
    /// Creates a new instance of Pair.
    /// </summary>
    /// <param name="item1">First item of the pair.</param>
    /// <param name="item2">Second item of the pair.</param>
    /// <returns>a new Pair containing the items.</returns>
    static member Create(item1, item2) = Pair(item1, item2)
 
 
/// <summary>
/// Creates an new instance of Point3dTree.
/// </summary>
/// <param name="points">The Point3d collection to fill the tree.</param>
/// <param name="ignoreZ">A value indicating if the Z coordinate of points is ignored
/// (as if all points were projected to the XY plane).</param>
type Point3dTree (points: Point3d seq, ignoreZ: bool) =
    do if points = null then raise (System.ArgumentNullException("points"))
 
    let dimension = if ignoreZ then 2 else 3
    let sqrDist (p1: Point3d) (p2: Point3d) = 
        if ignoreZ 
        then (p1.X - p2.X) * (p1.X - p2.X) + (p1.Y - p2.Y) * (p1.Y - p2.Y)
        else (p1.X - p2.X) * (p1.X - p2.X) + (p1.Y - p2.Y) * (p1.Y - p2.Y) + (p1.Z - p2.Z) * (p1.Z - p2.Z)
 
    let rec shift n d =
        if n >>> d > 1 then shift n (d+1) else d
    let pDepth = shift System.Environment.ProcessorCount 0
 
    let create pts =
        let rec loop depth parent = function
            | [||] -> Empty
            | pts ->
                let d = depth % dimension
                let left, median, right = 
                    pts |> Array.median(fun (p1: Point3d) p2 -> compare p1.[d] p2.[d])
                let children = 
                    if depth < pDepth then
                        [ async { return loop (depth + 1) (Some(median)) left };
                          async { return loop (depth + 1) (Some(median)) right } ]
                        |> Async.Parallel
                        |> Async.RunSynchronously
                    else
                        [| loop (depth + 1) (Some(median)) left; 
                           loop (depth + 1) (Some(median)) right |]
                Node(depth, parent, median, children.[0], children.[1])
        loop 0 None pts
 
    let root = points |> Seq.distinct |> Seq.toArray |> create
 
    let rec findNeighbour location node (current, bestDist) =
        match node with
        | Empty -> (current, bestDist)
        | Node(depth, _, point, left, right) ->
            let dist = sqrDist point location
            let d = depth % dimension
            let bestPair = 
                if dist < bestDist
                then point, dist
                else current, bestDist
            if bestDist < (location.[d] - point.[d]) * (location.[d] - point.[d]) then 
                findNeighbour location (if location.[d] < point.[d] then left else right) bestPair
            else
                findNeighbour location left bestPair
                |> findNeighbour location right
 
    let rec getNeighbours center radius node acc =
        match node with
        | Empty -> acc
        | Node(depth, _, point, left, right) ->
            let acc = if sqrDist center point <= radius then point :: acc else acc
            let d= depth % dimension;
            let coordCen, coordPt = center.[d], point.[d]
            if (coordCen - coordPt) * (coordCen - coordPt) > radius then
                getNeighbours center radius (if coordCen < coordPt then left else right) acc
            else
                getNeighbours center radius left acc
                |> getNeighbours center radius right
 
    let rec getKNeighbours center number node (pairs: (float * Point3d) list) =
        match node with
        | Empty -> pairs
        | Node(depth, _, point, left, right) ->
            let dist = sqrDist center point
            let pairs =
                match pairs.Length with
                | 0 -> [ (dist, point) ]
                | l when l < number -> 
                    if (dist > fst pairs.Head)
                    then (dist, point) :: pairs
                    else pairs.Head :: (dist, point) :: pairs.Tail
                | _ ->
                    if dist < fst pairs.Head
                    then ((dist, point) :: pairs.Tail) |> List.sortBy(fun p -> -fst p)
                    else pairs
            let d = depth % dimension
            let coordCen, coordCur = center.[d], point.[d]
            if (coordCen - coordCur) * (coordCen - coordCur) > fst pairs.Head then
                getKNeighbours center number (if coordCen < coordCur then left else right) pairs
            else
                getKNeighbours center number left pairs
                |> getKNeighbours center number right
 
    let rec findRange (lowerLeft: Point3d) (upperRight: Point3d) node (acc: Point3d list) =
        match node with
        | Empty -> acc
        | Node(depth, _, point, left, right) ->
            let acc = 
                if ignoreZ then
                    if point.X >= lowerLeft.X && point.X <= upperRight.X &&
                       point.Y >= lowerLeft.Y && point.Y <= upperRight.Y
                    then point :: acc else acc
                else
                    if point.X >= lowerLeft.X && point.X <= upperRight.X &&
                       point.Y >= lowerLeft.Y && point.Y <= upperRight.Y &&
                       point.Z >= lowerLeft.Z && point.Z <= upperRight.Z
                    then point :: acc else acc
            let d = depth % dimension
            if upperRight.[d] < point.[d] 
            then findRange lowerLeft upperRight left acc
            elif lowerLeft.[d] > point.[d] 
            then findRange lowerLeft upperRight right acc
            else findRange lowerLeft upperRight left acc 
                 |> findRange lowerLeft upperRight right
                
    let rec getConnexions node radius (nodes, pairs) =
        match node with
        | Empty -> (nodes, pairs)
        | Node(depth, parent, point, left, right) -> 
            let pairs =
                nodes 
                |> List.fold(fun acc (n: TreeNode) -> 
                    match n with
                    | Empty -> acc
                    | Node(dep, par, _, _, _) ->
                        let pt = par.Value
                        let d = (dep + 1) % dimension
                        if (point.[d] - pt.[d]) * (point.[d] - pt.[d]) <= radius
                        then getNeighbours point radius n acc
                        else acc)
                    (getNeighbours point radius left [] 
                    |> getNeighbours point radius right)
                |> List.map(fun p -> Pair<Point3d, Point3d>(point, p))
                |> List.fold(fun acc p -> p :: acc) pairs
            let nodes = 
                match nodes with
                | [] -> if right = Empty then [] else [right]
                | h :: t  -> 
                    if right = Empty 
                    then if left = Empty then t else nodes
                    else right :: nodes
            getConnexions left radius (nodes, pairs) 
            |> getConnexions right radius             
    
    /// <summary>
    /// Creates an new instance of Point3dTree with ignoreZ = false (default).
    /// </summary>
    /// <param name="points">The Point3d collection to fill the tree.</param>
    new (points: Point3d seq) = Point3dTree(points, false)
 
    /// <summary>
    /// Gets the nearest neighbour.
    /// </summary>
    /// <param name="point">The point from which search the nearest neighbour.</param>
    /// <returns>The nearest point in the collection from the specified one.</returns>
    member this.NearestNeighbour(location) =
        match root with
        | Empty -> raise (System.ArgumentNullException("root"))
        | Node(_, _, point, _, _) ->
            findNeighbour location root (point, Double.MaxValue) 
            |> fst
 
    /// <summary>
    /// Gets the neighbours within the specified distance.
    /// </summary>
    /// <param name="point">The point from which search the nearest neighbours.</param>
    /// <param name="radius">The distance in which collect the neighbours.</param>
    /// <returns>The points which distance from the specified point is less or equal to the specified distance.</returns>
    member this.NearestNeighbours(center, radius) =
        getNeighbours center (radius * radius) root [] 
        |> List.toArray
 
    /// <summary>
    /// Gets the given number of nearest neighbours.
    /// </summary>
    /// <param name="point">The point from which search the nearest neighbours.</param>
    /// <param name="number">The number of points to collect.</param>
    /// <returns>The n nearest neighbours of the specified point.</returns>
    member this.NearestNeighbours(center, number) =
        getKNeighbours center number root []
        |> List.map(fun p -> snd p)
        |> List.toArray
    
    /// <summary>
    /// Gets the points in a range.
    /// </summary>
    /// <param name="pt1">The first corner of range.</param>
    /// <param name="pt2">The opposite corner of the range.</param>
    /// <returns>All points within the box.</returns>
    member this.BoxedRange(pt1: Point3d, pt2: Point3d) =
        let lowerLeft = Point3d(min pt1.X pt2.X, min pt1.Y pt2.Y, min pt1.Z pt2.Z)
        let upperRight = Point3d(max pt1.X pt2.X, max pt1.Y pt2.Y, max pt1.Z pt2.Z)
        findRange lowerLeft upperRight root []
        |> List.toArray
 
    /// <summary>
    /// Gets all the pairs of points which distance is less or equal than the specified distance.
    /// </summary>
    /// <param name="radius">The maximum distance between two points. </param>
    /// <returns>The pairs of points which distance is less or equal than the specified distance.</returns>
    member this.ConnectAll(radius) =
        getConnexions root (radius * radius) ([], []) 
        |> snd
        |> List.toArray

Title: Re: Kd Tree
Post by: kaefer on April 27, 2013, 07:32:23 AM

Quote from: gile on April 26, 2013, 02:47:20 PM

Here's the F# version.

Wow, that's an impressive piece of work.

I'm in no way qualified to comment on its inner workings; but what irks me, from a theoretical perspective at least, is that your tree handling functions aren't tail recursive. In this case they have no reason to, since you're guaranteed to encounter nicely balanced trees only, whose depth would never exceed the stack limit without exhausting the available memory.

Just in case, here's (http://lorgonblog.wordpress.com/2008/04/06/catamorphisms-part-two/) a link to a classic blog post demonstrating continuation-passing style and this (http://zbray.com/2011/11/02/solving-the-0-1-knapsack-problem-using-continuation-passing-style-with-memoization-in-f/) is an example of the continuation monad, such that

Code - F#: [Select]

// a tree
type 'a T = E | N of 'a T * 'a * 'a T 
 
type ContinuationBuilder() =
    member b.Bind(x, f) = fun k -> x (fun x -> f x k)
    member b.Return x = fun k -> k x
 
let cont = ContinuationBuilder()
 
module Tree =
    let foldTree nodeFunc z0 t =
        let rec aux t = cont{
            match t with
            | E -> return z0
            | N(left, z, right) ->
                let! leftRes = aux left
                let! rightRes = aux right
                return nodeFunc leftRes z rightRes }
        aux t id
 
// example
N(N(E, 2, E), 1, N(E, 3, N(E, 4, E)))
|> Tree.foldTree (fun l x r -> l + x + r) 0 // returns 10

Title: Re: Kd Tree
Post by: gile on April 27, 2013, 01:59:35 PM

Hi kaefer,

It seems to me that all the tree handling functions are tail recursive using an accumulator (C# translations with IlSpy or Reflector shows a 'while' statements for all).

Anyway, thanks for the interesting links (despite the fact that all the tests I did with continuations shows a significant loss of performances).

Title: Re: Kd Tree
Post by: kaefer on April 27, 2013, 02:17:41 PM

Quote from: gile on April 27, 2013, 01:59:35 PM

It seems to me that all the tree handling functions are tail recursive using an accumulator (C# translations with IlSpy or Reflector shows a 'while' statements for all).

Yes. And no, if I'm not mistaken. Can we agree on them being both? E.g.

Code - C#: [Select]

internal Tuple<FSharpList<TreeNode>, FSharpList<Pair<Point3d, Point3d>>> getConnexions(TreeNode node, double radius, FSharpList<TreeNode> nodes, FSharpList<Pair<Point3d, Point3d>> pairs)
{
        while (node is TreeNode.Node)
        {
                ...
                Tuple<FSharpList<TreeNode>, FSharpList<Pair<Point3d, Point3d>>> connexions = this.getConnexions(item3, radius, nodes2, pairs2);
                ...

The call on the right sub-tree gets converted to a while loop, but the one on the left does not, potentially blowing the stack. Anyway, I think the point is moot when dealing with balanced trees, and its performance implications would preclude a theoretically sound approach.

TheSwamp

Code Red => .NET => Topic started by: gile on April 01, 2013, 05:08:56 PM