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