public int Calc_H(World w, GridSearchNode gridNode)
{
var rsdGridNode = gridNode as RsdGridSearchNode;
rsdGridNode.Reachable = new BitArray(w.LinearSize);
Queue <Location> open = new Queue <Location>();
HashSet <Location> closed = new HashSet <Location>();
open.Enqueue(w.Goal);
bool goalReachableFromHead = false;
int g = 0;
while (open.Count > 0)
{
var current = open.Dequeue();
if (closed.Contains(current) || current.X < 0 || current.X >= w.Width || current.Y < 0 || current.Y >= w.Height)
{ //Already seen or Out of the frame
continue;
}
closed.Add(current);
if (current.Equals(rsdGridNode.HeadLocation))
{
goalReachableFromHead = true;
}
if (!w.IsBlocked(current) && !rsdGridNode.IsVisited(current))
{
open.Enqueue(current.GetMovedLocation(MoveDirection.Up));
open.Enqueue(current.GetMovedLocation(MoveDirection.Down));
open.Enqueue(current.GetMovedLocation(MoveDirection.Left));
open.Enqueue(current.GetMovedLocation(MoveDirection.Right));
rsdGridNode.Reachable[current.Y * w.Width + current.X] = true;
g++;
}
}
return(goalReachableFromHead ? g:0);
}
public RsdGridSearchNode(GridSearchNode parentNode, MoveDirection move) : base(parentNode, move)
{
}
private int GetHashValue(GridSearchNode node)
{
return(ComputeFnvHash(BitArrayToByteArray(node.Visited)));
}
private int GetLinearHeadLocation(GridSearchNode node)
{
return(node.HeadLocation.Y * node.World.Width + node.HeadLocation.X);
}
public int Calc_H(World w, GridSearchNode gridNode)
{
var bcc = new BiconnectedComponents(w, gridNode);
if (!bcc.LinearLocationWasVisitedDuringBuild(w.Goal))
{
if (gridNode is RsdGridSearchNode)
{
((RsdGridSearchNode)gridNode).Reachable = new BitArray(w.LinearSize);
}
return(0); //Goal not reachable
}
var bct = bcc.GetMaxPathBlockCutTree(gridNode.HeadLocation, w.Goal);
//instead of g we will count odd and even separatly
int h = 0;
for (int i = 0; i + 2 < bct.Count; i += 2)
{
int head = bct.ElementAt(i)[0];
var firstStepEven = !IsEvenLocation(w, head);
var goalEven = IsEvenLocation(w, bct.ElementAt(i + 2)[0]);
int odd = 0;
int even = 0;
var currBlk = bct.ElementAt(i + 1);
for (int j = 0; j < currBlk.Length; j++)
{
if (currBlk[j] == head)
{
continue;
}
if (IsEvenLocation(w, currBlk[j]))
{
even++;
}
else
{
odd++;
}
}
h += AlternateStepsHeuristic.CalculateAlternateStepHeuristic(firstStepEven, goalEven, odd, even);
}
if (gridNode is RsdGridSearchNode)
{
((RsdGridSearchNode)gridNode).Reachable = new BitArray(w.LinearSize);
for (int i = 0; i + 1 < bct.Count; i += 2)
{
var currBlk = bct.ElementAt(i + 1);
for (int j = 0; j < currBlk.Length; j++)
{
((RsdGridSearchNode)gridNode).Reachable[currBlk[j]] = true;
}
}
}
return(h);
}
public int CalculateHeuristic(GridSearchNode node)
{
return(HeuristicFunction.Calc_H(this, node));
}
