Exemplo n.º 1
0
		public static void Recalculate(INavigationTarget target) {
			cellSize = target.CellSize;
			width = target.XCells;
			height = target.ZCells;
			maxHeight = target.MaxHeight;
			maxSlope = target.MaxSlope;
			worldOffset = new Vector3(-width * cellSize / 2f, 0, -height * cellSize / 2f);

			linearCost = cellSize;
			diagonalCost = (float)Math.Sqrt(2) * cellSize;

			var halfCell = new Vector3(cellSize / 2, 0, cellSize / 2);
			cells = new Cell[width, height];

			// Scan the area of the mesh from above to find the walkable parts
			int xLength = cells.GetLength(0);
			int zLength = cells.GetLength(1);
			for (int x = 0; x < xLength; x++) {
				for (int z = 0; z < zLength; z++) {
					cells[x, z] = new Cell {
						Center = worldOffset + new Vector3(x * cellSize, maxHeight, z * cellSize) + halfCell,
						X = x,
						Z = z
					};

					target.SetCellIntersections(cells[x, z]);
				}
			}
		}
Exemplo n.º 2
0
		public static void FindPath(Vector3 start, Vector3 end, Action<List<Vector3>> onComplete, bool ignoreWalkable) {
			Cell startCell = GetCell(start);
			Cell endCell = GetCell(end);

			foreach (Cell cell in cells) {
				cell.Open = false;
				cell.Closed = false;
			}

			// Initialize search sets
			startCell.Node = new Node();
			startCell.FScore = GetEndDistance(startCell, endCell);
			startCell.GScore = 0;
			startCell.Open = true;

			var open = new CellMinPriorityQueue();
			open.Insert(startCell);

			// Placeholder for neighbors, use as ref to avoid garbage collection
			Cell[] neighbors = new Cell[8];

			while (open.Length > 0) {
				Cell current = open.Peek();

				// Are we at the end?
				if (current == endCell) {
					ConstructPath(start, end, onComplete, current);
					return;
				}

				current.Open = false;
				current.Closed = true;
				open.Remove();

				// Get all of the neighbours of the current cell that are walkable
				GetWalkableNeighbors(current, ignoreWalkable, ref neighbors);
				foreach (Cell neighbor in neighbors) {
					if (null == neighbor) {
						continue;
					}

					// Work out the cost to the neighbour via the current node
					float tentativeGScore = current.GScore + neighbor.NeighborDistance;

					// Have we processed this already?
					if (neighbor.Closed && tentativeGScore >= neighbor.GScore) {
						continue;
					}

					if (neighbor.Open && !(tentativeGScore < neighbor.GScore)) {
						continue;
					}

					neighbor.Node = new Node {
						Previous = current
					};
					neighbor.GScore = tentativeGScore;
					neighbor.FScore = tentativeGScore + GetEndDistance(neighbor, endCell);

					if (neighbor.Open) {
						continue;
					}

					neighbor.Open = true;
					open.Insert(neighbor);
				}
			}
		}
Exemplo n.º 3
0
			public void SetCellIntersections(Cell cell) {
				cell.Center.y = 0;
				cell.Walkable = true;
			}
Exemplo n.º 4
0
		private static void GetWalkableNeighbors(Cell cell, bool ignoreWalkable, ref Cell[] neighbors) {
			int i = 0;

			foreach (int[] offset in NEIGHBOR_OFFSETS) {
				int x = offset[0];
				int z = offset[1];

				if (cell.X + x < 0 || cell.X + x >= width || cell.Z + z < 0 || cell.Z + z >= height) {
					continue;
				}

				Cell neighbor = cells[cell.X + x, cell.Z + z];
				neighbor.NeighborDistance = (x + z) % 2 == 0 ? diagonalCost : linearCost;

				float heightOffset = Mathf.Abs(cell.Center.y - neighbor.Center.y);
				if ((neighbor.Walkable && heightOffset < maxSlope) || ignoreWalkable) {
					neighbors[i++] = neighbor;
				}
			}

			// Clear out any left over neighbors we may have had
			for (; i < neighbors.Length; i++) {
				neighbors[i] = null;
			}
		}
Exemplo n.º 5
0
		private static float GetEndDistance(Cell current, Cell end) {
			float xDist = (end.X - current.X) * cellSize;
			float zDist = (end.Z - current.Z) * cellSize;

			return (float)Math.Sqrt(xDist * xDist + zDist * zDist);
		}
Exemplo n.º 6
0
		private static void ConstructPath(Vector3 start, Vector3 end, Action<List<Vector3>> onComplete, Cell current) {
			Node scan = current.Node;
			List<Vector3> path = new List<Vector3> {
				end // Use the actual end spot and not the cell center once we found the end cell
			};

			// Scan backwards from the end along the path until Previous = 0
			while (null != scan?.Previous) {
				path.Insert(0, scan.Previous.Center);
				scan = scan.Previous.Node;
			}

			// Replace the start cell (center) with the actual start location
			path.RemoveAt(0);
			path.Insert(0, start);

			// return the path we found
			onComplete(path);
		}