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;
}