// ConstructSubgoals constructs subgoals within the specified area and tries to connect them
public void ConstructSubgoals(int startX, int startY, int endX, int endY)
{
// Construct subgoals
for (var x = startX; x < endX; x++)
{
for (var y = startY; y < endY; y++)
{
if (grid.IsWalkable(x, y) == false)
{
continue;
}
for (var d = 4; d < 8; d++)
{
if (grid.IsWalkable(x + directions[d][0], y + directions[d][1]) == false)
{
if (grid.IsWalkable(x + directions[d][0], y) && grid.IsWalkable(x, y + directions[d][1]))
{
subGoals[x * sizeY + y] = new SubGoal(x, y);
}
}
}
}
}
}
public SubGoal Clone()
{
var cloned = new SubGoal(x, y);
cloned.edges = new SubGoalEdge[edges.Length];
Array.Copy(edges, cloned.edges, edges.Length);
return(cloned);
}
public static float Diagonal(SubGoal a, SubGoal b)
{
var dx = Math.Abs(a.x - b.x);
var dy = Math.Abs(a.y - b.y);
return(AStarSearch.LateralCost * (float)(dx + dy) + (AStarSearch.DiagonalCost - 2f * AStarSearch.LateralCost) * (float)Math.Min(dx, dy));
//return Mathf.Sqrt((a.x - b.x) * (a.x - b.x) + (a.y - b.y) * (a.y - b.y));
}
public bool IsDirectHReachable(int x, int y, SubGoal subGoal)
{
// Get cardinal reachable
for (int i = 0; i < 8; i++)
{
var clearance = ClearanceWithSubgoal(grid, x, y, directions[i][0], directions[i][1], subGoal.x, subGoal.y);
var subgoal = new Position(x + clearance * directions[i][0], y + clearance * directions[i][1]);
if (subGoals.TryGetValue(subgoal.x * sizeY + subgoal.y, out SubGoal subGoalRef) && subGoalRef == subGoal)
{
return(true);
}
}
// Get diagonal reachable
for (int d = 4; d < 8; d++)
{
for (int c = 0; c <= 1; c++)
{
var cx = c == 0 ? directions[d][0] : 0;
var cy = c == 0 ? 0 : directions[d][1];
var max = Clearance(x, y, cx, cy);
var diag = Clearance(x, y, directions[d][0], directions[d][1]);
if (IsSubGoal(x + max * cx, y + max * cy))
{
max--;
}
if (IsSubGoal(x + diag * directions[d][0], y + diag * directions[d][1]))
{
diag--;
}
for (int i = 1; i < diag; i++)
{
var newPosX = x + i * directions[d][0];
var newPosY = y + i * directions[d][1];
var j = Clearance(newPosX, newPosY, cx, cy);
if (j <= max && subGoals.TryGetValue((newPosX + j * cx) * sizeY + (newPosY + j * cy), out SubGoal subGoalRef) && subGoalRef == subGoal)
{
return(true);
}
if (j < max)
{
max = j;
}
}
}
}
return(false);
}
