public static void Main() { var h = new HeapQueue <int>(delegate(int x, int y){ return(x - y); }); Console.WriteLine(h.Count); Console.WriteLine(h.Push(30)); Console.WriteLine(h.Push(10)); Console.WriteLine(h.Push(-90)); Console.WriteLine(h.Push(+20)); Console.WriteLine(h.Push(-50)); Console.WriteLine(h.Push(-40)); Console.WriteLine(h.Push(+70)); Console.WriteLine(h); Console.WriteLine(h.Pop()); Console.WriteLine(h); }
/// <summary> /// Finds a path from any of start to goal, such that the path has no links that are steeper than the unit's maxGrade. /// </summary> /// <returns>The path.</returns> /// <param name="startPointList">List of PointWithDistances for points to start from</param> /// <param name="snappedGoalPoint">HexPoint3 to go to</param> /// <param name="unit">moving unit</param> /// <param name="moveType">move type - walk, sprint</param> /// <param name="targetRadius">how close to get to the target</param> /// <param name="actorAware">Discard nodes where other actors reside</param> public static List <PointWithCost> FindPath(List <PointWithCost> startPointList, HexPoint3 snappedGoalPoint, AbstractActor unit, MoveType moveType, float targetRadius, bool actorAware) { MapMetaData mapMetaData = unit.Combat.MapMetaData; HexGrid hexGrid = unit.Combat.HexGrid; unit.Pathing.MoveType = moveType; List <AbstractActor> actors = null; bool startedInEncounterBounds = false; BattleTech.Designed.EncounterBoundaryChunkGameLogic boundaryChunk = unit.Combat.EncounterLayerData.encounterBoundaryChunk; for (int spi = 0; spi < startPointList.Count; ++spi) { PointWithCost sp = startPointList[spi]; //Vector3 wp = HexPoint3ToWorldPoint(sp.point, hexGrid); if (boundaryChunk.IsInEncounterBounds(unit.CurrentPosition)) { startedInEncounterBounds = true; break; } } actorAware = unit.BehaviorTree.GetBehaviorVariableValue(BehaviorVariableName.Bool_EnableLongRangePathfindingBeActorAware).BoolVal ? true : actorAware; if (actorAware) { actors = unit.Combat.AllActors; actors.Remove(unit); } List <PointWithCost> path = new List <PointWithCost>(); HeapQueue <PointWithCost> openHeap = new HeapQueue <PointWithCost>(); Dictionary <HexPoint3, float> bestCostDict = new Dictionary <HexPoint3, float>(); Dictionary <HexPoint3, PointWithCost> bestPrevPoint = new Dictionary <HexPoint3, PointWithCost>(); Vector3 worldGoalPoint = HexPoint3ToWorldPoint(snappedGoalPoint, hexGrid); float bestPathCost = float.MaxValue; bool anyPathFound = false; PointWithCost bestGoalPoint = new PointWithCost(new HexPoint3(-4000, -4000), float.MaxValue, float.MaxValue); for (int startIndex = 0; startIndex < startPointList.Count; ++startIndex) { PointWithCost pwd = startPointList[startIndex]; openHeap.Push(pwd); bestCostDict[pwd.point] = pwd.cost; bestPrevPoint[pwd.point] = null; Vector3 wp = HexPoint3ToWorldPoint(pwd.point, hexGrid); if ((pwd.point.Equals(snappedGoalPoint)) || (AIUtil.Get2DDistanceBetweenVector3s(wp, worldGoalPoint) < targetRadius)) { if (pwd.cost < bestPathCost) { anyPathFound = true; bestPathCost = pwd.cost; bestGoalPoint = pwd; } } } while (!openHeap.IsEmpty()) { PointWithCost ptWithCost = openHeap.PopMinimum(); if (ptWithCost.estimatedTotalCost > bestPathCost) { continue; } Vector3 worldPoint = HexPoint3ToWorldPoint(ptWithCost.point, hexGrid); if (actorAware && CheckForOccupiedPoint(actors, worldPoint)) { continue; } if (startedInEncounterBounds && (!boundaryChunk.IsInEncounterBounds(worldPoint))) { continue; } for (int direction = 0; direction < 6; ++direction) { HexPoint3 neighborHexPoint = ptWithCost.point.Step(direction, 1); Vector3 neighborWorldPoint = HexPoint3ToWorldPoint(neighborHexPoint, hexGrid); if ((!mapMetaData.IsWithinBounds(neighborWorldPoint)) || (unit.Pathing.CurrentGrid.FindBlockerReciprocal(worldPoint, neighborWorldPoint))) { continue; } Debug.DrawLine(worldPoint, neighborWorldPoint, Color.yellow, 15.0f); float linkCost = unit.Pathing.CurrentGrid.GetTerrainModifiedCost(worldPoint, neighborWorldPoint); float newCost = ptWithCost.cost + linkCost; if (newCost >= bestPathCost) { continue; } if ((!bestCostDict.ContainsKey(neighborHexPoint)) || (newCost < bestCostDict[neighborHexPoint])) { bestCostDict[neighborHexPoint] = newCost; bestPrevPoint[neighborHexPoint] = ptWithCost; if ((neighborHexPoint.Equals(snappedGoalPoint)) || ((neighborWorldPoint - worldGoalPoint).magnitude < targetRadius)) { if (newCost < bestPathCost) { anyPathFound = true; bestPathCost = newCost; bestGoalPoint = new PointWithCost(neighborHexPoint, newCost, 0.0f); } } else { Vector3 remainingDistance = (worldGoalPoint - neighborWorldPoint); float estRemainingCost = remainingDistance.magnitude; openHeap.Push(new PointWithCost(neighborHexPoint, newCost, newCost + estRemainingCost)); } } } } if (anyPathFound) { PointWithCost p = bestGoalPoint; path.Add(p); while (bestPrevPoint.ContainsKey(p.point)) { PointWithCost prevPoint = bestPrevPoint[p.point]; if ((prevPoint == null) || (path.Contains(prevPoint))) { break; } path.Insert(0, prevPoint); p = prevPoint; } } else { // draw the failed path data const int SIDES = 3; const float RADIUS = 12; foreach (PointWithCost startPoint in startPointList) { Vector3 worldStartPoint = HexPoint3ToWorldPoint(startPoint.point, hexGrid); for (int i = 0; i < SIDES; ++i) { float dx0 = RADIUS * Mathf.Cos(i * Mathf.PI * 2 / SIDES); float dz0 = RADIUS * Mathf.Sin(i * Mathf.PI * 2 / SIDES); float dx1 = RADIUS * Mathf.Cos((i + 1) * Mathf.PI * 2 / SIDES); float dz1 = RADIUS * Mathf.Sin((i + 1) * Mathf.PI * 2 / SIDES); Vector3 wp0 = new Vector3(worldStartPoint.x + dx0, 0, worldStartPoint.z + dz0); Vector3 wp1 = new Vector3(worldStartPoint.x + dx1, 0, worldStartPoint.z + dz1); Debug.DrawLine(wp0, wp1, Color.magenta, 15.0f); } } Vector3 worldEndPoint = HexPoint3ToWorldPoint(snappedGoalPoint, hexGrid); Color orangeColor = new Color(1.0f, 0.5f, 0.0f); for (int i = 0; i < SIDES; ++i) { float dx0 = RADIUS * Mathf.Cos(i * Mathf.PI * 2 / SIDES); float dz0 = RADIUS * Mathf.Sin(i * Mathf.PI * 2 / SIDES); float dx1 = RADIUS * Mathf.Cos((i + 1) * Mathf.PI * 2 / SIDES); float dz1 = RADIUS * Mathf.Sin((i + 1) * Mathf.PI * 2 / SIDES); Vector3 wp0 = new Vector3(worldEndPoint.x + dx0, 0, worldEndPoint.z + dz0); Vector3 wp1 = new Vector3(worldEndPoint.x + dx1, 0, worldEndPoint.z + dz1); Debug.DrawLine(wp0, wp1, orangeColor, 15.0f); } } int removedCount = 0; // Now, check to see if the end of the path is in "danger". If it is, prune until it's not, which might lead to an empty path. while (path.Count > 0) { PointWithCost lastHexPoint = path[path.Count - 1]; Vector3 lastWorldPoint = HexPoint3ToWorldPoint(lastHexPoint.point, hexGrid); MapTerrainDataCell dataCell = unit.Combat.MapMetaData.GetCellAt(lastWorldPoint); if (SplatMapInfo.IsDropshipLandingZone(dataCell.terrainMask) || SplatMapInfo.IsDangerousLocation(dataCell.terrainMask) || SplatMapInfo.IsDropPodLandingZone(dataCell.terrainMask)) { path.RemoveAt(path.Count - 1); ++removedCount; } else { break; } } if (removedCount > 0) { if (path.Count == 0) { BehaviorNode.LogAI(unit, string.Format("DANGER TRIM: removed all {0} points, bracing", removedCount)); } else { BehaviorNode.LogAI(unit, string.Format("DANGER TRIM: removed {0} points, moving to {1}", removedCount, path[path.Count - 1])); } } return(path); }
/// <summary> /// performs a Branch and Bound search of the state space of partial tours /// stops when time limit expires and uses BSSF as solution /// @returns results array for GUI that contains three ints: cost of solution, time spent to find solution, number of solutions found during search (not counting initial BSSF estimate) public string[] bBSolveProblem() { // counts the number of bssf updates / solutions found int count = 0; // counts the number of pruned states int pruned = 0; // counts the number of total states int total_states = 0; //container for results string[] results = new string[3]; //priority queue implementation HeapQueue myqueue = new HeapQueue(); Route = new ArrayList(); Stopwatch timer = new Stopwatch(); timer.Start(); Route.Clear(); //generate greedy bssf as initial guess greedySolveProblem(); //initialize a matrix with all the cities bBNode init = new bBNode(GetCities()); /** * Starts at the first city (0). * It doesn't matter where we start because we are looking for a "loop" of cities. * And we don't want to repeatedly find the same solution by starting at a different city. */ bBNode root = new bBNode(init, 0); bBNode next; //initialize queue myqueue.MakeQueue((int)Math.Pow(Cities.Length, 2), root); total_states++; //while the queue is not empty and we haven't hit the time limit . . . while (!myqueue.IsEmpty() && timer.ElapsedMilliseconds < this.time_limit) { // take the top node in the queue next = (bBNode)myqueue.DeleteBestRoute(); //if it has a better solution than the bssf, go deeper, otherwise, we prune it. if (next.getBound() < bssf.costOfRoute()) { // check if the route is finished if (next.getRouteLength() == Cities.Length) { //We found a better solution! bssf = new TSPSolution(next.getRoute()); count++; } else { // expand the node, and only insert the children that do better than the bssf List <bBNode> children = next.Expand(); foreach (bBNode c in children) { total_states++; if (c.getBound() < bssf.costOfRoute()) { myqueue.Insert(c); } else { pruned++; } } } } else { pruned++; } } timer.Stop(); Console.WriteLine("**************************************"); Console.WriteLine("# cities: " + Cities.Length); Console.WriteLine("# seed: " + _seed); Console.WriteLine("RunningTime: " + timer.Elapsed); Console.WriteLine("Best tour: " + costOfBssf()); Console.WriteLine("Max stored states: " + myqueue.getMaxStoredStates()); Console.WriteLine("Number of Solutions: " + count); Console.WriteLine("Number total states: " + total_states); Console.WriteLine("Total pruned states: " + pruned); results[COST] = costOfBssf().ToString(); // load results array results[TIME] = timer.Elapsed.ToString(); results[COUNT] = count.ToString(); return(results); }
/// <summary> /// Finds a path from any of the points in startPointList to goal, such that the path has no links that are steeper up than maxIncline /// nor steeper down than maxDecline. /// </summary> /// <returns>The path.</returns> /// <param name="startPointList">PointWithDistance structs for starting area</param> /// <param name="goalPoint">map indices of point to go to</param> List <PointWithDistance> FindPath(List <PointWithDistance> startPointList, InclineIndexPoint goalPoint, float maxIncline, float maxDecline) { List <PointWithDistance> path = new List <PointWithDistance>(); HeapQueue <PointWithDistance> openHeap = new HeapQueue <PointWithDistance>(); Dictionary <InclineIndexPoint, float> bestDistanceDict = new Dictionary <InclineIndexPoint, float>(); Dictionary <InclineIndexPoint, PointWithDistance> bestPrevPoint = new Dictionary <InclineIndexPoint, PointWithDistance>(); Point goalMapPoint = InclineIndicesToMapIndices(goalPoint); Vector3 worldGoalPoint = mapMetaData.getWorldPos(goalMapPoint); for (int startIndex = 0; startIndex < startPointList.Count; ++startIndex) { PointWithDistance pwd = startPointList[startIndex]; openHeap.Push(pwd); bestDistanceDict[pwd.point] = pwd.distance; bestPrevPoint[pwd.point] = null; } float bestPathLength = -1; PointWithDistance bestGoalPoint = new PointWithDistance(new InclineIndexPoint(-1024, -1024), float.MaxValue, float.MaxValue); while (!openHeap.IsEmpty()) { PointWithDistance ptWithDist = openHeap.PopMinimum(); if ((bestPathLength > 0) && ((ptWithDist.estimatedTotalDistance > bestPathLength) || TAKE_FIRST_PATH)) { break; } int[] xOffsets = { 1, 0, -1, 0 }; int[] zOffsets = { 0, 1, 0, -1 }; InclineMeshNode node = nodes[ptWithDist.point.X, ptWithDist.point.Z]; Vector3 worldNodePoint = InclineIndicesToWorldPoint(ptWithDist.point); for (int direction = 0; direction < 4; ++direction) { int dx = xOffsets[direction]; int dz = zOffsets[direction]; int nx = ptWithDist.point.X + dx; int nz = ptWithDist.point.Z + dz; InclineIndexPoint neighborPoint = new InclineIndexPoint(nx, nz); Point mapNeighborPoint = InclineIndicesToMapIndices(neighborPoint); if ((!mapMetaData.IsWithinBounds(mapNeighborPoint)) || (node.NeighborLinks[direction] == null)) { continue; } Vector3 worldNeighborPoint = InclineIndicesToWorldPoint(neighborPoint); for (int linkIndex = 0; linkIndex < node.NeighborLinks[direction].Count; ++linkIndex) { Debug.DrawLine(worldNodePoint, worldNeighborPoint, Color.yellow, 15.0f); InclineLinkData link = node.NeighborLinks[direction][linkIndex]; if ((link.declineAsFloat() > maxDecline) || (link.inclineAsFloat() > maxIncline)) { continue; } float linkDistance = (worldNeighborPoint - worldNodePoint).magnitude; float totalDistance = ptWithDist.distance + linkDistance; if ((bestPathLength >= 0) && (totalDistance >= bestPathLength)) { continue; } if ((!bestDistanceDict.ContainsKey(neighborPoint)) || (totalDistance < bestDistanceDict[neighborPoint])) { bestDistanceDict[neighborPoint] = totalDistance; bestPrevPoint[neighborPoint] = ptWithDist; float distanceToGoal = (worldNeighborPoint - worldGoalPoint).magnitude; if (neighborPoint.Equals(goalPoint)) { if ((bestPathLength < 0) || (totalDistance < bestPathLength)) { bestPathLength = totalDistance; bestGoalPoint = new PointWithDistance(neighborPoint, totalDistance, 0.0f); } } else { openHeap.Push(new PointWithDistance(neighborPoint, totalDistance, totalDistance + distanceToGoal)); } } break; } } } if (bestPathLength >= 0) { PointWithDistance p = bestGoalPoint; path.Add(p); while (bestPrevPoint.ContainsKey(p.point)) { PointWithDistance prevPoint = bestPrevPoint[p.point]; if ((prevPoint == null) || (path.Contains(prevPoint))) { break; } path.Insert(0, prevPoint); p = prevPoint; } } return(path); }