static Cell FindNearestCell(ACircularUnit my)
{
double ds = Const.MapSize / GridSize;
var I = (int)(my.X / ds + Const.Eps);
var J = (int)(my.Y / ds + Const.Eps);
int seldI = int.MaxValue, seldJ = int.MaxValue;
double minDist = int.MaxValue;
var obstacles = _obstacles
.Concat(BuildingsObserver.Buildings)
.Concat(TreesObserver.Trees)
.ToArray();
for (var di = 0; di < 2; di++)
{
for (var dj = 0; dj < 2; dj++)
{
var dst = WizardPath.GetSegmentWeight(_points[I + di, J + dj], my, true);
if (dst < minDist &&
obstacles.All(ob =>
!Geom.SegmentCircleIntersects(my, _points[I + di, J + dj], ob, ob.Radius + my.Radius + MagicConst.RadiusAdditionalEpsilon))
)
{
minDist = dst;
seldI = di;
seldJ = dj;
}
}
}
if (seldI == int.MaxValue)
{
return(null);
}
return(new Cell(I + seldI, J + seldJ));
}
public static void DijkstraStart(Cell start, DijkstraCellStopFunc stopCondition, DijkstraPointCostFunc costFunc)
{
var q = new PriorityQueue <Pair <double, Cell> >();
q.Push(new Pair <double, Cell>(0.0, start));
for (var i = 0; i <= GridSize; i++)
{
for (var j = 0; j <= GridSize; j++)
{
_distMap[i, j] = Const.Infinity;
_distPrev[i, j] = null;
}
}
_distMap[start.I, start.J] = 0;
while (q.Count > 0)
{
var cur = q.Top().Second;
var minDist = -q.Top().First;
q.Pop();
if (minDist > _distMap[cur.I, cur.J])
{
continue;
}
if (stopCondition(cur))
{
break;
}
for (var k = 0; k < _dx.Length; k++)
{
var I = cur.I + _dx[k];
var J = cur.J + _dy[k];
if (I < 0 || J < 0 || I > GridSize || J > GridSize)
{
continue;
}
var distTo = _distMap[cur.I, cur.J] + WizardPath.GetSegmentWeight(_points[cur.I, cur.J], _points[I, J], false);
if (costFunc != null)
{
distTo += costFunc(_points[I, J]);
}
if (distTo < _distMap[I, J] && CanPass(_points[cur.I, cur.J], _points[I, J]))
{
_distMap[I, J] = distTo;
_distPrev[I, J] = cur;
q.Push(new Pair <double, Cell>(-distTo, new Cell(I, J)));
}
}
;
}
}
