diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IPriorityQueue.cs.meta b/Assets/KdTreeLib/IPriorityQueue.cs.meta new file mode 100755 index 0000000..e37c0b2 --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0551921b902b13842aa2e99c95aa537c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IPriorityQueue.cs.meta b/Assets/KdTreeLib/IPriorityQueue.cs.meta new file mode 100755 index 0000000..e37c0b2 --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0551921b902b13842aa2e99c95aa537c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTree.cs b/Assets/KdTreeLib/KdTree.cs new file mode 100755 index 0000000..4dfe924 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs @@ -0,0 +1,676 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Runtime.Serialization.Formatters.Binary; +using System.Text; + +namespace KdTree +{ + public enum AddDuplicateBehavior + { + Skip, + Error, + Update + } + + public class DuplicateNodeError : Exception + { + public DuplicateNodeError() + : base("Cannot Add Node With Duplicate Coordinates") + { + } + } + + [Serializable] + public class KdTree : IKdTree + { + public KdTree(int dimensions, ITypeMath typeMath) + { + this.dimensions = dimensions; + this.typeMath = typeMath; + Count = 0; + } + + public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) + : this(dimensions, typeMath) + { + AddDuplicateBehavior = addDuplicateBehavior; + } + + private int dimensions; + + private ITypeMath typeMath = null; + + private KdTreeNode root = null; + + public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } + + public bool Add(TKey[] point, TValue value) + { + var nodeToAdd = new KdTreeNode(point, value); + + if (root == null) + { + root = new KdTreeNode(point, value); + } + else + { + int dimension = -1; + KdTreeNode parent = root; + + do + { + // Increment the dimension we're searching in + dimension = (dimension + 1) % dimensions; + + // Does the node we're adding have the same hyperpoint as this node? + if (typeMath.AreEqual(point, parent.Point)) + { + switch (AddDuplicateBehavior) + { + case AddDuplicateBehavior.Skip: + return false; + + case AddDuplicateBehavior.Error: + throw new DuplicateNodeError(); + + case AddDuplicateBehavior.Update: + parent.Value = value; + return true; + + default: + // Should never happen + throw new Exception("Unexpected AddDuplicateBehavior"); + } + } + + // Which side does this node sit under in relation to it's parent at this level? + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + + if (parent[compare] == null) + { + parent[compare] = nodeToAdd; + break; + } + else + { + parent = parent[compare]; + } + } + while (true); + } + + Count++; + return true; + } + + private void ReaddChildNodes(KdTreeNode removedNode) + { + if (removedNode.IsLeaf) + return; + + // The folllowing code might seem a little redundant but we're using + // 2 queues so we can add the child nodes back in, in (more or less) + // the same order they were added in the first place + var nodesToReadd = new Queue>(); + + var nodesToReaddQueue = new Queue>(); + + if (removedNode.LeftChild != null) + nodesToReaddQueue.Enqueue(removedNode.LeftChild); + + if (removedNode.RightChild != null) + nodesToReaddQueue.Enqueue(removedNode.RightChild); + + while (nodesToReaddQueue.Count > 0) + { + var nodeToReadd = nodesToReaddQueue.Dequeue(); + + nodesToReadd.Enqueue(nodeToReadd); + + for (int side = -1; side <= 1; side += 2) + { + if (nodeToReadd[side] != null) + { + nodesToReaddQueue.Enqueue(nodeToReadd[side]); + + nodeToReadd[side] = null; + } + } + } + + while (nodesToReadd.Count > 0) + { + var nodeToReadd = nodesToReadd.Dequeue(); + + Count--; + Add(nodeToReadd.Point, nodeToReadd.Value); + } + } + + public void RemoveAt(TKey[] point) + { + // Is tree empty? + if (root == null) + return; + + KdTreeNode node; + + if (typeMath.AreEqual(point, root.Point)) + { + node = root; + root = null; + Count--; + ReaddChildNodes(node); + return; + } + + node = root; + + int dimension = -1; + do + { + dimension = (dimension + 1) % dimensions; + + int compare = typeMath.Compare(point[dimension], node.Point[dimension]); + + if (node[compare] == null) + // Can't find node + return; + + if (typeMath.AreEqual(point, node[compare].Point)) + { + var nodeToRemove = node[compare]; + node[compare] = null; + Count--; + + ReaddChildNodes(nodeToRemove); + } + else + node = node[compare]; + } + while (node != null); + } + + public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) + { + if (count > Count) + count = Count; + + if (count < 0) + { + throw new ArgumentException("Number of neighbors cannot be negative"); + } + + if (count == 0) + return new KdTreeNode[0]; + + var neighbours = new KdTreeNode[count]; + + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + + var rect = HyperRect.Infinite(dimensions, typeMath); + + AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); + + count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + /* + * 1. Search for the target + * + * 1.1 Start by splitting the specified hyper rect + * on the specified node's point along the current + * dimension so that we end up with 2 sub hyper rects + * (current dimension = depth % dimensions) + * + * 1.2 Check what sub rectangle the the target point resides in + * under the current dimension + * + * 1.3 Set that rect to the nearer rect and also the corresponding + * child node to the nearest rect and node and the other rect + * and child node to the further rect and child node (for use later) + * + * 1.4 Travel into the nearer rect and node by calling function + * recursively with nearer rect and node and incrementing + * the depth + * + * 2. Add leaf to list of nearest neighbours + * + * 3. Walk back up tree and at each level: + * + * 3.1 Add node to nearest neighbours if + * we haven't filled our nearest neighbour + * list yet or if it has a distance to target less + * than any of the distances in our current nearest + * neighbours. + * + * 3.2 If there is any point in the further rectangle that is closer to + * the target than our furtherest nearest neighbour then travel into + * that rect and node + * + * That's it, when it finally finishes traversing the branches + * it needs to we'll have our list! + */ + + private void AddNearestNeighbours( + KdTreeNode node, + TKey[] target, + HyperRect rect, + int depth, + NearestNeighbourList, TKey> nearestNeighbours, + TKey maxSearchRadiusSquared) + { + if (node == null) + return; + + // Work out the current dimension + int dimension = depth % dimensions; + + // Split our hyper-rect into 2 sub rects along the current + // node's point on the current dimension + var leftRect = rect.Clone(); + leftRect.MaxPoint[dimension] = node.Point[dimension]; + + var rightRect = rect.Clone(); + rightRect.MinPoint[dimension] = node.Point[dimension]; + + // Which side does the target reside in? + int compare = typeMath.Compare(target[dimension], node.Point[dimension]); + + var nearerRect = compare <= 0 ? leftRect : rightRect; + var furtherRect = compare <= 0 ? rightRect : leftRect; + + var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; + var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; + + // Let's walk down into the nearer branch + if (nearerNode != null) + { + AddNearestNeighbours( + nearerNode, + target, + nearerRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + + TKey distanceSquaredToTarget; + + // Walk down into the further branch but only if our capacity hasn't been reached + // OR if there's a region in the further rect that's closer to the target than our + // current furtherest nearest neighbour + TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + { + if (nearestNeighbours.IsCapacityReached) + { + if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + else + { + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + } + + // Try to add the current node to our nearest neighbours list + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + nearestNeighbours.Add(node, distanceSquaredToTarget); + } + + /// + /// Performs a radial search. + /// + /// Center point + /// Radius to find neighbours within + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + /// + /// Performs a radial search up to a maximum count. + /// + /// Center point + /// Radius to find neighbours within + /// Maximum number of neighbours + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + private KdTreeNode[] RadialSearch(TKey[] center, TKey radius, NearestNeighbourList, TKey> nearestNeighbours) + { + AddNearestNeighbours( + root, + center, + HyperRect.Infinite(dimensions, typeMath), + 0, + nearestNeighbours, + typeMath.Multiply(radius, radius)); + + var count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + public int Count { get; private set; } + + public bool TryFindValueAt(TKey[] point, out TValue value) + { + var parent = root; + int dimension = -1; + do + { + if (parent == null) + { + value = default(TValue); + return false; + } + else if (typeMath.AreEqual(point, parent.Point)) + { + value = parent.Value; + return true; + } + + // Keep searching + dimension = (dimension + 1) % dimensions; + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + parent = parent[compare]; + } + while (true); + } + + public TValue FindValueAt(TKey[] point) + { + if (TryFindValueAt(point, out TValue value)) + return value; + else + return default(TValue); + } + + public bool TryFindValue(TValue value, out TKey[] point) + { + if (root == null) + { + point = null; + return false; + } + + // First-in, First-out list of nodes to search + var nodesToSearch = new Queue>(); + + nodesToSearch.Enqueue(root); + + while (nodesToSearch.Count > 0) + { + var nodeToSearch = nodesToSearch.Dequeue(); + + if (nodeToSearch.Value.Equals(value)) + { + point = nodeToSearch.Point; + return true; + } + else + { + for (int side = -1; side <= 1; side += 2) + { + var childNode = nodeToSearch[side]; + + if (childNode != null) + nodesToSearch.Enqueue(childNode); + } + } + } + + point = null; + return false; + } + + public TKey[] FindValue(TValue value) + { + if (TryFindValue(value, out TKey[] point)) + return point; + else + return null; + } + + private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) + { + sb.AppendLine(node.ToString()); + + for (var side = -1; side <= 1; side += 2) + { + for (var index = 0; index <= depth; index++) + sb.Append("\t"); + + sb.Append(side == -1 ? "L " : "R "); + + if (node[side] == null) + sb.AppendLine(""); + else + AddNodeToStringBuilder(node[side], sb, depth + 1); + } + } + + public override string ToString() + { + if (root == null) + return ""; + + var sb = new StringBuilder(); + AddNodeToStringBuilder(root, sb, 0); + return sb.ToString(); + } + + private void AddNodesToList(KdTreeNode node, List> nodes) + { + if (node == null) + return; + + nodes.Add(node); + + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + AddNodesToList(node[side], nodes); + node[side] = null; + } + } + } + + private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + for (var index = fromIndex + 1; index <= toIndex; index++) + { + var newIndex = index; + + while (true) + { + var a = nodes[newIndex - 1]; + var b = nodes[newIndex]; + if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) + { + nodes[newIndex - 1] = b; + nodes[newIndex] = a; + } + else + break; + } + } + } + + private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + if (fromIndex == toIndex) + { + Add(nodes[fromIndex].Point, nodes[fromIndex].Value); + nodes[fromIndex] = null; + return; + } + + // Sort the array from the fromIndex to the toIndex + SortNodesArray(nodes, byDimension, fromIndex, toIndex); + + // Find the splitting point + int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; + + // Add the splitting point + Add(nodes[midIndex].Point, nodes[midIndex].Value); + nodes[midIndex] = null; + + // Recurse + int nextDimension = (byDimension + 1) % dimensions; + + if (fromIndex < midIndex) + AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); + + if (toIndex > midIndex) + AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); + } + + public void Balance() + { + var nodeList = new List>(); + AddNodesToList(root, nodeList); + + Clear(); + + AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); + } + + private void RemoveChildNodes(KdTreeNode node) + { + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + RemoveChildNodes(node[side]); + node[side] = null; + } + } + } + + public void Clear() + { + if (root != null) + RemoveChildNodes(root); + } + + public void SaveToFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Create(filename)) + { + formatter.Serialize(stream, this); + stream.Flush(); + } + } + + public static KdTree LoadFromFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Open(filename, FileMode.Open)) + { + return (KdTree)formatter.Deserialize(stream); + } + + } + + public IEnumerator> GetEnumerator() + { + var left = new Stack>(); + var right = new Stack>(); + + void addLeft(KdTreeNode node) + { + if (node.LeftChild != null) + { + left.Push(node.LeftChild); + } + } + + void addRight(KdTreeNode node) + { + if (node.RightChild != null) + { + right.Push(node.RightChild); + } + } + + if (root != null) + { + yield return root; + + addLeft(root); + addRight(root); + + while (true) + { + if (left.Any()) + { + var item = left.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else if (right.Any()) + { + var item = right.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else + { + break; + } + } + } + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IPriorityQueue.cs.meta b/Assets/KdTreeLib/IPriorityQueue.cs.meta new file mode 100755 index 0000000..e37c0b2 --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0551921b902b13842aa2e99c95aa537c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTree.cs b/Assets/KdTreeLib/KdTree.cs new file mode 100755 index 0000000..4dfe924 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs @@ -0,0 +1,676 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Runtime.Serialization.Formatters.Binary; +using System.Text; + +namespace KdTree +{ + public enum AddDuplicateBehavior + { + Skip, + Error, + Update + } + + public class DuplicateNodeError : Exception + { + public DuplicateNodeError() + : base("Cannot Add Node With Duplicate Coordinates") + { + } + } + + [Serializable] + public class KdTree : IKdTree + { + public KdTree(int dimensions, ITypeMath typeMath) + { + this.dimensions = dimensions; + this.typeMath = typeMath; + Count = 0; + } + + public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) + : this(dimensions, typeMath) + { + AddDuplicateBehavior = addDuplicateBehavior; + } + + private int dimensions; + + private ITypeMath typeMath = null; + + private KdTreeNode root = null; + + public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } + + public bool Add(TKey[] point, TValue value) + { + var nodeToAdd = new KdTreeNode(point, value); + + if (root == null) + { + root = new KdTreeNode(point, value); + } + else + { + int dimension = -1; + KdTreeNode parent = root; + + do + { + // Increment the dimension we're searching in + dimension = (dimension + 1) % dimensions; + + // Does the node we're adding have the same hyperpoint as this node? + if (typeMath.AreEqual(point, parent.Point)) + { + switch (AddDuplicateBehavior) + { + case AddDuplicateBehavior.Skip: + return false; + + case AddDuplicateBehavior.Error: + throw new DuplicateNodeError(); + + case AddDuplicateBehavior.Update: + parent.Value = value; + return true; + + default: + // Should never happen + throw new Exception("Unexpected AddDuplicateBehavior"); + } + } + + // Which side does this node sit under in relation to it's parent at this level? + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + + if (parent[compare] == null) + { + parent[compare] = nodeToAdd; + break; + } + else + { + parent = parent[compare]; + } + } + while (true); + } + + Count++; + return true; + } + + private void ReaddChildNodes(KdTreeNode removedNode) + { + if (removedNode.IsLeaf) + return; + + // The folllowing code might seem a little redundant but we're using + // 2 queues so we can add the child nodes back in, in (more or less) + // the same order they were added in the first place + var nodesToReadd = new Queue>(); + + var nodesToReaddQueue = new Queue>(); + + if (removedNode.LeftChild != null) + nodesToReaddQueue.Enqueue(removedNode.LeftChild); + + if (removedNode.RightChild != null) + nodesToReaddQueue.Enqueue(removedNode.RightChild); + + while (nodesToReaddQueue.Count > 0) + { + var nodeToReadd = nodesToReaddQueue.Dequeue(); + + nodesToReadd.Enqueue(nodeToReadd); + + for (int side = -1; side <= 1; side += 2) + { + if (nodeToReadd[side] != null) + { + nodesToReaddQueue.Enqueue(nodeToReadd[side]); + + nodeToReadd[side] = null; + } + } + } + + while (nodesToReadd.Count > 0) + { + var nodeToReadd = nodesToReadd.Dequeue(); + + Count--; + Add(nodeToReadd.Point, nodeToReadd.Value); + } + } + + public void RemoveAt(TKey[] point) + { + // Is tree empty? + if (root == null) + return; + + KdTreeNode node; + + if (typeMath.AreEqual(point, root.Point)) + { + node = root; + root = null; + Count--; + ReaddChildNodes(node); + return; + } + + node = root; + + int dimension = -1; + do + { + dimension = (dimension + 1) % dimensions; + + int compare = typeMath.Compare(point[dimension], node.Point[dimension]); + + if (node[compare] == null) + // Can't find node + return; + + if (typeMath.AreEqual(point, node[compare].Point)) + { + var nodeToRemove = node[compare]; + node[compare] = null; + Count--; + + ReaddChildNodes(nodeToRemove); + } + else + node = node[compare]; + } + while (node != null); + } + + public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) + { + if (count > Count) + count = Count; + + if (count < 0) + { + throw new ArgumentException("Number of neighbors cannot be negative"); + } + + if (count == 0) + return new KdTreeNode[0]; + + var neighbours = new KdTreeNode[count]; + + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + + var rect = HyperRect.Infinite(dimensions, typeMath); + + AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); + + count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + /* + * 1. Search for the target + * + * 1.1 Start by splitting the specified hyper rect + * on the specified node's point along the current + * dimension so that we end up with 2 sub hyper rects + * (current dimension = depth % dimensions) + * + * 1.2 Check what sub rectangle the the target point resides in + * under the current dimension + * + * 1.3 Set that rect to the nearer rect and also the corresponding + * child node to the nearest rect and node and the other rect + * and child node to the further rect and child node (for use later) + * + * 1.4 Travel into the nearer rect and node by calling function + * recursively with nearer rect and node and incrementing + * the depth + * + * 2. Add leaf to list of nearest neighbours + * + * 3. Walk back up tree and at each level: + * + * 3.1 Add node to nearest neighbours if + * we haven't filled our nearest neighbour + * list yet or if it has a distance to target less + * than any of the distances in our current nearest + * neighbours. + * + * 3.2 If there is any point in the further rectangle that is closer to + * the target than our furtherest nearest neighbour then travel into + * that rect and node + * + * That's it, when it finally finishes traversing the branches + * it needs to we'll have our list! + */ + + private void AddNearestNeighbours( + KdTreeNode node, + TKey[] target, + HyperRect rect, + int depth, + NearestNeighbourList, TKey> nearestNeighbours, + TKey maxSearchRadiusSquared) + { + if (node == null) + return; + + // Work out the current dimension + int dimension = depth % dimensions; + + // Split our hyper-rect into 2 sub rects along the current + // node's point on the current dimension + var leftRect = rect.Clone(); + leftRect.MaxPoint[dimension] = node.Point[dimension]; + + var rightRect = rect.Clone(); + rightRect.MinPoint[dimension] = node.Point[dimension]; + + // Which side does the target reside in? + int compare = typeMath.Compare(target[dimension], node.Point[dimension]); + + var nearerRect = compare <= 0 ? leftRect : rightRect; + var furtherRect = compare <= 0 ? rightRect : leftRect; + + var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; + var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; + + // Let's walk down into the nearer branch + if (nearerNode != null) + { + AddNearestNeighbours( + nearerNode, + target, + nearerRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + + TKey distanceSquaredToTarget; + + // Walk down into the further branch but only if our capacity hasn't been reached + // OR if there's a region in the further rect that's closer to the target than our + // current furtherest nearest neighbour + TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + { + if (nearestNeighbours.IsCapacityReached) + { + if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + else + { + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + } + + // Try to add the current node to our nearest neighbours list + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + nearestNeighbours.Add(node, distanceSquaredToTarget); + } + + /// + /// Performs a radial search. + /// + /// Center point + /// Radius to find neighbours within + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + /// + /// Performs a radial search up to a maximum count. + /// + /// Center point + /// Radius to find neighbours within + /// Maximum number of neighbours + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + private KdTreeNode[] RadialSearch(TKey[] center, TKey radius, NearestNeighbourList, TKey> nearestNeighbours) + { + AddNearestNeighbours( + root, + center, + HyperRect.Infinite(dimensions, typeMath), + 0, + nearestNeighbours, + typeMath.Multiply(radius, radius)); + + var count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + public int Count { get; private set; } + + public bool TryFindValueAt(TKey[] point, out TValue value) + { + var parent = root; + int dimension = -1; + do + { + if (parent == null) + { + value = default(TValue); + return false; + } + else if (typeMath.AreEqual(point, parent.Point)) + { + value = parent.Value; + return true; + } + + // Keep searching + dimension = (dimension + 1) % dimensions; + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + parent = parent[compare]; + } + while (true); + } + + public TValue FindValueAt(TKey[] point) + { + if (TryFindValueAt(point, out TValue value)) + return value; + else + return default(TValue); + } + + public bool TryFindValue(TValue value, out TKey[] point) + { + if (root == null) + { + point = null; + return false; + } + + // First-in, First-out list of nodes to search + var nodesToSearch = new Queue>(); + + nodesToSearch.Enqueue(root); + + while (nodesToSearch.Count > 0) + { + var nodeToSearch = nodesToSearch.Dequeue(); + + if (nodeToSearch.Value.Equals(value)) + { + point = nodeToSearch.Point; + return true; + } + else + { + for (int side = -1; side <= 1; side += 2) + { + var childNode = nodeToSearch[side]; + + if (childNode != null) + nodesToSearch.Enqueue(childNode); + } + } + } + + point = null; + return false; + } + + public TKey[] FindValue(TValue value) + { + if (TryFindValue(value, out TKey[] point)) + return point; + else + return null; + } + + private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) + { + sb.AppendLine(node.ToString()); + + for (var side = -1; side <= 1; side += 2) + { + for (var index = 0; index <= depth; index++) + sb.Append("\t"); + + sb.Append(side == -1 ? "L " : "R "); + + if (node[side] == null) + sb.AppendLine(""); + else + AddNodeToStringBuilder(node[side], sb, depth + 1); + } + } + + public override string ToString() + { + if (root == null) + return ""; + + var sb = new StringBuilder(); + AddNodeToStringBuilder(root, sb, 0); + return sb.ToString(); + } + + private void AddNodesToList(KdTreeNode node, List> nodes) + { + if (node == null) + return; + + nodes.Add(node); + + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + AddNodesToList(node[side], nodes); + node[side] = null; + } + } + } + + private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + for (var index = fromIndex + 1; index <= toIndex; index++) + { + var newIndex = index; + + while (true) + { + var a = nodes[newIndex - 1]; + var b = nodes[newIndex]; + if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) + { + nodes[newIndex - 1] = b; + nodes[newIndex] = a; + } + else + break; + } + } + } + + private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + if (fromIndex == toIndex) + { + Add(nodes[fromIndex].Point, nodes[fromIndex].Value); + nodes[fromIndex] = null; + return; + } + + // Sort the array from the fromIndex to the toIndex + SortNodesArray(nodes, byDimension, fromIndex, toIndex); + + // Find the splitting point + int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; + + // Add the splitting point + Add(nodes[midIndex].Point, nodes[midIndex].Value); + nodes[midIndex] = null; + + // Recurse + int nextDimension = (byDimension + 1) % dimensions; + + if (fromIndex < midIndex) + AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); + + if (toIndex > midIndex) + AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); + } + + public void Balance() + { + var nodeList = new List>(); + AddNodesToList(root, nodeList); + + Clear(); + + AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); + } + + private void RemoveChildNodes(KdTreeNode node) + { + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + RemoveChildNodes(node[side]); + node[side] = null; + } + } + } + + public void Clear() + { + if (root != null) + RemoveChildNodes(root); + } + + public void SaveToFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Create(filename)) + { + formatter.Serialize(stream, this); + stream.Flush(); + } + } + + public static KdTree LoadFromFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Open(filename, FileMode.Open)) + { + return (KdTree)formatter.Deserialize(stream); + } + + } + + public IEnumerator> GetEnumerator() + { + var left = new Stack>(); + var right = new Stack>(); + + void addLeft(KdTreeNode node) + { + if (node.LeftChild != null) + { + left.Push(node.LeftChild); + } + } + + void addRight(KdTreeNode node) + { + if (node.RightChild != null) + { + right.Push(node.RightChild); + } + } + + if (root != null) + { + yield return root; + + addLeft(root); + addRight(root); + + while (true) + { + if (left.Any()) + { + var item = left.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else if (right.Any()) + { + var item = right.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else + { + break; + } + } + } + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTree.cs.meta b/Assets/KdTreeLib/KdTree.cs.meta new file mode 100755 index 0000000..d7f1d14 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b61eae78ccd40f0459eb72df19d37003 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IPriorityQueue.cs.meta b/Assets/KdTreeLib/IPriorityQueue.cs.meta new file mode 100755 index 0000000..e37c0b2 --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0551921b902b13842aa2e99c95aa537c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTree.cs b/Assets/KdTreeLib/KdTree.cs new file mode 100755 index 0000000..4dfe924 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs @@ -0,0 +1,676 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Runtime.Serialization.Formatters.Binary; +using System.Text; + +namespace KdTree +{ + public enum AddDuplicateBehavior + { + Skip, + Error, + Update + } + + public class DuplicateNodeError : Exception + { + public DuplicateNodeError() + : base("Cannot Add Node With Duplicate Coordinates") + { + } + } + + [Serializable] + public class KdTree : IKdTree + { + public KdTree(int dimensions, ITypeMath typeMath) + { + this.dimensions = dimensions; + this.typeMath = typeMath; + Count = 0; + } + + public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) + : this(dimensions, typeMath) + { + AddDuplicateBehavior = addDuplicateBehavior; + } + + private int dimensions; + + private ITypeMath typeMath = null; + + private KdTreeNode root = null; + + public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } + + public bool Add(TKey[] point, TValue value) + { + var nodeToAdd = new KdTreeNode(point, value); + + if (root == null) + { + root = new KdTreeNode(point, value); + } + else + { + int dimension = -1; + KdTreeNode parent = root; + + do + { + // Increment the dimension we're searching in + dimension = (dimension + 1) % dimensions; + + // Does the node we're adding have the same hyperpoint as this node? + if (typeMath.AreEqual(point, parent.Point)) + { + switch (AddDuplicateBehavior) + { + case AddDuplicateBehavior.Skip: + return false; + + case AddDuplicateBehavior.Error: + throw new DuplicateNodeError(); + + case AddDuplicateBehavior.Update: + parent.Value = value; + return true; + + default: + // Should never happen + throw new Exception("Unexpected AddDuplicateBehavior"); + } + } + + // Which side does this node sit under in relation to it's parent at this level? + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + + if (parent[compare] == null) + { + parent[compare] = nodeToAdd; + break; + } + else + { + parent = parent[compare]; + } + } + while (true); + } + + Count++; + return true; + } + + private void ReaddChildNodes(KdTreeNode removedNode) + { + if (removedNode.IsLeaf) + return; + + // The folllowing code might seem a little redundant but we're using + // 2 queues so we can add the child nodes back in, in (more or less) + // the same order they were added in the first place + var nodesToReadd = new Queue>(); + + var nodesToReaddQueue = new Queue>(); + + if (removedNode.LeftChild != null) + nodesToReaddQueue.Enqueue(removedNode.LeftChild); + + if (removedNode.RightChild != null) + nodesToReaddQueue.Enqueue(removedNode.RightChild); + + while (nodesToReaddQueue.Count > 0) + { + var nodeToReadd = nodesToReaddQueue.Dequeue(); + + nodesToReadd.Enqueue(nodeToReadd); + + for (int side = -1; side <= 1; side += 2) + { + if (nodeToReadd[side] != null) + { + nodesToReaddQueue.Enqueue(nodeToReadd[side]); + + nodeToReadd[side] = null; + } + } + } + + while (nodesToReadd.Count > 0) + { + var nodeToReadd = nodesToReadd.Dequeue(); + + Count--; + Add(nodeToReadd.Point, nodeToReadd.Value); + } + } + + public void RemoveAt(TKey[] point) + { + // Is tree empty? + if (root == null) + return; + + KdTreeNode node; + + if (typeMath.AreEqual(point, root.Point)) + { + node = root; + root = null; + Count--; + ReaddChildNodes(node); + return; + } + + node = root; + + int dimension = -1; + do + { + dimension = (dimension + 1) % dimensions; + + int compare = typeMath.Compare(point[dimension], node.Point[dimension]); + + if (node[compare] == null) + // Can't find node + return; + + if (typeMath.AreEqual(point, node[compare].Point)) + { + var nodeToRemove = node[compare]; + node[compare] = null; + Count--; + + ReaddChildNodes(nodeToRemove); + } + else + node = node[compare]; + } + while (node != null); + } + + public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) + { + if (count > Count) + count = Count; + + if (count < 0) + { + throw new ArgumentException("Number of neighbors cannot be negative"); + } + + if (count == 0) + return new KdTreeNode[0]; + + var neighbours = new KdTreeNode[count]; + + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + + var rect = HyperRect.Infinite(dimensions, typeMath); + + AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); + + count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + /* + * 1. Search for the target + * + * 1.1 Start by splitting the specified hyper rect + * on the specified node's point along the current + * dimension so that we end up with 2 sub hyper rects + * (current dimension = depth % dimensions) + * + * 1.2 Check what sub rectangle the the target point resides in + * under the current dimension + * + * 1.3 Set that rect to the nearer rect and also the corresponding + * child node to the nearest rect and node and the other rect + * and child node to the further rect and child node (for use later) + * + * 1.4 Travel into the nearer rect and node by calling function + * recursively with nearer rect and node and incrementing + * the depth + * + * 2. Add leaf to list of nearest neighbours + * + * 3. Walk back up tree and at each level: + * + * 3.1 Add node to nearest neighbours if + * we haven't filled our nearest neighbour + * list yet or if it has a distance to target less + * than any of the distances in our current nearest + * neighbours. + * + * 3.2 If there is any point in the further rectangle that is closer to + * the target than our furtherest nearest neighbour then travel into + * that rect and node + * + * That's it, when it finally finishes traversing the branches + * it needs to we'll have our list! + */ + + private void AddNearestNeighbours( + KdTreeNode node, + TKey[] target, + HyperRect rect, + int depth, + NearestNeighbourList, TKey> nearestNeighbours, + TKey maxSearchRadiusSquared) + { + if (node == null) + return; + + // Work out the current dimension + int dimension = depth % dimensions; + + // Split our hyper-rect into 2 sub rects along the current + // node's point on the current dimension + var leftRect = rect.Clone(); + leftRect.MaxPoint[dimension] = node.Point[dimension]; + + var rightRect = rect.Clone(); + rightRect.MinPoint[dimension] = node.Point[dimension]; + + // Which side does the target reside in? + int compare = typeMath.Compare(target[dimension], node.Point[dimension]); + + var nearerRect = compare <= 0 ? leftRect : rightRect; + var furtherRect = compare <= 0 ? rightRect : leftRect; + + var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; + var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; + + // Let's walk down into the nearer branch + if (nearerNode != null) + { + AddNearestNeighbours( + nearerNode, + target, + nearerRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + + TKey distanceSquaredToTarget; + + // Walk down into the further branch but only if our capacity hasn't been reached + // OR if there's a region in the further rect that's closer to the target than our + // current furtherest nearest neighbour + TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + { + if (nearestNeighbours.IsCapacityReached) + { + if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + else + { + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + } + + // Try to add the current node to our nearest neighbours list + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + nearestNeighbours.Add(node, distanceSquaredToTarget); + } + + /// + /// Performs a radial search. + /// + /// Center point + /// Radius to find neighbours within + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + /// + /// Performs a radial search up to a maximum count. + /// + /// Center point + /// Radius to find neighbours within + /// Maximum number of neighbours + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + private KdTreeNode[] RadialSearch(TKey[] center, TKey radius, NearestNeighbourList, TKey> nearestNeighbours) + { + AddNearestNeighbours( + root, + center, + HyperRect.Infinite(dimensions, typeMath), + 0, + nearestNeighbours, + typeMath.Multiply(radius, radius)); + + var count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + public int Count { get; private set; } + + public bool TryFindValueAt(TKey[] point, out TValue value) + { + var parent = root; + int dimension = -1; + do + { + if (parent == null) + { + value = default(TValue); + return false; + } + else if (typeMath.AreEqual(point, parent.Point)) + { + value = parent.Value; + return true; + } + + // Keep searching + dimension = (dimension + 1) % dimensions; + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + parent = parent[compare]; + } + while (true); + } + + public TValue FindValueAt(TKey[] point) + { + if (TryFindValueAt(point, out TValue value)) + return value; + else + return default(TValue); + } + + public bool TryFindValue(TValue value, out TKey[] point) + { + if (root == null) + { + point = null; + return false; + } + + // First-in, First-out list of nodes to search + var nodesToSearch = new Queue>(); + + nodesToSearch.Enqueue(root); + + while (nodesToSearch.Count > 0) + { + var nodeToSearch = nodesToSearch.Dequeue(); + + if (nodeToSearch.Value.Equals(value)) + { + point = nodeToSearch.Point; + return true; + } + else + { + for (int side = -1; side <= 1; side += 2) + { + var childNode = nodeToSearch[side]; + + if (childNode != null) + nodesToSearch.Enqueue(childNode); + } + } + } + + point = null; + return false; + } + + public TKey[] FindValue(TValue value) + { + if (TryFindValue(value, out TKey[] point)) + return point; + else + return null; + } + + private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) + { + sb.AppendLine(node.ToString()); + + for (var side = -1; side <= 1; side += 2) + { + for (var index = 0; index <= depth; index++) + sb.Append("\t"); + + sb.Append(side == -1 ? "L " : "R "); + + if (node[side] == null) + sb.AppendLine(""); + else + AddNodeToStringBuilder(node[side], sb, depth + 1); + } + } + + public override string ToString() + { + if (root == null) + return ""; + + var sb = new StringBuilder(); + AddNodeToStringBuilder(root, sb, 0); + return sb.ToString(); + } + + private void AddNodesToList(KdTreeNode node, List> nodes) + { + if (node == null) + return; + + nodes.Add(node); + + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + AddNodesToList(node[side], nodes); + node[side] = null; + } + } + } + + private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + for (var index = fromIndex + 1; index <= toIndex; index++) + { + var newIndex = index; + + while (true) + { + var a = nodes[newIndex - 1]; + var b = nodes[newIndex]; + if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) + { + nodes[newIndex - 1] = b; + nodes[newIndex] = a; + } + else + break; + } + } + } + + private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + if (fromIndex == toIndex) + { + Add(nodes[fromIndex].Point, nodes[fromIndex].Value); + nodes[fromIndex] = null; + return; + } + + // Sort the array from the fromIndex to the toIndex + SortNodesArray(nodes, byDimension, fromIndex, toIndex); + + // Find the splitting point + int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; + + // Add the splitting point + Add(nodes[midIndex].Point, nodes[midIndex].Value); + nodes[midIndex] = null; + + // Recurse + int nextDimension = (byDimension + 1) % dimensions; + + if (fromIndex < midIndex) + AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); + + if (toIndex > midIndex) + AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); + } + + public void Balance() + { + var nodeList = new List>(); + AddNodesToList(root, nodeList); + + Clear(); + + AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); + } + + private void RemoveChildNodes(KdTreeNode node) + { + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + RemoveChildNodes(node[side]); + node[side] = null; + } + } + } + + public void Clear() + { + if (root != null) + RemoveChildNodes(root); + } + + public void SaveToFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Create(filename)) + { + formatter.Serialize(stream, this); + stream.Flush(); + } + } + + public static KdTree LoadFromFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Open(filename, FileMode.Open)) + { + return (KdTree)formatter.Deserialize(stream); + } + + } + + public IEnumerator> GetEnumerator() + { + var left = new Stack>(); + var right = new Stack>(); + + void addLeft(KdTreeNode node) + { + if (node.LeftChild != null) + { + left.Push(node.LeftChild); + } + } + + void addRight(KdTreeNode node) + { + if (node.RightChild != null) + { + right.Push(node.RightChild); + } + } + + if (root != null) + { + yield return root; + + addLeft(root); + addRight(root); + + while (true) + { + if (left.Any()) + { + var item = left.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else if (right.Any()) + { + var item = right.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else + { + break; + } + } + } + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTree.cs.meta b/Assets/KdTreeLib/KdTree.cs.meta new file mode 100755 index 0000000..d7f1d14 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b61eae78ccd40f0459eb72df19d37003 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTreeNode.cs b/Assets/KdTreeLib/KdTreeNode.cs new file mode 100755 index 0000000..e08f24d --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs @@ -0,0 +1,68 @@ +using System; +using System.Text; + +namespace KdTree +{ + [Serializable] + public class KdTreeNode + { + public KdTreeNode() + { + } + + public KdTreeNode(TKey[] point, TValue value) + { + Point = point; + Value = value; + } + + public TKey[] Point; + public TValue Value = default(TValue); + + internal KdTreeNode LeftChild = null; + internal KdTreeNode RightChild = null; + + internal KdTreeNode this[int compare] + { + get + { + if (compare <= 0) + return LeftChild; + else + return RightChild; + } + set + { + if (compare <= 0) + LeftChild = value; + else + RightChild = value; + } + } + + public bool IsLeaf + { + get + { + return (LeftChild == null) && (RightChild == null); + } + } + + public override string ToString() + { + var sb = new StringBuilder(); + + for (var dimension = 0; dimension < Point.Length; dimension++) + { + sb.Append(Point[dimension].ToString() + "\t"); + } + + if (Value == null) + sb.Append("null"); + else + sb.Append(Value.ToString()); + + return sb.ToString(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IPriorityQueue.cs.meta b/Assets/KdTreeLib/IPriorityQueue.cs.meta new file mode 100755 index 0000000..e37c0b2 --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0551921b902b13842aa2e99c95aa537c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTree.cs b/Assets/KdTreeLib/KdTree.cs new file mode 100755 index 0000000..4dfe924 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs @@ -0,0 +1,676 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Runtime.Serialization.Formatters.Binary; +using System.Text; + +namespace KdTree +{ + public enum AddDuplicateBehavior + { + Skip, + Error, + Update + } + + public class DuplicateNodeError : Exception + { + public DuplicateNodeError() + : base("Cannot Add Node With Duplicate Coordinates") + { + } + } + + [Serializable] + public class KdTree : IKdTree + { + public KdTree(int dimensions, ITypeMath typeMath) + { + this.dimensions = dimensions; + this.typeMath = typeMath; + Count = 0; + } + + public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) + : this(dimensions, typeMath) + { + AddDuplicateBehavior = addDuplicateBehavior; + } + + private int dimensions; + + private ITypeMath typeMath = null; + + private KdTreeNode root = null; + + public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } + + public bool Add(TKey[] point, TValue value) + { + var nodeToAdd = new KdTreeNode(point, value); + + if (root == null) + { + root = new KdTreeNode(point, value); + } + else + { + int dimension = -1; + KdTreeNode parent = root; + + do + { + // Increment the dimension we're searching in + dimension = (dimension + 1) % dimensions; + + // Does the node we're adding have the same hyperpoint as this node? + if (typeMath.AreEqual(point, parent.Point)) + { + switch (AddDuplicateBehavior) + { + case AddDuplicateBehavior.Skip: + return false; + + case AddDuplicateBehavior.Error: + throw new DuplicateNodeError(); + + case AddDuplicateBehavior.Update: + parent.Value = value; + return true; + + default: + // Should never happen + throw new Exception("Unexpected AddDuplicateBehavior"); + } + } + + // Which side does this node sit under in relation to it's parent at this level? + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + + if (parent[compare] == null) + { + parent[compare] = nodeToAdd; + break; + } + else + { + parent = parent[compare]; + } + } + while (true); + } + + Count++; + return true; + } + + private void ReaddChildNodes(KdTreeNode removedNode) + { + if (removedNode.IsLeaf) + return; + + // The folllowing code might seem a little redundant but we're using + // 2 queues so we can add the child nodes back in, in (more or less) + // the same order they were added in the first place + var nodesToReadd = new Queue>(); + + var nodesToReaddQueue = new Queue>(); + + if (removedNode.LeftChild != null) + nodesToReaddQueue.Enqueue(removedNode.LeftChild); + + if (removedNode.RightChild != null) + nodesToReaddQueue.Enqueue(removedNode.RightChild); + + while (nodesToReaddQueue.Count > 0) + { + var nodeToReadd = nodesToReaddQueue.Dequeue(); + + nodesToReadd.Enqueue(nodeToReadd); + + for (int side = -1; side <= 1; side += 2) + { + if (nodeToReadd[side] != null) + { + nodesToReaddQueue.Enqueue(nodeToReadd[side]); + + nodeToReadd[side] = null; + } + } + } + + while (nodesToReadd.Count > 0) + { + var nodeToReadd = nodesToReadd.Dequeue(); + + Count--; + Add(nodeToReadd.Point, nodeToReadd.Value); + } + } + + public void RemoveAt(TKey[] point) + { + // Is tree empty? + if (root == null) + return; + + KdTreeNode node; + + if (typeMath.AreEqual(point, root.Point)) + { + node = root; + root = null; + Count--; + ReaddChildNodes(node); + return; + } + + node = root; + + int dimension = -1; + do + { + dimension = (dimension + 1) % dimensions; + + int compare = typeMath.Compare(point[dimension], node.Point[dimension]); + + if (node[compare] == null) + // Can't find node + return; + + if (typeMath.AreEqual(point, node[compare].Point)) + { + var nodeToRemove = node[compare]; + node[compare] = null; + Count--; + + ReaddChildNodes(nodeToRemove); + } + else + node = node[compare]; + } + while (node != null); + } + + public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) + { + if (count > Count) + count = Count; + + if (count < 0) + { + throw new ArgumentException("Number of neighbors cannot be negative"); + } + + if (count == 0) + return new KdTreeNode[0]; + + var neighbours = new KdTreeNode[count]; + + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + + var rect = HyperRect.Infinite(dimensions, typeMath); + + AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); + + count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + /* + * 1. Search for the target + * + * 1.1 Start by splitting the specified hyper rect + * on the specified node's point along the current + * dimension so that we end up with 2 sub hyper rects + * (current dimension = depth % dimensions) + * + * 1.2 Check what sub rectangle the the target point resides in + * under the current dimension + * + * 1.3 Set that rect to the nearer rect and also the corresponding + * child node to the nearest rect and node and the other rect + * and child node to the further rect and child node (for use later) + * + * 1.4 Travel into the nearer rect and node by calling function + * recursively with nearer rect and node and incrementing + * the depth + * + * 2. Add leaf to list of nearest neighbours + * + * 3. Walk back up tree and at each level: + * + * 3.1 Add node to nearest neighbours if + * we haven't filled our nearest neighbour + * list yet or if it has a distance to target less + * than any of the distances in our current nearest + * neighbours. + * + * 3.2 If there is any point in the further rectangle that is closer to + * the target than our furtherest nearest neighbour then travel into + * that rect and node + * + * That's it, when it finally finishes traversing the branches + * it needs to we'll have our list! + */ + + private void AddNearestNeighbours( + KdTreeNode node, + TKey[] target, + HyperRect rect, + int depth, + NearestNeighbourList, TKey> nearestNeighbours, + TKey maxSearchRadiusSquared) + { + if (node == null) + return; + + // Work out the current dimension + int dimension = depth % dimensions; + + // Split our hyper-rect into 2 sub rects along the current + // node's point on the current dimension + var leftRect = rect.Clone(); + leftRect.MaxPoint[dimension] = node.Point[dimension]; + + var rightRect = rect.Clone(); + rightRect.MinPoint[dimension] = node.Point[dimension]; + + // Which side does the target reside in? + int compare = typeMath.Compare(target[dimension], node.Point[dimension]); + + var nearerRect = compare <= 0 ? leftRect : rightRect; + var furtherRect = compare <= 0 ? rightRect : leftRect; + + var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; + var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; + + // Let's walk down into the nearer branch + if (nearerNode != null) + { + AddNearestNeighbours( + nearerNode, + target, + nearerRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + + TKey distanceSquaredToTarget; + + // Walk down into the further branch but only if our capacity hasn't been reached + // OR if there's a region in the further rect that's closer to the target than our + // current furtherest nearest neighbour + TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + { + if (nearestNeighbours.IsCapacityReached) + { + if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + else + { + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + } + + // Try to add the current node to our nearest neighbours list + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + nearestNeighbours.Add(node, distanceSquaredToTarget); + } + + /// + /// Performs a radial search. + /// + /// Center point + /// Radius to find neighbours within + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + /// + /// Performs a radial search up to a maximum count. + /// + /// Center point + /// Radius to find neighbours within + /// Maximum number of neighbours + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + private KdTreeNode[] RadialSearch(TKey[] center, TKey radius, NearestNeighbourList, TKey> nearestNeighbours) + { + AddNearestNeighbours( + root, + center, + HyperRect.Infinite(dimensions, typeMath), + 0, + nearestNeighbours, + typeMath.Multiply(radius, radius)); + + var count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + public int Count { get; private set; } + + public bool TryFindValueAt(TKey[] point, out TValue value) + { + var parent = root; + int dimension = -1; + do + { + if (parent == null) + { + value = default(TValue); + return false; + } + else if (typeMath.AreEqual(point, parent.Point)) + { + value = parent.Value; + return true; + } + + // Keep searching + dimension = (dimension + 1) % dimensions; + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + parent = parent[compare]; + } + while (true); + } + + public TValue FindValueAt(TKey[] point) + { + if (TryFindValueAt(point, out TValue value)) + return value; + else + return default(TValue); + } + + public bool TryFindValue(TValue value, out TKey[] point) + { + if (root == null) + { + point = null; + return false; + } + + // First-in, First-out list of nodes to search + var nodesToSearch = new Queue>(); + + nodesToSearch.Enqueue(root); + + while (nodesToSearch.Count > 0) + { + var nodeToSearch = nodesToSearch.Dequeue(); + + if (nodeToSearch.Value.Equals(value)) + { + point = nodeToSearch.Point; + return true; + } + else + { + for (int side = -1; side <= 1; side += 2) + { + var childNode = nodeToSearch[side]; + + if (childNode != null) + nodesToSearch.Enqueue(childNode); + } + } + } + + point = null; + return false; + } + + public TKey[] FindValue(TValue value) + { + if (TryFindValue(value, out TKey[] point)) + return point; + else + return null; + } + + private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) + { + sb.AppendLine(node.ToString()); + + for (var side = -1; side <= 1; side += 2) + { + for (var index = 0; index <= depth; index++) + sb.Append("\t"); + + sb.Append(side == -1 ? "L " : "R "); + + if (node[side] == null) + sb.AppendLine(""); + else + AddNodeToStringBuilder(node[side], sb, depth + 1); + } + } + + public override string ToString() + { + if (root == null) + return ""; + + var sb = new StringBuilder(); + AddNodeToStringBuilder(root, sb, 0); + return sb.ToString(); + } + + private void AddNodesToList(KdTreeNode node, List> nodes) + { + if (node == null) + return; + + nodes.Add(node); + + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + AddNodesToList(node[side], nodes); + node[side] = null; + } + } + } + + private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + for (var index = fromIndex + 1; index <= toIndex; index++) + { + var newIndex = index; + + while (true) + { + var a = nodes[newIndex - 1]; + var b = nodes[newIndex]; + if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) + { + nodes[newIndex - 1] = b; + nodes[newIndex] = a; + } + else + break; + } + } + } + + private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + if (fromIndex == toIndex) + { + Add(nodes[fromIndex].Point, nodes[fromIndex].Value); + nodes[fromIndex] = null; + return; + } + + // Sort the array from the fromIndex to the toIndex + SortNodesArray(nodes, byDimension, fromIndex, toIndex); + + // Find the splitting point + int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; + + // Add the splitting point + Add(nodes[midIndex].Point, nodes[midIndex].Value); + nodes[midIndex] = null; + + // Recurse + int nextDimension = (byDimension + 1) % dimensions; + + if (fromIndex < midIndex) + AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); + + if (toIndex > midIndex) + AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); + } + + public void Balance() + { + var nodeList = new List>(); + AddNodesToList(root, nodeList); + + Clear(); + + AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); + } + + private void RemoveChildNodes(KdTreeNode node) + { + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + RemoveChildNodes(node[side]); + node[side] = null; + } + } + } + + public void Clear() + { + if (root != null) + RemoveChildNodes(root); + } + + public void SaveToFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Create(filename)) + { + formatter.Serialize(stream, this); + stream.Flush(); + } + } + + public static KdTree LoadFromFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Open(filename, FileMode.Open)) + { + return (KdTree)formatter.Deserialize(stream); + } + + } + + public IEnumerator> GetEnumerator() + { + var left = new Stack>(); + var right = new Stack>(); + + void addLeft(KdTreeNode node) + { + if (node.LeftChild != null) + { + left.Push(node.LeftChild); + } + } + + void addRight(KdTreeNode node) + { + if (node.RightChild != null) + { + right.Push(node.RightChild); + } + } + + if (root != null) + { + yield return root; + + addLeft(root); + addRight(root); + + while (true) + { + if (left.Any()) + { + var item = left.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else if (right.Any()) + { + var item = right.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else + { + break; + } + } + } + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTree.cs.meta b/Assets/KdTreeLib/KdTree.cs.meta new file mode 100755 index 0000000..d7f1d14 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b61eae78ccd40f0459eb72df19d37003 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTreeNode.cs b/Assets/KdTreeLib/KdTreeNode.cs new file mode 100755 index 0000000..e08f24d --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs @@ -0,0 +1,68 @@ +using System; +using System.Text; + +namespace KdTree +{ + [Serializable] + public class KdTreeNode + { + public KdTreeNode() + { + } + + public KdTreeNode(TKey[] point, TValue value) + { + Point = point; + Value = value; + } + + public TKey[] Point; + public TValue Value = default(TValue); + + internal KdTreeNode LeftChild = null; + internal KdTreeNode RightChild = null; + + internal KdTreeNode this[int compare] + { + get + { + if (compare <= 0) + return LeftChild; + else + return RightChild; + } + set + { + if (compare <= 0) + LeftChild = value; + else + RightChild = value; + } + } + + public bool IsLeaf + { + get + { + return (LeftChild == null) && (RightChild == null); + } + } + + public override string ToString() + { + var sb = new StringBuilder(); + + for (var dimension = 0; dimension < Point.Length; dimension++) + { + sb.Append(Point[dimension].ToString() + "\t"); + } + + if (Value == null) + sb.Append("null"); + else + sb.Append(Value.ToString()); + + return sb.ToString(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTreeNode.cs.meta b/Assets/KdTreeLib/KdTreeNode.cs.meta new file mode 100755 index 0000000..7fa00e6 --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c70bc7c92452a3b4e8de75cf4a32e85f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IPriorityQueue.cs.meta b/Assets/KdTreeLib/IPriorityQueue.cs.meta new file mode 100755 index 0000000..e37c0b2 --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0551921b902b13842aa2e99c95aa537c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTree.cs b/Assets/KdTreeLib/KdTree.cs new file mode 100755 index 0000000..4dfe924 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs @@ -0,0 +1,676 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Runtime.Serialization.Formatters.Binary; +using System.Text; + +namespace KdTree +{ + public enum AddDuplicateBehavior + { + Skip, + Error, + Update + } + + public class DuplicateNodeError : Exception + { + public DuplicateNodeError() + : base("Cannot Add Node With Duplicate Coordinates") + { + } + } + + [Serializable] + public class KdTree : IKdTree + { + public KdTree(int dimensions, ITypeMath typeMath) + { + this.dimensions = dimensions; + this.typeMath = typeMath; + Count = 0; + } + + public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) + : this(dimensions, typeMath) + { + AddDuplicateBehavior = addDuplicateBehavior; + } + + private int dimensions; + + private ITypeMath typeMath = null; + + private KdTreeNode root = null; + + public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } + + public bool Add(TKey[] point, TValue value) + { + var nodeToAdd = new KdTreeNode(point, value); + + if (root == null) + { + root = new KdTreeNode(point, value); + } + else + { + int dimension = -1; + KdTreeNode parent = root; + + do + { + // Increment the dimension we're searching in + dimension = (dimension + 1) % dimensions; + + // Does the node we're adding have the same hyperpoint as this node? + if (typeMath.AreEqual(point, parent.Point)) + { + switch (AddDuplicateBehavior) + { + case AddDuplicateBehavior.Skip: + return false; + + case AddDuplicateBehavior.Error: + throw new DuplicateNodeError(); + + case AddDuplicateBehavior.Update: + parent.Value = value; + return true; + + default: + // Should never happen + throw new Exception("Unexpected AddDuplicateBehavior"); + } + } + + // Which side does this node sit under in relation to it's parent at this level? + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + + if (parent[compare] == null) + { + parent[compare] = nodeToAdd; + break; + } + else + { + parent = parent[compare]; + } + } + while (true); + } + + Count++; + return true; + } + + private void ReaddChildNodes(KdTreeNode removedNode) + { + if (removedNode.IsLeaf) + return; + + // The folllowing code might seem a little redundant but we're using + // 2 queues so we can add the child nodes back in, in (more or less) + // the same order they were added in the first place + var nodesToReadd = new Queue>(); + + var nodesToReaddQueue = new Queue>(); + + if (removedNode.LeftChild != null) + nodesToReaddQueue.Enqueue(removedNode.LeftChild); + + if (removedNode.RightChild != null) + nodesToReaddQueue.Enqueue(removedNode.RightChild); + + while (nodesToReaddQueue.Count > 0) + { + var nodeToReadd = nodesToReaddQueue.Dequeue(); + + nodesToReadd.Enqueue(nodeToReadd); + + for (int side = -1; side <= 1; side += 2) + { + if (nodeToReadd[side] != null) + { + nodesToReaddQueue.Enqueue(nodeToReadd[side]); + + nodeToReadd[side] = null; + } + } + } + + while (nodesToReadd.Count > 0) + { + var nodeToReadd = nodesToReadd.Dequeue(); + + Count--; + Add(nodeToReadd.Point, nodeToReadd.Value); + } + } + + public void RemoveAt(TKey[] point) + { + // Is tree empty? + if (root == null) + return; + + KdTreeNode node; + + if (typeMath.AreEqual(point, root.Point)) + { + node = root; + root = null; + Count--; + ReaddChildNodes(node); + return; + } + + node = root; + + int dimension = -1; + do + { + dimension = (dimension + 1) % dimensions; + + int compare = typeMath.Compare(point[dimension], node.Point[dimension]); + + if (node[compare] == null) + // Can't find node + return; + + if (typeMath.AreEqual(point, node[compare].Point)) + { + var nodeToRemove = node[compare]; + node[compare] = null; + Count--; + + ReaddChildNodes(nodeToRemove); + } + else + node = node[compare]; + } + while (node != null); + } + + public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) + { + if (count > Count) + count = Count; + + if (count < 0) + { + throw new ArgumentException("Number of neighbors cannot be negative"); + } + + if (count == 0) + return new KdTreeNode[0]; + + var neighbours = new KdTreeNode[count]; + + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + + var rect = HyperRect.Infinite(dimensions, typeMath); + + AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); + + count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + /* + * 1. Search for the target + * + * 1.1 Start by splitting the specified hyper rect + * on the specified node's point along the current + * dimension so that we end up with 2 sub hyper rects + * (current dimension = depth % dimensions) + * + * 1.2 Check what sub rectangle the the target point resides in + * under the current dimension + * + * 1.3 Set that rect to the nearer rect and also the corresponding + * child node to the nearest rect and node and the other rect + * and child node to the further rect and child node (for use later) + * + * 1.4 Travel into the nearer rect and node by calling function + * recursively with nearer rect and node and incrementing + * the depth + * + * 2. Add leaf to list of nearest neighbours + * + * 3. Walk back up tree and at each level: + * + * 3.1 Add node to nearest neighbours if + * we haven't filled our nearest neighbour + * list yet or if it has a distance to target less + * than any of the distances in our current nearest + * neighbours. + * + * 3.2 If there is any point in the further rectangle that is closer to + * the target than our furtherest nearest neighbour then travel into + * that rect and node + * + * That's it, when it finally finishes traversing the branches + * it needs to we'll have our list! + */ + + private void AddNearestNeighbours( + KdTreeNode node, + TKey[] target, + HyperRect rect, + int depth, + NearestNeighbourList, TKey> nearestNeighbours, + TKey maxSearchRadiusSquared) + { + if (node == null) + return; + + // Work out the current dimension + int dimension = depth % dimensions; + + // Split our hyper-rect into 2 sub rects along the current + // node's point on the current dimension + var leftRect = rect.Clone(); + leftRect.MaxPoint[dimension] = node.Point[dimension]; + + var rightRect = rect.Clone(); + rightRect.MinPoint[dimension] = node.Point[dimension]; + + // Which side does the target reside in? + int compare = typeMath.Compare(target[dimension], node.Point[dimension]); + + var nearerRect = compare <= 0 ? leftRect : rightRect; + var furtherRect = compare <= 0 ? rightRect : leftRect; + + var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; + var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; + + // Let's walk down into the nearer branch + if (nearerNode != null) + { + AddNearestNeighbours( + nearerNode, + target, + nearerRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + + TKey distanceSquaredToTarget; + + // Walk down into the further branch but only if our capacity hasn't been reached + // OR if there's a region in the further rect that's closer to the target than our + // current furtherest nearest neighbour + TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + { + if (nearestNeighbours.IsCapacityReached) + { + if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + else + { + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + } + + // Try to add the current node to our nearest neighbours list + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + nearestNeighbours.Add(node, distanceSquaredToTarget); + } + + /// + /// Performs a radial search. + /// + /// Center point + /// Radius to find neighbours within + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + /// + /// Performs a radial search up to a maximum count. + /// + /// Center point + /// Radius to find neighbours within + /// Maximum number of neighbours + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + private KdTreeNode[] RadialSearch(TKey[] center, TKey radius, NearestNeighbourList, TKey> nearestNeighbours) + { + AddNearestNeighbours( + root, + center, + HyperRect.Infinite(dimensions, typeMath), + 0, + nearestNeighbours, + typeMath.Multiply(radius, radius)); + + var count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + public int Count { get; private set; } + + public bool TryFindValueAt(TKey[] point, out TValue value) + { + var parent = root; + int dimension = -1; + do + { + if (parent == null) + { + value = default(TValue); + return false; + } + else if (typeMath.AreEqual(point, parent.Point)) + { + value = parent.Value; + return true; + } + + // Keep searching + dimension = (dimension + 1) % dimensions; + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + parent = parent[compare]; + } + while (true); + } + + public TValue FindValueAt(TKey[] point) + { + if (TryFindValueAt(point, out TValue value)) + return value; + else + return default(TValue); + } + + public bool TryFindValue(TValue value, out TKey[] point) + { + if (root == null) + { + point = null; + return false; + } + + // First-in, First-out list of nodes to search + var nodesToSearch = new Queue>(); + + nodesToSearch.Enqueue(root); + + while (nodesToSearch.Count > 0) + { + var nodeToSearch = nodesToSearch.Dequeue(); + + if (nodeToSearch.Value.Equals(value)) + { + point = nodeToSearch.Point; + return true; + } + else + { + for (int side = -1; side <= 1; side += 2) + { + var childNode = nodeToSearch[side]; + + if (childNode != null) + nodesToSearch.Enqueue(childNode); + } + } + } + + point = null; + return false; + } + + public TKey[] FindValue(TValue value) + { + if (TryFindValue(value, out TKey[] point)) + return point; + else + return null; + } + + private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) + { + sb.AppendLine(node.ToString()); + + for (var side = -1; side <= 1; side += 2) + { + for (var index = 0; index <= depth; index++) + sb.Append("\t"); + + sb.Append(side == -1 ? "L " : "R "); + + if (node[side] == null) + sb.AppendLine(""); + else + AddNodeToStringBuilder(node[side], sb, depth + 1); + } + } + + public override string ToString() + { + if (root == null) + return ""; + + var sb = new StringBuilder(); + AddNodeToStringBuilder(root, sb, 0); + return sb.ToString(); + } + + private void AddNodesToList(KdTreeNode node, List> nodes) + { + if (node == null) + return; + + nodes.Add(node); + + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + AddNodesToList(node[side], nodes); + node[side] = null; + } + } + } + + private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + for (var index = fromIndex + 1; index <= toIndex; index++) + { + var newIndex = index; + + while (true) + { + var a = nodes[newIndex - 1]; + var b = nodes[newIndex]; + if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) + { + nodes[newIndex - 1] = b; + nodes[newIndex] = a; + } + else + break; + } + } + } + + private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + if (fromIndex == toIndex) + { + Add(nodes[fromIndex].Point, nodes[fromIndex].Value); + nodes[fromIndex] = null; + return; + } + + // Sort the array from the fromIndex to the toIndex + SortNodesArray(nodes, byDimension, fromIndex, toIndex); + + // Find the splitting point + int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; + + // Add the splitting point + Add(nodes[midIndex].Point, nodes[midIndex].Value); + nodes[midIndex] = null; + + // Recurse + int nextDimension = (byDimension + 1) % dimensions; + + if (fromIndex < midIndex) + AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); + + if (toIndex > midIndex) + AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); + } + + public void Balance() + { + var nodeList = new List>(); + AddNodesToList(root, nodeList); + + Clear(); + + AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); + } + + private void RemoveChildNodes(KdTreeNode node) + { + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + RemoveChildNodes(node[side]); + node[side] = null; + } + } + } + + public void Clear() + { + if (root != null) + RemoveChildNodes(root); + } + + public void SaveToFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Create(filename)) + { + formatter.Serialize(stream, this); + stream.Flush(); + } + } + + public static KdTree LoadFromFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Open(filename, FileMode.Open)) + { + return (KdTree)formatter.Deserialize(stream); + } + + } + + public IEnumerator> GetEnumerator() + { + var left = new Stack>(); + var right = new Stack>(); + + void addLeft(KdTreeNode node) + { + if (node.LeftChild != null) + { + left.Push(node.LeftChild); + } + } + + void addRight(KdTreeNode node) + { + if (node.RightChild != null) + { + right.Push(node.RightChild); + } + } + + if (root != null) + { + yield return root; + + addLeft(root); + addRight(root); + + while (true) + { + if (left.Any()) + { + var item = left.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else if (right.Any()) + { + var item = right.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else + { + break; + } + } + } + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTree.cs.meta b/Assets/KdTreeLib/KdTree.cs.meta new file mode 100755 index 0000000..d7f1d14 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b61eae78ccd40f0459eb72df19d37003 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTreeNode.cs b/Assets/KdTreeLib/KdTreeNode.cs new file mode 100755 index 0000000..e08f24d --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs @@ -0,0 +1,68 @@ +using System; +using System.Text; + +namespace KdTree +{ + [Serializable] + public class KdTreeNode + { + public KdTreeNode() + { + } + + public KdTreeNode(TKey[] point, TValue value) + { + Point = point; + Value = value; + } + + public TKey[] Point; + public TValue Value = default(TValue); + + internal KdTreeNode LeftChild = null; + internal KdTreeNode RightChild = null; + + internal KdTreeNode this[int compare] + { + get + { + if (compare <= 0) + return LeftChild; + else + return RightChild; + } + set + { + if (compare <= 0) + LeftChild = value; + else + RightChild = value; + } + } + + public bool IsLeaf + { + get + { + return (LeftChild == null) && (RightChild == null); + } + } + + public override string ToString() + { + var sb = new StringBuilder(); + + for (var dimension = 0; dimension < Point.Length; dimension++) + { + sb.Append(Point[dimension].ToString() + "\t"); + } + + if (Value == null) + sb.Append("null"); + else + sb.Append(Value.ToString()); + + return sb.ToString(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTreeNode.cs.meta b/Assets/KdTreeLib/KdTreeNode.cs.meta new file mode 100755 index 0000000..7fa00e6 --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c70bc7c92452a3b4e8de75cf4a32e85f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math.meta b/Assets/KdTreeLib/Math.meta new file mode 100755 index 0000000..7e307d1 --- /dev/null +++ b/Assets/KdTreeLib/Math.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 250be181763ace04ebe9e51229d5b664 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IPriorityQueue.cs.meta b/Assets/KdTreeLib/IPriorityQueue.cs.meta new file mode 100755 index 0000000..e37c0b2 --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0551921b902b13842aa2e99c95aa537c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTree.cs b/Assets/KdTreeLib/KdTree.cs new file mode 100755 index 0000000..4dfe924 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs @@ -0,0 +1,676 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Runtime.Serialization.Formatters.Binary; +using System.Text; + +namespace KdTree +{ + public enum AddDuplicateBehavior + { + Skip, + Error, + Update + } + + public class DuplicateNodeError : Exception + { + public DuplicateNodeError() + : base("Cannot Add Node With Duplicate Coordinates") + { + } + } + + [Serializable] + public class KdTree : IKdTree + { + public KdTree(int dimensions, ITypeMath typeMath) + { + this.dimensions = dimensions; + this.typeMath = typeMath; + Count = 0; + } + + public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) + : this(dimensions, typeMath) + { + AddDuplicateBehavior = addDuplicateBehavior; + } + + private int dimensions; + + private ITypeMath typeMath = null; + + private KdTreeNode root = null; + + public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } + + public bool Add(TKey[] point, TValue value) + { + var nodeToAdd = new KdTreeNode(point, value); + + if (root == null) + { + root = new KdTreeNode(point, value); + } + else + { + int dimension = -1; + KdTreeNode parent = root; + + do + { + // Increment the dimension we're searching in + dimension = (dimension + 1) % dimensions; + + // Does the node we're adding have the same hyperpoint as this node? + if (typeMath.AreEqual(point, parent.Point)) + { + switch (AddDuplicateBehavior) + { + case AddDuplicateBehavior.Skip: + return false; + + case AddDuplicateBehavior.Error: + throw new DuplicateNodeError(); + + case AddDuplicateBehavior.Update: + parent.Value = value; + return true; + + default: + // Should never happen + throw new Exception("Unexpected AddDuplicateBehavior"); + } + } + + // Which side does this node sit under in relation to it's parent at this level? + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + + if (parent[compare] == null) + { + parent[compare] = nodeToAdd; + break; + } + else + { + parent = parent[compare]; + } + } + while (true); + } + + Count++; + return true; + } + + private void ReaddChildNodes(KdTreeNode removedNode) + { + if (removedNode.IsLeaf) + return; + + // The folllowing code might seem a little redundant but we're using + // 2 queues so we can add the child nodes back in, in (more or less) + // the same order they were added in the first place + var nodesToReadd = new Queue>(); + + var nodesToReaddQueue = new Queue>(); + + if (removedNode.LeftChild != null) + nodesToReaddQueue.Enqueue(removedNode.LeftChild); + + if (removedNode.RightChild != null) + nodesToReaddQueue.Enqueue(removedNode.RightChild); + + while (nodesToReaddQueue.Count > 0) + { + var nodeToReadd = nodesToReaddQueue.Dequeue(); + + nodesToReadd.Enqueue(nodeToReadd); + + for (int side = -1; side <= 1; side += 2) + { + if (nodeToReadd[side] != null) + { + nodesToReaddQueue.Enqueue(nodeToReadd[side]); + + nodeToReadd[side] = null; + } + } + } + + while (nodesToReadd.Count > 0) + { + var nodeToReadd = nodesToReadd.Dequeue(); + + Count--; + Add(nodeToReadd.Point, nodeToReadd.Value); + } + } + + public void RemoveAt(TKey[] point) + { + // Is tree empty? + if (root == null) + return; + + KdTreeNode node; + + if (typeMath.AreEqual(point, root.Point)) + { + node = root; + root = null; + Count--; + ReaddChildNodes(node); + return; + } + + node = root; + + int dimension = -1; + do + { + dimension = (dimension + 1) % dimensions; + + int compare = typeMath.Compare(point[dimension], node.Point[dimension]); + + if (node[compare] == null) + // Can't find node + return; + + if (typeMath.AreEqual(point, node[compare].Point)) + { + var nodeToRemove = node[compare]; + node[compare] = null; + Count--; + + ReaddChildNodes(nodeToRemove); + } + else + node = node[compare]; + } + while (node != null); + } + + public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) + { + if (count > Count) + count = Count; + + if (count < 0) + { + throw new ArgumentException("Number of neighbors cannot be negative"); + } + + if (count == 0) + return new KdTreeNode[0]; + + var neighbours = new KdTreeNode[count]; + + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + + var rect = HyperRect.Infinite(dimensions, typeMath); + + AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); + + count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + /* + * 1. Search for the target + * + * 1.1 Start by splitting the specified hyper rect + * on the specified node's point along the current + * dimension so that we end up with 2 sub hyper rects + * (current dimension = depth % dimensions) + * + * 1.2 Check what sub rectangle the the target point resides in + * under the current dimension + * + * 1.3 Set that rect to the nearer rect and also the corresponding + * child node to the nearest rect and node and the other rect + * and child node to the further rect and child node (for use later) + * + * 1.4 Travel into the nearer rect and node by calling function + * recursively with nearer rect and node and incrementing + * the depth + * + * 2. Add leaf to list of nearest neighbours + * + * 3. Walk back up tree and at each level: + * + * 3.1 Add node to nearest neighbours if + * we haven't filled our nearest neighbour + * list yet or if it has a distance to target less + * than any of the distances in our current nearest + * neighbours. + * + * 3.2 If there is any point in the further rectangle that is closer to + * the target than our furtherest nearest neighbour then travel into + * that rect and node + * + * That's it, when it finally finishes traversing the branches + * it needs to we'll have our list! + */ + + private void AddNearestNeighbours( + KdTreeNode node, + TKey[] target, + HyperRect rect, + int depth, + NearestNeighbourList, TKey> nearestNeighbours, + TKey maxSearchRadiusSquared) + { + if (node == null) + return; + + // Work out the current dimension + int dimension = depth % dimensions; + + // Split our hyper-rect into 2 sub rects along the current + // node's point on the current dimension + var leftRect = rect.Clone(); + leftRect.MaxPoint[dimension] = node.Point[dimension]; + + var rightRect = rect.Clone(); + rightRect.MinPoint[dimension] = node.Point[dimension]; + + // Which side does the target reside in? + int compare = typeMath.Compare(target[dimension], node.Point[dimension]); + + var nearerRect = compare <= 0 ? leftRect : rightRect; + var furtherRect = compare <= 0 ? rightRect : leftRect; + + var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; + var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; + + // Let's walk down into the nearer branch + if (nearerNode != null) + { + AddNearestNeighbours( + nearerNode, + target, + nearerRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + + TKey distanceSquaredToTarget; + + // Walk down into the further branch but only if our capacity hasn't been reached + // OR if there's a region in the further rect that's closer to the target than our + // current furtherest nearest neighbour + TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + { + if (nearestNeighbours.IsCapacityReached) + { + if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + else + { + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + } + + // Try to add the current node to our nearest neighbours list + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + nearestNeighbours.Add(node, distanceSquaredToTarget); + } + + /// + /// Performs a radial search. + /// + /// Center point + /// Radius to find neighbours within + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + /// + /// Performs a radial search up to a maximum count. + /// + /// Center point + /// Radius to find neighbours within + /// Maximum number of neighbours + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + private KdTreeNode[] RadialSearch(TKey[] center, TKey radius, NearestNeighbourList, TKey> nearestNeighbours) + { + AddNearestNeighbours( + root, + center, + HyperRect.Infinite(dimensions, typeMath), + 0, + nearestNeighbours, + typeMath.Multiply(radius, radius)); + + var count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + public int Count { get; private set; } + + public bool TryFindValueAt(TKey[] point, out TValue value) + { + var parent = root; + int dimension = -1; + do + { + if (parent == null) + { + value = default(TValue); + return false; + } + else if (typeMath.AreEqual(point, parent.Point)) + { + value = parent.Value; + return true; + } + + // Keep searching + dimension = (dimension + 1) % dimensions; + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + parent = parent[compare]; + } + while (true); + } + + public TValue FindValueAt(TKey[] point) + { + if (TryFindValueAt(point, out TValue value)) + return value; + else + return default(TValue); + } + + public bool TryFindValue(TValue value, out TKey[] point) + { + if (root == null) + { + point = null; + return false; + } + + // First-in, First-out list of nodes to search + var nodesToSearch = new Queue>(); + + nodesToSearch.Enqueue(root); + + while (nodesToSearch.Count > 0) + { + var nodeToSearch = nodesToSearch.Dequeue(); + + if (nodeToSearch.Value.Equals(value)) + { + point = nodeToSearch.Point; + return true; + } + else + { + for (int side = -1; side <= 1; side += 2) + { + var childNode = nodeToSearch[side]; + + if (childNode != null) + nodesToSearch.Enqueue(childNode); + } + } + } + + point = null; + return false; + } + + public TKey[] FindValue(TValue value) + { + if (TryFindValue(value, out TKey[] point)) + return point; + else + return null; + } + + private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) + { + sb.AppendLine(node.ToString()); + + for (var side = -1; side <= 1; side += 2) + { + for (var index = 0; index <= depth; index++) + sb.Append("\t"); + + sb.Append(side == -1 ? "L " : "R "); + + if (node[side] == null) + sb.AppendLine(""); + else + AddNodeToStringBuilder(node[side], sb, depth + 1); + } + } + + public override string ToString() + { + if (root == null) + return ""; + + var sb = new StringBuilder(); + AddNodeToStringBuilder(root, sb, 0); + return sb.ToString(); + } + + private void AddNodesToList(KdTreeNode node, List> nodes) + { + if (node == null) + return; + + nodes.Add(node); + + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + AddNodesToList(node[side], nodes); + node[side] = null; + } + } + } + + private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + for (var index = fromIndex + 1; index <= toIndex; index++) + { + var newIndex = index; + + while (true) + { + var a = nodes[newIndex - 1]; + var b = nodes[newIndex]; + if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) + { + nodes[newIndex - 1] = b; + nodes[newIndex] = a; + } + else + break; + } + } + } + + private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + if (fromIndex == toIndex) + { + Add(nodes[fromIndex].Point, nodes[fromIndex].Value); + nodes[fromIndex] = null; + return; + } + + // Sort the array from the fromIndex to the toIndex + SortNodesArray(nodes, byDimension, fromIndex, toIndex); + + // Find the splitting point + int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; + + // Add the splitting point + Add(nodes[midIndex].Point, nodes[midIndex].Value); + nodes[midIndex] = null; + + // Recurse + int nextDimension = (byDimension + 1) % dimensions; + + if (fromIndex < midIndex) + AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); + + if (toIndex > midIndex) + AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); + } + + public void Balance() + { + var nodeList = new List>(); + AddNodesToList(root, nodeList); + + Clear(); + + AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); + } + + private void RemoveChildNodes(KdTreeNode node) + { + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + RemoveChildNodes(node[side]); + node[side] = null; + } + } + } + + public void Clear() + { + if (root != null) + RemoveChildNodes(root); + } + + public void SaveToFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Create(filename)) + { + formatter.Serialize(stream, this); + stream.Flush(); + } + } + + public static KdTree LoadFromFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Open(filename, FileMode.Open)) + { + return (KdTree)formatter.Deserialize(stream); + } + + } + + public IEnumerator> GetEnumerator() + { + var left = new Stack>(); + var right = new Stack>(); + + void addLeft(KdTreeNode node) + { + if (node.LeftChild != null) + { + left.Push(node.LeftChild); + } + } + + void addRight(KdTreeNode node) + { + if (node.RightChild != null) + { + right.Push(node.RightChild); + } + } + + if (root != null) + { + yield return root; + + addLeft(root); + addRight(root); + + while (true) + { + if (left.Any()) + { + var item = left.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else if (right.Any()) + { + var item = right.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else + { + break; + } + } + } + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTree.cs.meta b/Assets/KdTreeLib/KdTree.cs.meta new file mode 100755 index 0000000..d7f1d14 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b61eae78ccd40f0459eb72df19d37003 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTreeNode.cs b/Assets/KdTreeLib/KdTreeNode.cs new file mode 100755 index 0000000..e08f24d --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs @@ -0,0 +1,68 @@ +using System; +using System.Text; + +namespace KdTree +{ + [Serializable] + public class KdTreeNode + { + public KdTreeNode() + { + } + + public KdTreeNode(TKey[] point, TValue value) + { + Point = point; + Value = value; + } + + public TKey[] Point; + public TValue Value = default(TValue); + + internal KdTreeNode LeftChild = null; + internal KdTreeNode RightChild = null; + + internal KdTreeNode this[int compare] + { + get + { + if (compare <= 0) + return LeftChild; + else + return RightChild; + } + set + { + if (compare <= 0) + LeftChild = value; + else + RightChild = value; + } + } + + public bool IsLeaf + { + get + { + return (LeftChild == null) && (RightChild == null); + } + } + + public override string ToString() + { + var sb = new StringBuilder(); + + for (var dimension = 0; dimension < Point.Length; dimension++) + { + sb.Append(Point[dimension].ToString() + "\t"); + } + + if (Value == null) + sb.Append("null"); + else + sb.Append(Value.ToString()); + + return sb.ToString(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTreeNode.cs.meta b/Assets/KdTreeLib/KdTreeNode.cs.meta new file mode 100755 index 0000000..7fa00e6 --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c70bc7c92452a3b4e8de75cf4a32e85f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math.meta b/Assets/KdTreeLib/Math.meta new file mode 100755 index 0000000..7e307d1 --- /dev/null +++ b/Assets/KdTreeLib/Math.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 250be181763ace04ebe9e51229d5b664 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs b/Assets/KdTreeLib/Math/DoubleMath.cs new file mode 100755 index 0000000..a3a95a3 --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class DoubleMath : TypeMath + { + public override int Compare(double a, double b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(double a, double b) + { + return a == b; + } + + public override double MinValue + { + get { return double.MinValue; } + } + + public override double MaxValue + { + get { return double.MaxValue; } + } + + public override double Zero + { + get { return 0; } + } + + public override double NegativeInfinity { get { return double.NegativeInfinity; } } + + public override double PositiveInfinity { get { return double.PositiveInfinity; } } + + public override double Add(double a, double b) + { + return a + b; + } + + public override double Subtract(double a, double b) + { + return a - b; + } + + public override double Multiply(double a, double b) + { + return a * b; + } + + public override double DistanceSquaredBetweenPoints(double[] a, double[] b) + { + double distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + double distOnThisAxis = Subtract(a[dimension], b[dimension]); + double distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IPriorityQueue.cs.meta b/Assets/KdTreeLib/IPriorityQueue.cs.meta new file mode 100755 index 0000000..e37c0b2 --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0551921b902b13842aa2e99c95aa537c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTree.cs b/Assets/KdTreeLib/KdTree.cs new file mode 100755 index 0000000..4dfe924 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs @@ -0,0 +1,676 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Runtime.Serialization.Formatters.Binary; +using System.Text; + +namespace KdTree +{ + public enum AddDuplicateBehavior + { + Skip, + Error, + Update + } + + public class DuplicateNodeError : Exception + { + public DuplicateNodeError() + : base("Cannot Add Node With Duplicate Coordinates") + { + } + } + + [Serializable] + public class KdTree : IKdTree + { + public KdTree(int dimensions, ITypeMath typeMath) + { + this.dimensions = dimensions; + this.typeMath = typeMath; + Count = 0; + } + + public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) + : this(dimensions, typeMath) + { + AddDuplicateBehavior = addDuplicateBehavior; + } + + private int dimensions; + + private ITypeMath typeMath = null; + + private KdTreeNode root = null; + + public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } + + public bool Add(TKey[] point, TValue value) + { + var nodeToAdd = new KdTreeNode(point, value); + + if (root == null) + { + root = new KdTreeNode(point, value); + } + else + { + int dimension = -1; + KdTreeNode parent = root; + + do + { + // Increment the dimension we're searching in + dimension = (dimension + 1) % dimensions; + + // Does the node we're adding have the same hyperpoint as this node? + if (typeMath.AreEqual(point, parent.Point)) + { + switch (AddDuplicateBehavior) + { + case AddDuplicateBehavior.Skip: + return false; + + case AddDuplicateBehavior.Error: + throw new DuplicateNodeError(); + + case AddDuplicateBehavior.Update: + parent.Value = value; + return true; + + default: + // Should never happen + throw new Exception("Unexpected AddDuplicateBehavior"); + } + } + + // Which side does this node sit under in relation to it's parent at this level? + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + + if (parent[compare] == null) + { + parent[compare] = nodeToAdd; + break; + } + else + { + parent = parent[compare]; + } + } + while (true); + } + + Count++; + return true; + } + + private void ReaddChildNodes(KdTreeNode removedNode) + { + if (removedNode.IsLeaf) + return; + + // The folllowing code might seem a little redundant but we're using + // 2 queues so we can add the child nodes back in, in (more or less) + // the same order they were added in the first place + var nodesToReadd = new Queue>(); + + var nodesToReaddQueue = new Queue>(); + + if (removedNode.LeftChild != null) + nodesToReaddQueue.Enqueue(removedNode.LeftChild); + + if (removedNode.RightChild != null) + nodesToReaddQueue.Enqueue(removedNode.RightChild); + + while (nodesToReaddQueue.Count > 0) + { + var nodeToReadd = nodesToReaddQueue.Dequeue(); + + nodesToReadd.Enqueue(nodeToReadd); + + for (int side = -1; side <= 1; side += 2) + { + if (nodeToReadd[side] != null) + { + nodesToReaddQueue.Enqueue(nodeToReadd[side]); + + nodeToReadd[side] = null; + } + } + } + + while (nodesToReadd.Count > 0) + { + var nodeToReadd = nodesToReadd.Dequeue(); + + Count--; + Add(nodeToReadd.Point, nodeToReadd.Value); + } + } + + public void RemoveAt(TKey[] point) + { + // Is tree empty? + if (root == null) + return; + + KdTreeNode node; + + if (typeMath.AreEqual(point, root.Point)) + { + node = root; + root = null; + Count--; + ReaddChildNodes(node); + return; + } + + node = root; + + int dimension = -1; + do + { + dimension = (dimension + 1) % dimensions; + + int compare = typeMath.Compare(point[dimension], node.Point[dimension]); + + if (node[compare] == null) + // Can't find node + return; + + if (typeMath.AreEqual(point, node[compare].Point)) + { + var nodeToRemove = node[compare]; + node[compare] = null; + Count--; + + ReaddChildNodes(nodeToRemove); + } + else + node = node[compare]; + } + while (node != null); + } + + public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) + { + if (count > Count) + count = Count; + + if (count < 0) + { + throw new ArgumentException("Number of neighbors cannot be negative"); + } + + if (count == 0) + return new KdTreeNode[0]; + + var neighbours = new KdTreeNode[count]; + + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + + var rect = HyperRect.Infinite(dimensions, typeMath); + + AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); + + count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + /* + * 1. Search for the target + * + * 1.1 Start by splitting the specified hyper rect + * on the specified node's point along the current + * dimension so that we end up with 2 sub hyper rects + * (current dimension = depth % dimensions) + * + * 1.2 Check what sub rectangle the the target point resides in + * under the current dimension + * + * 1.3 Set that rect to the nearer rect and also the corresponding + * child node to the nearest rect and node and the other rect + * and child node to the further rect and child node (for use later) + * + * 1.4 Travel into the nearer rect and node by calling function + * recursively with nearer rect and node and incrementing + * the depth + * + * 2. Add leaf to list of nearest neighbours + * + * 3. Walk back up tree and at each level: + * + * 3.1 Add node to nearest neighbours if + * we haven't filled our nearest neighbour + * list yet or if it has a distance to target less + * than any of the distances in our current nearest + * neighbours. + * + * 3.2 If there is any point in the further rectangle that is closer to + * the target than our furtherest nearest neighbour then travel into + * that rect and node + * + * That's it, when it finally finishes traversing the branches + * it needs to we'll have our list! + */ + + private void AddNearestNeighbours( + KdTreeNode node, + TKey[] target, + HyperRect rect, + int depth, + NearestNeighbourList, TKey> nearestNeighbours, + TKey maxSearchRadiusSquared) + { + if (node == null) + return; + + // Work out the current dimension + int dimension = depth % dimensions; + + // Split our hyper-rect into 2 sub rects along the current + // node's point on the current dimension + var leftRect = rect.Clone(); + leftRect.MaxPoint[dimension] = node.Point[dimension]; + + var rightRect = rect.Clone(); + rightRect.MinPoint[dimension] = node.Point[dimension]; + + // Which side does the target reside in? + int compare = typeMath.Compare(target[dimension], node.Point[dimension]); + + var nearerRect = compare <= 0 ? leftRect : rightRect; + var furtherRect = compare <= 0 ? rightRect : leftRect; + + var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; + var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; + + // Let's walk down into the nearer branch + if (nearerNode != null) + { + AddNearestNeighbours( + nearerNode, + target, + nearerRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + + TKey distanceSquaredToTarget; + + // Walk down into the further branch but only if our capacity hasn't been reached + // OR if there's a region in the further rect that's closer to the target than our + // current furtherest nearest neighbour + TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + { + if (nearestNeighbours.IsCapacityReached) + { + if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + else + { + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + } + + // Try to add the current node to our nearest neighbours list + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + nearestNeighbours.Add(node, distanceSquaredToTarget); + } + + /// + /// Performs a radial search. + /// + /// Center point + /// Radius to find neighbours within + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + /// + /// Performs a radial search up to a maximum count. + /// + /// Center point + /// Radius to find neighbours within + /// Maximum number of neighbours + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + private KdTreeNode[] RadialSearch(TKey[] center, TKey radius, NearestNeighbourList, TKey> nearestNeighbours) + { + AddNearestNeighbours( + root, + center, + HyperRect.Infinite(dimensions, typeMath), + 0, + nearestNeighbours, + typeMath.Multiply(radius, radius)); + + var count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + public int Count { get; private set; } + + public bool TryFindValueAt(TKey[] point, out TValue value) + { + var parent = root; + int dimension = -1; + do + { + if (parent == null) + { + value = default(TValue); + return false; + } + else if (typeMath.AreEqual(point, parent.Point)) + { + value = parent.Value; + return true; + } + + // Keep searching + dimension = (dimension + 1) % dimensions; + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + parent = parent[compare]; + } + while (true); + } + + public TValue FindValueAt(TKey[] point) + { + if (TryFindValueAt(point, out TValue value)) + return value; + else + return default(TValue); + } + + public bool TryFindValue(TValue value, out TKey[] point) + { + if (root == null) + { + point = null; + return false; + } + + // First-in, First-out list of nodes to search + var nodesToSearch = new Queue>(); + + nodesToSearch.Enqueue(root); + + while (nodesToSearch.Count > 0) + { + var nodeToSearch = nodesToSearch.Dequeue(); + + if (nodeToSearch.Value.Equals(value)) + { + point = nodeToSearch.Point; + return true; + } + else + { + for (int side = -1; side <= 1; side += 2) + { + var childNode = nodeToSearch[side]; + + if (childNode != null) + nodesToSearch.Enqueue(childNode); + } + } + } + + point = null; + return false; + } + + public TKey[] FindValue(TValue value) + { + if (TryFindValue(value, out TKey[] point)) + return point; + else + return null; + } + + private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) + { + sb.AppendLine(node.ToString()); + + for (var side = -1; side <= 1; side += 2) + { + for (var index = 0; index <= depth; index++) + sb.Append("\t"); + + sb.Append(side == -1 ? "L " : "R "); + + if (node[side] == null) + sb.AppendLine(""); + else + AddNodeToStringBuilder(node[side], sb, depth + 1); + } + } + + public override string ToString() + { + if (root == null) + return ""; + + var sb = new StringBuilder(); + AddNodeToStringBuilder(root, sb, 0); + return sb.ToString(); + } + + private void AddNodesToList(KdTreeNode node, List> nodes) + { + if (node == null) + return; + + nodes.Add(node); + + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + AddNodesToList(node[side], nodes); + node[side] = null; + } + } + } + + private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + for (var index = fromIndex + 1; index <= toIndex; index++) + { + var newIndex = index; + + while (true) + { + var a = nodes[newIndex - 1]; + var b = nodes[newIndex]; + if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) + { + nodes[newIndex - 1] = b; + nodes[newIndex] = a; + } + else + break; + } + } + } + + private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + if (fromIndex == toIndex) + { + Add(nodes[fromIndex].Point, nodes[fromIndex].Value); + nodes[fromIndex] = null; + return; + } + + // Sort the array from the fromIndex to the toIndex + SortNodesArray(nodes, byDimension, fromIndex, toIndex); + + // Find the splitting point + int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; + + // Add the splitting point + Add(nodes[midIndex].Point, nodes[midIndex].Value); + nodes[midIndex] = null; + + // Recurse + int nextDimension = (byDimension + 1) % dimensions; + + if (fromIndex < midIndex) + AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); + + if (toIndex > midIndex) + AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); + } + + public void Balance() + { + var nodeList = new List>(); + AddNodesToList(root, nodeList); + + Clear(); + + AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); + } + + private void RemoveChildNodes(KdTreeNode node) + { + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + RemoveChildNodes(node[side]); + node[side] = null; + } + } + } + + public void Clear() + { + if (root != null) + RemoveChildNodes(root); + } + + public void SaveToFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Create(filename)) + { + formatter.Serialize(stream, this); + stream.Flush(); + } + } + + public static KdTree LoadFromFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Open(filename, FileMode.Open)) + { + return (KdTree)formatter.Deserialize(stream); + } + + } + + public IEnumerator> GetEnumerator() + { + var left = new Stack>(); + var right = new Stack>(); + + void addLeft(KdTreeNode node) + { + if (node.LeftChild != null) + { + left.Push(node.LeftChild); + } + } + + void addRight(KdTreeNode node) + { + if (node.RightChild != null) + { + right.Push(node.RightChild); + } + } + + if (root != null) + { + yield return root; + + addLeft(root); + addRight(root); + + while (true) + { + if (left.Any()) + { + var item = left.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else if (right.Any()) + { + var item = right.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else + { + break; + } + } + } + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTree.cs.meta b/Assets/KdTreeLib/KdTree.cs.meta new file mode 100755 index 0000000..d7f1d14 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b61eae78ccd40f0459eb72df19d37003 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTreeNode.cs b/Assets/KdTreeLib/KdTreeNode.cs new file mode 100755 index 0000000..e08f24d --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs @@ -0,0 +1,68 @@ +using System; +using System.Text; + +namespace KdTree +{ + [Serializable] + public class KdTreeNode + { + public KdTreeNode() + { + } + + public KdTreeNode(TKey[] point, TValue value) + { + Point = point; + Value = value; + } + + public TKey[] Point; + public TValue Value = default(TValue); + + internal KdTreeNode LeftChild = null; + internal KdTreeNode RightChild = null; + + internal KdTreeNode this[int compare] + { + get + { + if (compare <= 0) + return LeftChild; + else + return RightChild; + } + set + { + if (compare <= 0) + LeftChild = value; + else + RightChild = value; + } + } + + public bool IsLeaf + { + get + { + return (LeftChild == null) && (RightChild == null); + } + } + + public override string ToString() + { + var sb = new StringBuilder(); + + for (var dimension = 0; dimension < Point.Length; dimension++) + { + sb.Append(Point[dimension].ToString() + "\t"); + } + + if (Value == null) + sb.Append("null"); + else + sb.Append(Value.ToString()); + + return sb.ToString(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTreeNode.cs.meta b/Assets/KdTreeLib/KdTreeNode.cs.meta new file mode 100755 index 0000000..7fa00e6 --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c70bc7c92452a3b4e8de75cf4a32e85f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math.meta b/Assets/KdTreeLib/Math.meta new file mode 100755 index 0000000..7e307d1 --- /dev/null +++ b/Assets/KdTreeLib/Math.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 250be181763ace04ebe9e51229d5b664 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs b/Assets/KdTreeLib/Math/DoubleMath.cs new file mode 100755 index 0000000..a3a95a3 --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class DoubleMath : TypeMath + { + public override int Compare(double a, double b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(double a, double b) + { + return a == b; + } + + public override double MinValue + { + get { return double.MinValue; } + } + + public override double MaxValue + { + get { return double.MaxValue; } + } + + public override double Zero + { + get { return 0; } + } + + public override double NegativeInfinity { get { return double.NegativeInfinity; } } + + public override double PositiveInfinity { get { return double.PositiveInfinity; } } + + public override double Add(double a, double b) + { + return a + b; + } + + public override double Subtract(double a, double b) + { + return a - b; + } + + public override double Multiply(double a, double b) + { + return a * b; + } + + public override double DistanceSquaredBetweenPoints(double[] a, double[] b) + { + double distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + double distOnThisAxis = Subtract(a[dimension], b[dimension]); + double distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs.meta b/Assets/KdTreeLib/Math/DoubleMath.cs.meta new file mode 100755 index 0000000..e0e7d9d --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6878fdbadd195164393a12f522125dec +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IPriorityQueue.cs.meta b/Assets/KdTreeLib/IPriorityQueue.cs.meta new file mode 100755 index 0000000..e37c0b2 --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0551921b902b13842aa2e99c95aa537c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTree.cs b/Assets/KdTreeLib/KdTree.cs new file mode 100755 index 0000000..4dfe924 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs @@ -0,0 +1,676 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Runtime.Serialization.Formatters.Binary; +using System.Text; + +namespace KdTree +{ + public enum AddDuplicateBehavior + { + Skip, + Error, + Update + } + + public class DuplicateNodeError : Exception + { + public DuplicateNodeError() + : base("Cannot Add Node With Duplicate Coordinates") + { + } + } + + [Serializable] + public class KdTree : IKdTree + { + public KdTree(int dimensions, ITypeMath typeMath) + { + this.dimensions = dimensions; + this.typeMath = typeMath; + Count = 0; + } + + public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) + : this(dimensions, typeMath) + { + AddDuplicateBehavior = addDuplicateBehavior; + } + + private int dimensions; + + private ITypeMath typeMath = null; + + private KdTreeNode root = null; + + public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } + + public bool Add(TKey[] point, TValue value) + { + var nodeToAdd = new KdTreeNode(point, value); + + if (root == null) + { + root = new KdTreeNode(point, value); + } + else + { + int dimension = -1; + KdTreeNode parent = root; + + do + { + // Increment the dimension we're searching in + dimension = (dimension + 1) % dimensions; + + // Does the node we're adding have the same hyperpoint as this node? + if (typeMath.AreEqual(point, parent.Point)) + { + switch (AddDuplicateBehavior) + { + case AddDuplicateBehavior.Skip: + return false; + + case AddDuplicateBehavior.Error: + throw new DuplicateNodeError(); + + case AddDuplicateBehavior.Update: + parent.Value = value; + return true; + + default: + // Should never happen + throw new Exception("Unexpected AddDuplicateBehavior"); + } + } + + // Which side does this node sit under in relation to it's parent at this level? + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + + if (parent[compare] == null) + { + parent[compare] = nodeToAdd; + break; + } + else + { + parent = parent[compare]; + } + } + while (true); + } + + Count++; + return true; + } + + private void ReaddChildNodes(KdTreeNode removedNode) + { + if (removedNode.IsLeaf) + return; + + // The folllowing code might seem a little redundant but we're using + // 2 queues so we can add the child nodes back in, in (more or less) + // the same order they were added in the first place + var nodesToReadd = new Queue>(); + + var nodesToReaddQueue = new Queue>(); + + if (removedNode.LeftChild != null) + nodesToReaddQueue.Enqueue(removedNode.LeftChild); + + if (removedNode.RightChild != null) + nodesToReaddQueue.Enqueue(removedNode.RightChild); + + while (nodesToReaddQueue.Count > 0) + { + var nodeToReadd = nodesToReaddQueue.Dequeue(); + + nodesToReadd.Enqueue(nodeToReadd); + + for (int side = -1; side <= 1; side += 2) + { + if (nodeToReadd[side] != null) + { + nodesToReaddQueue.Enqueue(nodeToReadd[side]); + + nodeToReadd[side] = null; + } + } + } + + while (nodesToReadd.Count > 0) + { + var nodeToReadd = nodesToReadd.Dequeue(); + + Count--; + Add(nodeToReadd.Point, nodeToReadd.Value); + } + } + + public void RemoveAt(TKey[] point) + { + // Is tree empty? + if (root == null) + return; + + KdTreeNode node; + + if (typeMath.AreEqual(point, root.Point)) + { + node = root; + root = null; + Count--; + ReaddChildNodes(node); + return; + } + + node = root; + + int dimension = -1; + do + { + dimension = (dimension + 1) % dimensions; + + int compare = typeMath.Compare(point[dimension], node.Point[dimension]); + + if (node[compare] == null) + // Can't find node + return; + + if (typeMath.AreEqual(point, node[compare].Point)) + { + var nodeToRemove = node[compare]; + node[compare] = null; + Count--; + + ReaddChildNodes(nodeToRemove); + } + else + node = node[compare]; + } + while (node != null); + } + + public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) + { + if (count > Count) + count = Count; + + if (count < 0) + { + throw new ArgumentException("Number of neighbors cannot be negative"); + } + + if (count == 0) + return new KdTreeNode[0]; + + var neighbours = new KdTreeNode[count]; + + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + + var rect = HyperRect.Infinite(dimensions, typeMath); + + AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); + + count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + /* + * 1. Search for the target + * + * 1.1 Start by splitting the specified hyper rect + * on the specified node's point along the current + * dimension so that we end up with 2 sub hyper rects + * (current dimension = depth % dimensions) + * + * 1.2 Check what sub rectangle the the target point resides in + * under the current dimension + * + * 1.3 Set that rect to the nearer rect and also the corresponding + * child node to the nearest rect and node and the other rect + * and child node to the further rect and child node (for use later) + * + * 1.4 Travel into the nearer rect and node by calling function + * recursively with nearer rect and node and incrementing + * the depth + * + * 2. Add leaf to list of nearest neighbours + * + * 3. Walk back up tree and at each level: + * + * 3.1 Add node to nearest neighbours if + * we haven't filled our nearest neighbour + * list yet or if it has a distance to target less + * than any of the distances in our current nearest + * neighbours. + * + * 3.2 If there is any point in the further rectangle that is closer to + * the target than our furtherest nearest neighbour then travel into + * that rect and node + * + * That's it, when it finally finishes traversing the branches + * it needs to we'll have our list! + */ + + private void AddNearestNeighbours( + KdTreeNode node, + TKey[] target, + HyperRect rect, + int depth, + NearestNeighbourList, TKey> nearestNeighbours, + TKey maxSearchRadiusSquared) + { + if (node == null) + return; + + // Work out the current dimension + int dimension = depth % dimensions; + + // Split our hyper-rect into 2 sub rects along the current + // node's point on the current dimension + var leftRect = rect.Clone(); + leftRect.MaxPoint[dimension] = node.Point[dimension]; + + var rightRect = rect.Clone(); + rightRect.MinPoint[dimension] = node.Point[dimension]; + + // Which side does the target reside in? + int compare = typeMath.Compare(target[dimension], node.Point[dimension]); + + var nearerRect = compare <= 0 ? leftRect : rightRect; + var furtherRect = compare <= 0 ? rightRect : leftRect; + + var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; + var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; + + // Let's walk down into the nearer branch + if (nearerNode != null) + { + AddNearestNeighbours( + nearerNode, + target, + nearerRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + + TKey distanceSquaredToTarget; + + // Walk down into the further branch but only if our capacity hasn't been reached + // OR if there's a region in the further rect that's closer to the target than our + // current furtherest nearest neighbour + TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + { + if (nearestNeighbours.IsCapacityReached) + { + if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + else + { + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + } + + // Try to add the current node to our nearest neighbours list + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + nearestNeighbours.Add(node, distanceSquaredToTarget); + } + + /// + /// Performs a radial search. + /// + /// Center point + /// Radius to find neighbours within + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + /// + /// Performs a radial search up to a maximum count. + /// + /// Center point + /// Radius to find neighbours within + /// Maximum number of neighbours + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + private KdTreeNode[] RadialSearch(TKey[] center, TKey radius, NearestNeighbourList, TKey> nearestNeighbours) + { + AddNearestNeighbours( + root, + center, + HyperRect.Infinite(dimensions, typeMath), + 0, + nearestNeighbours, + typeMath.Multiply(radius, radius)); + + var count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + public int Count { get; private set; } + + public bool TryFindValueAt(TKey[] point, out TValue value) + { + var parent = root; + int dimension = -1; + do + { + if (parent == null) + { + value = default(TValue); + return false; + } + else if (typeMath.AreEqual(point, parent.Point)) + { + value = parent.Value; + return true; + } + + // Keep searching + dimension = (dimension + 1) % dimensions; + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + parent = parent[compare]; + } + while (true); + } + + public TValue FindValueAt(TKey[] point) + { + if (TryFindValueAt(point, out TValue value)) + return value; + else + return default(TValue); + } + + public bool TryFindValue(TValue value, out TKey[] point) + { + if (root == null) + { + point = null; + return false; + } + + // First-in, First-out list of nodes to search + var nodesToSearch = new Queue>(); + + nodesToSearch.Enqueue(root); + + while (nodesToSearch.Count > 0) + { + var nodeToSearch = nodesToSearch.Dequeue(); + + if (nodeToSearch.Value.Equals(value)) + { + point = nodeToSearch.Point; + return true; + } + else + { + for (int side = -1; side <= 1; side += 2) + { + var childNode = nodeToSearch[side]; + + if (childNode != null) + nodesToSearch.Enqueue(childNode); + } + } + } + + point = null; + return false; + } + + public TKey[] FindValue(TValue value) + { + if (TryFindValue(value, out TKey[] point)) + return point; + else + return null; + } + + private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) + { + sb.AppendLine(node.ToString()); + + for (var side = -1; side <= 1; side += 2) + { + for (var index = 0; index <= depth; index++) + sb.Append("\t"); + + sb.Append(side == -1 ? "L " : "R "); + + if (node[side] == null) + sb.AppendLine(""); + else + AddNodeToStringBuilder(node[side], sb, depth + 1); + } + } + + public override string ToString() + { + if (root == null) + return ""; + + var sb = new StringBuilder(); + AddNodeToStringBuilder(root, sb, 0); + return sb.ToString(); + } + + private void AddNodesToList(KdTreeNode node, List> nodes) + { + if (node == null) + return; + + nodes.Add(node); + + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + AddNodesToList(node[side], nodes); + node[side] = null; + } + } + } + + private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + for (var index = fromIndex + 1; index <= toIndex; index++) + { + var newIndex = index; + + while (true) + { + var a = nodes[newIndex - 1]; + var b = nodes[newIndex]; + if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) + { + nodes[newIndex - 1] = b; + nodes[newIndex] = a; + } + else + break; + } + } + } + + private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + if (fromIndex == toIndex) + { + Add(nodes[fromIndex].Point, nodes[fromIndex].Value); + nodes[fromIndex] = null; + return; + } + + // Sort the array from the fromIndex to the toIndex + SortNodesArray(nodes, byDimension, fromIndex, toIndex); + + // Find the splitting point + int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; + + // Add the splitting point + Add(nodes[midIndex].Point, nodes[midIndex].Value); + nodes[midIndex] = null; + + // Recurse + int nextDimension = (byDimension + 1) % dimensions; + + if (fromIndex < midIndex) + AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); + + if (toIndex > midIndex) + AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); + } + + public void Balance() + { + var nodeList = new List>(); + AddNodesToList(root, nodeList); + + Clear(); + + AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); + } + + private void RemoveChildNodes(KdTreeNode node) + { + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + RemoveChildNodes(node[side]); + node[side] = null; + } + } + } + + public void Clear() + { + if (root != null) + RemoveChildNodes(root); + } + + public void SaveToFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Create(filename)) + { + formatter.Serialize(stream, this); + stream.Flush(); + } + } + + public static KdTree LoadFromFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Open(filename, FileMode.Open)) + { + return (KdTree)formatter.Deserialize(stream); + } + + } + + public IEnumerator> GetEnumerator() + { + var left = new Stack>(); + var right = new Stack>(); + + void addLeft(KdTreeNode node) + { + if (node.LeftChild != null) + { + left.Push(node.LeftChild); + } + } + + void addRight(KdTreeNode node) + { + if (node.RightChild != null) + { + right.Push(node.RightChild); + } + } + + if (root != null) + { + yield return root; + + addLeft(root); + addRight(root); + + while (true) + { + if (left.Any()) + { + var item = left.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else if (right.Any()) + { + var item = right.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else + { + break; + } + } + } + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTree.cs.meta b/Assets/KdTreeLib/KdTree.cs.meta new file mode 100755 index 0000000..d7f1d14 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b61eae78ccd40f0459eb72df19d37003 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTreeNode.cs b/Assets/KdTreeLib/KdTreeNode.cs new file mode 100755 index 0000000..e08f24d --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs @@ -0,0 +1,68 @@ +using System; +using System.Text; + +namespace KdTree +{ + [Serializable] + public class KdTreeNode + { + public KdTreeNode() + { + } + + public KdTreeNode(TKey[] point, TValue value) + { + Point = point; + Value = value; + } + + public TKey[] Point; + public TValue Value = default(TValue); + + internal KdTreeNode LeftChild = null; + internal KdTreeNode RightChild = null; + + internal KdTreeNode this[int compare] + { + get + { + if (compare <= 0) + return LeftChild; + else + return RightChild; + } + set + { + if (compare <= 0) + LeftChild = value; + else + RightChild = value; + } + } + + public bool IsLeaf + { + get + { + return (LeftChild == null) && (RightChild == null); + } + } + + public override string ToString() + { + var sb = new StringBuilder(); + + for (var dimension = 0; dimension < Point.Length; dimension++) + { + sb.Append(Point[dimension].ToString() + "\t"); + } + + if (Value == null) + sb.Append("null"); + else + sb.Append(Value.ToString()); + + return sb.ToString(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTreeNode.cs.meta b/Assets/KdTreeLib/KdTreeNode.cs.meta new file mode 100755 index 0000000..7fa00e6 --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c70bc7c92452a3b4e8de75cf4a32e85f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math.meta b/Assets/KdTreeLib/Math.meta new file mode 100755 index 0000000..7e307d1 --- /dev/null +++ b/Assets/KdTreeLib/Math.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 250be181763ace04ebe9e51229d5b664 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs b/Assets/KdTreeLib/Math/DoubleMath.cs new file mode 100755 index 0000000..a3a95a3 --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class DoubleMath : TypeMath + { + public override int Compare(double a, double b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(double a, double b) + { + return a == b; + } + + public override double MinValue + { + get { return double.MinValue; } + } + + public override double MaxValue + { + get { return double.MaxValue; } + } + + public override double Zero + { + get { return 0; } + } + + public override double NegativeInfinity { get { return double.NegativeInfinity; } } + + public override double PositiveInfinity { get { return double.PositiveInfinity; } } + + public override double Add(double a, double b) + { + return a + b; + } + + public override double Subtract(double a, double b) + { + return a - b; + } + + public override double Multiply(double a, double b) + { + return a * b; + } + + public override double DistanceSquaredBetweenPoints(double[] a, double[] b) + { + double distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + double distOnThisAxis = Subtract(a[dimension], b[dimension]); + double distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs.meta b/Assets/KdTreeLib/Math/DoubleMath.cs.meta new file mode 100755 index 0000000..e0e7d9d --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6878fdbadd195164393a12f522125dec +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/FloatMath.cs b/Assets/KdTreeLib/Math/FloatMath.cs new file mode 100755 index 0000000..e530cca --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class FloatMath : TypeMath + { + public override int Compare(float a, float b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(float a, float b) + { + return a == b; + } + + public override float MinValue + { + get { return float.MinValue; } + } + + public override float MaxValue + { + get { return float.MaxValue; } + } + + public override float Zero + { + get { return 0; } + } + + public override float NegativeInfinity { get { return float.NegativeInfinity; } } + + public override float PositiveInfinity { get { return float.PositiveInfinity; } } + + public override float Add(float a, float b) + { + return a + b; + } + + public override float Subtract(float a, float b) + { + return a - b; + } + + public override float Multiply(float a, float b) + { + return a * b; + } + + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + float distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + float distOnThisAxis = Subtract(a[dimension], b[dimension]); + float distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IPriorityQueue.cs.meta b/Assets/KdTreeLib/IPriorityQueue.cs.meta new file mode 100755 index 0000000..e37c0b2 --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0551921b902b13842aa2e99c95aa537c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTree.cs b/Assets/KdTreeLib/KdTree.cs new file mode 100755 index 0000000..4dfe924 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs @@ -0,0 +1,676 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Runtime.Serialization.Formatters.Binary; +using System.Text; + +namespace KdTree +{ + public enum AddDuplicateBehavior + { + Skip, + Error, + Update + } + + public class DuplicateNodeError : Exception + { + public DuplicateNodeError() + : base("Cannot Add Node With Duplicate Coordinates") + { + } + } + + [Serializable] + public class KdTree : IKdTree + { + public KdTree(int dimensions, ITypeMath typeMath) + { + this.dimensions = dimensions; + this.typeMath = typeMath; + Count = 0; + } + + public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) + : this(dimensions, typeMath) + { + AddDuplicateBehavior = addDuplicateBehavior; + } + + private int dimensions; + + private ITypeMath typeMath = null; + + private KdTreeNode root = null; + + public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } + + public bool Add(TKey[] point, TValue value) + { + var nodeToAdd = new KdTreeNode(point, value); + + if (root == null) + { + root = new KdTreeNode(point, value); + } + else + { + int dimension = -1; + KdTreeNode parent = root; + + do + { + // Increment the dimension we're searching in + dimension = (dimension + 1) % dimensions; + + // Does the node we're adding have the same hyperpoint as this node? + if (typeMath.AreEqual(point, parent.Point)) + { + switch (AddDuplicateBehavior) + { + case AddDuplicateBehavior.Skip: + return false; + + case AddDuplicateBehavior.Error: + throw new DuplicateNodeError(); + + case AddDuplicateBehavior.Update: + parent.Value = value; + return true; + + default: + // Should never happen + throw new Exception("Unexpected AddDuplicateBehavior"); + } + } + + // Which side does this node sit under in relation to it's parent at this level? + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + + if (parent[compare] == null) + { + parent[compare] = nodeToAdd; + break; + } + else + { + parent = parent[compare]; + } + } + while (true); + } + + Count++; + return true; + } + + private void ReaddChildNodes(KdTreeNode removedNode) + { + if (removedNode.IsLeaf) + return; + + // The folllowing code might seem a little redundant but we're using + // 2 queues so we can add the child nodes back in, in (more or less) + // the same order they were added in the first place + var nodesToReadd = new Queue>(); + + var nodesToReaddQueue = new Queue>(); + + if (removedNode.LeftChild != null) + nodesToReaddQueue.Enqueue(removedNode.LeftChild); + + if (removedNode.RightChild != null) + nodesToReaddQueue.Enqueue(removedNode.RightChild); + + while (nodesToReaddQueue.Count > 0) + { + var nodeToReadd = nodesToReaddQueue.Dequeue(); + + nodesToReadd.Enqueue(nodeToReadd); + + for (int side = -1; side <= 1; side += 2) + { + if (nodeToReadd[side] != null) + { + nodesToReaddQueue.Enqueue(nodeToReadd[side]); + + nodeToReadd[side] = null; + } + } + } + + while (nodesToReadd.Count > 0) + { + var nodeToReadd = nodesToReadd.Dequeue(); + + Count--; + Add(nodeToReadd.Point, nodeToReadd.Value); + } + } + + public void RemoveAt(TKey[] point) + { + // Is tree empty? + if (root == null) + return; + + KdTreeNode node; + + if (typeMath.AreEqual(point, root.Point)) + { + node = root; + root = null; + Count--; + ReaddChildNodes(node); + return; + } + + node = root; + + int dimension = -1; + do + { + dimension = (dimension + 1) % dimensions; + + int compare = typeMath.Compare(point[dimension], node.Point[dimension]); + + if (node[compare] == null) + // Can't find node + return; + + if (typeMath.AreEqual(point, node[compare].Point)) + { + var nodeToRemove = node[compare]; + node[compare] = null; + Count--; + + ReaddChildNodes(nodeToRemove); + } + else + node = node[compare]; + } + while (node != null); + } + + public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) + { + if (count > Count) + count = Count; + + if (count < 0) + { + throw new ArgumentException("Number of neighbors cannot be negative"); + } + + if (count == 0) + return new KdTreeNode[0]; + + var neighbours = new KdTreeNode[count]; + + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + + var rect = HyperRect.Infinite(dimensions, typeMath); + + AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); + + count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + /* + * 1. Search for the target + * + * 1.1 Start by splitting the specified hyper rect + * on the specified node's point along the current + * dimension so that we end up with 2 sub hyper rects + * (current dimension = depth % dimensions) + * + * 1.2 Check what sub rectangle the the target point resides in + * under the current dimension + * + * 1.3 Set that rect to the nearer rect and also the corresponding + * child node to the nearest rect and node and the other rect + * and child node to the further rect and child node (for use later) + * + * 1.4 Travel into the nearer rect and node by calling function + * recursively with nearer rect and node and incrementing + * the depth + * + * 2. Add leaf to list of nearest neighbours + * + * 3. Walk back up tree and at each level: + * + * 3.1 Add node to nearest neighbours if + * we haven't filled our nearest neighbour + * list yet or if it has a distance to target less + * than any of the distances in our current nearest + * neighbours. + * + * 3.2 If there is any point in the further rectangle that is closer to + * the target than our furtherest nearest neighbour then travel into + * that rect and node + * + * That's it, when it finally finishes traversing the branches + * it needs to we'll have our list! + */ + + private void AddNearestNeighbours( + KdTreeNode node, + TKey[] target, + HyperRect rect, + int depth, + NearestNeighbourList, TKey> nearestNeighbours, + TKey maxSearchRadiusSquared) + { + if (node == null) + return; + + // Work out the current dimension + int dimension = depth % dimensions; + + // Split our hyper-rect into 2 sub rects along the current + // node's point on the current dimension + var leftRect = rect.Clone(); + leftRect.MaxPoint[dimension] = node.Point[dimension]; + + var rightRect = rect.Clone(); + rightRect.MinPoint[dimension] = node.Point[dimension]; + + // Which side does the target reside in? + int compare = typeMath.Compare(target[dimension], node.Point[dimension]); + + var nearerRect = compare <= 0 ? leftRect : rightRect; + var furtherRect = compare <= 0 ? rightRect : leftRect; + + var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; + var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; + + // Let's walk down into the nearer branch + if (nearerNode != null) + { + AddNearestNeighbours( + nearerNode, + target, + nearerRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + + TKey distanceSquaredToTarget; + + // Walk down into the further branch but only if our capacity hasn't been reached + // OR if there's a region in the further rect that's closer to the target than our + // current furtherest nearest neighbour + TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + { + if (nearestNeighbours.IsCapacityReached) + { + if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + else + { + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + } + + // Try to add the current node to our nearest neighbours list + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + nearestNeighbours.Add(node, distanceSquaredToTarget); + } + + /// + /// Performs a radial search. + /// + /// Center point + /// Radius to find neighbours within + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + /// + /// Performs a radial search up to a maximum count. + /// + /// Center point + /// Radius to find neighbours within + /// Maximum number of neighbours + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + private KdTreeNode[] RadialSearch(TKey[] center, TKey radius, NearestNeighbourList, TKey> nearestNeighbours) + { + AddNearestNeighbours( + root, + center, + HyperRect.Infinite(dimensions, typeMath), + 0, + nearestNeighbours, + typeMath.Multiply(radius, radius)); + + var count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + public int Count { get; private set; } + + public bool TryFindValueAt(TKey[] point, out TValue value) + { + var parent = root; + int dimension = -1; + do + { + if (parent == null) + { + value = default(TValue); + return false; + } + else if (typeMath.AreEqual(point, parent.Point)) + { + value = parent.Value; + return true; + } + + // Keep searching + dimension = (dimension + 1) % dimensions; + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + parent = parent[compare]; + } + while (true); + } + + public TValue FindValueAt(TKey[] point) + { + if (TryFindValueAt(point, out TValue value)) + return value; + else + return default(TValue); + } + + public bool TryFindValue(TValue value, out TKey[] point) + { + if (root == null) + { + point = null; + return false; + } + + // First-in, First-out list of nodes to search + var nodesToSearch = new Queue>(); + + nodesToSearch.Enqueue(root); + + while (nodesToSearch.Count > 0) + { + var nodeToSearch = nodesToSearch.Dequeue(); + + if (nodeToSearch.Value.Equals(value)) + { + point = nodeToSearch.Point; + return true; + } + else + { + for (int side = -1; side <= 1; side += 2) + { + var childNode = nodeToSearch[side]; + + if (childNode != null) + nodesToSearch.Enqueue(childNode); + } + } + } + + point = null; + return false; + } + + public TKey[] FindValue(TValue value) + { + if (TryFindValue(value, out TKey[] point)) + return point; + else + return null; + } + + private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) + { + sb.AppendLine(node.ToString()); + + for (var side = -1; side <= 1; side += 2) + { + for (var index = 0; index <= depth; index++) + sb.Append("\t"); + + sb.Append(side == -1 ? "L " : "R "); + + if (node[side] == null) + sb.AppendLine(""); + else + AddNodeToStringBuilder(node[side], sb, depth + 1); + } + } + + public override string ToString() + { + if (root == null) + return ""; + + var sb = new StringBuilder(); + AddNodeToStringBuilder(root, sb, 0); + return sb.ToString(); + } + + private void AddNodesToList(KdTreeNode node, List> nodes) + { + if (node == null) + return; + + nodes.Add(node); + + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + AddNodesToList(node[side], nodes); + node[side] = null; + } + } + } + + private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + for (var index = fromIndex + 1; index <= toIndex; index++) + { + var newIndex = index; + + while (true) + { + var a = nodes[newIndex - 1]; + var b = nodes[newIndex]; + if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) + { + nodes[newIndex - 1] = b; + nodes[newIndex] = a; + } + else + break; + } + } + } + + private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + if (fromIndex == toIndex) + { + Add(nodes[fromIndex].Point, nodes[fromIndex].Value); + nodes[fromIndex] = null; + return; + } + + // Sort the array from the fromIndex to the toIndex + SortNodesArray(nodes, byDimension, fromIndex, toIndex); + + // Find the splitting point + int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; + + // Add the splitting point + Add(nodes[midIndex].Point, nodes[midIndex].Value); + nodes[midIndex] = null; + + // Recurse + int nextDimension = (byDimension + 1) % dimensions; + + if (fromIndex < midIndex) + AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); + + if (toIndex > midIndex) + AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); + } + + public void Balance() + { + var nodeList = new List>(); + AddNodesToList(root, nodeList); + + Clear(); + + AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); + } + + private void RemoveChildNodes(KdTreeNode node) + { + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + RemoveChildNodes(node[side]); + node[side] = null; + } + } + } + + public void Clear() + { + if (root != null) + RemoveChildNodes(root); + } + + public void SaveToFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Create(filename)) + { + formatter.Serialize(stream, this); + stream.Flush(); + } + } + + public static KdTree LoadFromFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Open(filename, FileMode.Open)) + { + return (KdTree)formatter.Deserialize(stream); + } + + } + + public IEnumerator> GetEnumerator() + { + var left = new Stack>(); + var right = new Stack>(); + + void addLeft(KdTreeNode node) + { + if (node.LeftChild != null) + { + left.Push(node.LeftChild); + } + } + + void addRight(KdTreeNode node) + { + if (node.RightChild != null) + { + right.Push(node.RightChild); + } + } + + if (root != null) + { + yield return root; + + addLeft(root); + addRight(root); + + while (true) + { + if (left.Any()) + { + var item = left.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else if (right.Any()) + { + var item = right.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else + { + break; + } + } + } + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTree.cs.meta b/Assets/KdTreeLib/KdTree.cs.meta new file mode 100755 index 0000000..d7f1d14 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b61eae78ccd40f0459eb72df19d37003 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTreeNode.cs b/Assets/KdTreeLib/KdTreeNode.cs new file mode 100755 index 0000000..e08f24d --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs @@ -0,0 +1,68 @@ +using System; +using System.Text; + +namespace KdTree +{ + [Serializable] + public class KdTreeNode + { + public KdTreeNode() + { + } + + public KdTreeNode(TKey[] point, TValue value) + { + Point = point; + Value = value; + } + + public TKey[] Point; + public TValue Value = default(TValue); + + internal KdTreeNode LeftChild = null; + internal KdTreeNode RightChild = null; + + internal KdTreeNode this[int compare] + { + get + { + if (compare <= 0) + return LeftChild; + else + return RightChild; + } + set + { + if (compare <= 0) + LeftChild = value; + else + RightChild = value; + } + } + + public bool IsLeaf + { + get + { + return (LeftChild == null) && (RightChild == null); + } + } + + public override string ToString() + { + var sb = new StringBuilder(); + + for (var dimension = 0; dimension < Point.Length; dimension++) + { + sb.Append(Point[dimension].ToString() + "\t"); + } + + if (Value == null) + sb.Append("null"); + else + sb.Append(Value.ToString()); + + return sb.ToString(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTreeNode.cs.meta b/Assets/KdTreeLib/KdTreeNode.cs.meta new file mode 100755 index 0000000..7fa00e6 --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c70bc7c92452a3b4e8de75cf4a32e85f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math.meta b/Assets/KdTreeLib/Math.meta new file mode 100755 index 0000000..7e307d1 --- /dev/null +++ b/Assets/KdTreeLib/Math.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 250be181763ace04ebe9e51229d5b664 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs b/Assets/KdTreeLib/Math/DoubleMath.cs new file mode 100755 index 0000000..a3a95a3 --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class DoubleMath : TypeMath + { + public override int Compare(double a, double b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(double a, double b) + { + return a == b; + } + + public override double MinValue + { + get { return double.MinValue; } + } + + public override double MaxValue + { + get { return double.MaxValue; } + } + + public override double Zero + { + get { return 0; } + } + + public override double NegativeInfinity { get { return double.NegativeInfinity; } } + + public override double PositiveInfinity { get { return double.PositiveInfinity; } } + + public override double Add(double a, double b) + { + return a + b; + } + + public override double Subtract(double a, double b) + { + return a - b; + } + + public override double Multiply(double a, double b) + { + return a * b; + } + + public override double DistanceSquaredBetweenPoints(double[] a, double[] b) + { + double distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + double distOnThisAxis = Subtract(a[dimension], b[dimension]); + double distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs.meta b/Assets/KdTreeLib/Math/DoubleMath.cs.meta new file mode 100755 index 0000000..e0e7d9d --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6878fdbadd195164393a12f522125dec +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/FloatMath.cs b/Assets/KdTreeLib/Math/FloatMath.cs new file mode 100755 index 0000000..e530cca --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class FloatMath : TypeMath + { + public override int Compare(float a, float b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(float a, float b) + { + return a == b; + } + + public override float MinValue + { + get { return float.MinValue; } + } + + public override float MaxValue + { + get { return float.MaxValue; } + } + + public override float Zero + { + get { return 0; } + } + + public override float NegativeInfinity { get { return float.NegativeInfinity; } } + + public override float PositiveInfinity { get { return float.PositiveInfinity; } } + + public override float Add(float a, float b) + { + return a + b; + } + + public override float Subtract(float a, float b) + { + return a - b; + } + + public override float Multiply(float a, float b) + { + return a * b; + } + + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + float distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + float distOnThisAxis = Subtract(a[dimension], b[dimension]); + float distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/FloatMath.cs.meta b/Assets/KdTreeLib/Math/FloatMath.cs.meta new file mode 100755 index 0000000..d1e609c --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e1a4895e13ce1b64491965b3a4b65201 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IPriorityQueue.cs.meta b/Assets/KdTreeLib/IPriorityQueue.cs.meta new file mode 100755 index 0000000..e37c0b2 --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0551921b902b13842aa2e99c95aa537c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTree.cs b/Assets/KdTreeLib/KdTree.cs new file mode 100755 index 0000000..4dfe924 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs @@ -0,0 +1,676 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Runtime.Serialization.Formatters.Binary; +using System.Text; + +namespace KdTree +{ + public enum AddDuplicateBehavior + { + Skip, + Error, + Update + } + + public class DuplicateNodeError : Exception + { + public DuplicateNodeError() + : base("Cannot Add Node With Duplicate Coordinates") + { + } + } + + [Serializable] + public class KdTree : IKdTree + { + public KdTree(int dimensions, ITypeMath typeMath) + { + this.dimensions = dimensions; + this.typeMath = typeMath; + Count = 0; + } + + public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) + : this(dimensions, typeMath) + { + AddDuplicateBehavior = addDuplicateBehavior; + } + + private int dimensions; + + private ITypeMath typeMath = null; + + private KdTreeNode root = null; + + public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } + + public bool Add(TKey[] point, TValue value) + { + var nodeToAdd = new KdTreeNode(point, value); + + if (root == null) + { + root = new KdTreeNode(point, value); + } + else + { + int dimension = -1; + KdTreeNode parent = root; + + do + { + // Increment the dimension we're searching in + dimension = (dimension + 1) % dimensions; + + // Does the node we're adding have the same hyperpoint as this node? + if (typeMath.AreEqual(point, parent.Point)) + { + switch (AddDuplicateBehavior) + { + case AddDuplicateBehavior.Skip: + return false; + + case AddDuplicateBehavior.Error: + throw new DuplicateNodeError(); + + case AddDuplicateBehavior.Update: + parent.Value = value; + return true; + + default: + // Should never happen + throw new Exception("Unexpected AddDuplicateBehavior"); + } + } + + // Which side does this node sit under in relation to it's parent at this level? + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + + if (parent[compare] == null) + { + parent[compare] = nodeToAdd; + break; + } + else + { + parent = parent[compare]; + } + } + while (true); + } + + Count++; + return true; + } + + private void ReaddChildNodes(KdTreeNode removedNode) + { + if (removedNode.IsLeaf) + return; + + // The folllowing code might seem a little redundant but we're using + // 2 queues so we can add the child nodes back in, in (more or less) + // the same order they were added in the first place + var nodesToReadd = new Queue>(); + + var nodesToReaddQueue = new Queue>(); + + if (removedNode.LeftChild != null) + nodesToReaddQueue.Enqueue(removedNode.LeftChild); + + if (removedNode.RightChild != null) + nodesToReaddQueue.Enqueue(removedNode.RightChild); + + while (nodesToReaddQueue.Count > 0) + { + var nodeToReadd = nodesToReaddQueue.Dequeue(); + + nodesToReadd.Enqueue(nodeToReadd); + + for (int side = -1; side <= 1; side += 2) + { + if (nodeToReadd[side] != null) + { + nodesToReaddQueue.Enqueue(nodeToReadd[side]); + + nodeToReadd[side] = null; + } + } + } + + while (nodesToReadd.Count > 0) + { + var nodeToReadd = nodesToReadd.Dequeue(); + + Count--; + Add(nodeToReadd.Point, nodeToReadd.Value); + } + } + + public void RemoveAt(TKey[] point) + { + // Is tree empty? + if (root == null) + return; + + KdTreeNode node; + + if (typeMath.AreEqual(point, root.Point)) + { + node = root; + root = null; + Count--; + ReaddChildNodes(node); + return; + } + + node = root; + + int dimension = -1; + do + { + dimension = (dimension + 1) % dimensions; + + int compare = typeMath.Compare(point[dimension], node.Point[dimension]); + + if (node[compare] == null) + // Can't find node + return; + + if (typeMath.AreEqual(point, node[compare].Point)) + { + var nodeToRemove = node[compare]; + node[compare] = null; + Count--; + + ReaddChildNodes(nodeToRemove); + } + else + node = node[compare]; + } + while (node != null); + } + + public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) + { + if (count > Count) + count = Count; + + if (count < 0) + { + throw new ArgumentException("Number of neighbors cannot be negative"); + } + + if (count == 0) + return new KdTreeNode[0]; + + var neighbours = new KdTreeNode[count]; + + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + + var rect = HyperRect.Infinite(dimensions, typeMath); + + AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); + + count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + /* + * 1. Search for the target + * + * 1.1 Start by splitting the specified hyper rect + * on the specified node's point along the current + * dimension so that we end up with 2 sub hyper rects + * (current dimension = depth % dimensions) + * + * 1.2 Check what sub rectangle the the target point resides in + * under the current dimension + * + * 1.3 Set that rect to the nearer rect and also the corresponding + * child node to the nearest rect and node and the other rect + * and child node to the further rect and child node (for use later) + * + * 1.4 Travel into the nearer rect and node by calling function + * recursively with nearer rect and node and incrementing + * the depth + * + * 2. Add leaf to list of nearest neighbours + * + * 3. Walk back up tree and at each level: + * + * 3.1 Add node to nearest neighbours if + * we haven't filled our nearest neighbour + * list yet or if it has a distance to target less + * than any of the distances in our current nearest + * neighbours. + * + * 3.2 If there is any point in the further rectangle that is closer to + * the target than our furtherest nearest neighbour then travel into + * that rect and node + * + * That's it, when it finally finishes traversing the branches + * it needs to we'll have our list! + */ + + private void AddNearestNeighbours( + KdTreeNode node, + TKey[] target, + HyperRect rect, + int depth, + NearestNeighbourList, TKey> nearestNeighbours, + TKey maxSearchRadiusSquared) + { + if (node == null) + return; + + // Work out the current dimension + int dimension = depth % dimensions; + + // Split our hyper-rect into 2 sub rects along the current + // node's point on the current dimension + var leftRect = rect.Clone(); + leftRect.MaxPoint[dimension] = node.Point[dimension]; + + var rightRect = rect.Clone(); + rightRect.MinPoint[dimension] = node.Point[dimension]; + + // Which side does the target reside in? + int compare = typeMath.Compare(target[dimension], node.Point[dimension]); + + var nearerRect = compare <= 0 ? leftRect : rightRect; + var furtherRect = compare <= 0 ? rightRect : leftRect; + + var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; + var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; + + // Let's walk down into the nearer branch + if (nearerNode != null) + { + AddNearestNeighbours( + nearerNode, + target, + nearerRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + + TKey distanceSquaredToTarget; + + // Walk down into the further branch but only if our capacity hasn't been reached + // OR if there's a region in the further rect that's closer to the target than our + // current furtherest nearest neighbour + TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + { + if (nearestNeighbours.IsCapacityReached) + { + if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + else + { + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + } + + // Try to add the current node to our nearest neighbours list + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + nearestNeighbours.Add(node, distanceSquaredToTarget); + } + + /// + /// Performs a radial search. + /// + /// Center point + /// Radius to find neighbours within + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + /// + /// Performs a radial search up to a maximum count. + /// + /// Center point + /// Radius to find neighbours within + /// Maximum number of neighbours + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + private KdTreeNode[] RadialSearch(TKey[] center, TKey radius, NearestNeighbourList, TKey> nearestNeighbours) + { + AddNearestNeighbours( + root, + center, + HyperRect.Infinite(dimensions, typeMath), + 0, + nearestNeighbours, + typeMath.Multiply(radius, radius)); + + var count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + public int Count { get; private set; } + + public bool TryFindValueAt(TKey[] point, out TValue value) + { + var parent = root; + int dimension = -1; + do + { + if (parent == null) + { + value = default(TValue); + return false; + } + else if (typeMath.AreEqual(point, parent.Point)) + { + value = parent.Value; + return true; + } + + // Keep searching + dimension = (dimension + 1) % dimensions; + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + parent = parent[compare]; + } + while (true); + } + + public TValue FindValueAt(TKey[] point) + { + if (TryFindValueAt(point, out TValue value)) + return value; + else + return default(TValue); + } + + public bool TryFindValue(TValue value, out TKey[] point) + { + if (root == null) + { + point = null; + return false; + } + + // First-in, First-out list of nodes to search + var nodesToSearch = new Queue>(); + + nodesToSearch.Enqueue(root); + + while (nodesToSearch.Count > 0) + { + var nodeToSearch = nodesToSearch.Dequeue(); + + if (nodeToSearch.Value.Equals(value)) + { + point = nodeToSearch.Point; + return true; + } + else + { + for (int side = -1; side <= 1; side += 2) + { + var childNode = nodeToSearch[side]; + + if (childNode != null) + nodesToSearch.Enqueue(childNode); + } + } + } + + point = null; + return false; + } + + public TKey[] FindValue(TValue value) + { + if (TryFindValue(value, out TKey[] point)) + return point; + else + return null; + } + + private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) + { + sb.AppendLine(node.ToString()); + + for (var side = -1; side <= 1; side += 2) + { + for (var index = 0; index <= depth; index++) + sb.Append("\t"); + + sb.Append(side == -1 ? "L " : "R "); + + if (node[side] == null) + sb.AppendLine(""); + else + AddNodeToStringBuilder(node[side], sb, depth + 1); + } + } + + public override string ToString() + { + if (root == null) + return ""; + + var sb = new StringBuilder(); + AddNodeToStringBuilder(root, sb, 0); + return sb.ToString(); + } + + private void AddNodesToList(KdTreeNode node, List> nodes) + { + if (node == null) + return; + + nodes.Add(node); + + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + AddNodesToList(node[side], nodes); + node[side] = null; + } + } + } + + private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + for (var index = fromIndex + 1; index <= toIndex; index++) + { + var newIndex = index; + + while (true) + { + var a = nodes[newIndex - 1]; + var b = nodes[newIndex]; + if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) + { + nodes[newIndex - 1] = b; + nodes[newIndex] = a; + } + else + break; + } + } + } + + private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + if (fromIndex == toIndex) + { + Add(nodes[fromIndex].Point, nodes[fromIndex].Value); + nodes[fromIndex] = null; + return; + } + + // Sort the array from the fromIndex to the toIndex + SortNodesArray(nodes, byDimension, fromIndex, toIndex); + + // Find the splitting point + int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; + + // Add the splitting point + Add(nodes[midIndex].Point, nodes[midIndex].Value); + nodes[midIndex] = null; + + // Recurse + int nextDimension = (byDimension + 1) % dimensions; + + if (fromIndex < midIndex) + AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); + + if (toIndex > midIndex) + AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); + } + + public void Balance() + { + var nodeList = new List>(); + AddNodesToList(root, nodeList); + + Clear(); + + AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); + } + + private void RemoveChildNodes(KdTreeNode node) + { + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + RemoveChildNodes(node[side]); + node[side] = null; + } + } + } + + public void Clear() + { + if (root != null) + RemoveChildNodes(root); + } + + public void SaveToFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Create(filename)) + { + formatter.Serialize(stream, this); + stream.Flush(); + } + } + + public static KdTree LoadFromFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Open(filename, FileMode.Open)) + { + return (KdTree)formatter.Deserialize(stream); + } + + } + + public IEnumerator> GetEnumerator() + { + var left = new Stack>(); + var right = new Stack>(); + + void addLeft(KdTreeNode node) + { + if (node.LeftChild != null) + { + left.Push(node.LeftChild); + } + } + + void addRight(KdTreeNode node) + { + if (node.RightChild != null) + { + right.Push(node.RightChild); + } + } + + if (root != null) + { + yield return root; + + addLeft(root); + addRight(root); + + while (true) + { + if (left.Any()) + { + var item = left.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else if (right.Any()) + { + var item = right.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else + { + break; + } + } + } + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTree.cs.meta b/Assets/KdTreeLib/KdTree.cs.meta new file mode 100755 index 0000000..d7f1d14 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b61eae78ccd40f0459eb72df19d37003 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTreeNode.cs b/Assets/KdTreeLib/KdTreeNode.cs new file mode 100755 index 0000000..e08f24d --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs @@ -0,0 +1,68 @@ +using System; +using System.Text; + +namespace KdTree +{ + [Serializable] + public class KdTreeNode + { + public KdTreeNode() + { + } + + public KdTreeNode(TKey[] point, TValue value) + { + Point = point; + Value = value; + } + + public TKey[] Point; + public TValue Value = default(TValue); + + internal KdTreeNode LeftChild = null; + internal KdTreeNode RightChild = null; + + internal KdTreeNode this[int compare] + { + get + { + if (compare <= 0) + return LeftChild; + else + return RightChild; + } + set + { + if (compare <= 0) + LeftChild = value; + else + RightChild = value; + } + } + + public bool IsLeaf + { + get + { + return (LeftChild == null) && (RightChild == null); + } + } + + public override string ToString() + { + var sb = new StringBuilder(); + + for (var dimension = 0; dimension < Point.Length; dimension++) + { + sb.Append(Point[dimension].ToString() + "\t"); + } + + if (Value == null) + sb.Append("null"); + else + sb.Append(Value.ToString()); + + return sb.ToString(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTreeNode.cs.meta b/Assets/KdTreeLib/KdTreeNode.cs.meta new file mode 100755 index 0000000..7fa00e6 --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c70bc7c92452a3b4e8de75cf4a32e85f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math.meta b/Assets/KdTreeLib/Math.meta new file mode 100755 index 0000000..7e307d1 --- /dev/null +++ b/Assets/KdTreeLib/Math.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 250be181763ace04ebe9e51229d5b664 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs b/Assets/KdTreeLib/Math/DoubleMath.cs new file mode 100755 index 0000000..a3a95a3 --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class DoubleMath : TypeMath + { + public override int Compare(double a, double b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(double a, double b) + { + return a == b; + } + + public override double MinValue + { + get { return double.MinValue; } + } + + public override double MaxValue + { + get { return double.MaxValue; } + } + + public override double Zero + { + get { return 0; } + } + + public override double NegativeInfinity { get { return double.NegativeInfinity; } } + + public override double PositiveInfinity { get { return double.PositiveInfinity; } } + + public override double Add(double a, double b) + { + return a + b; + } + + public override double Subtract(double a, double b) + { + return a - b; + } + + public override double Multiply(double a, double b) + { + return a * b; + } + + public override double DistanceSquaredBetweenPoints(double[] a, double[] b) + { + double distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + double distOnThisAxis = Subtract(a[dimension], b[dimension]); + double distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs.meta b/Assets/KdTreeLib/Math/DoubleMath.cs.meta new file mode 100755 index 0000000..e0e7d9d --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6878fdbadd195164393a12f522125dec +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/FloatMath.cs b/Assets/KdTreeLib/Math/FloatMath.cs new file mode 100755 index 0000000..e530cca --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class FloatMath : TypeMath + { + public override int Compare(float a, float b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(float a, float b) + { + return a == b; + } + + public override float MinValue + { + get { return float.MinValue; } + } + + public override float MaxValue + { + get { return float.MaxValue; } + } + + public override float Zero + { + get { return 0; } + } + + public override float NegativeInfinity { get { return float.NegativeInfinity; } } + + public override float PositiveInfinity { get { return float.PositiveInfinity; } } + + public override float Add(float a, float b) + { + return a + b; + } + + public override float Subtract(float a, float b) + { + return a - b; + } + + public override float Multiply(float a, float b) + { + return a * b; + } + + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + float distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + float distOnThisAxis = Subtract(a[dimension], b[dimension]); + float distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/FloatMath.cs.meta b/Assets/KdTreeLib/Math/FloatMath.cs.meta new file mode 100755 index 0000000..d1e609c --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e1a4895e13ce1b64491965b3a4b65201 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoMath.cs b/Assets/KdTreeLib/Math/GeoMath.cs new file mode 100755 index 0000000..7a71408 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs @@ -0,0 +1,14 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class GeoMath : FloatMath + { + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + double dst = GeoUtils.Distance(a[0], a[1], b[0], b[1], 'K'); + return (float)(dst * dst); + } + } +} diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IPriorityQueue.cs.meta b/Assets/KdTreeLib/IPriorityQueue.cs.meta new file mode 100755 index 0000000..e37c0b2 --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0551921b902b13842aa2e99c95aa537c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTree.cs b/Assets/KdTreeLib/KdTree.cs new file mode 100755 index 0000000..4dfe924 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs @@ -0,0 +1,676 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Runtime.Serialization.Formatters.Binary; +using System.Text; + +namespace KdTree +{ + public enum AddDuplicateBehavior + { + Skip, + Error, + Update + } + + public class DuplicateNodeError : Exception + { + public DuplicateNodeError() + : base("Cannot Add Node With Duplicate Coordinates") + { + } + } + + [Serializable] + public class KdTree : IKdTree + { + public KdTree(int dimensions, ITypeMath typeMath) + { + this.dimensions = dimensions; + this.typeMath = typeMath; + Count = 0; + } + + public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) + : this(dimensions, typeMath) + { + AddDuplicateBehavior = addDuplicateBehavior; + } + + private int dimensions; + + private ITypeMath typeMath = null; + + private KdTreeNode root = null; + + public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } + + public bool Add(TKey[] point, TValue value) + { + var nodeToAdd = new KdTreeNode(point, value); + + if (root == null) + { + root = new KdTreeNode(point, value); + } + else + { + int dimension = -1; + KdTreeNode parent = root; + + do + { + // Increment the dimension we're searching in + dimension = (dimension + 1) % dimensions; + + // Does the node we're adding have the same hyperpoint as this node? + if (typeMath.AreEqual(point, parent.Point)) + { + switch (AddDuplicateBehavior) + { + case AddDuplicateBehavior.Skip: + return false; + + case AddDuplicateBehavior.Error: + throw new DuplicateNodeError(); + + case AddDuplicateBehavior.Update: + parent.Value = value; + return true; + + default: + // Should never happen + throw new Exception("Unexpected AddDuplicateBehavior"); + } + } + + // Which side does this node sit under in relation to it's parent at this level? + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + + if (parent[compare] == null) + { + parent[compare] = nodeToAdd; + break; + } + else + { + parent = parent[compare]; + } + } + while (true); + } + + Count++; + return true; + } + + private void ReaddChildNodes(KdTreeNode removedNode) + { + if (removedNode.IsLeaf) + return; + + // The folllowing code might seem a little redundant but we're using + // 2 queues so we can add the child nodes back in, in (more or less) + // the same order they were added in the first place + var nodesToReadd = new Queue>(); + + var nodesToReaddQueue = new Queue>(); + + if (removedNode.LeftChild != null) + nodesToReaddQueue.Enqueue(removedNode.LeftChild); + + if (removedNode.RightChild != null) + nodesToReaddQueue.Enqueue(removedNode.RightChild); + + while (nodesToReaddQueue.Count > 0) + { + var nodeToReadd = nodesToReaddQueue.Dequeue(); + + nodesToReadd.Enqueue(nodeToReadd); + + for (int side = -1; side <= 1; side += 2) + { + if (nodeToReadd[side] != null) + { + nodesToReaddQueue.Enqueue(nodeToReadd[side]); + + nodeToReadd[side] = null; + } + } + } + + while (nodesToReadd.Count > 0) + { + var nodeToReadd = nodesToReadd.Dequeue(); + + Count--; + Add(nodeToReadd.Point, nodeToReadd.Value); + } + } + + public void RemoveAt(TKey[] point) + { + // Is tree empty? + if (root == null) + return; + + KdTreeNode node; + + if (typeMath.AreEqual(point, root.Point)) + { + node = root; + root = null; + Count--; + ReaddChildNodes(node); + return; + } + + node = root; + + int dimension = -1; + do + { + dimension = (dimension + 1) % dimensions; + + int compare = typeMath.Compare(point[dimension], node.Point[dimension]); + + if (node[compare] == null) + // Can't find node + return; + + if (typeMath.AreEqual(point, node[compare].Point)) + { + var nodeToRemove = node[compare]; + node[compare] = null; + Count--; + + ReaddChildNodes(nodeToRemove); + } + else + node = node[compare]; + } + while (node != null); + } + + public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) + { + if (count > Count) + count = Count; + + if (count < 0) + { + throw new ArgumentException("Number of neighbors cannot be negative"); + } + + if (count == 0) + return new KdTreeNode[0]; + + var neighbours = new KdTreeNode[count]; + + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + + var rect = HyperRect.Infinite(dimensions, typeMath); + + AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); + + count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + /* + * 1. Search for the target + * + * 1.1 Start by splitting the specified hyper rect + * on the specified node's point along the current + * dimension so that we end up with 2 sub hyper rects + * (current dimension = depth % dimensions) + * + * 1.2 Check what sub rectangle the the target point resides in + * under the current dimension + * + * 1.3 Set that rect to the nearer rect and also the corresponding + * child node to the nearest rect and node and the other rect + * and child node to the further rect and child node (for use later) + * + * 1.4 Travel into the nearer rect and node by calling function + * recursively with nearer rect and node and incrementing + * the depth + * + * 2. Add leaf to list of nearest neighbours + * + * 3. Walk back up tree and at each level: + * + * 3.1 Add node to nearest neighbours if + * we haven't filled our nearest neighbour + * list yet or if it has a distance to target less + * than any of the distances in our current nearest + * neighbours. + * + * 3.2 If there is any point in the further rectangle that is closer to + * the target than our furtherest nearest neighbour then travel into + * that rect and node + * + * That's it, when it finally finishes traversing the branches + * it needs to we'll have our list! + */ + + private void AddNearestNeighbours( + KdTreeNode node, + TKey[] target, + HyperRect rect, + int depth, + NearestNeighbourList, TKey> nearestNeighbours, + TKey maxSearchRadiusSquared) + { + if (node == null) + return; + + // Work out the current dimension + int dimension = depth % dimensions; + + // Split our hyper-rect into 2 sub rects along the current + // node's point on the current dimension + var leftRect = rect.Clone(); + leftRect.MaxPoint[dimension] = node.Point[dimension]; + + var rightRect = rect.Clone(); + rightRect.MinPoint[dimension] = node.Point[dimension]; + + // Which side does the target reside in? + int compare = typeMath.Compare(target[dimension], node.Point[dimension]); + + var nearerRect = compare <= 0 ? leftRect : rightRect; + var furtherRect = compare <= 0 ? rightRect : leftRect; + + var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; + var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; + + // Let's walk down into the nearer branch + if (nearerNode != null) + { + AddNearestNeighbours( + nearerNode, + target, + nearerRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + + TKey distanceSquaredToTarget; + + // Walk down into the further branch but only if our capacity hasn't been reached + // OR if there's a region in the further rect that's closer to the target than our + // current furtherest nearest neighbour + TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + { + if (nearestNeighbours.IsCapacityReached) + { + if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + else + { + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + } + + // Try to add the current node to our nearest neighbours list + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + nearestNeighbours.Add(node, distanceSquaredToTarget); + } + + /// + /// Performs a radial search. + /// + /// Center point + /// Radius to find neighbours within + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + /// + /// Performs a radial search up to a maximum count. + /// + /// Center point + /// Radius to find neighbours within + /// Maximum number of neighbours + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + private KdTreeNode[] RadialSearch(TKey[] center, TKey radius, NearestNeighbourList, TKey> nearestNeighbours) + { + AddNearestNeighbours( + root, + center, + HyperRect.Infinite(dimensions, typeMath), + 0, + nearestNeighbours, + typeMath.Multiply(radius, radius)); + + var count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + public int Count { get; private set; } + + public bool TryFindValueAt(TKey[] point, out TValue value) + { + var parent = root; + int dimension = -1; + do + { + if (parent == null) + { + value = default(TValue); + return false; + } + else if (typeMath.AreEqual(point, parent.Point)) + { + value = parent.Value; + return true; + } + + // Keep searching + dimension = (dimension + 1) % dimensions; + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + parent = parent[compare]; + } + while (true); + } + + public TValue FindValueAt(TKey[] point) + { + if (TryFindValueAt(point, out TValue value)) + return value; + else + return default(TValue); + } + + public bool TryFindValue(TValue value, out TKey[] point) + { + if (root == null) + { + point = null; + return false; + } + + // First-in, First-out list of nodes to search + var nodesToSearch = new Queue>(); + + nodesToSearch.Enqueue(root); + + while (nodesToSearch.Count > 0) + { + var nodeToSearch = nodesToSearch.Dequeue(); + + if (nodeToSearch.Value.Equals(value)) + { + point = nodeToSearch.Point; + return true; + } + else + { + for (int side = -1; side <= 1; side += 2) + { + var childNode = nodeToSearch[side]; + + if (childNode != null) + nodesToSearch.Enqueue(childNode); + } + } + } + + point = null; + return false; + } + + public TKey[] FindValue(TValue value) + { + if (TryFindValue(value, out TKey[] point)) + return point; + else + return null; + } + + private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) + { + sb.AppendLine(node.ToString()); + + for (var side = -1; side <= 1; side += 2) + { + for (var index = 0; index <= depth; index++) + sb.Append("\t"); + + sb.Append(side == -1 ? "L " : "R "); + + if (node[side] == null) + sb.AppendLine(""); + else + AddNodeToStringBuilder(node[side], sb, depth + 1); + } + } + + public override string ToString() + { + if (root == null) + return ""; + + var sb = new StringBuilder(); + AddNodeToStringBuilder(root, sb, 0); + return sb.ToString(); + } + + private void AddNodesToList(KdTreeNode node, List> nodes) + { + if (node == null) + return; + + nodes.Add(node); + + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + AddNodesToList(node[side], nodes); + node[side] = null; + } + } + } + + private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + for (var index = fromIndex + 1; index <= toIndex; index++) + { + var newIndex = index; + + while (true) + { + var a = nodes[newIndex - 1]; + var b = nodes[newIndex]; + if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) + { + nodes[newIndex - 1] = b; + nodes[newIndex] = a; + } + else + break; + } + } + } + + private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + if (fromIndex == toIndex) + { + Add(nodes[fromIndex].Point, nodes[fromIndex].Value); + nodes[fromIndex] = null; + return; + } + + // Sort the array from the fromIndex to the toIndex + SortNodesArray(nodes, byDimension, fromIndex, toIndex); + + // Find the splitting point + int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; + + // Add the splitting point + Add(nodes[midIndex].Point, nodes[midIndex].Value); + nodes[midIndex] = null; + + // Recurse + int nextDimension = (byDimension + 1) % dimensions; + + if (fromIndex < midIndex) + AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); + + if (toIndex > midIndex) + AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); + } + + public void Balance() + { + var nodeList = new List>(); + AddNodesToList(root, nodeList); + + Clear(); + + AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); + } + + private void RemoveChildNodes(KdTreeNode node) + { + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + RemoveChildNodes(node[side]); + node[side] = null; + } + } + } + + public void Clear() + { + if (root != null) + RemoveChildNodes(root); + } + + public void SaveToFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Create(filename)) + { + formatter.Serialize(stream, this); + stream.Flush(); + } + } + + public static KdTree LoadFromFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Open(filename, FileMode.Open)) + { + return (KdTree)formatter.Deserialize(stream); + } + + } + + public IEnumerator> GetEnumerator() + { + var left = new Stack>(); + var right = new Stack>(); + + void addLeft(KdTreeNode node) + { + if (node.LeftChild != null) + { + left.Push(node.LeftChild); + } + } + + void addRight(KdTreeNode node) + { + if (node.RightChild != null) + { + right.Push(node.RightChild); + } + } + + if (root != null) + { + yield return root; + + addLeft(root); + addRight(root); + + while (true) + { + if (left.Any()) + { + var item = left.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else if (right.Any()) + { + var item = right.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else + { + break; + } + } + } + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTree.cs.meta b/Assets/KdTreeLib/KdTree.cs.meta new file mode 100755 index 0000000..d7f1d14 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b61eae78ccd40f0459eb72df19d37003 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTreeNode.cs b/Assets/KdTreeLib/KdTreeNode.cs new file mode 100755 index 0000000..e08f24d --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs @@ -0,0 +1,68 @@ +using System; +using System.Text; + +namespace KdTree +{ + [Serializable] + public class KdTreeNode + { + public KdTreeNode() + { + } + + public KdTreeNode(TKey[] point, TValue value) + { + Point = point; + Value = value; + } + + public TKey[] Point; + public TValue Value = default(TValue); + + internal KdTreeNode LeftChild = null; + internal KdTreeNode RightChild = null; + + internal KdTreeNode this[int compare] + { + get + { + if (compare <= 0) + return LeftChild; + else + return RightChild; + } + set + { + if (compare <= 0) + LeftChild = value; + else + RightChild = value; + } + } + + public bool IsLeaf + { + get + { + return (LeftChild == null) && (RightChild == null); + } + } + + public override string ToString() + { + var sb = new StringBuilder(); + + for (var dimension = 0; dimension < Point.Length; dimension++) + { + sb.Append(Point[dimension].ToString() + "\t"); + } + + if (Value == null) + sb.Append("null"); + else + sb.Append(Value.ToString()); + + return sb.ToString(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTreeNode.cs.meta b/Assets/KdTreeLib/KdTreeNode.cs.meta new file mode 100755 index 0000000..7fa00e6 --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c70bc7c92452a3b4e8de75cf4a32e85f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math.meta b/Assets/KdTreeLib/Math.meta new file mode 100755 index 0000000..7e307d1 --- /dev/null +++ b/Assets/KdTreeLib/Math.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 250be181763ace04ebe9e51229d5b664 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs b/Assets/KdTreeLib/Math/DoubleMath.cs new file mode 100755 index 0000000..a3a95a3 --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class DoubleMath : TypeMath + { + public override int Compare(double a, double b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(double a, double b) + { + return a == b; + } + + public override double MinValue + { + get { return double.MinValue; } + } + + public override double MaxValue + { + get { return double.MaxValue; } + } + + public override double Zero + { + get { return 0; } + } + + public override double NegativeInfinity { get { return double.NegativeInfinity; } } + + public override double PositiveInfinity { get { return double.PositiveInfinity; } } + + public override double Add(double a, double b) + { + return a + b; + } + + public override double Subtract(double a, double b) + { + return a - b; + } + + public override double Multiply(double a, double b) + { + return a * b; + } + + public override double DistanceSquaredBetweenPoints(double[] a, double[] b) + { + double distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + double distOnThisAxis = Subtract(a[dimension], b[dimension]); + double distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs.meta b/Assets/KdTreeLib/Math/DoubleMath.cs.meta new file mode 100755 index 0000000..e0e7d9d --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6878fdbadd195164393a12f522125dec +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/FloatMath.cs b/Assets/KdTreeLib/Math/FloatMath.cs new file mode 100755 index 0000000..e530cca --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class FloatMath : TypeMath + { + public override int Compare(float a, float b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(float a, float b) + { + return a == b; + } + + public override float MinValue + { + get { return float.MinValue; } + } + + public override float MaxValue + { + get { return float.MaxValue; } + } + + public override float Zero + { + get { return 0; } + } + + public override float NegativeInfinity { get { return float.NegativeInfinity; } } + + public override float PositiveInfinity { get { return float.PositiveInfinity; } } + + public override float Add(float a, float b) + { + return a + b; + } + + public override float Subtract(float a, float b) + { + return a - b; + } + + public override float Multiply(float a, float b) + { + return a * b; + } + + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + float distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + float distOnThisAxis = Subtract(a[dimension], b[dimension]); + float distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/FloatMath.cs.meta b/Assets/KdTreeLib/Math/FloatMath.cs.meta new file mode 100755 index 0000000..d1e609c --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e1a4895e13ce1b64491965b3a4b65201 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoMath.cs b/Assets/KdTreeLib/Math/GeoMath.cs new file mode 100755 index 0000000..7a71408 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs @@ -0,0 +1,14 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class GeoMath : FloatMath + { + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + double dst = GeoUtils.Distance(a[0], a[1], b[0], b[1], 'K'); + return (float)(dst * dst); + } + } +} diff --git a/Assets/KdTreeLib/Math/GeoMath.cs.meta b/Assets/KdTreeLib/Math/GeoMath.cs.meta new file mode 100755 index 0000000..7dd9a54 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 88144d944655c7d4eb1e3286f6ce3eb4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IPriorityQueue.cs.meta b/Assets/KdTreeLib/IPriorityQueue.cs.meta new file mode 100755 index 0000000..e37c0b2 --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0551921b902b13842aa2e99c95aa537c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTree.cs b/Assets/KdTreeLib/KdTree.cs new file mode 100755 index 0000000..4dfe924 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs @@ -0,0 +1,676 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Runtime.Serialization.Formatters.Binary; +using System.Text; + +namespace KdTree +{ + public enum AddDuplicateBehavior + { + Skip, + Error, + Update + } + + public class DuplicateNodeError : Exception + { + public DuplicateNodeError() + : base("Cannot Add Node With Duplicate Coordinates") + { + } + } + + [Serializable] + public class KdTree : IKdTree + { + public KdTree(int dimensions, ITypeMath typeMath) + { + this.dimensions = dimensions; + this.typeMath = typeMath; + Count = 0; + } + + public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) + : this(dimensions, typeMath) + { + AddDuplicateBehavior = addDuplicateBehavior; + } + + private int dimensions; + + private ITypeMath typeMath = null; + + private KdTreeNode root = null; + + public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } + + public bool Add(TKey[] point, TValue value) + { + var nodeToAdd = new KdTreeNode(point, value); + + if (root == null) + { + root = new KdTreeNode(point, value); + } + else + { + int dimension = -1; + KdTreeNode parent = root; + + do + { + // Increment the dimension we're searching in + dimension = (dimension + 1) % dimensions; + + // Does the node we're adding have the same hyperpoint as this node? + if (typeMath.AreEqual(point, parent.Point)) + { + switch (AddDuplicateBehavior) + { + case AddDuplicateBehavior.Skip: + return false; + + case AddDuplicateBehavior.Error: + throw new DuplicateNodeError(); + + case AddDuplicateBehavior.Update: + parent.Value = value; + return true; + + default: + // Should never happen + throw new Exception("Unexpected AddDuplicateBehavior"); + } + } + + // Which side does this node sit under in relation to it's parent at this level? + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + + if (parent[compare] == null) + { + parent[compare] = nodeToAdd; + break; + } + else + { + parent = parent[compare]; + } + } + while (true); + } + + Count++; + return true; + } + + private void ReaddChildNodes(KdTreeNode removedNode) + { + if (removedNode.IsLeaf) + return; + + // The folllowing code might seem a little redundant but we're using + // 2 queues so we can add the child nodes back in, in (more or less) + // the same order they were added in the first place + var nodesToReadd = new Queue>(); + + var nodesToReaddQueue = new Queue>(); + + if (removedNode.LeftChild != null) + nodesToReaddQueue.Enqueue(removedNode.LeftChild); + + if (removedNode.RightChild != null) + nodesToReaddQueue.Enqueue(removedNode.RightChild); + + while (nodesToReaddQueue.Count > 0) + { + var nodeToReadd = nodesToReaddQueue.Dequeue(); + + nodesToReadd.Enqueue(nodeToReadd); + + for (int side = -1; side <= 1; side += 2) + { + if (nodeToReadd[side] != null) + { + nodesToReaddQueue.Enqueue(nodeToReadd[side]); + + nodeToReadd[side] = null; + } + } + } + + while (nodesToReadd.Count > 0) + { + var nodeToReadd = nodesToReadd.Dequeue(); + + Count--; + Add(nodeToReadd.Point, nodeToReadd.Value); + } + } + + public void RemoveAt(TKey[] point) + { + // Is tree empty? + if (root == null) + return; + + KdTreeNode node; + + if (typeMath.AreEqual(point, root.Point)) + { + node = root; + root = null; + Count--; + ReaddChildNodes(node); + return; + } + + node = root; + + int dimension = -1; + do + { + dimension = (dimension + 1) % dimensions; + + int compare = typeMath.Compare(point[dimension], node.Point[dimension]); + + if (node[compare] == null) + // Can't find node + return; + + if (typeMath.AreEqual(point, node[compare].Point)) + { + var nodeToRemove = node[compare]; + node[compare] = null; + Count--; + + ReaddChildNodes(nodeToRemove); + } + else + node = node[compare]; + } + while (node != null); + } + + public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) + { + if (count > Count) + count = Count; + + if (count < 0) + { + throw new ArgumentException("Number of neighbors cannot be negative"); + } + + if (count == 0) + return new KdTreeNode[0]; + + var neighbours = new KdTreeNode[count]; + + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + + var rect = HyperRect.Infinite(dimensions, typeMath); + + AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); + + count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + /* + * 1. Search for the target + * + * 1.1 Start by splitting the specified hyper rect + * on the specified node's point along the current + * dimension so that we end up with 2 sub hyper rects + * (current dimension = depth % dimensions) + * + * 1.2 Check what sub rectangle the the target point resides in + * under the current dimension + * + * 1.3 Set that rect to the nearer rect and also the corresponding + * child node to the nearest rect and node and the other rect + * and child node to the further rect and child node (for use later) + * + * 1.4 Travel into the nearer rect and node by calling function + * recursively with nearer rect and node and incrementing + * the depth + * + * 2. Add leaf to list of nearest neighbours + * + * 3. Walk back up tree and at each level: + * + * 3.1 Add node to nearest neighbours if + * we haven't filled our nearest neighbour + * list yet or if it has a distance to target less + * than any of the distances in our current nearest + * neighbours. + * + * 3.2 If there is any point in the further rectangle that is closer to + * the target than our furtherest nearest neighbour then travel into + * that rect and node + * + * That's it, when it finally finishes traversing the branches + * it needs to we'll have our list! + */ + + private void AddNearestNeighbours( + KdTreeNode node, + TKey[] target, + HyperRect rect, + int depth, + NearestNeighbourList, TKey> nearestNeighbours, + TKey maxSearchRadiusSquared) + { + if (node == null) + return; + + // Work out the current dimension + int dimension = depth % dimensions; + + // Split our hyper-rect into 2 sub rects along the current + // node's point on the current dimension + var leftRect = rect.Clone(); + leftRect.MaxPoint[dimension] = node.Point[dimension]; + + var rightRect = rect.Clone(); + rightRect.MinPoint[dimension] = node.Point[dimension]; + + // Which side does the target reside in? + int compare = typeMath.Compare(target[dimension], node.Point[dimension]); + + var nearerRect = compare <= 0 ? leftRect : rightRect; + var furtherRect = compare <= 0 ? rightRect : leftRect; + + var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; + var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; + + // Let's walk down into the nearer branch + if (nearerNode != null) + { + AddNearestNeighbours( + nearerNode, + target, + nearerRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + + TKey distanceSquaredToTarget; + + // Walk down into the further branch but only if our capacity hasn't been reached + // OR if there's a region in the further rect that's closer to the target than our + // current furtherest nearest neighbour + TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + { + if (nearestNeighbours.IsCapacityReached) + { + if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + else + { + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + } + + // Try to add the current node to our nearest neighbours list + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + nearestNeighbours.Add(node, distanceSquaredToTarget); + } + + /// + /// Performs a radial search. + /// + /// Center point + /// Radius to find neighbours within + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + /// + /// Performs a radial search up to a maximum count. + /// + /// Center point + /// Radius to find neighbours within + /// Maximum number of neighbours + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + private KdTreeNode[] RadialSearch(TKey[] center, TKey radius, NearestNeighbourList, TKey> nearestNeighbours) + { + AddNearestNeighbours( + root, + center, + HyperRect.Infinite(dimensions, typeMath), + 0, + nearestNeighbours, + typeMath.Multiply(radius, radius)); + + var count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + public int Count { get; private set; } + + public bool TryFindValueAt(TKey[] point, out TValue value) + { + var parent = root; + int dimension = -1; + do + { + if (parent == null) + { + value = default(TValue); + return false; + } + else if (typeMath.AreEqual(point, parent.Point)) + { + value = parent.Value; + return true; + } + + // Keep searching + dimension = (dimension + 1) % dimensions; + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + parent = parent[compare]; + } + while (true); + } + + public TValue FindValueAt(TKey[] point) + { + if (TryFindValueAt(point, out TValue value)) + return value; + else + return default(TValue); + } + + public bool TryFindValue(TValue value, out TKey[] point) + { + if (root == null) + { + point = null; + return false; + } + + // First-in, First-out list of nodes to search + var nodesToSearch = new Queue>(); + + nodesToSearch.Enqueue(root); + + while (nodesToSearch.Count > 0) + { + var nodeToSearch = nodesToSearch.Dequeue(); + + if (nodeToSearch.Value.Equals(value)) + { + point = nodeToSearch.Point; + return true; + } + else + { + for (int side = -1; side <= 1; side += 2) + { + var childNode = nodeToSearch[side]; + + if (childNode != null) + nodesToSearch.Enqueue(childNode); + } + } + } + + point = null; + return false; + } + + public TKey[] FindValue(TValue value) + { + if (TryFindValue(value, out TKey[] point)) + return point; + else + return null; + } + + private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) + { + sb.AppendLine(node.ToString()); + + for (var side = -1; side <= 1; side += 2) + { + for (var index = 0; index <= depth; index++) + sb.Append("\t"); + + sb.Append(side == -1 ? "L " : "R "); + + if (node[side] == null) + sb.AppendLine(""); + else + AddNodeToStringBuilder(node[side], sb, depth + 1); + } + } + + public override string ToString() + { + if (root == null) + return ""; + + var sb = new StringBuilder(); + AddNodeToStringBuilder(root, sb, 0); + return sb.ToString(); + } + + private void AddNodesToList(KdTreeNode node, List> nodes) + { + if (node == null) + return; + + nodes.Add(node); + + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + AddNodesToList(node[side], nodes); + node[side] = null; + } + } + } + + private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + for (var index = fromIndex + 1; index <= toIndex; index++) + { + var newIndex = index; + + while (true) + { + var a = nodes[newIndex - 1]; + var b = nodes[newIndex]; + if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) + { + nodes[newIndex - 1] = b; + nodes[newIndex] = a; + } + else + break; + } + } + } + + private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + if (fromIndex == toIndex) + { + Add(nodes[fromIndex].Point, nodes[fromIndex].Value); + nodes[fromIndex] = null; + return; + } + + // Sort the array from the fromIndex to the toIndex + SortNodesArray(nodes, byDimension, fromIndex, toIndex); + + // Find the splitting point + int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; + + // Add the splitting point + Add(nodes[midIndex].Point, nodes[midIndex].Value); + nodes[midIndex] = null; + + // Recurse + int nextDimension = (byDimension + 1) % dimensions; + + if (fromIndex < midIndex) + AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); + + if (toIndex > midIndex) + AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); + } + + public void Balance() + { + var nodeList = new List>(); + AddNodesToList(root, nodeList); + + Clear(); + + AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); + } + + private void RemoveChildNodes(KdTreeNode node) + { + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + RemoveChildNodes(node[side]); + node[side] = null; + } + } + } + + public void Clear() + { + if (root != null) + RemoveChildNodes(root); + } + + public void SaveToFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Create(filename)) + { + formatter.Serialize(stream, this); + stream.Flush(); + } + } + + public static KdTree LoadFromFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Open(filename, FileMode.Open)) + { + return (KdTree)formatter.Deserialize(stream); + } + + } + + public IEnumerator> GetEnumerator() + { + var left = new Stack>(); + var right = new Stack>(); + + void addLeft(KdTreeNode node) + { + if (node.LeftChild != null) + { + left.Push(node.LeftChild); + } + } + + void addRight(KdTreeNode node) + { + if (node.RightChild != null) + { + right.Push(node.RightChild); + } + } + + if (root != null) + { + yield return root; + + addLeft(root); + addRight(root); + + while (true) + { + if (left.Any()) + { + var item = left.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else if (right.Any()) + { + var item = right.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else + { + break; + } + } + } + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTree.cs.meta b/Assets/KdTreeLib/KdTree.cs.meta new file mode 100755 index 0000000..d7f1d14 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b61eae78ccd40f0459eb72df19d37003 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTreeNode.cs b/Assets/KdTreeLib/KdTreeNode.cs new file mode 100755 index 0000000..e08f24d --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs @@ -0,0 +1,68 @@ +using System; +using System.Text; + +namespace KdTree +{ + [Serializable] + public class KdTreeNode + { + public KdTreeNode() + { + } + + public KdTreeNode(TKey[] point, TValue value) + { + Point = point; + Value = value; + } + + public TKey[] Point; + public TValue Value = default(TValue); + + internal KdTreeNode LeftChild = null; + internal KdTreeNode RightChild = null; + + internal KdTreeNode this[int compare] + { + get + { + if (compare <= 0) + return LeftChild; + else + return RightChild; + } + set + { + if (compare <= 0) + LeftChild = value; + else + RightChild = value; + } + } + + public bool IsLeaf + { + get + { + return (LeftChild == null) && (RightChild == null); + } + } + + public override string ToString() + { + var sb = new StringBuilder(); + + for (var dimension = 0; dimension < Point.Length; dimension++) + { + sb.Append(Point[dimension].ToString() + "\t"); + } + + if (Value == null) + sb.Append("null"); + else + sb.Append(Value.ToString()); + + return sb.ToString(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTreeNode.cs.meta b/Assets/KdTreeLib/KdTreeNode.cs.meta new file mode 100755 index 0000000..7fa00e6 --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c70bc7c92452a3b4e8de75cf4a32e85f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math.meta b/Assets/KdTreeLib/Math.meta new file mode 100755 index 0000000..7e307d1 --- /dev/null +++ b/Assets/KdTreeLib/Math.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 250be181763ace04ebe9e51229d5b664 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs b/Assets/KdTreeLib/Math/DoubleMath.cs new file mode 100755 index 0000000..a3a95a3 --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class DoubleMath : TypeMath + { + public override int Compare(double a, double b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(double a, double b) + { + return a == b; + } + + public override double MinValue + { + get { return double.MinValue; } + } + + public override double MaxValue + { + get { return double.MaxValue; } + } + + public override double Zero + { + get { return 0; } + } + + public override double NegativeInfinity { get { return double.NegativeInfinity; } } + + public override double PositiveInfinity { get { return double.PositiveInfinity; } } + + public override double Add(double a, double b) + { + return a + b; + } + + public override double Subtract(double a, double b) + { + return a - b; + } + + public override double Multiply(double a, double b) + { + return a * b; + } + + public override double DistanceSquaredBetweenPoints(double[] a, double[] b) + { + double distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + double distOnThisAxis = Subtract(a[dimension], b[dimension]); + double distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs.meta b/Assets/KdTreeLib/Math/DoubleMath.cs.meta new file mode 100755 index 0000000..e0e7d9d --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6878fdbadd195164393a12f522125dec +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/FloatMath.cs b/Assets/KdTreeLib/Math/FloatMath.cs new file mode 100755 index 0000000..e530cca --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class FloatMath : TypeMath + { + public override int Compare(float a, float b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(float a, float b) + { + return a == b; + } + + public override float MinValue + { + get { return float.MinValue; } + } + + public override float MaxValue + { + get { return float.MaxValue; } + } + + public override float Zero + { + get { return 0; } + } + + public override float NegativeInfinity { get { return float.NegativeInfinity; } } + + public override float PositiveInfinity { get { return float.PositiveInfinity; } } + + public override float Add(float a, float b) + { + return a + b; + } + + public override float Subtract(float a, float b) + { + return a - b; + } + + public override float Multiply(float a, float b) + { + return a * b; + } + + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + float distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + float distOnThisAxis = Subtract(a[dimension], b[dimension]); + float distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/FloatMath.cs.meta b/Assets/KdTreeLib/Math/FloatMath.cs.meta new file mode 100755 index 0000000..d1e609c --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e1a4895e13ce1b64491965b3a4b65201 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoMath.cs b/Assets/KdTreeLib/Math/GeoMath.cs new file mode 100755 index 0000000..7a71408 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs @@ -0,0 +1,14 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class GeoMath : FloatMath + { + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + double dst = GeoUtils.Distance(a[0], a[1], b[0], b[1], 'K'); + return (float)(dst * dst); + } + } +} diff --git a/Assets/KdTreeLib/Math/GeoMath.cs.meta b/Assets/KdTreeLib/Math/GeoMath.cs.meta new file mode 100755 index 0000000..7dd9a54 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 88144d944655c7d4eb1e3286f6ce3eb4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoUtils.cs b/Assets/KdTreeLib/Math/GeoUtils.cs new file mode 100755 index 0000000..e593bb7 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoUtils.cs @@ -0,0 +1,42 @@ +namespace KdTree.Math +{ + // Via http://www.geodatasource.com/developers/c-sharp + // This code is licensed under LGPLv3. + public class GeoUtils + { + public static double Distance(double lat1, double lon1, double lat2, double lon2, char unit) + { + double theta = lon1 - lon2; + double dist = System.Math.Sin(Deg2rad(lat1)) * System.Math.Sin(Deg2rad(lat2)) + System.Math.Cos(Deg2rad(lat1)) * System.Math.Cos(Deg2rad(lat2)) * System.Math.Cos(Deg2rad(theta)); + dist = System.Math.Acos(dist); + dist = Rad2deg(dist); + dist = dist * 60 * 1.1515; + if (unit == 'K') + { + dist = dist * 1.609344; + } + else if (unit == 'N') + { + dist = dist * 0.8684; + } + return (dist); + } + + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + //:: This function converts decimal degrees to radians ::: + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + private static double Deg2rad(double deg) + { + return (deg * System.Math.PI / 180.0); + } + + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + //:: This function converts radians to decimal degrees ::: + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + private static double Rad2deg(double rad) + { + return (rad / System.Math.PI * 180.0); + } + + } +} diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IPriorityQueue.cs.meta b/Assets/KdTreeLib/IPriorityQueue.cs.meta new file mode 100755 index 0000000..e37c0b2 --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0551921b902b13842aa2e99c95aa537c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTree.cs b/Assets/KdTreeLib/KdTree.cs new file mode 100755 index 0000000..4dfe924 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs @@ -0,0 +1,676 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Runtime.Serialization.Formatters.Binary; +using System.Text; + +namespace KdTree +{ + public enum AddDuplicateBehavior + { + Skip, + Error, + Update + } + + public class DuplicateNodeError : Exception + { + public DuplicateNodeError() + : base("Cannot Add Node With Duplicate Coordinates") + { + } + } + + [Serializable] + public class KdTree : IKdTree + { + public KdTree(int dimensions, ITypeMath typeMath) + { + this.dimensions = dimensions; + this.typeMath = typeMath; + Count = 0; + } + + public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) + : this(dimensions, typeMath) + { + AddDuplicateBehavior = addDuplicateBehavior; + } + + private int dimensions; + + private ITypeMath typeMath = null; + + private KdTreeNode root = null; + + public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } + + public bool Add(TKey[] point, TValue value) + { + var nodeToAdd = new KdTreeNode(point, value); + + if (root == null) + { + root = new KdTreeNode(point, value); + } + else + { + int dimension = -1; + KdTreeNode parent = root; + + do + { + // Increment the dimension we're searching in + dimension = (dimension + 1) % dimensions; + + // Does the node we're adding have the same hyperpoint as this node? + if (typeMath.AreEqual(point, parent.Point)) + { + switch (AddDuplicateBehavior) + { + case AddDuplicateBehavior.Skip: + return false; + + case AddDuplicateBehavior.Error: + throw new DuplicateNodeError(); + + case AddDuplicateBehavior.Update: + parent.Value = value; + return true; + + default: + // Should never happen + throw new Exception("Unexpected AddDuplicateBehavior"); + } + } + + // Which side does this node sit under in relation to it's parent at this level? + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + + if (parent[compare] == null) + { + parent[compare] = nodeToAdd; + break; + } + else + { + parent = parent[compare]; + } + } + while (true); + } + + Count++; + return true; + } + + private void ReaddChildNodes(KdTreeNode removedNode) + { + if (removedNode.IsLeaf) + return; + + // The folllowing code might seem a little redundant but we're using + // 2 queues so we can add the child nodes back in, in (more or less) + // the same order they were added in the first place + var nodesToReadd = new Queue>(); + + var nodesToReaddQueue = new Queue>(); + + if (removedNode.LeftChild != null) + nodesToReaddQueue.Enqueue(removedNode.LeftChild); + + if (removedNode.RightChild != null) + nodesToReaddQueue.Enqueue(removedNode.RightChild); + + while (nodesToReaddQueue.Count > 0) + { + var nodeToReadd = nodesToReaddQueue.Dequeue(); + + nodesToReadd.Enqueue(nodeToReadd); + + for (int side = -1; side <= 1; side += 2) + { + if (nodeToReadd[side] != null) + { + nodesToReaddQueue.Enqueue(nodeToReadd[side]); + + nodeToReadd[side] = null; + } + } + } + + while (nodesToReadd.Count > 0) + { + var nodeToReadd = nodesToReadd.Dequeue(); + + Count--; + Add(nodeToReadd.Point, nodeToReadd.Value); + } + } + + public void RemoveAt(TKey[] point) + { + // Is tree empty? + if (root == null) + return; + + KdTreeNode node; + + if (typeMath.AreEqual(point, root.Point)) + { + node = root; + root = null; + Count--; + ReaddChildNodes(node); + return; + } + + node = root; + + int dimension = -1; + do + { + dimension = (dimension + 1) % dimensions; + + int compare = typeMath.Compare(point[dimension], node.Point[dimension]); + + if (node[compare] == null) + // Can't find node + return; + + if (typeMath.AreEqual(point, node[compare].Point)) + { + var nodeToRemove = node[compare]; + node[compare] = null; + Count--; + + ReaddChildNodes(nodeToRemove); + } + else + node = node[compare]; + } + while (node != null); + } + + public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) + { + if (count > Count) + count = Count; + + if (count < 0) + { + throw new ArgumentException("Number of neighbors cannot be negative"); + } + + if (count == 0) + return new KdTreeNode[0]; + + var neighbours = new KdTreeNode[count]; + + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + + var rect = HyperRect.Infinite(dimensions, typeMath); + + AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); + + count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + /* + * 1. Search for the target + * + * 1.1 Start by splitting the specified hyper rect + * on the specified node's point along the current + * dimension so that we end up with 2 sub hyper rects + * (current dimension = depth % dimensions) + * + * 1.2 Check what sub rectangle the the target point resides in + * under the current dimension + * + * 1.3 Set that rect to the nearer rect and also the corresponding + * child node to the nearest rect and node and the other rect + * and child node to the further rect and child node (for use later) + * + * 1.4 Travel into the nearer rect and node by calling function + * recursively with nearer rect and node and incrementing + * the depth + * + * 2. Add leaf to list of nearest neighbours + * + * 3. Walk back up tree and at each level: + * + * 3.1 Add node to nearest neighbours if + * we haven't filled our nearest neighbour + * list yet or if it has a distance to target less + * than any of the distances in our current nearest + * neighbours. + * + * 3.2 If there is any point in the further rectangle that is closer to + * the target than our furtherest nearest neighbour then travel into + * that rect and node + * + * That's it, when it finally finishes traversing the branches + * it needs to we'll have our list! + */ + + private void AddNearestNeighbours( + KdTreeNode node, + TKey[] target, + HyperRect rect, + int depth, + NearestNeighbourList, TKey> nearestNeighbours, + TKey maxSearchRadiusSquared) + { + if (node == null) + return; + + // Work out the current dimension + int dimension = depth % dimensions; + + // Split our hyper-rect into 2 sub rects along the current + // node's point on the current dimension + var leftRect = rect.Clone(); + leftRect.MaxPoint[dimension] = node.Point[dimension]; + + var rightRect = rect.Clone(); + rightRect.MinPoint[dimension] = node.Point[dimension]; + + // Which side does the target reside in? + int compare = typeMath.Compare(target[dimension], node.Point[dimension]); + + var nearerRect = compare <= 0 ? leftRect : rightRect; + var furtherRect = compare <= 0 ? rightRect : leftRect; + + var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; + var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; + + // Let's walk down into the nearer branch + if (nearerNode != null) + { + AddNearestNeighbours( + nearerNode, + target, + nearerRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + + TKey distanceSquaredToTarget; + + // Walk down into the further branch but only if our capacity hasn't been reached + // OR if there's a region in the further rect that's closer to the target than our + // current furtherest nearest neighbour + TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + { + if (nearestNeighbours.IsCapacityReached) + { + if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + else + { + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + } + + // Try to add the current node to our nearest neighbours list + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + nearestNeighbours.Add(node, distanceSquaredToTarget); + } + + /// + /// Performs a radial search. + /// + /// Center point + /// Radius to find neighbours within + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + /// + /// Performs a radial search up to a maximum count. + /// + /// Center point + /// Radius to find neighbours within + /// Maximum number of neighbours + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + private KdTreeNode[] RadialSearch(TKey[] center, TKey radius, NearestNeighbourList, TKey> nearestNeighbours) + { + AddNearestNeighbours( + root, + center, + HyperRect.Infinite(dimensions, typeMath), + 0, + nearestNeighbours, + typeMath.Multiply(radius, radius)); + + var count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + public int Count { get; private set; } + + public bool TryFindValueAt(TKey[] point, out TValue value) + { + var parent = root; + int dimension = -1; + do + { + if (parent == null) + { + value = default(TValue); + return false; + } + else if (typeMath.AreEqual(point, parent.Point)) + { + value = parent.Value; + return true; + } + + // Keep searching + dimension = (dimension + 1) % dimensions; + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + parent = parent[compare]; + } + while (true); + } + + public TValue FindValueAt(TKey[] point) + { + if (TryFindValueAt(point, out TValue value)) + return value; + else + return default(TValue); + } + + public bool TryFindValue(TValue value, out TKey[] point) + { + if (root == null) + { + point = null; + return false; + } + + // First-in, First-out list of nodes to search + var nodesToSearch = new Queue>(); + + nodesToSearch.Enqueue(root); + + while (nodesToSearch.Count > 0) + { + var nodeToSearch = nodesToSearch.Dequeue(); + + if (nodeToSearch.Value.Equals(value)) + { + point = nodeToSearch.Point; + return true; + } + else + { + for (int side = -1; side <= 1; side += 2) + { + var childNode = nodeToSearch[side]; + + if (childNode != null) + nodesToSearch.Enqueue(childNode); + } + } + } + + point = null; + return false; + } + + public TKey[] FindValue(TValue value) + { + if (TryFindValue(value, out TKey[] point)) + return point; + else + return null; + } + + private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) + { + sb.AppendLine(node.ToString()); + + for (var side = -1; side <= 1; side += 2) + { + for (var index = 0; index <= depth; index++) + sb.Append("\t"); + + sb.Append(side == -1 ? "L " : "R "); + + if (node[side] == null) + sb.AppendLine(""); + else + AddNodeToStringBuilder(node[side], sb, depth + 1); + } + } + + public override string ToString() + { + if (root == null) + return ""; + + var sb = new StringBuilder(); + AddNodeToStringBuilder(root, sb, 0); + return sb.ToString(); + } + + private void AddNodesToList(KdTreeNode node, List> nodes) + { + if (node == null) + return; + + nodes.Add(node); + + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + AddNodesToList(node[side], nodes); + node[side] = null; + } + } + } + + private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + for (var index = fromIndex + 1; index <= toIndex; index++) + { + var newIndex = index; + + while (true) + { + var a = nodes[newIndex - 1]; + var b = nodes[newIndex]; + if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) + { + nodes[newIndex - 1] = b; + nodes[newIndex] = a; + } + else + break; + } + } + } + + private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + if (fromIndex == toIndex) + { + Add(nodes[fromIndex].Point, nodes[fromIndex].Value); + nodes[fromIndex] = null; + return; + } + + // Sort the array from the fromIndex to the toIndex + SortNodesArray(nodes, byDimension, fromIndex, toIndex); + + // Find the splitting point + int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; + + // Add the splitting point + Add(nodes[midIndex].Point, nodes[midIndex].Value); + nodes[midIndex] = null; + + // Recurse + int nextDimension = (byDimension + 1) % dimensions; + + if (fromIndex < midIndex) + AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); + + if (toIndex > midIndex) + AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); + } + + public void Balance() + { + var nodeList = new List>(); + AddNodesToList(root, nodeList); + + Clear(); + + AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); + } + + private void RemoveChildNodes(KdTreeNode node) + { + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + RemoveChildNodes(node[side]); + node[side] = null; + } + } + } + + public void Clear() + { + if (root != null) + RemoveChildNodes(root); + } + + public void SaveToFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Create(filename)) + { + formatter.Serialize(stream, this); + stream.Flush(); + } + } + + public static KdTree LoadFromFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Open(filename, FileMode.Open)) + { + return (KdTree)formatter.Deserialize(stream); + } + + } + + public IEnumerator> GetEnumerator() + { + var left = new Stack>(); + var right = new Stack>(); + + void addLeft(KdTreeNode node) + { + if (node.LeftChild != null) + { + left.Push(node.LeftChild); + } + } + + void addRight(KdTreeNode node) + { + if (node.RightChild != null) + { + right.Push(node.RightChild); + } + } + + if (root != null) + { + yield return root; + + addLeft(root); + addRight(root); + + while (true) + { + if (left.Any()) + { + var item = left.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else if (right.Any()) + { + var item = right.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else + { + break; + } + } + } + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTree.cs.meta b/Assets/KdTreeLib/KdTree.cs.meta new file mode 100755 index 0000000..d7f1d14 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b61eae78ccd40f0459eb72df19d37003 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTreeNode.cs b/Assets/KdTreeLib/KdTreeNode.cs new file mode 100755 index 0000000..e08f24d --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs @@ -0,0 +1,68 @@ +using System; +using System.Text; + +namespace KdTree +{ + [Serializable] + public class KdTreeNode + { + public KdTreeNode() + { + } + + public KdTreeNode(TKey[] point, TValue value) + { + Point = point; + Value = value; + } + + public TKey[] Point; + public TValue Value = default(TValue); + + internal KdTreeNode LeftChild = null; + internal KdTreeNode RightChild = null; + + internal KdTreeNode this[int compare] + { + get + { + if (compare <= 0) + return LeftChild; + else + return RightChild; + } + set + { + if (compare <= 0) + LeftChild = value; + else + RightChild = value; + } + } + + public bool IsLeaf + { + get + { + return (LeftChild == null) && (RightChild == null); + } + } + + public override string ToString() + { + var sb = new StringBuilder(); + + for (var dimension = 0; dimension < Point.Length; dimension++) + { + sb.Append(Point[dimension].ToString() + "\t"); + } + + if (Value == null) + sb.Append("null"); + else + sb.Append(Value.ToString()); + + return sb.ToString(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTreeNode.cs.meta b/Assets/KdTreeLib/KdTreeNode.cs.meta new file mode 100755 index 0000000..7fa00e6 --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c70bc7c92452a3b4e8de75cf4a32e85f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math.meta b/Assets/KdTreeLib/Math.meta new file mode 100755 index 0000000..7e307d1 --- /dev/null +++ b/Assets/KdTreeLib/Math.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 250be181763ace04ebe9e51229d5b664 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs b/Assets/KdTreeLib/Math/DoubleMath.cs new file mode 100755 index 0000000..a3a95a3 --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class DoubleMath : TypeMath + { + public override int Compare(double a, double b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(double a, double b) + { + return a == b; + } + + public override double MinValue + { + get { return double.MinValue; } + } + + public override double MaxValue + { + get { return double.MaxValue; } + } + + public override double Zero + { + get { return 0; } + } + + public override double NegativeInfinity { get { return double.NegativeInfinity; } } + + public override double PositiveInfinity { get { return double.PositiveInfinity; } } + + public override double Add(double a, double b) + { + return a + b; + } + + public override double Subtract(double a, double b) + { + return a - b; + } + + public override double Multiply(double a, double b) + { + return a * b; + } + + public override double DistanceSquaredBetweenPoints(double[] a, double[] b) + { + double distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + double distOnThisAxis = Subtract(a[dimension], b[dimension]); + double distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs.meta b/Assets/KdTreeLib/Math/DoubleMath.cs.meta new file mode 100755 index 0000000..e0e7d9d --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6878fdbadd195164393a12f522125dec +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/FloatMath.cs b/Assets/KdTreeLib/Math/FloatMath.cs new file mode 100755 index 0000000..e530cca --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class FloatMath : TypeMath + { + public override int Compare(float a, float b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(float a, float b) + { + return a == b; + } + + public override float MinValue + { + get { return float.MinValue; } + } + + public override float MaxValue + { + get { return float.MaxValue; } + } + + public override float Zero + { + get { return 0; } + } + + public override float NegativeInfinity { get { return float.NegativeInfinity; } } + + public override float PositiveInfinity { get { return float.PositiveInfinity; } } + + public override float Add(float a, float b) + { + return a + b; + } + + public override float Subtract(float a, float b) + { + return a - b; + } + + public override float Multiply(float a, float b) + { + return a * b; + } + + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + float distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + float distOnThisAxis = Subtract(a[dimension], b[dimension]); + float distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/FloatMath.cs.meta b/Assets/KdTreeLib/Math/FloatMath.cs.meta new file mode 100755 index 0000000..d1e609c --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e1a4895e13ce1b64491965b3a4b65201 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoMath.cs b/Assets/KdTreeLib/Math/GeoMath.cs new file mode 100755 index 0000000..7a71408 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs @@ -0,0 +1,14 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class GeoMath : FloatMath + { + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + double dst = GeoUtils.Distance(a[0], a[1], b[0], b[1], 'K'); + return (float)(dst * dst); + } + } +} diff --git a/Assets/KdTreeLib/Math/GeoMath.cs.meta b/Assets/KdTreeLib/Math/GeoMath.cs.meta new file mode 100755 index 0000000..7dd9a54 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 88144d944655c7d4eb1e3286f6ce3eb4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoUtils.cs b/Assets/KdTreeLib/Math/GeoUtils.cs new file mode 100755 index 0000000..e593bb7 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoUtils.cs @@ -0,0 +1,42 @@ +namespace KdTree.Math +{ + // Via http://www.geodatasource.com/developers/c-sharp + // This code is licensed under LGPLv3. + public class GeoUtils + { + public static double Distance(double lat1, double lon1, double lat2, double lon2, char unit) + { + double theta = lon1 - lon2; + double dist = System.Math.Sin(Deg2rad(lat1)) * System.Math.Sin(Deg2rad(lat2)) + System.Math.Cos(Deg2rad(lat1)) * System.Math.Cos(Deg2rad(lat2)) * System.Math.Cos(Deg2rad(theta)); + dist = System.Math.Acos(dist); + dist = Rad2deg(dist); + dist = dist * 60 * 1.1515; + if (unit == 'K') + { + dist = dist * 1.609344; + } + else if (unit == 'N') + { + dist = dist * 0.8684; + } + return (dist); + } + + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + //:: This function converts decimal degrees to radians ::: + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + private static double Deg2rad(double deg) + { + return (deg * System.Math.PI / 180.0); + } + + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + //:: This function converts radians to decimal degrees ::: + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + private static double Rad2deg(double rad) + { + return (rad / System.Math.PI * 180.0); + } + + } +} diff --git a/Assets/KdTreeLib/Math/GeoUtils.cs.meta b/Assets/KdTreeLib/Math/GeoUtils.cs.meta new file mode 100755 index 0000000..0b49afa --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoUtils.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6a11a5f65900e7a4891eace9a9f9dd39 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IPriorityQueue.cs.meta b/Assets/KdTreeLib/IPriorityQueue.cs.meta new file mode 100755 index 0000000..e37c0b2 --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0551921b902b13842aa2e99c95aa537c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTree.cs b/Assets/KdTreeLib/KdTree.cs new file mode 100755 index 0000000..4dfe924 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs @@ -0,0 +1,676 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Runtime.Serialization.Formatters.Binary; +using System.Text; + +namespace KdTree +{ + public enum AddDuplicateBehavior + { + Skip, + Error, + Update + } + + public class DuplicateNodeError : Exception + { + public DuplicateNodeError() + : base("Cannot Add Node With Duplicate Coordinates") + { + } + } + + [Serializable] + public class KdTree : IKdTree + { + public KdTree(int dimensions, ITypeMath typeMath) + { + this.dimensions = dimensions; + this.typeMath = typeMath; + Count = 0; + } + + public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) + : this(dimensions, typeMath) + { + AddDuplicateBehavior = addDuplicateBehavior; + } + + private int dimensions; + + private ITypeMath typeMath = null; + + private KdTreeNode root = null; + + public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } + + public bool Add(TKey[] point, TValue value) + { + var nodeToAdd = new KdTreeNode(point, value); + + if (root == null) + { + root = new KdTreeNode(point, value); + } + else + { + int dimension = -1; + KdTreeNode parent = root; + + do + { + // Increment the dimension we're searching in + dimension = (dimension + 1) % dimensions; + + // Does the node we're adding have the same hyperpoint as this node? + if (typeMath.AreEqual(point, parent.Point)) + { + switch (AddDuplicateBehavior) + { + case AddDuplicateBehavior.Skip: + return false; + + case AddDuplicateBehavior.Error: + throw new DuplicateNodeError(); + + case AddDuplicateBehavior.Update: + parent.Value = value; + return true; + + default: + // Should never happen + throw new Exception("Unexpected AddDuplicateBehavior"); + } + } + + // Which side does this node sit under in relation to it's parent at this level? + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + + if (parent[compare] == null) + { + parent[compare] = nodeToAdd; + break; + } + else + { + parent = parent[compare]; + } + } + while (true); + } + + Count++; + return true; + } + + private void ReaddChildNodes(KdTreeNode removedNode) + { + if (removedNode.IsLeaf) + return; + + // The folllowing code might seem a little redundant but we're using + // 2 queues so we can add the child nodes back in, in (more or less) + // the same order they were added in the first place + var nodesToReadd = new Queue>(); + + var nodesToReaddQueue = new Queue>(); + + if (removedNode.LeftChild != null) + nodesToReaddQueue.Enqueue(removedNode.LeftChild); + + if (removedNode.RightChild != null) + nodesToReaddQueue.Enqueue(removedNode.RightChild); + + while (nodesToReaddQueue.Count > 0) + { + var nodeToReadd = nodesToReaddQueue.Dequeue(); + + nodesToReadd.Enqueue(nodeToReadd); + + for (int side = -1; side <= 1; side += 2) + { + if (nodeToReadd[side] != null) + { + nodesToReaddQueue.Enqueue(nodeToReadd[side]); + + nodeToReadd[side] = null; + } + } + } + + while (nodesToReadd.Count > 0) + { + var nodeToReadd = nodesToReadd.Dequeue(); + + Count--; + Add(nodeToReadd.Point, nodeToReadd.Value); + } + } + + public void RemoveAt(TKey[] point) + { + // Is tree empty? + if (root == null) + return; + + KdTreeNode node; + + if (typeMath.AreEqual(point, root.Point)) + { + node = root; + root = null; + Count--; + ReaddChildNodes(node); + return; + } + + node = root; + + int dimension = -1; + do + { + dimension = (dimension + 1) % dimensions; + + int compare = typeMath.Compare(point[dimension], node.Point[dimension]); + + if (node[compare] == null) + // Can't find node + return; + + if (typeMath.AreEqual(point, node[compare].Point)) + { + var nodeToRemove = node[compare]; + node[compare] = null; + Count--; + + ReaddChildNodes(nodeToRemove); + } + else + node = node[compare]; + } + while (node != null); + } + + public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) + { + if (count > Count) + count = Count; + + if (count < 0) + { + throw new ArgumentException("Number of neighbors cannot be negative"); + } + + if (count == 0) + return new KdTreeNode[0]; + + var neighbours = new KdTreeNode[count]; + + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + + var rect = HyperRect.Infinite(dimensions, typeMath); + + AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); + + count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + /* + * 1. Search for the target + * + * 1.1 Start by splitting the specified hyper rect + * on the specified node's point along the current + * dimension so that we end up with 2 sub hyper rects + * (current dimension = depth % dimensions) + * + * 1.2 Check what sub rectangle the the target point resides in + * under the current dimension + * + * 1.3 Set that rect to the nearer rect and also the corresponding + * child node to the nearest rect and node and the other rect + * and child node to the further rect and child node (for use later) + * + * 1.4 Travel into the nearer rect and node by calling function + * recursively with nearer rect and node and incrementing + * the depth + * + * 2. Add leaf to list of nearest neighbours + * + * 3. Walk back up tree and at each level: + * + * 3.1 Add node to nearest neighbours if + * we haven't filled our nearest neighbour + * list yet or if it has a distance to target less + * than any of the distances in our current nearest + * neighbours. + * + * 3.2 If there is any point in the further rectangle that is closer to + * the target than our furtherest nearest neighbour then travel into + * that rect and node + * + * That's it, when it finally finishes traversing the branches + * it needs to we'll have our list! + */ + + private void AddNearestNeighbours( + KdTreeNode node, + TKey[] target, + HyperRect rect, + int depth, + NearestNeighbourList, TKey> nearestNeighbours, + TKey maxSearchRadiusSquared) + { + if (node == null) + return; + + // Work out the current dimension + int dimension = depth % dimensions; + + // Split our hyper-rect into 2 sub rects along the current + // node's point on the current dimension + var leftRect = rect.Clone(); + leftRect.MaxPoint[dimension] = node.Point[dimension]; + + var rightRect = rect.Clone(); + rightRect.MinPoint[dimension] = node.Point[dimension]; + + // Which side does the target reside in? + int compare = typeMath.Compare(target[dimension], node.Point[dimension]); + + var nearerRect = compare <= 0 ? leftRect : rightRect; + var furtherRect = compare <= 0 ? rightRect : leftRect; + + var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; + var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; + + // Let's walk down into the nearer branch + if (nearerNode != null) + { + AddNearestNeighbours( + nearerNode, + target, + nearerRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + + TKey distanceSquaredToTarget; + + // Walk down into the further branch but only if our capacity hasn't been reached + // OR if there's a region in the further rect that's closer to the target than our + // current furtherest nearest neighbour + TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + { + if (nearestNeighbours.IsCapacityReached) + { + if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + else + { + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + } + + // Try to add the current node to our nearest neighbours list + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + nearestNeighbours.Add(node, distanceSquaredToTarget); + } + + /// + /// Performs a radial search. + /// + /// Center point + /// Radius to find neighbours within + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + /// + /// Performs a radial search up to a maximum count. + /// + /// Center point + /// Radius to find neighbours within + /// Maximum number of neighbours + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + private KdTreeNode[] RadialSearch(TKey[] center, TKey radius, NearestNeighbourList, TKey> nearestNeighbours) + { + AddNearestNeighbours( + root, + center, + HyperRect.Infinite(dimensions, typeMath), + 0, + nearestNeighbours, + typeMath.Multiply(radius, radius)); + + var count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + public int Count { get; private set; } + + public bool TryFindValueAt(TKey[] point, out TValue value) + { + var parent = root; + int dimension = -1; + do + { + if (parent == null) + { + value = default(TValue); + return false; + } + else if (typeMath.AreEqual(point, parent.Point)) + { + value = parent.Value; + return true; + } + + // Keep searching + dimension = (dimension + 1) % dimensions; + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + parent = parent[compare]; + } + while (true); + } + + public TValue FindValueAt(TKey[] point) + { + if (TryFindValueAt(point, out TValue value)) + return value; + else + return default(TValue); + } + + public bool TryFindValue(TValue value, out TKey[] point) + { + if (root == null) + { + point = null; + return false; + } + + // First-in, First-out list of nodes to search + var nodesToSearch = new Queue>(); + + nodesToSearch.Enqueue(root); + + while (nodesToSearch.Count > 0) + { + var nodeToSearch = nodesToSearch.Dequeue(); + + if (nodeToSearch.Value.Equals(value)) + { + point = nodeToSearch.Point; + return true; + } + else + { + for (int side = -1; side <= 1; side += 2) + { + var childNode = nodeToSearch[side]; + + if (childNode != null) + nodesToSearch.Enqueue(childNode); + } + } + } + + point = null; + return false; + } + + public TKey[] FindValue(TValue value) + { + if (TryFindValue(value, out TKey[] point)) + return point; + else + return null; + } + + private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) + { + sb.AppendLine(node.ToString()); + + for (var side = -1; side <= 1; side += 2) + { + for (var index = 0; index <= depth; index++) + sb.Append("\t"); + + sb.Append(side == -1 ? "L " : "R "); + + if (node[side] == null) + sb.AppendLine(""); + else + AddNodeToStringBuilder(node[side], sb, depth + 1); + } + } + + public override string ToString() + { + if (root == null) + return ""; + + var sb = new StringBuilder(); + AddNodeToStringBuilder(root, sb, 0); + return sb.ToString(); + } + + private void AddNodesToList(KdTreeNode node, List> nodes) + { + if (node == null) + return; + + nodes.Add(node); + + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + AddNodesToList(node[side], nodes); + node[side] = null; + } + } + } + + private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + for (var index = fromIndex + 1; index <= toIndex; index++) + { + var newIndex = index; + + while (true) + { + var a = nodes[newIndex - 1]; + var b = nodes[newIndex]; + if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) + { + nodes[newIndex - 1] = b; + nodes[newIndex] = a; + } + else + break; + } + } + } + + private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + if (fromIndex == toIndex) + { + Add(nodes[fromIndex].Point, nodes[fromIndex].Value); + nodes[fromIndex] = null; + return; + } + + // Sort the array from the fromIndex to the toIndex + SortNodesArray(nodes, byDimension, fromIndex, toIndex); + + // Find the splitting point + int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; + + // Add the splitting point + Add(nodes[midIndex].Point, nodes[midIndex].Value); + nodes[midIndex] = null; + + // Recurse + int nextDimension = (byDimension + 1) % dimensions; + + if (fromIndex < midIndex) + AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); + + if (toIndex > midIndex) + AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); + } + + public void Balance() + { + var nodeList = new List>(); + AddNodesToList(root, nodeList); + + Clear(); + + AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); + } + + private void RemoveChildNodes(KdTreeNode node) + { + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + RemoveChildNodes(node[side]); + node[side] = null; + } + } + } + + public void Clear() + { + if (root != null) + RemoveChildNodes(root); + } + + public void SaveToFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Create(filename)) + { + formatter.Serialize(stream, this); + stream.Flush(); + } + } + + public static KdTree LoadFromFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Open(filename, FileMode.Open)) + { + return (KdTree)formatter.Deserialize(stream); + } + + } + + public IEnumerator> GetEnumerator() + { + var left = new Stack>(); + var right = new Stack>(); + + void addLeft(KdTreeNode node) + { + if (node.LeftChild != null) + { + left.Push(node.LeftChild); + } + } + + void addRight(KdTreeNode node) + { + if (node.RightChild != null) + { + right.Push(node.RightChild); + } + } + + if (root != null) + { + yield return root; + + addLeft(root); + addRight(root); + + while (true) + { + if (left.Any()) + { + var item = left.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else if (right.Any()) + { + var item = right.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else + { + break; + } + } + } + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTree.cs.meta b/Assets/KdTreeLib/KdTree.cs.meta new file mode 100755 index 0000000..d7f1d14 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b61eae78ccd40f0459eb72df19d37003 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTreeNode.cs b/Assets/KdTreeLib/KdTreeNode.cs new file mode 100755 index 0000000..e08f24d --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs @@ -0,0 +1,68 @@ +using System; +using System.Text; + +namespace KdTree +{ + [Serializable] + public class KdTreeNode + { + public KdTreeNode() + { + } + + public KdTreeNode(TKey[] point, TValue value) + { + Point = point; + Value = value; + } + + public TKey[] Point; + public TValue Value = default(TValue); + + internal KdTreeNode LeftChild = null; + internal KdTreeNode RightChild = null; + + internal KdTreeNode this[int compare] + { + get + { + if (compare <= 0) + return LeftChild; + else + return RightChild; + } + set + { + if (compare <= 0) + LeftChild = value; + else + RightChild = value; + } + } + + public bool IsLeaf + { + get + { + return (LeftChild == null) && (RightChild == null); + } + } + + public override string ToString() + { + var sb = new StringBuilder(); + + for (var dimension = 0; dimension < Point.Length; dimension++) + { + sb.Append(Point[dimension].ToString() + "\t"); + } + + if (Value == null) + sb.Append("null"); + else + sb.Append(Value.ToString()); + + return sb.ToString(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTreeNode.cs.meta b/Assets/KdTreeLib/KdTreeNode.cs.meta new file mode 100755 index 0000000..7fa00e6 --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c70bc7c92452a3b4e8de75cf4a32e85f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math.meta b/Assets/KdTreeLib/Math.meta new file mode 100755 index 0000000..7e307d1 --- /dev/null +++ b/Assets/KdTreeLib/Math.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 250be181763ace04ebe9e51229d5b664 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs b/Assets/KdTreeLib/Math/DoubleMath.cs new file mode 100755 index 0000000..a3a95a3 --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class DoubleMath : TypeMath + { + public override int Compare(double a, double b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(double a, double b) + { + return a == b; + } + + public override double MinValue + { + get { return double.MinValue; } + } + + public override double MaxValue + { + get { return double.MaxValue; } + } + + public override double Zero + { + get { return 0; } + } + + public override double NegativeInfinity { get { return double.NegativeInfinity; } } + + public override double PositiveInfinity { get { return double.PositiveInfinity; } } + + public override double Add(double a, double b) + { + return a + b; + } + + public override double Subtract(double a, double b) + { + return a - b; + } + + public override double Multiply(double a, double b) + { + return a * b; + } + + public override double DistanceSquaredBetweenPoints(double[] a, double[] b) + { + double distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + double distOnThisAxis = Subtract(a[dimension], b[dimension]); + double distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs.meta b/Assets/KdTreeLib/Math/DoubleMath.cs.meta new file mode 100755 index 0000000..e0e7d9d --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6878fdbadd195164393a12f522125dec +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/FloatMath.cs b/Assets/KdTreeLib/Math/FloatMath.cs new file mode 100755 index 0000000..e530cca --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class FloatMath : TypeMath + { + public override int Compare(float a, float b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(float a, float b) + { + return a == b; + } + + public override float MinValue + { + get { return float.MinValue; } + } + + public override float MaxValue + { + get { return float.MaxValue; } + } + + public override float Zero + { + get { return 0; } + } + + public override float NegativeInfinity { get { return float.NegativeInfinity; } } + + public override float PositiveInfinity { get { return float.PositiveInfinity; } } + + public override float Add(float a, float b) + { + return a + b; + } + + public override float Subtract(float a, float b) + { + return a - b; + } + + public override float Multiply(float a, float b) + { + return a * b; + } + + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + float distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + float distOnThisAxis = Subtract(a[dimension], b[dimension]); + float distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/FloatMath.cs.meta b/Assets/KdTreeLib/Math/FloatMath.cs.meta new file mode 100755 index 0000000..d1e609c --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e1a4895e13ce1b64491965b3a4b65201 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoMath.cs b/Assets/KdTreeLib/Math/GeoMath.cs new file mode 100755 index 0000000..7a71408 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs @@ -0,0 +1,14 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class GeoMath : FloatMath + { + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + double dst = GeoUtils.Distance(a[0], a[1], b[0], b[1], 'K'); + return (float)(dst * dst); + } + } +} diff --git a/Assets/KdTreeLib/Math/GeoMath.cs.meta b/Assets/KdTreeLib/Math/GeoMath.cs.meta new file mode 100755 index 0000000..7dd9a54 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 88144d944655c7d4eb1e3286f6ce3eb4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoUtils.cs b/Assets/KdTreeLib/Math/GeoUtils.cs new file mode 100755 index 0000000..e593bb7 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoUtils.cs @@ -0,0 +1,42 @@ +namespace KdTree.Math +{ + // Via http://www.geodatasource.com/developers/c-sharp + // This code is licensed under LGPLv3. + public class GeoUtils + { + public static double Distance(double lat1, double lon1, double lat2, double lon2, char unit) + { + double theta = lon1 - lon2; + double dist = System.Math.Sin(Deg2rad(lat1)) * System.Math.Sin(Deg2rad(lat2)) + System.Math.Cos(Deg2rad(lat1)) * System.Math.Cos(Deg2rad(lat2)) * System.Math.Cos(Deg2rad(theta)); + dist = System.Math.Acos(dist); + dist = Rad2deg(dist); + dist = dist * 60 * 1.1515; + if (unit == 'K') + { + dist = dist * 1.609344; + } + else if (unit == 'N') + { + dist = dist * 0.8684; + } + return (dist); + } + + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + //:: This function converts decimal degrees to radians ::: + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + private static double Deg2rad(double deg) + { + return (deg * System.Math.PI / 180.0); + } + + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + //:: This function converts radians to decimal degrees ::: + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + private static double Rad2deg(double rad) + { + return (rad / System.Math.PI * 180.0); + } + + } +} diff --git a/Assets/KdTreeLib/Math/GeoUtils.cs.meta b/Assets/KdTreeLib/Math/GeoUtils.cs.meta new file mode 100755 index 0000000..0b49afa --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoUtils.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6a11a5f65900e7a4891eace9a9f9dd39 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/ITypeMath.cs b/Assets/KdTreeLib/Math/ITypeMath.cs new file mode 100755 index 0000000..efef222 --- /dev/null +++ b/Assets/KdTreeLib/Math/ITypeMath.cs @@ -0,0 +1,33 @@ +namespace KdTree +{ + public interface ITypeMath + { + int Compare(T a, T b); + + T MinValue { get; } + + T MaxValue { get; } + + T Min(T a, T b); + + T Max(T a, T b); + + bool AreEqual(T a, T b); + + bool AreEqual(T[] a, T[] b); + + T Add(T a, T b); + + T Subtract(T a, T b); + + T Multiply(T a, T b); + + T Zero { get; } + + T NegativeInfinity { get; } + + T PositiveInfinity { get; } + + T DistanceSquaredBetweenPoints(T[] a, T[] b); + } +} diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IPriorityQueue.cs.meta b/Assets/KdTreeLib/IPriorityQueue.cs.meta new file mode 100755 index 0000000..e37c0b2 --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0551921b902b13842aa2e99c95aa537c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTree.cs b/Assets/KdTreeLib/KdTree.cs new file mode 100755 index 0000000..4dfe924 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs @@ -0,0 +1,676 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Runtime.Serialization.Formatters.Binary; +using System.Text; + +namespace KdTree +{ + public enum AddDuplicateBehavior + { + Skip, + Error, + Update + } + + public class DuplicateNodeError : Exception + { + public DuplicateNodeError() + : base("Cannot Add Node With Duplicate Coordinates") + { + } + } + + [Serializable] + public class KdTree : IKdTree + { + public KdTree(int dimensions, ITypeMath typeMath) + { + this.dimensions = dimensions; + this.typeMath = typeMath; + Count = 0; + } + + public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) + : this(dimensions, typeMath) + { + AddDuplicateBehavior = addDuplicateBehavior; + } + + private int dimensions; + + private ITypeMath typeMath = null; + + private KdTreeNode root = null; + + public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } + + public bool Add(TKey[] point, TValue value) + { + var nodeToAdd = new KdTreeNode(point, value); + + if (root == null) + { + root = new KdTreeNode(point, value); + } + else + { + int dimension = -1; + KdTreeNode parent = root; + + do + { + // Increment the dimension we're searching in + dimension = (dimension + 1) % dimensions; + + // Does the node we're adding have the same hyperpoint as this node? + if (typeMath.AreEqual(point, parent.Point)) + { + switch (AddDuplicateBehavior) + { + case AddDuplicateBehavior.Skip: + return false; + + case AddDuplicateBehavior.Error: + throw new DuplicateNodeError(); + + case AddDuplicateBehavior.Update: + parent.Value = value; + return true; + + default: + // Should never happen + throw new Exception("Unexpected AddDuplicateBehavior"); + } + } + + // Which side does this node sit under in relation to it's parent at this level? + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + + if (parent[compare] == null) + { + parent[compare] = nodeToAdd; + break; + } + else + { + parent = parent[compare]; + } + } + while (true); + } + + Count++; + return true; + } + + private void ReaddChildNodes(KdTreeNode removedNode) + { + if (removedNode.IsLeaf) + return; + + // The folllowing code might seem a little redundant but we're using + // 2 queues so we can add the child nodes back in, in (more or less) + // the same order they were added in the first place + var nodesToReadd = new Queue>(); + + var nodesToReaddQueue = new Queue>(); + + if (removedNode.LeftChild != null) + nodesToReaddQueue.Enqueue(removedNode.LeftChild); + + if (removedNode.RightChild != null) + nodesToReaddQueue.Enqueue(removedNode.RightChild); + + while (nodesToReaddQueue.Count > 0) + { + var nodeToReadd = nodesToReaddQueue.Dequeue(); + + nodesToReadd.Enqueue(nodeToReadd); + + for (int side = -1; side <= 1; side += 2) + { + if (nodeToReadd[side] != null) + { + nodesToReaddQueue.Enqueue(nodeToReadd[side]); + + nodeToReadd[side] = null; + } + } + } + + while (nodesToReadd.Count > 0) + { + var nodeToReadd = nodesToReadd.Dequeue(); + + Count--; + Add(nodeToReadd.Point, nodeToReadd.Value); + } + } + + public void RemoveAt(TKey[] point) + { + // Is tree empty? + if (root == null) + return; + + KdTreeNode node; + + if (typeMath.AreEqual(point, root.Point)) + { + node = root; + root = null; + Count--; + ReaddChildNodes(node); + return; + } + + node = root; + + int dimension = -1; + do + { + dimension = (dimension + 1) % dimensions; + + int compare = typeMath.Compare(point[dimension], node.Point[dimension]); + + if (node[compare] == null) + // Can't find node + return; + + if (typeMath.AreEqual(point, node[compare].Point)) + { + var nodeToRemove = node[compare]; + node[compare] = null; + Count--; + + ReaddChildNodes(nodeToRemove); + } + else + node = node[compare]; + } + while (node != null); + } + + public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) + { + if (count > Count) + count = Count; + + if (count < 0) + { + throw new ArgumentException("Number of neighbors cannot be negative"); + } + + if (count == 0) + return new KdTreeNode[0]; + + var neighbours = new KdTreeNode[count]; + + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + + var rect = HyperRect.Infinite(dimensions, typeMath); + + AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); + + count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + /* + * 1. Search for the target + * + * 1.1 Start by splitting the specified hyper rect + * on the specified node's point along the current + * dimension so that we end up with 2 sub hyper rects + * (current dimension = depth % dimensions) + * + * 1.2 Check what sub rectangle the the target point resides in + * under the current dimension + * + * 1.3 Set that rect to the nearer rect and also the corresponding + * child node to the nearest rect and node and the other rect + * and child node to the further rect and child node (for use later) + * + * 1.4 Travel into the nearer rect and node by calling function + * recursively with nearer rect and node and incrementing + * the depth + * + * 2. Add leaf to list of nearest neighbours + * + * 3. Walk back up tree and at each level: + * + * 3.1 Add node to nearest neighbours if + * we haven't filled our nearest neighbour + * list yet or if it has a distance to target less + * than any of the distances in our current nearest + * neighbours. + * + * 3.2 If there is any point in the further rectangle that is closer to + * the target than our furtherest nearest neighbour then travel into + * that rect and node + * + * That's it, when it finally finishes traversing the branches + * it needs to we'll have our list! + */ + + private void AddNearestNeighbours( + KdTreeNode node, + TKey[] target, + HyperRect rect, + int depth, + NearestNeighbourList, TKey> nearestNeighbours, + TKey maxSearchRadiusSquared) + { + if (node == null) + return; + + // Work out the current dimension + int dimension = depth % dimensions; + + // Split our hyper-rect into 2 sub rects along the current + // node's point on the current dimension + var leftRect = rect.Clone(); + leftRect.MaxPoint[dimension] = node.Point[dimension]; + + var rightRect = rect.Clone(); + rightRect.MinPoint[dimension] = node.Point[dimension]; + + // Which side does the target reside in? + int compare = typeMath.Compare(target[dimension], node.Point[dimension]); + + var nearerRect = compare <= 0 ? leftRect : rightRect; + var furtherRect = compare <= 0 ? rightRect : leftRect; + + var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; + var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; + + // Let's walk down into the nearer branch + if (nearerNode != null) + { + AddNearestNeighbours( + nearerNode, + target, + nearerRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + + TKey distanceSquaredToTarget; + + // Walk down into the further branch but only if our capacity hasn't been reached + // OR if there's a region in the further rect that's closer to the target than our + // current furtherest nearest neighbour + TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + { + if (nearestNeighbours.IsCapacityReached) + { + if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + else + { + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + } + + // Try to add the current node to our nearest neighbours list + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + nearestNeighbours.Add(node, distanceSquaredToTarget); + } + + /// + /// Performs a radial search. + /// + /// Center point + /// Radius to find neighbours within + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + /// + /// Performs a radial search up to a maximum count. + /// + /// Center point + /// Radius to find neighbours within + /// Maximum number of neighbours + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + private KdTreeNode[] RadialSearch(TKey[] center, TKey radius, NearestNeighbourList, TKey> nearestNeighbours) + { + AddNearestNeighbours( + root, + center, + HyperRect.Infinite(dimensions, typeMath), + 0, + nearestNeighbours, + typeMath.Multiply(radius, radius)); + + var count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + public int Count { get; private set; } + + public bool TryFindValueAt(TKey[] point, out TValue value) + { + var parent = root; + int dimension = -1; + do + { + if (parent == null) + { + value = default(TValue); + return false; + } + else if (typeMath.AreEqual(point, parent.Point)) + { + value = parent.Value; + return true; + } + + // Keep searching + dimension = (dimension + 1) % dimensions; + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + parent = parent[compare]; + } + while (true); + } + + public TValue FindValueAt(TKey[] point) + { + if (TryFindValueAt(point, out TValue value)) + return value; + else + return default(TValue); + } + + public bool TryFindValue(TValue value, out TKey[] point) + { + if (root == null) + { + point = null; + return false; + } + + // First-in, First-out list of nodes to search + var nodesToSearch = new Queue>(); + + nodesToSearch.Enqueue(root); + + while (nodesToSearch.Count > 0) + { + var nodeToSearch = nodesToSearch.Dequeue(); + + if (nodeToSearch.Value.Equals(value)) + { + point = nodeToSearch.Point; + return true; + } + else + { + for (int side = -1; side <= 1; side += 2) + { + var childNode = nodeToSearch[side]; + + if (childNode != null) + nodesToSearch.Enqueue(childNode); + } + } + } + + point = null; + return false; + } + + public TKey[] FindValue(TValue value) + { + if (TryFindValue(value, out TKey[] point)) + return point; + else + return null; + } + + private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) + { + sb.AppendLine(node.ToString()); + + for (var side = -1; side <= 1; side += 2) + { + for (var index = 0; index <= depth; index++) + sb.Append("\t"); + + sb.Append(side == -1 ? "L " : "R "); + + if (node[side] == null) + sb.AppendLine(""); + else + AddNodeToStringBuilder(node[side], sb, depth + 1); + } + } + + public override string ToString() + { + if (root == null) + return ""; + + var sb = new StringBuilder(); + AddNodeToStringBuilder(root, sb, 0); + return sb.ToString(); + } + + private void AddNodesToList(KdTreeNode node, List> nodes) + { + if (node == null) + return; + + nodes.Add(node); + + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + AddNodesToList(node[side], nodes); + node[side] = null; + } + } + } + + private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + for (var index = fromIndex + 1; index <= toIndex; index++) + { + var newIndex = index; + + while (true) + { + var a = nodes[newIndex - 1]; + var b = nodes[newIndex]; + if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) + { + nodes[newIndex - 1] = b; + nodes[newIndex] = a; + } + else + break; + } + } + } + + private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + if (fromIndex == toIndex) + { + Add(nodes[fromIndex].Point, nodes[fromIndex].Value); + nodes[fromIndex] = null; + return; + } + + // Sort the array from the fromIndex to the toIndex + SortNodesArray(nodes, byDimension, fromIndex, toIndex); + + // Find the splitting point + int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; + + // Add the splitting point + Add(nodes[midIndex].Point, nodes[midIndex].Value); + nodes[midIndex] = null; + + // Recurse + int nextDimension = (byDimension + 1) % dimensions; + + if (fromIndex < midIndex) + AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); + + if (toIndex > midIndex) + AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); + } + + public void Balance() + { + var nodeList = new List>(); + AddNodesToList(root, nodeList); + + Clear(); + + AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); + } + + private void RemoveChildNodes(KdTreeNode node) + { + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + RemoveChildNodes(node[side]); + node[side] = null; + } + } + } + + public void Clear() + { + if (root != null) + RemoveChildNodes(root); + } + + public void SaveToFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Create(filename)) + { + formatter.Serialize(stream, this); + stream.Flush(); + } + } + + public static KdTree LoadFromFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Open(filename, FileMode.Open)) + { + return (KdTree)formatter.Deserialize(stream); + } + + } + + public IEnumerator> GetEnumerator() + { + var left = new Stack>(); + var right = new Stack>(); + + void addLeft(KdTreeNode node) + { + if (node.LeftChild != null) + { + left.Push(node.LeftChild); + } + } + + void addRight(KdTreeNode node) + { + if (node.RightChild != null) + { + right.Push(node.RightChild); + } + } + + if (root != null) + { + yield return root; + + addLeft(root); + addRight(root); + + while (true) + { + if (left.Any()) + { + var item = left.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else if (right.Any()) + { + var item = right.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else + { + break; + } + } + } + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTree.cs.meta b/Assets/KdTreeLib/KdTree.cs.meta new file mode 100755 index 0000000..d7f1d14 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b61eae78ccd40f0459eb72df19d37003 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTreeNode.cs b/Assets/KdTreeLib/KdTreeNode.cs new file mode 100755 index 0000000..e08f24d --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs @@ -0,0 +1,68 @@ +using System; +using System.Text; + +namespace KdTree +{ + [Serializable] + public class KdTreeNode + { + public KdTreeNode() + { + } + + public KdTreeNode(TKey[] point, TValue value) + { + Point = point; + Value = value; + } + + public TKey[] Point; + public TValue Value = default(TValue); + + internal KdTreeNode LeftChild = null; + internal KdTreeNode RightChild = null; + + internal KdTreeNode this[int compare] + { + get + { + if (compare <= 0) + return LeftChild; + else + return RightChild; + } + set + { + if (compare <= 0) + LeftChild = value; + else + RightChild = value; + } + } + + public bool IsLeaf + { + get + { + return (LeftChild == null) && (RightChild == null); + } + } + + public override string ToString() + { + var sb = new StringBuilder(); + + for (var dimension = 0; dimension < Point.Length; dimension++) + { + sb.Append(Point[dimension].ToString() + "\t"); + } + + if (Value == null) + sb.Append("null"); + else + sb.Append(Value.ToString()); + + return sb.ToString(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTreeNode.cs.meta b/Assets/KdTreeLib/KdTreeNode.cs.meta new file mode 100755 index 0000000..7fa00e6 --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c70bc7c92452a3b4e8de75cf4a32e85f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math.meta b/Assets/KdTreeLib/Math.meta new file mode 100755 index 0000000..7e307d1 --- /dev/null +++ b/Assets/KdTreeLib/Math.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 250be181763ace04ebe9e51229d5b664 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs b/Assets/KdTreeLib/Math/DoubleMath.cs new file mode 100755 index 0000000..a3a95a3 --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class DoubleMath : TypeMath + { + public override int Compare(double a, double b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(double a, double b) + { + return a == b; + } + + public override double MinValue + { + get { return double.MinValue; } + } + + public override double MaxValue + { + get { return double.MaxValue; } + } + + public override double Zero + { + get { return 0; } + } + + public override double NegativeInfinity { get { return double.NegativeInfinity; } } + + public override double PositiveInfinity { get { return double.PositiveInfinity; } } + + public override double Add(double a, double b) + { + return a + b; + } + + public override double Subtract(double a, double b) + { + return a - b; + } + + public override double Multiply(double a, double b) + { + return a * b; + } + + public override double DistanceSquaredBetweenPoints(double[] a, double[] b) + { + double distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + double distOnThisAxis = Subtract(a[dimension], b[dimension]); + double distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs.meta b/Assets/KdTreeLib/Math/DoubleMath.cs.meta new file mode 100755 index 0000000..e0e7d9d --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6878fdbadd195164393a12f522125dec +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/FloatMath.cs b/Assets/KdTreeLib/Math/FloatMath.cs new file mode 100755 index 0000000..e530cca --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class FloatMath : TypeMath + { + public override int Compare(float a, float b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(float a, float b) + { + return a == b; + } + + public override float MinValue + { + get { return float.MinValue; } + } + + public override float MaxValue + { + get { return float.MaxValue; } + } + + public override float Zero + { + get { return 0; } + } + + public override float NegativeInfinity { get { return float.NegativeInfinity; } } + + public override float PositiveInfinity { get { return float.PositiveInfinity; } } + + public override float Add(float a, float b) + { + return a + b; + } + + public override float Subtract(float a, float b) + { + return a - b; + } + + public override float Multiply(float a, float b) + { + return a * b; + } + + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + float distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + float distOnThisAxis = Subtract(a[dimension], b[dimension]); + float distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/FloatMath.cs.meta b/Assets/KdTreeLib/Math/FloatMath.cs.meta new file mode 100755 index 0000000..d1e609c --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e1a4895e13ce1b64491965b3a4b65201 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoMath.cs b/Assets/KdTreeLib/Math/GeoMath.cs new file mode 100755 index 0000000..7a71408 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs @@ -0,0 +1,14 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class GeoMath : FloatMath + { + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + double dst = GeoUtils.Distance(a[0], a[1], b[0], b[1], 'K'); + return (float)(dst * dst); + } + } +} diff --git a/Assets/KdTreeLib/Math/GeoMath.cs.meta b/Assets/KdTreeLib/Math/GeoMath.cs.meta new file mode 100755 index 0000000..7dd9a54 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 88144d944655c7d4eb1e3286f6ce3eb4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoUtils.cs b/Assets/KdTreeLib/Math/GeoUtils.cs new file mode 100755 index 0000000..e593bb7 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoUtils.cs @@ -0,0 +1,42 @@ +namespace KdTree.Math +{ + // Via http://www.geodatasource.com/developers/c-sharp + // This code is licensed under LGPLv3. + public class GeoUtils + { + public static double Distance(double lat1, double lon1, double lat2, double lon2, char unit) + { + double theta = lon1 - lon2; + double dist = System.Math.Sin(Deg2rad(lat1)) * System.Math.Sin(Deg2rad(lat2)) + System.Math.Cos(Deg2rad(lat1)) * System.Math.Cos(Deg2rad(lat2)) * System.Math.Cos(Deg2rad(theta)); + dist = System.Math.Acos(dist); + dist = Rad2deg(dist); + dist = dist * 60 * 1.1515; + if (unit == 'K') + { + dist = dist * 1.609344; + } + else if (unit == 'N') + { + dist = dist * 0.8684; + } + return (dist); + } + + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + //:: This function converts decimal degrees to radians ::: + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + private static double Deg2rad(double deg) + { + return (deg * System.Math.PI / 180.0); + } + + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + //:: This function converts radians to decimal degrees ::: + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + private static double Rad2deg(double rad) + { + return (rad / System.Math.PI * 180.0); + } + + } +} diff --git a/Assets/KdTreeLib/Math/GeoUtils.cs.meta b/Assets/KdTreeLib/Math/GeoUtils.cs.meta new file mode 100755 index 0000000..0b49afa --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoUtils.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6a11a5f65900e7a4891eace9a9f9dd39 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/ITypeMath.cs b/Assets/KdTreeLib/Math/ITypeMath.cs new file mode 100755 index 0000000..efef222 --- /dev/null +++ b/Assets/KdTreeLib/Math/ITypeMath.cs @@ -0,0 +1,33 @@ +namespace KdTree +{ + public interface ITypeMath + { + int Compare(T a, T b); + + T MinValue { get; } + + T MaxValue { get; } + + T Min(T a, T b); + + T Max(T a, T b); + + bool AreEqual(T a, T b); + + bool AreEqual(T[] a, T[] b); + + T Add(T a, T b); + + T Subtract(T a, T b); + + T Multiply(T a, T b); + + T Zero { get; } + + T NegativeInfinity { get; } + + T PositiveInfinity { get; } + + T DistanceSquaredBetweenPoints(T[] a, T[] b); + } +} diff --git a/Assets/KdTreeLib/Math/ITypeMath.cs.meta b/Assets/KdTreeLib/Math/ITypeMath.cs.meta new file mode 100755 index 0000000..d3fd504 --- /dev/null +++ b/Assets/KdTreeLib/Math/ITypeMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 2835d1b3924e9bb4f9c46e88cb981667 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IPriorityQueue.cs.meta b/Assets/KdTreeLib/IPriorityQueue.cs.meta new file mode 100755 index 0000000..e37c0b2 --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0551921b902b13842aa2e99c95aa537c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTree.cs b/Assets/KdTreeLib/KdTree.cs new file mode 100755 index 0000000..4dfe924 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs @@ -0,0 +1,676 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Runtime.Serialization.Formatters.Binary; +using System.Text; + +namespace KdTree +{ + public enum AddDuplicateBehavior + { + Skip, + Error, + Update + } + + public class DuplicateNodeError : Exception + { + public DuplicateNodeError() + : base("Cannot Add Node With Duplicate Coordinates") + { + } + } + + [Serializable] + public class KdTree : IKdTree + { + public KdTree(int dimensions, ITypeMath typeMath) + { + this.dimensions = dimensions; + this.typeMath = typeMath; + Count = 0; + } + + public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) + : this(dimensions, typeMath) + { + AddDuplicateBehavior = addDuplicateBehavior; + } + + private int dimensions; + + private ITypeMath typeMath = null; + + private KdTreeNode root = null; + + public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } + + public bool Add(TKey[] point, TValue value) + { + var nodeToAdd = new KdTreeNode(point, value); + + if (root == null) + { + root = new KdTreeNode(point, value); + } + else + { + int dimension = -1; + KdTreeNode parent = root; + + do + { + // Increment the dimension we're searching in + dimension = (dimension + 1) % dimensions; + + // Does the node we're adding have the same hyperpoint as this node? + if (typeMath.AreEqual(point, parent.Point)) + { + switch (AddDuplicateBehavior) + { + case AddDuplicateBehavior.Skip: + return false; + + case AddDuplicateBehavior.Error: + throw new DuplicateNodeError(); + + case AddDuplicateBehavior.Update: + parent.Value = value; + return true; + + default: + // Should never happen + throw new Exception("Unexpected AddDuplicateBehavior"); + } + } + + // Which side does this node sit under in relation to it's parent at this level? + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + + if (parent[compare] == null) + { + parent[compare] = nodeToAdd; + break; + } + else + { + parent = parent[compare]; + } + } + while (true); + } + + Count++; + return true; + } + + private void ReaddChildNodes(KdTreeNode removedNode) + { + if (removedNode.IsLeaf) + return; + + // The folllowing code might seem a little redundant but we're using + // 2 queues so we can add the child nodes back in, in (more or less) + // the same order they were added in the first place + var nodesToReadd = new Queue>(); + + var nodesToReaddQueue = new Queue>(); + + if (removedNode.LeftChild != null) + nodesToReaddQueue.Enqueue(removedNode.LeftChild); + + if (removedNode.RightChild != null) + nodesToReaddQueue.Enqueue(removedNode.RightChild); + + while (nodesToReaddQueue.Count > 0) + { + var nodeToReadd = nodesToReaddQueue.Dequeue(); + + nodesToReadd.Enqueue(nodeToReadd); + + for (int side = -1; side <= 1; side += 2) + { + if (nodeToReadd[side] != null) + { + nodesToReaddQueue.Enqueue(nodeToReadd[side]); + + nodeToReadd[side] = null; + } + } + } + + while (nodesToReadd.Count > 0) + { + var nodeToReadd = nodesToReadd.Dequeue(); + + Count--; + Add(nodeToReadd.Point, nodeToReadd.Value); + } + } + + public void RemoveAt(TKey[] point) + { + // Is tree empty? + if (root == null) + return; + + KdTreeNode node; + + if (typeMath.AreEqual(point, root.Point)) + { + node = root; + root = null; + Count--; + ReaddChildNodes(node); + return; + } + + node = root; + + int dimension = -1; + do + { + dimension = (dimension + 1) % dimensions; + + int compare = typeMath.Compare(point[dimension], node.Point[dimension]); + + if (node[compare] == null) + // Can't find node + return; + + if (typeMath.AreEqual(point, node[compare].Point)) + { + var nodeToRemove = node[compare]; + node[compare] = null; + Count--; + + ReaddChildNodes(nodeToRemove); + } + else + node = node[compare]; + } + while (node != null); + } + + public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) + { + if (count > Count) + count = Count; + + if (count < 0) + { + throw new ArgumentException("Number of neighbors cannot be negative"); + } + + if (count == 0) + return new KdTreeNode[0]; + + var neighbours = new KdTreeNode[count]; + + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + + var rect = HyperRect.Infinite(dimensions, typeMath); + + AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); + + count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + /* + * 1. Search for the target + * + * 1.1 Start by splitting the specified hyper rect + * on the specified node's point along the current + * dimension so that we end up with 2 sub hyper rects + * (current dimension = depth % dimensions) + * + * 1.2 Check what sub rectangle the the target point resides in + * under the current dimension + * + * 1.3 Set that rect to the nearer rect and also the corresponding + * child node to the nearest rect and node and the other rect + * and child node to the further rect and child node (for use later) + * + * 1.4 Travel into the nearer rect and node by calling function + * recursively with nearer rect and node and incrementing + * the depth + * + * 2. Add leaf to list of nearest neighbours + * + * 3. Walk back up tree and at each level: + * + * 3.1 Add node to nearest neighbours if + * we haven't filled our nearest neighbour + * list yet or if it has a distance to target less + * than any of the distances in our current nearest + * neighbours. + * + * 3.2 If there is any point in the further rectangle that is closer to + * the target than our furtherest nearest neighbour then travel into + * that rect and node + * + * That's it, when it finally finishes traversing the branches + * it needs to we'll have our list! + */ + + private void AddNearestNeighbours( + KdTreeNode node, + TKey[] target, + HyperRect rect, + int depth, + NearestNeighbourList, TKey> nearestNeighbours, + TKey maxSearchRadiusSquared) + { + if (node == null) + return; + + // Work out the current dimension + int dimension = depth % dimensions; + + // Split our hyper-rect into 2 sub rects along the current + // node's point on the current dimension + var leftRect = rect.Clone(); + leftRect.MaxPoint[dimension] = node.Point[dimension]; + + var rightRect = rect.Clone(); + rightRect.MinPoint[dimension] = node.Point[dimension]; + + // Which side does the target reside in? + int compare = typeMath.Compare(target[dimension], node.Point[dimension]); + + var nearerRect = compare <= 0 ? leftRect : rightRect; + var furtherRect = compare <= 0 ? rightRect : leftRect; + + var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; + var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; + + // Let's walk down into the nearer branch + if (nearerNode != null) + { + AddNearestNeighbours( + nearerNode, + target, + nearerRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + + TKey distanceSquaredToTarget; + + // Walk down into the further branch but only if our capacity hasn't been reached + // OR if there's a region in the further rect that's closer to the target than our + // current furtherest nearest neighbour + TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + { + if (nearestNeighbours.IsCapacityReached) + { + if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + else + { + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + } + + // Try to add the current node to our nearest neighbours list + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + nearestNeighbours.Add(node, distanceSquaredToTarget); + } + + /// + /// Performs a radial search. + /// + /// Center point + /// Radius to find neighbours within + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + /// + /// Performs a radial search up to a maximum count. + /// + /// Center point + /// Radius to find neighbours within + /// Maximum number of neighbours + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + private KdTreeNode[] RadialSearch(TKey[] center, TKey radius, NearestNeighbourList, TKey> nearestNeighbours) + { + AddNearestNeighbours( + root, + center, + HyperRect.Infinite(dimensions, typeMath), + 0, + nearestNeighbours, + typeMath.Multiply(radius, radius)); + + var count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + public int Count { get; private set; } + + public bool TryFindValueAt(TKey[] point, out TValue value) + { + var parent = root; + int dimension = -1; + do + { + if (parent == null) + { + value = default(TValue); + return false; + } + else if (typeMath.AreEqual(point, parent.Point)) + { + value = parent.Value; + return true; + } + + // Keep searching + dimension = (dimension + 1) % dimensions; + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + parent = parent[compare]; + } + while (true); + } + + public TValue FindValueAt(TKey[] point) + { + if (TryFindValueAt(point, out TValue value)) + return value; + else + return default(TValue); + } + + public bool TryFindValue(TValue value, out TKey[] point) + { + if (root == null) + { + point = null; + return false; + } + + // First-in, First-out list of nodes to search + var nodesToSearch = new Queue>(); + + nodesToSearch.Enqueue(root); + + while (nodesToSearch.Count > 0) + { + var nodeToSearch = nodesToSearch.Dequeue(); + + if (nodeToSearch.Value.Equals(value)) + { + point = nodeToSearch.Point; + return true; + } + else + { + for (int side = -1; side <= 1; side += 2) + { + var childNode = nodeToSearch[side]; + + if (childNode != null) + nodesToSearch.Enqueue(childNode); + } + } + } + + point = null; + return false; + } + + public TKey[] FindValue(TValue value) + { + if (TryFindValue(value, out TKey[] point)) + return point; + else + return null; + } + + private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) + { + sb.AppendLine(node.ToString()); + + for (var side = -1; side <= 1; side += 2) + { + for (var index = 0; index <= depth; index++) + sb.Append("\t"); + + sb.Append(side == -1 ? "L " : "R "); + + if (node[side] == null) + sb.AppendLine(""); + else + AddNodeToStringBuilder(node[side], sb, depth + 1); + } + } + + public override string ToString() + { + if (root == null) + return ""; + + var sb = new StringBuilder(); + AddNodeToStringBuilder(root, sb, 0); + return sb.ToString(); + } + + private void AddNodesToList(KdTreeNode node, List> nodes) + { + if (node == null) + return; + + nodes.Add(node); + + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + AddNodesToList(node[side], nodes); + node[side] = null; + } + } + } + + private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + for (var index = fromIndex + 1; index <= toIndex; index++) + { + var newIndex = index; + + while (true) + { + var a = nodes[newIndex - 1]; + var b = nodes[newIndex]; + if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) + { + nodes[newIndex - 1] = b; + nodes[newIndex] = a; + } + else + break; + } + } + } + + private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + if (fromIndex == toIndex) + { + Add(nodes[fromIndex].Point, nodes[fromIndex].Value); + nodes[fromIndex] = null; + return; + } + + // Sort the array from the fromIndex to the toIndex + SortNodesArray(nodes, byDimension, fromIndex, toIndex); + + // Find the splitting point + int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; + + // Add the splitting point + Add(nodes[midIndex].Point, nodes[midIndex].Value); + nodes[midIndex] = null; + + // Recurse + int nextDimension = (byDimension + 1) % dimensions; + + if (fromIndex < midIndex) + AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); + + if (toIndex > midIndex) + AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); + } + + public void Balance() + { + var nodeList = new List>(); + AddNodesToList(root, nodeList); + + Clear(); + + AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); + } + + private void RemoveChildNodes(KdTreeNode node) + { + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + RemoveChildNodes(node[side]); + node[side] = null; + } + } + } + + public void Clear() + { + if (root != null) + RemoveChildNodes(root); + } + + public void SaveToFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Create(filename)) + { + formatter.Serialize(stream, this); + stream.Flush(); + } + } + + public static KdTree LoadFromFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Open(filename, FileMode.Open)) + { + return (KdTree)formatter.Deserialize(stream); + } + + } + + public IEnumerator> GetEnumerator() + { + var left = new Stack>(); + var right = new Stack>(); + + void addLeft(KdTreeNode node) + { + if (node.LeftChild != null) + { + left.Push(node.LeftChild); + } + } + + void addRight(KdTreeNode node) + { + if (node.RightChild != null) + { + right.Push(node.RightChild); + } + } + + if (root != null) + { + yield return root; + + addLeft(root); + addRight(root); + + while (true) + { + if (left.Any()) + { + var item = left.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else if (right.Any()) + { + var item = right.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else + { + break; + } + } + } + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTree.cs.meta b/Assets/KdTreeLib/KdTree.cs.meta new file mode 100755 index 0000000..d7f1d14 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b61eae78ccd40f0459eb72df19d37003 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTreeNode.cs b/Assets/KdTreeLib/KdTreeNode.cs new file mode 100755 index 0000000..e08f24d --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs @@ -0,0 +1,68 @@ +using System; +using System.Text; + +namespace KdTree +{ + [Serializable] + public class KdTreeNode + { + public KdTreeNode() + { + } + + public KdTreeNode(TKey[] point, TValue value) + { + Point = point; + Value = value; + } + + public TKey[] Point; + public TValue Value = default(TValue); + + internal KdTreeNode LeftChild = null; + internal KdTreeNode RightChild = null; + + internal KdTreeNode this[int compare] + { + get + { + if (compare <= 0) + return LeftChild; + else + return RightChild; + } + set + { + if (compare <= 0) + LeftChild = value; + else + RightChild = value; + } + } + + public bool IsLeaf + { + get + { + return (LeftChild == null) && (RightChild == null); + } + } + + public override string ToString() + { + var sb = new StringBuilder(); + + for (var dimension = 0; dimension < Point.Length; dimension++) + { + sb.Append(Point[dimension].ToString() + "\t"); + } + + if (Value == null) + sb.Append("null"); + else + sb.Append(Value.ToString()); + + return sb.ToString(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTreeNode.cs.meta b/Assets/KdTreeLib/KdTreeNode.cs.meta new file mode 100755 index 0000000..7fa00e6 --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c70bc7c92452a3b4e8de75cf4a32e85f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math.meta b/Assets/KdTreeLib/Math.meta new file mode 100755 index 0000000..7e307d1 --- /dev/null +++ b/Assets/KdTreeLib/Math.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 250be181763ace04ebe9e51229d5b664 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs b/Assets/KdTreeLib/Math/DoubleMath.cs new file mode 100755 index 0000000..a3a95a3 --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class DoubleMath : TypeMath + { + public override int Compare(double a, double b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(double a, double b) + { + return a == b; + } + + public override double MinValue + { + get { return double.MinValue; } + } + + public override double MaxValue + { + get { return double.MaxValue; } + } + + public override double Zero + { + get { return 0; } + } + + public override double NegativeInfinity { get { return double.NegativeInfinity; } } + + public override double PositiveInfinity { get { return double.PositiveInfinity; } } + + public override double Add(double a, double b) + { + return a + b; + } + + public override double Subtract(double a, double b) + { + return a - b; + } + + public override double Multiply(double a, double b) + { + return a * b; + } + + public override double DistanceSquaredBetweenPoints(double[] a, double[] b) + { + double distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + double distOnThisAxis = Subtract(a[dimension], b[dimension]); + double distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs.meta b/Assets/KdTreeLib/Math/DoubleMath.cs.meta new file mode 100755 index 0000000..e0e7d9d --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6878fdbadd195164393a12f522125dec +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/FloatMath.cs b/Assets/KdTreeLib/Math/FloatMath.cs new file mode 100755 index 0000000..e530cca --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class FloatMath : TypeMath + { + public override int Compare(float a, float b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(float a, float b) + { + return a == b; + } + + public override float MinValue + { + get { return float.MinValue; } + } + + public override float MaxValue + { + get { return float.MaxValue; } + } + + public override float Zero + { + get { return 0; } + } + + public override float NegativeInfinity { get { return float.NegativeInfinity; } } + + public override float PositiveInfinity { get { return float.PositiveInfinity; } } + + public override float Add(float a, float b) + { + return a + b; + } + + public override float Subtract(float a, float b) + { + return a - b; + } + + public override float Multiply(float a, float b) + { + return a * b; + } + + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + float distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + float distOnThisAxis = Subtract(a[dimension], b[dimension]); + float distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/FloatMath.cs.meta b/Assets/KdTreeLib/Math/FloatMath.cs.meta new file mode 100755 index 0000000..d1e609c --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e1a4895e13ce1b64491965b3a4b65201 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoMath.cs b/Assets/KdTreeLib/Math/GeoMath.cs new file mode 100755 index 0000000..7a71408 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs @@ -0,0 +1,14 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class GeoMath : FloatMath + { + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + double dst = GeoUtils.Distance(a[0], a[1], b[0], b[1], 'K'); + return (float)(dst * dst); + } + } +} diff --git a/Assets/KdTreeLib/Math/GeoMath.cs.meta b/Assets/KdTreeLib/Math/GeoMath.cs.meta new file mode 100755 index 0000000..7dd9a54 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 88144d944655c7d4eb1e3286f6ce3eb4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoUtils.cs b/Assets/KdTreeLib/Math/GeoUtils.cs new file mode 100755 index 0000000..e593bb7 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoUtils.cs @@ -0,0 +1,42 @@ +namespace KdTree.Math +{ + // Via http://www.geodatasource.com/developers/c-sharp + // This code is licensed under LGPLv3. + public class GeoUtils + { + public static double Distance(double lat1, double lon1, double lat2, double lon2, char unit) + { + double theta = lon1 - lon2; + double dist = System.Math.Sin(Deg2rad(lat1)) * System.Math.Sin(Deg2rad(lat2)) + System.Math.Cos(Deg2rad(lat1)) * System.Math.Cos(Deg2rad(lat2)) * System.Math.Cos(Deg2rad(theta)); + dist = System.Math.Acos(dist); + dist = Rad2deg(dist); + dist = dist * 60 * 1.1515; + if (unit == 'K') + { + dist = dist * 1.609344; + } + else if (unit == 'N') + { + dist = dist * 0.8684; + } + return (dist); + } + + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + //:: This function converts decimal degrees to radians ::: + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + private static double Deg2rad(double deg) + { + return (deg * System.Math.PI / 180.0); + } + + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + //:: This function converts radians to decimal degrees ::: + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + private static double Rad2deg(double rad) + { + return (rad / System.Math.PI * 180.0); + } + + } +} diff --git a/Assets/KdTreeLib/Math/GeoUtils.cs.meta b/Assets/KdTreeLib/Math/GeoUtils.cs.meta new file mode 100755 index 0000000..0b49afa --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoUtils.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6a11a5f65900e7a4891eace9a9f9dd39 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/ITypeMath.cs b/Assets/KdTreeLib/Math/ITypeMath.cs new file mode 100755 index 0000000..efef222 --- /dev/null +++ b/Assets/KdTreeLib/Math/ITypeMath.cs @@ -0,0 +1,33 @@ +namespace KdTree +{ + public interface ITypeMath + { + int Compare(T a, T b); + + T MinValue { get; } + + T MaxValue { get; } + + T Min(T a, T b); + + T Max(T a, T b); + + bool AreEqual(T a, T b); + + bool AreEqual(T[] a, T[] b); + + T Add(T a, T b); + + T Subtract(T a, T b); + + T Multiply(T a, T b); + + T Zero { get; } + + T NegativeInfinity { get; } + + T PositiveInfinity { get; } + + T DistanceSquaredBetweenPoints(T[] a, T[] b); + } +} diff --git a/Assets/KdTreeLib/Math/ITypeMath.cs.meta b/Assets/KdTreeLib/Math/ITypeMath.cs.meta new file mode 100755 index 0000000..d3fd504 --- /dev/null +++ b/Assets/KdTreeLib/Math/ITypeMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 2835d1b3924e9bb4f9c46e88cb981667 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/TypeMath.cs b/Assets/KdTreeLib/Math/TypeMath.cs new file mode 100755 index 0000000..fb3cc2e --- /dev/null +++ b/Assets/KdTreeLib/Math/TypeMath.cs @@ -0,0 +1,65 @@ +using System; + +namespace KdTree.Math +{ + // Algebraic! + [Serializable] + public abstract class TypeMath : ITypeMath + { + #region ITypeMath members + + public abstract int Compare(T a, T b); + + public abstract bool AreEqual(T a, T b); + + public virtual bool AreEqual(T[] a, T[] b) + { + if (a.Length != b.Length) + return false; + + for (var index = 0; index < a.Length; index++) + { + if (!AreEqual(a[index], b[index])) + return false; + } + + return true; + } + + public abstract T MinValue { get; } + + public abstract T MaxValue { get; } + + public T Min(T a, T b) + { + if (Compare(a, b) < 0) + return a; + else + return b; + } + + public T Max(T a, T b) + { + if (Compare(a, b) > 0) + return a; + else + return b; + } + + public abstract T Zero { get; } + + public abstract T NegativeInfinity { get; } + + public abstract T PositiveInfinity { get; } + + public abstract T Add(T a, T b); + + public abstract T Subtract(T a, T b); + + public abstract T Multiply(T a, T b); + + public abstract T DistanceSquaredBetweenPoints(T[] a, T[] b); + + #endregion + } +} diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IPriorityQueue.cs.meta b/Assets/KdTreeLib/IPriorityQueue.cs.meta new file mode 100755 index 0000000..e37c0b2 --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0551921b902b13842aa2e99c95aa537c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTree.cs b/Assets/KdTreeLib/KdTree.cs new file mode 100755 index 0000000..4dfe924 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs @@ -0,0 +1,676 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Runtime.Serialization.Formatters.Binary; +using System.Text; + +namespace KdTree +{ + public enum AddDuplicateBehavior + { + Skip, + Error, + Update + } + + public class DuplicateNodeError : Exception + { + public DuplicateNodeError() + : base("Cannot Add Node With Duplicate Coordinates") + { + } + } + + [Serializable] + public class KdTree : IKdTree + { + public KdTree(int dimensions, ITypeMath typeMath) + { + this.dimensions = dimensions; + this.typeMath = typeMath; + Count = 0; + } + + public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) + : this(dimensions, typeMath) + { + AddDuplicateBehavior = addDuplicateBehavior; + } + + private int dimensions; + + private ITypeMath typeMath = null; + + private KdTreeNode root = null; + + public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } + + public bool Add(TKey[] point, TValue value) + { + var nodeToAdd = new KdTreeNode(point, value); + + if (root == null) + { + root = new KdTreeNode(point, value); + } + else + { + int dimension = -1; + KdTreeNode parent = root; + + do + { + // Increment the dimension we're searching in + dimension = (dimension + 1) % dimensions; + + // Does the node we're adding have the same hyperpoint as this node? + if (typeMath.AreEqual(point, parent.Point)) + { + switch (AddDuplicateBehavior) + { + case AddDuplicateBehavior.Skip: + return false; + + case AddDuplicateBehavior.Error: + throw new DuplicateNodeError(); + + case AddDuplicateBehavior.Update: + parent.Value = value; + return true; + + default: + // Should never happen + throw new Exception("Unexpected AddDuplicateBehavior"); + } + } + + // Which side does this node sit under in relation to it's parent at this level? + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + + if (parent[compare] == null) + { + parent[compare] = nodeToAdd; + break; + } + else + { + parent = parent[compare]; + } + } + while (true); + } + + Count++; + return true; + } + + private void ReaddChildNodes(KdTreeNode removedNode) + { + if (removedNode.IsLeaf) + return; + + // The folllowing code might seem a little redundant but we're using + // 2 queues so we can add the child nodes back in, in (more or less) + // the same order they were added in the first place + var nodesToReadd = new Queue>(); + + var nodesToReaddQueue = new Queue>(); + + if (removedNode.LeftChild != null) + nodesToReaddQueue.Enqueue(removedNode.LeftChild); + + if (removedNode.RightChild != null) + nodesToReaddQueue.Enqueue(removedNode.RightChild); + + while (nodesToReaddQueue.Count > 0) + { + var nodeToReadd = nodesToReaddQueue.Dequeue(); + + nodesToReadd.Enqueue(nodeToReadd); + + for (int side = -1; side <= 1; side += 2) + { + if (nodeToReadd[side] != null) + { + nodesToReaddQueue.Enqueue(nodeToReadd[side]); + + nodeToReadd[side] = null; + } + } + } + + while (nodesToReadd.Count > 0) + { + var nodeToReadd = nodesToReadd.Dequeue(); + + Count--; + Add(nodeToReadd.Point, nodeToReadd.Value); + } + } + + public void RemoveAt(TKey[] point) + { + // Is tree empty? + if (root == null) + return; + + KdTreeNode node; + + if (typeMath.AreEqual(point, root.Point)) + { + node = root; + root = null; + Count--; + ReaddChildNodes(node); + return; + } + + node = root; + + int dimension = -1; + do + { + dimension = (dimension + 1) % dimensions; + + int compare = typeMath.Compare(point[dimension], node.Point[dimension]); + + if (node[compare] == null) + // Can't find node + return; + + if (typeMath.AreEqual(point, node[compare].Point)) + { + var nodeToRemove = node[compare]; + node[compare] = null; + Count--; + + ReaddChildNodes(nodeToRemove); + } + else + node = node[compare]; + } + while (node != null); + } + + public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) + { + if (count > Count) + count = Count; + + if (count < 0) + { + throw new ArgumentException("Number of neighbors cannot be negative"); + } + + if (count == 0) + return new KdTreeNode[0]; + + var neighbours = new KdTreeNode[count]; + + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + + var rect = HyperRect.Infinite(dimensions, typeMath); + + AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); + + count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + /* + * 1. Search for the target + * + * 1.1 Start by splitting the specified hyper rect + * on the specified node's point along the current + * dimension so that we end up with 2 sub hyper rects + * (current dimension = depth % dimensions) + * + * 1.2 Check what sub rectangle the the target point resides in + * under the current dimension + * + * 1.3 Set that rect to the nearer rect and also the corresponding + * child node to the nearest rect and node and the other rect + * and child node to the further rect and child node (for use later) + * + * 1.4 Travel into the nearer rect and node by calling function + * recursively with nearer rect and node and incrementing + * the depth + * + * 2. Add leaf to list of nearest neighbours + * + * 3. Walk back up tree and at each level: + * + * 3.1 Add node to nearest neighbours if + * we haven't filled our nearest neighbour + * list yet or if it has a distance to target less + * than any of the distances in our current nearest + * neighbours. + * + * 3.2 If there is any point in the further rectangle that is closer to + * the target than our furtherest nearest neighbour then travel into + * that rect and node + * + * That's it, when it finally finishes traversing the branches + * it needs to we'll have our list! + */ + + private void AddNearestNeighbours( + KdTreeNode node, + TKey[] target, + HyperRect rect, + int depth, + NearestNeighbourList, TKey> nearestNeighbours, + TKey maxSearchRadiusSquared) + { + if (node == null) + return; + + // Work out the current dimension + int dimension = depth % dimensions; + + // Split our hyper-rect into 2 sub rects along the current + // node's point on the current dimension + var leftRect = rect.Clone(); + leftRect.MaxPoint[dimension] = node.Point[dimension]; + + var rightRect = rect.Clone(); + rightRect.MinPoint[dimension] = node.Point[dimension]; + + // Which side does the target reside in? + int compare = typeMath.Compare(target[dimension], node.Point[dimension]); + + var nearerRect = compare <= 0 ? leftRect : rightRect; + var furtherRect = compare <= 0 ? rightRect : leftRect; + + var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; + var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; + + // Let's walk down into the nearer branch + if (nearerNode != null) + { + AddNearestNeighbours( + nearerNode, + target, + nearerRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + + TKey distanceSquaredToTarget; + + // Walk down into the further branch but only if our capacity hasn't been reached + // OR if there's a region in the further rect that's closer to the target than our + // current furtherest nearest neighbour + TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + { + if (nearestNeighbours.IsCapacityReached) + { + if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + else + { + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + } + + // Try to add the current node to our nearest neighbours list + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + nearestNeighbours.Add(node, distanceSquaredToTarget); + } + + /// + /// Performs a radial search. + /// + /// Center point + /// Radius to find neighbours within + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + /// + /// Performs a radial search up to a maximum count. + /// + /// Center point + /// Radius to find neighbours within + /// Maximum number of neighbours + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + private KdTreeNode[] RadialSearch(TKey[] center, TKey radius, NearestNeighbourList, TKey> nearestNeighbours) + { + AddNearestNeighbours( + root, + center, + HyperRect.Infinite(dimensions, typeMath), + 0, + nearestNeighbours, + typeMath.Multiply(radius, radius)); + + var count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + public int Count { get; private set; } + + public bool TryFindValueAt(TKey[] point, out TValue value) + { + var parent = root; + int dimension = -1; + do + { + if (parent == null) + { + value = default(TValue); + return false; + } + else if (typeMath.AreEqual(point, parent.Point)) + { + value = parent.Value; + return true; + } + + // Keep searching + dimension = (dimension + 1) % dimensions; + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + parent = parent[compare]; + } + while (true); + } + + public TValue FindValueAt(TKey[] point) + { + if (TryFindValueAt(point, out TValue value)) + return value; + else + return default(TValue); + } + + public bool TryFindValue(TValue value, out TKey[] point) + { + if (root == null) + { + point = null; + return false; + } + + // First-in, First-out list of nodes to search + var nodesToSearch = new Queue>(); + + nodesToSearch.Enqueue(root); + + while (nodesToSearch.Count > 0) + { + var nodeToSearch = nodesToSearch.Dequeue(); + + if (nodeToSearch.Value.Equals(value)) + { + point = nodeToSearch.Point; + return true; + } + else + { + for (int side = -1; side <= 1; side += 2) + { + var childNode = nodeToSearch[side]; + + if (childNode != null) + nodesToSearch.Enqueue(childNode); + } + } + } + + point = null; + return false; + } + + public TKey[] FindValue(TValue value) + { + if (TryFindValue(value, out TKey[] point)) + return point; + else + return null; + } + + private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) + { + sb.AppendLine(node.ToString()); + + for (var side = -1; side <= 1; side += 2) + { + for (var index = 0; index <= depth; index++) + sb.Append("\t"); + + sb.Append(side == -1 ? "L " : "R "); + + if (node[side] == null) + sb.AppendLine(""); + else + AddNodeToStringBuilder(node[side], sb, depth + 1); + } + } + + public override string ToString() + { + if (root == null) + return ""; + + var sb = new StringBuilder(); + AddNodeToStringBuilder(root, sb, 0); + return sb.ToString(); + } + + private void AddNodesToList(KdTreeNode node, List> nodes) + { + if (node == null) + return; + + nodes.Add(node); + + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + AddNodesToList(node[side], nodes); + node[side] = null; + } + } + } + + private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + for (var index = fromIndex + 1; index <= toIndex; index++) + { + var newIndex = index; + + while (true) + { + var a = nodes[newIndex - 1]; + var b = nodes[newIndex]; + if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) + { + nodes[newIndex - 1] = b; + nodes[newIndex] = a; + } + else + break; + } + } + } + + private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + if (fromIndex == toIndex) + { + Add(nodes[fromIndex].Point, nodes[fromIndex].Value); + nodes[fromIndex] = null; + return; + } + + // Sort the array from the fromIndex to the toIndex + SortNodesArray(nodes, byDimension, fromIndex, toIndex); + + // Find the splitting point + int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; + + // Add the splitting point + Add(nodes[midIndex].Point, nodes[midIndex].Value); + nodes[midIndex] = null; + + // Recurse + int nextDimension = (byDimension + 1) % dimensions; + + if (fromIndex < midIndex) + AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); + + if (toIndex > midIndex) + AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); + } + + public void Balance() + { + var nodeList = new List>(); + AddNodesToList(root, nodeList); + + Clear(); + + AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); + } + + private void RemoveChildNodes(KdTreeNode node) + { + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + RemoveChildNodes(node[side]); + node[side] = null; + } + } + } + + public void Clear() + { + if (root != null) + RemoveChildNodes(root); + } + + public void SaveToFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Create(filename)) + { + formatter.Serialize(stream, this); + stream.Flush(); + } + } + + public static KdTree LoadFromFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Open(filename, FileMode.Open)) + { + return (KdTree)formatter.Deserialize(stream); + } + + } + + public IEnumerator> GetEnumerator() + { + var left = new Stack>(); + var right = new Stack>(); + + void addLeft(KdTreeNode node) + { + if (node.LeftChild != null) + { + left.Push(node.LeftChild); + } + } + + void addRight(KdTreeNode node) + { + if (node.RightChild != null) + { + right.Push(node.RightChild); + } + } + + if (root != null) + { + yield return root; + + addLeft(root); + addRight(root); + + while (true) + { + if (left.Any()) + { + var item = left.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else if (right.Any()) + { + var item = right.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else + { + break; + } + } + } + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTree.cs.meta b/Assets/KdTreeLib/KdTree.cs.meta new file mode 100755 index 0000000..d7f1d14 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b61eae78ccd40f0459eb72df19d37003 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTreeNode.cs b/Assets/KdTreeLib/KdTreeNode.cs new file mode 100755 index 0000000..e08f24d --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs @@ -0,0 +1,68 @@ +using System; +using System.Text; + +namespace KdTree +{ + [Serializable] + public class KdTreeNode + { + public KdTreeNode() + { + } + + public KdTreeNode(TKey[] point, TValue value) + { + Point = point; + Value = value; + } + + public TKey[] Point; + public TValue Value = default(TValue); + + internal KdTreeNode LeftChild = null; + internal KdTreeNode RightChild = null; + + internal KdTreeNode this[int compare] + { + get + { + if (compare <= 0) + return LeftChild; + else + return RightChild; + } + set + { + if (compare <= 0) + LeftChild = value; + else + RightChild = value; + } + } + + public bool IsLeaf + { + get + { + return (LeftChild == null) && (RightChild == null); + } + } + + public override string ToString() + { + var sb = new StringBuilder(); + + for (var dimension = 0; dimension < Point.Length; dimension++) + { + sb.Append(Point[dimension].ToString() + "\t"); + } + + if (Value == null) + sb.Append("null"); + else + sb.Append(Value.ToString()); + + return sb.ToString(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTreeNode.cs.meta b/Assets/KdTreeLib/KdTreeNode.cs.meta new file mode 100755 index 0000000..7fa00e6 --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c70bc7c92452a3b4e8de75cf4a32e85f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math.meta b/Assets/KdTreeLib/Math.meta new file mode 100755 index 0000000..7e307d1 --- /dev/null +++ b/Assets/KdTreeLib/Math.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 250be181763ace04ebe9e51229d5b664 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs b/Assets/KdTreeLib/Math/DoubleMath.cs new file mode 100755 index 0000000..a3a95a3 --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class DoubleMath : TypeMath + { + public override int Compare(double a, double b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(double a, double b) + { + return a == b; + } + + public override double MinValue + { + get { return double.MinValue; } + } + + public override double MaxValue + { + get { return double.MaxValue; } + } + + public override double Zero + { + get { return 0; } + } + + public override double NegativeInfinity { get { return double.NegativeInfinity; } } + + public override double PositiveInfinity { get { return double.PositiveInfinity; } } + + public override double Add(double a, double b) + { + return a + b; + } + + public override double Subtract(double a, double b) + { + return a - b; + } + + public override double Multiply(double a, double b) + { + return a * b; + } + + public override double DistanceSquaredBetweenPoints(double[] a, double[] b) + { + double distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + double distOnThisAxis = Subtract(a[dimension], b[dimension]); + double distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs.meta b/Assets/KdTreeLib/Math/DoubleMath.cs.meta new file mode 100755 index 0000000..e0e7d9d --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6878fdbadd195164393a12f522125dec +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/FloatMath.cs b/Assets/KdTreeLib/Math/FloatMath.cs new file mode 100755 index 0000000..e530cca --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class FloatMath : TypeMath + { + public override int Compare(float a, float b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(float a, float b) + { + return a == b; + } + + public override float MinValue + { + get { return float.MinValue; } + } + + public override float MaxValue + { + get { return float.MaxValue; } + } + + public override float Zero + { + get { return 0; } + } + + public override float NegativeInfinity { get { return float.NegativeInfinity; } } + + public override float PositiveInfinity { get { return float.PositiveInfinity; } } + + public override float Add(float a, float b) + { + return a + b; + } + + public override float Subtract(float a, float b) + { + return a - b; + } + + public override float Multiply(float a, float b) + { + return a * b; + } + + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + float distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + float distOnThisAxis = Subtract(a[dimension], b[dimension]); + float distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/FloatMath.cs.meta b/Assets/KdTreeLib/Math/FloatMath.cs.meta new file mode 100755 index 0000000..d1e609c --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e1a4895e13ce1b64491965b3a4b65201 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoMath.cs b/Assets/KdTreeLib/Math/GeoMath.cs new file mode 100755 index 0000000..7a71408 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs @@ -0,0 +1,14 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class GeoMath : FloatMath + { + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + double dst = GeoUtils.Distance(a[0], a[1], b[0], b[1], 'K'); + return (float)(dst * dst); + } + } +} diff --git a/Assets/KdTreeLib/Math/GeoMath.cs.meta b/Assets/KdTreeLib/Math/GeoMath.cs.meta new file mode 100755 index 0000000..7dd9a54 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 88144d944655c7d4eb1e3286f6ce3eb4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoUtils.cs b/Assets/KdTreeLib/Math/GeoUtils.cs new file mode 100755 index 0000000..e593bb7 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoUtils.cs @@ -0,0 +1,42 @@ +namespace KdTree.Math +{ + // Via http://www.geodatasource.com/developers/c-sharp + // This code is licensed under LGPLv3. + public class GeoUtils + { + public static double Distance(double lat1, double lon1, double lat2, double lon2, char unit) + { + double theta = lon1 - lon2; + double dist = System.Math.Sin(Deg2rad(lat1)) * System.Math.Sin(Deg2rad(lat2)) + System.Math.Cos(Deg2rad(lat1)) * System.Math.Cos(Deg2rad(lat2)) * System.Math.Cos(Deg2rad(theta)); + dist = System.Math.Acos(dist); + dist = Rad2deg(dist); + dist = dist * 60 * 1.1515; + if (unit == 'K') + { + dist = dist * 1.609344; + } + else if (unit == 'N') + { + dist = dist * 0.8684; + } + return (dist); + } + + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + //:: This function converts decimal degrees to radians ::: + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + private static double Deg2rad(double deg) + { + return (deg * System.Math.PI / 180.0); + } + + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + //:: This function converts radians to decimal degrees ::: + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + private static double Rad2deg(double rad) + { + return (rad / System.Math.PI * 180.0); + } + + } +} diff --git a/Assets/KdTreeLib/Math/GeoUtils.cs.meta b/Assets/KdTreeLib/Math/GeoUtils.cs.meta new file mode 100755 index 0000000..0b49afa --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoUtils.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6a11a5f65900e7a4891eace9a9f9dd39 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/ITypeMath.cs b/Assets/KdTreeLib/Math/ITypeMath.cs new file mode 100755 index 0000000..efef222 --- /dev/null +++ b/Assets/KdTreeLib/Math/ITypeMath.cs @@ -0,0 +1,33 @@ +namespace KdTree +{ + public interface ITypeMath + { + int Compare(T a, T b); + + T MinValue { get; } + + T MaxValue { get; } + + T Min(T a, T b); + + T Max(T a, T b); + + bool AreEqual(T a, T b); + + bool AreEqual(T[] a, T[] b); + + T Add(T a, T b); + + T Subtract(T a, T b); + + T Multiply(T a, T b); + + T Zero { get; } + + T NegativeInfinity { get; } + + T PositiveInfinity { get; } + + T DistanceSquaredBetweenPoints(T[] a, T[] b); + } +} diff --git a/Assets/KdTreeLib/Math/ITypeMath.cs.meta b/Assets/KdTreeLib/Math/ITypeMath.cs.meta new file mode 100755 index 0000000..d3fd504 --- /dev/null +++ b/Assets/KdTreeLib/Math/ITypeMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 2835d1b3924e9bb4f9c46e88cb981667 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/TypeMath.cs b/Assets/KdTreeLib/Math/TypeMath.cs new file mode 100755 index 0000000..fb3cc2e --- /dev/null +++ b/Assets/KdTreeLib/Math/TypeMath.cs @@ -0,0 +1,65 @@ +using System; + +namespace KdTree.Math +{ + // Algebraic! + [Serializable] + public abstract class TypeMath : ITypeMath + { + #region ITypeMath members + + public abstract int Compare(T a, T b); + + public abstract bool AreEqual(T a, T b); + + public virtual bool AreEqual(T[] a, T[] b) + { + if (a.Length != b.Length) + return false; + + for (var index = 0; index < a.Length; index++) + { + if (!AreEqual(a[index], b[index])) + return false; + } + + return true; + } + + public abstract T MinValue { get; } + + public abstract T MaxValue { get; } + + public T Min(T a, T b) + { + if (Compare(a, b) < 0) + return a; + else + return b; + } + + public T Max(T a, T b) + { + if (Compare(a, b) > 0) + return a; + else + return b; + } + + public abstract T Zero { get; } + + public abstract T NegativeInfinity { get; } + + public abstract T PositiveInfinity { get; } + + public abstract T Add(T a, T b); + + public abstract T Subtract(T a, T b); + + public abstract T Multiply(T a, T b); + + public abstract T DistanceSquaredBetweenPoints(T[] a, T[] b); + + #endregion + } +} diff --git a/Assets/KdTreeLib/Math/TypeMath.cs.meta b/Assets/KdTreeLib/Math/TypeMath.cs.meta new file mode 100755 index 0000000..d856885 --- /dev/null +++ b/Assets/KdTreeLib/Math/TypeMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 37a96951f6da4064eb31ad4bcfa2dd53 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IPriorityQueue.cs.meta b/Assets/KdTreeLib/IPriorityQueue.cs.meta new file mode 100755 index 0000000..e37c0b2 --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0551921b902b13842aa2e99c95aa537c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTree.cs b/Assets/KdTreeLib/KdTree.cs new file mode 100755 index 0000000..4dfe924 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs @@ -0,0 +1,676 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Runtime.Serialization.Formatters.Binary; +using System.Text; + +namespace KdTree +{ + public enum AddDuplicateBehavior + { + Skip, + Error, + Update + } + + public class DuplicateNodeError : Exception + { + public DuplicateNodeError() + : base("Cannot Add Node With Duplicate Coordinates") + { + } + } + + [Serializable] + public class KdTree : IKdTree + { + public KdTree(int dimensions, ITypeMath typeMath) + { + this.dimensions = dimensions; + this.typeMath = typeMath; + Count = 0; + } + + public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) + : this(dimensions, typeMath) + { + AddDuplicateBehavior = addDuplicateBehavior; + } + + private int dimensions; + + private ITypeMath typeMath = null; + + private KdTreeNode root = null; + + public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } + + public bool Add(TKey[] point, TValue value) + { + var nodeToAdd = new KdTreeNode(point, value); + + if (root == null) + { + root = new KdTreeNode(point, value); + } + else + { + int dimension = -1; + KdTreeNode parent = root; + + do + { + // Increment the dimension we're searching in + dimension = (dimension + 1) % dimensions; + + // Does the node we're adding have the same hyperpoint as this node? + if (typeMath.AreEqual(point, parent.Point)) + { + switch (AddDuplicateBehavior) + { + case AddDuplicateBehavior.Skip: + return false; + + case AddDuplicateBehavior.Error: + throw new DuplicateNodeError(); + + case AddDuplicateBehavior.Update: + parent.Value = value; + return true; + + default: + // Should never happen + throw new Exception("Unexpected AddDuplicateBehavior"); + } + } + + // Which side does this node sit under in relation to it's parent at this level? + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + + if (parent[compare] == null) + { + parent[compare] = nodeToAdd; + break; + } + else + { + parent = parent[compare]; + } + } + while (true); + } + + Count++; + return true; + } + + private void ReaddChildNodes(KdTreeNode removedNode) + { + if (removedNode.IsLeaf) + return; + + // The folllowing code might seem a little redundant but we're using + // 2 queues so we can add the child nodes back in, in (more or less) + // the same order they were added in the first place + var nodesToReadd = new Queue>(); + + var nodesToReaddQueue = new Queue>(); + + if (removedNode.LeftChild != null) + nodesToReaddQueue.Enqueue(removedNode.LeftChild); + + if (removedNode.RightChild != null) + nodesToReaddQueue.Enqueue(removedNode.RightChild); + + while (nodesToReaddQueue.Count > 0) + { + var nodeToReadd = nodesToReaddQueue.Dequeue(); + + nodesToReadd.Enqueue(nodeToReadd); + + for (int side = -1; side <= 1; side += 2) + { + if (nodeToReadd[side] != null) + { + nodesToReaddQueue.Enqueue(nodeToReadd[side]); + + nodeToReadd[side] = null; + } + } + } + + while (nodesToReadd.Count > 0) + { + var nodeToReadd = nodesToReadd.Dequeue(); + + Count--; + Add(nodeToReadd.Point, nodeToReadd.Value); + } + } + + public void RemoveAt(TKey[] point) + { + // Is tree empty? + if (root == null) + return; + + KdTreeNode node; + + if (typeMath.AreEqual(point, root.Point)) + { + node = root; + root = null; + Count--; + ReaddChildNodes(node); + return; + } + + node = root; + + int dimension = -1; + do + { + dimension = (dimension + 1) % dimensions; + + int compare = typeMath.Compare(point[dimension], node.Point[dimension]); + + if (node[compare] == null) + // Can't find node + return; + + if (typeMath.AreEqual(point, node[compare].Point)) + { + var nodeToRemove = node[compare]; + node[compare] = null; + Count--; + + ReaddChildNodes(nodeToRemove); + } + else + node = node[compare]; + } + while (node != null); + } + + public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) + { + if (count > Count) + count = Count; + + if (count < 0) + { + throw new ArgumentException("Number of neighbors cannot be negative"); + } + + if (count == 0) + return new KdTreeNode[0]; + + var neighbours = new KdTreeNode[count]; + + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + + var rect = HyperRect.Infinite(dimensions, typeMath); + + AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); + + count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + /* + * 1. Search for the target + * + * 1.1 Start by splitting the specified hyper rect + * on the specified node's point along the current + * dimension so that we end up with 2 sub hyper rects + * (current dimension = depth % dimensions) + * + * 1.2 Check what sub rectangle the the target point resides in + * under the current dimension + * + * 1.3 Set that rect to the nearer rect and also the corresponding + * child node to the nearest rect and node and the other rect + * and child node to the further rect and child node (for use later) + * + * 1.4 Travel into the nearer rect and node by calling function + * recursively with nearer rect and node and incrementing + * the depth + * + * 2. Add leaf to list of nearest neighbours + * + * 3. Walk back up tree and at each level: + * + * 3.1 Add node to nearest neighbours if + * we haven't filled our nearest neighbour + * list yet or if it has a distance to target less + * than any of the distances in our current nearest + * neighbours. + * + * 3.2 If there is any point in the further rectangle that is closer to + * the target than our furtherest nearest neighbour then travel into + * that rect and node + * + * That's it, when it finally finishes traversing the branches + * it needs to we'll have our list! + */ + + private void AddNearestNeighbours( + KdTreeNode node, + TKey[] target, + HyperRect rect, + int depth, + NearestNeighbourList, TKey> nearestNeighbours, + TKey maxSearchRadiusSquared) + { + if (node == null) + return; + + // Work out the current dimension + int dimension = depth % dimensions; + + // Split our hyper-rect into 2 sub rects along the current + // node's point on the current dimension + var leftRect = rect.Clone(); + leftRect.MaxPoint[dimension] = node.Point[dimension]; + + var rightRect = rect.Clone(); + rightRect.MinPoint[dimension] = node.Point[dimension]; + + // Which side does the target reside in? + int compare = typeMath.Compare(target[dimension], node.Point[dimension]); + + var nearerRect = compare <= 0 ? leftRect : rightRect; + var furtherRect = compare <= 0 ? rightRect : leftRect; + + var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; + var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; + + // Let's walk down into the nearer branch + if (nearerNode != null) + { + AddNearestNeighbours( + nearerNode, + target, + nearerRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + + TKey distanceSquaredToTarget; + + // Walk down into the further branch but only if our capacity hasn't been reached + // OR if there's a region in the further rect that's closer to the target than our + // current furtherest nearest neighbour + TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + { + if (nearestNeighbours.IsCapacityReached) + { + if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + else + { + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + } + + // Try to add the current node to our nearest neighbours list + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + nearestNeighbours.Add(node, distanceSquaredToTarget); + } + + /// + /// Performs a radial search. + /// + /// Center point + /// Radius to find neighbours within + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + /// + /// Performs a radial search up to a maximum count. + /// + /// Center point + /// Radius to find neighbours within + /// Maximum number of neighbours + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + private KdTreeNode[] RadialSearch(TKey[] center, TKey radius, NearestNeighbourList, TKey> nearestNeighbours) + { + AddNearestNeighbours( + root, + center, + HyperRect.Infinite(dimensions, typeMath), + 0, + nearestNeighbours, + typeMath.Multiply(radius, radius)); + + var count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + public int Count { get; private set; } + + public bool TryFindValueAt(TKey[] point, out TValue value) + { + var parent = root; + int dimension = -1; + do + { + if (parent == null) + { + value = default(TValue); + return false; + } + else if (typeMath.AreEqual(point, parent.Point)) + { + value = parent.Value; + return true; + } + + // Keep searching + dimension = (dimension + 1) % dimensions; + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + parent = parent[compare]; + } + while (true); + } + + public TValue FindValueAt(TKey[] point) + { + if (TryFindValueAt(point, out TValue value)) + return value; + else + return default(TValue); + } + + public bool TryFindValue(TValue value, out TKey[] point) + { + if (root == null) + { + point = null; + return false; + } + + // First-in, First-out list of nodes to search + var nodesToSearch = new Queue>(); + + nodesToSearch.Enqueue(root); + + while (nodesToSearch.Count > 0) + { + var nodeToSearch = nodesToSearch.Dequeue(); + + if (nodeToSearch.Value.Equals(value)) + { + point = nodeToSearch.Point; + return true; + } + else + { + for (int side = -1; side <= 1; side += 2) + { + var childNode = nodeToSearch[side]; + + if (childNode != null) + nodesToSearch.Enqueue(childNode); + } + } + } + + point = null; + return false; + } + + public TKey[] FindValue(TValue value) + { + if (TryFindValue(value, out TKey[] point)) + return point; + else + return null; + } + + private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) + { + sb.AppendLine(node.ToString()); + + for (var side = -1; side <= 1; side += 2) + { + for (var index = 0; index <= depth; index++) + sb.Append("\t"); + + sb.Append(side == -1 ? "L " : "R "); + + if (node[side] == null) + sb.AppendLine(""); + else + AddNodeToStringBuilder(node[side], sb, depth + 1); + } + } + + public override string ToString() + { + if (root == null) + return ""; + + var sb = new StringBuilder(); + AddNodeToStringBuilder(root, sb, 0); + return sb.ToString(); + } + + private void AddNodesToList(KdTreeNode node, List> nodes) + { + if (node == null) + return; + + nodes.Add(node); + + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + AddNodesToList(node[side], nodes); + node[side] = null; + } + } + } + + private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + for (var index = fromIndex + 1; index <= toIndex; index++) + { + var newIndex = index; + + while (true) + { + var a = nodes[newIndex - 1]; + var b = nodes[newIndex]; + if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) + { + nodes[newIndex - 1] = b; + nodes[newIndex] = a; + } + else + break; + } + } + } + + private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + if (fromIndex == toIndex) + { + Add(nodes[fromIndex].Point, nodes[fromIndex].Value); + nodes[fromIndex] = null; + return; + } + + // Sort the array from the fromIndex to the toIndex + SortNodesArray(nodes, byDimension, fromIndex, toIndex); + + // Find the splitting point + int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; + + // Add the splitting point + Add(nodes[midIndex].Point, nodes[midIndex].Value); + nodes[midIndex] = null; + + // Recurse + int nextDimension = (byDimension + 1) % dimensions; + + if (fromIndex < midIndex) + AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); + + if (toIndex > midIndex) + AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); + } + + public void Balance() + { + var nodeList = new List>(); + AddNodesToList(root, nodeList); + + Clear(); + + AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); + } + + private void RemoveChildNodes(KdTreeNode node) + { + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + RemoveChildNodes(node[side]); + node[side] = null; + } + } + } + + public void Clear() + { + if (root != null) + RemoveChildNodes(root); + } + + public void SaveToFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Create(filename)) + { + formatter.Serialize(stream, this); + stream.Flush(); + } + } + + public static KdTree LoadFromFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Open(filename, FileMode.Open)) + { + return (KdTree)formatter.Deserialize(stream); + } + + } + + public IEnumerator> GetEnumerator() + { + var left = new Stack>(); + var right = new Stack>(); + + void addLeft(KdTreeNode node) + { + if (node.LeftChild != null) + { + left.Push(node.LeftChild); + } + } + + void addRight(KdTreeNode node) + { + if (node.RightChild != null) + { + right.Push(node.RightChild); + } + } + + if (root != null) + { + yield return root; + + addLeft(root); + addRight(root); + + while (true) + { + if (left.Any()) + { + var item = left.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else if (right.Any()) + { + var item = right.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else + { + break; + } + } + } + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTree.cs.meta b/Assets/KdTreeLib/KdTree.cs.meta new file mode 100755 index 0000000..d7f1d14 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b61eae78ccd40f0459eb72df19d37003 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTreeNode.cs b/Assets/KdTreeLib/KdTreeNode.cs new file mode 100755 index 0000000..e08f24d --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs @@ -0,0 +1,68 @@ +using System; +using System.Text; + +namespace KdTree +{ + [Serializable] + public class KdTreeNode + { + public KdTreeNode() + { + } + + public KdTreeNode(TKey[] point, TValue value) + { + Point = point; + Value = value; + } + + public TKey[] Point; + public TValue Value = default(TValue); + + internal KdTreeNode LeftChild = null; + internal KdTreeNode RightChild = null; + + internal KdTreeNode this[int compare] + { + get + { + if (compare <= 0) + return LeftChild; + else + return RightChild; + } + set + { + if (compare <= 0) + LeftChild = value; + else + RightChild = value; + } + } + + public bool IsLeaf + { + get + { + return (LeftChild == null) && (RightChild == null); + } + } + + public override string ToString() + { + var sb = new StringBuilder(); + + for (var dimension = 0; dimension < Point.Length; dimension++) + { + sb.Append(Point[dimension].ToString() + "\t"); + } + + if (Value == null) + sb.Append("null"); + else + sb.Append(Value.ToString()); + + return sb.ToString(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTreeNode.cs.meta b/Assets/KdTreeLib/KdTreeNode.cs.meta new file mode 100755 index 0000000..7fa00e6 --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c70bc7c92452a3b4e8de75cf4a32e85f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math.meta b/Assets/KdTreeLib/Math.meta new file mode 100755 index 0000000..7e307d1 --- /dev/null +++ b/Assets/KdTreeLib/Math.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 250be181763ace04ebe9e51229d5b664 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs b/Assets/KdTreeLib/Math/DoubleMath.cs new file mode 100755 index 0000000..a3a95a3 --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class DoubleMath : TypeMath + { + public override int Compare(double a, double b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(double a, double b) + { + return a == b; + } + + public override double MinValue + { + get { return double.MinValue; } + } + + public override double MaxValue + { + get { return double.MaxValue; } + } + + public override double Zero + { + get { return 0; } + } + + public override double NegativeInfinity { get { return double.NegativeInfinity; } } + + public override double PositiveInfinity { get { return double.PositiveInfinity; } } + + public override double Add(double a, double b) + { + return a + b; + } + + public override double Subtract(double a, double b) + { + return a - b; + } + + public override double Multiply(double a, double b) + { + return a * b; + } + + public override double DistanceSquaredBetweenPoints(double[] a, double[] b) + { + double distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + double distOnThisAxis = Subtract(a[dimension], b[dimension]); + double distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs.meta b/Assets/KdTreeLib/Math/DoubleMath.cs.meta new file mode 100755 index 0000000..e0e7d9d --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6878fdbadd195164393a12f522125dec +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/FloatMath.cs b/Assets/KdTreeLib/Math/FloatMath.cs new file mode 100755 index 0000000..e530cca --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class FloatMath : TypeMath + { + public override int Compare(float a, float b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(float a, float b) + { + return a == b; + } + + public override float MinValue + { + get { return float.MinValue; } + } + + public override float MaxValue + { + get { return float.MaxValue; } + } + + public override float Zero + { + get { return 0; } + } + + public override float NegativeInfinity { get { return float.NegativeInfinity; } } + + public override float PositiveInfinity { get { return float.PositiveInfinity; } } + + public override float Add(float a, float b) + { + return a + b; + } + + public override float Subtract(float a, float b) + { + return a - b; + } + + public override float Multiply(float a, float b) + { + return a * b; + } + + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + float distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + float distOnThisAxis = Subtract(a[dimension], b[dimension]); + float distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/FloatMath.cs.meta b/Assets/KdTreeLib/Math/FloatMath.cs.meta new file mode 100755 index 0000000..d1e609c --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e1a4895e13ce1b64491965b3a4b65201 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoMath.cs b/Assets/KdTreeLib/Math/GeoMath.cs new file mode 100755 index 0000000..7a71408 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs @@ -0,0 +1,14 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class GeoMath : FloatMath + { + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + double dst = GeoUtils.Distance(a[0], a[1], b[0], b[1], 'K'); + return (float)(dst * dst); + } + } +} diff --git a/Assets/KdTreeLib/Math/GeoMath.cs.meta b/Assets/KdTreeLib/Math/GeoMath.cs.meta new file mode 100755 index 0000000..7dd9a54 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 88144d944655c7d4eb1e3286f6ce3eb4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoUtils.cs b/Assets/KdTreeLib/Math/GeoUtils.cs new file mode 100755 index 0000000..e593bb7 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoUtils.cs @@ -0,0 +1,42 @@ +namespace KdTree.Math +{ + // Via http://www.geodatasource.com/developers/c-sharp + // This code is licensed under LGPLv3. + public class GeoUtils + { + public static double Distance(double lat1, double lon1, double lat2, double lon2, char unit) + { + double theta = lon1 - lon2; + double dist = System.Math.Sin(Deg2rad(lat1)) * System.Math.Sin(Deg2rad(lat2)) + System.Math.Cos(Deg2rad(lat1)) * System.Math.Cos(Deg2rad(lat2)) * System.Math.Cos(Deg2rad(theta)); + dist = System.Math.Acos(dist); + dist = Rad2deg(dist); + dist = dist * 60 * 1.1515; + if (unit == 'K') + { + dist = dist * 1.609344; + } + else if (unit == 'N') + { + dist = dist * 0.8684; + } + return (dist); + } + + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + //:: This function converts decimal degrees to radians ::: + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + private static double Deg2rad(double deg) + { + return (deg * System.Math.PI / 180.0); + } + + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + //:: This function converts radians to decimal degrees ::: + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + private static double Rad2deg(double rad) + { + return (rad / System.Math.PI * 180.0); + } + + } +} diff --git a/Assets/KdTreeLib/Math/GeoUtils.cs.meta b/Assets/KdTreeLib/Math/GeoUtils.cs.meta new file mode 100755 index 0000000..0b49afa --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoUtils.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6a11a5f65900e7a4891eace9a9f9dd39 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/ITypeMath.cs b/Assets/KdTreeLib/Math/ITypeMath.cs new file mode 100755 index 0000000..efef222 --- /dev/null +++ b/Assets/KdTreeLib/Math/ITypeMath.cs @@ -0,0 +1,33 @@ +namespace KdTree +{ + public interface ITypeMath + { + int Compare(T a, T b); + + T MinValue { get; } + + T MaxValue { get; } + + T Min(T a, T b); + + T Max(T a, T b); + + bool AreEqual(T a, T b); + + bool AreEqual(T[] a, T[] b); + + T Add(T a, T b); + + T Subtract(T a, T b); + + T Multiply(T a, T b); + + T Zero { get; } + + T NegativeInfinity { get; } + + T PositiveInfinity { get; } + + T DistanceSquaredBetweenPoints(T[] a, T[] b); + } +} diff --git a/Assets/KdTreeLib/Math/ITypeMath.cs.meta b/Assets/KdTreeLib/Math/ITypeMath.cs.meta new file mode 100755 index 0000000..d3fd504 --- /dev/null +++ b/Assets/KdTreeLib/Math/ITypeMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 2835d1b3924e9bb4f9c46e88cb981667 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/TypeMath.cs b/Assets/KdTreeLib/Math/TypeMath.cs new file mode 100755 index 0000000..fb3cc2e --- /dev/null +++ b/Assets/KdTreeLib/Math/TypeMath.cs @@ -0,0 +1,65 @@ +using System; + +namespace KdTree.Math +{ + // Algebraic! + [Serializable] + public abstract class TypeMath : ITypeMath + { + #region ITypeMath members + + public abstract int Compare(T a, T b); + + public abstract bool AreEqual(T a, T b); + + public virtual bool AreEqual(T[] a, T[] b) + { + if (a.Length != b.Length) + return false; + + for (var index = 0; index < a.Length; index++) + { + if (!AreEqual(a[index], b[index])) + return false; + } + + return true; + } + + public abstract T MinValue { get; } + + public abstract T MaxValue { get; } + + public T Min(T a, T b) + { + if (Compare(a, b) < 0) + return a; + else + return b; + } + + public T Max(T a, T b) + { + if (Compare(a, b) > 0) + return a; + else + return b; + } + + public abstract T Zero { get; } + + public abstract T NegativeInfinity { get; } + + public abstract T PositiveInfinity { get; } + + public abstract T Add(T a, T b); + + public abstract T Subtract(T a, T b); + + public abstract T Multiply(T a, T b); + + public abstract T DistanceSquaredBetweenPoints(T[] a, T[] b); + + #endregion + } +} diff --git a/Assets/KdTreeLib/Math/TypeMath.cs.meta b/Assets/KdTreeLib/Math/TypeMath.cs.meta new file mode 100755 index 0000000..d856885 --- /dev/null +++ b/Assets/KdTreeLib/Math/TypeMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 37a96951f6da4064eb31ad4bcfa2dd53 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/NearestNeighbourList.cs b/Assets/KdTreeLib/NearestNeighbourList.cs new file mode 100755 index 0000000..d415829 --- /dev/null +++ b/Assets/KdTreeLib/NearestNeighbourList.cs @@ -0,0 +1,91 @@ +using System; + +namespace KdTree +{ + public interface INearestNeighbourList + { + bool Add(TItem item, TDistance distance); + TItem GetFurtherest(); + TItem RemoveFurtherest(); + + int MaxCapacity { get; } + int Count { get; } + } + + public class NearestNeighbourList : INearestNeighbourList + { + public NearestNeighbourList(int maxCapacity, ITypeMath distanceMath) + { + this.maxCapacity = maxCapacity; + this.distanceMath = distanceMath; + + queue = new PriorityQueue(maxCapacity, distanceMath); + } + + public NearestNeighbourList(ITypeMath distanceMath) + { + this.maxCapacity = int.MaxValue; + this.distanceMath = distanceMath; + + queue = new PriorityQueue(distanceMath); + } + + private PriorityQueue queue; + + private ITypeMath distanceMath; + + private int maxCapacity; + public int MaxCapacity { get { return maxCapacity; } } + + public int Count { get { return queue.Count; } } + + public bool Add(TItem item, TDistance distance) + { + if (queue.Count >= maxCapacity) + { + // If the distance of this item is less than the distance of the last item + // in our neighbour list then pop that neighbour off and push this one on + // otherwise don't even bother adding this item + if (distanceMath.Compare(distance, queue.GetHighestPriority()) < 0) + { + queue.Dequeue(); + queue.Enqueue(item, distance); + return true; + } + else + return false; + } + else + { + queue.Enqueue(item, distance); + return true; + } + } + + public TItem GetFurtherest() + { + if (Count == 0) + throw new Exception("List is empty"); + else + return queue.GetHighest(); + } + + public TDistance GetFurtherestDistance() + { + if (Count == 0) + throw new Exception("List is empty"); + else + return queue.GetHighestPriority(); + } + + public TItem RemoveFurtherest() + { + return queue.Dequeue(); + } + + public bool IsCapacityReached + { + get { return Count == MaxCapacity; } + } + } +} diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IPriorityQueue.cs.meta b/Assets/KdTreeLib/IPriorityQueue.cs.meta new file mode 100755 index 0000000..e37c0b2 --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0551921b902b13842aa2e99c95aa537c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTree.cs b/Assets/KdTreeLib/KdTree.cs new file mode 100755 index 0000000..4dfe924 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs @@ -0,0 +1,676 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Runtime.Serialization.Formatters.Binary; +using System.Text; + +namespace KdTree +{ + public enum AddDuplicateBehavior + { + Skip, + Error, + Update + } + + public class DuplicateNodeError : Exception + { + public DuplicateNodeError() + : base("Cannot Add Node With Duplicate Coordinates") + { + } + } + + [Serializable] + public class KdTree : IKdTree + { + public KdTree(int dimensions, ITypeMath typeMath) + { + this.dimensions = dimensions; + this.typeMath = typeMath; + Count = 0; + } + + public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) + : this(dimensions, typeMath) + { + AddDuplicateBehavior = addDuplicateBehavior; + } + + private int dimensions; + + private ITypeMath typeMath = null; + + private KdTreeNode root = null; + + public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } + + public bool Add(TKey[] point, TValue value) + { + var nodeToAdd = new KdTreeNode(point, value); + + if (root == null) + { + root = new KdTreeNode(point, value); + } + else + { + int dimension = -1; + KdTreeNode parent = root; + + do + { + // Increment the dimension we're searching in + dimension = (dimension + 1) % dimensions; + + // Does the node we're adding have the same hyperpoint as this node? + if (typeMath.AreEqual(point, parent.Point)) + { + switch (AddDuplicateBehavior) + { + case AddDuplicateBehavior.Skip: + return false; + + case AddDuplicateBehavior.Error: + throw new DuplicateNodeError(); + + case AddDuplicateBehavior.Update: + parent.Value = value; + return true; + + default: + // Should never happen + throw new Exception("Unexpected AddDuplicateBehavior"); + } + } + + // Which side does this node sit under in relation to it's parent at this level? + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + + if (parent[compare] == null) + { + parent[compare] = nodeToAdd; + break; + } + else + { + parent = parent[compare]; + } + } + while (true); + } + + Count++; + return true; + } + + private void ReaddChildNodes(KdTreeNode removedNode) + { + if (removedNode.IsLeaf) + return; + + // The folllowing code might seem a little redundant but we're using + // 2 queues so we can add the child nodes back in, in (more or less) + // the same order they were added in the first place + var nodesToReadd = new Queue>(); + + var nodesToReaddQueue = new Queue>(); + + if (removedNode.LeftChild != null) + nodesToReaddQueue.Enqueue(removedNode.LeftChild); + + if (removedNode.RightChild != null) + nodesToReaddQueue.Enqueue(removedNode.RightChild); + + while (nodesToReaddQueue.Count > 0) + { + var nodeToReadd = nodesToReaddQueue.Dequeue(); + + nodesToReadd.Enqueue(nodeToReadd); + + for (int side = -1; side <= 1; side += 2) + { + if (nodeToReadd[side] != null) + { + nodesToReaddQueue.Enqueue(nodeToReadd[side]); + + nodeToReadd[side] = null; + } + } + } + + while (nodesToReadd.Count > 0) + { + var nodeToReadd = nodesToReadd.Dequeue(); + + Count--; + Add(nodeToReadd.Point, nodeToReadd.Value); + } + } + + public void RemoveAt(TKey[] point) + { + // Is tree empty? + if (root == null) + return; + + KdTreeNode node; + + if (typeMath.AreEqual(point, root.Point)) + { + node = root; + root = null; + Count--; + ReaddChildNodes(node); + return; + } + + node = root; + + int dimension = -1; + do + { + dimension = (dimension + 1) % dimensions; + + int compare = typeMath.Compare(point[dimension], node.Point[dimension]); + + if (node[compare] == null) + // Can't find node + return; + + if (typeMath.AreEqual(point, node[compare].Point)) + { + var nodeToRemove = node[compare]; + node[compare] = null; + Count--; + + ReaddChildNodes(nodeToRemove); + } + else + node = node[compare]; + } + while (node != null); + } + + public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) + { + if (count > Count) + count = Count; + + if (count < 0) + { + throw new ArgumentException("Number of neighbors cannot be negative"); + } + + if (count == 0) + return new KdTreeNode[0]; + + var neighbours = new KdTreeNode[count]; + + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + + var rect = HyperRect.Infinite(dimensions, typeMath); + + AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); + + count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + /* + * 1. Search for the target + * + * 1.1 Start by splitting the specified hyper rect + * on the specified node's point along the current + * dimension so that we end up with 2 sub hyper rects + * (current dimension = depth % dimensions) + * + * 1.2 Check what sub rectangle the the target point resides in + * under the current dimension + * + * 1.3 Set that rect to the nearer rect and also the corresponding + * child node to the nearest rect and node and the other rect + * and child node to the further rect and child node (for use later) + * + * 1.4 Travel into the nearer rect and node by calling function + * recursively with nearer rect and node and incrementing + * the depth + * + * 2. Add leaf to list of nearest neighbours + * + * 3. Walk back up tree and at each level: + * + * 3.1 Add node to nearest neighbours if + * we haven't filled our nearest neighbour + * list yet or if it has a distance to target less + * than any of the distances in our current nearest + * neighbours. + * + * 3.2 If there is any point in the further rectangle that is closer to + * the target than our furtherest nearest neighbour then travel into + * that rect and node + * + * That's it, when it finally finishes traversing the branches + * it needs to we'll have our list! + */ + + private void AddNearestNeighbours( + KdTreeNode node, + TKey[] target, + HyperRect rect, + int depth, + NearestNeighbourList, TKey> nearestNeighbours, + TKey maxSearchRadiusSquared) + { + if (node == null) + return; + + // Work out the current dimension + int dimension = depth % dimensions; + + // Split our hyper-rect into 2 sub rects along the current + // node's point on the current dimension + var leftRect = rect.Clone(); + leftRect.MaxPoint[dimension] = node.Point[dimension]; + + var rightRect = rect.Clone(); + rightRect.MinPoint[dimension] = node.Point[dimension]; + + // Which side does the target reside in? + int compare = typeMath.Compare(target[dimension], node.Point[dimension]); + + var nearerRect = compare <= 0 ? leftRect : rightRect; + var furtherRect = compare <= 0 ? rightRect : leftRect; + + var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; + var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; + + // Let's walk down into the nearer branch + if (nearerNode != null) + { + AddNearestNeighbours( + nearerNode, + target, + nearerRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + + TKey distanceSquaredToTarget; + + // Walk down into the further branch but only if our capacity hasn't been reached + // OR if there's a region in the further rect that's closer to the target than our + // current furtherest nearest neighbour + TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + { + if (nearestNeighbours.IsCapacityReached) + { + if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + else + { + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + } + + // Try to add the current node to our nearest neighbours list + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + nearestNeighbours.Add(node, distanceSquaredToTarget); + } + + /// + /// Performs a radial search. + /// + /// Center point + /// Radius to find neighbours within + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + /// + /// Performs a radial search up to a maximum count. + /// + /// Center point + /// Radius to find neighbours within + /// Maximum number of neighbours + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + private KdTreeNode[] RadialSearch(TKey[] center, TKey radius, NearestNeighbourList, TKey> nearestNeighbours) + { + AddNearestNeighbours( + root, + center, + HyperRect.Infinite(dimensions, typeMath), + 0, + nearestNeighbours, + typeMath.Multiply(radius, radius)); + + var count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + public int Count { get; private set; } + + public bool TryFindValueAt(TKey[] point, out TValue value) + { + var parent = root; + int dimension = -1; + do + { + if (parent == null) + { + value = default(TValue); + return false; + } + else if (typeMath.AreEqual(point, parent.Point)) + { + value = parent.Value; + return true; + } + + // Keep searching + dimension = (dimension + 1) % dimensions; + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + parent = parent[compare]; + } + while (true); + } + + public TValue FindValueAt(TKey[] point) + { + if (TryFindValueAt(point, out TValue value)) + return value; + else + return default(TValue); + } + + public bool TryFindValue(TValue value, out TKey[] point) + { + if (root == null) + { + point = null; + return false; + } + + // First-in, First-out list of nodes to search + var nodesToSearch = new Queue>(); + + nodesToSearch.Enqueue(root); + + while (nodesToSearch.Count > 0) + { + var nodeToSearch = nodesToSearch.Dequeue(); + + if (nodeToSearch.Value.Equals(value)) + { + point = nodeToSearch.Point; + return true; + } + else + { + for (int side = -1; side <= 1; side += 2) + { + var childNode = nodeToSearch[side]; + + if (childNode != null) + nodesToSearch.Enqueue(childNode); + } + } + } + + point = null; + return false; + } + + public TKey[] FindValue(TValue value) + { + if (TryFindValue(value, out TKey[] point)) + return point; + else + return null; + } + + private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) + { + sb.AppendLine(node.ToString()); + + for (var side = -1; side <= 1; side += 2) + { + for (var index = 0; index <= depth; index++) + sb.Append("\t"); + + sb.Append(side == -1 ? "L " : "R "); + + if (node[side] == null) + sb.AppendLine(""); + else + AddNodeToStringBuilder(node[side], sb, depth + 1); + } + } + + public override string ToString() + { + if (root == null) + return ""; + + var sb = new StringBuilder(); + AddNodeToStringBuilder(root, sb, 0); + return sb.ToString(); + } + + private void AddNodesToList(KdTreeNode node, List> nodes) + { + if (node == null) + return; + + nodes.Add(node); + + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + AddNodesToList(node[side], nodes); + node[side] = null; + } + } + } + + private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + for (var index = fromIndex + 1; index <= toIndex; index++) + { + var newIndex = index; + + while (true) + { + var a = nodes[newIndex - 1]; + var b = nodes[newIndex]; + if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) + { + nodes[newIndex - 1] = b; + nodes[newIndex] = a; + } + else + break; + } + } + } + + private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + if (fromIndex == toIndex) + { + Add(nodes[fromIndex].Point, nodes[fromIndex].Value); + nodes[fromIndex] = null; + return; + } + + // Sort the array from the fromIndex to the toIndex + SortNodesArray(nodes, byDimension, fromIndex, toIndex); + + // Find the splitting point + int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; + + // Add the splitting point + Add(nodes[midIndex].Point, nodes[midIndex].Value); + nodes[midIndex] = null; + + // Recurse + int nextDimension = (byDimension + 1) % dimensions; + + if (fromIndex < midIndex) + AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); + + if (toIndex > midIndex) + AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); + } + + public void Balance() + { + var nodeList = new List>(); + AddNodesToList(root, nodeList); + + Clear(); + + AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); + } + + private void RemoveChildNodes(KdTreeNode node) + { + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + RemoveChildNodes(node[side]); + node[side] = null; + } + } + } + + public void Clear() + { + if (root != null) + RemoveChildNodes(root); + } + + public void SaveToFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Create(filename)) + { + formatter.Serialize(stream, this); + stream.Flush(); + } + } + + public static KdTree LoadFromFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Open(filename, FileMode.Open)) + { + return (KdTree)formatter.Deserialize(stream); + } + + } + + public IEnumerator> GetEnumerator() + { + var left = new Stack>(); + var right = new Stack>(); + + void addLeft(KdTreeNode node) + { + if (node.LeftChild != null) + { + left.Push(node.LeftChild); + } + } + + void addRight(KdTreeNode node) + { + if (node.RightChild != null) + { + right.Push(node.RightChild); + } + } + + if (root != null) + { + yield return root; + + addLeft(root); + addRight(root); + + while (true) + { + if (left.Any()) + { + var item = left.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else if (right.Any()) + { + var item = right.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else + { + break; + } + } + } + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTree.cs.meta b/Assets/KdTreeLib/KdTree.cs.meta new file mode 100755 index 0000000..d7f1d14 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b61eae78ccd40f0459eb72df19d37003 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTreeNode.cs b/Assets/KdTreeLib/KdTreeNode.cs new file mode 100755 index 0000000..e08f24d --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs @@ -0,0 +1,68 @@ +using System; +using System.Text; + +namespace KdTree +{ + [Serializable] + public class KdTreeNode + { + public KdTreeNode() + { + } + + public KdTreeNode(TKey[] point, TValue value) + { + Point = point; + Value = value; + } + + public TKey[] Point; + public TValue Value = default(TValue); + + internal KdTreeNode LeftChild = null; + internal KdTreeNode RightChild = null; + + internal KdTreeNode this[int compare] + { + get + { + if (compare <= 0) + return LeftChild; + else + return RightChild; + } + set + { + if (compare <= 0) + LeftChild = value; + else + RightChild = value; + } + } + + public bool IsLeaf + { + get + { + return (LeftChild == null) && (RightChild == null); + } + } + + public override string ToString() + { + var sb = new StringBuilder(); + + for (var dimension = 0; dimension < Point.Length; dimension++) + { + sb.Append(Point[dimension].ToString() + "\t"); + } + + if (Value == null) + sb.Append("null"); + else + sb.Append(Value.ToString()); + + return sb.ToString(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTreeNode.cs.meta b/Assets/KdTreeLib/KdTreeNode.cs.meta new file mode 100755 index 0000000..7fa00e6 --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c70bc7c92452a3b4e8de75cf4a32e85f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math.meta b/Assets/KdTreeLib/Math.meta new file mode 100755 index 0000000..7e307d1 --- /dev/null +++ b/Assets/KdTreeLib/Math.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 250be181763ace04ebe9e51229d5b664 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs b/Assets/KdTreeLib/Math/DoubleMath.cs new file mode 100755 index 0000000..a3a95a3 --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class DoubleMath : TypeMath + { + public override int Compare(double a, double b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(double a, double b) + { + return a == b; + } + + public override double MinValue + { + get { return double.MinValue; } + } + + public override double MaxValue + { + get { return double.MaxValue; } + } + + public override double Zero + { + get { return 0; } + } + + public override double NegativeInfinity { get { return double.NegativeInfinity; } } + + public override double PositiveInfinity { get { return double.PositiveInfinity; } } + + public override double Add(double a, double b) + { + return a + b; + } + + public override double Subtract(double a, double b) + { + return a - b; + } + + public override double Multiply(double a, double b) + { + return a * b; + } + + public override double DistanceSquaredBetweenPoints(double[] a, double[] b) + { + double distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + double distOnThisAxis = Subtract(a[dimension], b[dimension]); + double distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs.meta b/Assets/KdTreeLib/Math/DoubleMath.cs.meta new file mode 100755 index 0000000..e0e7d9d --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6878fdbadd195164393a12f522125dec +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/FloatMath.cs b/Assets/KdTreeLib/Math/FloatMath.cs new file mode 100755 index 0000000..e530cca --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class FloatMath : TypeMath + { + public override int Compare(float a, float b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(float a, float b) + { + return a == b; + } + + public override float MinValue + { + get { return float.MinValue; } + } + + public override float MaxValue + { + get { return float.MaxValue; } + } + + public override float Zero + { + get { return 0; } + } + + public override float NegativeInfinity { get { return float.NegativeInfinity; } } + + public override float PositiveInfinity { get { return float.PositiveInfinity; } } + + public override float Add(float a, float b) + { + return a + b; + } + + public override float Subtract(float a, float b) + { + return a - b; + } + + public override float Multiply(float a, float b) + { + return a * b; + } + + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + float distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + float distOnThisAxis = Subtract(a[dimension], b[dimension]); + float distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/FloatMath.cs.meta b/Assets/KdTreeLib/Math/FloatMath.cs.meta new file mode 100755 index 0000000..d1e609c --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e1a4895e13ce1b64491965b3a4b65201 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoMath.cs b/Assets/KdTreeLib/Math/GeoMath.cs new file mode 100755 index 0000000..7a71408 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs @@ -0,0 +1,14 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class GeoMath : FloatMath + { + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + double dst = GeoUtils.Distance(a[0], a[1], b[0], b[1], 'K'); + return (float)(dst * dst); + } + } +} diff --git a/Assets/KdTreeLib/Math/GeoMath.cs.meta b/Assets/KdTreeLib/Math/GeoMath.cs.meta new file mode 100755 index 0000000..7dd9a54 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 88144d944655c7d4eb1e3286f6ce3eb4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoUtils.cs b/Assets/KdTreeLib/Math/GeoUtils.cs new file mode 100755 index 0000000..e593bb7 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoUtils.cs @@ -0,0 +1,42 @@ +namespace KdTree.Math +{ + // Via http://www.geodatasource.com/developers/c-sharp + // This code is licensed under LGPLv3. + public class GeoUtils + { + public static double Distance(double lat1, double lon1, double lat2, double lon2, char unit) + { + double theta = lon1 - lon2; + double dist = System.Math.Sin(Deg2rad(lat1)) * System.Math.Sin(Deg2rad(lat2)) + System.Math.Cos(Deg2rad(lat1)) * System.Math.Cos(Deg2rad(lat2)) * System.Math.Cos(Deg2rad(theta)); + dist = System.Math.Acos(dist); + dist = Rad2deg(dist); + dist = dist * 60 * 1.1515; + if (unit == 'K') + { + dist = dist * 1.609344; + } + else if (unit == 'N') + { + dist = dist * 0.8684; + } + return (dist); + } + + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + //:: This function converts decimal degrees to radians ::: + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + private static double Deg2rad(double deg) + { + return (deg * System.Math.PI / 180.0); + } + + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + //:: This function converts radians to decimal degrees ::: + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + private static double Rad2deg(double rad) + { + return (rad / System.Math.PI * 180.0); + } + + } +} diff --git a/Assets/KdTreeLib/Math/GeoUtils.cs.meta b/Assets/KdTreeLib/Math/GeoUtils.cs.meta new file mode 100755 index 0000000..0b49afa --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoUtils.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6a11a5f65900e7a4891eace9a9f9dd39 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/ITypeMath.cs b/Assets/KdTreeLib/Math/ITypeMath.cs new file mode 100755 index 0000000..efef222 --- /dev/null +++ b/Assets/KdTreeLib/Math/ITypeMath.cs @@ -0,0 +1,33 @@ +namespace KdTree +{ + public interface ITypeMath + { + int Compare(T a, T b); + + T MinValue { get; } + + T MaxValue { get; } + + T Min(T a, T b); + + T Max(T a, T b); + + bool AreEqual(T a, T b); + + bool AreEqual(T[] a, T[] b); + + T Add(T a, T b); + + T Subtract(T a, T b); + + T Multiply(T a, T b); + + T Zero { get; } + + T NegativeInfinity { get; } + + T PositiveInfinity { get; } + + T DistanceSquaredBetweenPoints(T[] a, T[] b); + } +} diff --git a/Assets/KdTreeLib/Math/ITypeMath.cs.meta b/Assets/KdTreeLib/Math/ITypeMath.cs.meta new file mode 100755 index 0000000..d3fd504 --- /dev/null +++ b/Assets/KdTreeLib/Math/ITypeMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 2835d1b3924e9bb4f9c46e88cb981667 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/TypeMath.cs b/Assets/KdTreeLib/Math/TypeMath.cs new file mode 100755 index 0000000..fb3cc2e --- /dev/null +++ b/Assets/KdTreeLib/Math/TypeMath.cs @@ -0,0 +1,65 @@ +using System; + +namespace KdTree.Math +{ + // Algebraic! + [Serializable] + public abstract class TypeMath : ITypeMath + { + #region ITypeMath members + + public abstract int Compare(T a, T b); + + public abstract bool AreEqual(T a, T b); + + public virtual bool AreEqual(T[] a, T[] b) + { + if (a.Length != b.Length) + return false; + + for (var index = 0; index < a.Length; index++) + { + if (!AreEqual(a[index], b[index])) + return false; + } + + return true; + } + + public abstract T MinValue { get; } + + public abstract T MaxValue { get; } + + public T Min(T a, T b) + { + if (Compare(a, b) < 0) + return a; + else + return b; + } + + public T Max(T a, T b) + { + if (Compare(a, b) > 0) + return a; + else + return b; + } + + public abstract T Zero { get; } + + public abstract T NegativeInfinity { get; } + + public abstract T PositiveInfinity { get; } + + public abstract T Add(T a, T b); + + public abstract T Subtract(T a, T b); + + public abstract T Multiply(T a, T b); + + public abstract T DistanceSquaredBetweenPoints(T[] a, T[] b); + + #endregion + } +} diff --git a/Assets/KdTreeLib/Math/TypeMath.cs.meta b/Assets/KdTreeLib/Math/TypeMath.cs.meta new file mode 100755 index 0000000..d856885 --- /dev/null +++ b/Assets/KdTreeLib/Math/TypeMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 37a96951f6da4064eb31ad4bcfa2dd53 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/NearestNeighbourList.cs b/Assets/KdTreeLib/NearestNeighbourList.cs new file mode 100755 index 0000000..d415829 --- /dev/null +++ b/Assets/KdTreeLib/NearestNeighbourList.cs @@ -0,0 +1,91 @@ +using System; + +namespace KdTree +{ + public interface INearestNeighbourList + { + bool Add(TItem item, TDistance distance); + TItem GetFurtherest(); + TItem RemoveFurtherest(); + + int MaxCapacity { get; } + int Count { get; } + } + + public class NearestNeighbourList : INearestNeighbourList + { + public NearestNeighbourList(int maxCapacity, ITypeMath distanceMath) + { + this.maxCapacity = maxCapacity; + this.distanceMath = distanceMath; + + queue = new PriorityQueue(maxCapacity, distanceMath); + } + + public NearestNeighbourList(ITypeMath distanceMath) + { + this.maxCapacity = int.MaxValue; + this.distanceMath = distanceMath; + + queue = new PriorityQueue(distanceMath); + } + + private PriorityQueue queue; + + private ITypeMath distanceMath; + + private int maxCapacity; + public int MaxCapacity { get { return maxCapacity; } } + + public int Count { get { return queue.Count; } } + + public bool Add(TItem item, TDistance distance) + { + if (queue.Count >= maxCapacity) + { + // If the distance of this item is less than the distance of the last item + // in our neighbour list then pop that neighbour off and push this one on + // otherwise don't even bother adding this item + if (distanceMath.Compare(distance, queue.GetHighestPriority()) < 0) + { + queue.Dequeue(); + queue.Enqueue(item, distance); + return true; + } + else + return false; + } + else + { + queue.Enqueue(item, distance); + return true; + } + } + + public TItem GetFurtherest() + { + if (Count == 0) + throw new Exception("List is empty"); + else + return queue.GetHighest(); + } + + public TDistance GetFurtherestDistance() + { + if (Count == 0) + throw new Exception("List is empty"); + else + return queue.GetHighestPriority(); + } + + public TItem RemoveFurtherest() + { + return queue.Dequeue(); + } + + public bool IsCapacityReached + { + get { return Count == MaxCapacity; } + } + } +} diff --git a/Assets/KdTreeLib/NearestNeighbourList.cs.meta b/Assets/KdTreeLib/NearestNeighbourList.cs.meta new file mode 100755 index 0000000..7e9437c --- /dev/null +++ b/Assets/KdTreeLib/NearestNeighbourList.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b69bc0630cce0dd44aaf33dfe886d91a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IPriorityQueue.cs.meta b/Assets/KdTreeLib/IPriorityQueue.cs.meta new file mode 100755 index 0000000..e37c0b2 --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0551921b902b13842aa2e99c95aa537c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTree.cs b/Assets/KdTreeLib/KdTree.cs new file mode 100755 index 0000000..4dfe924 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs @@ -0,0 +1,676 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Runtime.Serialization.Formatters.Binary; +using System.Text; + +namespace KdTree +{ + public enum AddDuplicateBehavior + { + Skip, + Error, + Update + } + + public class DuplicateNodeError : Exception + { + public DuplicateNodeError() + : base("Cannot Add Node With Duplicate Coordinates") + { + } + } + + [Serializable] + public class KdTree : IKdTree + { + public KdTree(int dimensions, ITypeMath typeMath) + { + this.dimensions = dimensions; + this.typeMath = typeMath; + Count = 0; + } + + public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) + : this(dimensions, typeMath) + { + AddDuplicateBehavior = addDuplicateBehavior; + } + + private int dimensions; + + private ITypeMath typeMath = null; + + private KdTreeNode root = null; + + public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } + + public bool Add(TKey[] point, TValue value) + { + var nodeToAdd = new KdTreeNode(point, value); + + if (root == null) + { + root = new KdTreeNode(point, value); + } + else + { + int dimension = -1; + KdTreeNode parent = root; + + do + { + // Increment the dimension we're searching in + dimension = (dimension + 1) % dimensions; + + // Does the node we're adding have the same hyperpoint as this node? + if (typeMath.AreEqual(point, parent.Point)) + { + switch (AddDuplicateBehavior) + { + case AddDuplicateBehavior.Skip: + return false; + + case AddDuplicateBehavior.Error: + throw new DuplicateNodeError(); + + case AddDuplicateBehavior.Update: + parent.Value = value; + return true; + + default: + // Should never happen + throw new Exception("Unexpected AddDuplicateBehavior"); + } + } + + // Which side does this node sit under in relation to it's parent at this level? + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + + if (parent[compare] == null) + { + parent[compare] = nodeToAdd; + break; + } + else + { + parent = parent[compare]; + } + } + while (true); + } + + Count++; + return true; + } + + private void ReaddChildNodes(KdTreeNode removedNode) + { + if (removedNode.IsLeaf) + return; + + // The folllowing code might seem a little redundant but we're using + // 2 queues so we can add the child nodes back in, in (more or less) + // the same order they were added in the first place + var nodesToReadd = new Queue>(); + + var nodesToReaddQueue = new Queue>(); + + if (removedNode.LeftChild != null) + nodesToReaddQueue.Enqueue(removedNode.LeftChild); + + if (removedNode.RightChild != null) + nodesToReaddQueue.Enqueue(removedNode.RightChild); + + while (nodesToReaddQueue.Count > 0) + { + var nodeToReadd = nodesToReaddQueue.Dequeue(); + + nodesToReadd.Enqueue(nodeToReadd); + + for (int side = -1; side <= 1; side += 2) + { + if (nodeToReadd[side] != null) + { + nodesToReaddQueue.Enqueue(nodeToReadd[side]); + + nodeToReadd[side] = null; + } + } + } + + while (nodesToReadd.Count > 0) + { + var nodeToReadd = nodesToReadd.Dequeue(); + + Count--; + Add(nodeToReadd.Point, nodeToReadd.Value); + } + } + + public void RemoveAt(TKey[] point) + { + // Is tree empty? + if (root == null) + return; + + KdTreeNode node; + + if (typeMath.AreEqual(point, root.Point)) + { + node = root; + root = null; + Count--; + ReaddChildNodes(node); + return; + } + + node = root; + + int dimension = -1; + do + { + dimension = (dimension + 1) % dimensions; + + int compare = typeMath.Compare(point[dimension], node.Point[dimension]); + + if (node[compare] == null) + // Can't find node + return; + + if (typeMath.AreEqual(point, node[compare].Point)) + { + var nodeToRemove = node[compare]; + node[compare] = null; + Count--; + + ReaddChildNodes(nodeToRemove); + } + else + node = node[compare]; + } + while (node != null); + } + + public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) + { + if (count > Count) + count = Count; + + if (count < 0) + { + throw new ArgumentException("Number of neighbors cannot be negative"); + } + + if (count == 0) + return new KdTreeNode[0]; + + var neighbours = new KdTreeNode[count]; + + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + + var rect = HyperRect.Infinite(dimensions, typeMath); + + AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); + + count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + /* + * 1. Search for the target + * + * 1.1 Start by splitting the specified hyper rect + * on the specified node's point along the current + * dimension so that we end up with 2 sub hyper rects + * (current dimension = depth % dimensions) + * + * 1.2 Check what sub rectangle the the target point resides in + * under the current dimension + * + * 1.3 Set that rect to the nearer rect and also the corresponding + * child node to the nearest rect and node and the other rect + * and child node to the further rect and child node (for use later) + * + * 1.4 Travel into the nearer rect and node by calling function + * recursively with nearer rect and node and incrementing + * the depth + * + * 2. Add leaf to list of nearest neighbours + * + * 3. Walk back up tree and at each level: + * + * 3.1 Add node to nearest neighbours if + * we haven't filled our nearest neighbour + * list yet or if it has a distance to target less + * than any of the distances in our current nearest + * neighbours. + * + * 3.2 If there is any point in the further rectangle that is closer to + * the target than our furtherest nearest neighbour then travel into + * that rect and node + * + * That's it, when it finally finishes traversing the branches + * it needs to we'll have our list! + */ + + private void AddNearestNeighbours( + KdTreeNode node, + TKey[] target, + HyperRect rect, + int depth, + NearestNeighbourList, TKey> nearestNeighbours, + TKey maxSearchRadiusSquared) + { + if (node == null) + return; + + // Work out the current dimension + int dimension = depth % dimensions; + + // Split our hyper-rect into 2 sub rects along the current + // node's point on the current dimension + var leftRect = rect.Clone(); + leftRect.MaxPoint[dimension] = node.Point[dimension]; + + var rightRect = rect.Clone(); + rightRect.MinPoint[dimension] = node.Point[dimension]; + + // Which side does the target reside in? + int compare = typeMath.Compare(target[dimension], node.Point[dimension]); + + var nearerRect = compare <= 0 ? leftRect : rightRect; + var furtherRect = compare <= 0 ? rightRect : leftRect; + + var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; + var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; + + // Let's walk down into the nearer branch + if (nearerNode != null) + { + AddNearestNeighbours( + nearerNode, + target, + nearerRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + + TKey distanceSquaredToTarget; + + // Walk down into the further branch but only if our capacity hasn't been reached + // OR if there's a region in the further rect that's closer to the target than our + // current furtherest nearest neighbour + TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + { + if (nearestNeighbours.IsCapacityReached) + { + if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + else + { + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + } + + // Try to add the current node to our nearest neighbours list + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + nearestNeighbours.Add(node, distanceSquaredToTarget); + } + + /// + /// Performs a radial search. + /// + /// Center point + /// Radius to find neighbours within + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + /// + /// Performs a radial search up to a maximum count. + /// + /// Center point + /// Radius to find neighbours within + /// Maximum number of neighbours + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + private KdTreeNode[] RadialSearch(TKey[] center, TKey radius, NearestNeighbourList, TKey> nearestNeighbours) + { + AddNearestNeighbours( + root, + center, + HyperRect.Infinite(dimensions, typeMath), + 0, + nearestNeighbours, + typeMath.Multiply(radius, radius)); + + var count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + public int Count { get; private set; } + + public bool TryFindValueAt(TKey[] point, out TValue value) + { + var parent = root; + int dimension = -1; + do + { + if (parent == null) + { + value = default(TValue); + return false; + } + else if (typeMath.AreEqual(point, parent.Point)) + { + value = parent.Value; + return true; + } + + // Keep searching + dimension = (dimension + 1) % dimensions; + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + parent = parent[compare]; + } + while (true); + } + + public TValue FindValueAt(TKey[] point) + { + if (TryFindValueAt(point, out TValue value)) + return value; + else + return default(TValue); + } + + public bool TryFindValue(TValue value, out TKey[] point) + { + if (root == null) + { + point = null; + return false; + } + + // First-in, First-out list of nodes to search + var nodesToSearch = new Queue>(); + + nodesToSearch.Enqueue(root); + + while (nodesToSearch.Count > 0) + { + var nodeToSearch = nodesToSearch.Dequeue(); + + if (nodeToSearch.Value.Equals(value)) + { + point = nodeToSearch.Point; + return true; + } + else + { + for (int side = -1; side <= 1; side += 2) + { + var childNode = nodeToSearch[side]; + + if (childNode != null) + nodesToSearch.Enqueue(childNode); + } + } + } + + point = null; + return false; + } + + public TKey[] FindValue(TValue value) + { + if (TryFindValue(value, out TKey[] point)) + return point; + else + return null; + } + + private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) + { + sb.AppendLine(node.ToString()); + + for (var side = -1; side <= 1; side += 2) + { + for (var index = 0; index <= depth; index++) + sb.Append("\t"); + + sb.Append(side == -1 ? "L " : "R "); + + if (node[side] == null) + sb.AppendLine(""); + else + AddNodeToStringBuilder(node[side], sb, depth + 1); + } + } + + public override string ToString() + { + if (root == null) + return ""; + + var sb = new StringBuilder(); + AddNodeToStringBuilder(root, sb, 0); + return sb.ToString(); + } + + private void AddNodesToList(KdTreeNode node, List> nodes) + { + if (node == null) + return; + + nodes.Add(node); + + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + AddNodesToList(node[side], nodes); + node[side] = null; + } + } + } + + private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + for (var index = fromIndex + 1; index <= toIndex; index++) + { + var newIndex = index; + + while (true) + { + var a = nodes[newIndex - 1]; + var b = nodes[newIndex]; + if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) + { + nodes[newIndex - 1] = b; + nodes[newIndex] = a; + } + else + break; + } + } + } + + private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + if (fromIndex == toIndex) + { + Add(nodes[fromIndex].Point, nodes[fromIndex].Value); + nodes[fromIndex] = null; + return; + } + + // Sort the array from the fromIndex to the toIndex + SortNodesArray(nodes, byDimension, fromIndex, toIndex); + + // Find the splitting point + int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; + + // Add the splitting point + Add(nodes[midIndex].Point, nodes[midIndex].Value); + nodes[midIndex] = null; + + // Recurse + int nextDimension = (byDimension + 1) % dimensions; + + if (fromIndex < midIndex) + AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); + + if (toIndex > midIndex) + AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); + } + + public void Balance() + { + var nodeList = new List>(); + AddNodesToList(root, nodeList); + + Clear(); + + AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); + } + + private void RemoveChildNodes(KdTreeNode node) + { + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + RemoveChildNodes(node[side]); + node[side] = null; + } + } + } + + public void Clear() + { + if (root != null) + RemoveChildNodes(root); + } + + public void SaveToFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Create(filename)) + { + formatter.Serialize(stream, this); + stream.Flush(); + } + } + + public static KdTree LoadFromFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Open(filename, FileMode.Open)) + { + return (KdTree)formatter.Deserialize(stream); + } + + } + + public IEnumerator> GetEnumerator() + { + var left = new Stack>(); + var right = new Stack>(); + + void addLeft(KdTreeNode node) + { + if (node.LeftChild != null) + { + left.Push(node.LeftChild); + } + } + + void addRight(KdTreeNode node) + { + if (node.RightChild != null) + { + right.Push(node.RightChild); + } + } + + if (root != null) + { + yield return root; + + addLeft(root); + addRight(root); + + while (true) + { + if (left.Any()) + { + var item = left.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else if (right.Any()) + { + var item = right.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else + { + break; + } + } + } + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTree.cs.meta b/Assets/KdTreeLib/KdTree.cs.meta new file mode 100755 index 0000000..d7f1d14 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b61eae78ccd40f0459eb72df19d37003 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTreeNode.cs b/Assets/KdTreeLib/KdTreeNode.cs new file mode 100755 index 0000000..e08f24d --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs @@ -0,0 +1,68 @@ +using System; +using System.Text; + +namespace KdTree +{ + [Serializable] + public class KdTreeNode + { + public KdTreeNode() + { + } + + public KdTreeNode(TKey[] point, TValue value) + { + Point = point; + Value = value; + } + + public TKey[] Point; + public TValue Value = default(TValue); + + internal KdTreeNode LeftChild = null; + internal KdTreeNode RightChild = null; + + internal KdTreeNode this[int compare] + { + get + { + if (compare <= 0) + return LeftChild; + else + return RightChild; + } + set + { + if (compare <= 0) + LeftChild = value; + else + RightChild = value; + } + } + + public bool IsLeaf + { + get + { + return (LeftChild == null) && (RightChild == null); + } + } + + public override string ToString() + { + var sb = new StringBuilder(); + + for (var dimension = 0; dimension < Point.Length; dimension++) + { + sb.Append(Point[dimension].ToString() + "\t"); + } + + if (Value == null) + sb.Append("null"); + else + sb.Append(Value.ToString()); + + return sb.ToString(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTreeNode.cs.meta b/Assets/KdTreeLib/KdTreeNode.cs.meta new file mode 100755 index 0000000..7fa00e6 --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c70bc7c92452a3b4e8de75cf4a32e85f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math.meta b/Assets/KdTreeLib/Math.meta new file mode 100755 index 0000000..7e307d1 --- /dev/null +++ b/Assets/KdTreeLib/Math.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 250be181763ace04ebe9e51229d5b664 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs b/Assets/KdTreeLib/Math/DoubleMath.cs new file mode 100755 index 0000000..a3a95a3 --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class DoubleMath : TypeMath + { + public override int Compare(double a, double b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(double a, double b) + { + return a == b; + } + + public override double MinValue + { + get { return double.MinValue; } + } + + public override double MaxValue + { + get { return double.MaxValue; } + } + + public override double Zero + { + get { return 0; } + } + + public override double NegativeInfinity { get { return double.NegativeInfinity; } } + + public override double PositiveInfinity { get { return double.PositiveInfinity; } } + + public override double Add(double a, double b) + { + return a + b; + } + + public override double Subtract(double a, double b) + { + return a - b; + } + + public override double Multiply(double a, double b) + { + return a * b; + } + + public override double DistanceSquaredBetweenPoints(double[] a, double[] b) + { + double distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + double distOnThisAxis = Subtract(a[dimension], b[dimension]); + double distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs.meta b/Assets/KdTreeLib/Math/DoubleMath.cs.meta new file mode 100755 index 0000000..e0e7d9d --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6878fdbadd195164393a12f522125dec +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/FloatMath.cs b/Assets/KdTreeLib/Math/FloatMath.cs new file mode 100755 index 0000000..e530cca --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class FloatMath : TypeMath + { + public override int Compare(float a, float b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(float a, float b) + { + return a == b; + } + + public override float MinValue + { + get { return float.MinValue; } + } + + public override float MaxValue + { + get { return float.MaxValue; } + } + + public override float Zero + { + get { return 0; } + } + + public override float NegativeInfinity { get { return float.NegativeInfinity; } } + + public override float PositiveInfinity { get { return float.PositiveInfinity; } } + + public override float Add(float a, float b) + { + return a + b; + } + + public override float Subtract(float a, float b) + { + return a - b; + } + + public override float Multiply(float a, float b) + { + return a * b; + } + + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + float distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + float distOnThisAxis = Subtract(a[dimension], b[dimension]); + float distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/FloatMath.cs.meta b/Assets/KdTreeLib/Math/FloatMath.cs.meta new file mode 100755 index 0000000..d1e609c --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e1a4895e13ce1b64491965b3a4b65201 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoMath.cs b/Assets/KdTreeLib/Math/GeoMath.cs new file mode 100755 index 0000000..7a71408 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs @@ -0,0 +1,14 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class GeoMath : FloatMath + { + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + double dst = GeoUtils.Distance(a[0], a[1], b[0], b[1], 'K'); + return (float)(dst * dst); + } + } +} diff --git a/Assets/KdTreeLib/Math/GeoMath.cs.meta b/Assets/KdTreeLib/Math/GeoMath.cs.meta new file mode 100755 index 0000000..7dd9a54 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 88144d944655c7d4eb1e3286f6ce3eb4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoUtils.cs b/Assets/KdTreeLib/Math/GeoUtils.cs new file mode 100755 index 0000000..e593bb7 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoUtils.cs @@ -0,0 +1,42 @@ +namespace KdTree.Math +{ + // Via http://www.geodatasource.com/developers/c-sharp + // This code is licensed under LGPLv3. + public class GeoUtils + { + public static double Distance(double lat1, double lon1, double lat2, double lon2, char unit) + { + double theta = lon1 - lon2; + double dist = System.Math.Sin(Deg2rad(lat1)) * System.Math.Sin(Deg2rad(lat2)) + System.Math.Cos(Deg2rad(lat1)) * System.Math.Cos(Deg2rad(lat2)) * System.Math.Cos(Deg2rad(theta)); + dist = System.Math.Acos(dist); + dist = Rad2deg(dist); + dist = dist * 60 * 1.1515; + if (unit == 'K') + { + dist = dist * 1.609344; + } + else if (unit == 'N') + { + dist = dist * 0.8684; + } + return (dist); + } + + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + //:: This function converts decimal degrees to radians ::: + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + private static double Deg2rad(double deg) + { + return (deg * System.Math.PI / 180.0); + } + + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + //:: This function converts radians to decimal degrees ::: + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + private static double Rad2deg(double rad) + { + return (rad / System.Math.PI * 180.0); + } + + } +} diff --git a/Assets/KdTreeLib/Math/GeoUtils.cs.meta b/Assets/KdTreeLib/Math/GeoUtils.cs.meta new file mode 100755 index 0000000..0b49afa --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoUtils.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6a11a5f65900e7a4891eace9a9f9dd39 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/ITypeMath.cs b/Assets/KdTreeLib/Math/ITypeMath.cs new file mode 100755 index 0000000..efef222 --- /dev/null +++ b/Assets/KdTreeLib/Math/ITypeMath.cs @@ -0,0 +1,33 @@ +namespace KdTree +{ + public interface ITypeMath + { + int Compare(T a, T b); + + T MinValue { get; } + + T MaxValue { get; } + + T Min(T a, T b); + + T Max(T a, T b); + + bool AreEqual(T a, T b); + + bool AreEqual(T[] a, T[] b); + + T Add(T a, T b); + + T Subtract(T a, T b); + + T Multiply(T a, T b); + + T Zero { get; } + + T NegativeInfinity { get; } + + T PositiveInfinity { get; } + + T DistanceSquaredBetweenPoints(T[] a, T[] b); + } +} diff --git a/Assets/KdTreeLib/Math/ITypeMath.cs.meta b/Assets/KdTreeLib/Math/ITypeMath.cs.meta new file mode 100755 index 0000000..d3fd504 --- /dev/null +++ b/Assets/KdTreeLib/Math/ITypeMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 2835d1b3924e9bb4f9c46e88cb981667 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/TypeMath.cs b/Assets/KdTreeLib/Math/TypeMath.cs new file mode 100755 index 0000000..fb3cc2e --- /dev/null +++ b/Assets/KdTreeLib/Math/TypeMath.cs @@ -0,0 +1,65 @@ +using System; + +namespace KdTree.Math +{ + // Algebraic! + [Serializable] + public abstract class TypeMath : ITypeMath + { + #region ITypeMath members + + public abstract int Compare(T a, T b); + + public abstract bool AreEqual(T a, T b); + + public virtual bool AreEqual(T[] a, T[] b) + { + if (a.Length != b.Length) + return false; + + for (var index = 0; index < a.Length; index++) + { + if (!AreEqual(a[index], b[index])) + return false; + } + + return true; + } + + public abstract T MinValue { get; } + + public abstract T MaxValue { get; } + + public T Min(T a, T b) + { + if (Compare(a, b) < 0) + return a; + else + return b; + } + + public T Max(T a, T b) + { + if (Compare(a, b) > 0) + return a; + else + return b; + } + + public abstract T Zero { get; } + + public abstract T NegativeInfinity { get; } + + public abstract T PositiveInfinity { get; } + + public abstract T Add(T a, T b); + + public abstract T Subtract(T a, T b); + + public abstract T Multiply(T a, T b); + + public abstract T DistanceSquaredBetweenPoints(T[] a, T[] b); + + #endregion + } +} diff --git a/Assets/KdTreeLib/Math/TypeMath.cs.meta b/Assets/KdTreeLib/Math/TypeMath.cs.meta new file mode 100755 index 0000000..d856885 --- /dev/null +++ b/Assets/KdTreeLib/Math/TypeMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 37a96951f6da4064eb31ad4bcfa2dd53 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/NearestNeighbourList.cs b/Assets/KdTreeLib/NearestNeighbourList.cs new file mode 100755 index 0000000..d415829 --- /dev/null +++ b/Assets/KdTreeLib/NearestNeighbourList.cs @@ -0,0 +1,91 @@ +using System; + +namespace KdTree +{ + public interface INearestNeighbourList + { + bool Add(TItem item, TDistance distance); + TItem GetFurtherest(); + TItem RemoveFurtherest(); + + int MaxCapacity { get; } + int Count { get; } + } + + public class NearestNeighbourList : INearestNeighbourList + { + public NearestNeighbourList(int maxCapacity, ITypeMath distanceMath) + { + this.maxCapacity = maxCapacity; + this.distanceMath = distanceMath; + + queue = new PriorityQueue(maxCapacity, distanceMath); + } + + public NearestNeighbourList(ITypeMath distanceMath) + { + this.maxCapacity = int.MaxValue; + this.distanceMath = distanceMath; + + queue = new PriorityQueue(distanceMath); + } + + private PriorityQueue queue; + + private ITypeMath distanceMath; + + private int maxCapacity; + public int MaxCapacity { get { return maxCapacity; } } + + public int Count { get { return queue.Count; } } + + public bool Add(TItem item, TDistance distance) + { + if (queue.Count >= maxCapacity) + { + // If the distance of this item is less than the distance of the last item + // in our neighbour list then pop that neighbour off and push this one on + // otherwise don't even bother adding this item + if (distanceMath.Compare(distance, queue.GetHighestPriority()) < 0) + { + queue.Dequeue(); + queue.Enqueue(item, distance); + return true; + } + else + return false; + } + else + { + queue.Enqueue(item, distance); + return true; + } + } + + public TItem GetFurtherest() + { + if (Count == 0) + throw new Exception("List is empty"); + else + return queue.GetHighest(); + } + + public TDistance GetFurtherestDistance() + { + if (Count == 0) + throw new Exception("List is empty"); + else + return queue.GetHighestPriority(); + } + + public TItem RemoveFurtherest() + { + return queue.Dequeue(); + } + + public bool IsCapacityReached + { + get { return Count == MaxCapacity; } + } + } +} diff --git a/Assets/KdTreeLib/NearestNeighbourList.cs.meta b/Assets/KdTreeLib/NearestNeighbourList.cs.meta new file mode 100755 index 0000000..7e9437c --- /dev/null +++ b/Assets/KdTreeLib/NearestNeighbourList.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b69bc0630cce0dd44aaf33dfe886d91a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/PriorityQueue.cs b/Assets/KdTreeLib/PriorityQueue.cs new file mode 100755 index 0000000..73db395 --- /dev/null +++ b/Assets/KdTreeLib/PriorityQueue.cs @@ -0,0 +1,136 @@ +using System; + +struct ItemPriority +{ + public TItem Item; + public TPriority Priority; +} + +namespace KdTree +{ + public class PriorityQueue : IPriorityQueue + { + public PriorityQueue(int capacity, ITypeMath priorityMath) + { + if (capacity <= 0) + throw new ArgumentException("Capacity must be greater than zero"); + + this.capacity = capacity; + queue = new ItemPriority[capacity]; + + this.priorityMath = priorityMath; + } + + /// + ///This constructor will use a default capacity of 4. + /// + public PriorityQueue(ITypeMath priorityMath) + { + this.capacity = 4; + queue = new ItemPriority[capacity]; + + this.priorityMath = priorityMath; + } + + private ITypeMath priorityMath; + + private ItemPriority[] queue; + + private int capacity; + + private int count; + public int Count { get { return count; } } + + // Try to avoid unnecessary slow memory reallocations by creating your queue with an ample capacity + private void ExpandCapacity() + { + // Double our capacity + capacity *= 2; + + // Create a new queue + var newQueue = new ItemPriority[capacity]; + + // Copy the contents of the original queue to the new one + Array.Copy(queue, newQueue, queue.Length); + + // Copy the new queue over the original one + queue = newQueue; + } + + public void Enqueue(TItem item, TPriority priority) + { + if (++count > capacity) + ExpandCapacity(); + + int newItemIndex = count - 1; + + queue[newItemIndex] = new ItemPriority { Item = item, Priority = priority }; + + ReorderItem(newItemIndex, -1); + } + + public TItem Dequeue() + { + TItem item = queue[0].Item; + + queue[0].Item = default(TItem); + queue[0].Priority = priorityMath.MinValue; + + ReorderItem(0, 1); + + count--; + + return item; + } + + private void ReorderItem(int index, int direction) + { + if ((direction != -1) && (direction != 1)) + throw new ArgumentException("Invalid Direction"); + + var item = queue[index]; + + int nextIndex = index + direction; + + while ((nextIndex >= 0) && (nextIndex < count)) + { + var next = queue[nextIndex]; + + int compare = priorityMath.Compare(item.Priority, next.Priority); + + // If we're moving up and our priority is higher than the next priority then swap + // Or if we're moving down and our priority is lower than the next priority then swap + if ( + ((direction == -1) && (compare > 0)) + || + ((direction == 1) && (compare < 0)) + ) + { + queue[index] = next; + queue[nextIndex] = item; + + index += direction; + nextIndex += direction; + } + else + break; + } + } + + public TItem GetHighest() + { + if (count == 0) + throw new Exception("Queue is empty"); + else + return queue[0].Item; + } + + public TPriority GetHighestPriority() + { + if (count == 0) + throw new Exception("Queue is empty"); + else + return queue[0].Priority; + } + } +} diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IPriorityQueue.cs.meta b/Assets/KdTreeLib/IPriorityQueue.cs.meta new file mode 100755 index 0000000..e37c0b2 --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0551921b902b13842aa2e99c95aa537c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTree.cs b/Assets/KdTreeLib/KdTree.cs new file mode 100755 index 0000000..4dfe924 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs @@ -0,0 +1,676 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Runtime.Serialization.Formatters.Binary; +using System.Text; + +namespace KdTree +{ + public enum AddDuplicateBehavior + { + Skip, + Error, + Update + } + + public class DuplicateNodeError : Exception + { + public DuplicateNodeError() + : base("Cannot Add Node With Duplicate Coordinates") + { + } + } + + [Serializable] + public class KdTree : IKdTree + { + public KdTree(int dimensions, ITypeMath typeMath) + { + this.dimensions = dimensions; + this.typeMath = typeMath; + Count = 0; + } + + public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) + : this(dimensions, typeMath) + { + AddDuplicateBehavior = addDuplicateBehavior; + } + + private int dimensions; + + private ITypeMath typeMath = null; + + private KdTreeNode root = null; + + public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } + + public bool Add(TKey[] point, TValue value) + { + var nodeToAdd = new KdTreeNode(point, value); + + if (root == null) + { + root = new KdTreeNode(point, value); + } + else + { + int dimension = -1; + KdTreeNode parent = root; + + do + { + // Increment the dimension we're searching in + dimension = (dimension + 1) % dimensions; + + // Does the node we're adding have the same hyperpoint as this node? + if (typeMath.AreEqual(point, parent.Point)) + { + switch (AddDuplicateBehavior) + { + case AddDuplicateBehavior.Skip: + return false; + + case AddDuplicateBehavior.Error: + throw new DuplicateNodeError(); + + case AddDuplicateBehavior.Update: + parent.Value = value; + return true; + + default: + // Should never happen + throw new Exception("Unexpected AddDuplicateBehavior"); + } + } + + // Which side does this node sit under in relation to it's parent at this level? + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + + if (parent[compare] == null) + { + parent[compare] = nodeToAdd; + break; + } + else + { + parent = parent[compare]; + } + } + while (true); + } + + Count++; + return true; + } + + private void ReaddChildNodes(KdTreeNode removedNode) + { + if (removedNode.IsLeaf) + return; + + // The folllowing code might seem a little redundant but we're using + // 2 queues so we can add the child nodes back in, in (more or less) + // the same order they were added in the first place + var nodesToReadd = new Queue>(); + + var nodesToReaddQueue = new Queue>(); + + if (removedNode.LeftChild != null) + nodesToReaddQueue.Enqueue(removedNode.LeftChild); + + if (removedNode.RightChild != null) + nodesToReaddQueue.Enqueue(removedNode.RightChild); + + while (nodesToReaddQueue.Count > 0) + { + var nodeToReadd = nodesToReaddQueue.Dequeue(); + + nodesToReadd.Enqueue(nodeToReadd); + + for (int side = -1; side <= 1; side += 2) + { + if (nodeToReadd[side] != null) + { + nodesToReaddQueue.Enqueue(nodeToReadd[side]); + + nodeToReadd[side] = null; + } + } + } + + while (nodesToReadd.Count > 0) + { + var nodeToReadd = nodesToReadd.Dequeue(); + + Count--; + Add(nodeToReadd.Point, nodeToReadd.Value); + } + } + + public void RemoveAt(TKey[] point) + { + // Is tree empty? + if (root == null) + return; + + KdTreeNode node; + + if (typeMath.AreEqual(point, root.Point)) + { + node = root; + root = null; + Count--; + ReaddChildNodes(node); + return; + } + + node = root; + + int dimension = -1; + do + { + dimension = (dimension + 1) % dimensions; + + int compare = typeMath.Compare(point[dimension], node.Point[dimension]); + + if (node[compare] == null) + // Can't find node + return; + + if (typeMath.AreEqual(point, node[compare].Point)) + { + var nodeToRemove = node[compare]; + node[compare] = null; + Count--; + + ReaddChildNodes(nodeToRemove); + } + else + node = node[compare]; + } + while (node != null); + } + + public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) + { + if (count > Count) + count = Count; + + if (count < 0) + { + throw new ArgumentException("Number of neighbors cannot be negative"); + } + + if (count == 0) + return new KdTreeNode[0]; + + var neighbours = new KdTreeNode[count]; + + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + + var rect = HyperRect.Infinite(dimensions, typeMath); + + AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); + + count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + /* + * 1. Search for the target + * + * 1.1 Start by splitting the specified hyper rect + * on the specified node's point along the current + * dimension so that we end up with 2 sub hyper rects + * (current dimension = depth % dimensions) + * + * 1.2 Check what sub rectangle the the target point resides in + * under the current dimension + * + * 1.3 Set that rect to the nearer rect and also the corresponding + * child node to the nearest rect and node and the other rect + * and child node to the further rect and child node (for use later) + * + * 1.4 Travel into the nearer rect and node by calling function + * recursively with nearer rect and node and incrementing + * the depth + * + * 2. Add leaf to list of nearest neighbours + * + * 3. Walk back up tree and at each level: + * + * 3.1 Add node to nearest neighbours if + * we haven't filled our nearest neighbour + * list yet or if it has a distance to target less + * than any of the distances in our current nearest + * neighbours. + * + * 3.2 If there is any point in the further rectangle that is closer to + * the target than our furtherest nearest neighbour then travel into + * that rect and node + * + * That's it, when it finally finishes traversing the branches + * it needs to we'll have our list! + */ + + private void AddNearestNeighbours( + KdTreeNode node, + TKey[] target, + HyperRect rect, + int depth, + NearestNeighbourList, TKey> nearestNeighbours, + TKey maxSearchRadiusSquared) + { + if (node == null) + return; + + // Work out the current dimension + int dimension = depth % dimensions; + + // Split our hyper-rect into 2 sub rects along the current + // node's point on the current dimension + var leftRect = rect.Clone(); + leftRect.MaxPoint[dimension] = node.Point[dimension]; + + var rightRect = rect.Clone(); + rightRect.MinPoint[dimension] = node.Point[dimension]; + + // Which side does the target reside in? + int compare = typeMath.Compare(target[dimension], node.Point[dimension]); + + var nearerRect = compare <= 0 ? leftRect : rightRect; + var furtherRect = compare <= 0 ? rightRect : leftRect; + + var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; + var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; + + // Let's walk down into the nearer branch + if (nearerNode != null) + { + AddNearestNeighbours( + nearerNode, + target, + nearerRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + + TKey distanceSquaredToTarget; + + // Walk down into the further branch but only if our capacity hasn't been reached + // OR if there's a region in the further rect that's closer to the target than our + // current furtherest nearest neighbour + TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + { + if (nearestNeighbours.IsCapacityReached) + { + if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + else + { + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + } + + // Try to add the current node to our nearest neighbours list + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + nearestNeighbours.Add(node, distanceSquaredToTarget); + } + + /// + /// Performs a radial search. + /// + /// Center point + /// Radius to find neighbours within + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + /// + /// Performs a radial search up to a maximum count. + /// + /// Center point + /// Radius to find neighbours within + /// Maximum number of neighbours + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + private KdTreeNode[] RadialSearch(TKey[] center, TKey radius, NearestNeighbourList, TKey> nearestNeighbours) + { + AddNearestNeighbours( + root, + center, + HyperRect.Infinite(dimensions, typeMath), + 0, + nearestNeighbours, + typeMath.Multiply(radius, radius)); + + var count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + public int Count { get; private set; } + + public bool TryFindValueAt(TKey[] point, out TValue value) + { + var parent = root; + int dimension = -1; + do + { + if (parent == null) + { + value = default(TValue); + return false; + } + else if (typeMath.AreEqual(point, parent.Point)) + { + value = parent.Value; + return true; + } + + // Keep searching + dimension = (dimension + 1) % dimensions; + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + parent = parent[compare]; + } + while (true); + } + + public TValue FindValueAt(TKey[] point) + { + if (TryFindValueAt(point, out TValue value)) + return value; + else + return default(TValue); + } + + public bool TryFindValue(TValue value, out TKey[] point) + { + if (root == null) + { + point = null; + return false; + } + + // First-in, First-out list of nodes to search + var nodesToSearch = new Queue>(); + + nodesToSearch.Enqueue(root); + + while (nodesToSearch.Count > 0) + { + var nodeToSearch = nodesToSearch.Dequeue(); + + if (nodeToSearch.Value.Equals(value)) + { + point = nodeToSearch.Point; + return true; + } + else + { + for (int side = -1; side <= 1; side += 2) + { + var childNode = nodeToSearch[side]; + + if (childNode != null) + nodesToSearch.Enqueue(childNode); + } + } + } + + point = null; + return false; + } + + public TKey[] FindValue(TValue value) + { + if (TryFindValue(value, out TKey[] point)) + return point; + else + return null; + } + + private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) + { + sb.AppendLine(node.ToString()); + + for (var side = -1; side <= 1; side += 2) + { + for (var index = 0; index <= depth; index++) + sb.Append("\t"); + + sb.Append(side == -1 ? "L " : "R "); + + if (node[side] == null) + sb.AppendLine(""); + else + AddNodeToStringBuilder(node[side], sb, depth + 1); + } + } + + public override string ToString() + { + if (root == null) + return ""; + + var sb = new StringBuilder(); + AddNodeToStringBuilder(root, sb, 0); + return sb.ToString(); + } + + private void AddNodesToList(KdTreeNode node, List> nodes) + { + if (node == null) + return; + + nodes.Add(node); + + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + AddNodesToList(node[side], nodes); + node[side] = null; + } + } + } + + private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + for (var index = fromIndex + 1; index <= toIndex; index++) + { + var newIndex = index; + + while (true) + { + var a = nodes[newIndex - 1]; + var b = nodes[newIndex]; + if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) + { + nodes[newIndex - 1] = b; + nodes[newIndex] = a; + } + else + break; + } + } + } + + private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + if (fromIndex == toIndex) + { + Add(nodes[fromIndex].Point, nodes[fromIndex].Value); + nodes[fromIndex] = null; + return; + } + + // Sort the array from the fromIndex to the toIndex + SortNodesArray(nodes, byDimension, fromIndex, toIndex); + + // Find the splitting point + int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; + + // Add the splitting point + Add(nodes[midIndex].Point, nodes[midIndex].Value); + nodes[midIndex] = null; + + // Recurse + int nextDimension = (byDimension + 1) % dimensions; + + if (fromIndex < midIndex) + AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); + + if (toIndex > midIndex) + AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); + } + + public void Balance() + { + var nodeList = new List>(); + AddNodesToList(root, nodeList); + + Clear(); + + AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); + } + + private void RemoveChildNodes(KdTreeNode node) + { + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + RemoveChildNodes(node[side]); + node[side] = null; + } + } + } + + public void Clear() + { + if (root != null) + RemoveChildNodes(root); + } + + public void SaveToFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Create(filename)) + { + formatter.Serialize(stream, this); + stream.Flush(); + } + } + + public static KdTree LoadFromFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Open(filename, FileMode.Open)) + { + return (KdTree)formatter.Deserialize(stream); + } + + } + + public IEnumerator> GetEnumerator() + { + var left = new Stack>(); + var right = new Stack>(); + + void addLeft(KdTreeNode node) + { + if (node.LeftChild != null) + { + left.Push(node.LeftChild); + } + } + + void addRight(KdTreeNode node) + { + if (node.RightChild != null) + { + right.Push(node.RightChild); + } + } + + if (root != null) + { + yield return root; + + addLeft(root); + addRight(root); + + while (true) + { + if (left.Any()) + { + var item = left.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else if (right.Any()) + { + var item = right.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else + { + break; + } + } + } + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTree.cs.meta b/Assets/KdTreeLib/KdTree.cs.meta new file mode 100755 index 0000000..d7f1d14 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b61eae78ccd40f0459eb72df19d37003 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTreeNode.cs b/Assets/KdTreeLib/KdTreeNode.cs new file mode 100755 index 0000000..e08f24d --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs @@ -0,0 +1,68 @@ +using System; +using System.Text; + +namespace KdTree +{ + [Serializable] + public class KdTreeNode + { + public KdTreeNode() + { + } + + public KdTreeNode(TKey[] point, TValue value) + { + Point = point; + Value = value; + } + + public TKey[] Point; + public TValue Value = default(TValue); + + internal KdTreeNode LeftChild = null; + internal KdTreeNode RightChild = null; + + internal KdTreeNode this[int compare] + { + get + { + if (compare <= 0) + return LeftChild; + else + return RightChild; + } + set + { + if (compare <= 0) + LeftChild = value; + else + RightChild = value; + } + } + + public bool IsLeaf + { + get + { + return (LeftChild == null) && (RightChild == null); + } + } + + public override string ToString() + { + var sb = new StringBuilder(); + + for (var dimension = 0; dimension < Point.Length; dimension++) + { + sb.Append(Point[dimension].ToString() + "\t"); + } + + if (Value == null) + sb.Append("null"); + else + sb.Append(Value.ToString()); + + return sb.ToString(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTreeNode.cs.meta b/Assets/KdTreeLib/KdTreeNode.cs.meta new file mode 100755 index 0000000..7fa00e6 --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c70bc7c92452a3b4e8de75cf4a32e85f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math.meta b/Assets/KdTreeLib/Math.meta new file mode 100755 index 0000000..7e307d1 --- /dev/null +++ b/Assets/KdTreeLib/Math.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 250be181763ace04ebe9e51229d5b664 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs b/Assets/KdTreeLib/Math/DoubleMath.cs new file mode 100755 index 0000000..a3a95a3 --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class DoubleMath : TypeMath + { + public override int Compare(double a, double b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(double a, double b) + { + return a == b; + } + + public override double MinValue + { + get { return double.MinValue; } + } + + public override double MaxValue + { + get { return double.MaxValue; } + } + + public override double Zero + { + get { return 0; } + } + + public override double NegativeInfinity { get { return double.NegativeInfinity; } } + + public override double PositiveInfinity { get { return double.PositiveInfinity; } } + + public override double Add(double a, double b) + { + return a + b; + } + + public override double Subtract(double a, double b) + { + return a - b; + } + + public override double Multiply(double a, double b) + { + return a * b; + } + + public override double DistanceSquaredBetweenPoints(double[] a, double[] b) + { + double distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + double distOnThisAxis = Subtract(a[dimension], b[dimension]); + double distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs.meta b/Assets/KdTreeLib/Math/DoubleMath.cs.meta new file mode 100755 index 0000000..e0e7d9d --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6878fdbadd195164393a12f522125dec +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/FloatMath.cs b/Assets/KdTreeLib/Math/FloatMath.cs new file mode 100755 index 0000000..e530cca --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class FloatMath : TypeMath + { + public override int Compare(float a, float b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(float a, float b) + { + return a == b; + } + + public override float MinValue + { + get { return float.MinValue; } + } + + public override float MaxValue + { + get { return float.MaxValue; } + } + + public override float Zero + { + get { return 0; } + } + + public override float NegativeInfinity { get { return float.NegativeInfinity; } } + + public override float PositiveInfinity { get { return float.PositiveInfinity; } } + + public override float Add(float a, float b) + { + return a + b; + } + + public override float Subtract(float a, float b) + { + return a - b; + } + + public override float Multiply(float a, float b) + { + return a * b; + } + + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + float distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + float distOnThisAxis = Subtract(a[dimension], b[dimension]); + float distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/FloatMath.cs.meta b/Assets/KdTreeLib/Math/FloatMath.cs.meta new file mode 100755 index 0000000..d1e609c --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e1a4895e13ce1b64491965b3a4b65201 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoMath.cs b/Assets/KdTreeLib/Math/GeoMath.cs new file mode 100755 index 0000000..7a71408 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs @@ -0,0 +1,14 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class GeoMath : FloatMath + { + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + double dst = GeoUtils.Distance(a[0], a[1], b[0], b[1], 'K'); + return (float)(dst * dst); + } + } +} diff --git a/Assets/KdTreeLib/Math/GeoMath.cs.meta b/Assets/KdTreeLib/Math/GeoMath.cs.meta new file mode 100755 index 0000000..7dd9a54 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 88144d944655c7d4eb1e3286f6ce3eb4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoUtils.cs b/Assets/KdTreeLib/Math/GeoUtils.cs new file mode 100755 index 0000000..e593bb7 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoUtils.cs @@ -0,0 +1,42 @@ +namespace KdTree.Math +{ + // Via http://www.geodatasource.com/developers/c-sharp + // This code is licensed under LGPLv3. + public class GeoUtils + { + public static double Distance(double lat1, double lon1, double lat2, double lon2, char unit) + { + double theta = lon1 - lon2; + double dist = System.Math.Sin(Deg2rad(lat1)) * System.Math.Sin(Deg2rad(lat2)) + System.Math.Cos(Deg2rad(lat1)) * System.Math.Cos(Deg2rad(lat2)) * System.Math.Cos(Deg2rad(theta)); + dist = System.Math.Acos(dist); + dist = Rad2deg(dist); + dist = dist * 60 * 1.1515; + if (unit == 'K') + { + dist = dist * 1.609344; + } + else if (unit == 'N') + { + dist = dist * 0.8684; + } + return (dist); + } + + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + //:: This function converts decimal degrees to radians ::: + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + private static double Deg2rad(double deg) + { + return (deg * System.Math.PI / 180.0); + } + + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + //:: This function converts radians to decimal degrees ::: + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + private static double Rad2deg(double rad) + { + return (rad / System.Math.PI * 180.0); + } + + } +} diff --git a/Assets/KdTreeLib/Math/GeoUtils.cs.meta b/Assets/KdTreeLib/Math/GeoUtils.cs.meta new file mode 100755 index 0000000..0b49afa --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoUtils.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6a11a5f65900e7a4891eace9a9f9dd39 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/ITypeMath.cs b/Assets/KdTreeLib/Math/ITypeMath.cs new file mode 100755 index 0000000..efef222 --- /dev/null +++ b/Assets/KdTreeLib/Math/ITypeMath.cs @@ -0,0 +1,33 @@ +namespace KdTree +{ + public interface ITypeMath + { + int Compare(T a, T b); + + T MinValue { get; } + + T MaxValue { get; } + + T Min(T a, T b); + + T Max(T a, T b); + + bool AreEqual(T a, T b); + + bool AreEqual(T[] a, T[] b); + + T Add(T a, T b); + + T Subtract(T a, T b); + + T Multiply(T a, T b); + + T Zero { get; } + + T NegativeInfinity { get; } + + T PositiveInfinity { get; } + + T DistanceSquaredBetweenPoints(T[] a, T[] b); + } +} diff --git a/Assets/KdTreeLib/Math/ITypeMath.cs.meta b/Assets/KdTreeLib/Math/ITypeMath.cs.meta new file mode 100755 index 0000000..d3fd504 --- /dev/null +++ b/Assets/KdTreeLib/Math/ITypeMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 2835d1b3924e9bb4f9c46e88cb981667 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/TypeMath.cs b/Assets/KdTreeLib/Math/TypeMath.cs new file mode 100755 index 0000000..fb3cc2e --- /dev/null +++ b/Assets/KdTreeLib/Math/TypeMath.cs @@ -0,0 +1,65 @@ +using System; + +namespace KdTree.Math +{ + // Algebraic! + [Serializable] + public abstract class TypeMath : ITypeMath + { + #region ITypeMath members + + public abstract int Compare(T a, T b); + + public abstract bool AreEqual(T a, T b); + + public virtual bool AreEqual(T[] a, T[] b) + { + if (a.Length != b.Length) + return false; + + for (var index = 0; index < a.Length; index++) + { + if (!AreEqual(a[index], b[index])) + return false; + } + + return true; + } + + public abstract T MinValue { get; } + + public abstract T MaxValue { get; } + + public T Min(T a, T b) + { + if (Compare(a, b) < 0) + return a; + else + return b; + } + + public T Max(T a, T b) + { + if (Compare(a, b) > 0) + return a; + else + return b; + } + + public abstract T Zero { get; } + + public abstract T NegativeInfinity { get; } + + public abstract T PositiveInfinity { get; } + + public abstract T Add(T a, T b); + + public abstract T Subtract(T a, T b); + + public abstract T Multiply(T a, T b); + + public abstract T DistanceSquaredBetweenPoints(T[] a, T[] b); + + #endregion + } +} diff --git a/Assets/KdTreeLib/Math/TypeMath.cs.meta b/Assets/KdTreeLib/Math/TypeMath.cs.meta new file mode 100755 index 0000000..d856885 --- /dev/null +++ b/Assets/KdTreeLib/Math/TypeMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 37a96951f6da4064eb31ad4bcfa2dd53 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/NearestNeighbourList.cs b/Assets/KdTreeLib/NearestNeighbourList.cs new file mode 100755 index 0000000..d415829 --- /dev/null +++ b/Assets/KdTreeLib/NearestNeighbourList.cs @@ -0,0 +1,91 @@ +using System; + +namespace KdTree +{ + public interface INearestNeighbourList + { + bool Add(TItem item, TDistance distance); + TItem GetFurtherest(); + TItem RemoveFurtherest(); + + int MaxCapacity { get; } + int Count { get; } + } + + public class NearestNeighbourList : INearestNeighbourList + { + public NearestNeighbourList(int maxCapacity, ITypeMath distanceMath) + { + this.maxCapacity = maxCapacity; + this.distanceMath = distanceMath; + + queue = new PriorityQueue(maxCapacity, distanceMath); + } + + public NearestNeighbourList(ITypeMath distanceMath) + { + this.maxCapacity = int.MaxValue; + this.distanceMath = distanceMath; + + queue = new PriorityQueue(distanceMath); + } + + private PriorityQueue queue; + + private ITypeMath distanceMath; + + private int maxCapacity; + public int MaxCapacity { get { return maxCapacity; } } + + public int Count { get { return queue.Count; } } + + public bool Add(TItem item, TDistance distance) + { + if (queue.Count >= maxCapacity) + { + // If the distance of this item is less than the distance of the last item + // in our neighbour list then pop that neighbour off and push this one on + // otherwise don't even bother adding this item + if (distanceMath.Compare(distance, queue.GetHighestPriority()) < 0) + { + queue.Dequeue(); + queue.Enqueue(item, distance); + return true; + } + else + return false; + } + else + { + queue.Enqueue(item, distance); + return true; + } + } + + public TItem GetFurtherest() + { + if (Count == 0) + throw new Exception("List is empty"); + else + return queue.GetHighest(); + } + + public TDistance GetFurtherestDistance() + { + if (Count == 0) + throw new Exception("List is empty"); + else + return queue.GetHighestPriority(); + } + + public TItem RemoveFurtherest() + { + return queue.Dequeue(); + } + + public bool IsCapacityReached + { + get { return Count == MaxCapacity; } + } + } +} diff --git a/Assets/KdTreeLib/NearestNeighbourList.cs.meta b/Assets/KdTreeLib/NearestNeighbourList.cs.meta new file mode 100755 index 0000000..7e9437c --- /dev/null +++ b/Assets/KdTreeLib/NearestNeighbourList.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b69bc0630cce0dd44aaf33dfe886d91a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/PriorityQueue.cs b/Assets/KdTreeLib/PriorityQueue.cs new file mode 100755 index 0000000..73db395 --- /dev/null +++ b/Assets/KdTreeLib/PriorityQueue.cs @@ -0,0 +1,136 @@ +using System; + +struct ItemPriority +{ + public TItem Item; + public TPriority Priority; +} + +namespace KdTree +{ + public class PriorityQueue : IPriorityQueue + { + public PriorityQueue(int capacity, ITypeMath priorityMath) + { + if (capacity <= 0) + throw new ArgumentException("Capacity must be greater than zero"); + + this.capacity = capacity; + queue = new ItemPriority[capacity]; + + this.priorityMath = priorityMath; + } + + /// + ///This constructor will use a default capacity of 4. + /// + public PriorityQueue(ITypeMath priorityMath) + { + this.capacity = 4; + queue = new ItemPriority[capacity]; + + this.priorityMath = priorityMath; + } + + private ITypeMath priorityMath; + + private ItemPriority[] queue; + + private int capacity; + + private int count; + public int Count { get { return count; } } + + // Try to avoid unnecessary slow memory reallocations by creating your queue with an ample capacity + private void ExpandCapacity() + { + // Double our capacity + capacity *= 2; + + // Create a new queue + var newQueue = new ItemPriority[capacity]; + + // Copy the contents of the original queue to the new one + Array.Copy(queue, newQueue, queue.Length); + + // Copy the new queue over the original one + queue = newQueue; + } + + public void Enqueue(TItem item, TPriority priority) + { + if (++count > capacity) + ExpandCapacity(); + + int newItemIndex = count - 1; + + queue[newItemIndex] = new ItemPriority { Item = item, Priority = priority }; + + ReorderItem(newItemIndex, -1); + } + + public TItem Dequeue() + { + TItem item = queue[0].Item; + + queue[0].Item = default(TItem); + queue[0].Priority = priorityMath.MinValue; + + ReorderItem(0, 1); + + count--; + + return item; + } + + private void ReorderItem(int index, int direction) + { + if ((direction != -1) && (direction != 1)) + throw new ArgumentException("Invalid Direction"); + + var item = queue[index]; + + int nextIndex = index + direction; + + while ((nextIndex >= 0) && (nextIndex < count)) + { + var next = queue[nextIndex]; + + int compare = priorityMath.Compare(item.Priority, next.Priority); + + // If we're moving up and our priority is higher than the next priority then swap + // Or if we're moving down and our priority is lower than the next priority then swap + if ( + ((direction == -1) && (compare > 0)) + || + ((direction == 1) && (compare < 0)) + ) + { + queue[index] = next; + queue[nextIndex] = item; + + index += direction; + nextIndex += direction; + } + else + break; + } + } + + public TItem GetHighest() + { + if (count == 0) + throw new Exception("Queue is empty"); + else + return queue[0].Item; + } + + public TPriority GetHighestPriority() + { + if (count == 0) + throw new Exception("Queue is empty"); + else + return queue[0].Priority; + } + } +} diff --git a/Assets/KdTreeLib/PriorityQueue.cs.meta b/Assets/KdTreeLib/PriorityQueue.cs.meta new file mode 100755 index 0000000..d12d2a4 --- /dev/null +++ b/Assets/KdTreeLib/PriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: bacbb5001e7e7104ba9039eb5de6ec2e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib.meta b/Assets/KdTreeLib.meta new file mode 100755 index 0000000..a411b55 --- /dev/null +++ b/Assets/KdTreeLib.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 0536bc46566252741969c6ab038e5366 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/HyperRect.cs b/Assets/KdTreeLib/HyperRect.cs new file mode 100755 index 0000000..df9d8cd --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs @@ -0,0 +1,82 @@ +namespace KdTree +{ + public struct HyperRect + { + private T[] minPoint; + public T[] MinPoint + { + get + { + return minPoint; + } + set + { + minPoint = new T[value.Length]; + value.CopyTo(minPoint, 0); + } + } + + private T[] maxPoint; + public T[] MaxPoint + { + get + { + return maxPoint; + } + set + { + maxPoint = new T[value.Length]; + value.CopyTo(maxPoint, 0); + } + } + + public static HyperRect Infinite(int dimensions, ITypeMath math) + { + var rect = new HyperRect + { + MinPoint = new T[dimensions], + MaxPoint = new T[dimensions] + }; + + for (var dimension = 0; dimension < dimensions; dimension++) + { + rect.MinPoint[dimension] = math.NegativeInfinity; + rect.MaxPoint[dimension] = math.PositiveInfinity; + } + + return rect; + } + + public T[] GetClosestPoint(T[] toPoint, ITypeMath math) + { + T[] closest = new T[toPoint.Length]; + + for (var dimension = 0; dimension < toPoint.Length; dimension++) + { + if (math.Compare(minPoint[dimension], toPoint[dimension]) > 0) + { + closest[dimension] = minPoint[dimension]; + } + else if (math.Compare(maxPoint[dimension], toPoint[dimension]) < 0) + { + closest[dimension] = maxPoint[dimension]; + } + else + // Point is within rectangle, at least on this dimension + closest[dimension] = toPoint[dimension]; + } + + return closest; + } + + public HyperRect Clone() + { + var rect = new HyperRect + { + MinPoint = MinPoint, + MaxPoint = MaxPoint + }; + return rect; + } + } +} diff --git a/Assets/KdTreeLib/HyperRect.cs.meta b/Assets/KdTreeLib/HyperRect.cs.meta new file mode 100755 index 0000000..2c0ca55 --- /dev/null +++ b/Assets/KdTreeLib/HyperRect.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 8c8323b2297f1534289108dcab32beb2 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IKdTree.cs b/Assets/KdTreeLib/IKdTree.cs new file mode 100755 index 0000000..5eff623 --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs @@ -0,0 +1,27 @@ +using System.Collections.Generic; + +namespace KdTree +{ + public interface IKdTree : IEnumerable> + { + bool Add(TKey[] point, TValue value); + + bool TryFindValueAt(TKey[] point, out TValue value); + + TValue FindValueAt(TKey[] point); + + bool TryFindValue(TValue value, out TKey[] point); + + TKey[] FindValue(TValue value); + + KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count); + + void RemoveAt(TKey[] point); + + void Clear(); + + KdTreeNode[] GetNearestNeighbours(TKey[] point, int count = int.MaxValue); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IKdTree.cs.meta b/Assets/KdTreeLib/IKdTree.cs.meta new file mode 100755 index 0000000..e09766d --- /dev/null +++ b/Assets/KdTreeLib/IKdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: f82f5dc23b9f6ed44b1e7625bf66ee3f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/IPriorityQueue.cs b/Assets/KdTreeLib/IPriorityQueue.cs new file mode 100755 index 0000000..570889c --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs @@ -0,0 +1,11 @@ +namespace KdTree +{ + public interface IPriorityQueue + { + void Enqueue(TItem item, TPriority priority); + + TItem Dequeue(); + + int Count { get; } + } +} diff --git a/Assets/KdTreeLib/IPriorityQueue.cs.meta b/Assets/KdTreeLib/IPriorityQueue.cs.meta new file mode 100755 index 0000000..e37c0b2 --- /dev/null +++ b/Assets/KdTreeLib/IPriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 0551921b902b13842aa2e99c95aa537c +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTree.cs b/Assets/KdTreeLib/KdTree.cs new file mode 100755 index 0000000..4dfe924 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs @@ -0,0 +1,676 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.IO; +using System.Linq; +using System.Runtime.Serialization.Formatters.Binary; +using System.Text; + +namespace KdTree +{ + public enum AddDuplicateBehavior + { + Skip, + Error, + Update + } + + public class DuplicateNodeError : Exception + { + public DuplicateNodeError() + : base("Cannot Add Node With Duplicate Coordinates") + { + } + } + + [Serializable] + public class KdTree : IKdTree + { + public KdTree(int dimensions, ITypeMath typeMath) + { + this.dimensions = dimensions; + this.typeMath = typeMath; + Count = 0; + } + + public KdTree(int dimensions, ITypeMath typeMath, AddDuplicateBehavior addDuplicateBehavior) + : this(dimensions, typeMath) + { + AddDuplicateBehavior = addDuplicateBehavior; + } + + private int dimensions; + + private ITypeMath typeMath = null; + + private KdTreeNode root = null; + + public AddDuplicateBehavior AddDuplicateBehavior { get; private set; } + + public bool Add(TKey[] point, TValue value) + { + var nodeToAdd = new KdTreeNode(point, value); + + if (root == null) + { + root = new KdTreeNode(point, value); + } + else + { + int dimension = -1; + KdTreeNode parent = root; + + do + { + // Increment the dimension we're searching in + dimension = (dimension + 1) % dimensions; + + // Does the node we're adding have the same hyperpoint as this node? + if (typeMath.AreEqual(point, parent.Point)) + { + switch (AddDuplicateBehavior) + { + case AddDuplicateBehavior.Skip: + return false; + + case AddDuplicateBehavior.Error: + throw new DuplicateNodeError(); + + case AddDuplicateBehavior.Update: + parent.Value = value; + return true; + + default: + // Should never happen + throw new Exception("Unexpected AddDuplicateBehavior"); + } + } + + // Which side does this node sit under in relation to it's parent at this level? + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + + if (parent[compare] == null) + { + parent[compare] = nodeToAdd; + break; + } + else + { + parent = parent[compare]; + } + } + while (true); + } + + Count++; + return true; + } + + private void ReaddChildNodes(KdTreeNode removedNode) + { + if (removedNode.IsLeaf) + return; + + // The folllowing code might seem a little redundant but we're using + // 2 queues so we can add the child nodes back in, in (more or less) + // the same order they were added in the first place + var nodesToReadd = new Queue>(); + + var nodesToReaddQueue = new Queue>(); + + if (removedNode.LeftChild != null) + nodesToReaddQueue.Enqueue(removedNode.LeftChild); + + if (removedNode.RightChild != null) + nodesToReaddQueue.Enqueue(removedNode.RightChild); + + while (nodesToReaddQueue.Count > 0) + { + var nodeToReadd = nodesToReaddQueue.Dequeue(); + + nodesToReadd.Enqueue(nodeToReadd); + + for (int side = -1; side <= 1; side += 2) + { + if (nodeToReadd[side] != null) + { + nodesToReaddQueue.Enqueue(nodeToReadd[side]); + + nodeToReadd[side] = null; + } + } + } + + while (nodesToReadd.Count > 0) + { + var nodeToReadd = nodesToReadd.Dequeue(); + + Count--; + Add(nodeToReadd.Point, nodeToReadd.Value); + } + } + + public void RemoveAt(TKey[] point) + { + // Is tree empty? + if (root == null) + return; + + KdTreeNode node; + + if (typeMath.AreEqual(point, root.Point)) + { + node = root; + root = null; + Count--; + ReaddChildNodes(node); + return; + } + + node = root; + + int dimension = -1; + do + { + dimension = (dimension + 1) % dimensions; + + int compare = typeMath.Compare(point[dimension], node.Point[dimension]); + + if (node[compare] == null) + // Can't find node + return; + + if (typeMath.AreEqual(point, node[compare].Point)) + { + var nodeToRemove = node[compare]; + node[compare] = null; + Count--; + + ReaddChildNodes(nodeToRemove); + } + else + node = node[compare]; + } + while (node != null); + } + + public KdTreeNode[] GetNearestNeighbours(TKey[] point, int count) + { + if (count > Count) + count = Count; + + if (count < 0) + { + throw new ArgumentException("Number of neighbors cannot be negative"); + } + + if (count == 0) + return new KdTreeNode[0]; + + var neighbours = new KdTreeNode[count]; + + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + + var rect = HyperRect.Infinite(dimensions, typeMath); + + AddNearestNeighbours(root, point, rect, 0, nearestNeighbours, typeMath.MaxValue); + + count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + /* + * 1. Search for the target + * + * 1.1 Start by splitting the specified hyper rect + * on the specified node's point along the current + * dimension so that we end up with 2 sub hyper rects + * (current dimension = depth % dimensions) + * + * 1.2 Check what sub rectangle the the target point resides in + * under the current dimension + * + * 1.3 Set that rect to the nearer rect and also the corresponding + * child node to the nearest rect and node and the other rect + * and child node to the further rect and child node (for use later) + * + * 1.4 Travel into the nearer rect and node by calling function + * recursively with nearer rect and node and incrementing + * the depth + * + * 2. Add leaf to list of nearest neighbours + * + * 3. Walk back up tree and at each level: + * + * 3.1 Add node to nearest neighbours if + * we haven't filled our nearest neighbour + * list yet or if it has a distance to target less + * than any of the distances in our current nearest + * neighbours. + * + * 3.2 If there is any point in the further rectangle that is closer to + * the target than our furtherest nearest neighbour then travel into + * that rect and node + * + * That's it, when it finally finishes traversing the branches + * it needs to we'll have our list! + */ + + private void AddNearestNeighbours( + KdTreeNode node, + TKey[] target, + HyperRect rect, + int depth, + NearestNeighbourList, TKey> nearestNeighbours, + TKey maxSearchRadiusSquared) + { + if (node == null) + return; + + // Work out the current dimension + int dimension = depth % dimensions; + + // Split our hyper-rect into 2 sub rects along the current + // node's point on the current dimension + var leftRect = rect.Clone(); + leftRect.MaxPoint[dimension] = node.Point[dimension]; + + var rightRect = rect.Clone(); + rightRect.MinPoint[dimension] = node.Point[dimension]; + + // Which side does the target reside in? + int compare = typeMath.Compare(target[dimension], node.Point[dimension]); + + var nearerRect = compare <= 0 ? leftRect : rightRect; + var furtherRect = compare <= 0 ? rightRect : leftRect; + + var nearerNode = compare <= 0 ? node.LeftChild : node.RightChild; + var furtherNode = compare <= 0 ? node.RightChild : node.LeftChild; + + // Let's walk down into the nearer branch + if (nearerNode != null) + { + AddNearestNeighbours( + nearerNode, + target, + nearerRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + + TKey distanceSquaredToTarget; + + // Walk down into the further branch but only if our capacity hasn't been reached + // OR if there's a region in the further rect that's closer to the target than our + // current furtherest nearest neighbour + TKey[] closestPointInFurtherRect = furtherRect.GetClosestPoint(target, typeMath); + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(closestPointInFurtherRect, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + { + if (nearestNeighbours.IsCapacityReached) + { + if (typeMath.Compare(distanceSquaredToTarget, nearestNeighbours.GetFurtherestDistance()) < 0) + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + else + { + AddNearestNeighbours( + furtherNode, + target, + furtherRect, + depth + 1, + nearestNeighbours, + maxSearchRadiusSquared); + } + } + + // Try to add the current node to our nearest neighbours list + distanceSquaredToTarget = typeMath.DistanceSquaredBetweenPoints(node.Point, target); + + if (typeMath.Compare(distanceSquaredToTarget, maxSearchRadiusSquared) <= 0) + nearestNeighbours.Add(node, distanceSquaredToTarget); + } + + /// + /// Performs a radial search. + /// + /// Center point + /// Radius to find neighbours within + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + /// + /// Performs a radial search up to a maximum count. + /// + /// Center point + /// Radius to find neighbours within + /// Maximum number of neighbours + public KdTreeNode[] RadialSearch(TKey[] center, TKey radius, int count) + { + var nearestNeighbours = new NearestNeighbourList, TKey>(count, typeMath); + return RadialSearch(center, radius, nearestNeighbours); + } + + private KdTreeNode[] RadialSearch(TKey[] center, TKey radius, NearestNeighbourList, TKey> nearestNeighbours) + { + AddNearestNeighbours( + root, + center, + HyperRect.Infinite(dimensions, typeMath), + 0, + nearestNeighbours, + typeMath.Multiply(radius, radius)); + + var count = nearestNeighbours.Count; + + var neighbourArray = new KdTreeNode[count]; + + for (var index = 0; index < count; index++) + neighbourArray[count - index - 1] = nearestNeighbours.RemoveFurtherest(); + + return neighbourArray; + } + + public int Count { get; private set; } + + public bool TryFindValueAt(TKey[] point, out TValue value) + { + var parent = root; + int dimension = -1; + do + { + if (parent == null) + { + value = default(TValue); + return false; + } + else if (typeMath.AreEqual(point, parent.Point)) + { + value = parent.Value; + return true; + } + + // Keep searching + dimension = (dimension + 1) % dimensions; + int compare = typeMath.Compare(point[dimension], parent.Point[dimension]); + parent = parent[compare]; + } + while (true); + } + + public TValue FindValueAt(TKey[] point) + { + if (TryFindValueAt(point, out TValue value)) + return value; + else + return default(TValue); + } + + public bool TryFindValue(TValue value, out TKey[] point) + { + if (root == null) + { + point = null; + return false; + } + + // First-in, First-out list of nodes to search + var nodesToSearch = new Queue>(); + + nodesToSearch.Enqueue(root); + + while (nodesToSearch.Count > 0) + { + var nodeToSearch = nodesToSearch.Dequeue(); + + if (nodeToSearch.Value.Equals(value)) + { + point = nodeToSearch.Point; + return true; + } + else + { + for (int side = -1; side <= 1; side += 2) + { + var childNode = nodeToSearch[side]; + + if (childNode != null) + nodesToSearch.Enqueue(childNode); + } + } + } + + point = null; + return false; + } + + public TKey[] FindValue(TValue value) + { + if (TryFindValue(value, out TKey[] point)) + return point; + else + return null; + } + + private void AddNodeToStringBuilder(KdTreeNode node, StringBuilder sb, int depth) + { + sb.AppendLine(node.ToString()); + + for (var side = -1; side <= 1; side += 2) + { + for (var index = 0; index <= depth; index++) + sb.Append("\t"); + + sb.Append(side == -1 ? "L " : "R "); + + if (node[side] == null) + sb.AppendLine(""); + else + AddNodeToStringBuilder(node[side], sb, depth + 1); + } + } + + public override string ToString() + { + if (root == null) + return ""; + + var sb = new StringBuilder(); + AddNodeToStringBuilder(root, sb, 0); + return sb.ToString(); + } + + private void AddNodesToList(KdTreeNode node, List> nodes) + { + if (node == null) + return; + + nodes.Add(node); + + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + AddNodesToList(node[side], nodes); + node[side] = null; + } + } + } + + private void SortNodesArray(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + for (var index = fromIndex + 1; index <= toIndex; index++) + { + var newIndex = index; + + while (true) + { + var a = nodes[newIndex - 1]; + var b = nodes[newIndex]; + if (typeMath.Compare(b.Point[byDimension], a.Point[byDimension]) < 0) + { + nodes[newIndex - 1] = b; + nodes[newIndex] = a; + } + else + break; + } + } + } + + private void AddNodesBalanced(KdTreeNode[] nodes, int byDimension, int fromIndex, int toIndex) + { + if (fromIndex == toIndex) + { + Add(nodes[fromIndex].Point, nodes[fromIndex].Value); + nodes[fromIndex] = null; + return; + } + + // Sort the array from the fromIndex to the toIndex + SortNodesArray(nodes, byDimension, fromIndex, toIndex); + + // Find the splitting point + int midIndex = fromIndex + (int)System.Math.Round((toIndex + 1 - fromIndex) / 2f) - 1; + + // Add the splitting point + Add(nodes[midIndex].Point, nodes[midIndex].Value); + nodes[midIndex] = null; + + // Recurse + int nextDimension = (byDimension + 1) % dimensions; + + if (fromIndex < midIndex) + AddNodesBalanced(nodes, nextDimension, fromIndex, midIndex - 1); + + if (toIndex > midIndex) + AddNodesBalanced(nodes, nextDimension, midIndex + 1, toIndex); + } + + public void Balance() + { + var nodeList = new List>(); + AddNodesToList(root, nodeList); + + Clear(); + + AddNodesBalanced(nodeList.ToArray(), 0, 0, nodeList.Count - 1); + } + + private void RemoveChildNodes(KdTreeNode node) + { + for (var side = -1; side <= 1; side += 2) + { + if (node[side] != null) + { + RemoveChildNodes(node[side]); + node[side] = null; + } + } + } + + public void Clear() + { + if (root != null) + RemoveChildNodes(root); + } + + public void SaveToFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Create(filename)) + { + formatter.Serialize(stream, this); + stream.Flush(); + } + } + + public static KdTree LoadFromFile(string filename) + { + BinaryFormatter formatter = new BinaryFormatter(); + using (FileStream stream = File.Open(filename, FileMode.Open)) + { + return (KdTree)formatter.Deserialize(stream); + } + + } + + public IEnumerator> GetEnumerator() + { + var left = new Stack>(); + var right = new Stack>(); + + void addLeft(KdTreeNode node) + { + if (node.LeftChild != null) + { + left.Push(node.LeftChild); + } + } + + void addRight(KdTreeNode node) + { + if (node.RightChild != null) + { + right.Push(node.RightChild); + } + } + + if (root != null) + { + yield return root; + + addLeft(root); + addRight(root); + + while (true) + { + if (left.Any()) + { + var item = left.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else if (right.Any()) + { + var item = right.Pop(); + + addLeft(item); + addRight(item); + + yield return item; + } + else + { + break; + } + } + } + } + + IEnumerator IEnumerable.GetEnumerator() + { + return GetEnumerator(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTree.cs.meta b/Assets/KdTreeLib/KdTree.cs.meta new file mode 100755 index 0000000..d7f1d14 --- /dev/null +++ b/Assets/KdTreeLib/KdTree.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b61eae78ccd40f0459eb72df19d37003 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/KdTreeNode.cs b/Assets/KdTreeLib/KdTreeNode.cs new file mode 100755 index 0000000..e08f24d --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs @@ -0,0 +1,68 @@ +using System; +using System.Text; + +namespace KdTree +{ + [Serializable] + public class KdTreeNode + { + public KdTreeNode() + { + } + + public KdTreeNode(TKey[] point, TValue value) + { + Point = point; + Value = value; + } + + public TKey[] Point; + public TValue Value = default(TValue); + + internal KdTreeNode LeftChild = null; + internal KdTreeNode RightChild = null; + + internal KdTreeNode this[int compare] + { + get + { + if (compare <= 0) + return LeftChild; + else + return RightChild; + } + set + { + if (compare <= 0) + LeftChild = value; + else + RightChild = value; + } + } + + public bool IsLeaf + { + get + { + return (LeftChild == null) && (RightChild == null); + } + } + + public override string ToString() + { + var sb = new StringBuilder(); + + for (var dimension = 0; dimension < Point.Length; dimension++) + { + sb.Append(Point[dimension].ToString() + "\t"); + } + + if (Value == null) + sb.Append("null"); + else + sb.Append(Value.ToString()); + + return sb.ToString(); + } + } +} \ No newline at end of file diff --git a/Assets/KdTreeLib/KdTreeNode.cs.meta b/Assets/KdTreeLib/KdTreeNode.cs.meta new file mode 100755 index 0000000..7fa00e6 --- /dev/null +++ b/Assets/KdTreeLib/KdTreeNode.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: c70bc7c92452a3b4e8de75cf4a32e85f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math.meta b/Assets/KdTreeLib/Math.meta new file mode 100755 index 0000000..7e307d1 --- /dev/null +++ b/Assets/KdTreeLib/Math.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 250be181763ace04ebe9e51229d5b664 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs b/Assets/KdTreeLib/Math/DoubleMath.cs new file mode 100755 index 0000000..a3a95a3 --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class DoubleMath : TypeMath + { + public override int Compare(double a, double b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(double a, double b) + { + return a == b; + } + + public override double MinValue + { + get { return double.MinValue; } + } + + public override double MaxValue + { + get { return double.MaxValue; } + } + + public override double Zero + { + get { return 0; } + } + + public override double NegativeInfinity { get { return double.NegativeInfinity; } } + + public override double PositiveInfinity { get { return double.PositiveInfinity; } } + + public override double Add(double a, double b) + { + return a + b; + } + + public override double Subtract(double a, double b) + { + return a - b; + } + + public override double Multiply(double a, double b) + { + return a * b; + } + + public override double DistanceSquaredBetweenPoints(double[] a, double[] b) + { + double distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + double distOnThisAxis = Subtract(a[dimension], b[dimension]); + double distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/DoubleMath.cs.meta b/Assets/KdTreeLib/Math/DoubleMath.cs.meta new file mode 100755 index 0000000..e0e7d9d --- /dev/null +++ b/Assets/KdTreeLib/Math/DoubleMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6878fdbadd195164393a12f522125dec +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/FloatMath.cs b/Assets/KdTreeLib/Math/FloatMath.cs new file mode 100755 index 0000000..e530cca --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs @@ -0,0 +1,69 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class FloatMath : TypeMath + { + public override int Compare(float a, float b) + { + return a.CompareTo(b); + } + + public override bool AreEqual(float a, float b) + { + return a == b; + } + + public override float MinValue + { + get { return float.MinValue; } + } + + public override float MaxValue + { + get { return float.MaxValue; } + } + + public override float Zero + { + get { return 0; } + } + + public override float NegativeInfinity { get { return float.NegativeInfinity; } } + + public override float PositiveInfinity { get { return float.PositiveInfinity; } } + + public override float Add(float a, float b) + { + return a + b; + } + + public override float Subtract(float a, float b) + { + return a - b; + } + + public override float Multiply(float a, float b) + { + return a * b; + } + + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + float distance = Zero; + int dimensions = a.Length; + + // Return the absolute distance bewteen 2 hyper points + for (var dimension = 0; dimension < dimensions; dimension++) + { + float distOnThisAxis = Subtract(a[dimension], b[dimension]); + float distOnThisAxisSquared = Multiply(distOnThisAxis, distOnThisAxis); + + distance = Add(distance, distOnThisAxisSquared); + } + + return distance; + } + } +} diff --git a/Assets/KdTreeLib/Math/FloatMath.cs.meta b/Assets/KdTreeLib/Math/FloatMath.cs.meta new file mode 100755 index 0000000..d1e609c --- /dev/null +++ b/Assets/KdTreeLib/Math/FloatMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e1a4895e13ce1b64491965b3a4b65201 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoMath.cs b/Assets/KdTreeLib/Math/GeoMath.cs new file mode 100755 index 0000000..7a71408 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs @@ -0,0 +1,14 @@ +using System; + +namespace KdTree.Math +{ + [Serializable] + public class GeoMath : FloatMath + { + public override float DistanceSquaredBetweenPoints(float[] a, float[] b) + { + double dst = GeoUtils.Distance(a[0], a[1], b[0], b[1], 'K'); + return (float)(dst * dst); + } + } +} diff --git a/Assets/KdTreeLib/Math/GeoMath.cs.meta b/Assets/KdTreeLib/Math/GeoMath.cs.meta new file mode 100755 index 0000000..7dd9a54 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 88144d944655c7d4eb1e3286f6ce3eb4 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/GeoUtils.cs b/Assets/KdTreeLib/Math/GeoUtils.cs new file mode 100755 index 0000000..e593bb7 --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoUtils.cs @@ -0,0 +1,42 @@ +namespace KdTree.Math +{ + // Via http://www.geodatasource.com/developers/c-sharp + // This code is licensed under LGPLv3. + public class GeoUtils + { + public static double Distance(double lat1, double lon1, double lat2, double lon2, char unit) + { + double theta = lon1 - lon2; + double dist = System.Math.Sin(Deg2rad(lat1)) * System.Math.Sin(Deg2rad(lat2)) + System.Math.Cos(Deg2rad(lat1)) * System.Math.Cos(Deg2rad(lat2)) * System.Math.Cos(Deg2rad(theta)); + dist = System.Math.Acos(dist); + dist = Rad2deg(dist); + dist = dist * 60 * 1.1515; + if (unit == 'K') + { + dist = dist * 1.609344; + } + else if (unit == 'N') + { + dist = dist * 0.8684; + } + return (dist); + } + + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + //:: This function converts decimal degrees to radians ::: + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + private static double Deg2rad(double deg) + { + return (deg * System.Math.PI / 180.0); + } + + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + //:: This function converts radians to decimal degrees ::: + //::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::::: + private static double Rad2deg(double rad) + { + return (rad / System.Math.PI * 180.0); + } + + } +} diff --git a/Assets/KdTreeLib/Math/GeoUtils.cs.meta b/Assets/KdTreeLib/Math/GeoUtils.cs.meta new file mode 100755 index 0000000..0b49afa --- /dev/null +++ b/Assets/KdTreeLib/Math/GeoUtils.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 6a11a5f65900e7a4891eace9a9f9dd39 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/ITypeMath.cs b/Assets/KdTreeLib/Math/ITypeMath.cs new file mode 100755 index 0000000..efef222 --- /dev/null +++ b/Assets/KdTreeLib/Math/ITypeMath.cs @@ -0,0 +1,33 @@ +namespace KdTree +{ + public interface ITypeMath + { + int Compare(T a, T b); + + T MinValue { get; } + + T MaxValue { get; } + + T Min(T a, T b); + + T Max(T a, T b); + + bool AreEqual(T a, T b); + + bool AreEqual(T[] a, T[] b); + + T Add(T a, T b); + + T Subtract(T a, T b); + + T Multiply(T a, T b); + + T Zero { get; } + + T NegativeInfinity { get; } + + T PositiveInfinity { get; } + + T DistanceSquaredBetweenPoints(T[] a, T[] b); + } +} diff --git a/Assets/KdTreeLib/Math/ITypeMath.cs.meta b/Assets/KdTreeLib/Math/ITypeMath.cs.meta new file mode 100755 index 0000000..d3fd504 --- /dev/null +++ b/Assets/KdTreeLib/Math/ITypeMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 2835d1b3924e9bb4f9c46e88cb981667 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/Math/TypeMath.cs b/Assets/KdTreeLib/Math/TypeMath.cs new file mode 100755 index 0000000..fb3cc2e --- /dev/null +++ b/Assets/KdTreeLib/Math/TypeMath.cs @@ -0,0 +1,65 @@ +using System; + +namespace KdTree.Math +{ + // Algebraic! + [Serializable] + public abstract class TypeMath : ITypeMath + { + #region ITypeMath members + + public abstract int Compare(T a, T b); + + public abstract bool AreEqual(T a, T b); + + public virtual bool AreEqual(T[] a, T[] b) + { + if (a.Length != b.Length) + return false; + + for (var index = 0; index < a.Length; index++) + { + if (!AreEqual(a[index], b[index])) + return false; + } + + return true; + } + + public abstract T MinValue { get; } + + public abstract T MaxValue { get; } + + public T Min(T a, T b) + { + if (Compare(a, b) < 0) + return a; + else + return b; + } + + public T Max(T a, T b) + { + if (Compare(a, b) > 0) + return a; + else + return b; + } + + public abstract T Zero { get; } + + public abstract T NegativeInfinity { get; } + + public abstract T PositiveInfinity { get; } + + public abstract T Add(T a, T b); + + public abstract T Subtract(T a, T b); + + public abstract T Multiply(T a, T b); + + public abstract T DistanceSquaredBetweenPoints(T[] a, T[] b); + + #endregion + } +} diff --git a/Assets/KdTreeLib/Math/TypeMath.cs.meta b/Assets/KdTreeLib/Math/TypeMath.cs.meta new file mode 100755 index 0000000..d856885 --- /dev/null +++ b/Assets/KdTreeLib/Math/TypeMath.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 37a96951f6da4064eb31ad4bcfa2dd53 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/NearestNeighbourList.cs b/Assets/KdTreeLib/NearestNeighbourList.cs new file mode 100755 index 0000000..d415829 --- /dev/null +++ b/Assets/KdTreeLib/NearestNeighbourList.cs @@ -0,0 +1,91 @@ +using System; + +namespace KdTree +{ + public interface INearestNeighbourList + { + bool Add(TItem item, TDistance distance); + TItem GetFurtherest(); + TItem RemoveFurtherest(); + + int MaxCapacity { get; } + int Count { get; } + } + + public class NearestNeighbourList : INearestNeighbourList + { + public NearestNeighbourList(int maxCapacity, ITypeMath distanceMath) + { + this.maxCapacity = maxCapacity; + this.distanceMath = distanceMath; + + queue = new PriorityQueue(maxCapacity, distanceMath); + } + + public NearestNeighbourList(ITypeMath distanceMath) + { + this.maxCapacity = int.MaxValue; + this.distanceMath = distanceMath; + + queue = new PriorityQueue(distanceMath); + } + + private PriorityQueue queue; + + private ITypeMath distanceMath; + + private int maxCapacity; + public int MaxCapacity { get { return maxCapacity; } } + + public int Count { get { return queue.Count; } } + + public bool Add(TItem item, TDistance distance) + { + if (queue.Count >= maxCapacity) + { + // If the distance of this item is less than the distance of the last item + // in our neighbour list then pop that neighbour off and push this one on + // otherwise don't even bother adding this item + if (distanceMath.Compare(distance, queue.GetHighestPriority()) < 0) + { + queue.Dequeue(); + queue.Enqueue(item, distance); + return true; + } + else + return false; + } + else + { + queue.Enqueue(item, distance); + return true; + } + } + + public TItem GetFurtherest() + { + if (Count == 0) + throw new Exception("List is empty"); + else + return queue.GetHighest(); + } + + public TDistance GetFurtherestDistance() + { + if (Count == 0) + throw new Exception("List is empty"); + else + return queue.GetHighestPriority(); + } + + public TItem RemoveFurtherest() + { + return queue.Dequeue(); + } + + public bool IsCapacityReached + { + get { return Count == MaxCapacity; } + } + } +} diff --git a/Assets/KdTreeLib/NearestNeighbourList.cs.meta b/Assets/KdTreeLib/NearestNeighbourList.cs.meta new file mode 100755 index 0000000..7e9437c --- /dev/null +++ b/Assets/KdTreeLib/NearestNeighbourList.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: b69bc0630cce0dd44aaf33dfe886d91a +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/KdTreeLib/PriorityQueue.cs b/Assets/KdTreeLib/PriorityQueue.cs new file mode 100755 index 0000000..73db395 --- /dev/null +++ b/Assets/KdTreeLib/PriorityQueue.cs @@ -0,0 +1,136 @@ +using System; + +struct ItemPriority +{ + public TItem Item; + public TPriority Priority; +} + +namespace KdTree +{ + public class PriorityQueue : IPriorityQueue + { + public PriorityQueue(int capacity, ITypeMath priorityMath) + { + if (capacity <= 0) + throw new ArgumentException("Capacity must be greater than zero"); + + this.capacity = capacity; + queue = new ItemPriority[capacity]; + + this.priorityMath = priorityMath; + } + + /// + ///This constructor will use a default capacity of 4. + /// + public PriorityQueue(ITypeMath priorityMath) + { + this.capacity = 4; + queue = new ItemPriority[capacity]; + + this.priorityMath = priorityMath; + } + + private ITypeMath priorityMath; + + private ItemPriority[] queue; + + private int capacity; + + private int count; + public int Count { get { return count; } } + + // Try to avoid unnecessary slow memory reallocations by creating your queue with an ample capacity + private void ExpandCapacity() + { + // Double our capacity + capacity *= 2; + + // Create a new queue + var newQueue = new ItemPriority[capacity]; + + // Copy the contents of the original queue to the new one + Array.Copy(queue, newQueue, queue.Length); + + // Copy the new queue over the original one + queue = newQueue; + } + + public void Enqueue(TItem item, TPriority priority) + { + if (++count > capacity) + ExpandCapacity(); + + int newItemIndex = count - 1; + + queue[newItemIndex] = new ItemPriority { Item = item, Priority = priority }; + + ReorderItem(newItemIndex, -1); + } + + public TItem Dequeue() + { + TItem item = queue[0].Item; + + queue[0].Item = default(TItem); + queue[0].Priority = priorityMath.MinValue; + + ReorderItem(0, 1); + + count--; + + return item; + } + + private void ReorderItem(int index, int direction) + { + if ((direction != -1) && (direction != 1)) + throw new ArgumentException("Invalid Direction"); + + var item = queue[index]; + + int nextIndex = index + direction; + + while ((nextIndex >= 0) && (nextIndex < count)) + { + var next = queue[nextIndex]; + + int compare = priorityMath.Compare(item.Priority, next.Priority); + + // If we're moving up and our priority is higher than the next priority then swap + // Or if we're moving down and our priority is lower than the next priority then swap + if ( + ((direction == -1) && (compare > 0)) + || + ((direction == 1) && (compare < 0)) + ) + { + queue[index] = next; + queue[nextIndex] = item; + + index += direction; + nextIndex += direction; + } + else + break; + } + } + + public TItem GetHighest() + { + if (count == 0) + throw new Exception("Queue is empty"); + else + return queue[0].Item; + } + + public TPriority GetHighestPriority() + { + if (count == 0) + throw new Exception("Queue is empty"); + else + return queue[0].Priority; + } + } +} diff --git a/Assets/KdTreeLib/PriorityQueue.cs.meta b/Assets/KdTreeLib/PriorityQueue.cs.meta new file mode 100755 index 0000000..d12d2a4 --- /dev/null +++ b/Assets/KdTreeLib/PriorityQueue.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: bacbb5001e7e7104ba9039eb5de6ec2e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/Pulse.cs b/Assets/Scripts/Pulse.cs index ed9bd1d..c1927f3 100755 --- a/Assets/Scripts/Pulse.cs +++ b/Assets/Scripts/Pulse.cs @@ -1,9 +1,34 @@ -using System.Collections; +using KdTree.Math; +using System.Collections; using System.Collections.Generic; using UnityEngine; public class Pulse : MonoBehaviour { + + private static Vector3[] randomSpherePoints = null; + private static KdTree.KdTree randomSphereKd = new KdTree.KdTree(3, new FloatMath()); + + private static int randomSpherePointsCount = 300; + private static void GenerateRandomPoints() + { + randomSpherePoints = new Vector3[randomSpherePointsCount]; + for (int i = 0; i < randomSpherePointsCount; i++) + { + bool ok = false; + while (!ok) + { + Vector3 r = UnityEngine.Random.insideUnitSphere; + if (r.sqrMagnitude > 0.000001f) + { + randomSpherePoints[i] = r.normalized; + ok = true; + } + } + } + } + + public float Lobes = 10; public float TimeScale = 1; public float Amount = 0.6f;