private static bool ComputeAStar(SpatialGraphKDNode sourceNode, SpatialGraphKDNode destNode, ref List<Vector2> path)
        {
            AStarSearch search = new AStarSearch();

            search.SetStartAndGoalStates( sourceNode, destNode );

            while( AStarSearch.SearchState.Searching == search.SearchStep() )
            {

            }

            if( search.State == AStarSearch.SearchState.Succeeded )
            {
                //Get path
                path = new List<Vector2>();
                foreach (SpatialGraphKDNode node in search.Solution())
                {
                    path.Add(node.BBox.Centroid());
                }

                return true;
            }

            return false;
        }
 public void GetSuccessors(AStarSearch search, SpatialGraphKDNode parent)
 {
     for (int i = 0; i < _node.Neighbors.Count; ++i)
     {
         SpatialGraphKDNode successor = _node.Neighbors[i];
         if ((parent != null || parent != successor) && !successor.Blocked && _node.NeighborLOS[i])
             search.AddSuccessor(successor);
     }
 }