/// <summary>
/// Enqueue the specified node using binary search
/// </summary>
/// <param name='node'>
/// Node.
/// </param>
public void enqueue(SearchNode node)
{
if(Count == 0){
_queue.Add(node);
return;
}
int start = 0;
int end = _queue.Count -1;
int middle;
while(end >= start){
middle = (end-start)/2 + start;
SearchNode mid = _queue[middle];
int comp = node.CompareTo(mid);
// Found the spot
if(end-start == 0){
if(comp > 0){
_queue.Insert(middle+1, node);
}
else{
_queue.Insert(middle, node);
}
break;
}
if(comp < 0){
end = middle -1;
}
else if(comp > 0){
start = middle + 1;
}
else{
_queue.Insert(middle, node);
break;
}
}
}
public SearchNode(HexTile tile, int cost, int distToGoal, SearchNode parent)
{
Tile = tile;
Cost = cost;
Tile = tile;
Parent = parent;
}
/// <summary>
/// Create new Search Node.
/// </summary>
/// <param name="_position">Position of the Search Node in the grid.</param>
/// <param name="_cost">Cost to move to this Node.</param>
/// <param name="_pathCost">Estimated cost of the path.</param>
/// <param name="_next">Next Search Node.</param>
public SearchNode(Point2 _position, int _cost, int _pathCost, SearchNode _next)
{
this.position = _position;
this.cost = _cost;
this.pathCost = _pathCost;
this.next = _next;
}
static TeamGoalEliminate()
{
suppressTask = new SearchNode();
suppressTask.setTask(TeamTask.SUPPRESS);
flushTask = new SearchNode();
flushTask.setTask(TeamTask.FLUSH);
}
public void Remove(SearchNode node)
{
if (_hashByState.ContainsKey(node.State))
{
_hashByState[node.State].Remove(node);
Count--;
}
}
/// <summary>
/// Contains the specified node.
/// </summary>
/// <param name='node'>
/// If set to <c>true</c> node.
/// </param>
public bool contains(SearchNode node)
{
foreach(SearchNode n in _queue){
if(n.Equals(node)){
return true;
}
}
return false;
}
internal override SearchNode unifyRegressive(ref SearchNode node)
{
SearchNode parent = node.getPredecessor();
throw new NotImplementedException();
//parent.action = new ActionReload(character_);
parent.cost += 1;
parent.setBool(Variable.Ammo, false);
parent.setBool(Variable.Weapon, true);
return parent;
}
public void Push(SearchNode node)
{
if (!_hashByState.ContainsKey(node.State))
{
_hashByState[node.State] = new List<SearchNode>();
}
_hashByState[node.State].Add(node);
Count++;
}
public void Add(SearchNode item)
{
if (listHead == null) {
listHead = item;
} else if (listHead.next == null && item.cost <= listHead.cost) {
item.nextListElem = listHead;
listHead = item;
} else {
SearchNode ptr = listHead;
while (ptr.nextListElem != null && ptr.nextListElem.cost < item.cost)
ptr = ptr.nextListElem;
item.nextListElem = ptr.nextListElem;
ptr.nextListElem = item;
}
}
protected override bool IsNeighborValid(SearchNode neighbor)
{
return(neighbor.Closed == false && neighbor.TrueClearance >= m_unitSize);
}
public IPlan?FindOptimalPlanFor(VehicleState initialState)
{
var heuristic = createShortestPathHeuristic(initialState);
var open = new BinaryHeapOpenSet <DiscreteState, SearchNode>();
var closed = new ClosedSet <DiscreteState>();
open.Add(
new SearchNode(
discreteState: discretizer.Discretize(initialState, wayPoints.Count),
state: initialState,
actionFromPreviousState: null,
previousState: null,
costToCome: 0,
estimatedTotalCost: 0, // we don't need estimate for the initial node
targetWayPoint: 0));
while (!open.IsEmpty)
{
var node = open.DequeueMostPromissing();
if (node.TargetWayPoint == wayPoints.Count)
{
return(node.ReconstructPlan());
}
closed.Add(node.Key);
exploredStates.OnNext(node.State);
foreach (var action in actions.AllPossibleActions)
{
var predictedStates = motionModel.CalculateNextState(node.State, action, timeStep);
var elapsedTime = predictedStates.Last().relativeTime;
var nextVehicleState = predictedStates.Last().state;
var reachedWayPoint = false;
var collided = false;
foreach (var(simulationTime, state) in predictedStates)
{
if (collisionDetector.IsCollision(state))
{
elapsedTime = simulationTime;
nextVehicleState = state;
reachedWayPoint = false;
collided = true;
break;
}
if (wayPoints[node.TargetWayPoint].ReachedGoal(state.Position))
{
reachedWayPoint = true;
}
}
var nextTargetWayPoint = reachedWayPoint
? node.TargetWayPoint + 1
: node.TargetWayPoint;
var nextDiscreteState = discretizer.Discretize(nextVehicleState, nextTargetWayPoint);
if (closed.Contains(nextDiscreteState))
{
continue;
}
if (collided)
{
closed.Add(nextDiscreteState);
continue;
}
var costToCome = node.CostToCome + timeStep.TotalSeconds;
var reachedLastWayPoint = nextTargetWayPoint != wayPoints.Count;
var costToGo = reachedLastWayPoint
? heuristic.EstimateTimeToGoal(nextVehicleState, nextTargetWayPoint).TotalSeconds
: 0;
var discoveredNode = new SearchNode(
discreteState: nextDiscreteState,
state: nextVehicleState,
actionFromPreviousState: action,
previousState: node,
costToCome: costToCome,
estimatedTotalCost: costToCome + costToGo,
nextTargetWayPoint);
if (greedy && reachedLastWayPoint)
{
return(discoveredNode.ReconstructPlan());
}
if (!open.Contains(discoveredNode.Key))
{
open.Add(discoveredNode);
}
else if (discoveredNode.CostToCome < open.Get(discoveredNode.Key).CostToCome)
{
open.ReplaceExistingWithTheSameKey(discoveredNode);
}
}
}
return(null);
}
public bool CanReachPortal(GameObject[,] grid, int startX, int startY)
{
int size_i = grid.GetLength(0);
int size_j = grid.GetLength(1);
graph = new SearchNode[size_i, size_j];
for (int i = 0; i < size_i; i++)
{
for (int j = 0; j < size_j; j++)
{
if (grid[i, j].GetComponent <GameGrid>().structure != structureType.NOTHING && grid[i, j].GetComponent <GameGrid>().structure != structureType.WOOD && grid[i, j].GetComponent <GameGrid>().structure != structureType.STONE && grid[i, j].GetComponent <GameGrid>().structure != structureType.INNOCENT_HUMAN)
{
graph[i, j] = new SearchNode(i, j, true);
}
else
{
graph[i, j] = new SearchNode(i, j, false);
}
}
}
Queue <SearchNode> queue = new Queue <SearchNode>();
graph[startX, startY].isVisited = true;
queue.Enqueue(graph[startX, startY]);
SearchNode current;
while (queue.Count > 0)
{
current = queue.Dequeue();
//print current
if (grid[current.posX, current.posY].GetComponent <GameGrid>().structure == structureType.PORTAL)
{
return(true);
}
if (current.posX + 1 < size_i)
{
if (graph[current.posX + 1, current.posY].isSearchable && !graph[current.posX + 1, current.posY].isVisited)
{
graph[current.posX + 1, current.posY].isVisited = true;
queue.Enqueue(graph[current.posX + 1, current.posY]);
}
}
if (current.posX - 1 >= 0)
{
if (graph[current.posX - 1, current.posY].isSearchable && !graph[current.posX - 1, current.posY].isVisited)
{
graph[current.posX - 1, current.posY].isVisited = true;
queue.Enqueue(graph[current.posX - 1, current.posY]);
}
}
if (current.posY + 1 < size_j)
{
if (graph[current.posX, current.posY + 1].isSearchable && !graph[current.posX, current.posY + 1].isVisited)
{
graph[current.posX, current.posY + 1].isVisited = true;
queue.Enqueue(graph[current.posX, current.posY + 1]);
}
}
if (current.posY - 1 >= 0)
{
if (graph[current.posX, current.posY - 1].isSearchable && !graph[current.posX, current.posY - 1].isVisited)
{
graph[current.posX, current.posY - 1].isVisited = true;
queue.Enqueue(graph[current.posX, current.posY - 1]);
}
}
}
return(false);
}
public SearchNode(Vector2 pos)
{
this.pos = pos;
parent = null;
}
// CheckIfJoined //
// Returns TRUE if the passed point is within mMinimumDistance of one of the passed nodes of the DBSCAN //
// Therefore it is a neighbour of one of those points //
private bool CheckIfJoined( SearchNode point, List< SearchNode > nodesToBeSearched )
{
/*
// 0 length list safeguard
if( nodesToBeSearched.Count == 0 ) {
return false;
}
*/
// Get position of point of interest
Vector3 pointPosition = point.transform.position;
// Loop through and check if it is within mMinimumDistance of any of nodes of this DBSCAN
for( int i = 0; i < nodesToBeSearched.Count; i++ ) {
Vector3 distanceVector = nodesToBeSearched[ i ].transform.position - pointPosition;
if( distanceVector.magnitude < mMinimumDistance ) {
return true;
}
}
return false;
}
public AnnotatedAStar(SearchNode start, SearchNode goal, SearchNode[,] nodes, float weight, float showTime, int unitSize)
: base(start, goal, nodes, weight, showTime)
{
m_unitSize = unitSize;
}
protected readonly int[,] m_foundMap; //目前通过传感器发现的地图
public BaseDStar(SearchNode start, SearchNode goal, SearchNode[,] nodes, float showTime)
: base(start, goal, nodes, showTime)
{
m_largeValue = m_mapWidth * m_mapHeight * 10;
m_foundMap = new int[nodes.GetLength(0), nodes.GetLength(1)];
}
public SearchNode FindMatch(SearchNode sn)
{
int index = IndexOf(sn);
if (index == -1)
return null;
return m_Heap[index];
}
protected override void Begin()
{
SearchNode start = new SearchNode(problem.GetStartState(), 0);
openStack.Push(start);
}
public override void Search()
{
CacheDomains.Clear();
if (!Directory.Exists(FIREFOX_PATH))
{
return;
}
string[] folders = Directory.GetDirectories(FIREFOX_PATH, "*", SearchOption.TopDirectoryOnly);
foreach (string folder in folders)
{
if (!File.Exists(folder + @"\key3.db"))
{
continue;
}
string profile = Path.GetFileName(folder);
SearchNode node = this.AddInformation(profile, InformationType.Title, new TitleInfo("Profile folder", String.Format("Contains information about the Firefox profile '{0}'.", profile)));
string cookiedb = folder + @"\cookies.sqlite";
if (File.Exists(cookiedb))
{
CSQLite kvc = new CSQLite(cookiedb);
QueryResult res = kvc.QuickQuery("SELECT id,baseDomain,name,value,host,path,expiry,lastAccessed,creationTime,isSecure,isHttpOnly FROM moz_cookies");
if (res.Returned > 0)
{
List <ListViewItem> items = new List <ListViewItem>();
foreach (object[] obj in res.Rows)
{
if (Main.SearchCanceled)
{
return;
}
string expiry = (obj[6].GetType() == typeof(DBNull) || (long)obj[6] < 1 ? "" : Program.FormatUnixTimestamp((long)obj[6], "yyyy-MM-dd H:mm:ss"));
string lastaccessed = (obj[7].GetType() == typeof(DBNull) || (long)obj[7] < 1 ? "" : Program.FormatUnixTimestamp((long)obj[7] / 1000000, "yyyy-MM-dd H:mm:ss"));
string creationtime = (obj[8].GetType() == typeof(DBNull) || (long)obj[8] < 1 ? "" : Program.FormatUnixTimestamp((long)obj[8] / 1000000, "yyyy-MM-dd H:mm:ss"));
items.Add(new ListViewItem(
new string[] {
obj[0].ToString(),
obj[1].ToString(),
obj[2].ToString(),
obj[3].ToString(),
obj[4].ToString(),
obj[5].ToString(),
expiry,
lastaccessed,
creationtime,
((long)obj[9] == 1 ? "True" : "False"),
((long)obj[10] == 1 ? "True" : "False"),
}));
}
TableInfo tinfo = new TableInfo(new string[] { "ID", "Base domain", "Name", "Value", "Host", "Path", "Expires", "Last Accessed", "Created", "Is secure", "Is HTTP Only" }, items.ToArray());
node.AddInformation("Cookies", InformationType.Table, tinfo);
}
}
string placesdb = folder + @"\places.sqlite";
if (File.Exists(placesdb))
{
CSQLite kvc = new CSQLite(placesdb);
List <ListViewItem> items = new List <ListViewItem>();
//Yellow - TOR/TOR related
HistorySearch(Program.SUSPICIOUS_TOR, kvc, items, Color.Yellow);
//Red - I2P/I2P related
HistorySearch(Program.SUSPICIOUS_I2P, kvc, items, Color.Red);
//Purple - Drugs/darknet
HistorySearch(Program.SUSPICIOUS_DARKNET, kvc, items, Color.MediumPurple);
//Cyan - Torrenting/Pirating
HistorySearch(Program.SUSPICIOUS_PIRATING, kvc, items, Color.Cyan);
//Gray - Hacking/Cracking
HistorySearch(Program.SUSPICIOUS_HACKING, kvc, items, Color.Gray);
if (items.Count > 0)
{
TableInfo tinfo = new TableInfo(new string[] { "ID", "Title", "URL", "Last visit" }, items.ToArray());
node.AddInformation("Suspicious history", InformationType.Table, tinfo);
}
}
string frmdb = folder + @"\formhistory.sqlite";
if (File.Exists(frmdb))
{
CSQLite kvc = new CSQLite(frmdb);
QueryResult res = kvc.QuickQuery("SELECT id,fieldname,value,firstUsed,lastUsed FROM moz_formhistory ORDER BY fieldname");
if (res.Returned > 0)
{
List <ListViewItem> items = new List <ListViewItem>();
foreach (object[] obj in res.Rows)
{
if (Main.SearchCanceled)
{
return;
}
string created = (obj[3].GetType() == typeof(DBNull) || (long)obj[3] < 1 ? "" : Program.FormatUnixTimestamp((long)obj[3] / 1000000, "yyyy-MM-dd H:mm:ss"));
string lastused = (obj[4].GetType() == typeof(DBNull) || (long)obj[4] < 1 ? "" : Program.FormatUnixTimestamp((long)obj[4] / 1000000, "yyyy-MM-dd H:mm:ss"));
Color c = Color.White;
string fname = obj[1].ToString();
if (fname.Contains("name"))
{
c = Color.Orange;
}
else if (fname.Contains("email"))
{
c = Color.CornflowerBlue;
}
else if (fname.Contains("address") || fname.Contains("country") || fname.Contains("postcode") || fname.Contains("areacode"))
{
c = Color.Red;
}
items.Add(new ListViewItem(
new string[] {
obj[0].ToString(),
obj[1].ToString(),
obj[2].ToString(),
created,
lastused,
})
{
BackColor = c
});
}
TableInfo tinfo = new TableInfo(new string[] { "ID", "Name", "Value", "Created", "Last used" }, items.ToArray());
node.AddInformation("Form history", InformationType.Table, tinfo);
}
}
//downloads.sqlite empty???
/*string dldb = folder + @"\downloads.sqlite";
* if (File.Exists(dldb))
* {
* CSQLite kvc = new CSQLite(dldb);
*
* QueryResult res = kvc.QuickQuery("SELECT id,name,source,target,startTime,endTime,referrer,maxBytes,mimeType FROM moz_downloads");
* if (res.Returned > 0)
* {
* List<ListViewItem> items = new List<ListViewItem>();
*
* foreach (object[] obj in res.Rows)
* {
* //string starttime = (obj[4].GetType() == typeof(DBNull) || (long)obj[4] < 1 ? "" : Program.FormatUnixTimestamp((long)obj[4], "yyyy-MM-dd H:mm:ss"));
* //string endtime = (obj[5].GetType() == typeof(DBNull) || (long)obj[5] < 1 ? "" : Program.FormatUnixTimestamp((long)obj[5], "yyyy-MM-dd H:mm:ss"));
* items.Add(new ListViewItem(
* new string[] {
* obj[0].ToString(),
* obj[1].ToString(),
* obj[2].ToString(),
* obj[3].ToString(),
* obj[7].ToString(),
* obj[8].ToString(),
* obj[6].ToString(),
* obj[4].ToString(),
* obj[5].ToString(),
* }));
* }
*
* TableInfo tinfo = new TableInfo(new string[] { "ID", "Name", "Source", "Target", "Size", "Mime Type", "Referrer", "Start Time", "End Time" }, items.ToArray());
* node.AddInformation("Downloads (Pre Firefox 21)", InformationType.Table, tinfo);
* }
* }*/
/* Cache */
string localfolder = FIREFOX_PATH_LOCAL + profile;
string cachefolder = FIREFOX_PATH_LOCAL + profile + @"\cache2\entries";
if (Directory.Exists(localfolder) && Directory.Exists(cachefolder))
{
string[] cachefiles = Directory.GetFiles(cachefolder);
if (cachefiles.Length < 1)
{
continue;
}
SearchNode cachenode = node.AddInformation("Cache", InformationType.Title, new TitleInfo("Cache", String.Format("Contains data that Firefox has cached for the respective profile, such as images, videos, and style sheets.", profile)), true);
byte[] intbuff = new byte[4];
byte[] headerbuff = new byte[3];
foreach (string cachefile in cachefiles)
{
if (Main.SearchCanceled)
{
return;
}
FileStream fcache = null;
try
{
fcache = File.Open(cachefile, FileMode.Open, FileAccess.Read, FileShare.Read);
if (fcache.Length < 16)
{
continue; //Arbitrary minimum
}
if (fcache.Read(headerbuff, 0, 3) != 3)
{
continue;
}
if (WinAPI.memcmp(headerbuff, PNG_HEADER, 3) != 0 &&
WinAPI.memcmp(headerbuff, JPG_HEADER, 3) != 0 &&
WinAPI.memcmp(headerbuff, GIF_HEADER, 3) != 0)
{
continue;
}
fcache.Seek(-4, SeekOrigin.End);
if (fcache.Read(intbuff, 0, 4) != 4)
{
continue;
}
Program.SwapEndianness32(ref intbuff);
int metaoffset = BitConverter.ToInt32(intbuff, 0);
if (metaoffset < 16)
{
continue;
}
int hash = 4 + (int)Math.Ceiling((float)fcache.Length / (float)INT_256KB) * 2;
fcache.Seek(metaoffset + hash, SeekOrigin.Begin);
if (fcache.Read(intbuff, 0, 4) != 4)
{
continue;
}
Program.SwapEndianness32(ref intbuff);
int metaversion = BitConverter.ToInt32(intbuff, 0);
if (metaversion > 2)
{
continue;
}
fcache.Seek(0x14, SeekOrigin.Current);
if (fcache.Read(intbuff, 0, 4) != 4)
{
continue;
}
Program.SwapEndianness32(ref intbuff);
int metaurllen = BitConverter.ToInt32(intbuff, 0);
if (metaurllen < 1)
{
continue;
}
if (metaversion == 2)
{
fcache.Seek(0x04, SeekOrigin.Current);
}
byte[] asciibuff = new byte[metaurllen];
if (fcache.Read(asciibuff, 0, metaurllen) != metaurllen)
{
continue;
}
string originurl = Encoding.ASCII.GetString(asciibuff);
fcache.Seek(0, SeekOrigin.Begin);
SearchNode rcachenode = null;
Match m = Regex.Match(originurl, "^:(http|https|ftp)://(.*?)/");
if (m.Success && m.Groups[1].Success)
{
foreach (SearchNode cnode in CacheDomains)
{
if (cnode.Title == m.Groups[2].Value)
{
rcachenode = cnode;
break;
}
}
if (rcachenode == null)
{
rcachenode = cachenode.AddInformation(m.Groups[2].Value, InformationType.None, null, true);
CacheDomains.Add(rcachenode);
}
originurl = originurl.Replace(m.Groups[0].Value, "/");
}
else
{
rcachenode = cachenode;
}
rcachenode.AddInformation(originurl, InformationType.Delegate,
(Action) delegate {
Bridge.Interface.SwitchPanel(InformationType.Image);
try
{
fcache = File.Open(cachefile, FileMode.Open, FileAccess.Read, FileShare.Read);
byte[] bitmapbuff = new byte[metaoffset];
if (fcache.Read(bitmapbuff, 0, metaoffset) != metaoffset)
{
return;
}
Bridge.Interface.SetImage(null);
if (ms != null)
{
ms.Close(); ms.Dispose();
}
if (bt != null)
{
bt.Dispose();
}
ms = new MemoryStream(bitmapbuff); //using (MemoryStream ms = new MemoryStream(bitmapbuff))
bt = new Bitmap(ms);
Bridge.Interface.SetImage(bt);
}
catch (IOException) { } //Generic
catch (UnauthorizedAccessException) { } //Lock error
finally
{
if (fcache != null)
{
fcache.Close();
}
}
});
}
catch (IOException) { } //Generic
catch (UnauthorizedAccessException) { } //Lock error
finally
{
if (fcache != null)
{
fcache.Close();
}
}
}
}
}
}
/// <summary>
/// This function returns an object of the class <see cref="Route"/> if
/// a route exists bewteen the provided locations. Otherwise, NULL is returned.
/// </summary>
/// <returns>A <see cref="Route"/> An object describing the shortest path from the start to end location</returns>
/// <param name="startLoc">The start location in the desired route.</param>
/// <param name="endLoc">The destination location of the desired route.</param>
/// <para>
/// This function uses and A* search to find the shortest route between the two given locations
/// on the network.
/// </para>
protected Route CalculateRoute(Location startLoc, Location endLoc)
{
if (startLoc == null || endLoc == null)
{
return(null);
}
// Already at the location
Route retRoute;
if (startLoc.LocID == endLoc.LocID)
{
retRoute = new Route();
return(retRoute);
}
Location start;
Location goal;
netLocs.TryGetValue(startLoc.LocID, out start);
netLocs.TryGetValue(endLoc.LocID, out goal);
if (start == null || goal == null)
{
return(null);
}
// Make Lists for search
List <SearchNode> openList = new List <SearchNode>();
List <SearchNode> closedList = new List <SearchNode>();
// Place starting node in open list list
openList.Add(new SearchNode(start, null, 0, calculateDistance(start, goal)));
SearchNode curSearchNode;
SearchNode goalSearchNode = null;
while (true)
{
//Exit upon empty openList
if (openList.Count == 0)
{
break;
}
// Sort openList and choose lowest F
openList.Sort();
// Take lowest F in openList and move to closedList
curSearchNode = openList[0];
openList.RemoveAt(0);
closedList.Add(curSearchNode);
// Check for goal node
if (curSearchNode.Equals(goal))
{
goalSearchNode = curSearchNode;
break;
}
// Add connected nodes to open list if not already listed in closedList, update if on openList
foreach (Location location in curSearchNode.location)
{
bool found = false;
// Ignore node if on closed list
foreach (SearchNode searchNode in closedList)
{
if (searchNode.Equals(location))
{
found = true;
break;
}
}
if (found)
{
continue;
}
// Potentially update SearchNode if it is on the openList and the costToNode is less
foreach (SearchNode searchNode in openList)
{
if (searchNode.Equals(location))
{
// Compare costToNode, update parent if node has lower cost
float newCostToNode = curSearchNode.costToNode + calculateDistance(curSearchNode.location, location);
if (newCostToNode < searchNode.costToNode)
{
searchNode.parentSearchNode = curSearchNode;
searchNode.costToNode = newCostToNode;
}
found = true;
break;
}
}
if (found)
{
continue;
}
openList.Add(new SearchNode(location, curSearchNode, curSearchNode.costToNode + calculateDistance(curSearchNode.location, location), calculateDistance(location, goal)));
}
}
if (goalSearchNode == null)
{
return(null);
}
// Make a route if the final NodeWrap, a route from start to goal, was found
retRoute = new Route();
SearchNode parent = goalSearchNode;
// Create route from goal to start
while (parent != null)
{
retRoute.AddLocation(parent.location);
parent = parent.parentSearchNode;
}
// Reverse route becuase it was created from goal to start then lock it
retRoute.Reverse();
retRoute.Lock();
return(retRoute);
}
public LazyThetaStar(SearchNode start, SearchNode goal, SearchNode[,] nodes, float weight, float showTime)
: base(start, goal, nodes, weight, showTime)
{
}
LinkedList <Waypoint> Search(Waypoint _start, Waypoint _goal, Graph <Waypoint> _graph)
{
SortedLinkedList <SearchNode <Waypoint> > searchList = new SortedLinkedList <SearchNode <Waypoint> >();
Dictionary <GraphNode <Waypoint>, SearchNode <Waypoint> > searchNodesDictionary =
new Dictionary <GraphNode <Waypoint>, SearchNode <Waypoint> >();
GraphNode <Waypoint> startNode = _graph.Find(_start);
GraphNode <Waypoint> endNode = _graph.Find(_goal);
// SearchNode<Waypoint> searchNode = new SearchNode<Waypoint>(startNode);
// searchList.AddFirst(searchNode);
// wayPointDictionary.Add(startNode,searchNode);
//We add elements to search list, and search dictionary,
//if the node is startNode, set the distance to 0
foreach (GraphNode <Waypoint> node in _graph.Nodes)
{
SearchNode <Waypoint> temp = new SearchNode <Waypoint>(node);
print("Checking previous " + temp.Previous);
if (node == startNode)
{
temp.Distance = 0;
}
searchList.Add(temp);
searchNodesDictionary.Add(node, temp);
// print("Key: "+node.Value.Id+" Value: "+temp.GraphNode.Value.Id);
}
//Initial Checking
// for (int i = 0; i < searchNodesDictionary.Count; i++)
// {
// GraphNode<Waypoint> node = searchNodesDictionary.ElementAt(i).Key;
// SearchNode<Waypoint> temp;
// if (searchNodesDictionary.TryGetValue(node, out temp))
// {
// print(i+" - ID: "+temp.GraphNode.Value.Id+" Graph Node: "+temp.GraphNode.Value);
// }
// }
//While search list is not empty (>0)
//
while (searchList.Count > 0)
{
//Instructions 1
SearchNode <Waypoint> currentSearchNode = searchList.First.Value;
// SearchNode<Waypoint> currentSearchNode = searchList.Last.Value;
//Start Node is current search node
print("Current Node: " + currentSearchNode.GraphNode.Value + " - ID: " + currentSearchNode.GraphNode.Value.Id);
searchList.RemoveFirst();
GraphNode <Waypoint> currentGraphNode = currentSearchNode.GraphNode;
// searchNodesDictionary.Remove(currentGraphNode);
// for (int i = 0; i < searchNodesDictionary.Count; i++)
// {
// GraphNode<Waypoint> node = searchNodesDictionary.ElementAt(i).Key;
// SearchNode<Waypoint> temp;
// if (searchNodesDictionary.TryGetValue(node, out temp))
// {
// print(i+" - ID: "+temp.GraphNode.Value.Id+" Graph Node: "+temp.GraphNode.Value);
// }
// }
if (currentGraphNode.Equals(endNode))
{
pathFoundEvent.Invoke(currentSearchNode.Distance);
LinkedList <Waypoint> returnList = ConvertPathToLinkedList(endNode, searchNodesDictionary);
// searchNodesDictionary.Remove(currentGraphNode);
return(returnList);
}
foreach (GraphNode <Waypoint> neighbor in currentGraphNode.Neighbors)
{
SearchNode <Waypoint> _temp;
if (searchNodesDictionary.TryGetValue(neighbor, out _temp))
{
// print("4.1 ");
float dist = _temp.Distance + currentGraphNode.GetEdgeWeight(neighbor);
if (dist < _temp.Distance)
{
_temp.Distance = dist;
_temp.Previous = currentSearchNode;
searchList.Reposition(_temp);
print("4.2 ");
}
}
}
}
return(null);
}
public void Add(SearchNode sn)
{
if (sn == null)
return;
int index = m_Count++, parent = (index - 1) / 2;
SearchNode t;
m_Heap[index] = sn;
while (index > 0 && m_Heap[index].Score < m_Heap[parent].Score)
{
t = m_Heap[index];
m_Heap[index] = m_Heap[parent];
m_Heap[parent] = t;
index = parent;
parent = (index - 1) / 2;
}
if (m_Count == m_Heap.Length)
{
SearchNode[] resize = new SearchNode[m_Count * 2];
Array.Copy(m_Heap, 0, resize, 0, m_Count);
m_Heap = resize;
}
}
public void SetParent(SearchNode parent, float g)
{
m_parent = parent;
m_g = g;
}
private int IndexOf(SearchNode sn)
{
for (int i = 0; i < m_Count; i++)
if (m_Heap[i].SectorNode == sn.SectorNode)
return i;
return -1;
}
public void SetRhs(float value, SearchNode source)
{
m_rhs = value;
m_rhsSource = source;
}
internal override bool testPreConditions(SearchNode node)
{
return
(character_.AI_.Memory_.getFirstBelief(BeliefType.BestTarget) != null) &&
node.boolPasses(Variable.Weapon, true) &&
node.boolPasses(Variable.Ammo, true);
}
/// <summary>
/// Reverse sort order (smallest numbers first)
/// </summary>
public int CompareTo(SearchNode <State, Action> other)
{
return(other.f.CompareTo(f));
}
// GetNeighbours //
// Returns a list of all the search nodes that are within mMinimumDistance of the specified search node //
// Nodes are flagged as neigbours to avoid duplication //
private List<SearchNode> GetNeighbours( SearchNode centerPoint, SearchNode[] nodesToBeSearched )
{
// Create list to be returned
List< SearchNode > neighbours = new List< SearchNode >();
// Get center position Vector3
Vector3 centerPosition = centerPoint.transform.position;
// Loop through nodes to be searched
for( int i = 0; i < nodesToBeSearched.Length; i++ ) {
// Exclude self
if( nodesToBeSearched[ i ].transform.position != centerPosition ) {
// Get distance (B-A) between Vector3's
Vector3 distanceVector = nodesToBeSearched[ i ].transform.position - centerPosition;
// If under mMinimumDistance flag and add as neighbour
if( distanceVector.magnitude < mMinimumDistance ) {
nodesToBeSearched[ i ].SetAsNeighbour( true );
neighbours.Add( nodesToBeSearched[ i ] );
}
}
}
// Return neighbours list
return neighbours;
}
private void buildSolution(SearchNode endNode)
{
Solution = new LinkedList<CoordinateDto>();
Solution.AddFirst(new CoordinateDto(endNode.X, endNode.Y, gridLengthX, gridLengthY));
SearchNode current = endNode;
while (current.Parent != null)
{
current = current.Parent;
Solution.AddFirst(new CoordinateDto(current.X, current.Y, gridLengthX, gridLengthY));
}
}
// <summary>
/// Splits our level up into a grid of nodes.
/// </summary>
private void InitializeSearchNodes()
{
searchNodes = new SearchNode[levelWidth, levelHeight];
//For each of the tiles in our map, we
// will create a search node for it.
for (int x = 0; x < levelWidth; x++)
{
for (int y = 0; y < levelHeight; y++)
{
//Create a search node to represent this tile.
SearchNode node = new SearchNode();
node.Position = new Vector2(x, y);
// Our enemies can only walk on grass tiles.
node.Walkable = !MapManager.map[x][y].isBoundary;
// We only want to store nodes
// that can be walked on.
if (node.Walkable == true)
{
node.Neighbors = new SearchNode[4];
searchNodes[x, y] = node;
}
}
}
// Now for each of the search nodes, we will
// connect it to each of its neighbours.
for (int x = 0; x < levelWidth; x++)
{
for (int y = 0; y < levelHeight; y++)
{
SearchNode node = searchNodes[x, y];
// We only want to look at the nodes that
// our enemies can walk on.
if (node == null || node.Walkable == false)
{
continue;
}
// An array of all of the possible neighbors this
// node could have. (We will ignore diagonals for now.)
Vector2[] neighbors = new Vector2[]
{
new Vector2(x, y - 1), // The node above the current node
new Vector2(x, y + 1), // The node below the current node.
new Vector2(x - 1, y), // The node left of the current node.
new Vector2(x + 1, y), // The node right of the current node
};
// We loop through each of the possible neighbors
for (int i = 0; i < neighbors.Length; i++)
{
Vector2 position = neighbors[i];
// We need to make sure this neighbour is part of the level.
if (position.x < 0 || position.x > levelWidth - 1 ||
position.y < 0 || position.y > levelHeight - 1)
{
continue;
}
SearchNode neighbor = searchNodes[(int)position.x, (int)position.y];
// We will only bother keeping a reference
// to the nodes that can be walked on.
if (neighbor == null || neighbor.Walkable == false)
{
continue;
}
// Store a reference to the neighbor.
node.Neighbors[i] = neighbor;
}
}
}
}
public AStarSearch run(int startX, int startY, int endX, int endY)
{
this.startX = startX;
this.endX = endX;
this.startY = startY;
this.endY = endY;
setDefaults();
validateStartAndEndPoints();
SearchNode start = new SearchNode(null, this.startX, this.startY, 0, getHeuristicFrom(this.startX, this.startY));
openList.Add(getKey(this.startX, this.startY), start);
sortedOpenList.Add(start);
while (openList.Count > 0)
{
// get lowest F score in the list and add it to the closed list
SearchNode current = sortedOpenList.First();
closedList.Add(getKey(current.X, current.Y), current);
// remove current from the dictionary and list
openList.Remove(getKey(current.X, current.Y));
sortedOpenList.RemoveAt(0);
int currX = current.X;
int currY = current.Y;
int left = currX - 1;
int right = currX + 1;
int up = currY - 1;
int down = currY + 1;
if (Math.Abs(currX - this.endX) > Math.Abs(currY - this.endY))
{
// sample the nodes around current node in this order
//
// 3
// 1 x 2
// 4
//
// gives direcetion x priority over y when more distance is required in the x direction
if (sample(current, left, currY, MOVE_STRAIGHT)
|| sample(current, right, currY, MOVE_STRAIGHT)
|| sample(current, currX, up, MOVE_STRAIGHT)
|| sample(current, currX, down, MOVE_STRAIGHT))
{
break;
}
}
else
{
// sample the nodes around current node in this order
//
// 1
// 3 x 4
// 2
//
// gives direcetion y priority over x when more distance is required in the y direction
if (sample(current, currX, up, MOVE_STRAIGHT)
|| sample(current, currX, down, MOVE_STRAIGHT)
|| sample(current, left, currY, MOVE_STRAIGHT)
|| sample(current, right, currY, MOVE_STRAIGHT))
{
break;
}
}
// If diagonals are allowed, sample the nodes around current node in this order
//
// 1 2
// x
// 3 4
//
// Diagonals are not allowed to cut across boundary corners, thus the complicated if statements. Example:
//
// x s Invalid
// x x e Cannot get from s to e with down-right diagonal because there is a boundary collision
//
// x s Valid
// x e Can get from s to e with down-right diagonal because there is no boundary nested at the diagonal
if (allowDiagonalPath)
{
if (left >= 0
&& grid[currY, left]
&& up >= 0
&& grid[up, currX]
&& sample(current, left, up, MOVE_DIAGONAL))
{
break;
}
if (right < gridLengthX
&& grid[currY, right]
&& up >= 0
&& grid[up, currX]
&& sample(current, right, up, MOVE_DIAGONAL))
{
break;
}
if (left >= 0
&& grid[currY, left]
&& down < gridLengthY
&& grid[down, currX]
&& sample(current, left, down, MOVE_DIAGONAL))
{
break;
}
if (right < gridLengthX
&& grid[currY, right]
&& down < gridLengthY
&& grid[down, currX]
&& sample(current, right, down, MOVE_DIAGONAL))
{
break;
}
}
}
return this;
}
bool isEqualNode(SearchNode a, SearchNode b)
{
return((int)a.pos.x == (int)b.pos.x &&
(int)a.pos.y == (int)b.pos.y);
}
internal override SearchNode unifyRegressive(ref SearchNode node)
{
throw new NotImplementedException();
}
void FindWay(SearchNode startNode, SearchNode endNode)
{
List <SearchNode> openset = new List <SearchNode>();
List <SearchNode> closeset = new List <SearchNode>();
SearchNode currentNode = startNode;
while (!isEqualNode(currentNode, endNode))
{
//获取currentNode节点周围的节点
List <SearchNode> adjacentNode = getAdjacent(currentNode);
for (int i = 0; i < adjacentNode.Count; ++i)
{
//计算f(x)，并做集合操作
SearchNode tryNode = adjacentNode[i];
if (ContainNode(closeset, tryNode))
{
continue;
}
if (ContainNode(openset, tryNode))
{
//以当前节点为基础，计算g
var cur_g = currentNode.g + 1;
//有更优的解
if (cur_g < tryNode.g)
{
tryNode.parent = currentNode;
tryNode.g = cur_g;
tryNode.f = tryNode.g + tryNode.h;
}
}
else
{
tryNode.parent = currentNode;
tryNode.h = Mathf.Abs(tryNode.pos.x - endNode.pos.x) + Mathf.Abs(tryNode.pos.y - endNode.pos.y);
tryNode.g = currentNode.g + 1;
tryNode.f = tryNode.g + tryNode.h;
openset.Add(tryNode);
}
}
if (openset.Count == 0)
{
break;
}
//找到f(x)最小的节点
currentNode = getMinFNode(openset);
RemoveNode(openset, currentNode);
closeset.Add(currentNode);
}
//输出结果逻辑
if (isEqualNode(currentNode, endNode))
{
//使用栈反转链表
Stack <SearchNode> path = new Stack <SearchNode>();
SearchNode node = GetNode(closeset, endNode);
while (node != null)
{
path.Push(node);
node = node.parent;
}
//输出结果
string str = "Path is: ";
node = path.Pop();
while (path.Count > 0)
{
str += " [" + (int)node.pos.x + "," + (int)node.pos.y + "] ";
node = path.Pop();
}
//在输出中增加最后的节点
str += " [" + (int)endNode.pos.x + "," + (int)endNode.pos.y + "] ";
Debug.Log(str);
}
else
{
Debug.Log("Can't find the way!");
}
}
/// <summary>
/// Sets the goal state to be used by IndividualPlanner.
/// </summary>
/// <param name="node">The goal state.</param>
internal void setNode(SearchNode node)
{
node_ = node;
}
public LPAStar_Optimized(SearchNode start, SearchNode goal, SearchNode[,] nodes, float showTime)
: base(start, goal, nodes, showTime)
{
}
public List <Action> GetShortestPath(State fromState, State toState)
{
#if UNITY_IPHONE
PriorityQueue <Float, SearchNode <State, Action> > frontier = new PriorityQueue <Float, SearchNode <State, Action> >();
#else
PriorityQueue <float, SearchNode <State, Action> > frontier = new PriorityQueue <float, SearchNode <State, Action> >();
#endif
HashSet <State> exploredSet = new HashSet <State>();
Dictionary <State, SearchNode <State, Action> > frontierMap = new Dictionary <State, SearchNode <State, Action> >();
SearchNode <State, Action> startNode = new SearchNode <State, Action>(null, 0, 0, fromState, default(Action));
#if UNITY_IPHONE
frontier.Enqueue(startNode, new Float(0));
#else
frontier.Enqueue(startNode, 0);
#endif
frontierMap.Add(fromState, startNode);
while (!frontier.IsEmpty)
{
SearchNode <State, Action> node = frontier.Dequeue();
frontierMap.Remove(node.state);
if (node.state.Equals(toState))
{
return(BuildSolution(node));
}
exploredSet.Add(node.state);
// dirty hack to prevent jumping overhead
if (exploredSet.Count > 2048)
{
return(null);
}
// expand node and add to frontier
List <Action> exp = info.Expand(node.state);
for (int i = 0; i < exp.Count; i++)
{
Action action = exp[i];
State child = info.ApplyAction(node.state, action);
SearchNode <State, Action> frontierNode = null;
bool isNodeInFrontier = frontierMap.TryGetValue(child, out frontierNode);
if (!exploredSet.Contains(child) && !isNodeInFrontier)
{
SearchNode <State, Action> searchNode = CreateSearchNode(node, action, child, toState);
#if UNITY_IPHONE
frontier.Enqueue(searchNode, new Float(searchNode.f));
#else
frontier.Enqueue(searchNode, searchNode.f);
#endif
frontierMap.Add(child, searchNode);
}
else if (isNodeInFrontier)
{
SearchNode <State, Action> searchNode = CreateSearchNode(node, action, child, toState);
if (frontierNode.f > searchNode.f)
{
#if UNITY_IPHONE
frontier.Replace(frontierNode, new Float(frontierNode.f), new Float(searchNode.f));
#else
frontier.Replace(frontierNode, frontierNode.f, searchNode.f);
#endif
}
}
}
}
return(null);
}
/// <summary>
/// Construct a plan from the initial state to the goal using regressive
/// A* with variable unification.
/// </summary>
/// <param name="initial">Current perceived state of the world.</param>
/// <param name="goal">Desired state of the world.</param>
public void execute(SearchNode initial, SearchNode goal)
{
#if DEBUG
Stopwatch clock = new Stopwatch();
clock.Start();
#endif
List<SearchNode> openlist = new List<SearchNode>();
List<int> failedConditions = new List<int>();
SearchNode current = goal; // BACKWARDS, START WITH GOAL
// perform search
while (true)
{
current.unifiesWith(initial, failedConditions);
// If current state and initial states unify,
// we have found a complete plan, so finish
if (failedConditions.Count == 0)
{
break;
}
// Otherwise, we go through our action map looking up actions which
// fulfill all of the ways in which the current state didn't unify
// with the initial, and try those actions.
for (int i = 0; i < failedConditions.Count; i++)
{
List<ActionType> solutions = actionMap_[failedConditions[i]];
for (int j = 0; j < solutions.Count; j++)
{
if (solutions[j].testPreConditions(current))
{
SearchNode predecessor = solutions[j].unifyRegressive(ref current);
predecessor.fValue = predecessor.cost + predecessor.dist(initial);
openlist.Add(predecessor);
}
}
}
// No search states left in the open list, so no plan
// could be found
if (openlist.Count == 0)
{
current = null;
break;
}
// Get the cheapest search state and try from there
// TODO
// Might be worth it to profile this against a data structure
// with O(log n) insertion but no necessary sort (ie sorted list)
openlist.Sort(current);
current = openlist[0];
openlist.RemoveAt(0);
}
// Reconstruct the generated plan; plan will be empty if we failed.
// Therefore, an empty plan could have two meanings:
// 1. Plan failed, so we need to switch goals to try another.
// 2. Initial state satisfies the current goal, so we should try to
// satisfy a different goal anyway.
// This small amount of ambiguity helps reduce (plan == null) errors.
List<Action> plan = new List<Action>();
while (current != null && current.action != null)
{
plan.Add(current.action);
current = current.next;
}
// Note: the current.action != null check prevents a bug in the situation
// where the initial state fulfills the goal state
plan_ = plan;
#if DEBUG
clock.Stop();
Console.WriteLine(
"Planner " +
Convert.ToString(plan_.Count) +
": " +
Convert.ToString(clock.ElapsedMilliseconds)
);
#endif
}
/// <summary>
/// Finds the shortest path (if any exist) between two nodes in a graph.
/// </summary>
/// <param name="start">The start node.</param>
/// <param name="destination">The destination node.</param>
/// <param name="length">The length of the path.</param>
/// <returns>The nodes in the order they need to be visited to get from the start node to the destination node.</returns>
public IEnumerable <TNode> FindShortestPath(TNode start, TNode destination, out double length)
{
openList = new SortedSet <SearchNode>(this);
closedList = new SortedSet <SearchNode>(this);
SearchNode node = new SearchNode()
{
Node = start, CostFromSource = 0, CostToDestination = estimateDistance(start, destination)
};
openList.Add(node);
while (openList.Count > 0)
{
node = openList.First();
openList.Remove(node);
if (node.Node.Equals(destination))
{
length = node.CostFromSource;
LinkedList <TNode> path = new LinkedList <TNode>();
while (node != null)
{
path.AddFirst(node.Node);
node = node.Parent;
}
return(path);
}
foreach (TNode neighbor in getNeighbors(node.Node))
{
double costFromSource = node.CostFromSource + getDistance(node.Node, neighbor);
double costToDestination = estimateDistance(neighbor, destination);
double totalCost = costFromSource + costToDestination;
SearchNode existing = openList.FirstOrDefault(n => n.Node.Equals(neighbor));
if (existing != null)
{
if (totalCost < existing.TotalCost)
{
openList.Remove(existing);
existing.CostFromSource = costFromSource;
existing.CostToDestination = costToDestination;
existing.Parent = node;
openList.Add(existing);
}
}
else
{
existing = closedList.FirstOrDefault(n => n.Node.Equals(neighbor));
if (existing != null)
{
if (totalCost < existing.TotalCost)
{
existing.CostFromSource = costFromSource;
existing.CostToDestination = costToDestination;
existing.Parent = node;
UpdateChildren(existing);
}
}
else
{
openList.Add(new SearchNode()
{
Node = neighbor, Parent = node, CostFromSource = costFromSource, CostToDestination = costToDestination
});
}
}
}
closedList.Add(node);
}
length = -1;
return(null);
}
/// returns true if the destination node is found
private bool sample(SearchNode node, int x, int y, int cost)
{
// make sure x,y are in bounds and that the location is not an obstacle
if (x >= 0 && x < gridLengthX && y >= 0 && y < gridLengthY && grid[y,x])
{
string key = getKey(x, y);
// ignore nodes in the closed list
if (closedList.ContainsKey(key))
{
return false;
}
// check if the node is already in the openlist
else if (openList.ContainsKey(key))
{
SearchNode fromOpenList = openList[key];
// update the g-score of the encountered node and resort the list if necessary
if (fromOpenList.updateGScore(node, cost))
{
sortOpenList();
}
return false;
}
// we had better make a new node now and add it to the lists
SearchNode newNode = new SearchNode(node, x, y, node.G + cost, getHeuristicFrom(x, y));
openList.Add(key, newNode);
sortedOpenList.Add(newNode);
// now sort the sorted list so we keep it like a priority q
sortOpenList();
// we may have just created our end node, so build the soltution path if we can
if (x == endX && y == endY)
{
buildSolution(newNode);
return true;
}
}
return false;
}
void OnDrawGizmos()
{
if (Inc)
{
DebugI++;
Inc = false;
}
Tri = -1;
if (!gen())
{
; // return;
}
Gizmos.color = Color.white;
foreach (Node n in Nodes)
{
drawCross(n.Centre);
for (int i = 3; i-- > 0;)
{
var o = n.Nbrs[i];
if (o != null)
{
Gizmos.DrawLine(n.Centre, o.Centre);
}
}
}
if (Nodes.Count == 0 || !Obj1 || !Obj2)
{
return;
}
Gizmos.color = Color.red;
Node a = findNode(Obj1.position), b = findNode(Obj2.position);
if (a == null || b == null)
{
return;
}
SortedList <float, SearchNode> search = new SortedList <float, SearchNode>(new DuplicateKeyComparer <float>());
Vector2 desP = Obj2.position, strtP = Obj1.position;
var cur = new SearchNode();
cur.D = 0;
cur.N = a;
cur.Prev = null;
cur.P = Obj1.position;
cur.N.SN = cur;
float cD = float.MaxValue;
SearchNode cP = null;
for (; ;)
{
var n = cur.N;
for (int i = 3; i-- > 0;)
{
var o = n.Nbrs[i];
if (o == null)
{
continue;
}
if (o.SN != null)
{
// var d = cur.D + (cur.P - sn.P).magnitude;
continue;
}
// Gizmos.DrawLine(cur.P, n.Edge[i] );
var sn = new SearchNode();
sn.P = n.Edge[i];
sn.D = cur.D + (cur.P - sn.P).magnitude;
sn.N = o;
sn.Prev = cur;
sn.N.SN = sn;
sn.NbrI = i;
var d = sn.D + (desP - sn.P).magnitude;
if (d > cD)
{
continue;
}
if (o == b)
{
cD = d;
cP = sn;
// Gizmos.DrawLine(sn.P, desP);
continue;
}
search.Add(d, sn);
}
if (search.Count == 0)
{
break;
}
cur = search.Values[search.Count - 1];
if (search.Keys[search.Count - 1] > cD)
{
break;
}
search.RemoveAt(search.Count - 1);
}
if (cP == null)
{
return;
}
List <SmoothNode> smooth = new List <SmoothNode>();
//Gizmos.DrawLine( desP, cP.P );
SmoothNode smt = new SmoothNode();
smt.P = desP;
smooth.Add(smt);
for (; ;)
{
var n = cP.Prev;
if (n != null)
{
//Gizmos.DrawLine( n.P, cP.P );
// Gizmos.DrawLine(Verts[ n.N.Vi[ cP.NbrI ] ],Verts[ n.N.Vi[ (cP.NbrI+2)%3 ] ] );
smt = new SmoothNode();
smt.P = cP.P;
smt.E1 = Verts[n.N.Vi[cP.NbrI]];
smt.E2 = Verts[n.N.Vi[(cP.NbrI + 2) % 3]];// -smt.E1;
smooth.Add(smt);
cP = n;
}
else
{
//Gizmos.DrawLine( strtP, cP.P );
smt = new SmoothNode();
smt.P = strtP;
smooth.Add(smt);
break;
}
}
Gizmos.color = Color.red;
/*
* for(int iter =10; iter-->0;) {
* // break;
* for(int i = smooth.Count-1;--i > 0;) {
* smt = smooth[i];
*
*
* smt.P = (smt.P*0 + smooth[i+1].P+ smooth[i-1].P )*0.5f;
*
* //var op = smt.P;
*
* // Util.lineLineIntersection( smooth[i+1].P, smooth[i-1].P, smt.E1, smt.E1 + smt.E2, ref smt.P );
*
* var vec = smt.P - smt.E1;
* var dt = Vector2.Dot( vec, smt.E2 ); dt /= smt.E2.sqrMagnitude;
*
*
*
*
* dt = Mathf.Clamp01( dt );
* // Gizmos.DrawLine( smt.E1, smt.E1 + smt.E2 );
*
* // Gizmos.DrawLine(smooth[i + 1].P, smooth[i - 1].P);
* smt.P =smt.E1 + smt.E2*dt;
*
* // Gizmos.DrawLine(smt.P, op );
* break;
* }
* break;
* }
*
* Gizmos.color = Color.green;
* for(int i = smooth.Count-1;i-- > 0;) {
* Gizmos.DrawLine( smooth[i+1].P, smooth[i].P );
* } */
Vector2 vec, fnlA, fnlB;
Vector2 tPos = smooth[0].P, cPos = smooth[smooth.Count - 1].P;
int CurNode = smooth.Count - 2;
// tPos = smooth[CurNode].P;
fnlA = smooth[CurNode].E1;
fnlB = smooth[CurNode].E2;
float sgn = -1;
for (int ci = CurNode - 1; ; ci--)
{
if (ci <= 0)
{
break;
}
//Vector2 tPos = smooth[CurNode].P;
Vector2 fnlA2 = smooth[ci].E1;
Vector2 fnlB2 = smooth[ci].E2;
if ((fnlA2 - fnlA).sqrMagnitude > (fnlB2 - fnlB).sqrMagnitude)
{
Debug.DrawLine(cPos, fnlA2, Color.black);
sgn = Util.sign(fnlA2, cPos, fnlA);
if (Util.sign(fnlA2, cPos, fnlA) > 0)
{
if (Util.sign(fnlB, cPos, fnlA) < 0)
{
tPos = fnlB;
break;
}
fnlA = fnlA2;
}
else
{
// tPos = fnlA;
// break;
}
}
else
{
Debug.DrawLine(cPos, fnlB2, Color.black);
sgn = Util.sign(fnlB, cPos, fnlB2);
if (Util.sign(fnlB, cPos, fnlB2) > 0)
{
if (Util.sign(fnlB, cPos, fnlA) < 0)
{
// tPos = fnlA;
// break;
}
fnlB = fnlB2;
}
else
{
// tPos = fnlB;
// break;
}
}
}
if (Util.sign(fnlB, cPos, tPos) < 0)
{
tPos = fnlB;
}
if (Util.sign(tPos, cPos, fnlA) < 0)
{
tPos = fnlA;
}
// Debug.Log("sgn " + sgn);
Debug.DrawLine(cPos, fnlA, Color.green);
Debug.DrawLine(cPos, fnlB, Color.red);
Debug.DrawLine(cPos, tPos, Color.white);
}
internal override bool testPreConditions(SearchNode node)
{
throw new NotImplementedException();
}
public AStar_GoalBounding(SearchNode start, SearchNode end, SearchNode[,] nodes, float weight, float showTime)
: base(start, end, nodes, weight, showTime)
{
}
internal GoalTeamwork(AI ai)
: base(ai)
{
// This will be replaced by the TeamPlanner using setNode()
node_ = new SearchNode();
}
public SearchNode(SearchNode parent, int city)
{
Parent = parent;
City = city;
}
public SearchNode(SectorNode sn, SearchNode parent, int g, Point3D goal)
{
SectorNode = sn;
Parent = parent;
G = g;
Score = g + Heuristic(goal);
}
public static bool SearchConvertPath(Type source, Type target, out SearchResult searchResult)
{
if (source.IsEnum)
{
source = GetEmumType(source);
}
if (target.IsEnum)
{
target = GetEmumType(target);
}
searchResult = default;
if (!s_map.TryGetValue(source, out ConcurrentDictionary <Type, ConvertItem> value) || value.Keys.Count == 0)
{
return(false);
}
List <SearchNode> nodes = new List <SearchNode>();
List <Type> logs = new List <Type>();
foreach (KeyValuePair <Type, ConvertItem> kv in value.ToArray().OrderByDescending(a => a.Value.Order))
{
int lost = GetLost(source, kv.Key);
nodes.Add(new SearchNode()
{
ParentIndex = -1,
Target = kv.Key,
ConvertItem = kv.Value,
ConsumptionWeight = kv.Value.ConsumptionWeight,
Inherit = lost > 0 ? kv.Key : source,
LostWeight = lost,
Deep = 1
});
}
int currentDeep = 1;
for (int i = 0; i < nodes.Count; i++)
{
SearchNode current = nodes[i];
if (searchResult != null)
{
if (searchResult.LostWeight < current.LostWeight)
{
continue;
}
if (searchResult.LostWeight == current.LostWeight)
{
if (searchResult.Length < current.Deep)
{
continue;
}
if (searchResult.Length == current.Deep)
{
if (searchResult.ConsumptionWeight <= current.ConsumptionWeight)
{
continue;
}
}
}
}
if (current.Target == target)
{
KeyValuePair <Type, ConvertItem>[] convertItems = new KeyValuePair <Type, ConvertItem> [current.Deep];
SearchNode t = current;
for (int k = 0; k < convertItems.Length; k++)
{
convertItems[convertItems.Length - k - 1] = new KeyValuePair <Type, ConvertItem>(t.Target, t.ConvertItem);
if (t.ParentIndex >= 0)
{
t = nodes[t.ParentIndex];
}
}
searchResult = new SearchResult()
{
ConsumptionWeight = current.ConsumptionWeight,
LostWeight = current.LostWeight,
Items = convertItems
};
}
else
{
if (s_map.TryGetValue(current.Target, out ConcurrentDictionary <Type, ConvertItem> cur))
{
int deep = current.Deep + 1;
if (currentDeep < deep)
{
logs.AddRange(nodes.Where(x => x.Deep == currentDeep).Select(y => y.Target));
currentDeep = deep;
}
foreach (KeyValuePair <Type, ConvertItem> kv in cur.ToArray().OrderByDescending(a => a.Value.Order))
{
if (logs.Contains(kv.Key))
{
continue;
}
int lost = GetLost(current.Inherit, kv.Key);
nodes.Add(new SearchNode()
{
ParentIndex = i,
Target = kv.Key,
ConvertItem = kv.Value,
ConsumptionWeight = current.ConsumptionWeight + kv.Value.ConsumptionWeight,
Inherit = lost > 0 ? current.Inherit : kv.Key,
LostWeight = current.LostWeight + lost,
Deep = deep
});
}
}
}
}
return(searchResult != default);
}
public bool ContainsMatch(SearchNode sn)
{
return IndexOf(sn) != -1;
}
private LinkedList <Waypoint> doDijkstraSearch()
{
Graph <Waypoint> graph = GraphBuilder.Graph;
//Create a search list (a sorted linked list) of search nodes (I provided a SearchNode class, which you should instantiate with Waypoint. I also provided a SortedLinkedList class)
SortedLinkedList <SearchNode <Waypoint> > searchList = new SortedLinkedList <SearchNode <Waypoint> >();
//Create a dictionary of search nodes keyed by the corresponding graph node.This dictionary gives us a very fast way to determine if the search node corresponding to a graph node is still in the search list
Dictionary <GraphNode <Waypoint>, SearchNode <Waypoint> > searchNodeMap = new Dictionary <GraphNode <Waypoint>, SearchNode <Waypoint> >();
//Save references to the start and end graph nodes in variables
start = GraphBuilder.Start;
end = GraphBuilder.End;
GraphNode <Waypoint> startGraphNode = graph.Find(start);
GraphNode <Waypoint> endGraphNode = graph.Find(end);
//For each graph node in the graph
foreach (GraphNode <Waypoint> graphNode in graph.Nodes)
{
//Create a search node for the graph node (the constructor I provided in the SearchNode class initializes distance to the max float value and previous to null)
SearchNode <Waypoint> searchNode = new SearchNode <Waypoint>(graphNode);
//If the graph node is the start node
if (graphNode == startGraphNode)
{
// Set the distance for the search node to 0
searchNode.Distance = 0;
}
//Add the search node to the search list
searchList.Add(searchNode);
//Add the search node to the dictionary keyed by the graph node
searchNodeMap.Add(graphNode, searchNode);
}
//While the search list isn't empty
while (searchList.Count > 0)
{
//Save a reference to the current search node(the first search node in the search list) in a variable
SearchNode <Waypoint> currentSearchNode = searchList.First.Value;
//Remove the first search node from the search list
searchList.RemoveFirst();
//Save a reference to the current graph node for the current search node in a variable
GraphNode <Waypoint> currentGraphNode = currentSearchNode.GraphNode;
//Remove the search node from the dictionary(because it's no longer in the search list)
searchNodeMap.Remove(currentGraphNode);
//If the current graph node is the end node
if (currentGraphNode == endGraphNode)
{
//Display the distance for the current search node as the path length in the scene(Hint: I used the HUD and the event system to do this)
pathFoundEvent.Invoke(currentSearchNode.Distance);
//Return a linked list of the waypoints from the start node to the end node(Hint: The lecture code for the Searching a Graph lecture builds a linked list for a path in the ConvertPathToString method)
LinkedList <Waypoint> pathLinkedList = new LinkedList <Waypoint>();
while (currentSearchNode != null)
{
GraphNode <Waypoint> graphNode = currentSearchNode.GraphNode;
Waypoint waypoint = graphNode.Value;
pathLinkedList.AddFirst(waypoint);
currentSearchNode = currentSearchNode.Previous;
}
return(pathLinkedList);
}
//For each of the current graph node's neighbors
foreach (GraphNode <Waypoint> neighbour in currentGraphNode.Neighbors)
{
//If the neighbor is still in the search list(use the dictionary to check this)
SearchNode <Waypoint> neighbourSearchNode = null;
if (searchNodeMap.TryGetValue(neighbour, out neighbourSearchNode))
{
//Save the distance for the current graph node + the weight of the edge from the current graph node to the current neighbor in a variable
float distanceToStart = currentSearchNode.Distance + currentGraphNode.GetEdgeWeight(neighbour);
//If the distance you just calculated is less than the current distance for the neighbor search node(Hint: You can retrieve the neighbor search node from the dictionary using the neighbor graph node)
if (distanceToStart < neighbourSearchNode.Distance)
{
//Set the distance for the neighbor search node to the new distance
neighbourSearchNode.Distance = distanceToStart;
//Set the previous node for the neighbor search node to the current search node
neighbourSearchNode.Previous = currentSearchNode;
//Tell the search list to Reposition the neighbor search node. We need to do this because the change to the distance for the neighbor search node could have moved it forward in the search list
searchList.Reposition(neighbourSearchNode);
}
}
}
}
return(null);
}
public void Adjust(SearchNode sn)
{
int index = IndexOf(sn);
if (index == -1)
return;
m_Heap[index] = sn;
int parent = index;
do
{
index = parent;
PercolateDown(index);
parent = (index - 1) / 2;
} while (parent >= 0 && m_Heap[parent].Score > m_Heap[index].Score);
}
// Advances search one step
public SearchState SearchStep()
{
// Next I want it to be safe to do a searchstep once the search has succeeded...
if ((searchState == SearchState.SUCCEEDED) || (searchState == SearchState.FAILED))
{
return(searchState);
}
// Failure is defined as emptying the open list as there is nothing left to
// search...
// New: Allow user abort
if (openNodes.Count == 0 || cancelRequested)
{
FreeUnusedNodes();
searchState = SearchState.FAILED;
return(searchState);
}
// Incremement step count
searchSteps++;
// removing the front will not change the sort order
SearchNode node = openNodes[0];
openNodes.RemoveAt(0);
// Check for the goal, once we pop that we're done
if (node.position.Compare(endNode.position))
{
// The user is going to use the Goal Node he passed in
// so copy the parent pointer of n
endNode.prevNode = node.prevNode;
// A special case is that the goal was passed in as the start state
// so handle that here
if (node.position.Compare(startNode.position) == false)
{
// set the child pointers in each node (except Goal which has no child)
SearchNode nextNode = endNode;
SearchNode prevNode = endNode.prevNode;
do
{
prevNode.nextNode = nextNode;
nextNode = prevNode;
prevNode = prevNode.prevNode;
}while(nextNode != startNode); // Start is always the first node by definition
}
// delete nodes that aren't needed for the solution
FreeUnusedNodes();
searchState = SearchState.SUCCEEDED;
return(searchState);
}
else // not goal
{
successors.Clear(); // empty vector of successor nodes to n
node.position.FindSuccessors(this, node.prevNode != null ? node.prevNode.position : null, allowDiagonal);
// Now handle each successor to the current node ...
foreach (SearchNode successor in successors)
{
// The g value for this successor ...
float new_g = node.g + node.position.GetCost();
// Now we need to find whether the node is on the open or closed lists
// If it is but the node that is already on them is better (lower g)
// then we can forget about this successor
// First linear search of open list to find node
int openlist_result = 0;
foreach (SearchNode open in openNodes)
{
if (open.position.Compare(successor.position))
{
break;
}
openlist_result++;
}
if (openlist_result != openNodes.Count)
{
// we found this state on open
if (openNodes[openlist_result].g <= new_g)
{
// the one on Open is cheaper than this one
continue;
}
}
int closedlist_result = 0;
foreach (SearchNode closed in closedNodes)
{
if (closed.position.Compare(successor.position))
{
break;
}
closedlist_result++;
}
if (closedlist_result != closedNodes.Count)
{
// we found this state on closed
if (closedNodes[closedlist_result].g <= new_g)
{
// the one on Closed is cheaper than this one
continue;
}
}
// This node is the best node so far
// so lets keep it and set up its AStar specific data ...
successor.prevNode = node;
successor.g = new_g;
successor.h = successor.position.EstimatedDistanceToNode(endNode.position);
successor.f = successor.g + successor.h;
// Remove successor from closed if it was on it
if (closedlist_result != closedNodes.Count)
{
// remove it from Closed
closedNodes.RemoveAt(closedlist_result);
}
// Update old version of this node
if (openlist_result != openNodes.Count)
{
openNodes.RemoveAt(openlist_result);
}
InsertSorted(openNodes, successor);
}
// push n onto Closed, as we have expanded it now
closedNodes.Add(node);
} // end else (not goal so expand)
return(searchState); // Succeeded bool is false at this point.
}
public static Point3D[] FindWaypoints(Mobile m, Map map, Point3D start, Point3D goal)
{
DateTime s = DateTime.Now;
if (m_Nodes == null)
return null; // sanity check
if (!SameIsland(start, goal))
return null;
if (!Utility.InRange(start, goal, 512))
return null;
SearchNodeHeap open = new SearchNodeHeap();
ArrayList closed = new ArrayList();
SectorNode goalnode = m_Nodes[goal.X >> Map.SectorShift, goal.Y >> Map.SectorShift];
open.Add(new SearchNode(m_Nodes[start.X >> Map.SectorShift, start.Y >> Map.SectorShift], null, 0, goal));
while (open.Count > 0)
{
SearchNode curnode = open.Pop();
closed.Add(curnode);
if (curnode.SectorNode == goalnode)
break;
for (int i = 0; i < curnode.SectorNode.NumLinks; i++)
{
SearchNode newnode = new SearchNode(curnode.SectorNode.Links[i], curnode, curnode.G + curnode.SectorNode.Distances[i], goal);
if (closed.Contains(newnode))
continue;
SearchNode existing = open.FindMatch(newnode);
if (existing == null)
open.Add(newnode);
else if (newnode.G < existing.G)
open.Adjust(newnode);
// else skip
}
}
SearchNode sn = (SearchNode)closed[closed.Count - 1];
if (sn.SectorNode == goalnode)
{
Stack nodes = new Stack();
while (sn != null)
{
closed.Remove(sn);
nodes.Push(sn);
sn = sn.Parent;
}
Point3D[] points = new Point3D[nodes.Count + 1];
for (int i = 0; i < points.Length - 1; i++)
points[i] = ((SearchNode)nodes.Pop()).SectorNode.Point;
points[points.Length - 1] = goal;
return points;
}
return null;
}
void Start()
{
Node = (initialNode == null) ? SearchNode.GetRandomSearchNode() : initialNode;
}
internal override SearchNode unifyRegressive(ref SearchNode node)
{
Belief target = character_.AI_.Memory_.getFirstBelief(BeliefType.BestTarget);
SearchNode parent = node.getPredecessor();
parent.action = new ActionAttackRanged(character_, target.handle_);
parent.cost += COST;
parent.setInt(Variable.TargetHealth, node.values[Variable.TargetHealth].i + 1);
parent.setBool(Variable.Ammo, true);
return parent;
}
public SearchNode(SearchNode parent, int rotation, IntVector3 pos)
{
this.parent = parent;
this.pos = pos;
this.rotation = rotation;
}
// Private Functions //
// ExpandCluster //
// Requires and initial search node and a list of its neighbours, along with full list of search nodes for expansion //
// Returns a Cluster Class object - which deals with its own variables //
private Cluster ExpandCluster( SearchNode initialPoint, List< SearchNode > neighbours, SearchNode[] nodesToBeSearched )
{
// Create new cluster
Cluster cluster = new Cluster();
// Flag and add initial point to cluster
initialPoint.SetAsPartOfCluster( true );
cluster.AddSearchNode( initialPoint );
// Loop through passed neighbour points
for( int i = 0; i < neighbours.Count; i++ ) {
// If it has NOT been visited
if( !neighbours[ i ].HasBeenVisited() ) {
// Now flag as visited
neighbours[ i ].SetVisited( true );
// Get neighbours of each point
List< SearchNode > localNeighbours = GetNeighbours( neighbours[ i ], nodesToBeSearched );
// If this point has enough neighbours
if( localNeighbours.Count >= mMinimumPoints ) {
for( int j = 0; j < localNeighbours.Count; j++ ) {
// If this point isnt already a neighbour or has NOT been previously visited
// Add it as a brand new neighbour
//if( ( !localNeighbours[ j ].IsNeighbour() ) && ( !localNeighbours[ j ].HasBeenVisited() ) ) {
if( !localNeighbours[ j ].HasBeenVisited() ) {
// Add ONLY add localNeighbours that are 'joined' with nodes in neighbours
if( CheckIfJoined( localNeighbours[ j ], neighbours ) ) {
neighbours.Add( localNeighbours[ j ] );
}
}
}
// If this point is not part of a cluster - flag and add it!
if( !neighbours[ i ].PartOfCluster() ) {
neighbours[ i ].SetAsPartOfCluster( true );
cluster.AddSearchNode( neighbours[ i ] );
}
}
}
}
// Return final cluster
return cluster;
}
private bool CloseCircuit(HashSet <TrackNode> nodes, IntVector3 start, int startRotation)
{
// use A* to close track
// Track ends at (0, 0, 0) rotation 0
// Start from where cursor left off
int endRotation = 0;
IntVector3 end = new IntVector3();
HashSet <SearchNode> closed = new HashSet <SearchNode>();
LinkedList <SearchNode> open = new LinkedList <SearchNode>();
SearchNode first = new SearchNode(null, startRotation, start);
first.g = 0;
first.h = start.dst(ref end);
open.AddLast(first);
int countdown = A_STAR_TIMEOUT * length;
HashSet <IntVector3> currentPathPositions = new HashSet <IntVector3>();
while (open.Count > 0)
{
if (countdown-- < 0)
{
return(false);
}
SearchNode current = open.First.Value;
open.RemoveFirst();
if (closed.Contains(current))
{
continue;
}
closed.Add(current);
if (current.pos.Equals(end) && current.rotation == endRotation)
{
FinalizeCloseCircuit(nodes, current);
return(true);
}
currentPathPositions.Clear();
SearchNode tmp = current;
while (tmp != null)
{
currentPathPositions.Add(tmp.pos);
tmp = tmp.parent;
}
List <SearchNode> neighbours = GetNeighboursFor(nodes, current);
foreach (SearchNode neighbour in neighbours)
{
if (closed.Contains(neighbour) || currentPathPositions.Contains(neighbour.pos))
{
continue;
}
neighbour.g = current.g; //+ 1;
neighbour.h = neighbour.pos.dst(ref end);
AddOrdered(open, neighbour);
}
}
return(false);
}
// DBSCAN //
// Density-based spatial clustering of applications with noise //
// http://en.wikipedia.org/wiki/DBSCAN //
public void DBSCAN( SearchNode[] nodesToBeSearched )
{
// Clear lists
mClusters.Clear();
mNoise.Clear();
// Zero length array safeguard
if( nodesToBeSearched.Length > 0 ) {
// DBSCAN Main Loop //
for( int i = 0; i < nodesToBeSearched.Length; i++ ) {
// For each point NOT visited
if( !nodesToBeSearched[ i ].HasBeenVisited() ) {
// Flag search node as visited
nodesToBeSearched[ i ].SetVisited( true );
// Get list of neighbours for this search node
List< SearchNode > neighbours = GetNeighbours( nodesToBeSearched[ i ], nodesToBeSearched );
// If the neighbours lists count is under mMinimumPoints
// It is NOT part of a cluster and therefore NOISE
if( neighbours.Count < mMinimumPoints ) {
nodesToBeSearched[ i ].mIsNoise = true;
mNoise.Add( nodesToBeSearched[ i ] );
// Else is has sufficent neighbours to be part of a cluster
// Send this search node and neighbours to begin ExpandCluster
// Returns the full cluster which is added to mClusters list
} else {
mClusters.Add( ExpandCluster( nodesToBeSearched[ i ], neighbours, nodesToBeSearched ) );
}
}
}
// Rogue Nodes
for( int i = 0; i < nodesToBeSearched.Length; i++ ) {
if( ( !nodesToBeSearched[ i ].PartOfCluster() ) && ( !nodesToBeSearched[ i ].mIsNoise ) ) {
//Debug.Log( "Rogue NOISE at - " + nodesToBeSearched[ i ].transform.position );
nodesToBeSearched[ i ].mIsNoise = true;
mNoise.Add( nodesToBeSearched[ i ] );
}
}
// Number clusters
for( int i = 0; i < mClusters.Count; i++ ) {
mClusters[ i ].mClusterNumber = i;
mClusters[ i ].SetClusterNumber();
mClusters[ i ].CalculateCenter();
}
}
}
/// <summary>
/// Finds the optimal path from one point to another.
/// </summary>
public List <Vector2> FindPath(Vector2 startPoint, Vector2 endPoint)
{
// Only try to find a path if the start and end points are different.
if (startPoint == endPoint)
{
return(new List <Vector2>());
}
/////////////////////////////////////////////////////////////////////
// Step 1 : Clear the Open and Closed Lists and reset each node’s F
// and G values in case they are still set from the last
// time we tried to find a path.
/////////////////////////////////////////////////////////////////////
ResetSearchNodes();
// Store references to the start and end nodes for convenience.
SearchNode startNode = searchNodes[(int)startPoint.x, (int)startPoint.y];
SearchNode endNode = searchNodes[(int)endPoint.x, (int)endPoint.y];
/////////////////////////////////////////////////////////////////////
// Step 2 : Set the start node’s G value to 0 and its F value to the
// estimated distance between the start node and goal node
// (this is where our H function comes in) and add it to the
// Open List.
/////////////////////////////////////////////////////////////////////
startNode.InOpenList = true;
startNode.DistanceToGoal = Heuristic(startPoint, endPoint);
startNode.DistanceTraveled = 0;
openList.Add(startNode);
/////////////////////////////////////////////////////////////////////
// Setp 3 : While there are still nodes to look at in the Open list :
/////////////////////////////////////////////////////////////////////
while (openList.Count > 0)
{
/////////////////////////////////////////////////////////////////
// a) : Loop through the Open List and find the node that
// has the smallest F value.
/////////////////////////////////////////////////////////////////
SearchNode currentNode = FindBestNode();
/////////////////////////////////////////////////////////////////
// b) : If the Open List empty or no node can be found,
// no path can be found so the algorithm terminates.
/////////////////////////////////////////////////////////////////
if (currentNode == null)
{
break;
}
/////////////////////////////////////////////////////////////////
// c) : If the Active Node is the goal node, we will
// find and return the final path.
/////////////////////////////////////////////////////////////////
if (currentNode == endNode)
{
// Trace our path back to the start.
return(FindFinalPath(startNode, endNode));
}
/////////////////////////////////////////////////////////////////
// d) : Else, for each of the Active Node’s neighbours :
/////////////////////////////////////////////////////////////////
for (int i = 0; i < currentNode.Neighbors.Length; i++)
{
SearchNode neighbor = currentNode.Neighbors[i];
//////////////////////////////////////////////////
// i) : Make sure that the neighbouring node can
// be walked across.
//////////////////////////////////////////////////
if (neighbor == null || neighbor.Walkable == false)
{
continue;
}
//////////////////////////////////////////////////
// ii) Calculate a new G value for the neighbouring node.
//////////////////////////////////////////////////
float distanceTraveled = currentNode.DistanceTraveled + 1;
// An estimate of the distance from this node to the end node.
float heuristic = Heuristic(neighbor.Position, endPoint);
//////////////////////////////////////////////////
// iii) If the neighbouring node is not in either the Open
// List or the Closed List :
//////////////////////////////////////////////////
if (neighbor.InOpenList == false && neighbor.InClosedList == false)
{
// (1) Set the neighbouring node’s G value to the G value we just calculated.
neighbor.DistanceTraveled = distanceTraveled;
// (2) Set the neighbouring node’s F value to the new G value + the estimated
// distance between the neighbouring node and goal node.
neighbor.DistanceToGoal = distanceTraveled + heuristic;
// (3) Set the neighbouring node’s Parent property to point at the Active Node.
neighbor.Parent = currentNode;
// (4) Add the neighbouring node to the Open List.
neighbor.InOpenList = true;
openList.Add(neighbor);
}
//////////////////////////////////////////////////
// iv) Else if the neighbouring node is in either the Open
// List or the Closed List :
//////////////////////////////////////////////////
else if (neighbor.InOpenList || neighbor.InClosedList)
{
// (1) If our new G value is less than the neighbouring
// node’s G value, we basically do exactly the same
// steps as if the nodes are not in the Open and
// Closed Lists except we do not need to add this node
// the Open List again.
if (neighbor.DistanceTraveled > distanceTraveled)
{
neighbor.DistanceTraveled = distanceTraveled;
neighbor.DistanceToGoal = distanceTraveled + heuristic;
neighbor.Parent = currentNode;
}
}
}
/////////////////////////////////////////////////////////////////
// e) Remove the Active Node from the Open List and add it to the
// Closed List
/////////////////////////////////////////////////////////////////
openList.Remove(currentNode);
currentNode.InClosedList = true;
}
// No path could be found.
return(new List <Vector2>());
}
