public void TestRotateBy()
{
var another = new Translation2d(Length.FromMeters(3), Length.FromMeters(0));
var rotated = another.RotateBy(new Rotation2d(Angle.FromDegrees(90)));
AssertEqualLengths(Length.FromMeters(0.0), rotated.X);
AssertEqualLengths(Length.FromMeters(3.0), rotated.Y);
}
/**
* fits a parabola to 3 points
*
* @return the x coordinate of the vertex of the parabola
*/
private static double fitParabola(Translation2d p1, Translation2d p2, Translation2d p3)
{
double A = (p3.x() * (p2.y() - p1.y()) + p2.x() * (p1.y() - p3.y()) + p1.x() * (p3.y() - p2.y()));
double B = (p3.x() * p3.x() * (p1.y() - p2.y()) + p2.x() * p2.x() * (p3.y() - p1.y())
+ p1.x() * p1.x() * (p2.y() - p3.y()));
return(-B / (2 * A));
}
public void TestDifference()
{
var one = new Translation2d(Length.FromMeters(1), Length.FromMeters(3));
var two = new Translation2d(Length.FromMeters(2), Length.FromMeters(5));
var diff = one - two;
AssertEqualLengths(Length.FromMeters(-1.0), diff.X);
AssertEqualLengths(Length.FromMeters(-2.0), diff.Y);
}
public void TestSum()
{
var one = new Translation2d(Length.FromMeters(1), Length.FromMeters(3));
var two = new Translation2d(Length.FromMeters(2), Length.FromMeters(5));
var sum = one + two;
AssertEqualLengths(Length.FromMeters(3.0), sum.X);
AssertEqualLengths(Length.FromMeters(8.0), sum.Y);
}
private static void getSegmentArc(Spline s, ref List <Pose2dWithCurvature> rv, double t0, double t1, double maxDx,
double maxDy, double maxDTheta)
{
Translation2d p0 = s.getPoint(t0);
Translation2d p1 = s.getPoint(t1);
Rotation2d r0 = s.getHeading(t0);
Rotation2d r1 = s.getHeading(t1);
Pose2d transformation = new Pose2d(new Translation2d(p0, p1).rotateBy(r0.inverse()), r1.rotateBy(r0.inverse()));
Twist2d twist = Pose2d.log(transformation);
if (twist.dy > maxDy || twist.dx > maxDx || twist.dtheta > maxDTheta)
{
getSegmentArc(s, ref rv, t0, (t0 + t1) / 2, maxDx, maxDy, maxDTheta);
getSegmentArc(s, ref rv, (t0 + t1) / 2, t1, maxDx, maxDy, maxDTheta);
}
else
{
rv.Add(s.getPose2dWithCurvature(t1));
}
}
public static void DrawFunctionOutput(this Bitmap self, Func <double, double, double> func, Translation2d translation)
{
double min = double.MaxValue;
double max = double.MinValue;
double[,] outputs = new double[self.Width, self.Height];
for (int py = 0; py < self.Height; py++)
{
for (int px = 0; px < self.Width; px++)
{
translation(px, py, out double x, out double y);
double output = func(x, y);
outputs[px, py] = output;
if (output < min)
{
min = output;
}
if (output > max)
{
max = output;
}
}
}
for (int py = 0; py < self.Height; py++)
{
for (int px = 0; px < self.Width; px++)
{
//translation(px, py, out double x, out double y);
double output = outputs[px, py];
if (output < 0)
{
int c = (int)(255.0 / +min * output);
self.SetPixel(px, py, Color.FromArgb(c, 255, 255));
}
else
{
int c = (int)(255.0 / max * output);
self.SetPixel(px, py, Color.FromArgb(255, 255, c));
}
}
}
}
public static void DrawTestData(this Bitmap self, IEnumerable <Projection> testData, Func <double[], Pen> classifier, Translation2d translation)
{
if (testData != null)
{
using (Graphics graphics = Graphics.FromImage(self))
{
foreach (Projection testItem in testData)
{
translation(testItem.Input[0], testItem.Input[1], out double px, out double py);
Pen pen = classifier(testItem.Output);//[0] < 0 ? Pens.Blue : Pens.Red;
graphics.DrawEllipse(pen, px - 3, py - 3, 6, 6);
}
}
}
}
/**
* Runs a single optimization iteration
*/
private static void runOptimizationIteration(ref List <QuinticSpline> splines)
{
// can't optimize anything with less than 2 splines
if (splines.Count <= 1)
{
return;
}
ControlPoint[] controlPoints = new ControlPoint[splines.Count - 1];
double magnitude = 0;
for (int i = 0; i < splines.Count - 1; ++i)
{
// don't try to optimize colinear points
if (splines[i].getStartPose().isColinear(splines[i + 1].getStartPose()) ||
splines[i].getEndPose().isColinear(splines[i + 1].getEndPose()))
{
continue;
}
double original = sumDCurvature2(splines);
QuinticSpline temp, temp1;
temp = splines[i];
temp1 = splines[i + 1];
controlPoints[i] = new ControlPoint(); // holds the gradient at a control point
// calculate partial derivatives of sumDCurvature2
splines[i] = new QuinticSpline(temp.x0, temp.x1, temp.dx0, temp.dx1, temp.ddx0,
temp.ddx1 + kEpsilon, temp.y0, temp.y1, temp.dy0, temp.dy1, temp.ddy0, temp.ddy1);
splines[i + 1] = new QuinticSpline(temp1.x0, temp1.x1, temp1.dx0, temp1.dx1, temp1.ddx0 + kEpsilon,
temp1.ddx1, temp1.y0, temp1.y1, temp1.dy0, temp1.dy1, temp1.ddy0, temp1.ddy1);
controlPoints[i].ddx = (sumDCurvature2(splines) - original) / kEpsilon;
splines[i] = new QuinticSpline(temp.x0, temp.x1, temp.dx0, temp.dx1, temp.ddx0, temp.ddx1, temp.y0,
temp.y1, temp.dy0, temp.dy1, temp.ddy0, temp.ddy1 + kEpsilon);
splines[i + 1] = new QuinticSpline(temp1.x0, temp1.x1, temp1.dx0, temp1.dx1, temp1.ddx0,
temp1.ddx1, temp1.y0, temp1.y1, temp1.dy0, temp1.dy1, temp1.ddy0 + kEpsilon, temp1.ddy1);
controlPoints[i].ddy = (sumDCurvature2(splines) - original) / kEpsilon;
splines[i] = temp;
splines[i + 1] = temp1;
magnitude += controlPoints[i].ddx * controlPoints[i].ddx + controlPoints[i].ddy * controlPoints[i].ddy;
}
magnitude = Math.Sqrt(magnitude);
// minimize along the direction of the gradient
// first calculate 3 points along the direction of the gradient
Translation2d p1, p2, p3;
p2 = new Translation2d(0, sumDCurvature2(splines)); // middle point is at the current location
for (int i = 0; i < splines.Count - 1; ++i)
{ // first point is offset from the middle location by -stepSize
if (splines[i].getStartPose().isColinear(splines[i + 1].getStartPose()) ||
splines[i].getEndPose().isColinear(splines[i + 1].getEndPose()))
{
continue;
}
// normalize to step size
controlPoints[i].ddx *= kStepSize / magnitude;
controlPoints[i].ddy *= kStepSize / magnitude;
// move opposite the gradient by step size amount
splines[i].ddx1 -= controlPoints[i].ddx;
splines[i].ddy1 -= controlPoints[i].ddy;
splines[i + 1].ddx0 -= controlPoints[i].ddx;
splines[i + 1].ddy0 -= controlPoints[i].ddy;
// recompute the spline's coefficients to account for new second derivatives
splines[i].computeCoefficients();
splines[i + 1].computeCoefficients();
}
p1 = new Translation2d(-kStepSize, sumDCurvature2(splines));
for (int i = 0; i < splines.Count - 1; ++i)
{ // last point is offset from the middle location by +stepSize
if (splines[i].getStartPose().isColinear(splines[i + 1].getStartPose()) ||
splines[i].getEndPose().isColinear(splines[i + 1].getEndPose()))
{
continue;
}
// move along the gradient by 2 times the step size amount (to return to
// original location and move by 1
// step)
splines[i].ddx1 += 2 * controlPoints[i].ddx;
splines[i].ddy1 += 2 * controlPoints[i].ddy;
splines[i + 1].ddx0 += 2 * controlPoints[i].ddx;
splines[i + 1].ddy0 += 2 * controlPoints[i].ddy;
// recompute the spline's coefficients to account for new second derivatives
splines[i].computeCoefficients();
splines[i + 1].computeCoefficients();
}
p3 = new Translation2d(kStepSize, sumDCurvature2(splines));
double stepSize = fitParabola(p1, p2, p3); // approximate step size to minimize sumDCurvature2 along the
// gradient
for (int i = 0; i < splines.Count - 1; ++i)
{
if (splines[i].getStartPose().isColinear(splines[i + 1].getStartPose()) ||
splines[i].getEndPose().isColinear(splines[i + 1].getEndPose()))
{
continue;
}
// move by the step size calculated by the parabola fit (+1 to offset for the
// final transformation to find
// p3)
controlPoints[i].ddx *= 1 + stepSize / kStepSize;
controlPoints[i].ddy *= 1 + stepSize / kStepSize;
splines[i].ddx1 += controlPoints[i].ddx;
splines[i].ddy1 += controlPoints[i].ddy;
splines[i + 1].ddx0 += controlPoints[i].ddx;
splines[i + 1].ddy0 += controlPoints[i].ddy;
// recompute the spline's coefficients to account for new second derivatives
splines[i].computeCoefficients();
splines[i + 1].computeCoefficients();
}
}
