示例#1
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);
        }
示例#2
0
        public void Interval(KDTree tree, Vector3 min, Vector3 max, List <int> resultIndices)
        {
            Reset();

            Vector3[] 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;

                    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;

                        // 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];

                            Vector3 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);
                            }
                        }
                    }
                }
            }
        }
示例#3
0
        public void ClosestPoint(KDTree tree, float2 queryPosition, List <int> resultIndices, List <float> resultDistances = null)
        {
            Reset();

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

            if (points.Length == 0)
            {
                return;
            }

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


            var rootNode = tree.RootNode;

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

            PushToHeap(rootNode, rootClosestPoint, queryPosition);

            KDQueryNode queryNode = null;
            KDNode      node      = null;

            int   partitionAxis;
            float partitionCoord;

            float2 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 = lengthsq(points[index] - queryPosition);

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

            resultIndices.Add(smallestIndex);

            if (resultDistances != null)
            {
                resultDistances.Add(SSR);
            }
        }
示例#4
0
        /// <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 void Radius(KDTree tree, Vector3 queryPosition, float queryRadius, List <int> resultIndices)
        {
            Reset();

            Vector3[] 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;

                    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 <= 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 = Vector3.SqrMagnitude(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];

                        if (Vector3.SqrMagnitude(points[index] - queryPosition) <= squaredRadius)
                        {
                            resultIndices.Add(index);
                        }
                    }
                }
            }
        }
示例#5
0
        // returns a 3-tuple of:
        // - hasValue (did find anything)
        // - index of the closest point (undefined if not found)
        // - distance to that point (undefined if not found)
        public (bool, int, float) ClosestPoint(KDTree tree, Vector3 queryPosition)
        {
            Reset();

            if (points.Count == 0)
            {
                return(false, -1, -1f);
            }

            Vector3[] points      = tree.Points;
            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;
                        }
                    }
                }
            }

            return(true, smallestIndex, SSR);
        }