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 }