Example #1
0
        public void AddSuccessor( SpatialGraphKDNode State )
        {
            Node node = new Node();
            node._node = State;

            _Successors.Add( node );
        }
        public SpatialGraph(float entityWidth, ref BoundingBox2D startBox)
        {
            _entityWidth = entityWidth;
            float maxDimension = MathHelper.Max(startBox.Max.Y - startBox.Min.Y, startBox.Max.X - startBox.Min.X);
            int depth = 0;
            while (maxDimension > _entityWidth)
            {
                maxDimension /= 2.0f;
                depth += 2;
            }
            _depth = depth > 1 ? depth : 1;
            if (_depth > 24)
                _depth = 24;

            uint depthMask = ~(0xFFFFFFFF << _depth);

            _dirMasks[0] = 0x1;
            _dirMasks[1] = 0x2;
            _dirMasks[2] = 0xaaaaaaaa & depthMask;
            _dirMasks[3] = _dirMasks[2] >> 1;

            _root = CreateTree(_depth + 1, ref startBox, null, 0);

            //Get smallest dimension
            _smallestDimensions = startBox.Max - startBox.Min;
            for (int i = 0; i < _depth; i++)
            {
                if (i % 2 != 0)
                    _smallestDimensions.Y *= 0.5f;
                else
                    _smallestDimensions.X *= 0.5f;
            }

            ComputeNeighbors(_root);
            ValidateNeighbors(_root);
        }
        private SpatialGraphKDNode CreateTree(int depth, ref BoundingBox2D bbox, SpatialGraphKDNode parent, int index)
        {
            SpatialGraphKDNode node = new SpatialGraphKDNode(bbox, parent);
            node.Tree = this;
            node.Blocked = false;

            //query physics to see if we're blocked
            node.Blocked = IsBlocked( ref bbox );

            //Calculate my index
            if( parent != null )
                node.Index = index;
            else
                node.Index = 0;

            //Bail out if we reach max depth
            depth--;
            node.Depth = _depth - depth;
            if (depth > 0 && node.Blocked )
            {
                BoundingBox2D LHSbbox, RHSbbox;
                MathUtil.SplitBoundingBox(ref bbox, depth % 2 == 0 ? MathUtil.AABBSplittingAxis.Y : MathUtil.AABBSplittingAxis.X, out LHSbbox, out RHSbbox);
                node.LHC = CreateTree(depth, ref LHSbbox, node, node.Index << 1);
                node.RHC = CreateTree(depth, ref RHSbbox, node, (node.Index << 1) + 1);

                uint iMask = ~(0xFFFFFFFF << depth );
                //If I have children, pad my index
                node.Index = (int)((((uint)node.Index) << depth) | iMask);

                //If all my children are blocked, then destroy my children
                if( IsFullyBlocked(node) )
                {
                    node.LHC = null;
                    node.RHC = null;
                }
            }

            return node;
        }
        private void ComputeNeighbors(SpatialGraphKDNode node)
        {
            if( node.HasChildren )
            {
                ComputeNeighbors(node.LHC);
                ComputeNeighbors(node.RHC);
                return;
            }

            Vector2 checkN = Vector2.UnitY * _smallestDimensions.Y;
            Vector2 checkS = Vector2.UnitY * -_smallestDimensions.Y;
            Vector2 checkE = Vector2.UnitX * _smallestDimensions.X;
            Vector2 checkW = Vector2.UnitX * -_smallestDimensions.X;

            Vector2 centroid = node.BBox.Centroid();

            List<Vector2> gridPoints;
            int xPoints, yPoints;
            node.GetGridPoints(out gridPoints, out xPoints, out yPoints );

            //Check north neighbors
            for( int i = 0; i < xPoints; i++ )
            {
                AddNeighbor( node, gridPoints[GetColumnMajorIndex(i,0,xPoints)] + checkN );
            }

            //Check south neighbors
            for( int i = 0; i < xPoints; i++ )
            {
                AddNeighbor( node, gridPoints[GetColumnMajorIndex(i,yPoints-1,xPoints)] + checkS );
            }

            //Check east neighbors
            for( int i = 0; i < yPoints; i++ )
            {
                AddNeighbor( node, gridPoints[GetColumnMajorIndex(xPoints-1,i,xPoints)] + checkE );
            }

            //Check west neighbors
            for( int i = 0; i < yPoints; i++ )
            {
                AddNeighbor( node, gridPoints[GetColumnMajorIndex(0,i,xPoints)] + checkW );
            }
        }
 private bool CanGoNodeToNode(SpatialGraphKDNode sourceNode, SpatialGraphKDNode destNode)
 {
     Vector2 sourceCenter = sourceNode.BBox.Centroid();
     Vector2 destCenter = destNode.BBox.Centroid();
     return CanGoInternal(ref sourceCenter, ref destCenter, sourceNode, destNode);
 }
        private bool CanGoInternal(ref Vector2 vFrom, ref Vector2 vTo, SpatialGraphKDNode sourceNode, SpatialGraphKDNode destNode)
        {
            //If source is dest, we definitely can go (we're both within bounding box)
            if( sourceNode == destNode )
                return true;

            Vector2 vUseFrom = vFrom;
            Vector2 vUseTo = vTo;

            NudgePointOnPlane( ref sourceNode.BBox, ref vUseFrom );
            NudgePointOnPlane( ref destNode.BBox, ref vUseTo );

            Ray2D ray = Ray2D.CreateRayFromTo( ref vUseFrom, ref vUseTo );

            Dictionary<SpatialGraphKDNode, int> NodeList = new Dictionary<SpatialGraphKDNode,int>();
            SpatialGraphKDNode current = sourceNode;

            while( true )
            {
                //Mark current as visited
                NodeList[current] = 1;
                SpatialGraphKDNode nearestNeighbor = null;
                float fNearestNeighborDistance = float.MaxValue;
                //iterate over currents neighbors to see if they intersect the ray
                for( int i = 0; i < current.Neighbors.Count; ++i )
                {
                    SpatialGraphKDNode neighbor = current.Neighbors[i];

                    //Ignore neighbors we've already visited
                    if( NodeList.ContainsKey(neighbor) )
                        continue;

                    float fDistanceAlongRay;
                    if( neighbor.BBox.Intersects( ref ray, out fDistanceAlongRay ) )
                    {
                        if( fDistanceAlongRay < fNearestNeighborDistance )
                        {
                            fNearestNeighborDistance = fDistanceAlongRay;
                            nearestNeighbor = neighbor;
                        }
                    }
                }

                //If we couldn't find a nearest neighbor, or the neighbor is blocked bail out
                if( nearestNeighbor == null || nearestNeighbor.Blocked )
                    break;

                //If the nearest neighbor is our destination, we found it!
                if( nearestNeighbor == destNode )
                    return true;

                //otherwise, check our neighbor
                current = nearestNeighbor;
            }

            return false;
        }
Example #7
0
 public float GetCost(SpatialGraphKDNode successor)
 {
     return Vector2.Distance(_node.BBox.Centroid(), successor.BBox.Centroid());
 }
 public SpatialGraphKDNode(BoundingBox2D bb, SpatialGraphKDNode parent)
 {
     BBox = bb;
     Parent = parent;
 }
        public SpatialGraphKDNode FindNode(SpatialGraphKDNode node, ref BoundingBox2D bbox)
        {
            if (node == null)
                return null;
            //check if this bbox fits entirely within our node
            if (node.BBox.Contains(ref bbox) == BoundingBox2D.ContainmentType.Within)
            {
                //Check our children
                SpatialGraphKDNode retVal = FindNode(node.LHC, ref bbox);
                if (retVal != null)
                    return retVal;
                retVal = FindNode(node.RHC, ref bbox);
                if (retVal != null)
                    return retVal;

                //otherwise, return ourselves
                return node;
            }

            return null;
        }
Example #10
0
 public bool IsSameState(SpatialGraphKDNode goal)
 {
     return _node == goal;
 }
Example #11
0
 public bool IsGoal(SpatialGraphKDNode goal)
 {
     return IsSameState(goal);
 }
Example #12
0
 public float GoalDistanceEstimate(SpatialGraphKDNode goal)
 {
     return GetCost(goal);
 }
Example #13
0
 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);
     }
 }
        private bool IsFullyBlocked(SpatialGraphKDNode node)
        {
            if (node == null)
                return true;

            return node.Blocked && IsFullyBlocked(node.LHC) && IsFullyBlocked(node.RHC);
        }
        public SpatialGraphKDNode FindNode(SpatialGraphKDNode node, ref Vector2 point)
        {
            if (node == null)
                return null;
            //check if this bbox fits entirely within our node
            if (node.BBox.Contains(ref point))
            {
                //Check our children
                SpatialGraphKDNode retVal = FindNode(node.LHC, ref point);
                if (retVal != null)
                    return retVal;
                retVal = FindNode(node.RHC, ref point);
                if (retVal != null)
                    return retVal;

                //otherwise, return ourselves
                return node;

            }

            return null;
        }
 private void ValidateNeighbors(SpatialGraphKDNode node)
 {
     if (node.HasChildren)
     {
         ValidateNeighbors(node.LHC);
         ValidateNeighbors(node.RHC);
         return;
     }
     //Validate neighbors
     for (int i = 0; i < node.Neighbors.Count; i++)
     {
         //Todo, incorporate entity width
         if (!CanGoNodeToNode(node, node.Neighbors[i]))
         {
             node.NeighborLOS[i] = false;
         }
     }
 }
        private void AddNeighbor(SpatialGraphKDNode node, Vector2 pos)
        {
            SpatialGraphKDNode neighbor = node.Tree.FindNode(ref pos);
            if (neighbor != null)
            {
                //Add unique
                for (int i = 0; i < node.Neighbors.Count; i++)
                {
                    if (node.Neighbors[i] == neighbor)
                        return;
                }

                node.Neighbors.Add(neighbor);
                node.NeighborLOS.Add(true);
            }
        }
        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;
        }
Example #19
0
        public void SetStartAndGoalStates(SpatialGraphKDNode Start, SpatialGraphKDNode Goal)
        {
            _CancelRequest = false;

            _Start = new Node();
            _Goal = new Node();

            _Start._node = Start;
            _Goal._node = Goal;

            _State = SearchState.Searching;

            // Initialise the AStar specific parts of the Start Node
            // The user only needs fill out the state information

            _Start.g = 0;
            _Start.h = _Start.GoalDistanceEstimate( _Goal._node );
            _Start.f = _Start.g + _Start.h;
            _Start.parent = null;

            // Push the start node on the Open list
            _OpenList.Clear();  // This isn't in the original implementation, but it makes sense.  Aren't we starting over?

            InsertIntoOpenList(_Start);

            // Initialise counter for search steps
            _Steps = 0;
        }