private bool PathIntersectsAnyPointLine(PathFindingState state, Point point)
{
var pathPointLine = new PointLine(state.Point, point);
foreach (var pointLine in _environment.PointLines)
{
if (PathIntersectsPointLine(state, pointLine, pathPointLine, point))
{
return(true);
}
}
return(false);
}
private bool PathIntersectsPointLine(PathFindingState state, PointLine pointLine, PointLine pathPointLine, Point point)
{
if (pathPointLine.M == pointLine.M)
{
return(false);
}
double intersection;
if (double.IsInfinity(pathPointLine.M) && double.IsInfinity(pointLine.M))
{
return(false);
}
else if (double.IsInfinity(pathPointLine.M))
{
intersection = pathPointLine.Point1.X;
}
else if (double.IsInfinity(pointLine.M))
{
intersection = pointLine.Point1.X;
}
else
{
var x = pathPointLine.M - pointLine.M;
var t = pointLine.T - pathPointLine.T;
intersection = t / x;
}
if (state.Point.X < point.X)
{
if (intersection <= state.Point.X || intersection >= point.X)
{
return(false);
}
}
else
{
if (intersection >= state.Point.X || intersection <= point.X)
{
return(false);
}
}
return(true);
}
private void ButtonSolveClick(object sender, RoutedEventArgs e)
{
if (_currentPolyLine.Points.Count >= 2)
{
_currentPolyLine.Points.Add(_currentPolyLine.Points[0]);
}
var initialState = new PathFindingState(new Search.Types.Point(100, 100));
var goalState = new PathFindingState(new Search.Types.Point(800, 600));
var environment = new PathFindingEnvironment();
foreach (var polyLine in _polyLines)
{
var points = polyLine.Points.Select(p => p).Distinct().ToList();
for (int i = 0; i < points.Count; i++)
{
for (int j = i + 1; j < points.Count; j++)
{
if (i == j) continue;
var pointLine = new PointLine(
new Point(points[i].X, points[i].Y),
new Point(points[j].X, points[j].Y));
environment.PointLines.Add(pointLine);
}
}
}
var actionFunction = new PathFindingActionFunction(environment, goalState);
var resultFunction = new PathFindingResultFunction();
var goalTest = new PathFindingGoalTest(goalState);
var stepCost = new PathFindingStepCost();
var searchAlgorithm = new GraphSearch<PathFindingState, PathFindingAction>();
var problem = new Problem<PathFindingState, PathFindingAction>(initialState, actionFunction, resultFunction, goalTest, stepCost);
var solution = searchAlgorithm.Search(problem);
for (int i = 1; i < solution.Count; i++)
{
AddLine(
solution[i - 1].State.Point.X,
solution[i - 1].State.Point.Y,
solution[i].State.Point.X,
solution[i].State.Point.Y);
}
}