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; }
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; }
public bool IsSameState(SpatialGraphKDNode goal) { return _node == goal; }
public bool IsGoal(SpatialGraphKDNode goal) { return IsSameState(goal); }
public float GoalDistanceEstimate(SpatialGraphKDNode goal) { return GetCost(goal); }
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; }
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; }