public void testCreate(float dx, float dy)
{
controlPointsX.Clear();
controlPointsY.Clear();
foreach (PointF p in controlPoints)
{
controlPointsX.Add(p.X);
controlPointsY.Add(p.Y);
}
if (dx == 0 && dy == 0)
{
path = new Spline.ParametricSpline(controlPointsX.ToArray(), controlPointsY.ToArray(), resolution, out xs, out ys);
}
else
{
path = new Spline.ParametricSpline(controlPointsX.ToArray(), controlPointsY.ToArray(), resolution, out xs, out ys, dx, dy);
}
velocityMap.setLength(path.distance.Last());
buildMaps();
PointF p1 = path.Eval(distance[distance.Length - 1]);
PointF p2 = path.Eval(distance[distance.Length - 2]);
this.dx = (p2.X - p1.X) / (float)velocityMap.time;
this.dy = (p2.Y - p1.Y) / (float)velocityMap.time;
}
private void CreateSpline()
{
controlPointsX.Clear();
controlPointsY.Clear();
foreach (PointF p in controlPoints)
{
controlPointsX.Add(p.X);
controlPointsY.Add(p.Y);
}
path = new Spline.ParametricSpline(controlPointsX.ToArray(), controlPointsY.ToArray(), resolution, out xs, out ys);
velocityMap.setLength(path.distance.Last());
}
public static List <ControlPointSegment> GenSpline(List <ControlPoint> points, List <VelocityPoint> velocityPoints = null)
{
double[] xOrig = new double[0];
List <ControlPointSegment> splineSegments = new List <ControlPointSegment>();
splineSegments.Clear();
List <double> xs = new List <double>();
List <double> ys = new List <double>();
foreach (ControlPoint point in points)
{
xs.Add(point.X);
ys.Add(point.Y);
}
List <CubicSplinePoint> outxs;
List <CubicSplinePoint> outys;
spline = new ParametricSpline(xs.ToArray(), ys.ToArray(), 100, out outxs, out outys);
xOrig = spline.xSpline.xOrig;
spline = new ParametricSpline(xs.ToArray(), ys.ToArray(), (int)(spline.length), out outxs, out outys);
List <double> CPDistances = new List <double>();
double d = 0.0;
CPDistances.Add(d);
for (int i = 0; i < outxs.Last().ControlPointNum + 1; i++)
{
for (int x = 1; x < outxs.Count; x++)
{
if (outxs[x].ControlPointNum == i)
{
d += GetDistance(outxs[x - 1].Y, outys[x - 1].Y, outxs[x].Y, outys[x].Y);
}
}
CPDistances.Add(d);
}
xs.Clear();
ys.Clear();
foreach (CubicSplinePoint point in outxs)
{
xs.Add(point.Y);
}
foreach (CubicSplinePoint point in outys)
{
ys.Add(point.Y);
}
spline = new ParametricSpline(xs.ToArray(), ys.ToArray(), 100, out outxs, out outys);
if (velocityPoints == null)
{
for (int i = 0; i < outxs.Last().ControlPointNum + 1; i++)
{
ControlPointSegment seg = new ControlPointSegment();
for (int x = 0; x < outxs.Count; x++)
{
if (outxs[x].ControlPointNum == i)
{
seg.points.Add(new SplinePoint(outxs[x].Y, outys[x].Y, i));
}
}
splineSegments.Add(seg);
}
}
else
{
ControlPointSegment seg = new ControlPointSegment();
int lastControlPointNum = 0;
for (int i = 0; i < velocityPoints.Count; i++)
{
SplinePoint spoint = spline.Eval(Math.Abs((double)velocityPoints[i].Pos));
double distance = Math.Abs((double)velocityPoints[i].Pos);
for (int dis = 1; dis < CPDistances.Count; dis++)
{
if (distance >= CPDistances[dis - 1] && distance <= CPDistances[dis])
{
spoint.ControlPointNum = dis - 1;
seg.PathNum = points[dis - 1].getGraphIndex();
if (spoint.ControlPointNum != lastControlPointNum)
{
splineSegments.Add(seg);
seg = new ControlPointSegment();
}
seg.points.Add(spoint);
lastControlPointNum = dis - 1;
}
if (distance >= CPDistances[dis] && dis == CPDistances.Count - 1)
{
seg.points.Add(spoint);
//we add any of the points that over shoot the last distance point by just a little because
//we have an error somewhere but the ending error is less then 5mm less than the accuracy of
//the robot anyways so.
}
}
}
seg.PathNum = points[CPDistances.Count - 1].getGraphIndex();
splineSegments.Add(seg);
}
return(splineSegments);
}
