/// <summary>
        /// Return the KDTreeNode that 'vertex' belongs to according to the split axis.
        /// </summary>
        /// <param name="vertex">3D vertex to bin</param>
        /// <returns>KDTreeNode containing 'vertex'</returns>
        public KDNodeJeremyC GetSplitNode(VertexKDTree vertex)
        {
            if (ChildLeft == null || ChildRight == null)
            {
                ChildLeft = new KDNodeJeremyC {
                    Parent = this
                };
                ChildRight = new KDNodeJeremyC {
                    Parent = this
                };

                ChildLeft.Sibling  = ChildRight;
                ChildRight.Sibling = ChildLeft;
            }


            float mid_value = Leaf.Vector[(int)SplitAxis];
            float pt_value  = vertex.Vector[(int)SplitAxis];

            //float diff = pt_value - mid_value;
            //float diff2 = Math.Abs(diff / pt_value);

            //if (diff > 0)
            //    return ChildLeft;
            //else if (diff < 0 && diff2 < 1e-7)
            //    return ChildLeft;
            //else
            //    return ChildRight;
            KDNodeJeremyC child = (pt_value >= mid_value) ? ChildLeft : ChildRight;

            return(child);
        }
Exemple #2
0
        /// <summary>
        /// Recursively split node across its pivot axis.
        /// </summary>
        /// <param name="node">KDTreeNode to split</param>
        /// <param name="depth">Current recursion depth, set lower if you get stack overflow</param>
        /// <returns>True if split was a success or max_depth/max_node_size criterion met</returns>
        bool BuildNode(KDNodeJeremyC node, int depth)
        {
            if (depth >= MaxNodeDepth)
            {
                return(true);
            }
            if (node.Indices.Count <= MaxNodeSize)
            {
                return(true);
            }

            foreach (var index in node.Indices)
            {
                VertexKDTree vertex = TreeVectors[index];

                if (!node.IsBuilt)
                {
                    node.Build();
                }
                KDNodeJeremyC child = node.GetSplitNode(vertex);
                child.AddVertex(index, vertex);
            }

            // TODO:  Do we need to check if either child is empty?  Since we're calculating the split axis
            //		  using raw data it's unlikely.
            node.Clear();
            node.ChildLeft.Build();
            node.ChildRight.Build();

            BuildNode(node.ChildLeft, depth + 1);
            BuildNode(node.ChildRight, depth + 1);
            return(true);
        }
Exemple #3
0
        ///// <summary>
        ///// Add a new vertex/point to the KDTree build process.  Points must be added before calling KDTree.Build()
        ///// </summary>
        ///// <param name="vertex">3D vertex</param>
        //public void AddPoint(VectorWithIndex vertex)
        //{
        //    Vectors.Add(vertex);
        //}

        ///// <summary>
        ///// Add a new vertex/point to the KDTree build process.  Points must be added before calling KDTree.Build()
        ///// </summary>
        ///// <param name="x">VectorWithIndex.x</param>
        ///// <param name="y">VectorWithIndex.y</param>
        ///// <param name="z">VectorWithIndex.z</param>
        //public void AddPoint(float x, float y, float z)
        //{
        //    Vectors.Add(new VectorWithIndex(new Vector3(x,y,z), -1));
        //}

        /// <summary>
        /// Add a new list of vertices/points to the KDTree build process.  Points must be added before calling KDTree.Build()
        /// </summary>
        /// <param name="vertex">List of 3D vertices</param>
        //public void AddPoints(List<VectorWithIndex> vertices)
        //{
        //    Vectors = Vectors.Concat(vertices).ToList();
        //}

        ///// <summary>
        ///// Add a new list of vertices/points to the KDTree build process.  Points must be added before calling KDTree.Build()
        ///// </summary>
        ///// <param name="vertex">List of 3D vertices stored as a 1D array w/ every 3 elements defining a vertex.</param>
        //public void AddPoints(List<float> vertices)
        //{
        //    int element_size = 3;
        //    if (vertices.Count % element_size != 0) return;	//vertex count must be a multiple of element_size
        //    for(int i = 0; i < vertices.Count; i+=element_size){
        //        float x = vertices[i];
        //        float y = vertices[i+1];
        //        float z = vertices[i+2];
        //        Vectors.Add(new VectorWithIndex(new Vector3(x, y, z), -1));
        //    }

        //}
        /// <summary>
        /// Build KDTree
        /// </summary>
        /// <notes>
        /// Points are added via KDTree.AddPoint(s) methods, the Build process uses
        /// those points to create the final tree.  If new points are added after a
        /// tree has been built, a new tree must be created.
        /// </notes>
        /// <returns></returns>
        public bool Build(PointCloud pcTarget)
        {
            this.targetTreee = pcTarget;


            this.treePointCloud = pcTarget;
            this.TreeVectors    = treePointCloud.VectorsWithIndex;


            if (VertexCount <= 0)
            {
                Console.WriteLine("[Warning] - No vertices added to KDTree, aborting build.");
                return(false);
            }
            Root = new KDNodeJeremyC();            // { SplitAxis = largest_split_axis, AABB = new BoundingBoxAxisAligned(min,max), MidPoint = center, Parent = null};



            {                   // Fill the Root
                int index = 0;
                foreach (var vertex in TreeVectors)
                {
                    root.AddVertex(index, vertex);
                    index++;
                }
            }

            BuildNode(Root, 0);
            return(true);
        }
Exemple #4
0
        /// <summary>
        /// Find closest point using an axis aligned search boundary
        /// </summary>
        /// <param name="node"></param>
        /// <param name="point"></param>
        /// <returns></returns>
        VertexKDTree FindClosestPoint_Recursive(KDNodeJeremyC node, VertexKDTree vertex, BoundingBoxAxisAligned search_bounds, ref int nearest_index)
        {
            int tmp_index = -1;

            if (node.IsLeaf)
            {
                tmp_index = -1;
                VertexKDTree result = GetClosestVertexFromIndices(node.Indices, vertex, ref tmp_index);
                if (result != null)
                {
                    nearest_index = tmp_index;
                    return(result);
                }
                else
                {
                    return(null);
                }
            }

            tmp_index = -1;
            KDNodeJeremyC near_child = node.GetSplitNode(vertex);

            VertexKDTree retV = FindClosestPoint_Recursive(near_child, vertex, search_bounds, ref tmp_index);
            //Edgar - TakenVector implementation - have to go one level up if vector is taken
            float near_distance = float.MaxValue;

            if (retV != null)
            {
                //near_distance = retV.Vector.Distance(vertex.Vector);
                near_distance = retV.Vector.DistanceSquared(vertex.Vector);
                nearest_index = tmp_index;
            }

            KDNodeJeremyC far_child = near_child.Sibling;

            if (search_bounds != null && far_child.BoundingBox.Intersects(search_bounds))
            {
                VertexKDTree far_result = FindClosestPoint_Recursive(far_child, vertex, search_bounds, ref tmp_index);
                //Edgar - TakenVector implementation - have to go one level up if vector is taken
                if (far_result != null)
                {
                    //float far_distance = far_result.Vector.Distance(vertex.Vector);
                    float far_distance = far_result.Vector.DistanceSquared(vertex.Vector);
                    if (far_distance < near_distance)
                    {
                        nearest_index = tmp_index;
                        retV          = far_result;
                    }
                }
            }

            if (retV == null)
            {
            }
            return(retV);
        }
Exemple #5
0
        /// <summary>
        /// Find closest point using a centered bounding sphere
        /// </summary>
        /// <param name="node"></param>
        /// <param name="vertex"></param>
        /// <param name="search_bounds"></param>
        /// <param name="nearest_index"></param>
        /// <returns></returns>
        VertexKDTree FindClosestPoint(KDNodeJeremyC node, VertexKDTree vertex, BoundingSphere search_bounds, ref int nearest_index)
        {
            int tmp_index = -1;

            if (node.IsLeaf)
            {
                tmp_index = -1;
                VertexKDTree result = GetClosestVertexFromIndices(node.Indices, vertex, ref tmp_index);
                nearest_index = tmp_index;
                return(result);
            }

            tmp_index = -1;
            KDNodeJeremyC near_child  = node.GetSplitNode(vertex);
            VertexKDTree  near_result = FindClosestPoint(near_child, vertex, search_bounds, ref tmp_index);
            VertexKDTree  ret         = near_result;
            //float near_distance = near_result.Vector.Distance(vertex.Vector);
            float near_distance = near_result.Vector.DistanceSquared(vertex.Vector);

            nearest_index = tmp_index;

            KDNodeJeremyC far_child = near_child.Sibling;

            if (far_child.BoundingBox.Intersects(search_bounds))
            {
                VertexKDTree far_result   = FindClosestPoint(far_child, vertex, search_bounds, ref tmp_index);
                float        far_distance = far_result.Vector.DistanceSquared(vertex.Vector);
                //float far_distance = far_result.Vector.Distance(vertex.Vector);
                if (far_distance < near_distance)
                {
                    nearest_index = tmp_index;
                    ret           = far_result;
                }
            }
            return(ret);
        }
Exemple #6
0
 /// <summary>
 /// Purge vertex memory and root
 /// </summary>
 public void Clear()
 {
     TreeVectors.Clear();
     root = null;
 }