using libsecondlife;
using System;
using System.Collections.Generic;
using System.Text;
using System.Drawing;
namespace AStar
{
public class Pathfinding
{
private class PathNode
{
private LLVector3 _Position;
/// <summary>
/// A container for a node position which includes details for calculating movement cost
/// </summary>
/// <param name="position"></param>
/// <param name="parent"></param>
public PathNode(LLVector3 position)
{
_Position = position;
}
/// <summary>
/// The coordinates of this node
/// </summary>
public LLVector3 Position
{
get { return _Position; }
}
/// <summary>
/// The parent of this node
/// </summary>
public LLVector3 Parent;
/// <summary>
/// The cost of moving from to this node from its parent (10 if adjacent, 14 if diagonal)
/// </summary>
public int G;
/// <summary>
/// The "Manhattan method" score for this node in relation to the target position
/// </summary>
public int H;
/// <summary>
/// The total cost of chosing this node
/// </summary>
public int F;
}
/// <summary>
/// Checks the specified location on the wander map to see if we are permitted to go there
/// </summary>
/// <param name="x"></param>
/// <param name="y"></param>
/// <returns>true if we are allowed to go there, false if we are not</returns>
private static bool MapCheckPosition(Bitmap map, LLVector3 position)
{
int x = (int)position.X;
int y = 255 - (int)position.Y;
int z = (int)position.Z;
if (map == null || x > 255 || y > 255 || x < 0 || y < 0) return false;
List<Color> pixels = new List<Color>();
pixels.Add(map.GetPixel(x, y));
//if (y + 1 <= 255) pixels.Add(map.GetPixel(x, y + 1));
//if (y - 1 >= 0) pixels.Add(map.GetPixel(x, y - 1));
//etc... add all 9
foreach (Color c in pixels)
{
if (c.R != 00 || c.B != 00 || c.G != 00) return false;
}
return true;
}
/// <summary>
/// Returns the an efficient path from pointA to pointB
/// </summary>
/// <param name="target"></param>
/// <returns></returns>
public static List<LLVector3> AStarPath(Bitmap map, LLVector3 pointA, LLVector3 pointB)
{
pointA = new LLVector3((int)pointA.X, (int)pointA.Y, (int)pointA.Z);
pointB = new LLVector3((int)pointB.X, (int)pointB.Y, (int)pointB.Z);
List<LLVector3> path = new List<LLVector3>();
if (!MapCheckPosition(map, pointA)) return path;
if (!MapCheckPosition(map, pointB)) return path;
Dictionary<LLVector3, PathNode> open = new Dictionary<LLVector3, PathNode>();
Dictionary<LLVector3, PathNode> closed = new Dictionary<LLVector3, PathNode>();
//Add the starting square (or node) to the open list.
PathNode current = new PathNode(pointA);
current.Parent = pointA; //the start parent is itself (as a marker)
current.G = 0;
current.H = (int)Math.Abs(pointB.X - pointA.X) + (int)Math.Abs(pointB.Y - pointA.Y);
current.F = current.G + current.H;
open.Add(current.Position, current);
do
{
//Look for the lowest F cost square on the open list.
//We refer to this as the current square.
Nullable<int> lowest = null;
foreach (PathNode node in open.Values)
{
if (lowest == null || node.F < lowest)
{
lowest = node.F;
current = node;
}
}
if (lowest == null) throw new Exception("No available path was detected.");
PathNode parent = current;
//Switch it to the closed list.
open.Remove(current.Position);
closed.Add(current.Position, current);
if (current.Position == pointB) break;
//For each of the 8 squares adjacent to this current square...
int x = (int)current.Position.X;
int y = (int)current.Position.Y;
int z = (int)current.Position.Z;
List<LLVector3> adjacent = new List<LLVector3>();
adjacent.Add(new LLVector3(x + 1, y + 1, 0));
adjacent.Add(new LLVector3(x - 1, y - 1, 0));
adjacent.Add(new LLVector3(x + 1, y - 1, 0));
adjacent.Add(new LLVector3(x - 1, y + 1, 0));
adjacent.Add(new LLVector3(x + 1, y, 0));
adjacent.Add(new LLVector3(x - 1, y, 0));
adjacent.Add(new LLVector3(x, y + 1, 0));
adjacent.Add(new LLVector3(x, y - 1, 0));
foreach(LLVector3 position in adjacent)
{
//If it is not walkable or if it is on the closed list, ignore it.
if (!MapCheckPosition(map, position) || closed.ContainsKey(position)) continue;
//Otherwise do the following.
//If it isn’t on the open list, add it to the open list.
if (!open.ContainsKey(position))
{
open.Add(position, new PathNode(position));
//Make the current square the parent of this square.
open[position].Parent = current.Position;
//Record the F, G, and H costs of the square.
if (position.X == x || position.Y == y) open[position].G = parent.G + 10; //adjacent
else open[position].G = parent.G + 14; //diagonal
open[position].H = (int)Math.Abs(position.X - x) + (int)Math.Abs(position.Y - y);
open[position].F = open[position].G + open[position].H;
}
//If so, change the parent of the square to the current square, and
//recalculate the G and F scores of the square.
else
{
open[position].Parent = current.Position;
if (position.X == x || position.Y == y) open[position].G = parent.G + 10; //adjacent
else open[position].G = parent.G + 14; //diagonal
open[position].F = open[position].G + open[position].H;
}
}
}
while (open.Count > 0);
if (current.Position != pointB) throw new Exception("Unable to find a path!");
//Backtrack from the end to the beginning
do
{
path.Insert(0, current.Position);
current = closed[current.Parent];
}
while (current.Parent != closed[current.Parent].Parent); //the start parent is itself
return path;
}
}
}