Exemple #1
0
        protected int queryIndex = 0;                 // current index at stack

        /// <summary>
        /// Returns initialized node from stack that also acts as a pool
        /// The returned reference to node stays in stack
        /// </summary>
        /// <returns>Reference to pooled node</returns>
        private KDQueryNode PushGetQueue()
        {
            KDQueryNode node = null;

            if (count < queueArray.Length)
            {
                if (queueArray[count] == null)
                {
                    queueArray[count] = node = new KDQueryNode();
                }
                else
                {
                    node = queueArray[count];
                }
            }
            else
            {
                // automatic resize of pool
                Array.Resize(ref queueArray, queueArray.Length * 2);
                node = queueArray[count] = new KDQueryNode();
            }

            count++;

            return(node);
        }
Exemple #2
0
        protected KDQueryNode PopFromHeap()
        {
            KDQueryNode heapNode = minHeap.PopObj();

            queueArray[queryIndex] = heapNode;
            queryIndex++;

            return(heapNode);
        }
Exemple #3
0
        public void Radius <T>(KDTree <T> tree, Vector3 queryPosition, float maxRadius, float minRadius, List <int> resultIndices) where T : Component
        {
            Reset();

            Vector3[] points      = tree.Positions;
            int[]     permutation = tree.Permutation;

            float squaredMaxRadius = maxRadius * maxRadius;
            float squaredMinRadius = minRadius * minRadius;

            var rootNode = tree.RootNode;

            PushToQueue(rootNode, rootNode.bounds.ClosestPoint(queryPosition));

            KDQueryNode queryNode = null;
            KDNode      node      = null;

            // KD search with pruning (don't visit areas which distance is more away than range)
            // Recursion done on Stack
            while (LeftToProcess > 0)
            {
                queryNode = PopFromQueue();
                node      = queryNode.node;

                if (!node.Leaf)
                {
                    int   partitionAxis  = node.partitionAxis;
                    float partitionCoord = node.partitionCoordinate;

                    Vector3 tempClosestPoint = queryNode.tempClosestPoint;

                    if ((tempClosestPoint[partitionAxis] - partitionCoord) < 0)
                    {
                        // we already know we are inside negative bound/node,
                        // so we don't need to test for distance
                        // push to stack for later querying

                        // tempClosestPoint is inside negative side
                        // assign it to negativeChild
                        PushToQueue(node.negativeChild, tempClosestPoint);

                        tempClosestPoint[partitionAxis] = partitionCoord;

                        float sqrDist = Vector3.SqrMagnitude(tempClosestPoint - queryPosition);

                        // testing other side
                        if (node.positiveChild.Count != 0 &&
                            sqrDist <= squaredMaxRadius &&
                            sqrDist >= squaredMinRadius)
                        {
                            PushToQueue(node.positiveChild, tempClosestPoint);
                        }
                    }
                    else
                    {
                        // we already know we are inside positive bound/node,
                        // so we don't need to test for distance
                        // push to stack for later querying

                        // tempClosestPoint is inside positive side
                        // assign it to positiveChild
                        PushToQueue(node.positiveChild, tempClosestPoint);

                        // project the tempClosestPoint to other bound
                        tempClosestPoint[partitionAxis] = partitionCoord;

                        float sqrDist = Vector3.SqrMagnitude(tempClosestPoint - queryPosition);

                        // testing other side
                        if (node.negativeChild.Count != 0 &&
                            sqrDist <= squaredMaxRadius &&
                            sqrDist >= squaredMinRadius)
                        {
                            PushToQueue(node.negativeChild, tempClosestPoint);
                        }
                    }
                }
                else
                {
                    // LEAF
                    for (int i = node.start; i < node.end; i++)
                    {
                        int index = permutation[i];

                        float sqrMagnitude = Vector3.SqrMagnitude(points[index] - queryPosition);
                        if (squaredMinRadius <= sqrMagnitude && sqrMagnitude <= squaredMaxRadius)
                        {
                            resultIndices.Add(index);
                        }
                    }
                }
            }
        }
Exemple #4
0
        public void ClosestPoint <T>(KDTree <T> tree, Vector3 queryPosition, List <int> resultIndices, List <float> resultDistances = null) where T : Component
        {
            Reset();

            Vector3[] points      = tree.Positions;
            int[]     permutation = tree.Permutation;

            int smallestIndex = 0;
            /// Smallest Squared Radius
            float SSR = Single.PositiveInfinity;


            var rootNode = tree.RootNode;

            Vector3 rootClosestPoint = rootNode.bounds.ClosestPoint(queryPosition);

            PushToHeap(rootNode, rootClosestPoint, queryPosition);

            KDQueryNode queryNode = null;
            KDNode      node      = null;

            int   partitionAxis;
            float partitionCoord;

            Vector3 tempClosestPoint;

            // searching
            while (minHeap.Count > 0)
            {
                queryNode = PopFromHeap();

                if (queryNode.distance > SSR)
                {
                    continue;
                }

                node = queryNode.node;

                if (!node.Leaf)
                {
                    partitionAxis  = node.partitionAxis;
                    partitionCoord = node.partitionCoordinate;

                    tempClosestPoint = queryNode.tempClosestPoint;

                    if ((tempClosestPoint[partitionAxis] - partitionCoord) < 0)
                    {
                        // we already know we are on the side of negative bound/node,
                        // so we don't need to test for distance
                        // push to stack for later querying

                        PushToHeap(node.negativeChild, tempClosestPoint, queryPosition);
                        // project the tempClosestPoint to other bound
                        tempClosestPoint[partitionAxis] = partitionCoord;

                        if (node.positiveChild.Count != 0)
                        {
                            PushToHeap(node.positiveChild, tempClosestPoint, queryPosition);
                        }
                    }
                    else
                    {
                        // we already know we are on the side of positive bound/node,
                        // so we don't need to test for distance
                        // push to stack for later querying

                        PushToHeap(node.positiveChild, tempClosestPoint, queryPosition);
                        // project the tempClosestPoint to other bound
                        tempClosestPoint[partitionAxis] = partitionCoord;

                        if (node.positiveChild.Count != 0)
                        {
                            PushToHeap(node.negativeChild, tempClosestPoint, queryPosition);
                        }
                    }
                }
                else
                {
                    float sqrDist;
                    // LEAF
                    for (int i = node.start; i < node.end; i++)
                    {
                        int index = permutation[i];

                        sqrDist = Vector3.SqrMagnitude(points[index] - queryPosition);

                        if (sqrDist <= SSR)
                        {
                            SSR           = sqrDist;
                            smallestIndex = index;
                        }
                    }
                }
            }

            resultIndices.Add(smallestIndex);

            if (resultDistances != null)
            {
                resultDistances.Add(SSR);
            }
        }
Exemple #5
0
        public void Interval(KDTree tree, float2 min, float2 max, List <int> resultIndices)
        {
            Reset();

            float2[] points      = tree.Points;
            int[]    permutation = tree.Permutation;

            var rootNode = tree.RootNode;

            PushToQueue(

                rootNode,
                rootNode.bounds.ClosestPoint((min + max) / 2)
                );

            KDQueryNode queryNode = null;
            KDNode      node      = null;


            // KD search with pruning (don't visit areas which distance is more away than range)
            // Recursion done on Stack
            while (LeftToProcess > 0)
            {
                queryNode = PopFromQueue();
                node      = queryNode.node;

                if (!node.Leaf)
                {
                    int   partitionAxis  = node.partitionAxis;
                    float partitionCoord = node.partitionCoordinate;

                    float2 tempClosestPoint = queryNode.tempClosestPoint;

                    if ((tempClosestPoint[partitionAxis] - partitionCoord) < 0)
                    {
                        // we already know we are inside negative bound/node,
                        // so we don't need to test for distance
                        // push to stack for later querying

                        // tempClosestPoint is inside negative side
                        // assign it to negativeChild
                        PushToQueue(node.negativeChild, tempClosestPoint);

                        tempClosestPoint[partitionAxis] = partitionCoord;

                        // testing other side
                        if (node.positiveChild.Count != 0 &&
                            tempClosestPoint[partitionAxis] <= max[partitionAxis])
                        {
                            PushToQueue(node.positiveChild, tempClosestPoint);
                        }
                    }
                    else
                    {
                        // we already know we are inside positive bound/node,
                        // so we don't need to test for distance
                        // push to stack for later querying

                        // tempClosestPoint is inside positive side
                        // assign it to positiveChild
                        PushToQueue(node.positiveChild, tempClosestPoint);

                        // project the tempClosestPoint to other bound
                        tempClosestPoint[partitionAxis] = partitionCoord;

                        // testing other side
                        if (node.negativeChild.Count != 0 &&
                            tempClosestPoint[partitionAxis] >= min[partitionAxis])
                        {
                            PushToQueue(node.negativeChild, tempClosestPoint);
                        }
                    }
                }
                else
                {
                    // LEAF

                    // testing if node bounds are inside the query interval
                    if (node.bounds.min[0] >= min[0] &&
                        node.bounds.min[1] >= min[1] &&
                        node.bounds.min[2] >= min[2]

                        && node.bounds.max[0] <= max[0] &&
                        node.bounds.max[1] <= max[1] &&
                        node.bounds.max[2] <= max[2])
                    {
                        for (int i = node.start; i < node.end; i++)
                        {
                            resultIndices.Add(permutation[i]);
                        }
                    }
                    // node is not inside query interval, need to do test on each point separately
                    else
                    {
                        for (int i = node.start; i < node.end; i++)
                        {
                            int index = permutation[i];

                            float2 v = points[index];

                            if (v[0] >= min[0] &&
                                v[1] >= min[1] &&
                                v[2] >= min[2]

                                && v[0] <= max[0] &&
                                v[1] <= max[1] &&
                                v[2] <= max[2])
                            {
                                resultIndices.Add(index);
                            }
                        }
                    }
                }
            }
        }
Exemple #6
0
        /// <summary>
        /// Returns indices to k closest points, and optionaly can return distances
        /// </summary>
        /// <param name="tree">Tree to do search on</param>
        /// <param name="queryPosition">Position</param>
        /// <param name="k">Max number of points</param>
        /// <param name="resultIndices">List where resulting indices will be stored</param>
        /// <param name="resultDistances">Optional list where resulting distances will be stored</param>
        public void KNearest(KDTree tree, Vector3 queryPosition, int k, List <int> resultIndices, List <float> resultDistances = null)
        {
            // pooled heap arrays
            KSmallestHeap <int> kHeap;

            _heaps.TryGetValue(k, out kHeap);

            if (kHeap == null)
            {
                kHeap = new KSmallestHeap <int>(k);
                _heaps.Add(k, kHeap);
            }

            kHeap.Clear();
            Reset();

            Vector3[] points      = tree.Points;
            int[]     permutation = tree.Permutation;

            ///Biggest Smallest Squared Radius
            float BSSR = Single.PositiveInfinity;

            var rootNode = tree.RootNode;

            Vector3 rootClosestPoint = rootNode.bounds.ClosestPoint(queryPosition);

            PushToHeap(rootNode, rootClosestPoint, queryPosition);

            KDQueryNode queryNode = null;
            KDNode      node      = null;

            int   partitionAxis;
            float partitionCoord;

            Vector3 tempClosestPoint;

            // searching
            while (minHeap.Count > 0)
            {
                queryNode = PopFromHeap();

                if (queryNode.distance > BSSR)
                {
                    continue;
                }

                node = queryNode.node;

                if (!node.Leaf)
                {
                    partitionAxis  = node.partitionAxis;
                    partitionCoord = node.partitionCoordinate;

                    tempClosestPoint = queryNode.tempClosestPoint;

                    if ((tempClosestPoint[partitionAxis] - partitionCoord) < 0)
                    {
                        // we already know we are on the side of negative bound/node,
                        // so we don't need to test for distance
                        // push to stack for later querying

                        PushToHeap(node.negativeChild, tempClosestPoint, queryPosition);
                        // project the tempClosestPoint to other bound
                        tempClosestPoint[partitionAxis] = partitionCoord;

                        if (node.positiveChild.Count != 0)
                        {
                            PushToHeap(node.positiveChild, tempClosestPoint, queryPosition);
                        }
                    }
                    else
                    {
                        // we already know we are on the side of positive bound/node,
                        // so we don't need to test for distance
                        // push to stack for later querying

                        PushToHeap(node.positiveChild, tempClosestPoint, queryPosition);
                        // project the tempClosestPoint to other bound
                        tempClosestPoint[partitionAxis] = partitionCoord;

                        if (node.positiveChild.Count != 0)
                        {
                            PushToHeap(node.negativeChild, tempClosestPoint, queryPosition);
                        }
                    }
                }
                else
                {
                    float sqrDist;
                    // LEAF
                    for (int i = node.start; i < node.end; i++)
                    {
                        int index = permutation[i];

                        sqrDist = Vector3.SqrMagnitude(points[index] - queryPosition);

                        if (sqrDist <= BSSR)
                        {
                            kHeap.PushObj(index, sqrDist);

                            if (kHeap.Full)
                            {
                                BSSR = kHeap.HeadValue;
                            }
                        }
                    }
                }
            }

            kHeap.FlushResult(resultIndices, resultDistances);
        }
        /// <summary>
        /// Search by radius method.
        /// </summary>
        /// <param name="tree">Tree to do search on</param>
        /// <param name="queryPosition">Position</param>
        /// <param name="queryRadius">Radius</param>
        /// <param name="resultIndices">Initialized list, cleared.</param>
        public IEnumerable <(int index, float sqrDistance)> Radius(KDTree tree, float2 queryPosition, float queryRadius)
        {
            Reset();

            float2[] points      = tree.Points;
            int[]    permutation = tree.Permutation;

            float squaredRadius = queryRadius * queryRadius;

            var rootNode = tree.RootNode;

            PushToQueue(rootNode, rootNode.bounds.ClosestPoint(queryPosition));

            KDQueryNode queryNode = null;
            KDNode      node      = null;

            // KD search with pruning (don't visit areas which distance is more away than range)
            // Recursion done on Stack
            while (LeftToProcess > 0)
            {
                queryNode = PopFromQueue();
                node      = queryNode.node;

                if (!node.Leaf)
                {
                    int   partitionAxis  = node.partitionAxis;
                    float partitionCoord = node.partitionCoordinate;

                    float2 tempClosestPoint = queryNode.tempClosestPoint;

                    if ((tempClosestPoint[partitionAxis] - partitionCoord) < 0)
                    {
                        // we already know we are inside negative bound/node,
                        // so we don't need to test for distance
                        // push to stack for later querying

                        // tempClosestPoint is inside negative side
                        // assign it to negativeChild
                        PushToQueue(node.negativeChild, tempClosestPoint);

                        tempClosestPoint[partitionAxis] = partitionCoord;

                        float sqrDist = lengthsq(tempClosestPoint - queryPosition);

                        // testing other side
                        if (node.positiveChild.Count != 0 &&
                            sqrDist <= squaredRadius)
                        {
                            PushToQueue(node.positiveChild, tempClosestPoint);
                        }
                    }
                    else
                    {
                        // we already know we are inside positive bound/node,
                        // so we don't need to test for distance
                        // push to stack for later querying

                        // tempClosestPoint is inside positive side
                        // assign it to positiveChild
                        PushToQueue(node.positiveChild, tempClosestPoint);

                        // project the tempClosestPoint to other bound
                        tempClosestPoint[partitionAxis] = partitionCoord;

                        float sqrDist = lengthsq(tempClosestPoint - queryPosition);

                        // testing other side
                        if (node.negativeChild.Count != 0 &&
                            sqrDist <= squaredRadius)
                        {
                            PushToQueue(node.negativeChild, tempClosestPoint);
                        }
                    }
                }
                else
                {
                    // LEAF
                    for (int i = node.start; i < node.end; i++)
                    {
                        int index = permutation[i];

                        var lengthSq = lengthsq(points[index] - queryPosition);
                        if (lengthSq <= squaredRadius)
                        {
                            yield return(index, lengthSq);
                        }
                    }
                }
            }
        }