82 lines
1.9 KiB
C#
82 lines
1.9 KiB
C#
using System;
|
|
using System.Collections.Generic;
|
|
using UnityEngine;
|
|
|
|
namespace Mtree
|
|
{
|
|
public class KDTree
|
|
{
|
|
private KDTreeNode root;
|
|
|
|
public KDTree(Vector3[] points, Vector3 t, string TreeName)
|
|
{
|
|
if (points.Length == 0)
|
|
{
|
|
points = new Vector3[1] { t };
|
|
}
|
|
root = new KDTreeNode(points[0]);
|
|
for (int i = 1; i < points.Length; i++)
|
|
{
|
|
AddPoint(points[i]);
|
|
}
|
|
}
|
|
|
|
public KDTree()
|
|
{
|
|
root = new KDTreeNode(Vector3.zero);
|
|
}
|
|
|
|
public void AddPoint(Vector3 point)
|
|
{
|
|
root.AddPoint(point, 0);
|
|
}
|
|
|
|
private KDTreeNode BuildFromPointsRec(Vector3[] points, int axis, int start, int end)
|
|
{
|
|
Debug.Log(start + " " + end);
|
|
if (start == end - 1)
|
|
{
|
|
Debug.Log(start);
|
|
return new KDTreeNode(points[start]);
|
|
}
|
|
int num = UnityEngine.Random.Range(start, end - 1);
|
|
KDTreeNode kDTreeNode = new KDTreeNode(points[num]);
|
|
axis = (axis + 1) % 3;
|
|
kDTreeNode.left = BuildFromPointsRec(points, axis, start, num);
|
|
kDTreeNode.right = BuildFromPointsRec(points, axis, num, end);
|
|
return kDTreeNode;
|
|
}
|
|
|
|
public Vector3 FindClosest(Vector3 point)
|
|
{
|
|
float currBestDist = float.PositiveInfinity;
|
|
Vector3 currBest = root.position;
|
|
root.FindClosestRec(point, ref currBestDist, ref currBest, 0);
|
|
return currBest;
|
|
}
|
|
|
|
public List<Vector3> RadiusSearch(Vector3 center, float radius)
|
|
{
|
|
List<Vector3> neighbours = new List<Vector3>();
|
|
root.RadiusSearchRec(center, radius * radius, ref neighbours, 0);
|
|
return neighbours;
|
|
}
|
|
|
|
private int SelectMedian(Vector3[] points, int axis, int start, int end)
|
|
{
|
|
float[] array = new float[end - start];
|
|
for (int i = start; i < end; i++)
|
|
{
|
|
array[i - start] = points[i][axis];
|
|
}
|
|
int[] array2 = new int[array.Length];
|
|
for (int j = 0; j < array2.Length; j++)
|
|
{
|
|
array2[j] = j;
|
|
}
|
|
Array.Sort(array, array2);
|
|
return array2[array2.Length / 2];
|
|
}
|
|
}
|
|
}
|