/// <summary>
/// walks along the bezier path and creates a straight line segment approximation
/// </summary>
/// <param name="path"></param>
/// <param name="sampleDistance"></param>
/// <returns></returns>
public PointPath ConvertBezierPathToPointPath(IPath path, double sampleDistance)
{
PointPath pathOut = new PointPath();
foreach (IPathSegment seg in path)
{
if (seg is BezierPathSegment == false)
{
throw new InvalidOperationException();
}
BezierPathSegment bseg = seg as BezierPathSegment;
PointOnPath p = seg.StartPoint;
double refDist = 0;
while (refDist == 0)
{
PointOnPath p2;
refDist = sampleDistance;
p2 = seg.AdvancePoint(p, ref refDist);
pathOut.Add(new LinePathSegment(p.pt, p2.pt));
p = p2;
}
}
return(pathOut);
}
public IPath Wp2CubicPath(IPath path, List <Polygon> obstacles, double nothing)
{
PointPath ret = new PointPath();
CubicBezier[] bez = SmoothingSpline.BuildCardinalSpline(PathUtils.IPathToPoints(path).ToArray(), null, null, 1.0);
foreach (CubicBezier b in bez)
{
ret.Add(new BezierPathSegment(b, null, false));
}
return(ret);
}
/// <summary>
/// Output the last planned path
/// </summary>
public IPath GetPathToGoal()
{
lock (pathLock)
{
PointPath newPath = new PointPath();
if (outputNodeList.Count == 1)
{
//If there is only one ode in the planned path, return an IPath with a degenerate segment
newPath.Add(new LinePathSegment(outputNodeList[0].Value, outputNodeList[0].Value));
}
else if (outputNodeList.Count > 1)
{
//Build an IPath based on the values of each node in the last planned path
int segmentCount = outputNodeList.Count - 1;
for (int i = 0; i < segmentCount; i++)
{
newPath.Add(new LinePathSegment(outputNodeList[i].Value, outputNodeList[i + 1].Value));
}
}
return(newPath);
}
}