Skip to content

Instantly share code, notes, and snippets.

@redxdev
Last active February 4, 2018 23:01
Show Gist options
  • Save redxdev/5a94088ea8bc69d7bec9bae008efae56 to your computer and use it in GitHub Desktop.
Save redxdev/5a94088ea8bc69d7bec9bae008efae56 to your computer and use it in GitHub Desktop.
Old C# pathfinder
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Navier_Boats.Engine.Level;
namespace Navier_Boats.Engine.Pathfinding
{
public static class Heuristics
{
// manhattan distance
public static float Manhattan(Vector2 current, Vector2 end, float resistance)
{
float inverse = resistance == 0 ? 1 : 1f / resistance;
return (Math.Abs(current.X - end.X) + Math.Abs(current.Y - end.Y)) * inverse;
}
// real distance
// WARNING: very slow due to square root
public static float Distance(Vector2 current, Vector2 end, float resistance)
{
float inverse = resistance == 0 ? 1 : 1f / resistance;
return Vector2.Distance(current, end) * inverse;
}
// squared distance
public static float DistanceSquared(Vector2 current, Vector2 end, float resistance)
{
float inverse = resistance == 0 ? 1 : 1f / resistance;
return Vector2.DistanceSquared(current, end) * inverse;
}
// Dijkstra's, or: let's be lazy and return 0
// Do not actually use this one, as it simply flood-fills everything until it finds the end node.
// There are uses for this heuristic, but the current pathfinder implementation does not lend itself
// to them.
public static float Dijkstras(Vector2 current, Vector2 end, float resistance)
{
return 0;
}
public static Pathfinder.Heuristic MultiplyResistance(float multiplier, Pathfinder.Heuristic heuristic)
{
return (current, end, resistance) =>
{
return heuristic(current, end, resistance * multiplier);
};
}
public static Pathfinder.Heuristic ResistanceRandomness(double min, double max, Pathfinder.Heuristic heuristic)
{
return (current, end, resistance) =>
{
float res = (float)(CurrentLevel.GetRandom().NextDouble() * (max - min) + min) + resistance;
return heuristic(current, end, res);
};
}
}
}
using System;
using System.Collections.Generic;
using System.Collections.Concurrent;
using System.Linq;
using System.Text;
using System.Diagnostics;
using Microsoft.Xna.Framework;
using Navier_Boats.Engine.Level;
using Navier_Boats.Engine.Entities;
namespace Navier_Boats.Engine.Pathfinding
{
// if you want to use threading with this, use the PathThread class
public class Pathfinder
{
public delegate float Heuristic(Vector2 current, Vector2 end, float resistance);
public static float GetTileWalkSpeed(short collisionData)
{
switch(collisionData)
{
case 0:
return Entity.ROAD_SPEED_MULT;
case 1:
return Entity.GRASS_SPEED_MULT;
case 2:
return Entity.SAND_SPEED_MULT;
case 3:
return Entity.WATER_SPEED_MULT;
}
return float.PositiveInfinity;
}
private ConcurrentDictionary<Vector2, SearchNode> searchNodes = new ConcurrentDictionary<Vector2, SearchNode>();
private List<SearchNode> openList = new List<SearchNode>();
private List<SearchNode> closedList = new List<SearchNode>();
private CurrentLevel level = null;
public ConcurrentDictionary<Vector2, SearchNode> SearchNodes
{
get
{
return searchNodes;
}
}
public Stopwatch Timer
{
get;
protected set;
}
public Pathfinder(CurrentLevel level)
{
this.level = level;
this.Timer = new Stopwatch();
}
public PathNode QueryPosition(Vector2 position)
{
PathNode node = new PathNode();
node.Position = position;
short tileData = this.level.GetTileDataAtPoint(TileLayer.COLLISION_LAYER, position);
node.Walkable = GetTileWalkSpeed(tileData) != float.PositiveInfinity;
node.Resistance = GetTileWalkSpeed(tileData);
return node;
}
public SearchNode GetNode(Vector2 position)
{
SearchNode node = null;
if (searchNodes.TryGetValue(position, out node)) // if we already built this node
return node; // return it
node = new SearchNode();
node.Node = QueryPosition(position); // otherwise, query the position
searchNodes.TryAdd(position, node);
return node;
}
public List<Vector2> FindPath(Vector2 startPoint, Vector2 endPoint, Heuristic heuristic, float size, float maxTime)
{
try
{
this.Timer.Restart();
if (startPoint == endPoint) // don't do any calculations if we aren't going anywhere
return new List<Vector2>();
searchNodes = new ConcurrentDictionary<Vector2, SearchNode>(); // clear out everything
openList = new List<SearchNode>();
closedList = new List<SearchNode>();
Vector2 newStart = new Vector2(startPoint.X - startPoint.X % size, startPoint.Y - startPoint.Y % size);
Vector2 newEnd = new Vector2(endPoint.X - endPoint.X % size, endPoint.Y - endPoint.Y % size);
SearchNode startNode = GetNode(newStart); // get the start and end nodes
SearchNode endNode = GetNode(newEnd);
startNode.InOpenList = true;
startNode.DistanceToGoal = heuristic(startPoint, endPoint, startNode.Node.Resistance);
startNode.DistanceTraveled = 0;
openList.Add(startNode);
Vector2[] neighbors = new Vector2[]
{
new Vector2(size, 0),
new Vector2(-size, 0),
new Vector2(0, size),
new Vector2(0, -size),
new Vector2(size, size),
new Vector2(size, -size),
new Vector2(-size, size),
new Vector2(-size, -size)
};
while (openList.Count > 0)
{
if (this.Timer.Elapsed.TotalSeconds > maxTime)
{
throw new PathException("No path found (maximum time exceeded)");
}
SearchNode currentNode = FindBestNode();
if (currentNode == null)
{
throw new PathException("No path found");
}
if (currentNode.Equals(endNode))
{
return FindFinalPath(startNode, endNode);
}
for (int i = 0; i < neighbors.Length; i++)
{
SearchNode neighbor = GetNode(neighbors[i] + currentNode.Node.Position);
if (neighbor == null || !neighbor.Node.Walkable)
continue;
float distanceTraveled = currentNode.DistanceTraveled + 1;
float h = heuristic(neighbor.Node.Position, endPoint, neighbor.Node.Resistance);
if (!neighbor.InOpenList && !neighbor.InClosedList)
{
neighbor.DistanceTraveled = distanceTraveled;
neighbor.DistanceToGoal = distanceTraveled + h;
neighbor.Parent = currentNode;
neighbor.InOpenList = true;
openList.Add(neighbor);
}
else
{
if (neighbor.DistanceTraveled > distanceTraveled)
{
neighbor.DistanceTraveled = distanceTraveled;
neighbor.DistanceToGoal = distanceTraveled + h;
neighbor.Parent = currentNode;
}
}
}
openList.Remove(currentNode);
currentNode.InClosedList = true;
}
throw new PathException("Open list ended without finding result");
}
finally
{
this.Timer.Stop();
}
}
private SearchNode FindBestNode()
{
SearchNode current = openList[0];
float smallestDistanceToGoal = float.MaxValue;
for (int i = 0; i < openList.Count; i++)
{
if (openList[i].DistanceToGoal < smallestDistanceToGoal)
{
current = openList[i];
smallestDistanceToGoal = current.DistanceToGoal;
}
}
return current;
}
private List<Vector2> FindFinalPath(SearchNode startNode, SearchNode endNode)
{
closedList.Add(endNode);
SearchNode parent = endNode.Parent;
while (parent != startNode)
{
if (parent == null)
throw new PathException("Unable to find final path (node parent is null)");
closedList.Add(parent);
parent = parent.Parent;
if (parent == null)
return new List<Vector2>();
}
List<Vector2> finalPath = new List<Vector2>();
for (int i = closedList.Count - 1; i >= 0; i--)
{
SearchNode node = closedList[i];
node.InFinalPath = true;
Vector2 pos = node.Node.Position;
finalPath.Add(pos);
}
return finalPath;
}
}
}
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
namespace Navier_Boats.Engine.Pathfinding
{
public class PathNode
{
public Vector2 Position
{
get;
set;
}
public bool Walkable
{
get;
set;
}
public float Resistance
{
get;
set;
}
}
}
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
namespace Navier_Boats.Engine.Pathfinding
{
public class SearchNode
{
public PathNode Node
{
get;
set;
}
public SearchNode Parent
{
get;
set;
}
public bool InOpenList
{
get;
set;
}
public bool InClosedList
{
get;
set;
}
public bool InFinalPath
{
get;
set;
}
public float DistanceToGoal
{
get;
set;
}
public float DistanceTraveled
{
get;
set;
}
public bool Equals(SearchNode obj)
{
return this.Node.Position.Equals(obj.Node.Position);
}
}
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment