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);
}
