namespace Gile.Point3dTree
open System
open Autodesk.AutoCAD.Geometry
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