public int InsertAbstractNode(HierarchicalMap map, Position pos, int start)
{
var nodeId = pos.Y * map.Width + pos.X;
var result = InsertStal(map, nodeId, pos, start);
InsertStalHEdges(map, nodeId);
return result;
}
public int EntranceLocalIdx { get; set; } // local id
public LocalEntrance(int nodeId, int absNodeId, int localIdx, Position relativePosition)
{
Id = nodeId;
AbsNodeId = absNodeId;
EntranceLocalIdx = localIdx;
RelativePos = relativePosition;
}
public AbsTilingNodeInfo(int id, int level, int clId,
Position position, int centerId,
int localIdxCluster)
{
Id = id;
Level = level;
ClusterId = clId;
Position = position;
CenterId = centerId;
LocalIdxCluster = localIdxCluster;
}
public AbsTilingNodeInfo(int id, int level, int clId,
Position position, int centerId,
int localEntranceId)
{
Id = id;
Level = level;
ClusterId = clId;
Position = position;
CenterId = centerId;
this.LocalEntranceId = localEntranceId;
}
public Position Origin { get; set; } // The position where this cluster starts in the main grid
public Cluster(ConcreteMap concreteMap, int id, int clusterX, int clusterY, Position origin, Size size)
{
SubConcreteMap = concreteMap.Slice(origin.X, origin.Y, size.Width, size.Height, concreteMap.Passability);
Id = id;
ClusterY = clusterY;
ClusterX = clusterX;
Origin = origin;
Size = size;
Distances = new Dictionary<Tuple<int, int>, int>();
CachedPaths = new Dictionary<Tuple<int, int>, List<int>>();
DistanceCalculated = new Dictionary<Tuple<int, int>, bool>();
EntrancePoints = new List<EntrancePoint>();
}
public static bool AreAligned(Position p1, Position p2)
{
return p1.X == p2.X || p1.Y == p2.Y;
}
public TilingNodeInfo(bool isObstacle, int cost, Position position)
{
IsObstacle = isObstacle;
Position = position;
Cost = cost;
}
/// <summary>
/// Tells whether we can move from p1 to p2 in line. Bear in mind
/// this function does not consider intermediate points (it is
/// assumed you can jump between intermediate points)
/// </summary>
public bool CanJump(Position p1, Position p2)
{
if (TileType != TileType.OCTILE && this.TileType != TileType.OCTILE_UNICOST)
return true;
if (Helpers.AreAligned(p1, p2))
return true;
// The following piece of code existed in the original implementation.
// It basically checks that you do not forcefully cross a blocked diagonal.
// Honestly, this is weird, bad designed and supposes that each position is adjacent to each other.
var nodeInfo12 = Graph.GetNode(GetNodeIdFromPos(p2.X, p1.Y)).Info;
var nodeInfo21 = Graph.GetNode(GetNodeIdFromPos(p1.X, p2.Y)).Info;
return !(nodeInfo12.IsObstacle && nodeInfo21.IsObstacle);
}
// insert a new node, such as start or target, to the abstract graph and
// returns the id of the newly created node in the abstract graph
// x and y are the positions where I want to put the node
private int InsertStal(HierarchicalMap map, int nodeId, Position pos, int start)
{
// If the node already existed (for instance, it was the an entrance point already
// existing in the graph, we need to keep track of the previous status in order
// to be able to restore it once we delete this STAL
if (map.AbsNodeIds[nodeId] != Constants.NO_NODE)
{
m_stalLevel[start] = map.AbstractGraph.GetNodeInfo(map.AbsNodeIds[nodeId]).Level;
m_stalEdges[start] = map.GetNodeEdges(nodeId);
m_stalUsed[start] = true;
return map.AbsNodeIds[nodeId];
}
m_stalUsed[start] = false;
var cluster = map.FindClusterForPosition(pos);
// create global entrance
var absNodeId = map.NrNodes;
var localEntranceIdx = cluster.AddEntrance(absNodeId, new Position(pos.X - cluster.Origin.X, pos.Y - cluster.Origin.Y));
cluster.UpdatePaths(localEntranceIdx);
map.AbsNodeIds[nodeId] = absNodeId;
var info = new AbsTilingNodeInfo(
absNodeId,
1,
cluster.Id,
pos,
nodeId,
localEntranceIdx);
map.AbstractGraph.AddNode(absNodeId, info);
// add new edges to the abstract graph
for (var k = 0; k < cluster.GetNrEntrances() - 1; k++)
{
if (cluster.AreConnected(localEntranceIdx, k))
{
map.AddEdge(
cluster.GetGlobalAbsNodeId(k),
cluster.GetGlobalAbsNodeId(localEntranceIdx),
cluster.GetDistance(localEntranceIdx, k));
map.AddEdge(
cluster.GetGlobalAbsNodeId(localEntranceIdx),
cluster.GetGlobalAbsNodeId(k),
cluster.GetDistance(k, localEntranceIdx));
}
}
return absNodeId;
}
public bool CanEnter(Position pos, out int cost)
{
cost = Constants.COST_ONE;
return !obstacles[pos.X, pos.Y];
}
public int EntranceLocalIdx { get; set; } // local id
public EntrancePoint(int absNodeId, int localIdx, Position relativePosition)
{
AbsNodeId = absNodeId;
EntranceLocalIdx = localIdx;
RelativePos = relativePosition;
}
/// <summary>
/// Adds an entrance point to the cluster and returns the entrance index assigned for the point
/// </summary>
public int AddEntrance(int abstractNodeId, Position relativePosition)
{
var entranceLocalIdx = EntrancePoints.Count;
var localEntrance = new EntrancePoint(
abstractNodeId,
EntrancePoints.Count,
relativePosition);
EntrancePoints.Add(localEntrance);
return entranceLocalIdx;
}
/// <summary>
/// Defines the bounding box of the cluster we want to process based on a given level and a position in the grid
/// </summary>
public void SetCurrentCluster(Position pos, int level)
{
// if the level surpasses the MaxLevel, just set the whole map as a cluster
if (level > MaxLevel)
{
this.currentClusterY0 = 0;
this.currentClusterY1 = this.Height - 1;
this.currentClusterX0 = 0;
this.currentClusterX1 = this.Width - 1;
return;
}
var offset = GetOffset(level);
var nodeY = pos.Y; // nodeId / this.Width;
var nodeX = pos.X; // nodeId % this.Width;
this.currentClusterY0 = nodeY - (nodeY % offset);
this.currentClusterY1 = Math.Min(this.Height - 1, this.currentClusterY0 + offset - 1);
this.currentClusterX0 = nodeX - (nodeX % offset);
this.currentClusterX1 = Math.Min(this.Width - 1, this.currentClusterX0 + offset - 1);
}
public bool PositionInCurrentCluster(Position position)
{
var y = position.Y;
var x = position.X;
return y >= this.currentClusterY0 && y <= this.currentClusterY1 && x >= this.currentClusterX0 && x <= this.currentClusterX1;
}
public Cluster FindClusterForPosition(Position pos)
{
var cluster = this.Clusters
.First(cl =>
cl.Origin.Y <= pos.Y &&
pos.Y < cl.Origin.Y + cl.Size.Height &&
cl.Origin.X <= pos.X &&
pos.X < cl.Origin.X + cl.Size.Width);
return cluster;
}
