public static float distance(TileIndex tile1, TileIndex tile2)
{
return (float)Math.Sqrt(Math.Pow(tile1.x_ - tile2.x_, 2) + Math.Pow(tile1.y_ - tile2.y_, 2));
}
private static TileIndex getNearest(TileGrid grid, TileIndex start, TileIndex destination, float radius, Height height)
{
radius = radius / (TileGrid.TILEHEIGHT + TileGrid.TILEWIDTH) / 2;
Tile dest = grid.getTile(destination);
if (!dest.collides(height, radius))
{
return destination;
}
short manhattan = 1;
// Expand outward by increasing the manhattan distance
// Until finding some subset of tiles which we can walk on
// TODO
// Don't use heap space here
List<TileIndex> closest = new List<TileIndex>();
while (closest.Count == 0)
{
Tile cur;
int newX;
int newY;
for (short i = 0; i < manhattan - 1; i++)
{
// Quadrant I
newX = destination.x_ + i;
newY = destination.y_ + (manhattan - i);
cur = grid.getTile(newX, newY);
if (!cur.collides(height, radius))
{
closest.Add(new TileIndex((short)newX, (short)newY));
}
// Quadrant II
newX = destination.x_ - i;
newY = destination.y_ + (manhattan - i);
cur = grid.getTile(newX, newY);
if (!cur.collides(height, radius))
{
closest.Add(new TileIndex((short)newX, (short)newY));
}
// Quadrant III
newX = destination.x_ - i;
newY = destination.y_ - (manhattan - i);
cur = grid.getTile(newX, newY);
if (!cur.collides(height, radius))
{
closest.Add(new TileIndex((short)newX, (short)newY));
}
// Quadrant IV
newX = destination.x_ + i;
newY = destination.y_ - (manhattan - i);
cur = grid.getTile(newX, newY);
if (!cur.collides(height, radius))
{
closest.Add(new TileIndex((short)newX, (short)newY));
}
}
manhattan++;
if (manhattan > 10)
{
return new TileIndex(-1, -1);
}
}
// We've found walkable tiles, and they are all the same manhattan
// distance from the destination, so pick closest to the start
// TODO
// Determine whether it's better to pick closest real distance to
// destination first
TileIndex best = new TileIndex(-1, -1);
float bestScore = float.MaxValue;
for (int i = 0; i < closest.Count; i++)
{
float score = CommonFunctions.distance(closest[i], start);
if (score < bestScore)
{
bestScore = score;
best = closest[i];
}
}
return best;
}
internal ActionTakeCover(NonPlayableCharacterAbstract character, ref TileIndex coverLocation)
: base(character)
{
coverLocation_ = coverLocation;
}
/// <summary>
/// Add a particular node in the search space to the open list if
/// it qualifies.
/// </summary>
/// <param name="x">X Index of the node to be added</param>
/// <param name="y">Y Index of the node to be added</param>
/// <param name="g">Cost to reach this node</param>
/// <param name="parent">Node expanded to reach this node</param>
protected static void mark(short x, short y, float g, ref TileIndex parent)
{
// Verify that we are not outside the search space
if (x < 0 || x >= SEARCH_SPACE_WIDTH || y < 0 || y >= SEARCH_SPACE_HEIGHT)
{
return;
}
// Verify that we can walk on this particular tile
if (collision(getTile(x,y)))
{
return;
}
// If we haven't been here yet or our cost is better, update it
// and add to the openlist
if (!touched_[x, y] || (searchSpace_[x, y].open && g < searchSpace_[x, y].g))
{
searchSpace_[x, y].parent = parent;
searchSpace_[x, y].g = g;
searchSpace_[x, y].f = g + heuristicDistance(x, y);
// Hasn't been found before, so add it to openlist
if (!touched_[x, y])
{
touched_[x, y] = true;
searchSpace_[x, y].open = true;
TileIndex tile = new TileIndex(x, y); // stack
openlist_.Add(tile);
}
}
}
protected static void setupSearch(TileGrid grid, TileIndex start, TileIndex destination, float radius, Height height)
{
// Initialize the static structures used by the search
reset();
start_ = start;
goal_ = destination;
// Project our start and goal nodes into the search space, if possible
// TODO
// For now, if they are farther apart than the size of the search space in
// any dimension, we give up - in the future, they will try to get as close
// as possible
short manhattanX = (short)Math.Abs(start_.x_ - goal_.x_);
short manhattanY = (short)Math.Abs(start_.y_ - goal_.y_);
short leftOffset = (short)Math.Floor((SEARCH_SPACE_WIDTH - manhattanX) / 2.0f);
short rightOffset = (short)Math.Ceiling((SEARCH_SPACE_WIDTH - manhattanX) / 2.0f);
short bottomOffset = (short)Math.Floor((SEARCH_SPACE_HEIGHT - manhattanY) / 2.0f);
short topOffset = (short)Math.Ceiling((SEARCH_SPACE_HEIGHT - manhattanY) / 2.0f);
gridXOffset_ = (short)(Math.Min(start_.x_, goal_.x_) - leftOffset);
gridYOffset_ = (short)(Math.Min(start_.y_, goal_.y_) - topOffset);
if (gridXOffset_ < 0) gridXOffset_ = 0;
if (gridYOffset_ < 0) gridYOffset_ = 0;
start_.x_ -= gridXOffset_;
start_.y_ -= gridYOffset_;
goal_.x_ -= gridXOffset_;
goal_.y_ -= gridYOffset_;
searchRadius_ =
(float)(radius / ((TileGrid.TILEWIDTH + TileGrid.TILEHEIGHT) / 2.0f));
grid_ = grid;
searchHeight_ = height;
}
/// <summary>
/// Scan the openlist for the lowest f-value, which is the next node to
/// expand, remove it from the openlist, and return it.
/// </summary>
/// <returns>The next node for A* to expand</returns>
protected static TileIndex getNext()
{
short bestX = -1;
short bestY = -1;
int bestIndex = -1;
float bestVal = float.MaxValue;
for (int i = 0; i < openlist_.Count; i++)
{
short curX = openlist_[i].x_;
short curY = openlist_[i].y_;
if (searchSpace_[curX, curY].f < bestVal && searchSpace_[curX, curY].open)
{
bestVal = searchSpace_[curX, curY].f;
bestX = curX;
bestY = curY;
bestIndex = i;
}
}
TileIndex best = new TileIndex(bestX, bestY); // stack
openlist_.RemoveAt(bestIndex);
return best;
}
/*
/// <summary>
/// Possibly unnecessary function, try without it
/// </summary>
protected static void init()
{
// testing
reset();
for (int i = 0; i < 60; i++)
{
for (int j = 0; j < 60; j++)
{
searchSpace[i, j].open = false;
}
}
}
*/
protected static Tile getTile(TileIndex index)
{
return grid_.getTile(index.x_ + gridXOffset_, index.y_ + gridYOffset_);
}
public static List<TileIndex> calculateNearbyPath(TileGrid grid, TileIndex start, TileIndex destination, float radius, Height height)
{
TileIndex newdest = getNearest(grid, start, destination, radius, height);
if (newdest.x_ == -1 || newdest.y_ == -1)
{
//throw new Exception("AStarPathfinder failed to find a nearest tile!");
return null;
}
return calculateExactPath(grid, start, newdest, radius, height);
}
/// <summary>
/// Add legal neighbor nodes to the open list.
/// </summary>
/// <param name="cur">Current position in the grid</param>
/// <param name="cost">Cost to reach this position</param>
protected static void expand(ref TileIndex cur, float cost)
{
short x = cur.x_;
short y = cur.y_;
// Mark nodes in 8 directions around the current position
mark((short)(x - 1), (short)(y - 1), cost + 1.41f, ref cur);
mark((short)(x - 1), y, cost + 1.0f, ref cur);
mark((short)(x - 1), (short)(y + 1), cost + 1.41f, ref cur);
mark(x, (short)(y - 1), cost + 1.0f, ref cur);
mark(x, (short)(y + 1), cost + 1.0f, ref cur);
mark((short)(x + 1), (short)(y - 1), cost + 1.41f, ref cur);
mark((short)(x + 1), y, cost + 1.0f, ref cur);
mark((short)(x + 1), (short)(y + 1), cost + 1.41f, ref cur);
}
internal ActionPickupAmmo(NonPlayableCharacterAbstract character, ref TileIndex ammoLocation, Object tempHandle)
: base(character)
{
ammoLocation_ = ammoLocation;
tempHandle_ = tempHandle;
}
/// <summary>
/// Run the A* algorithm to get a navigation path.
/// </summary>
/// <param name="grid">The area being searched</param>
/// <param name="start">Index of the start tile.</param>
/// <param name="destination">Index of the destination tile.</param>
/// <param name="radius">Radius of the character performing the search</param>
/// <param name="tileHeight">Height of the character performing the search</param>
/// <returns>Waypoints which should allow the character to reach the goal.</returns>
public static List<TileIndex> calculateExactPath(TileGrid grid, TileIndex start, TileIndex destination, float radius, Height height)
{
#if DEBUG
clock.Start();
#endif
setupSearch(grid, start, destination, radius, height);
//Console.WriteLine("Pathfind Start: " + start_.x_ + "," + start_.y_);
//Console.WriteLine("Pathfind Goal: " + goal_.x_ + "," + goal_.y_);
TileIndex cur = start_;
searchSpace_[cur.x_, cur.y_].g = 0;
searchSpace_[cur.x_, cur.y_].f = heuristicDistance(cur.x_, cur.y_);
searchSpace_[cur.x_, cur.y_].parent = new TileIndex(-1, -1); // stack
touched_[cur.x_, cur.y_] = true;
// As long as we aren't at the goal, keep expanding outward and checking
// the next most promising node
while (!TileIndex.equals(goal_, cur))
{
searchSpace_[cur.x_, cur.y_].open = false;
expand(ref cur, searchSpace_[cur.x_, cur.y_].g);
// If we are out of nodes to check, no path exists
if (openlist_.Count == 0)
{
#if DEBUG
clock.Stop();
#endif
return null;
}
else
{
cur = getNext();
}
}
#if DEBUG
clock.Stop();
#endif
return recreatePath();
}
public bool isPointWithinTile(Vector2 position, TileIndex tile)
{
TileIndex actual = getTileIndex(position);
return TileIndex.equals(actual, tile);
}
public Vector2 getTileCenter(TileIndex tile)
{
float x = tile.x_ * TILEWIDTH + TILEWIDTH / 2;
float y = tile.y_ * TILEHEIGHT + TILEHEIGHT / 2;
return new Vector2(x, y);
}
public Tile getTile(TileIndex index)
{
if (index.x_ >= width_ || index.y_ >= height_ || index.x_ < 0 || index.y_ < 0)
{
return IMPASSABLE;
}
return tiles_[index.y_, index.x_]; // TILES IS Y, X
}
public static bool equals(TileIndex lhs, TileIndex rhs)
{
return lhs.x_ == rhs.x_ && lhs.y_ == rhs.y_;
}
public ActionGoto(NonPlayableCharacterAbstract character, TileIndex target)
: base(character)
{
target_ = target;
}
