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 );
for( int i = 0; i < l1; i++ )
left[i] = items[i].Value;
++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 );
}
}
}