public double IntegrateLength(double startX, double endX)
{
CubicFunction deriv = Differentiate();
switch (Degree)
{
case 0:
return(endX - startX);
case 1:
CubicFunction integral = new CubicFunction(0, 0, Math.Sqrt(Math.Pow(deriv.d, 2) + 1), 0);
return(integral.Evaluate(endX) - integral.Evaluate(startX));
case 2:
double integralEndValue = (Math.Sqrt(Math.Pow((deriv.c * endX + deriv.d), 2) + 1) * (deriv.c * endX + deriv.d)
+ arsinh(deriv.c * endX + deriv.d)) / (2 * deriv.c);
double integralStartValue = (Math.Sqrt(Math.Pow((deriv.c * startX + deriv.d), 2) + 1) * (deriv.c * startX + deriv.d)
+ arsinh(deriv.c * startX + deriv.d)) / (2 * deriv.c);
return(integralEndValue - integralStartValue);
case 3:
return(0);
}
throw new Exception("Something went very wrong here...");
}
public List <Point> GeneratePath(double interval)
{
List <Point> path = new List <Point>();
int direction = Math.Sign(EndX - StartX);
CubicFunction func = GetFunction();
for (double x = StartX; direction == 1 ? x <EndX : x> EndX; x += direction * interval)
{
path.Add(new Point(x, func.Evaluate(x)));
}
path.Add(new Point(EndX, func.Evaluate(EndX)));
return(path);
}
public static void Export(string filename, IEnumerable <Path> paths, double granularity)
{
using (StreamWriter file = File.CreateText(filename))
{
file.WriteLine("distance,wheelangle");
foreach (Path p in paths)
{
file.WriteLine("#####{0}#####", p.Name);
//todo get motion direction, then do this evaluation per granularity increment
int direction = Math.Sign(p.EndX - p.StartX);
CubicFunction func = p.GetFunction();
CubicFunction deriv = func.Differentiate();
for (double x = p.StartX; direction > 0 ? x <p.EndX : x> p.EndX; x += granularity * direction)
{
WriteDistancePoint(x, p.StartX, direction, func, deriv, file);
}
WriteDistancePoint(p.EndX, p.StartX, direction, func, deriv, file);
}
}
}
private static void WriteDistancePoint(double x, double start, int direction, CubicFunction func, CubicFunction deriv, StreamWriter file)
{
double dist = Math.Abs(func.IntegrateLength(start, x));
double angle = Math.Atan(deriv.Evaluate(x)) * 180 / Math.PI;
//if we're moving to the left, that means we just flip the wheels around
if (direction < 0)
{
angle += 180;
}
//subtract 90 degrees, while making sure that the angle is positive
angle += 270;
//range from 0 to 360.
angle %= 360;
file.WriteLine("{0},{1}", dist, angle);
}
