我有一个我正在使用的c＃pathfinding实现 . 我遇到了一些问题 . 它工作正常，但它不会给我我想要的路径类型 . 最好的例子是当我在无障碍环境中寻找A到B之间的路径而不是直接到目标的锯齿形时（我希望如此），它将需要更少的转弯并且更多地形成L形或钝角 . 我的算法是否有利于减少转弯？我用启发式方法搞砸了几个小时，并尝试了很多东西 . 使用平局破坏者，尝试曼哈顿和octile启发式 . 改变启发式似乎并没有影响行为 . 现在我认为它在系统的其他地方？
这张照片是我得到的行为，下面的图片是我想要的 .
这是我的长寻路代码 . 原谅混乱：使用UnityEngine;使用System.Collections;
公共类PathFinder {
//Declare readonlyants
private int onClosedList = 10;
private readonly int notStarted = 0;
private readonly int found = 1;
private readonly int nonexistent = 2;
private readonly int walkable = 0; // walkability array readonlyants
private readonly int unwalkable = 1;
private readonly int diagonal_cost = 14;
private readonly int horizontal_cost = 10;
//Create needed arrays
private int[] openList;// = new int[(int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) * (int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 2]; //1 dimensional array holding ID# of open list items
private int[,] whichList;// = new int[(int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 1, (int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 1]; //2 dimensional array used to record
//Whether a cell is on the open list or on the closed list.
private int[] openX;// = new int[(int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) * (int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 2]; //1d array stores the x location of an item on the open list
private int[] openY;// = new int[(int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) * (int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 2]; //1d array stores the y location of an item on the open list
private int[,] parentX;// = new int[(int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 1, (int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 1]; //2d array to store parent of each cell (x)
private int[,] parentY;// = new int[(int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 1, (int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 1]; //2d array to store parent of each cell (y)
private float[] Fcost;// = new int[(int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) * (int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 2]; //1d array to store F cost of a cell on the open list
private float[,] Gcost;// = new int[(int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 1, (int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 1]; //2d array to store G cost for each cell.
private float[] Hcost;// = new int[(int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) * (int) ((Config.MAP_SIZE * TileRow) /(float) Config.CLIP_MAP_SQUARE_SIZE) + 2]; //1d array to store H cost of a cell on the open list
private int pathLength; //stores length of the found path for critter
private int pathLocation; //stores current position along the chosen path for critter
private Vector3[] pathBank;
//Path reading variables
private int pathStatus;
private int xPath;
private int yPath;
public PathFinder (int MapSizeX, int MapSizeY, float WaypointDistance)
{
int totalX = Mathf.RoundToInt (MapSizeX / WaypointDistance);
int totalY = Mathf.RoundToInt (MapSizeY / WaypointDistance);
//Correct is totalX * totalY, but we don't want it to try to create paths 5000 units long and lag the server,
//so we ruduce the array size allowing us to set a MAXIMUM path length befoe it gives up.
int total = totalX * totalY;//2000;//totalX * totalY;
openList = new int[total];
whichList = new int[totalX + 1, totalY + 1];
openX = new int[total];
openY = new int[total];
parentX = new int[totalX, totalY];
parentY = new int[totalX, totalY];
Fcost = new float[total];
Gcost = new float[totalX, totalY];
Hcost = new float[total];
}
//==========================================================
//READ PATH DATA: These functions read the path data and convert
//it to screen pixel coordinates.
public Vector3[] GetPath (float WaypointDistance)
{
Vector3[] pathBank2 = new Vector3 [pathBank.Length];
for (int i = 0; i < pathBank2.Length; i++) {
pathBank2 [i] = new Vector3 (Mathf.RoundToInt (pathBank [i].x * WaypointDistance), pathBank [i].y, Mathf.RoundToInt (pathBank [i].z * WaypointDistance));
}
return pathBank2;
}
//-----------------------------------------------------------------------------
// Name: FindPath
// Desc: Finds a path using A*
//-----------------------------------------------------------------------------
public int FindPath (ref Vector3[,,] ClipMap, Vector3 startPos, Vector3 endPos, int heightLevel, float entitySize, int MapSizeX, int MapSizeY, float WaypointDistance)
{
int entityClearance = Mathf.CeilToInt (entitySize / 2 / WaypointDistance);
int onOpenList = 0;
int parentXval = 0;
int parentYval = 0;
int a = 0;
int b = 0;
int m = 0;
int u = 0;
int v = 0;
int temp = 0;
int corner = 0;
int numberOfOpenListItems = 0;
int addedGCost = 0;
float tempGcost = 0;
int path = 0;
int tempx;
int pathX;
int pathY;
int cellPosition;
int newOpenListItemID = 0;
//1. Convert location data to coordinates in the walkability array.
int startX = Mathf.RoundToInt (startPos.x / WaypointDistance);
int startY = Mathf.RoundToInt (startPos.z / WaypointDistance);
int targetX = Mathf.RoundToInt (endPos.x / WaypointDistance);
int targetY = Mathf.RoundToInt (endPos.z / WaypointDistance);
if (targetX >= ClipMap.GetLength (0) || targetX < 0 || targetY >= ClipMap.GetLength (1) || targetY < 0 || heightLevel < 0 || heightLevel >= ClipMap.GetLength (2)) {
Debug.Log ("Here");
return nonexistent;
}
if(ClipMap[targetX, targetY, heightLevel].x == unwalkable) {
Debug.Log ("Here");
return nonexistent;
}
//If starting location and target are in the same location...
if (startX == targetX && startY == targetY && pathLocation >= 0) {
if (ClipMap [startX, startY, heightLevel].x == walkable) {
pathBank = new Vector3[] {new Vector3 (startX, ClipMap [startX, startY, heightLevel].y, startY)};
return found;
} else {
Debug.Log ("Here");
return nonexistent;
}
}
// If standing on unwalkable square, continue on last path
if (ClipMap [startX, startY, heightLevel].x == unwalkable) {
Debug.Log ("Here");
return nonexistent;
}
// If target square is unwalkable, return that it's a nonexistent path.
if (ClipMap [startX, startY, heightLevel].x == unwalkable || ClipMap [targetX, targetY, heightLevel].z < entityClearance) {
Debug.Log ("Here");
goto noPath;
}
//3.Reset some variables that need to be cleared
if (onClosedList > 1000000) { //reset whichList occasionally
for (int x = 0; x < Mathf.RoundToInt (ClipMap.GetLength (0) / WaypointDistance); x++) {
for (int y = 0; y < Mathf.RoundToInt (ClipMap.GetLength (1) / WaypointDistance); y++) {
whichList [x, y] = 0;
}
}
onClosedList = 10;
}
onClosedList = onClosedList + 2; //changing the values of onOpenList and onClosed list is faster than redimming whichList() array
onOpenList = onClosedList - 1;
pathLength = notStarted; //i.e, = 0
pathLocation = notStarted; //i.e, = 0
Gcost [startX, startY] = 0; //reset starting square's G value to 0
//4.Add the starting location to the open list of squares to be checked.
numberOfOpenListItems = 1;
openList [1] = 1; //assign it as the top (and currently only) item in the open list, which is maintained as a binary heap (explained below)
openX [1] = startX;
openY [1] = startY;
//5.Do the following until a path is found or deemed nonexistent.
do {
//6.If the open list is not empty, take the first cell off of the list.
// This is the lowest F cost cell on the open list.
if (numberOfOpenListItems != 0) {
//7. Pop the first item off the open list.
parentXval = openX [openList [1]];
parentYval = openY [openList [1]]; //record cell coordinates of the item
whichList [parentXval, parentYval] = onClosedList; //add the item to the closed list
// Open List = Binary Heap: Delete this item from the open list, which
// is maintained as a binary heap. For more information on binary heaps, see:
// http://www.policyalmanac.org/games/binaryHeaps.htm
numberOfOpenListItems = numberOfOpenListItems - 1; //reduce number of open list items by 1
// Delete the top item in binary heap and reorder the heap, with the lowest F cost item rising to the top.
openList [1] = openList [numberOfOpenListItems + 1]; //move the last item in the heap up to slot #1
v = 1;
// Repeat the following until the new item in slot #1 sinks to its proper spot in the heap.
do {
u = v;
if (2 * u + 1 <= numberOfOpenListItems) { //if both children exist
//Check if the F cost of the parent is greater than each child.
//Select the lowest of the two children.
if (Fcost [openList [u]] >= Fcost [openList [2 * u]]) {
v = 2 * u;
}
if (Fcost [openList [v]] >= Fcost [openList [2 * u + 1]]) {
v = 2 * u + 1;
}
} else {
if (2 * u <= numberOfOpenListItems) { //if only child #1 exists
//Check if the F cost of the parent is greater than child #1
if (Fcost [openList [u]] >= Fcost [openList [2 * u]]) {
v = 2 * u;
}
}
}
if (u != v) { //if parent's F is > one of its children, swap them
temp = openList [u];
openList [u] = openList [v];
openList [v] = temp;
} else
break; //otherwise, exit loop
} while (true); //reorder the binary heap
//7.Check the adjacent squares. (Its "children" -- these path children
// are similar, conceptually, to the binary heap children mentioned
// above, but don't confuse them. They are different. Path children
// are portrayed in Demo 1 with grey pointers pointing toward
// their parents.) Add these adjacent child squares to the open list
// for later consideration if appropriate (see various if statements
// below).
for (b = parentYval - 1; b <= parentYval + 1; b++) {
for (a = parentXval - 1; a <= parentXval + 1; a++) {
// If not off the map (do this first to avoid array out-of-bounds errors)
if (a != -1 && b != -1 && a != ClipMap.GetLength (0) && b != ClipMap.GetLength (1)) {
// If not already on the closed list (items on the closed list have
// already been considered and can now be ignored).
if (whichList [a, b] != onClosedList) {
// If not a wall/obstacle square.
if (ClipMap [a, b, heightLevel].x != unwalkable && ClipMap [a, b, heightLevel].z >= entityClearance) {
// Don't cut across corners
corner = walkable;
if (a == parentXval - 1) {
if (b == parentYval - 1) {
if (ClipMap [parentXval - 1, parentYval, heightLevel].x == unwalkable || ClipMap [parentXval - 1, parentYval, heightLevel].z < entityClearance || ClipMap [parentXval, parentYval - 1, heightLevel].x == unwalkable || ClipMap [parentXval, parentYval - 1, heightLevel].z < entityClearance) {
corner = unwalkable;
}
} else if (b == parentYval + 1) {
if (ClipMap [parentXval, parentYval + 1, heightLevel].x == unwalkable || ClipMap [parentXval, parentYval + 1, heightLevel].z < entityClearance || ClipMap [parentXval - 1, parentYval, heightLevel].x == unwalkable || ClipMap [parentXval - 1, parentYval, heightLevel].z < entityClearance) {
corner = unwalkable;
}
}
} else if (a == parentXval + 1) {
if (b == parentYval - 1) {
if (ClipMap [parentXval, parentYval - 1, heightLevel].x == unwalkable || ClipMap [parentXval, parentYval - 1, heightLevel].z < entityClearance || ClipMap [parentXval + 1, parentYval, heightLevel].x == unwalkable || ClipMap [parentXval + 1, parentYval, heightLevel].z < entityClearance) {
corner = unwalkable;
}
} else if (b == parentYval + 1) {
if (ClipMap [parentXval + 1, parentYval, heightLevel].x == unwalkable || ClipMap [parentXval + 1, parentYval, heightLevel].z < entityClearance || ClipMap [parentXval, parentYval + 1, heightLevel].x == unwalkable || ClipMap [parentXval, parentYval + 1, heightLevel].z < entityClearance) {
corner = unwalkable;
}
}
}
if (corner == walkable) {
// If not already on the open list, add it to the open list.
if (whichList [a, b] != onOpenList) {
//Create a new open list item in the binary heap.
newOpenListItemID = newOpenListItemID + 1; //each new item has a unique ID #
m = numberOfOpenListItems + 1;
openList [m] = newOpenListItemID; //place the new open list item (actually, its ID#) at the bottom of the heap
if (newOpenListItemID >= openX.Length) {
Debug.Log ("Here");
return nonexistent;
}
openX [newOpenListItemID] = a;
openY [newOpenListItemID] = b; //record the x and y coordinates of the new item
//Figure out its G cost
if (Mathf.Abs (a - parentXval) == 1 && Mathf.Abs (b - parentYval) == 1) {
addedGCost = diagonal_cost; //cost of going to diagonal squares
} else {
addedGCost = horizontal_cost; //cost of going to non-diagonal squares
}
Gcost [a, b] = Gcost [parentXval, parentYval] + addedGCost;
//Figure out its H and F costs and parent
float d = horizontal_cost;
float d2 = diagonal_cost;
//Hcost [openList [m]] = 10 * (Mathf.Abs (a - targetX) + Mathf.Abs (b - targetY));
int dx = Mathf.Abs (a - targetX);
int dy = Mathf.Abs (b - targetY);
/*int dx2 = startX - targetX;
int dy2 = startY - targetY;
int cross = Mathf.Abs (dx*dy2 - dx2*dy);
float p = 0.001f;*/
Hcost [openList [m]] = (d * (dx + dy) + (d2 - 2.0f * d) * (float) Mathf.Min (dx, dy));
Fcost [openList [m]] = Gcost [a, b] + Hcost [openList [m]];
parentX [a, b] = parentXval;
parentY [a, b] = parentYval;
//Move the new open list item to the proper place in the binary heap.
//Starting at the bottom, successively compare to parent items,
//swapping as needed until the item finds its place in the heap
//or bubbles all the way to the top (if it has the lowest F cost).
while (m != 1) { //While item hasn't bubbled to the top (m=1)
//Check if child's F cost is < parent's F cost. If so, swap them.
if (Fcost [openList [m]] <= Fcost [openList [m / 2]]) {
temp = openList [m / 2];
openList [m / 2] = openList [m];
openList [m] = temp;
m = m / 2;
} else
break;
}
numberOfOpenListItems = numberOfOpenListItems + 1; //add one to the number of items in the heap
//Change whichList to show that the new item is on the open list.
whichList [a, b] = onOpenList;
}
//8.If adjacent cell is already on the open list, check to see if this
// path to that cell from the starting location is a better one.
// If so, change the parent of the cell and its G and F costs.
else { //If whichList(a,b) = onOpenList
//Figure out the G cost of this possible new path
if (Mathf.Abs (a - parentXval) == 1 && Mathf.Abs (b - parentYval) == 1) {
addedGCost = diagonal_cost; //cost of going to diagonal tiles
} else {
addedGCost = horizontal_cost; //cost of going to non-diagonal tiles
}
tempGcost = Gcost [parentXval, parentYval] + addedGCost;
//If this path is shorter (G cost is lower) then change
//the parent cell, G cost and F cost.
if (tempGcost < Gcost [a, b]) { //if G cost is less,
parentX [a, b] = parentXval; //change the square's parent
parentY [a, b] = parentYval;
Gcost [a, b] = tempGcost; //change the G cost
//Because changing the G cost also changes the F cost, if
//the item is on the open list we need to change the item's
//recorded F cost and its position on the open list to make
//sure that we maintain a properly ordered open list.
for (int x = 1; x <= numberOfOpenListItems; x++) { //look for the item in the heap
if (openX [openList [x]] == a && openY [openList [x]] == b) { //item found
Fcost [openList [x]] = Gcost [a, b] + Hcost [openList [x]]; //change the F cost
//See if changing the F score bubbles the item up from it's current location in the heap
m = x;
while (m != 1) { //While item hasn't bubbled to the top (m=1)
//Check if child is < parent. If so, swap them.
if (Fcost [openList [m]] < Fcost [openList [m / 2]]) {
temp = openList [m / 2];
openList [m / 2] = openList [m];
openList [m] = temp;
m = m / 2;
} else
break;
}
break; //exit for x = loop
} //If openX(openList(x)) = a
} //For x = 1 To numberOfOpenListItems
} //If tempGcost < Gcost(a,b)
} //else If whichList(a,b) = onOpenList
} //If not cutting a corner
} //If not a wall/obstacle square.
} //If not already on the closed list
} //If not off the map
} //for (a = parentXval-1; a <= parentXval+1; a++){
} //for (b = parentYval-1; b <= parentYval+1; b++){
} //if (numberOfOpenListItems != 0)
//9.If open list is empty then there is no path.
else {
Debug.Log ("Here");
path = nonexistent;
break;
}
//If target is added to open list then path has been found.
if (whichList [targetX, targetY] == onOpenList) {
path = found;
break;
}
} while (true); //Do until path is found or deemed nonexistent
//10.Save the path if it exists.
if (path == found) {
//a.Working backwards from the target to the starting location by checking
// each cell's parent, figure out the length of the path.
pathX = targetX;
pathY = targetY;
do {
//Look up the parent of the current cell.
tempx = parentX [pathX, pathY];
pathY = parentY [pathX, pathY];
pathX = tempx;
//Figure out the path length
pathLength = pathLength + 1;
} while (pathX != startX || pathY != startY);
//b.Resize the data bank to the right size in bytes
pathBank = new Vector3[pathLength];
//c. Now copy the path information over to the databank. Since we are
// working backwards from the target to the start location, we copy
// the information to the data bank in reverse order. The result is
// a properly ordered set of path data, from the first step to the
// last.
pathX = targetX;
pathY = targetY;
cellPosition = pathLength; //start at the end
do {
cellPosition = cellPosition - 1; //work backwards 1 integer
pathBank [cellPosition] = new Vector3 (pathX, ClipMap [pathX, pathY, heightLevel].y, pathY);
//d.Look up the parent of the current cell.
tempx = parentX [pathX, pathY];
pathY = parentY [pathX, pathY];
pathX = tempx;
//e.If we have reached the starting square, exit the loop.
} while (pathX != startX || pathY != startY);
}
return path;
//13.If there is no path to the selected target, return non existant
noPath:
return nonexistent;
}
1 回答
您需要使用的不是路径查找器，而是线条绘制算法 . 基本上，你只是画一条从A到B的线 . 如果有障碍，你需要一个探路者 .
查看Bresenham的线算法（https://en.wikipedia.org/wiki/Bresenham's_line_algorithm），它将帮助您实现目标 .