99 lines
2.2 KiB
C#
99 lines
2.2 KiB
C#
using System.Collections.Generic;
|
|
using UnityEngine;
|
|
|
|
namespace Mtree
|
|
{
|
|
public class KDTreeNode
|
|
{
|
|
public Vector3 position;
|
|
|
|
public KDTreeNode left;
|
|
|
|
public KDTreeNode right;
|
|
|
|
public KDTreeNode(Vector3 position)
|
|
{
|
|
this.position = position;
|
|
}
|
|
|
|
public void FindClosestRec(Vector3 point, ref float currBestDist, ref Vector3 currBest, int axis)
|
|
{
|
|
float sqrMagnitude = (point - position).sqrMagnitude;
|
|
if (sqrMagnitude < currBestDist)
|
|
{
|
|
currBestDist = sqrMagnitude;
|
|
currBest = position;
|
|
}
|
|
float num = point[axis] - position[axis];
|
|
bool flag = num <= 0f;
|
|
axis = (axis + 1) % 3;
|
|
if (flag)
|
|
{
|
|
if (left != null)
|
|
{
|
|
left.FindClosestRec(point, ref currBestDist, ref currBest, axis);
|
|
}
|
|
if (num * num < currBestDist && right != null)
|
|
{
|
|
right.FindClosestRec(point, ref currBestDist, ref currBest, axis);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if (right != null)
|
|
{
|
|
right.FindClosestRec(point, ref currBestDist, ref currBest, axis);
|
|
}
|
|
if (num * num < currBestDist && left != null)
|
|
{
|
|
left.FindClosestRec(point, ref currBestDist, ref currBest, axis);
|
|
}
|
|
}
|
|
}
|
|
|
|
public void RadiusSearchRec(Vector3 center, float radiusSquared, ref List<Vector3> neighbours, int axis)
|
|
{
|
|
float num = Vector3.SqrMagnitude(center - position);
|
|
if (num < radiusSquared)
|
|
{
|
|
neighbours.Add(position);
|
|
}
|
|
float num2 = position[axis] - center[axis];
|
|
axis = (axis + 1) % 3;
|
|
bool flag = num2 * num2 * Mathf.Sign(num2) > (0f - radiusSquared) / 4f;
|
|
bool flag2 = num2 * num2 * Mathf.Sign(num2) < radiusSquared / 4f;
|
|
if (flag && left != null)
|
|
{
|
|
left.RadiusSearchRec(center, radiusSquared, ref neighbours, axis);
|
|
}
|
|
if (flag2 && right != null)
|
|
{
|
|
right.RadiusSearchRec(center, radiusSquared, ref neighbours, axis);
|
|
}
|
|
}
|
|
|
|
public void AddPoint(Vector3 point, int axis)
|
|
{
|
|
bool flag = position[axis] > point[axis];
|
|
KDTreeNode kDTreeNode = ((!flag) ? right : left);
|
|
axis = (axis + 1) % 3;
|
|
if (kDTreeNode == null)
|
|
{
|
|
kDTreeNode = new KDTreeNode(point);
|
|
if (flag)
|
|
{
|
|
left = kDTreeNode;
|
|
}
|
|
else
|
|
{
|
|
right = kDTreeNode;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
kDTreeNode.AddPoint(point, axis);
|
|
}
|
|
}
|
|
}
|
|
}
|