/// <summary>
/// Enumerates the neighbors points for a given node.
/// </summary>
/// <param name="currentNode">The current node.</param>
/// <param name="stopFunction">The stop function.</param>
/// <returns></returns>
protected virtual IEnumerable <Point> OnEnumerateNeighbors(TNode currentNode, StopFunction stopFunction)
{
return(Directions.
// creates next step in this direction from current position
Select(direction => DirectionHelper.GetNextStep(currentNode.Point, direction)).
// selects only points that are within bounds of map
Where(point => point.X >= 0 && point.Y >= 0 && point.X < Width && point.Y < Height));
}
private static DirectionType RotateUntil(Point startPoint, DirectionType direction, StopFunction stopFunction, bool leftSide, bool untilFree)
{
bool condition;
do // rotates until the conditions are fullfilled (determined by untilFree)
{
direction = DirectionHelper.Rotate(direction, leftSide, false);
Point nextLeftPoint = DirectionHelper.GetNextStep(startPoint, direction);
condition = untilFree ? !stopFunction(nextLeftPoint.X, nextLeftPoint.Y) : stopFunction(nextLeftPoint.X, nextLeftPoint.Y);
}while (!condition);
return(direction);
}
/// <summary>
/// Enumerates the neighbors points for a given node.
/// </summary>
/// <param name="currentNode">The current node.</param>
/// <param name="stopFunction">The stop function.</param>
/// <returns></returns>
protected override IEnumerable <Vector2> OnEnumerateNeighbors(TNode currentNode, StopFunction <Vector2> stopFunction)
{
return(Directions.
// creates next step in this direction from current position
Select(direction => DirectionHelper.GetNextStep(currentNode.Value, direction)));
}
private void GenerateDistanceMap(Bitmap bitmap, bool showDistanceMap)
{
// cannot create map for invalid image, just skip it
if (bitmap == null)
{
return;
}
// prepares for the processing
List <Point> nextRound = new List <Point>();
// resets the distance map to Empty = 0, Blocked = 1
for (int y = 0; y < Height; y++)
{
for (int x = 0; x < Width; x++)
{
Point point = new Point(x, y);
// determines whether the pixel is blocked, or on the boundaries (disables off-screen pathfinding)
if (x == 0 || y == 0 || x == Width - 1 || y == Height - 1 || OnIsBlocked(x, y))
{
this[x, y] = 1;
nextRound.Add(point);
}
else
{
this[x, y] = 0;
}
}
}
// round to does actual classification
ushort round = 2;
do // perform rounds if there is still at least one pixel to process
{
// switches the next round for current round
List <Point> thisRound = nextRound;
nextRound = new List <Point>();
// processes all the neighbor points along the border of already classified pixels
foreach (Point point in thisRound)
{
foreach (DirectionType direction in Directions)
{
Point neighborPoint = DirectionHelper.GetNextStep(point, direction);
// checks whether it is within valid bounds
if (neighborPoint.X >= 0 && neighborPoint.Y >= 0 &&
neighborPoint.X < Width && neighborPoint.Y < Height)
{
ushort value = this[neighborPoint.X, neighborPoint.Y];
// if this neighbor is still unclassified, do it, and add it for the next round
if (value == 0)
{
if (showDistanceMap)
{
DrawDistanceIntensity(bitmap, neighborPoint.X, neighborPoint.Y, round);
}
this[neighborPoint.X, neighborPoint.Y] = round;
nextRound.Add(neighborPoint);
}
}
}
}
round++;
}while (nextRound.Count > 0);
}
private static bool TryScanObstacle(
Point startPoint, // starting point (one before hitting the obstacle)
DirectionType direction, // direction of hitting the obstacle
StopFunction stopFunction,
IEnumerable <PathSegment> segments, // path segments detected from start to end
int segmentIndex, // starting segment in which the obstacle was hit
out EvasionObstacleInfo obstacleInfo) // useful informations about obstacle, and optimal path around
{
// initializes the result structures
bool result = false;
List <Point> cornerPointList = new List <Point>();
obstacleInfo = default(EvasionObstacleInfo);
// detects all the starting points from relevant segments (to be tested for potential evading path end)
IList <Point> finishPoints = segments.
Skip(segmentIndex + 1).
Select(segment => segment.FirstPoint).
ToList();
// initalizes the parameters
int oldSegmentIndex = segmentIndex;
Point position = startPoint;
int totalStepCount = 0;
// expected direction in which start point should be re-entered (this check is essential)
DirectionType entryDirection = RotateUntil(startPoint, direction, stopFunction, false, true);
entryDirection = DirectionHelper.Reverse(entryDirection);
// rotates until the direction is no longer in a collision course (in a given hand side)
direction = RotateUntil(startPoint, direction, stopFunction, true, true);
do
{
// retrieves next point in a actual direction
Point nextPoint = DirectionHelper.GetNextStep(position, direction);
totalStepCount++;
// we've ended up in the start position, that means we can terminate this scan
if (nextPoint == startPoint && direction == entryDirection)
{
break;
}
// detects whether this point coincides with any finish point
int finishPointIndex = finishPoints.IndexOf(nextPoint);
// we've ended up on ther other side, path was found, continue if we can find better finish point (in a higher segment)
if (finishPointIndex >= 0 && finishPointIndex + oldSegmentIndex >= segmentIndex)
{
obstacleInfo = new EvasionObstacleInfo(cornerPointList, totalStepCount, finishPointIndex);
result = true;
}
// rotates (starting at opposite direction) from the wall until it finds a passable spot
DirectionType previousDirection = direction;
direction = RotateUntil(nextPoint, DirectionHelper.Reverse(direction), stopFunction, true, true);
if (direction != previousDirection)
{
cornerPointList.Add(nextPoint);
}
// advances to next point
position = nextPoint;
}while (true);
// returns the points, and operation result
if (result)
{
obstacleInfo.SetTotalStepCount(totalStepCount);
}
return(result);
}
