优化寻路算法少走弯路
主要修改getCost方法实现
using System;using System.Collections.Generic;
using UnityEngine;
public class GridNode
{
public int x = 0;
public int y = 0;
public float weight = 0;
public float h = -1f;
public float g = 0;
public float f = 0;
public bool closed = false;
public bool visited = false;
public GridNode parent = null;
public GridNode(int x, int y, float weight)
{
this.x = x;
this.y = y;
this.weight = weight;
}
public string toString()
{
return "[" + this.x.ToString() + " " + this.y.ToString() + "]";
}
public float getCost(GridNode fromNeighbor)
{
float weight = this.weight;
// Take diagonal weight into consideration.
if (fromNeighbor != null)
{
if (fromNeighbor.x != this.x && fromNeighbor.y != this.y)
weight *= 1.41421f;
if (fromNeighbor.parent != null)
{
if (fromNeighbor.x - fromNeighbor.parent.x == this.x - fromNeighbor.x
&& fromNeighbor.y - fromNeighbor.parent.y == this.y - fromNeighbor.y)
{
weight *= 0.5f;
}
}
}
return weight;
}
public bool isWall()
{
return this.weight == 0;
}
}
class BinaryHeap
{
public static BinaryHeap getHeap()
{
return new BinaryHeap(node => { return node.f; });
}
public List<GridNode> content = null;
public delegate float ScoreFunction(GridNode node);
private ScoreFunction scoreFunction = null;
public BinaryHeap(ScoreFunction scoreFunction)
{
this.content = new List<GridNode>();
this.scoreFunction = scoreFunction;
}
public void push(GridNode element)
{
// Add the new element to the end of the array.
this.content.Add(element);
// Allow it to sink down.
this.sinkDown(this.content.Count - 1);
}
public GridNode pop()
{
// Store the first element so we can return it later.
var result = this.content[0];
// Get the element at the end of the array.
var endIndex = this.content.Count - 1;
var end = this.content[endIndex];
this.content.RemoveAt(endIndex);
// If there are any elements left, put the end element at the
// start, and let it bubble up.
if (this.content.Count > 0)
{
this.content[0] = end;
this.bubbleUp(0);
}
return result;
}
public void remove(GridNode node)
{
var i = this.content.IndexOf(node);
// When it is found, the process seen in 'pop' is repeated
// to fill up the hole.
var endIndex = this.content.Count - 1;
var end = this.content[endIndex];
this.content.RemoveAt(endIndex);
if (i != this.content.Count - 1)
{
this.content[i] = end;
if (this.scoreFunction(end) < this.scoreFunction(node))
{
this.sinkDown(i);
}
else
{
this.bubbleUp(i);
}
}
}
public int size()
{
return this.content.Count;
}
public void rescoreElement(GridNode node)
{
this.sinkDown(this.content.IndexOf(node));
}
public void sinkDown(int n)
{
// Fetch the element that has to be sunk.
var element = this.content[n];
// When at 0, an element can not sink any further.
while (n > 0)
{
// Compute the parent element's index, and fetch it.
var parentN = ((n + 1) >> 1) - 1;
var parent = this.content[parentN];
// Swap the elements if the parent is greater.
if (this.scoreFunction(element) < this.scoreFunction(parent))
{
this.content[parentN] = element;
this.content[n] = parent;
// Update 'n' to continue at the new position.
n = parentN;
}
// Found a parent that is less, no need to sink any further.
else
{
break;
}
}
}
public void bubbleUp(int n)
{
// Look up the target element and its score.
var length = this.content.Count;
var element = this.content[n];
var elemScore = this.scoreFunction(element);
while (true)
{
// Compute the indices of the child elements.
var child2N = (n + 1) << 1;
var child1N = child2N - 1;
// This is used to store the new position of the element, if any.
int swap = int.MinValue;
float child1Score = float.NaN;
// If the first child exists (is inside the array)...
if (child1N < length)
{
// Look it up and compute its score.
var child1 = this.content[child1N];
child1Score = this.scoreFunction(child1);
// If the score is less than our element's, we need to swap.
if (child1Score < elemScore)
{
swap = child1N;
}
}
// Do the same checks for the other child.
if (child2N < length)
{
var child2 = this.content[child2N];
var child2Score = this.scoreFunction(child2);
if (child2Score < (swap < 0 ? elemScore : child1Score))
{
swap = child2N;
}
}
// If the element needs to be moved, swap it, and continue.
if (swap >= 0)
{
this.content[n] = this.content[swap];
this.content[swap] = element;
n = swap;
}
// Otherwise, we are done.
else
{
break;
}
}
}
}
class AstarGraph
{
List<GridNode> nodes = null;
bool diagonal = true;
GridNode[][] grid = null;
List<GridNode> dirtyNodes = new List<GridNode>();
public class GraphOption
{
public bool diagonal = true;
public bool closest = false;
public heuristics.HeuristicFunc heuristic = null;
}
/**
* A graph memory structure
* @param {Array} gridIn 2D array of input weights
* @param {Object} [options]
* @param {bool} [options.diagonal] Specifies whether diagonal moves are allowed
*/
public AstarGraph(int[][] gridIn, GraphOption options)
{
options = options == null ? new GraphOption() : options;
this.diagonal = options.diagonal;
this.init(gridIn);
}
private void init(int[][] gridIn)
{
if (this.nodes == null)
{
this.nodes = new List<GridNode>();
}
this.grid = new GridNode[gridIn.Length][];
for (var x = 0; x < gridIn.Length; x++)
{
var row = gridIn[x];
this.grid[x] = new GridNode[row.Length];
for (var y = 0; y < row.Length; y++)
{
var index = x * row.Length + y;
GridNode node = null;
if (index < this.nodes.Count)
{
node = this.nodes[index];
node.x = x;
node.y = y;
node.weight = row[y];
}
else
{
node = new GridNode(x, y, row[y]);
this.nodes.Add(node);
}
this.grid[x][y] = node;
}
}
this.dirtyNodes.Clear();
for (var i = 0; i < this.nodes.Count; i++)
{
Astar.cleanNode(this.nodes[i]);
}
}
public void cleanDirty()
{
for (var i = 0; i < this.dirtyNodes.Count; i++)
{
Astar.cleanNode(this.dirtyNodes[i]);
}
this.dirtyNodes.Clear();
}
public void markDirty(GridNode node)
{
this.dirtyNodes.Add(node);
}
public List<GridNode> neighbors(GridNode node)
{
List<GridNode> ret = new List<GridNode>();
var x = node.x;
var y = node.y;
var grid = this.grid;
// West
if (x > 1 && grid[x - 1] != null && grid[x - 1][y] != null)
{
ret.Add(grid[x - 1][y]);
}
// East
if (x < grid.Length - 1 && grid[x + 1] != null && grid[x + 1][y] != null)
{
ret.Add(grid[x + 1][y]);
}
// South
if (grid[x] != null && y > 1 && grid[x][y - 1] != null)
{
ret.Add(grid[x][y - 1]);
}
// North
if (grid[x] != null && y < grid[x].Length - 1 && grid[x][y + 1] != null)
{
ret.Add(grid[x][y + 1]);
}
if (this.diagonal)
{
// Southwest
if (x > 1 && grid[x - 1] != null && y > 1 && grid[x - 1][y - 1] != null)
{
ret.Add(grid[x - 1][y - 1]);
}
// Southeast
if (x < grid.Length - 1 && grid[x + 1] != null && y > 1 && grid[x + 1][y - 1] != null)
{
ret.Add(grid[x + 1][y - 1]);
}
// Northwest
if (x > 1 && grid[x - 1] != null && y < grid[x - 1].Length - 1 && grid[x - 1][y + 1] != null)
{
ret.Add(grid[x - 1][y + 1]);
}
// Northeast
if (x < grid.Length - 1 && grid[x + 1] != null && y < grid[x + 1].Length - 1 && grid[x + 1][y + 1] != null)
{
ret.Add(grid[x + 1][y + 1]);
}
}
return ret;
}
// public toString() {
// var graphString = [];
// var nodes = this.grid;
// for (var x = 0; x < nodes.length; x++) {
// var rowDebug = [];
// var row = nodes[x];
// for (var y = 0; y < row.length; y++) {
// rowDebug.push(row[y].weight);
// }
// graphString.push(rowDebug.join(" "));
// }
// return graphString.join("\n");
// };
}
// See list of heuristics: http://theory.stanford.edu/~amitp/GameProgramming/Heuristics.html
class heuristics
{
public delegate float HeuristicFunc(GridNode pos0, GridNode pos1);
public static float manhattan(GridNode pos0, GridNode pos1)
{
var d1 = Mathf.Abs(pos1.x - pos0.x);
var d2 = Mathf.Abs(pos1.y - pos0.y);
return d1 + d2;
}
public static float diagonal(GridNode pos0, GridNode pos1)
{
float D = 1;
float D2 = Mathf.Sqrt(2);
float d1 = Mathf.Abs(pos1.x - pos0.x);
float d2 = Mathf.Abs(pos1.y - pos0.y);
return (D * (d1 + d2)) + ((D2 - (2 * D)) * Math.Min(d1, d2));
}
}
class Astar
{
public static int GRIEWEIGHT_WALL = 0;
public static int GRIDWEIGHT_ROAD = 50;
public static int GRIDWEIGHT_FREE = 100;
private static List<GridNode> pathTo(GridNode node)
{
var curr = node;
var path = new List<GridNode>();
while (curr.parent != null)
{
path.Insert(0, curr);
curr = curr.parent;
}
return path;
}
/**
* Perform an A* Search on a graph given a start and end node.
* @param {AstarGraph} graph
* @param {GridNode} start
* @param {GridNode} end
* @param {Object} [options]
* @param {bool} [options.closest] Specifies whether to return the
path to the closest node if the target is unreachable.
* @param {Function} [options.heuristic] Heuristic function (see
* astar.heuristics).
*/
public static List<GridNode> search(AstarGraph graph, GridNode start, GridNode end, AstarGraph.GraphOption options)
{
graph.cleanDirty();
options = options == null ? new AstarGraph.GraphOption() : options;
var heuristic = options.heuristic != null ? options.heuristic : heuristics.manhattan;
var closest = options.closest || false;
var openHeap = BinaryHeap.getHeap();
var closestNode = start; // set the start node to be the closest if required
start.h = heuristic(start, end);
graph.markDirty(start);
openHeap.push(start);
while (openHeap.size() > 0)
{
// Grab the lowest f(x) to process next. Heap keeps this sorted for us.
var currentNode = openHeap.pop();
// End case -- result has been found, return the traced path.
// if (currentNode === end) {
// return pathTo(currentNode);
// }
if (currentNode.x == end.x && currentNode.y == end.y)
{
return pathTo(currentNode);
}
// Normal case -- move currentNode from open to closed, process each of its neighbors.
currentNode.closed = true;
// Find all neighbors for the current node.
var neighbors = graph.neighbors(currentNode);
for (int i = 0, il = neighbors.Count; i < il; ++i)
{
var neighbor = neighbors[i];
if (neighbor.closed || neighbor.isWall())
{
// Not a valid node to process, skip to next neighbor.
continue;
}
// The g score is the shortest distance from start to current node.
// We need to check if the path we have arrived at this neighbor is the shortest one we have seen yet.
var gScore = currentNode.g + neighbor.getCost(currentNode);
var beenVisited = neighbor.visited;
if (!beenVisited || gScore < neighbor.g)
{
// Found an optimal (so far) path to this node. Take score for node to see how good it is.
neighbor.visited = true;
neighbor.parent = currentNode;
neighbor.h = neighbor.h < 0 ? heuristic(neighbor, end) : neighbor.h;
neighbor.g = gScore;
neighbor.f = neighbor.g + neighbor.h;
graph.markDirty(neighbor);
if (closest)
{
// If the neighbour is closer than the current closestNode or if it's equally close but has
// a cheaper path than the current closest node then it becomes the closest node
if (neighbor.h < closestNode.h || (!(neighbor.h > closestNode.h) && neighbor.g < closestNode.g))
{
closestNode = neighbor;
}
}
if (!beenVisited)
{
// Pushing to heap will put it in proper place based on the 'f' value.
openHeap.push(neighbor);
}
else
{
// Already seen the node, but since it has been rescored we need to reorder it in the heap
openHeap.rescoreElement(neighbor);
}
}
}
}
if (closest)
{
return pathTo(closestNode);
}
// No result was found - empty array signifies failure to find path.
return null;
}
public static void cleanNode(GridNode node)
{
node.f = 0;
node.g = 0;
node.h = 0;
node.visited = false;
node.closed = false;
node.parent = null;
}
}
以上是 优化寻路算法少走弯路 的全部内容， 来源链接： utcz.com/z/509403.html
