private bool FindPath(Board board, CellPath path, Cell origin)
{
var availableCells = board.GetAvailableCells(origin);
foreach (var cell in availableCells)
{
path.AddCell(cell);
//Console.WriteLine(_printingService.ToString(board, path));
if (board.IsSolved())
{
return(true);
}
if (!BoardHasStillSolution(board, cell))
{
//Console.WriteLine("Impossible solution, rolling back.");
}
else
{
if (FindPath(board, path, cell))
{
return(true);
}
}
path.Undo(1);
}
return(false);
}
void OnDestroy()
{
if (instance == this)
{
instance = null;
}
}
/// <summary>
/// Rate paths based on their length and deviation from the goal bearing.
/// Better paths have higher value.
/// </summary>
/// <param name="cellPath"></param>
/// <param name="goalBearingRelative"></param>
/// <returns></returns>
private double pathRatingFunction(CellPath cellPath, double goalBearingRelative, double sweepAngleHalf)
{
double overObstacleLength = cellPath.lengthMeters - ObstacleDistanceMeters;
if (overObstacleLength < 0.0d)
{
return(0.0d);
}
double distanceFactor = Math.Min((pathDistanceMax - ObstacleDistanceMeters) * 0.9d, overObstacleLength);
double bearing = cellPath.firstHeadingRelative - goalBearingRelative;
bearing = Direction.to180(bearing);
// at this point bearing is between -180...180
if (bearing > sweepAngleHalf)
{
return(0.0d);
}
double deflection = Math.Abs(bearing) / sweepAngleHalf; // 0 pointing to goal, 1 pointing to the side ray.
double directionFactor = 1.0d - 0.9d * deflection; // 1.0 when pointing to goal, 0.1 when pointing to the side ray
double rating = distanceFactor * directionFactor;
//double rating = directionFactor;
return(rating);
}
private void addCandidate(CellPathData c, int w, int di, int dj, List <CellPath> candidates, CellPath path)
{
var i = c.i;
var j = c.j;
// The total weight of the candidate is the weight of previous path
// plus its weight (calculated based on occupancy and speed factor)
var distanceToDestination = Math.Sqrt((di - i) * (di - i) + (dj - j) * (dj - j));
w = w / c.speed + c.weight;
if (c.candidateRef == null)
{
var candidateRef = new CellPath(i, j, path.w + w, (int)distanceToDestination, path);
//candidates.push(candidateRef);
candidates.Add(candidateRef);
c.candidateRef = candidateRef;
}
else
{
var currentWeight = c.candidateRef.w;
var newWeight = path.w + w;
if (newWeight < currentWeight)
{
c.candidateRef.w = newWeight;
c.candidateRef.path = path;
}
}
}
void Awake()
{
if ( instance )
Destroy( this );
else
instance = this;
}
// Constructor
internal CellPath(int i, int j, double w, double d, CellPath path)
{
I = i;
J = j;
W = w;
D = d;
Path = path;
}
public CellPath(int i, int j, int w, int d, CellPath path)
{
this.i = i; // position i in the grid
this.j = j; // position j in the grid
this.w = w; // weight of the path
this.d = d; // remaining distance to destination
// positions previously taken in the path
this.path = path;
}
void Awake()
{
if (instance)
{
Destroy(this);
}
else
{
instance = this;
}
}
public CellPathData(int i, int j)
{
this.i = i;
this.j = j;
this.floor = -1;
this.zone = -1;
this.speed = 1;
this.weight = 0;
this.candidateRef = null;
}
internal CellPath Solve(Board board)
{
var path = new CellPath();
path.AddCell(board.Start);
FindPath(board, path, board.Start);
if (board.IsSolved())
{
return(path);
}
throw new Exception("Impossible problem");
}
private string PrintPathWay(Board board, CellPath path, int index)
{
if (IsSolutionLastStep(board, path, index))
{
return("╬");
}
else if (path.Count - 1 == index)
{
return(ToString(path.Last()));
}
var orientation = _orientationService.GetOrientation(path[index], path[index + 1]);
return(ToString(orientation));
}
public string ToString(Board board, CellPath path)
{
return(DrawBoard(board, (b, x, y) =>
{
var pathCell = path.FirstIndexOf(c => c.Equals(x, y));
if (pathCell.Index != -1)
{
return PrintPathWay(board, path, pathCell.Index);
}
else
{
return ToString(board.GetCell(x, y));
}
}
));
}
/// <summary>
/// registers a geo cell at (x,y) with the cell path, storing current pathDistance.
/// </summary>
/// <param name="cellPath"></param>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="pathDistance"></param>
/// <returns></returns>
protected bool registerCell(CellPath cellPath, int x, int y, double pathDistance, out MapCell mc)
{
mc = _mapper.geoCellAt(x, y);
if (mc == null || mc.val > 0)
{
return(true); // ray hit an obstacle or the wall
}
//if (!cellPath.ContainsCell(mc))
{
cellPath.Add(new CellPathElement()
{
mapCell = mc, distanceMeters = pathDistance, cellPath = cellPath
});
}
return(false); // not hit any obstacle or wall
}
/// <summary>
/// Rate paths based on their length and deviation from the goal bearing.
/// Better paths have higher value.
/// </summary>
/// <param name="cellPath"></param>
/// <param name="goalBearingRelative"></param>
/// <param name="distanceToGoalMeters"></param>
/// <param name="sweepAngleHalf"></param>
/// <returns></returns>
private double pathRatingFunction(CellPath cellPath, double goalBearingRelative, double?distanceToGoalMeters, double sweepAngleHalf)
{
double overObstacleLength = cellPath.lengthMeters - ObstacleDistanceMeters;
if (overObstacleLength < 0.0d)
{
return(0.0d); // if an obstacle comes within ObstacleDistanceMeters the robot stops moving. Rate shorter paths poorly.
}
double distanceFactor = Math.Min((pathDistanceMax - ObstacleDistanceMeters) * 0.9d, overObstacleLength);
double bearing = cellPath.firstHeadingRelative - goalBearingRelative;
bearing = Direction.to180(bearing);
// at this point bearing is between -180...180
if (bearing > sweepAngleHalf)
{
return(0.0d); // rate poorly all paths outside the sweep sector
}
double deflection = Math.Abs(bearing) / sweepAngleHalf; // 0 pointing to goal, 1 pointing to the side ray.
double directionFactor = 1.0d - 0.9d * deflection; // 1.0 when pointing to goal, 0.1 when pointing to the side ray
double rating = distanceFactor * directionFactor;
//double rating = directionFactor;
if (distanceToGoalMeters.HasValue && deflection < 0.1 && cellPath.lengthMeters > distanceToGoalMeters.Value)
{
rating *= 1000000.0d; // highly rate paths directly hitting the goal.
}
return(rating);
}
///// <summary>
///// </summary>
//public void readMission()
//{
// if (File.Exists(missionFilename))
// {
// using (TextReader reader = new StreamReader(missionFilename))
// {
// string line;
// while ((line = reader.ReadLine()) != null)
// {
// Tracer.Trace(line);
// }
// }
// }
//}
/// <summary>
/// creates a RoutePlan, based on mapper's robotDirection and cells on the geo plane (using _mapper.geoCellAt()); analyses obstacles (busy cells)
/// </summary>
/// <returns></returns>
public RoutePlan planRoute()
{
DateTime started = DateTime.Now;
RoutePlan plan = new RoutePlan();
// note: _mapper.robotDirection.distanceToGoalMeters may contain distance to goal, in which case a shorter leg going straight to and ending at goal wins
if (_mapper.robotDirection.heading.HasValue)
{
double?distanceToGoalMeters = _mapper.robotDirection.distanceToGoalMeters;
lock (this)
{
try
{
cellPaths.Clear();
double sweepAngleHalf = sweepAngleNormal / 2.0d;
double goalBearingRelative = 0.0d; // straight in front is default
if (_mapper.robotDirection.bearing.HasValue)
{
goalBearingRelative = (double)_mapper.robotDirection.turnRelative;
}
if (Math.Abs(goalBearingRelative) > sweepAngleHalf)
{
// robot is pointing away from the goal, make him turn towards the goal first:
plan.bestHeading = Direction.to360(_mapper.robotDirection.course + goalBearingRelative);
plan.legMeters = null;
plan.closestObstacleAlongBestPathMeters = _mapper.robotState.robotLengthMeters / 2.0d;
}
else
{
int nsteps = (int)Math.Round(sweepAngleHalf / raySweepDegrees);
for (int i = -nsteps; i < nsteps; i++)
{
double pathHeadingRelative = raySweepDegrees * i;
Direction dir = new Direction()
{
heading = _mapper.robotDirection.heading, bearingRelative = pathHeadingRelative
}; // related to robot heading;
CellPath cellPath = shootRay(dir);
cellPath.firstHeadingRelative = pathHeadingRelative;
cellPaths.Add(cellPath);
}
// order (low to high) paths based on their length and deviation from the goal bearing - using pathRatingFunction():
CellPath bestPath = cellPaths.OrderBy(c => pathRatingFunction(c, goalBearingRelative, distanceToGoalMeters, sweepAngleHalf)).Last();
bestPath.isBest = true;
plan.bestHeading = Direction.to360(_mapper.robotDirection.course + bestPath.firstHeadingRelative);
plan.legMeters = bestPath.lengthMeters;
plan.closestObstacleAlongBestPathMeters = bestPath.lengthMeters - _mapper.robotState.robotLengthMeters / 2.0d;
}
}
catch (Exception exc)
{
Console.WriteLine("planRoute() - " + exc);
}
}
}
plan.timeSpentPlanning = DateTime.Now - started;
return(plan);
}
/// <summary>
/// fills cells that are along the path from the center to border
/// </summary>
/// <param name="angle">degrees, from vertical up, -180 to 180 or 0...360 (any angle will be brought to -180...180 range)</param>
protected CellPath shootRay(Direction dir)
{
CellPath cellPath = new CellPath();
bool hitObstacle = false;
double pathDistance = _mapper.robotState.robotLengthMeters / 2.0d;
MapCell mc = null;
double angle = (double)dir.bearing;
int startX = nW / 2;
int startY = nH / 2;
int robotWidthCells = (int)Math.Floor(_mapper.robotState.robotWidthMeters / MapperSettings.elementSizeMeters) + 1;
int halfWidth = (int)Math.Ceiling((robotWidthCells - 1) / 2.0d);
angle = Direction.to180(angle);
bool verticalUp = Math.Abs(angle) <= 1.0d;
bool verticalDown = angle >= 179.0d || angle <= -179.0d;
int y = startY;
int dy = angle > 90.0d || angle < -90.0d ? 1 : -1;
if (verticalUp || verticalDown)
{
int endY = verticalUp ? 0 : nH - 1;
while (!hitObstacle && y >= 0 && y < nH)
{
pathDistance = Math.Abs(y - startY) * MapperSettings.elementSizeMeters;
for (int xx = startX - halfWidth; !hitObstacle && xx <= startX + halfWidth; xx++)
{
hitObstacle = registerCell(cellPath, xx, y, pathDistance, out mc);
}
y += dy;
}
}
else
{
double angleR = (90.0d - angle) * Math.PI / 180.0d;
double factor = verticalUp ? 100000.0d : (verticalDown ? -100000.0d : Math.Tan(angleR));
int dx = angle > 0.0d ? 1 : -1;
bool spreadV = angle >= 45.0d && angle <= 135.0d || angle <-45.0d && angle> -135.0d;
for (int x = startX; !hitObstacle && x >= 0 && x < nW && y >= 0 && y < nH; x += dx)
{
double pathDistanceX = Math.Abs(x - startX) * MapperSettings.elementSizeMeters;
double pathDistanceY = Math.Abs(y - startY) * MapperSettings.elementSizeMeters;
pathDistance = Math.Sqrt(pathDistanceX * pathDistanceX + pathDistanceY * pathDistanceY);
if (pathDistance >= pathDistanceMax)
{
break;
}
int endY = Math.Max(0, Math.Min(startY - ((int)Math.Round((x - startX) * factor)), nH - 1));
while (!hitObstacle && y >= 0 && y < nH && (dy > 0 && y <= endY || dy < 0 && y >= endY))
{
if (spreadV)
{
hitObstacle = registerCell(cellPath, x, y, pathDistance, out mc);
}
else
{
for (int xx = Math.Max(0, x - halfWidth); !hitObstacle && xx <= Math.Min(x + halfWidth, nW - 1); xx++)
{
hitObstacle = registerCell(cellPath, xx, y, pathDistance, out mc);
}
}
y += dy;
}
if (spreadV)
{
for (int yy = Math.Max(0, endY - halfWidth); !hitObstacle && yy <= Math.Min(endY + halfWidth, nH - 1); yy++)
{
hitObstacle = registerCell(cellPath, x, yy, pathDistance, out mc);
}
}
else if (!hitObstacle)
{
hitObstacle = registerCell(cellPath, x, endY, pathDistance, out mc);
}
}
}
cellPath.lengthMeters = pathDistance;
cellPath.hitObstacle = hitObstacle;
return(cellPath);
}
private void addCandidates(CellPath path, int di, int dj, List <CellPath> candidates,
bool allowDiagonals = false)
{
var i = path.i;
var j = path.j;
var c = Grid[i][j];
// Searching whether adjacent cells can be candidates to lengthen the path
// Adjacent cells
var c01 = Grid[i - 1][j];
var c10 = Grid[i][j - 1];
var c12 = Grid[i][j + 1];
var c21 = Grid[i + 1][j];
// weight of path in straight line = 1
var weightStraight = 1;
if (areCommunicating(c, c01))
{
addCandidate(c01, weightStraight, di, dj, candidates, path);
}
if (areCommunicating(c, c21))
{
addCandidate(c21, weightStraight, di, dj, candidates, path);
}
if (areCommunicating(c, c10))
{
addCandidate(c10, weightStraight, di, dj, candidates, path);
}
if (areCommunicating(c, c12))
{
addCandidate(c12, weightStraight, di, dj, candidates, path);
}
// Searching whether diagonally adjacent cells can be candidates to lengthen the path
// Diagonally adjacent cells
var c00 = Grid[i - 1][j - 1];
var c02 = Grid[i - 1][j + 1];
var c20 = Grid[i + 1][j - 1];
var c22 = Grid[i + 1][j + 1];
// weight of path in diagonal = Math.sqrt(2)
var weightDiagonal = Math.Sqrt(2);
if (allowDiagonals)
{
if (canMoveDiagonallyTo(c, c00, c01, c10))
{
addCandidate(c00, (int)weightDiagonal, di, dj, candidates, path);
}
if (canMoveDiagonallyTo(c, c20, c21, c10))
{
addCandidate(c20, (int)weightDiagonal, di, dj, candidates, path);
}
if (canMoveDiagonallyTo(c, c02, c01, c12))
{
addCandidate(c02, (int)weightDiagonal, di, dj, candidates, path);
}
if (canMoveDiagonallyTo(c, c22, c21, c12))
{
addCandidate(c22, (int)weightDiagonal, di, dj, candidates, path);
}
}
}
/// <summary>
/// Rate paths based on their length and deviation from the goal bearing.
/// Better paths have higher value.
/// </summary>
/// <param name="cellPath"></param>
/// <param name="goalBearingRelative"></param>
/// <param name="distanceToGoalMeters"></param>
/// <param name="sweepAngleHalf"></param>
/// <returns></returns>
private double pathRatingFunction(CellPath cellPath, double goalBearingRelative, double? distanceToGoalMeters, double sweepAngleHalf)
{
double overObstacleLength = cellPath.lengthMeters - ObstacleDistanceMeters;
if (overObstacleLength < 0.0d)
{
return 0.0d; // if an obstacle comes within ObstacleDistanceMeters the robot stops moving. Rate shorter paths poorly.
}
double distanceFactor = Math.Min((pathDistanceMax - ObstacleDistanceMeters) * 0.9d, overObstacleLength);
double bearing = cellPath.firstHeadingRelative - goalBearingRelative;
bearing = Direction.to180(bearing);
// at this point bearing is between -180...180
if (bearing > sweepAngleHalf)
{
return 0.0d; // rate poorly all paths outside the sweep sector
}
double deflection = Math.Abs(bearing) / sweepAngleHalf; // 0 pointing to goal, 1 pointing to the side ray.
double directionFactor = 1.0d - 0.9d * deflection; // 1.0 when pointing to goal, 0.1 when pointing to the side ray
double rating = distanceFactor * directionFactor;
//double rating = directionFactor;
if (distanceToGoalMeters.HasValue && deflection < 0.1 && cellPath.lengthMeters > distanceToGoalMeters.Value)
{
rating *= 1000000.0d; // highly rate paths directly hitting the goal.
}
return rating;
}
private bool IsSolutionLastStep(Board board, CellPath path, int index)
{
return(board.IsSolved() && path.Count - 1 == index);
}
void GenWalls()
{
if (useRandomSeed)
{
SimpleRNG.SetSeedFromSystemTime();
}
else
{
SimpleRNG.SetSeed((uint)manualSeed);
}
foreach (Cell c in cells)
{
unassignedRooms.Add(c);
}
Cell startRoom = null;
int curRoom = 1;
while (unassignedRooms.Count > 0)
{
// add room of random size
int nextRoomSize = (int)(SimpleRNG.GetUniform() * (maxRoomSize - minRoomSize)) + minRoomSize;
Cell centreCell = unassignedRooms[(int)(SimpleRNG.GetUniform() * (unassignedRooms.Count - 1))];
if (startRoom == null)
{
startRoom = centreCell;
}
// work out ideal bounds of new room
int startX = centreCell.x - Mathf.CeilToInt(nextRoomSize / 2f) + 1;
int endX = Mathf.Min(startX + nextRoomSize, cellsPerSide);
startX = Mathf.Max(0, startX);
int startY = centreCell.y - Mathf.CeilToInt(nextRoomSize / 2f) + 1;
int endY = Mathf.Min(startY + nextRoomSize, cellsPerSide);
startY = Mathf.Max(0, startY);
var roomCells = new List <Cell>();
var lastInRoom = new List <int>(); // which rows in the last column had a room on it? If no rows match, column won't be inited and room will stop. Avoids split rooms
for (int x = startX; x < endX; x++)
{
var cellsThisColumn = new List <int>();
if (lastInRoom.Count == 0)
{
// no cells in room yet, add first block
bool started = false;
for (int y = startY; y < endY; y++)
{
if (cells[x, y].room == 0)
{
cellsThisColumn.Add(y);
started = true;
}
else if (started)
{
break;
}
}
}
else
{
// add last column's rooms to this column if valid, then spread up and down until hits another room
foreach (int roomRow in lastInRoom)
{
if (!cellsThisColumn.Contains(roomRow) && cells[x, roomRow].room == 0)
{
cellsThisColumn.Add(roomRow);
for (int south = roomRow - 1; south >= startY; south--)
{
if (cells[x, south].room == 0)
{
cellsThisColumn.Add(south);
}
else
{
break;
}
}
for (int north = roomRow + 1; north < endY; north++)
{
if (cells[x, north].room == 0)
{
cellsThisColumn.Add(north);
}
else
{
break;
}
}
}
}
// if no valid connection after room has started, stop making room
if (cellsThisColumn.Count == 0)
{
break;
}
}
// actually make rooms
foreach (int row in cellsThisColumn)
{
// for each cell within room edges, add walls between neighbouring rooms (if not in another room already)
// add each valid room to list, and if can't path to first room after all rooms done, make holes
Cell roomCell = cells[x, row];
if (AddCellToRoom(roomCell, curRoom))
{
roomCells.Add(roomCell);
}
}
lastInRoom = cellsThisColumn;
}
Debug.Log("Room made");
PrintLayout();
// try to path to start room
if (roomCells.Count > 0 && CellPath.PathTo(startRoom.centrePosition, roomCells[0].centrePosition) == null)
{
// no path, make corridor to first cell
Cell pathEnd = null;
int distToTarg = int.MaxValue;
foreach (Cell edgeCell in roomCells)
{
int newDist = Mathf.Abs(edgeCell.x - startRoom.x) + Mathf.Abs(edgeCell.y - startRoom.y);
if (newDist < distToTarg)
{
distToTarg = newDist;
pathEnd = edgeCell;
}
}
while (pathEnd.room == curRoom)
{
Debug.Log("Opening path from " + pathEnd);
int xDist = startRoom.x - pathEnd.x;
int yDist = startRoom.y - pathEnd.y;
if (xDist >= Mathf.Abs(yDist))
{
pathEnd = OpenCellInDirection(pathEnd, Direction.East);
}
else if (xDist <= -Mathf.Abs(yDist))
{
pathEnd = OpenCellInDirection(pathEnd, Direction.West);
}
else if (yDist > Mathf.Abs(xDist))
{
pathEnd = OpenCellInDirection(pathEnd, Direction.North);
}
else if (yDist < -Mathf.Abs(xDist))
{
pathEnd = OpenCellInDirection(pathEnd, Direction.South);
}
}
// check if can path. JUST IN CASE
if (CellPath.PathTo(startRoom.centrePosition, roomCells[0].centrePosition) == null)
{
Debug.LogWarning("Still no path from room " + curRoom);
PrintLayout();
}
}
curRoom++;
}
Debug.Log("Layout complete...");
PrintLayout();
// Instantiate walls?
var verticalWalls = new Cell[cellsPerSide, cellsPerSide];
for (int x = 0; x < cellsPerSide - 1; x++)
{
int wallType = Random.Range(0, wallPrefabs.Length);
for (int y = 0; y < cellsPerSide; y++)
{
if (!cells[x, y].canGoEast)
{
CreateWall(cells[x, y], Direction.East, wallType);
verticalWalls[x, y] = cells[x, y];
if (y > 0 && verticalWalls[x, y - 1] == null)
{
CreateWallCap(cells[x, y], true);
}
}
else
{
wallType = Random.Range(0, wallPrefabs.Length);
if (y > 0 && verticalWalls[x, y - 1] != null)
{
CreateWallCap(cells[x, y], true);
}
}
}
}
var horizontalWalls = new Cell[cellsPerSide, cellsPerSide];
for (int y = 0; y < cellsPerSide - 1; y++)
{
int wallType = Random.Range(0, wallPrefabs.Length);
for (int x = 0; x < cellsPerSide; x++)
{
if (!cells[x, y].canGoNorth)
{
CreateWall(cells[x, y], Direction.North, wallType);
horizontalWalls[x, y] = cells[x, y];
if (x > 0 && horizontalWalls[x - 1, y] == null)
{
CreateWallCap(cells[x, y], false);
}
}
else
{
wallType = Random.Range(0, wallPrefabs.Length);
if (x > 0 && horizontalWalls[x - 1, y] != null)
{
CreateWallCap(cells[x, y], false);
}
}
}
}
}
void OnDestroy()
{
if ( instance == this )
instance = null;
}
public Task <int[]> GetPath(Map map, int startCellId, int destCellId, List <int> occupiedCells,
bool allowDiagonals = false,
bool stopNextToTarget = false)
{
CellPath candidate = null;
fillPathGrid(map, map.CellChangeMaps.Count == 0);
var srcPos = getMapPoint(startCellId); // source index
var dstPos = getMapPoint(destCellId); // source index
var si = srcPos.X + 1;
var sj = srcPos.Y + 1;
var srcCell = Grid[si][sj];
if (srcCell.zone == -1)
{
// Searching for accessible cell around source
CellPathData bestFit = null;
var bestDist = Math.Pow(10, 1000);
var bestFloorDiff = Math.Pow(10, 1000);
for (var i = -1; i <= 1; i += 1)
{
for (var j = -1; j <= 1; j += 1)
{
if (i == 0 && j == 0)
{
continue;
}
var cell = Grid[si + i][sj + j];
if (cell.zone == -1)
{
continue;
}
var floorDiff = Math.Abs(cell.floor - srcCell.floor);
var dist = Math.Abs(i) + Math.Abs(j);
if (bestFit == null || floorDiff < bestFloorDiff ||
(floorDiff <= bestFloorDiff && dist < bestDist))
{
bestFit = cell;
bestDist = dist;
bestFloorDiff = floorDiff;
}
}
}
if (bestFit != null)
{
return(Task.FromResult(new int[2] {
startCellId, getCellId(bestFit.i - 1, bestFit.j - 1)
}));
}
Console.WriteLine("[pathFinder.getPath] Player is stuck in {0}/{1}", si, sj);
return(Task.FromResult(new int[1] {
startCellId
}));
}
var di = dstPos.X + 1; // destination i
var dj = dstPos.Y + 1; // destination j
// marking cells as occupied
Point cellPos;
//var cellId = 0;
foreach (var cellId in occupiedCells)
{
cellPos = getMapPoint(cellId);
Grid[cellPos.X + 1][cellPos.Y + 1].weight += OCCUPIED_CELL_WEIGHT;
}
// First cell in the path
var distSrcDst = Math.Sqrt((si - di) * (si - di) + (sj - dj) * (sj - dj));
var selection = new CellPath(si, sj, 0, (int)distSrcDst, null);
List <CellPath> candidates = new List <CellPath>();
List <CellPath> selections = new List <CellPath>();
// Adding cells to path until destination has been reached
CellPath reachingPath = null;
var closestPath = selection;
while (selection.i != di || selection.j != dj)
{
addCandidates(selection, di, dj, candidates, allowDiagonals);
// Looking for candidate with the smallest additional length to path
// in O(number of candidates)
var n = candidates.Count;
if (n == 0)
{
// No possible path
// returning the closest path to destination
selection = closestPath;
break;
}
var minPotentialWeight = Math.Pow(10, 1000);
var selectionIndex = 0;
for (int c = 0; c < n; c += 1)
{
candidate = candidates[c];
if (candidate.w + candidate.d < minPotentialWeight)
{
selection = candidate;
minPotentialWeight = candidate.w + candidate.d;
selectionIndex = c;
}
}
selections.Add(selection);
candidates.RemoveAt(selectionIndex);
// If stopNextToTarget
// then when reaching a distance of less than Math.sqrt(2) the destination is considered as reached
// (the threshold has to be bigger than sqrt(2) but smaller than 2, to be safe we use the value 1.5)
if (selection.d == 0 || (stopNextToTarget && selection.d < 1.5))
{
// Selected path reached destination
if (reachingPath == null || selection.w < reachingPath.w)
{
reachingPath = selection;
closestPath = selection;
// Clearing candidates dominated by current solution to speed up the algorithm
List <CellPath> trimmedCandidates = new List <CellPath>();
for (int c = 0; c < candidates.Count; c += 1)
{
candidate = candidates[c];
if (candidate.w + candidate.d < reachingPath.w)
{
trimmedCandidates.Add(candidate);
}
else
{
Grid[candidate.i][candidate.j].candidateRef = null;
}
}
candidates = trimmedCandidates;
}
}
else
{
if (selection.d < closestPath.d)
{
// 'selection' is the new closest path to destination
closestPath = selection;
}
}
}
// Removing candidate reference in each cell in selections and active candidates
for (int c = 0; c < candidates.Count; c += 1)
{
candidate = candidates[c];
Grid[candidate.i][candidate.j].candidateRef = null;
}
for (var s = 0; s < selections.Count; s += 1)
{
selection = selections[s];
Grid[selection.i][selection.j].candidateRef = null;
}
// Marking cells as unoccupied
foreach (var cell in occupiedCells)
{
cellPos = getMapPoint(cell);
Grid[cellPos.X + 1][cellPos.Y + 1].weight -= OCCUPIED_CELL_WEIGHT;
}
List <int> shortestPath = new List <int>();
while (closestPath != null)
{
shortestPath.Add(getCellId(closestPath.i - 1, closestPath.j - 1));
closestPath = closestPath.path;
}
return(Task.FromResult(shortestPath.ToArray()));
}
/// <summary>
/// Rate paths based on their length and deviation from the goal bearing.
/// Better paths have higher value.
/// </summary>
/// <param name="cellPath"></param>
/// <param name="goalBearingRelative"></param>
/// <returns></returns>
private double pathRatingFunction(CellPath cellPath, double goalBearingRelative, double sweepAngleHalf)
{
double overObstacleLength = cellPath.lengthMeters - ObstacleDistanceMeters;
if (overObstacleLength < 0.0d)
{
return 0.0d;
}
double distanceFactor = Math.Min((pathDistanceMax - ObstacleDistanceMeters) * 0.9d, overObstacleLength);
double bearing = cellPath.firstHeadingRelative - goalBearingRelative;
bearing = Direction.to180(bearing);
// at this point bearing is between -180...180
if (bearing > sweepAngleHalf)
{
return 0.0d;
}
double deflection = Math.Abs(bearing) / sweepAngleHalf; // 0 pointing to goal, 1 pointing to the side ray.
double directionFactor = 1.0d - 0.9d * deflection; // 1.0 when pointing to goal, 0.1 when pointing to the side ray
double rating = distanceFactor * directionFactor;
//double rating = directionFactor;
return rating;
}
/// <summary>
/// fills cells that are along the path from the center to border
/// </summary>
/// <param name="angle">degrees, from vertical up, -180 to 180 or 0...360 (any angle will be brought to -180...180 range)</param>
protected CellPath shootRay(Direction dir)
{
CellPath cellPath = new CellPath();
bool hitObstacle = false;
double pathDistance = _mapper.robotState.robotLengthMeters / 2.0d;
MapCell mc = null;
double angle = (double)dir.bearing;
int startX = nW / 2;
int startY = nH / 2;
int robotWidthCells = (int)Math.Floor(_mapper.robotState.robotWidthMeters / MapperSettings.elementSizeMeters) + 1;
int halfWidth = (int)Math.Ceiling((robotWidthCells - 1) / 2.0d);
angle = Direction.to180(angle);
bool verticalUp = Math.Abs(angle) <= 1.0d;
bool verticalDown = angle >= 179.0d || angle <= -179.0d;
int y = startY;
int dy = angle > 90.0d || angle < -90.0d ? 1 : -1;
if (verticalUp || verticalDown)
{
int endY = verticalUp ? 0 : nH - 1;
while (!hitObstacle && y >= 0 && y < nH)
{
pathDistance = Math.Abs(y - startY) * MapperSettings.elementSizeMeters;
for (int xx = startX - halfWidth; !hitObstacle && xx <= startX + halfWidth; xx++)
{
hitObstacle = registerCell(cellPath, xx, y, pathDistance, out mc);
}
y += dy;
}
}
else
{
double angleR = (90.0d - angle) * Math.PI / 180.0d;
double factor = verticalUp ? 100000.0d : (verticalDown ? -100000.0d : Math.Tan(angleR));
int dx = angle > 0.0d ? 1 : -1;
bool spreadV = angle >= 45.0d && angle <= 135.0d || angle < -45.0d && angle > -135.0d;
for (int x = startX; !hitObstacle && x >= 0 && x < nW && y >= 0 && y < nH; x += dx)
{
double pathDistanceX = Math.Abs(x - startX) * MapperSettings.elementSizeMeters;
double pathDistanceY = Math.Abs(y - startY) * MapperSettings.elementSizeMeters;
pathDistance = Math.Sqrt(pathDistanceX * pathDistanceX + pathDistanceY * pathDistanceY);
if (pathDistance >= pathDistanceMax)
{
break;
}
int endY = Math.Max(0, Math.Min(startY - ((int)Math.Round((x - startX) * factor)), nH - 1));
while (!hitObstacle && y >= 0 && y < nH && (dy > 0 && y <= endY || dy < 0 && y >= endY))
{
if (spreadV)
{
hitObstacle = registerCell(cellPath, x, y, pathDistance, out mc);
}
else
{
for (int xx = Math.Max(0, x - halfWidth); !hitObstacle && xx <= Math.Min(x + halfWidth, nW - 1); xx++)
{
hitObstacle = registerCell(cellPath, xx, y, pathDistance, out mc);
}
}
y += dy;
}
if (spreadV)
{
for (int yy = Math.Max(0, endY - halfWidth); !hitObstacle && yy <= Math.Min(endY + halfWidth, nH - 1); yy++)
{
hitObstacle = registerCell(cellPath, x, yy, pathDistance, out mc);
}
}
else if(!hitObstacle)
{
hitObstacle = registerCell(cellPath, x, endY, pathDistance, out mc);
}
}
}
cellPath.lengthMeters = pathDistance;
cellPath.hitObstacle = hitObstacle;
return cellPath;
}
/// <summary>
/// registers a geo cell at (x,y) with the cell path, storing current pathDistance.
/// </summary>
/// <param name="cellPath"></param>
/// <param name="x"></param>
/// <param name="y"></param>
/// <param name="pathDistance"></param>
/// <returns></returns>
protected bool registerCell(CellPath cellPath, int x, int y, double pathDistance, out MapCell mc)
{
mc = _mapper.geoCellAt(x, y);
if (mc == null || mc.val > 0)
{
return true; // ray hit an obstacle or the wall
}
//if (!cellPath.ContainsCell(mc))
{
cellPath.Add(new CellPathElement() { mapCell = mc, distanceMeters = pathDistance, cellPath = cellPath });
}
return false; // not hit any obstacle or wall
}
