///<summary><para>Searches path using A* Algorithm. Finds the path to target using heuristics</para>
///<para>Nodes are prioritized by heuristic of how far is node from target and distance traveled from the start. Finds performant reliable path</para></summary>
private void SearchPathAStar(Node node)
{
if (node != null)
{
for (int i = 0; i < node.Neighbors.Count; i++)
{
//if the current neighbor is non explored node
if (!_closedSet.Contains(node.Neighbors[i]))
{
float costToNeighbor = PathMath.GetDistance(node, node.Neighbors[i]);
float newCostSoFar = costToNeighbor + node.CostSoFar + node.Weight;
//re-route if a shorter path exists, positive inifinity is non opened node
if (float.IsPositiveInfinity(node.Neighbors[i].CostSoFar) || newCostSoFar < node.Neighbors[i].CostSoFar)
{
node.Neighbors[i].Previous = node;
node.Neighbors[i].CostSoFar = newCostSoFar;
}
//enqueue if this neighbor is not in currently opened set
if (!_openSet.Contains(node.Neighbors[i]))
{
float distanceToGoal = PathMath.GetHeuristicDistance(node.Neighbors[i], _targetNode, _heuriticsType);
node.Neighbors[i].Priority = node.Neighbors[i].CostSoFar + distanceToGoal;
_openSet.Enqueue(node.Neighbors[i]);
}
}
}
}
}
///<summary><para>Searches path using Breadth First Algorithm. Scan each node in the first level starting from the leftmost node, moving towards the right</para>
///<para>Doesn't use heuristics for search. </para></summary>
private void SearchPathBreadthFirst(Node node)
{
if (node != null)
{
for (int i = 0; i < node.Neighbors.Count; i++)
{
//if the current neighbor is non explored node and is non opened node
if (!_closedSet.Contains(node.Neighbors[i]) && !_openSet.Contains(node.Neighbors[i]))
{
float costToNeighbour = PathMath.GetDistance(node, node.Neighbors[i]);
float newCostSoFar = costToNeighbour + node.CostSoFar + node.Weight;
node.Neighbors[i].CostSoFar = newCostSoFar;
node.Neighbors[i].Previous = node;
node.Neighbors[i].Priority = _closedSet.Count;
_openSet.Enqueue(node.Neighbors[i]);
}
}
}
}
public IEnumerable <ImageFrame> GetFrames()
{
var globalShift = new Vector2(-MaxValues.MinX, -MaxValues.MinY);
float scaleKoeff = (_pathSource.Size.X > _pathSource.Size.Y)
? (MaxValues.MaxX - MaxValues.MinX) / _pathSource.Size.X
: (MaxValues.MaxY - MaxValues.MinY) / _pathSource.Size.Y;
var globalScale = new Vector2(scaleKoeff, scaleKoeff);
// TODO: streaming
var pathFrames = _pathSource.GetFrames();
var res = new List <ImageFrame>();
var points = new List <ImagePoint>();
var sw = new Stopwatch();
sw.Start();
foreach (var pathFrame in pathFrames)
{
// TODO: cache scaled
// TODO: cache stroke, fill update only
if (_strokeConverter != null)
{
foreach (var path in pathFrame.Where(p => p.StrokeColor.HasValue))
{
var strokePrimitives = PathMath.ScalePaths(path.Primitives, globalScale, globalShift).ToArray();
strokePrimitives = _strokeConverter.Convert(strokePrimitives).ToArray();
if (strokePrimitives.Any())
{
points.Add(ToImagePoint(strokePrimitives.First().FirstPoint, GetChannelValue(path.StrokeColor.Value, _strokeBrightness)));
foreach (var prim in strokePrimitives)
{
points.Add(ToImagePoint(prim.LastPoint, GetChannelValue(path.StrokeColor.Value, _strokeBrightness)));
}
var lastpoint = points.Last();//.Clone();
lastpoint.Blanking = true;
points.Add(lastpoint);
}
}
}
if (_fillGeneratorFactory != null)
{
foreach (var path in pathFrame.Where(p => p.FillColor.HasValue))
{
var fillpolygon = PathMath.ScalePaths(path.Primitives, globalScale, globalShift).ToArray();
if (_fillConverter != null)
{
fillpolygon = _fillConverter.Convert(fillpolygon).ToArray();
}
var fillPath = (Path)path.Clone();
fillPath.Primitives = fillpolygon;
fillPath.FillColor = Color.FromArgb(GetChannelValue(fillPath.FillColor.Value, _fillIntensity), 255, 255, 255);
var fillPrimitives = _fillGeneratorFactory(fillPath).GenerateFill();
foreach (var prim in fillPrimitives.Primitives)
{
points.Add(ToImagePoint(prim.FirstPoint, GetChannelValue(path.FillColor.Value, _fillBrightness)));
}
var lastpoint = points.Last().Clone();
lastpoint.Blanking = true;
points.Add(lastpoint);
}
}
}
sw.Stop();
return(new[] { new ImageFrame()
{
Duration = 0,
Number = 1,
Points = points.ToArray()
} });
}
public static double CalculateFlattenedLength(this PathGeometry pathGeom)
{
pathGeom = pathGeom.GetFlattenedPathGeometry();
double runningTotal = 0.0;
Point startPoint = pathGeom.Figures.First().StartPoint;
foreach (PathSegment segment in pathGeom.Figures.SelectMany(f => f.Segments))
{
Point endPoint;
if (segment is ArcSegment arc)
{
// TODO - fix math, should be elipses solver
endPoint = arc.Point;
runningTotal += PathMath.CalculateLinearBezierArcLength(startPoint.X, startPoint.Y, endPoint.X, endPoint.Y);
}
else if (segment is BezierSegment bezier)
{
// TODO - fix path, should calculate control points Point1 & Point2
runningTotal += PathMath.CalculateLinearBezierArcLength(startPoint.X, startPoint.Y, bezier.Point3.X, bezier.Point3.Y);
endPoint = bezier.Point3;
}
else if (segment is LineSegment lineSegment)
{
endPoint = lineSegment.Point;
runningTotal += (endPoint - startPoint).Length;
startPoint = endPoint;
}
else if (segment is PolyBezierSegment polyBezier)
{
// TODO - fix
foreach (Point point in polyBezier.Points)
{
runningTotal += PathMath.CalculateLinearBezierArcLength(startPoint.X, startPoint.Y, point.X, point.Y);
startPoint = point;
}
endPoint = polyBezier.Points.Last();
}
else if (segment is PolyLineSegment polyLine)
{
foreach (Point point in polyLine.Points)
{
runningTotal += (point - startPoint).Length;
startPoint = point;
}
endPoint = polyLine.Points.Last();
}
else if (segment is PolyQuadraticBezierSegment polyQuadBezier)
{
// TODO - fix
foreach (Point point in polyQuadBezier.Points)
{
runningTotal += PathMath.CalculateLinearBezierArcLength(startPoint.X, startPoint.Y, point.X, point.Y);
startPoint = point;
}
endPoint = polyQuadBezier.Points.Last();
}
else if (segment is QuadraticBezierSegment quadBezier)
{
// TODO - fix
endPoint = quadBezier.Point2;
runningTotal += PathMath.CalculateLinearBezierArcLength(startPoint.X, startPoint.Y, endPoint.X, endPoint.Y);
}
else
{
endPoint = startPoint;
}
startPoint = endPoint;
}
return(runningTotal);
}
